fdt_sw.c 8.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383
  1. // SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
  2. /*
  3. * libfdt - Flat Device Tree manipulation
  4. * Copyright (C) 2006 David Gibson, IBM Corporation.
  5. */
  6. #include "libfdt_env.h"
  7. #include <fdt.h>
  8. #include <libfdt.h>
  9. #include "libfdt_internal.h"
  10. static int fdt_sw_probe_(void *fdt)
  11. {
  12. if (fdt_chk_basic()) {
  13. if (fdt_magic(fdt) == FDT_MAGIC)
  14. return -FDT_ERR_BADSTATE;
  15. else if (fdt_magic(fdt) != FDT_SW_MAGIC)
  16. return -FDT_ERR_BADMAGIC;
  17. }
  18. return 0;
  19. }
  20. #define FDT_SW_PROBE(fdt) \
  21. { \
  22. int err; \
  23. if (fdt_chk_basic() && (err = fdt_sw_probe_(fdt)) != 0) \
  24. return err; \
  25. }
  26. /* 'memrsv' state: Initial state after fdt_create()
  27. *
  28. * Allowed functions:
  29. * fdt_add_reservmap_entry()
  30. * fdt_finish_reservemap() [moves to 'struct' state]
  31. */
  32. static int fdt_sw_probe_memrsv_(void *fdt)
  33. {
  34. int err = fdt_sw_probe_(fdt);
  35. if (err)
  36. return err;
  37. if (fdt_chk_extra() && fdt_off_dt_strings(fdt) != 0)
  38. return -FDT_ERR_BADSTATE;
  39. return 0;
  40. }
  41. #define FDT_SW_PROBE_MEMRSV(fdt) \
  42. { \
  43. int err; \
  44. if (fdt_chk_extra() && (err = fdt_sw_probe_memrsv_(fdt)) != 0) \
  45. return err; \
  46. }
  47. /* 'struct' state: Enter this state after fdt_finish_reservemap()
  48. *
  49. * Allowed functions:
  50. * fdt_begin_node()
  51. * fdt_end_node()
  52. * fdt_property*()
  53. * fdt_finish() [moves to 'complete' state]
  54. */
  55. static int fdt_sw_probe_struct_(void *fdt)
  56. {
  57. int err;
  58. if (!fdt_chk_extra())
  59. return 0;
  60. err = fdt_sw_probe_(fdt);
  61. if (err)
  62. return err;
  63. if (fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
  64. return -FDT_ERR_BADSTATE;
  65. return 0;
  66. }
  67. #define FDT_SW_PROBE_STRUCT(fdt) \
  68. { \
  69. int err; \
  70. if (fdt_chk_extra() && (err = fdt_sw_probe_struct_(fdt)) != 0) \
  71. return err; \
  72. }
  73. static inline uint32_t sw_flags(void *fdt)
  74. {
  75. /* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */
  76. return fdt_last_comp_version(fdt);
  77. }
  78. /* 'complete' state: Enter this state after fdt_finish()
  79. *
  80. * Allowed functions: none
  81. */
  82. static void *fdt_grab_space_(void *fdt, size_t len)
  83. {
  84. int offset = fdt_size_dt_struct(fdt);
  85. int spaceleft;
  86. spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
  87. - fdt_size_dt_strings(fdt);
  88. if ((offset + len < offset) || (offset + len > spaceleft))
  89. return NULL;
  90. fdt_set_size_dt_struct(fdt, offset + len);
  91. return fdt_offset_ptr_w_(fdt, offset);
  92. }
  93. int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags)
  94. {
  95. const size_t hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
  96. sizeof(struct fdt_reserve_entry));
  97. void *fdt = buf;
  98. if (bufsize < hdrsize)
  99. return -FDT_ERR_NOSPACE;
  100. if (flags & ~FDT_CREATE_FLAGS_ALL)
  101. return -FDT_ERR_BADFLAGS;
  102. memset(buf, 0, bufsize);
  103. /*
  104. * magic and last_comp_version keep intermediate state during the fdt
  105. * creation process, which is replaced with the proper FDT format by
  106. * fdt_finish().
  107. *
  108. * flags should be accessed with sw_flags().
  109. */
  110. fdt_set_magic(fdt, FDT_SW_MAGIC);
  111. fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
  112. fdt_set_last_comp_version(fdt, flags);
  113. fdt_set_totalsize(fdt, bufsize);
  114. fdt_set_off_mem_rsvmap(fdt, hdrsize);
  115. fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
  116. fdt_set_off_dt_strings(fdt, 0);
  117. return 0;
  118. }
  119. int fdt_create(void *buf, int bufsize)
  120. {
  121. return fdt_create_with_flags(buf, bufsize, 0);
  122. }
  123. int fdt_resize(void *fdt, void *buf, int bufsize)
  124. {
  125. size_t headsize, tailsize;
  126. char *oldtail, *newtail;
  127. FDT_SW_PROBE(fdt);
  128. headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
  129. tailsize = fdt_size_dt_strings(fdt);
  130. if (fdt_chk_extra() && (headsize + tailsize) > fdt_totalsize(fdt))
  131. return -FDT_ERR_INTERNAL;
  132. if ((headsize + tailsize) > bufsize)
  133. return -FDT_ERR_NOSPACE;
  134. oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
  135. newtail = (char *)buf + bufsize - tailsize;
  136. /* Two cases to avoid clobbering data if the old and new
  137. * buffers partially overlap */
  138. if (buf <= fdt) {
  139. memmove(buf, fdt, headsize);
  140. memmove(newtail, oldtail, tailsize);
  141. } else {
  142. memmove(newtail, oldtail, tailsize);
  143. memmove(buf, fdt, headsize);
  144. }
  145. fdt_set_totalsize(buf, bufsize);
  146. if (fdt_off_dt_strings(buf))
  147. fdt_set_off_dt_strings(buf, bufsize);
  148. return 0;
  149. }
  150. int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
  151. {
  152. struct fdt_reserve_entry *re;
  153. int offset;
  154. FDT_SW_PROBE_MEMRSV(fdt);
  155. offset = fdt_off_dt_struct(fdt);
  156. if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
  157. return -FDT_ERR_NOSPACE;
  158. re = (struct fdt_reserve_entry *)((char *)fdt + offset);
  159. re->address = cpu_to_fdt64(addr);
  160. re->size = cpu_to_fdt64(size);
  161. fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
  162. return 0;
  163. }
  164. int fdt_finish_reservemap(void *fdt)
  165. {
  166. int err = fdt_add_reservemap_entry(fdt, 0, 0);
  167. if (err)
  168. return err;
  169. fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
  170. return 0;
  171. }
  172. int fdt_begin_node(void *fdt, const char *name)
  173. {
  174. struct fdt_node_header *nh;
  175. int namelen;
  176. FDT_SW_PROBE_STRUCT(fdt);
  177. namelen = strlen(name) + 1;
  178. nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
  179. if (! nh)
  180. return -FDT_ERR_NOSPACE;
  181. nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
  182. memcpy(nh->name, name, namelen);
  183. return 0;
  184. }
  185. int fdt_end_node(void *fdt)
  186. {
  187. fdt32_t *en;
  188. FDT_SW_PROBE_STRUCT(fdt);
  189. en = fdt_grab_space_(fdt, FDT_TAGSIZE);
  190. if (! en)
  191. return -FDT_ERR_NOSPACE;
  192. *en = cpu_to_fdt32(FDT_END_NODE);
  193. return 0;
  194. }
  195. static int fdt_add_string_(void *fdt, const char *s)
  196. {
  197. char *strtab = (char *)fdt + fdt_totalsize(fdt);
  198. int strtabsize = fdt_size_dt_strings(fdt);
  199. int len = strlen(s) + 1;
  200. int struct_top, offset;
  201. offset = -strtabsize - len;
  202. struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
  203. if (fdt_totalsize(fdt) + offset < struct_top)
  204. return 0; /* no more room :( */
  205. memcpy(strtab + offset, s, len);
  206. fdt_set_size_dt_strings(fdt, strtabsize + len);
  207. return offset;
  208. }
  209. /* Must only be used to roll back in case of error */
  210. static void fdt_del_last_string_(void *fdt, const char *s)
  211. {
  212. int strtabsize = fdt_size_dt_strings(fdt);
  213. int len = strlen(s) + 1;
  214. fdt_set_size_dt_strings(fdt, strtabsize - len);
  215. }
  216. static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
  217. {
  218. char *strtab = (char *)fdt + fdt_totalsize(fdt);
  219. int strtabsize = fdt_size_dt_strings(fdt);
  220. const char *p;
  221. *allocated = 0;
  222. p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
  223. if (p)
  224. return p - strtab;
  225. *allocated = 1;
  226. return fdt_add_string_(fdt, s);
  227. }
  228. int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
  229. {
  230. struct fdt_property *prop;
  231. int nameoff;
  232. int allocated;
  233. FDT_SW_PROBE_STRUCT(fdt);
  234. /* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
  235. if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
  236. allocated = 1;
  237. nameoff = fdt_add_string_(fdt, name);
  238. } else {
  239. nameoff = fdt_find_add_string_(fdt, name, &allocated);
  240. }
  241. if (nameoff == 0)
  242. return -FDT_ERR_NOSPACE;
  243. prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
  244. if (! prop) {
  245. if (allocated)
  246. fdt_del_last_string_(fdt, name);
  247. return -FDT_ERR_NOSPACE;
  248. }
  249. prop->tag = cpu_to_fdt32(FDT_PROP);
  250. prop->nameoff = cpu_to_fdt32(nameoff);
  251. prop->len = cpu_to_fdt32(len);
  252. *valp = prop->data;
  253. return 0;
  254. }
  255. int fdt_property(void *fdt, const char *name, const void *val, int len)
  256. {
  257. void *ptr;
  258. int ret;
  259. ret = fdt_property_placeholder(fdt, name, len, &ptr);
  260. if (ret)
  261. return ret;
  262. memcpy(ptr, val, len);
  263. return 0;
  264. }
  265. int fdt_finish(void *fdt)
  266. {
  267. char *p = (char *)fdt;
  268. fdt32_t *end;
  269. int oldstroffset, newstroffset;
  270. uint32_t tag;
  271. int offset, nextoffset;
  272. FDT_SW_PROBE_STRUCT(fdt);
  273. /* Add terminator */
  274. end = fdt_grab_space_(fdt, sizeof(*end));
  275. if (! end)
  276. return -FDT_ERR_NOSPACE;
  277. *end = cpu_to_fdt32(FDT_END);
  278. /* Relocate the string table */
  279. oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
  280. newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
  281. memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
  282. fdt_set_off_dt_strings(fdt, newstroffset);
  283. /* Walk the structure, correcting string offsets */
  284. offset = 0;
  285. while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
  286. if (tag == FDT_PROP) {
  287. struct fdt_property *prop =
  288. fdt_offset_ptr_w_(fdt, offset);
  289. int nameoff;
  290. nameoff = fdt32_to_cpu(prop->nameoff);
  291. nameoff += fdt_size_dt_strings(fdt);
  292. prop->nameoff = cpu_to_fdt32(nameoff);
  293. }
  294. offset = nextoffset;
  295. }
  296. if (nextoffset < 0)
  297. return nextoffset;
  298. /* Finally, adjust the header */
  299. fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
  300. /* And fix up fields that were keeping intermediate state. */
  301. fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
  302. fdt_set_magic(fdt, FDT_MAGIC);
  303. return 0;
  304. }