118 lines
4 KiB
C++
118 lines
4 KiB
C++
/*
|
|
* UniversalisOS minimal FDT reader — implementation (MP0).
|
|
*
|
|
* Walks the DTB structure block to find named nodes + properties. This is a
|
|
* simplified reader: it does a single linear pass, tracking the current node
|
|
* path, and returns the first property match. Sufficient for boot-time hardware
|
|
* discovery (GIC/UART/timer/memory).
|
|
*/
|
|
#include "fdt_reader.h"
|
|
|
|
/* Big-endian accessors (DTB is always big-endian). */
|
|
static uint32_t be32(const uint8_t* p) {
|
|
return ((uint32_t)p[0]<<24)|((uint32_t)p[1]<<16)|((uint32_t)p[2]<<8)|p[3];
|
|
}
|
|
|
|
int fdt_check(const void* dtb) {
|
|
const fdt_header_t* h = (const fdt_header_t*)dtb;
|
|
return (be32((const uint8_t*)&h->magic) == FDT_MAGIC) ? 0 : -1;
|
|
}
|
|
|
|
static const uint8_t* fdt_struct(const void* dtb) {
|
|
const fdt_header_t* h = (const fdt_header_t*)dtb;
|
|
return (const uint8_t*)dtb + be32((const uint8_t*)&h->off_dt_struct);
|
|
}
|
|
|
|
static const char* fdt_strings(const void* dtb) {
|
|
const fdt_header_t* h = (const fdt_header_t*)dtb;
|
|
return (const char*)dtb + be32((const uint8_t*)&h->off_dt_strings);
|
|
}
|
|
|
|
static int streq(const char* a, const char* b) {
|
|
while (*a && *b) { if (*a != *b) return 0; a++; b++; }
|
|
return *a == *b;
|
|
}
|
|
|
|
static const uint8_t* skip_str(const uint8_t* p) {
|
|
while (*p) p++;
|
|
return (const uint8_t*)(((uintptr_t)p + 4) & ~3u); /* align to 4 */
|
|
}
|
|
|
|
int fdt_get_prop(const void* dtb, const char* node_name, const char* prop_name, fdt_prop_t* out) {
|
|
const uint8_t* s = fdt_struct(dtb);
|
|
const char* strs = fdt_strings(dtb);
|
|
int in_target = 0;
|
|
int depth = 0;
|
|
|
|
for (;;) {
|
|
uint32_t tok = be32(s); s += 4;
|
|
if (tok == FDT_BEGIN_NODE) {
|
|
const char* name = (const char*)s;
|
|
s = skip_str(s);
|
|
depth++;
|
|
/* Match node_name at any depth (first component match). */
|
|
if (streq(name, node_name) || (name[0] == node_name[0] && streq(name, node_name))) {
|
|
in_target = depth;
|
|
}
|
|
} else if (tok == FDT_END_NODE) {
|
|
if (in_target == depth) in_target = 0;
|
|
depth--;
|
|
} else if (tok == FDT_PROP) {
|
|
uint32_t len = be32(s); s += 4;
|
|
uint32_t noff = be32(s); s += 4;
|
|
const char* pname = strs + noff;
|
|
if (in_target && streq(pname, prop_name)) {
|
|
out->addr = (uint64_t)(uintptr_t)s;
|
|
out->len = len;
|
|
return 0;
|
|
}
|
|
s += (len + 3) & ~3u; /* align to 4 */
|
|
} else if (tok == FDT_NOP) {
|
|
/* skip */
|
|
} else if (tok == FDT_END) {
|
|
break;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
uint64_t fdt_read_addr(const void* dtb, const char* node_name) {
|
|
fdt_prop_t p;
|
|
if (fdt_get_prop(dtb, node_name, "reg", &p) != 0) return 0;
|
|
/* reg for a simple bus node: 2 cells (addr-hi, addr-lo) or 1 cell. */
|
|
if (p.len >= 8) {
|
|
return ((uint64_t)be32((const uint8_t*)p.addr) << 32) |
|
|
be32((const uint8_t*)(p.addr + 4));
|
|
}
|
|
if (p.len >= 4) return be32((const uint8_t*)p.addr);
|
|
return 0;
|
|
}
|
|
|
|
void fdt_get_memory(const void* dtb, uint64_t* base, uint64_t* size) {
|
|
fdt_prop_t p;
|
|
*base = 0; *size = 0;
|
|
if (fdt_get_prop(dtb, "memory", "reg", &p) != 0) return;
|
|
if (p.len >= 16) {
|
|
/* 2 address-cells + 2 size-cells (each 32-bit) = 16 bytes. */
|
|
const uint8_t* a = (const uint8_t*)p.addr;
|
|
*base = ((uint64_t)be32(a) << 32) | be32(a+4);
|
|
*size = ((uint64_t)be32(a+8) << 32) | be32(a+12);
|
|
} else if (p.len >= 8) {
|
|
const uint8_t* a = (const uint8_t*)p.addr;
|
|
*base = be32(a);
|
|
*size = be32(a+4);
|
|
}
|
|
}
|
|
|
|
uint32_t fdt_get_cntfrq(const void* dtb) {
|
|
fdt_prop_t p;
|
|
/* The timer node often carries clock-frequency. */
|
|
if (fdt_get_prop(dtb, "timer", "clock-frequency", &p) == 0 && p.len >= 4) {
|
|
return be32((const uint8_t*)p.addr);
|
|
}
|
|
/* Fallback: read from the ARM virtual timer node. */
|
|
if (fdt_get_prop(dtb, "timer", "clocks", &p) == 0 && p.len >= 4) {
|
|
return be32((const uint8_t*)p.addr);
|
|
}
|
|
return 0;
|
|
}
|