fdt_rw.c 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686
  1. /*
  2. * libfdt - Flat Device Tree manipulation
  3. * Copyright (C) 2006 David Gibson, IBM Corporation.
  4. *
  5. * libfdt is dual licensed: you can use it either under the terms of
  6. * the GPL, or the BSD license, at your option.
  7. *
  8. * a) This library is free software; you can redistribute it and/or
  9. * modify it under the terms of the GNU General Public License as
  10. * published by the Free Software Foundation; either version 2 of the
  11. * License, or (at your option) any later version.
  12. *
  13. * This library is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public
  19. * License along with this library; if not, write to the Free
  20. * Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
  21. * MA 02110-1301 USA
  22. *
  23. * Alternatively,
  24. *
  25. * b) Redistribution and use in source and binary forms, with or
  26. * without modification, are permitted provided that the following
  27. * conditions are met:
  28. *
  29. * 1. Redistributions of source code must retain the above
  30. * copyright notice, this list of conditions and the following
  31. * disclaimer.
  32. * 2. Redistributions in binary form must reproduce the above
  33. * copyright notice, this list of conditions and the following
  34. * disclaimer in the documentation and/or other materials
  35. * provided with the distribution.
  36. *
  37. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  38. * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
  39. * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  40. * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  41. * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
  42. * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  43. * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  44. * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  45. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  46. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  47. * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  48. * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  49. * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  50. */
  51. #include "libfdt_env.h"
  52. #include <fdt.h>
  53. #include <libfdt.h>
  54. #include "libfdt_internal.h"
  55. static int
  56. _fdt_blocks_misordered (
  57. const void *fdt,
  58. int mem_rsv_size,
  59. int struct_size
  60. )
  61. {
  62. return (fdt_off_mem_rsvmap (fdt) < FDT_ALIGN (sizeof (struct fdt_header), 8))
  63. || (fdt_off_dt_struct (fdt) <
  64. (fdt_off_mem_rsvmap (fdt) + mem_rsv_size))
  65. || (fdt_off_dt_strings (fdt) <
  66. (fdt_off_dt_struct (fdt) + struct_size))
  67. || (fdt_totalsize (fdt) <
  68. (fdt_off_dt_strings (fdt) + fdt_size_dt_strings (fdt)));
  69. }
  70. static int
  71. _fdt_rw_check_header (
  72. void *fdt
  73. )
  74. {
  75. FDT_CHECK_HEADER (fdt);
  76. if (fdt_version (fdt) < 17) {
  77. return -FDT_ERR_BADVERSION;
  78. }
  79. if (_fdt_blocks_misordered (
  80. fdt,
  81. sizeof (struct fdt_reserve_entry),
  82. fdt_size_dt_struct (fdt)
  83. ))
  84. {
  85. return -FDT_ERR_BADLAYOUT;
  86. }
  87. if (fdt_version (fdt) > 17) {
  88. fdt_set_version (fdt, 17);
  89. }
  90. return 0;
  91. }
  92. #define FDT_RW_CHECK_HEADER(fdt) \
  93. { \
  94. int __err; \
  95. if ((__err = _fdt_rw_check_header(fdt)) != 0) \
  96. return __err; \
  97. }
  98. static inline int
  99. _fdt_data_size (
  100. void *fdt
  101. )
  102. {
  103. return fdt_off_dt_strings (fdt) + fdt_size_dt_strings (fdt);
  104. }
  105. static int
  106. _fdt_splice (
  107. void *fdt,
  108. void *splicepoint,
  109. int oldlen,
  110. int newlen
  111. )
  112. {
  113. char *p = splicepoint;
  114. char *end = (char *)fdt + _fdt_data_size (fdt);
  115. if (((p + oldlen) < p) || ((p + oldlen) > end)) {
  116. return -FDT_ERR_BADOFFSET;
  117. }
  118. if ((p < (char *)fdt) || ((end - oldlen + newlen) < (char *)fdt)) {
  119. return -FDT_ERR_BADOFFSET;
  120. }
  121. if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize (fdt))) {
  122. return -FDT_ERR_NOSPACE;
  123. }
  124. memmove (p + newlen, p + oldlen, end - p - oldlen);
  125. return 0;
  126. }
  127. static int
  128. _fdt_splice_mem_rsv (
  129. void *fdt,
  130. struct fdt_reserve_entry *p,
  131. int oldn,
  132. int newn
  133. )
  134. {
  135. int delta = (newn - oldn) * sizeof (*p);
  136. int err;
  137. err = _fdt_splice (fdt, p, oldn * sizeof (*p), newn * sizeof (*p));
  138. if (err) {
  139. return err;
  140. }
  141. fdt_set_off_dt_struct (fdt, fdt_off_dt_struct (fdt) + delta);
  142. fdt_set_off_dt_strings (fdt, fdt_off_dt_strings (fdt) + delta);
  143. return 0;
  144. }
  145. static int
  146. _fdt_splice_struct (
  147. void *fdt,
  148. void *p,
  149. int oldlen,
  150. int newlen
  151. )
  152. {
  153. int delta = newlen - oldlen;
  154. int err;
  155. if ((err = _fdt_splice (fdt, p, oldlen, newlen))) {
  156. return err;
  157. }
  158. fdt_set_size_dt_struct (fdt, fdt_size_dt_struct (fdt) + delta);
  159. fdt_set_off_dt_strings (fdt, fdt_off_dt_strings (fdt) + delta);
  160. return 0;
  161. }
  162. static int
  163. _fdt_splice_string (
  164. void *fdt,
  165. int newlen
  166. )
  167. {
  168. void *p = (char *)fdt
  169. + fdt_off_dt_strings (fdt) + fdt_size_dt_strings (fdt);
  170. int err;
  171. if ((err = _fdt_splice (fdt, p, 0, newlen))) {
  172. return err;
  173. }
  174. fdt_set_size_dt_strings (fdt, fdt_size_dt_strings (fdt) + newlen);
  175. return 0;
  176. }
  177. static int
  178. _fdt_find_add_string (
  179. void *fdt,
  180. const char *s
  181. )
  182. {
  183. char *strtab = (char *)fdt + fdt_off_dt_strings (fdt);
  184. const char *p;
  185. char *new;
  186. int len = strlen (s) + 1;
  187. int err;
  188. p = _fdt_find_string (strtab, fdt_size_dt_strings (fdt), s);
  189. if (p) {
  190. /* found it */
  191. return (p - strtab);
  192. }
  193. new = strtab + fdt_size_dt_strings (fdt);
  194. err = _fdt_splice_string (fdt, len);
  195. if (err) {
  196. return err;
  197. }
  198. memcpy (new, s, len);
  199. return (new - strtab);
  200. }
  201. int
  202. fdt_add_mem_rsv (
  203. void *fdt,
  204. uint64_t address,
  205. uint64_t size
  206. )
  207. {
  208. struct fdt_reserve_entry *re;
  209. int err;
  210. FDT_RW_CHECK_HEADER (fdt);
  211. re = _fdt_mem_rsv_w (fdt, fdt_num_mem_rsv (fdt));
  212. err = _fdt_splice_mem_rsv (fdt, re, 0, 1);
  213. if (err) {
  214. return err;
  215. }
  216. re->address = cpu_to_fdt64 (address);
  217. re->size = cpu_to_fdt64 (size);
  218. return 0;
  219. }
  220. int
  221. fdt_del_mem_rsv (
  222. void *fdt,
  223. int n
  224. )
  225. {
  226. struct fdt_reserve_entry *re = _fdt_mem_rsv_w (fdt, n);
  227. FDT_RW_CHECK_HEADER (fdt);
  228. if (n >= fdt_num_mem_rsv (fdt)) {
  229. return -FDT_ERR_NOTFOUND;
  230. }
  231. return _fdt_splice_mem_rsv (fdt, re, 1, 0);
  232. }
  233. static int
  234. _fdt_resize_property (
  235. void *fdt,
  236. int nodeoffset,
  237. const char *name,
  238. int len,
  239. struct fdt_property **prop
  240. )
  241. {
  242. int oldlen;
  243. int err;
  244. *prop = fdt_get_property_w (fdt, nodeoffset, name, &oldlen);
  245. if (!*prop) {
  246. return oldlen;
  247. }
  248. if ((err = _fdt_splice_struct (
  249. fdt,
  250. (*prop)->data,
  251. FDT_TAGALIGN (oldlen),
  252. FDT_TAGALIGN (len)
  253. )))
  254. {
  255. return err;
  256. }
  257. (*prop)->len = cpu_to_fdt32 (len);
  258. return 0;
  259. }
  260. static int
  261. _fdt_add_property (
  262. void *fdt,
  263. int nodeoffset,
  264. const char *name,
  265. int len,
  266. struct fdt_property **prop
  267. )
  268. {
  269. int proplen;
  270. int nextoffset;
  271. int namestroff;
  272. int err;
  273. if ((nextoffset = _fdt_check_node_offset (fdt, nodeoffset)) < 0) {
  274. return nextoffset;
  275. }
  276. namestroff = _fdt_find_add_string (fdt, name);
  277. if (namestroff < 0) {
  278. return namestroff;
  279. }
  280. *prop = _fdt_offset_ptr_w (fdt, nextoffset);
  281. proplen = sizeof (**prop) + FDT_TAGALIGN (len);
  282. err = _fdt_splice_struct (fdt, *prop, 0, proplen);
  283. if (err) {
  284. return err;
  285. }
  286. (*prop)->tag = cpu_to_fdt32 (FDT_PROP);
  287. (*prop)->nameoff = cpu_to_fdt32 (namestroff);
  288. (*prop)->len = cpu_to_fdt32 (len);
  289. return 0;
  290. }
  291. int
  292. fdt_set_name (
  293. void *fdt,
  294. int nodeoffset,
  295. const char *name
  296. )
  297. {
  298. char *namep;
  299. int oldlen, newlen;
  300. int err;
  301. FDT_RW_CHECK_HEADER (fdt);
  302. namep = (char *)(uintptr_t)fdt_get_name (fdt, nodeoffset, &oldlen);
  303. if (!namep) {
  304. return oldlen;
  305. }
  306. newlen = strlen (name);
  307. err = _fdt_splice_struct (
  308. fdt,
  309. namep,
  310. FDT_TAGALIGN (oldlen+1),
  311. FDT_TAGALIGN (newlen+1)
  312. );
  313. if (err) {
  314. return err;
  315. }
  316. memcpy (namep, name, newlen+1);
  317. return 0;
  318. }
  319. int
  320. fdt_setprop_placeholder (
  321. void *fdt,
  322. int nodeoffset,
  323. const char *name,
  324. int len,
  325. void **prop_data
  326. )
  327. {
  328. struct fdt_property *prop;
  329. int err;
  330. FDT_RW_CHECK_HEADER (fdt);
  331. err = _fdt_resize_property (fdt, nodeoffset, name, len, &prop);
  332. if (err == -FDT_ERR_NOTFOUND) {
  333. err = _fdt_add_property (fdt, nodeoffset, name, len, &prop);
  334. }
  335. if (err) {
  336. return err;
  337. }
  338. *prop_data = prop->data;
  339. return 0;
  340. }
  341. int
  342. fdt_setprop (
  343. void *fdt,
  344. int nodeoffset,
  345. const char *name,
  346. const void *val,
  347. int len
  348. )
  349. {
  350. void *prop_data;
  351. int err;
  352. err = fdt_setprop_placeholder (fdt, nodeoffset, name, len, &prop_data);
  353. if (err) {
  354. return err;
  355. }
  356. if (len) {
  357. memcpy (prop_data, val, len);
  358. }
  359. return 0;
  360. }
  361. int
  362. fdt_appendprop (
  363. void *fdt,
  364. int nodeoffset,
  365. const char *name,
  366. const void *val,
  367. int len
  368. )
  369. {
  370. struct fdt_property *prop;
  371. int err, oldlen, newlen;
  372. FDT_RW_CHECK_HEADER (fdt);
  373. prop = fdt_get_property_w (fdt, nodeoffset, name, &oldlen);
  374. if (prop) {
  375. newlen = len + oldlen;
  376. err = _fdt_splice_struct (
  377. fdt,
  378. prop->data,
  379. FDT_TAGALIGN (oldlen),
  380. FDT_TAGALIGN (newlen)
  381. );
  382. if (err) {
  383. return err;
  384. }
  385. prop->len = cpu_to_fdt32 (newlen);
  386. memcpy (prop->data + oldlen, val, len);
  387. } else {
  388. err = _fdt_add_property (fdt, nodeoffset, name, len, &prop);
  389. if (err) {
  390. return err;
  391. }
  392. memcpy (prop->data, val, len);
  393. }
  394. return 0;
  395. }
  396. int
  397. fdt_delprop (
  398. void *fdt,
  399. int nodeoffset,
  400. const char *name
  401. )
  402. {
  403. struct fdt_property *prop;
  404. int len, proplen;
  405. FDT_RW_CHECK_HEADER (fdt);
  406. prop = fdt_get_property_w (fdt, nodeoffset, name, &len);
  407. if (!prop) {
  408. return len;
  409. }
  410. proplen = sizeof (*prop) + FDT_TAGALIGN (len);
  411. return _fdt_splice_struct (fdt, prop, proplen, 0);
  412. }
  413. int
  414. fdt_add_subnode_namelen (
  415. void *fdt,
  416. int parentoffset,
  417. const char *name,
  418. int namelen
  419. )
  420. {
  421. struct fdt_node_header *nh;
  422. int offset, nextoffset;
  423. int nodelen;
  424. int err;
  425. uint32_t tag;
  426. fdt32_t *endtag;
  427. FDT_RW_CHECK_HEADER (fdt);
  428. offset = fdt_subnode_offset_namelen (fdt, parentoffset, name, namelen);
  429. if (offset >= 0) {
  430. return -FDT_ERR_EXISTS;
  431. } else if (offset != -FDT_ERR_NOTFOUND) {
  432. return offset;
  433. }
  434. /* Try to place the new node after the parent's properties */
  435. fdt_next_tag (fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */
  436. do {
  437. offset = nextoffset;
  438. tag = fdt_next_tag (fdt, offset, &nextoffset);
  439. } while ((tag == FDT_PROP) || (tag == FDT_NOP));
  440. nh = _fdt_offset_ptr_w (fdt, offset);
  441. nodelen = sizeof (*nh) + FDT_TAGALIGN (namelen+1) + FDT_TAGSIZE;
  442. err = _fdt_splice_struct (fdt, nh, 0, nodelen);
  443. if (err) {
  444. return err;
  445. }
  446. nh->tag = cpu_to_fdt32 (FDT_BEGIN_NODE);
  447. memset (nh->name, 0, FDT_TAGALIGN (namelen+1));
  448. memcpy (nh->name, name, namelen);
  449. endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
  450. *endtag = cpu_to_fdt32 (FDT_END_NODE);
  451. return offset;
  452. }
  453. int
  454. fdt_add_subnode (
  455. void *fdt,
  456. int parentoffset,
  457. const char *name
  458. )
  459. {
  460. return fdt_add_subnode_namelen (fdt, parentoffset, name, strlen (name));
  461. }
  462. int
  463. fdt_del_node (
  464. void *fdt,
  465. int nodeoffset
  466. )
  467. {
  468. int endoffset;
  469. FDT_RW_CHECK_HEADER (fdt);
  470. endoffset = _fdt_node_end_offset (fdt, nodeoffset);
  471. if (endoffset < 0) {
  472. return endoffset;
  473. }
  474. return _fdt_splice_struct (
  475. fdt,
  476. _fdt_offset_ptr_w (fdt, nodeoffset),
  477. endoffset - nodeoffset,
  478. 0
  479. );
  480. }
  481. static void
  482. _fdt_packblocks (
  483. const char *old,
  484. char *new,
  485. int mem_rsv_size,
  486. int struct_size
  487. )
  488. {
  489. int mem_rsv_off, struct_off, strings_off;
  490. mem_rsv_off = FDT_ALIGN (sizeof (struct fdt_header), 8);
  491. struct_off = mem_rsv_off + mem_rsv_size;
  492. strings_off = struct_off + struct_size;
  493. memmove (new + mem_rsv_off, old + fdt_off_mem_rsvmap (old), mem_rsv_size);
  494. fdt_set_off_mem_rsvmap (new, mem_rsv_off);
  495. memmove (new + struct_off, old + fdt_off_dt_struct (old), struct_size);
  496. fdt_set_off_dt_struct (new, struct_off);
  497. fdt_set_size_dt_struct (new, struct_size);
  498. memmove (
  499. new + strings_off,
  500. old + fdt_off_dt_strings (old),
  501. fdt_size_dt_strings (old)
  502. );
  503. fdt_set_off_dt_strings (new, strings_off);
  504. fdt_set_size_dt_strings (new, fdt_size_dt_strings (old));
  505. }
  506. int
  507. fdt_open_into (
  508. const void *fdt,
  509. void *buf,
  510. int bufsize
  511. )
  512. {
  513. int err;
  514. int mem_rsv_size, struct_size;
  515. int newsize;
  516. const char *fdtstart = fdt;
  517. const char *fdtend = fdtstart + fdt_totalsize (fdt);
  518. char *tmp;
  519. FDT_CHECK_HEADER (fdt);
  520. mem_rsv_size = (fdt_num_mem_rsv (fdt)+1)
  521. * sizeof (struct fdt_reserve_entry);
  522. if (fdt_version (fdt) >= 17) {
  523. struct_size = fdt_size_dt_struct (fdt);
  524. } else {
  525. struct_size = 0;
  526. while (fdt_next_tag (fdt, struct_size, &struct_size) != FDT_END) {
  527. }
  528. if (struct_size < 0) {
  529. return struct_size;
  530. }
  531. }
  532. if (!_fdt_blocks_misordered (fdt, mem_rsv_size, struct_size)) {
  533. /* no further work necessary */
  534. err = fdt_move (fdt, buf, bufsize);
  535. if (err) {
  536. return err;
  537. }
  538. fdt_set_version (buf, 17);
  539. fdt_set_size_dt_struct (buf, struct_size);
  540. fdt_set_totalsize (buf, bufsize);
  541. return 0;
  542. }
  543. /* Need to reorder */
  544. newsize = FDT_ALIGN (sizeof (struct fdt_header), 8) + mem_rsv_size
  545. + struct_size + fdt_size_dt_strings (fdt);
  546. if (bufsize < newsize) {
  547. return -FDT_ERR_NOSPACE;
  548. }
  549. /* First attempt to build converted tree at beginning of buffer */
  550. tmp = buf;
  551. /* But if that overlaps with the old tree... */
  552. if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
  553. /* Try right after the old tree instead */
  554. tmp = (char *)(uintptr_t)fdtend;
  555. if ((tmp + newsize) > ((char *)buf + bufsize)) {
  556. return -FDT_ERR_NOSPACE;
  557. }
  558. }
  559. _fdt_packblocks (fdt, tmp, mem_rsv_size, struct_size);
  560. memmove (buf, tmp, newsize);
  561. fdt_set_magic (buf, FDT_MAGIC);
  562. fdt_set_totalsize (buf, bufsize);
  563. fdt_set_version (buf, 17);
  564. fdt_set_last_comp_version (buf, 16);
  565. fdt_set_boot_cpuid_phys (buf, fdt_boot_cpuid_phys (fdt));
  566. return 0;
  567. }
  568. int
  569. fdt_pack (
  570. void *fdt
  571. )
  572. {
  573. int mem_rsv_size;
  574. FDT_RW_CHECK_HEADER (fdt);
  575. mem_rsv_size = (fdt_num_mem_rsv (fdt)+1)
  576. * sizeof (struct fdt_reserve_entry);
  577. _fdt_packblocks (fdt, fdt, mem_rsv_size, fdt_size_dt_struct (fdt));
  578. fdt_set_totalsize (fdt, _fdt_data_size (fdt));
  579. return 0;
  580. }