tinyfdt.c 5.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280
  1. /*
  2. * SPDX-License-Identifier: BSD-2-Clause
  3. *
  4. * Copyright (c) 2019 Western Digital Corporation or its affiliates.
  5. *
  6. * Authors:
  7. * Anup Patel <anup.patel@wdc.com>
  8. */
  9. #include <plat/string.h>
  10. #include <plat/tinyfdt.h>
  11. #define FDT_MAGIC 0xd00dfeed
  12. #define FDT_VERSION 17
  13. struct fdt_header {
  14. u32 magic;
  15. u32 totalsize;
  16. u32 off_dt_struct;
  17. u32 off_dt_strings;
  18. u32 off_mem_rsvmap;
  19. u32 version;
  20. u32 last_comp_version; /* <= 17 */
  21. u32 boot_cpuid_phys;
  22. u32 size_dt_strings;
  23. u32 size_dt_struct;
  24. } __attribute__((packed));
  25. #define FDT_BEGIN_NODE 1
  26. #define FDT_END_NODE 2
  27. #define FDT_PROP 3
  28. #define FDT_NOP 4
  29. #define FDT_END 9
  30. u32 fdt_rev32(u32 v)
  31. {
  32. return ((v & 0x000000FF) << 24) | ((v & 0x0000FF00) << 8) |
  33. ((v & 0x00FF0000) >> 8) | ((v & 0xFF000000) >> 24);
  34. }
  35. int fdt_prop_string_index(const struct fdt_prop *prop, const char *str)
  36. {
  37. int i;
  38. ulong l = 0;
  39. const char *p, *end;
  40. p = prop->value;
  41. end = p + prop->len;
  42. for (i = 0; p < end; i++, p += l) {
  43. l = strlen(p) + 1;
  44. if (p + l > end)
  45. return -1;
  46. if (strcmp(str, p) == 0)
  47. return i; /* Found it; return index */
  48. }
  49. return -1;
  50. }
  51. struct recursive_iter_info {
  52. void (*fn)(const struct fdt_node *node, const struct fdt_prop *prop,
  53. void *priv);
  54. void *fn_priv;
  55. const char *str;
  56. };
  57. #define DATA32(ptr) fdt_rev32(*((u32 *)ptr))
  58. static void recursive_iter(char **data, struct recursive_iter_info *info,
  59. const struct fdt_node *parent)
  60. {
  61. struct fdt_node node;
  62. struct fdt_prop prop;
  63. if (DATA32(*data) != FDT_BEGIN_NODE)
  64. return;
  65. node.data = *data;
  66. (*data) += sizeof(u32);
  67. node.parent = parent;
  68. node.name = *data;
  69. *data += strlen(*data) + 1;
  70. while ((ulong)(*data) % sizeof(u32) != 0)
  71. (*data)++;
  72. node.depth = (parent) ? (parent->depth + 1) : 1;
  73. /* Default cell counts, as per the FDT spec */
  74. node.address_cells = 2;
  75. node.size_cells = 1;
  76. info->fn(&node, NULL, info->fn_priv);
  77. while (DATA32(*data) != FDT_END_NODE) {
  78. switch (DATA32(*data)) {
  79. case FDT_PROP:
  80. prop.node = &node;
  81. *data += sizeof(u32);
  82. prop.len = DATA32(*data);
  83. *data += sizeof(u32);
  84. prop.name = &info->str[DATA32(*data)];
  85. *data += sizeof(u32);
  86. prop.value = *data;
  87. *data += prop.len;
  88. while ((ulong)(*data) % sizeof(u32) != 0)
  89. (*data)++;
  90. info->fn(&node, &prop, info->fn_priv);
  91. break;
  92. case FDT_NOP:
  93. *data += sizeof(u32);
  94. break;
  95. case FDT_BEGIN_NODE:
  96. recursive_iter(data, info, &node);
  97. break;
  98. default:
  99. return;
  100. };
  101. }
  102. *data += sizeof(u32);
  103. }
  104. struct match_iter_info {
  105. int (*match)(const struct fdt_node *node, const struct fdt_prop *prop,
  106. void *priv);
  107. void *match_priv;
  108. void (*fn)(const struct fdt_node *node, const struct fdt_prop *prop,
  109. void *priv);
  110. void *fn_priv;
  111. const char *str;
  112. };
  113. static void match_iter(const struct fdt_node *node, const struct fdt_prop *prop,
  114. void *priv)
  115. {
  116. char *data;
  117. struct match_iter_info *minfo = priv;
  118. struct fdt_prop nprop;
  119. /* Do nothing if node+prop dont match */
  120. if (!minfo->match(node, prop, minfo->match_priv))
  121. return;
  122. /* Call function for node */
  123. if (minfo->fn)
  124. minfo->fn(node, NULL, minfo->fn_priv);
  125. /* Convert node to character stream */
  126. data = node->data;
  127. data += sizeof(u32);
  128. /* Skip node name */
  129. data += strlen(data) + 1;
  130. while ((ulong)(data) % sizeof(u32) != 0)
  131. data++;
  132. /* Find node property and its value */
  133. while (DATA32(data) == FDT_PROP) {
  134. nprop.node = node;
  135. data += sizeof(u32);
  136. nprop.len = DATA32(data);
  137. data += sizeof(u32);
  138. nprop.name = &minfo->str[DATA32(data)];
  139. data += sizeof(u32);
  140. nprop.value = data;
  141. data += nprop.len;
  142. while ((ulong)(data) % sizeof(u32) != 0)
  143. (data)++;
  144. /* Call function for every property */
  145. if (minfo->fn)
  146. minfo->fn(node, &nprop, minfo->fn_priv);
  147. }
  148. }
  149. int fdt_match_node_prop(void *fdt,
  150. int (*match)(const struct fdt_node *node,
  151. const struct fdt_prop *prop, void *priv),
  152. void *match_priv,
  153. void (*fn)(const struct fdt_node *node,
  154. const struct fdt_prop *prop, void *priv),
  155. void *fn_priv)
  156. {
  157. char *data;
  158. u32 string_offset, data_offset;
  159. struct fdt_header *header;
  160. struct match_iter_info minfo;
  161. struct recursive_iter_info rinfo;
  162. if (!fdt || !match)
  163. return -1;
  164. header = fdt;
  165. if (fdt_rev32(header->magic) != FDT_MAGIC ||
  166. fdt_rev32(header->last_comp_version) > FDT_VERSION)
  167. return -1;
  168. string_offset = fdt_rev32(header->off_dt_strings);
  169. data_offset = fdt_rev32(header->off_dt_struct);
  170. minfo.match = match;
  171. minfo.match_priv = match_priv;
  172. minfo.fn = fn;
  173. minfo.fn_priv = fn_priv;
  174. minfo.str = (const char *)(fdt + string_offset);
  175. rinfo.fn = match_iter;
  176. rinfo.fn_priv = &minfo;
  177. rinfo.str = minfo.str;
  178. data = (char *)(fdt + data_offset);
  179. recursive_iter(&data, &rinfo, NULL);
  180. return 0;
  181. }
  182. struct match_compat_info {
  183. const char *compat;
  184. };
  185. static int match_compat(const struct fdt_node *node,
  186. const struct fdt_prop *prop, void *priv)
  187. {
  188. struct match_compat_info *cinfo = priv;
  189. if (!prop)
  190. return 0;
  191. if (strcmp(prop->name, "compatible"))
  192. return 0;
  193. if (fdt_prop_string_index(prop, cinfo->compat) < 0)
  194. return 0;
  195. return 1;
  196. }
  197. int fdt_compat_node_prop(void *fdt, const char *compat,
  198. void (*fn)(const struct fdt_node *node,
  199. const struct fdt_prop *prop, void *priv),
  200. void *fn_priv)
  201. {
  202. struct match_compat_info cinfo = { .compat = compat };
  203. return fdt_match_node_prop(fdt, match_compat, &cinfo, fn, fn_priv);
  204. }
  205. static int match_walk(const struct fdt_node *node, const struct fdt_prop *prop,
  206. void *priv)
  207. {
  208. if (!prop)
  209. return 1;
  210. return 0;
  211. }
  212. int fdt_walk(void *fdt,
  213. void (*fn)(const struct fdt_node *node,
  214. const struct fdt_prop *prop, void *priv),
  215. void *fn_priv)
  216. {
  217. return fdt_match_node_prop(fdt, match_walk, NULL, fn, fn_priv);
  218. }
  219. u32 fdt_size(void *fdt)
  220. {
  221. struct fdt_header *header;
  222. if (!fdt)
  223. return 0;
  224. header = fdt;
  225. if (fdt_rev32(header->magic) != FDT_MAGIC ||
  226. fdt_rev32(header->last_comp_version) > FDT_VERSION)
  227. return 0;
  228. return fdt_rev32(header->totalsize);
  229. }