fdt_sw.c 8.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386
  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. unsigned int offset = fdt_size_dt_struct(fdt);
  85. unsigned 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 int 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. if (bufsize < 0)
  129. return -FDT_ERR_NOSPACE;
  130. headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
  131. tailsize = fdt_size_dt_strings(fdt);
  132. if (fdt_chk_extra() && (headsize + tailsize) > fdt_totalsize(fdt))
  133. return -FDT_ERR_INTERNAL;
  134. if ((headsize + tailsize) > (unsigned)bufsize)
  135. return -FDT_ERR_NOSPACE;
  136. oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
  137. newtail = (char *)buf + bufsize - tailsize;
  138. /* Two cases to avoid clobbering data if the old and new
  139. * buffers partially overlap */
  140. if (buf <= fdt) {
  141. memmove(buf, fdt, headsize);
  142. memmove(newtail, oldtail, tailsize);
  143. } else {
  144. memmove(newtail, oldtail, tailsize);
  145. memmove(buf, fdt, headsize);
  146. }
  147. fdt_set_totalsize(buf, bufsize);
  148. if (fdt_off_dt_strings(buf))
  149. fdt_set_off_dt_strings(buf, bufsize);
  150. return 0;
  151. }
  152. int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
  153. {
  154. struct fdt_reserve_entry *re;
  155. int offset;
  156. FDT_SW_PROBE_MEMRSV(fdt);
  157. offset = fdt_off_dt_struct(fdt);
  158. if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
  159. return -FDT_ERR_NOSPACE;
  160. re = (struct fdt_reserve_entry *)((char *)fdt + offset);
  161. re->address = cpu_to_fdt64(addr);
  162. re->size = cpu_to_fdt64(size);
  163. fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
  164. return 0;
  165. }
  166. int fdt_finish_reservemap(void *fdt)
  167. {
  168. int err = fdt_add_reservemap_entry(fdt, 0, 0);
  169. if (err)
  170. return err;
  171. fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
  172. return 0;
  173. }
  174. int fdt_begin_node(void *fdt, const char *name)
  175. {
  176. struct fdt_node_header *nh;
  177. int namelen;
  178. FDT_SW_PROBE_STRUCT(fdt);
  179. namelen = strlen(name) + 1;
  180. nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
  181. if (! nh)
  182. return -FDT_ERR_NOSPACE;
  183. nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
  184. memcpy(nh->name, name, namelen);
  185. return 0;
  186. }
  187. int fdt_end_node(void *fdt)
  188. {
  189. fdt32_t *en;
  190. FDT_SW_PROBE_STRUCT(fdt);
  191. en = fdt_grab_space_(fdt, FDT_TAGSIZE);
  192. if (! en)
  193. return -FDT_ERR_NOSPACE;
  194. *en = cpu_to_fdt32(FDT_END_NODE);
  195. return 0;
  196. }
  197. static int fdt_add_string_(void *fdt, const char *s)
  198. {
  199. char *strtab = (char *)fdt + fdt_totalsize(fdt);
  200. unsigned int strtabsize = fdt_size_dt_strings(fdt);
  201. unsigned int len = strlen(s) + 1;
  202. unsigned int struct_top, offset;
  203. offset = strtabsize + len;
  204. struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
  205. if (fdt_totalsize(fdt) - offset < struct_top)
  206. return 0; /* no more room :( */
  207. memcpy(strtab - offset, s, len);
  208. fdt_set_size_dt_strings(fdt, strtabsize + len);
  209. return -offset;
  210. }
  211. /* Must only be used to roll back in case of error */
  212. static void fdt_del_last_string_(void *fdt, const char *s)
  213. {
  214. int strtabsize = fdt_size_dt_strings(fdt);
  215. int len = strlen(s) + 1;
  216. fdt_set_size_dt_strings(fdt, strtabsize - len);
  217. }
  218. static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
  219. {
  220. char *strtab = (char *)fdt + fdt_totalsize(fdt);
  221. int strtabsize = fdt_size_dt_strings(fdt);
  222. const char *p;
  223. *allocated = 0;
  224. p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
  225. if (p)
  226. return p - strtab;
  227. *allocated = 1;
  228. return fdt_add_string_(fdt, s);
  229. }
  230. int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
  231. {
  232. struct fdt_property *prop;
  233. int nameoff;
  234. int allocated;
  235. FDT_SW_PROBE_STRUCT(fdt);
  236. /* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
  237. if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
  238. allocated = 1;
  239. nameoff = fdt_add_string_(fdt, name);
  240. } else {
  241. nameoff = fdt_find_add_string_(fdt, name, &allocated);
  242. }
  243. if (nameoff == 0)
  244. return -FDT_ERR_NOSPACE;
  245. prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
  246. if (! prop) {
  247. if (allocated)
  248. fdt_del_last_string_(fdt, name);
  249. return -FDT_ERR_NOSPACE;
  250. }
  251. prop->tag = cpu_to_fdt32(FDT_PROP);
  252. prop->nameoff = cpu_to_fdt32(nameoff);
  253. prop->len = cpu_to_fdt32(len);
  254. *valp = prop->data;
  255. return 0;
  256. }
  257. int fdt_property(void *fdt, const char *name, const void *val, int len)
  258. {
  259. void *ptr;
  260. int ret;
  261. ret = fdt_property_placeholder(fdt, name, len, &ptr);
  262. if (ret)
  263. return ret;
  264. memcpy(ptr, val, len);
  265. return 0;
  266. }
  267. int fdt_finish(void *fdt)
  268. {
  269. char *p = (char *)fdt;
  270. fdt32_t *end;
  271. int oldstroffset, newstroffset;
  272. uint32_t tag;
  273. int offset, nextoffset;
  274. FDT_SW_PROBE_STRUCT(fdt);
  275. /* Add terminator */
  276. end = fdt_grab_space_(fdt, sizeof(*end));
  277. if (! end)
  278. return -FDT_ERR_NOSPACE;
  279. *end = cpu_to_fdt32(FDT_END);
  280. /* Relocate the string table */
  281. oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
  282. newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
  283. memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
  284. fdt_set_off_dt_strings(fdt, newstroffset);
  285. /* Walk the structure, correcting string offsets */
  286. offset = 0;
  287. while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
  288. if (tag == FDT_PROP) {
  289. struct fdt_property *prop =
  290. fdt_offset_ptr_w_(fdt, offset);
  291. int nameoff;
  292. nameoff = fdt32_to_cpu(prop->nameoff);
  293. nameoff += fdt_size_dt_strings(fdt);
  294. prop->nameoff = cpu_to_fdt32(nameoff);
  295. }
  296. offset = nextoffset;
  297. }
  298. if (nextoffset < 0)
  299. return nextoffset;
  300. /* Finally, adjust the header */
  301. fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
  302. /* And fix up fields that were keeping intermediate state. */
  303. fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
  304. fdt_set_magic(fdt, FDT_MAGIC);
  305. return 0;
  306. }