111 lines
3.0 KiB
C
111 lines
3.0 KiB
C
#include <logger.h>
|
|
#include <errcode.h>
|
|
#include <dtb.h>
|
|
#include <utils.h>
|
|
#include <string.h>
|
|
#include <kmalloc.h>
|
|
#include <vector.h>
|
|
|
|
void *dtb_start;
|
|
void *dtb_end;
|
|
void *dtb_memory_start;
|
|
void *dtb_memory_end;
|
|
vector_t *dtb_reserved_entries;
|
|
|
|
fdt_reserve_entry_t *fdt_memory_cur;
|
|
uint32_t *fdt_struct_cur;
|
|
|
|
static inline
|
|
void _fdt_struct_callback(const char *path, const vector_t *cbs, const vector_t *props)
|
|
{
|
|
for (int i = 0; i < (int)cbs->size; ++i)
|
|
if (!strcmp(path, VEC_AT(fdt_callback_t, cbs, i)->name))
|
|
VEC_AT(fdt_callback_t, cbs, i)->func(props);
|
|
}
|
|
|
|
static inline
|
|
void _memory_traverse()
|
|
{
|
|
dtb_reserved_entries = make_vector(0);
|
|
while (fdt_memory_cur->address != 0 || fdt_memory_cur->size != 0) {
|
|
DEBUG(ntoh64(fdt_memory_cur->address));
|
|
DEBUG(ntoh64(fdt_memory_cur->size));
|
|
|
|
VEC_PUSH(dtb_reserved_entries, fdt_memory_cur++);
|
|
}
|
|
}
|
|
|
|
#define nextnode(ptr) (void *)((((uint64_t)(ptr) >> 2) + 1) << 2)
|
|
static inline
|
|
void _struct_traverse(const char *path, const vector_t *cbs)
|
|
{
|
|
vector_t *props = make_vector(0);
|
|
|
|
fdt_prop_t *prop;
|
|
const fdt_prop_header_t *prop_header;
|
|
const char *fdt_node_path = 0x0;
|
|
|
|
for (;;) {
|
|
fdt_struct_cur = ALIGN4(fdt_struct_cur);
|
|
|
|
switch (ntoh32(*fdt_struct_cur)) {
|
|
case FDT_BEGIN_NODE:
|
|
++fdt_struct_cur;
|
|
fdt_node_path = (void *)fdt_struct_cur;
|
|
do {
|
|
BUMP(uint32_t, char, fdt_struct_cur);
|
|
} while (*(char *)fdt_struct_cur != '\0');
|
|
BUMP(uint32_t, char, fdt_struct_cur); // '\0'
|
|
|
|
fdt_struct_cur = ALIGN4(fdt_struct_cur);
|
|
_struct_traverse(fdt_node_path, cbs);
|
|
break;
|
|
case FDT_END_NODE:
|
|
fdt_struct_cur = nextnode(fdt_struct_cur);
|
|
goto traverse_struct_callback;
|
|
case FDT_PROP:
|
|
++fdt_struct_cur;
|
|
prop_header = BUMP(uint32_t, fdt_prop_header_t, fdt_struct_cur);
|
|
|
|
prop = kmalloc(sizeof(fdt_prop_t));
|
|
prop->len = ntoh32(prop_header->len);
|
|
prop->name = dtb_start +
|
|
ntoh32(((fdt_header_t *)dtb_start)->off_dt_strings) +
|
|
ntoh32(prop_header->nameoff);
|
|
prop->value = (void *)fdt_struct_cur;
|
|
VEC_PUSH(props, prop);
|
|
|
|
fdt_struct_cur = (void *)((uint64_t)fdt_struct_cur +
|
|
ntoh32(prop_header->len));
|
|
fdt_struct_cur = ALIGN4(fdt_struct_cur);
|
|
break;
|
|
case FDT_NOP:
|
|
fdt_struct_cur = nextnode(fdt_struct_cur);
|
|
break;
|
|
case FDT_END:
|
|
return;
|
|
default:
|
|
exit(ERR_UNREACHABLE);
|
|
}
|
|
}
|
|
traverse_struct_callback:
|
|
_fdt_struct_callback(path, cbs, props);
|
|
}
|
|
|
|
void fdt_traverse(const vector_t *struct_cbs)
|
|
{
|
|
fdt_memory_cur = dtb_start +
|
|
ntoh32(((fdt_header_t *)dtb_start)->off_mem_rsvmap);
|
|
fdt_struct_cur = dtb_start +
|
|
ntoh32(((fdt_header_t *)dtb_start)->off_dt_struct);
|
|
dtb_end = dtb_start +
|
|
ntoh32(((fdt_header_t *)dtb_start)->totalsize);
|
|
|
|
DEBUG(fdt_memory_cur);
|
|
DEBUG(fdt_struct_cur);
|
|
_memory_traverse();
|
|
DEBUG_DTB("memory traverse done");
|
|
_struct_traverse("", struct_cbs);
|
|
DEBUG_DTB("struct traverse done");
|
|
}
|