--- /dev/null
+#include <lunaix/mm/valloc.h>
+#include <lunaix/syslog.h>
+
+#include <klibc/string.h>
+
+#include <hal/devtree.h>
+
+LOG_MODULE("dtb")
+
+static struct dt_context dtctx;
+
+void
+fdt_itbegin(struct fdt_iter* fdti, struct fdt_header* fdt_hdr)
+{
+ unsigned int off_struct, off_str;
+ struct fdt_token* tok;
+ const char* str_blk;
+
+ off_str = le(fdt_hdr->off_dt_strings);
+ off_struct = le(fdt_hdr->off_dt_struct);
+
+ tok = offset_t(fdt_hdr, struct fdt_token, off_struct);
+ str_blk = offset_t(fdt_hdr, const char, off_str);
+
+ *fdti = (struct fdt_iter) {
+ .pos = tok,
+ .str_block = str_blk
+ };
+}
+
+void
+fdt_itend(struct fdt_iter* fdti)
+{
+ fdti->pos = NULL;
+}
+
+bool
+fdt_itnext(struct fdt_iter* fdti)
+{
+ struct fdt_token *current;
+ struct fdt_prop *prop;
+
+ current = fdti->pos;
+ if (!current) {
+ return false;
+ }
+
+ do
+ {
+ if (fdt_nope(current)) {
+ continue;
+ }
+
+ if (fdt_prop(current)) {
+ prop = (struct fdt_prop*) current;
+ current = offset(current, prop->len);
+ continue;
+ }
+
+ if (fdt_node_end(current)) {
+ fdti->depth--;
+ continue;
+ }
+
+ // node begin
+
+ fdti->depth++;
+ if (fdti->depth == 1) {
+ // enter root node
+ break;
+ }
+
+ while (!fdt_prop(current) && !fdt_node_end(current)) {
+ current++;
+ }
+
+ if (fdt_prop(current)) {
+ break;
+ }
+
+ current++;
+
+ } while (fdt_nope(current) && fdti->depth > 0);
+
+ return fdti->depth > 0;
+}
+
+bool
+fdt_itnext_at(struct fdt_iter* fdti, int level)
+{
+ while (fdti->depth != level && fdt_itnext(fdti));
+
+ return fdti->depth == level;
+}
+
+void
+fdt_memrsvd_itbegin(struct fdt_memrsvd_iter* rsvdi,
+ struct fdt_header* fdt_hdr)
+{
+ size_t off = le(fdt_hdr->off_mem_rsvmap);
+
+ rsvdi->block =
+ offset_t(fdt_hdr, typeof(*rsvdi->block), off);
+
+ rsvdi->block = &rsvdi->block[-1];
+}
+
+bool
+fdt_memrsvd_itnext(struct fdt_memrsvd_iter* rsvdi)
+{
+ struct fdt_memrsvd_ent* ent;
+
+ ent = rsvdi->block;
+ if (!ent) {
+ return false;
+ }
+
+ rsvdi->block++;
+
+ return ent->addr || ent->size;
+}
+
+void
+fdt_memrsvd_itend(struct fdt_memrsvd_iter* rsvdi)
+{
+ rsvdi->block = NULL;
+}
+
+static inline bool
+propeq(struct fdt_iter* it, const char* key)
+{
+ return streq(fdtit_prop_key(it), key);
+}
+
+static inline void
+__mkprop_val32(struct fdt_iter* it, struct dt_prop_val* val)
+{
+ val->u32_val = le(*(u32_t*)&it->prop[1]);
+ val->size = le(it->prop->len);
+}
+
+static inline void
+__mkprop_val64(struct fdt_iter* it, struct dt_prop_val* val)
+{
+ val->u64_val = le64(*(u64_t*)&it->prop[1]);
+ val->size = le(it->prop->len);
+}
+
+static inline void
+__mkprop_ptr(struct fdt_iter* it, struct dt_prop_val* val)
+{
+ val->ptr_val = __ptr(&it->prop[1]);
+ val->size = le(it->prop->len);
+}
+
+static inline u32_t
+__prop_getu32(struct fdt_iter* it)
+{
+ return le(*(u32_t*)&it->prop[1]);
+}
+
+static bool
+__parse_stdbase_prop(struct fdt_iter* it, struct dt_node_base* node)
+{
+ struct fdt_prop* prop;
+
+ prop = it->prop;
+
+ if (propeq(it, "compatible")) {
+ __mkprop_ptr(it, &node->compat);
+ }
+
+ else if (propeq(it, "model")) {
+ node->model = (const char*)&prop[1];
+ }
+
+ else if (propeq(it, "phandle")) {
+ node->phandle = __prop_getu32(it);
+ hashtable_hash_in(dtctx.phnds_table,
+ &node->phnd_link, node->phandle);
+ }
+
+ else if (propeq(it, "#address-cells")) {
+ node->addr_c = (char)__prop_getu32(it);
+ }
+
+ else if (propeq(it, "#size-cells")) {
+ node->sz_c = (char)__prop_getu32(it);
+ }
+
+ else if (propeq(it, "#interrupt-cells")) {
+ node->intr_c = (char)__prop_getu32(it);
+ }
+
+ else if (propeq(it, "status")) {
+ char peek = *(char*)&it->prop[1];
+ if (peek == 'o') {
+ node->status = STATUS_OK;
+ }
+ else if (peek == 'r') {
+ node->status = STATUS_RSVD;
+ }
+ else if (peek == 'd') {
+ node->status = STATUS_DISABLE;
+ }
+ else if (peek == 'f') {
+ node->status = STATUS_FAIL;
+ }
+ }
+
+ else {
+ return false;
+ }
+
+ return true;
+}
+
+static bool
+__parse_stdnode_prop(struct fdt_iter* it, struct dt_node* node)
+{
+ if (propeq(it, "reg")) {
+ __mkprop_ptr(it, &node->reg);
+ }
+
+ else if (propeq(it, "virtual-reg")) {
+ __mkprop_ptr(it, &node->vreg);
+ }
+
+ else if (propeq(it, "ranges")) {
+ __mkprop_ptr(it, &node->ranges);
+ }
+
+ else if (propeq(it, "dma-ranges")) {
+ __mkprop_ptr(it, &node->dma_ranges);
+ }
+
+ else {
+ return false;
+ }
+
+ return true;
+}
+
+static bool
+__parse_stdintr_prop(struct fdt_iter* it, struct dt_intr_node* node)
+{
+ if (propeq(it, "interrupt-map")) {
+ __mkprop_ptr(it, &node->intr_map);
+ }
+
+ else if (propeq(it, "interrupt-map-mask")) {
+ __mkprop_ptr(it, &node->intr_map_mask);
+ }
+
+ else if (propeq(it, "interrupt-parent")) {
+ node->parent_hnd = __prop_getu32(it);
+ }
+
+ else if (propeq(it, "interrupt-extended")) {
+ node->intr.extended = true;
+ __mkprop_ptr(it, &node->intr.arr);
+ }
+
+ else if (!node->intr.extended && propeq(it, "interrupts")) {
+ __mkprop_ptr(it, &node->intr.arr);
+ }
+
+ else {
+ return false;
+ }
+
+ return true;
+}
+
+static bool
+__parse_stdflags(struct fdt_iter* it, struct dt_node_base* node)
+{
+ if (propeq(it, "dma-coherent")) {
+ node->dma_coherent = true;
+ }
+
+ else if (propeq(it, "dma-noncoherent")) {
+ node->dma_ncoherent = true;
+ }
+
+ else if (propeq(it, "interrupt-controller")) {
+ node->intr_controll = true;
+ }
+
+ else {
+ return false;
+ }
+
+ return true;
+}
+
+static void
+__parse_other_prop(struct fdt_iter* it, struct dt_node_base* node)
+{
+ struct dt_prop* prop;
+ const char* key;
+ unsigned int hash;
+
+ prop = valloc(sizeof(*prop));
+ key = fdtit_prop_key(it);
+
+ prop->key = HSTR(key, strlen(key));
+ __mkprop_ptr(it, &prop->val);
+
+ hstr_rehash(&prop->key, HSTR_FULL_HASH);
+ hash = prop->key.hash;
+
+ hashtable_hash_in(node->_op_bucket, &prop->ht, hash);
+}
+
+static void
+__fill_node(struct fdt_iter* it, struct dt_node* node)
+{
+ if (__parse_stdflags(it, &node->base)) {
+ return;
+ }
+
+ if (__parse_stdbase_prop(it, &node->base)) {
+ return;
+ }
+
+ if (__parse_stdnode_prop(it, node)) {
+ return;
+ }
+
+ if (__parse_stdintr_prop(it, &node->intr)) {
+ return;
+ }
+
+ __parse_other_prop(it, &node->base);
+}
+
+static void
+__fill_root(struct fdt_iter* it, struct dt_root* node)
+{
+ if (__parse_stdflags(it, &node->base)) {
+ return;
+ }
+
+ if (__parse_stdbase_prop(it, &node->base)) {
+ return;
+ }
+
+ struct fdt_prop* prop;
+
+ prop = it->prop;
+ if (propeq(it, "serial-number")) {
+ node->serial = (const char*)&prop[1];
+ }
+
+ else if (propeq(it, "chassis-type")) {
+ node->chassis = (const char*)&prop[1];
+ }
+
+ __parse_other_prop(it, &node->base);
+}
+
+static inline void
+__init_node(struct dt_node_base* node)
+{
+ hashtable_init(node->_op_bucket);
+ llist_init_head(&node->children);
+}
+
+static inline void
+__init_node_regular(struct dt_node* node)
+{
+ __init_node(&node->base);
+ node->intr.parent_hnd = PHND_NULL;
+}
+
+static void
+__expand_extended_intr(struct dt_intr_node* intrupt)
+{
+ struct dt_prop_iter it;
+ struct dt_prop_val arr;
+ struct dt_node *node;
+ struct dt_node *master;
+ struct dt_intr_prop* intr_prop;
+
+ if (!intrupt->intr.extended) {
+ return;
+ }
+
+ arr = intrupt->intr.arr;
+ node = DT_NODE(intrupt);
+
+ llist_init_head(&intrupt->intr.values);
+
+ dt_decode(&it, &node->base, &arr, 1);
+
+ dt_phnd_t phnd;
+ while(dtprop_next(&it)) {
+ phnd = dtprop_to_u32(it.prop_loc);
+ master = dt_resolve_phandle(phnd);
+
+ if (!master) {
+ WARN("dtb: (intr_extended) malformed phandle: %d", phnd);
+ continue;
+ }
+
+ intr_prop = valloc(sizeof(*intr_prop));
+
+ intr_prop->master = &master->intr;
+ intr_prop->val = (struct dt_prop_val) {
+ .encoded = it.prop_loc_next,
+ .size = master->base.intr_c
+ };
+
+ llist_append(&intrupt->intr.values, &intr_prop->props);
+ dtprop_next_n(&it, intr_prop->val.size);
+ }
+}
+
+static void
+__resolve_phnd_references()
+{
+ struct dt_node_base *pos, *n;
+ struct dt_node *node, *parent, *default_parent;
+ struct dt_intr_node* intrupt;
+ dt_phnd_t phnd;
+
+ llist_for_each(pos, n, &dtctx.nodes, nodes)
+ {
+ node = (struct dt_node*)pos;
+ intrupt = &node->intr;
+ if (!node->base.intr_c) {
+ continue;
+ }
+
+ phnd = intrupt->parent_hnd;
+ default_parent = (struct dt_node*)node->base.parent;
+ parent = default_parent;
+
+ if (phnd != PHND_NULL) {
+ parent = dt_resolve_phandle(phnd);
+ }
+
+ if (!parent) {
+ WARN("dtb: (phnd_resolve) malformed phandle: %d", phnd);
+ parent = default_parent;
+ }
+
+ intrupt->parent = &parent->intr;
+
+ __expand_extended_intr(intrupt);
+ }
+}
+
+bool
+dt_load(ptr_t dtb_dropoff)
+{
+ dtctx.reloacted_dtb = dtb_dropoff;
+
+ if (dtctx.fdt->magic != FDT_MAGIC) {
+ ERROR("invalid dtb, unexpected magic: 0x%x", dtctx.fdt->magic);
+ return false;
+ }
+
+ size_t str_off = le(dtctx.fdt->size_dt_strings);
+ dtctx.str_block = offset_t(dtb_dropoff, const char, str_off);
+
+ llist_init_head(&dtctx.nodes);
+ hashtable_init(dtctx.phnds_table);
+
+ struct fdt_iter it;
+ struct fdt_token* tok;
+ struct dt_node_base *node, *prev;
+
+ struct dt_node_base* depth[16];
+ bool is_root_level, filled;
+
+ node = NULL;
+ depth[0] = NULL;
+ fdt_itbegin(&it, dtctx.fdt);
+
+ while (fdt_itnext(&it)) {
+ is_root_level = it.depth == 1;
+
+ if (it.depth >= 16) {
+ // tree too deep
+ ERROR("strange dtb, too deep to dive.");
+ return false;
+ }
+
+ depth[it.depth] = NULL;
+ node = depth[it.depth - 1];
+
+ if (!node) {
+ // need new node
+ if (unlikely(is_root_level)) {
+ node = valloc(sizeof(struct dt_root));
+ __init_node(node);
+ }
+ else {
+ node = valloc(sizeof(struct dt_node));
+ prev = depth[it.depth - 2];
+
+ __init_node_regular((struct dt_node*)node);
+ llist_append(&prev->children, &node->siblings);
+ node->parent = prev;
+
+ llist_append(&dtctx.nodes, &node->nodes);
+ }
+
+ node->name = (const char*)&it.pos[1];
+ }
+
+ if (unlikely(is_root_level)) {
+ __fill_root(&it, (struct dt_root*)node);
+ }
+ else {
+ __fill_node(&it, (struct dt_node*)node);
+ }
+ }
+
+ fdt_itend(&it);
+
+ dtctx.root = (struct dt_root*)depth[0];
+
+ __resolve_phnd_references();
+
+ return true;
+}
+
+static bool
+__name_starts_with(struct dt_node_base* node, const char* name)
+{
+ int i = 0;
+ const char* be_matched = node->name;
+
+ while (be_matched[i] && name[i])
+ {
+ if (be_matched[i] != name[i]) {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+struct dt_node*
+dt_resolve_phandle(dt_phnd_t phandle)
+{
+ struct dt_node_base *pos, *n;
+ hashtable_hash_foreach(dtctx.phnds_table, phandle, pos, n, phnd_link)
+ {
+ if (pos->phandle == phandle) {
+ return (struct dt_node*)pos;
+ }
+ }
+
+ return NULL;
+}
+
+void
+dt_begin_find(struct dt_node_iter* iter,
+ struct dt_node* node, const char* name)
+{
+ node = node ? : (struct dt_node*)dtctx.root;
+
+ iter->head = &node->base;
+ iter->matched = NULL;
+ iter->name = name;
+
+ struct dt_node_base *pos, *n;
+ llist_for_each(pos, n, &node->base.children, siblings)
+ {
+ if (__name_starts_with(pos, name)) {
+ iter->matched = pos;
+ break;
+ }
+ }
+}
+
+bool
+dt_find_next(struct dt_node_iter* iter,
+ struct dt_node_base** matched)
+{
+ if (!dt_found_any(iter)) {
+ return false;
+ }
+
+ struct dt_node_base *pos, *head;
+
+ head = iter->head;
+ pos = iter->matched;
+ *matched = pos;
+
+ while (&pos->siblings != &head->children)
+ {
+ pos = list_next(pos, struct dt_node_base, siblings);
+
+ if (!__name_starts_with(pos, iter->name)) {
+ continue;
+ }
+
+ iter->matched = pos;
+ return true;
+ }
+
+ return false;
+}
+
+struct dt_prop_val*
+dt_getprop(struct dt_node* node, const char* name)
+{
+ struct hstr hashed_name;
+ struct dt_prop *pos, *n;
+ unsigned int hash;
+
+ hashed_name = HSTR(name, strlen(name));
+ hstr_rehash(&hashed_name, HSTR_FULL_HASH);
+ hash = hashed_name.hash;
+
+ hashtable_hash_foreach(node->base._op_bucket, hash, pos, n, ht)
+ {
+ if (HSTR_EQ(&pos->key, &hashed_name)) {
+ return &pos->val;
+ }
+ }
+
+ return NULL;
+}
\ No newline at end of file
git://scm.lunaixsky.com / lunaix-os.git / blobdiff