arch/arm-raspi/boot/devicetree.c

   1 #include <aros/macros.h>
   2 #include "devicetree.h"
   3 #include "boot.h"
   4 #include <stdlib.h>
   5 #include <string.h>
   6
   7 #define D(x)
   8
   9 of_node_t *root = NULL;
  10 uint32_t *data;
  11 char *strings;
  12
  13 of_node_t * dt_build_node(of_node_t *parent)
  14 {
  15     of_node_t *e = malloc(sizeof(of_node_t));
  16
  17     if (e != NULL)
  18     {
  19         if (parent != NULL)
  20         {
  21             ADDTAIL(&parent->on_children, e);
  22         }
  23         NEWLIST(&e->on_children);
  24         NEWLIST(&e->on_properties);
  25         e->on_name = (char *)data;
  26         data += (strlen((char *)data) + 4) / 4;
  27
  28         D(kprintf("[BOOT] new node %s\n", e->on_name));
  29
  30         while(1)
  31         {
  32             switch (AROS_BE2LONG(*data++))
  33             {
  34                 case FDT_BEGIN_NODE:
  35                 {
  36                     dt_build_node(e);
  37                     break;
  38                 }
  39
  40                 case FDT_PROP:
  41                 {
  42                     of_property_t *p = malloc(sizeof(of_property_t));
  43                     p->op_length = AROS_BE2LONG(*data++);
  44                     p->op_name = &strings[AROS_BE2LONG(*data++)];
  45                     if (p->op_length)
  46                         p->op_value = data;
  47                     else
  48                         p->op_value = NULL;
  49                     ADDTAIL(&e->on_properties, p);
  50                     data += (p->op_length + 3)/4;
  51                     D(kprintf("[BOOT] prop %s with length %d\n", p->op_name, p->op_length));
  52                     break;
  53                 }
  54
  55                 case FDT_NOP:
  56                     break;
  57
  58                 case FDT_END_NODE:
  59                     return e;
  60             }
  61         }
  62     }
  63     return e;
  64 }
  65
  66 static struct fdt_header *hdr;
  67
  68 long dt_total_size()
  69 {
  70     if (hdr != NULL)
  71         return AROS_BE2LONG(hdr->totalsize);
  72     else
  73         return 0;
  74 }
  75
  76 of_node_t * dt_parse(void *dt)
  77 {
  78     uint32_t token = 0;
  79
  80     hdr = dt;
  81
  82     D(kprintf("[BOOT] Checking device tree at %p\n", hdr));
  83     D(kprintf("[BOOT] magic=%08x\n", AROS_LONG2BE(hdr->magic)));
  84
  85     if (hdr->magic == AROS_LONG2BE(FDT_MAGIC))
  86     {
  87         D(kprintf("[BOOT] Appears to be a valid device tree\n"));
  88         D(kprintf("[BOOT] size=%d\n", AROS_BE2LONG(hdr->totalsize)));
  89         D(kprintf("[BOOT] off_dt_struct=%d\n", AROS_BE2LONG(hdr->off_dt_struct)));
  90         D(kprintf("[BOOT] off_dt_strings=%d\n", AROS_BE2LONG(hdr->off_dt_strings)));
  91         D(kprintf("[BOOT] off_mem_rsvmap=%d\n", AROS_BE2LONG(hdr->off_mem_rsvmap)));
  92
  93         strings = dt + AROS_BE2LONG(hdr->off_dt_strings);
  94         data = dt + AROS_BE2LONG(hdr->off_dt_struct);
  95
  96         if (hdr->off_mem_rsvmap)
  97         {
  98             struct fdt_reserve_entry *rsrvd = dt + AROS_BE2LONG(hdr->off_mem_rsvmap);
  99
 100             while (rsrvd->address != 0 || rsrvd->size != 0) {
 101                 D(kprintf("[BOOT]   reserved: %08x-%08x\n",
 102                     (uint32_t)AROS_BE2QUAD(rsrvd->address),
 103                     (uint32_t)(AROS_BE2QUAD(rsrvd->address) + AROS_BE2QUAD(rsrvd->size - 1)))
 104                 );
 105                 rsrvd++;
 106             }
 107         }
 108
 109         do
 110         {
 111             token = AROS_BE2LONG(*data++);
 112
 113             switch (token)
 114             {
 115                 case FDT_BEGIN_NODE:
 116                     root = dt_build_node(NULL);
 117                     break;
 118                 case FDT_PROP:
 119                 {
 120                     kprintf("[BOOT] Property outside root node?");
 121                     break;
 122                 }
 123             }
 124         } while (token != FDT_END);
 125     }
 126     else
 127     {
 128         hdr = NULL;
 129     }
 130
 131     return root;
 132 }
 133
 134 static of_node_t * dt_find_by_phandle(uint32_t phandle, of_node_t *root)
 135 {
 136     of_property_t *p = dt_find_property(root, "phandle");
 137
 138     if (p && *((uint32_t *)p->op_value) == AROS_LONG2BE(phandle))
 139         return root;
 140     else {
 141         of_node_t *c;
 142         ForeachNode(&root->on_children, c)
 143         {
 144             of_node_t *found = dt_find_by_phandle(phandle, c);
 145             if (found)
 146                 return found;
 147         }
 148     }
 149     return NULL;
 150 }
 151
 152 of_node_t * dt_find_node_by_phandle(uint32_t phandle)
 153 {
 154     return dt_find_by_phandle(phandle, root);
 155 }
 156
 157 #define MAX_KEY_SIZE    64
 158 char ptrbuf[64];
 159
 160 of_node_t * dt_find_node(char *key)
 161 {
 162     int i;
 163     of_node_t *node, *ret = NULL;
 164
 165     if (*key == '/')
 166     {
 167         ret = root;
 168
 169         while(*key)
 170         {
 171             key++;
 172             for (i=0; i < 63; i++)
 173             {
 174                 if (*key == '/' || *key == 0)
 175                     break;
 176                 ptrbuf[i] = *key;
 177                 key++;
 178             }
 179
 180             ptrbuf[i] = 0;
 181
 182             ForeachNode(&ret->on_children, node)
 183             {
 184                 if (!strcmp(node->on_name, ptrbuf))
 185                 {
 186                     ret = node;
 187                     break;
 188                 }
 189             }
 190         }
 191     }
 192
 193     return ret;
 194 }
 195
 196 of_property_t *dt_find_property(void *key, char *propname)
 197 {
 198     of_node_t *node = (of_node_t *)key;
 199     of_property_t *p, *prop = NULL;
 200
 201     if (node)
 202     {
 203         ForeachNode(&node->on_properties, p)
 204         {
 205             if (!strcmp(p->op_name, propname))
 206             {
 207                 prop = p;
 208                 break;
 209             }
 210         }
 211     }
 212     return prop;
 213 }
 214
 215 char fill[] = "                         ";
 216
 217 void dt_dump_node(of_node_t *n, int level)
 218 {
 219     of_property_t *p;
 220     of_node_t *c;
 221
 222     kprintf("[BOOT] %s%s\n", &fill[25-2*level], n->on_name);
 223     ForeachNode(&n->on_properties, p)
 224     {
 225         kprintf("[BOOT] %s  %s=", &fill[25-2*level], p->op_name);
 226         for (int i=0; i < p->op_length; i++) {
 227             char *pchar = p->op_value + i;
 228             if (*pchar >= ' ' && *pchar <= 'z')
 229                 kprintf("%c", *pchar);
 230             else
 231                 kprintf(".");
 232         }
 233
 234         if (p->op_length) {
 235             kprintf(" (");
 236             int max = 16;
 237             if (max > p->op_length)
 238                 max = p->op_length;
 239
 240             for (int i=0; i < p->op_length; i++) {
 241                 char *pchar = p->op_value + i;
 242                 kprintf("%02x", *pchar);
 243             }
 244             kprintf(")");
 245         }
 246         kprintf("\n");
 247     }
 248
 249     ForeachNode(&n->on_children, c)
 250     {
 251         dt_dump_node(c, level+1);
 252     }
 253 }
 254
 255 void dt_dump_tree()
 256 {
 257     kprintf("[BOOT] Device Tree dump:\n");
 258
 259     dt_dump_node(root, 0);
 260 }