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 | |
53 | #include <fdt.h> |
54 | #include <libfdt.h> |
55 | |
56 | #include "libfdt_internal.h" |
57 | |
58 | static int fdt_blocks_misordered_(const void *fdt, |
59 | int mem_rsv_size, int struct_size) |
60 | { |
61 | return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8)) |
62 | || (fdt_off_dt_struct(fdt) < |
63 | (fdt_off_mem_rsvmap(fdt) + mem_rsv_size)) |
64 | || (fdt_off_dt_strings(fdt) < |
65 | (fdt_off_dt_struct(fdt) + struct_size)) |
66 | || (fdt_totalsize(fdt) < |
67 | (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt))); |
68 | } |
69 | |
70 | static int fdt_rw_probe_(void *fdt) |
71 | { |
72 | FDT_RO_PROBE(fdt); |
73 | |
74 | if (fdt_version(fdt) < 17) |
75 | return -FDT_ERR_BADVERSION; |
76 | if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry), |
77 | fdt_size_dt_struct(fdt))) |
78 | return -FDT_ERR_BADLAYOUT; |
79 | if (fdt_version(fdt) > 17) |
80 | fdt_set_version(fdt, 17); |
81 | |
82 | return 0; |
83 | } |
84 | |
85 | #define FDT_RW_PROBE(fdt) \ |
86 | { \ |
87 | int err_; \ |
88 | if ((err_ = fdt_rw_probe_(fdt)) != 0) \ |
89 | return err_; \ |
90 | } |
91 | |
92 | static inline int fdt_data_size_(void *fdt) |
93 | { |
94 | return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt); |
95 | } |
96 | |
97 | static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen) |
98 | { |
99 | char *p = splicepoint; |
100 | char *end = (char *)fdt + fdt_data_size_(fdt); |
101 | |
102 | if (((p + oldlen) < p) || ((p + oldlen) > end)) |
103 | return -FDT_ERR_BADOFFSET; |
104 | if ((p < (char *)fdt) || ((end - oldlen + newlen) < (char *)fdt)) |
105 | return -FDT_ERR_BADOFFSET; |
106 | if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize(fdt))) |
107 | return -FDT_ERR_NOSPACE; |
108 | memmove(p + newlen, p + oldlen, end - p - oldlen); |
109 | return 0; |
110 | } |
111 | |
112 | static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p, |
113 | int oldn, int newn) |
114 | { |
115 | int delta = (newn - oldn) * sizeof(*p); |
116 | int err; |
117 | err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p)); |
118 | if (err) |
119 | return err; |
120 | fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta); |
121 | fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta); |
122 | return 0; |
123 | } |
124 | |
125 | static int fdt_splice_struct_(void *fdt, void *p, |
126 | int oldlen, int newlen) |
127 | { |
128 | int delta = newlen - oldlen; |
129 | int err; |
130 | |
131 | if ((err = fdt_splice_(fdt, p, oldlen, newlen))) |
132 | return err; |
133 | |
134 | fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta); |
135 | fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta); |
136 | return 0; |
137 | } |
138 | |
139 | static int fdt_splice_string_(void *fdt, int newlen) |
140 | { |
141 | void *p = (char *)fdt |
142 | + fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt); |
143 | int err; |
144 | |
145 | if ((err = fdt_splice_(fdt, p, 0, newlen))) |
146 | return err; |
147 | |
148 | fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen); |
149 | return 0; |
150 | } |
151 | |
152 | static int fdt_find_add_string_(void *fdt, const char *s) |
153 | { |
154 | char *strtab = (char *)fdt + fdt_off_dt_strings(fdt); |
155 | const char *p; |
156 | char *new; |
157 | int len = strlen(s) + 1; |
158 | int err; |
159 | |
160 | p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s); |
161 | if (p) |
162 | /* found it */ |
163 | return (p - strtab); |
164 | |
165 | new = strtab + fdt_size_dt_strings(fdt); |
166 | err = fdt_splice_string_(fdt, len); |
167 | if (err) |
168 | return err; |
169 | |
170 | memcpy(new, s, len); |
171 | return (new - strtab); |
172 | } |
173 | |
174 | int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size) |
175 | { |
176 | struct fdt_reserve_entry *re; |
177 | int err; |
178 | |
179 | FDT_RW_PROBE(fdt); |
180 | |
181 | re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt)); |
182 | err = fdt_splice_mem_rsv_(fdt, re, 0, 1); |
183 | if (err) |
184 | return err; |
185 | |
186 | re->address = cpu_to_fdt64(address); |
187 | re->size = cpu_to_fdt64(size); |
188 | return 0; |
189 | } |
190 | |
191 | int fdt_del_mem_rsv(void *fdt, int n) |
192 | { |
193 | struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n); |
194 | |
195 | FDT_RW_PROBE(fdt); |
196 | |
197 | if (n >= fdt_num_mem_rsv(fdt)) |
198 | return -FDT_ERR_NOTFOUND; |
199 | |
200 | return fdt_splice_mem_rsv_(fdt, re, 1, 0); |
201 | } |
202 | |
203 | static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name, |
204 | int len, struct fdt_property **prop) |
205 | { |
206 | int oldlen; |
207 | int err; |
208 | |
209 | *prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen); |
210 | if (!*prop) |
211 | return oldlen; |
212 | |
213 | if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen), |
214 | FDT_TAGALIGN(len)))) |
215 | return err; |
216 | |
217 | (*prop)->len = cpu_to_fdt32(len); |
218 | return 0; |
219 | } |
220 | |
221 | static int fdt_add_property_(void *fdt, int nodeoffset, const char *name, |
222 | int len, struct fdt_property **prop) |
223 | { |
224 | int proplen; |
225 | int nextoffset; |
226 | int namestroff; |
227 | int err; |
228 | |
229 | if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0) |
230 | return nextoffset; |
231 | |
232 | namestroff = fdt_find_add_string_(fdt, name); |
233 | if (namestroff < 0) |
234 | return namestroff; |
235 | |
236 | *prop = fdt_offset_ptr_w_(fdt, nextoffset); |
237 | proplen = sizeof(**prop) + FDT_TAGALIGN(len); |
238 | |
239 | err = fdt_splice_struct_(fdt, *prop, 0, proplen); |
240 | if (err) |
241 | return err; |
242 | |
243 | (*prop)->tag = cpu_to_fdt32(FDT_PROP); |
244 | (*prop)->nameoff = cpu_to_fdt32(namestroff); |
245 | (*prop)->len = cpu_to_fdt32(len); |
246 | return 0; |
247 | } |
248 | |
249 | int fdt_set_name(void *fdt, int nodeoffset, const char *name) |
250 | { |
251 | char *namep; |
252 | int oldlen, newlen; |
253 | int err; |
254 | |
255 | FDT_RW_PROBE(fdt); |
256 | |
257 | namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen); |
258 | if (!namep) |
259 | return oldlen; |
260 | |
261 | newlen = strlen(name); |
262 | |
263 | err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1), |
264 | FDT_TAGALIGN(newlen+1)); |
265 | if (err) |
266 | return err; |
267 | |
268 | memcpy(namep, name, newlen+1); |
269 | return 0; |
270 | } |
271 | |
272 | int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name, |
273 | int len, void **prop_data) |
274 | { |
275 | struct fdt_property *prop; |
276 | int err; |
277 | |
278 | FDT_RW_PROBE(fdt); |
279 | |
280 | err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop); |
281 | if (err == -FDT_ERR_NOTFOUND) |
282 | err = fdt_add_property_(fdt, nodeoffset, name, len, &prop); |
283 | if (err) |
284 | return err; |
285 | |
286 | *prop_data = prop->data; |
287 | return 0; |
288 | } |
289 | |
290 | int fdt_setprop(void *fdt, int nodeoffset, const char *name, |
291 | const void *val, int len) |
292 | { |
293 | void *prop_data; |
294 | int err; |
295 | |
296 | err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data); |
297 | if (err) |
298 | return err; |
299 | |
300 | if (len) |
301 | memcpy(prop_data, val, len); |
302 | return 0; |
303 | } |
304 | |
305 | int fdt_appendprop(void *fdt, int nodeoffset, const char *name, |
306 | const void *val, int len) |
307 | { |
308 | struct fdt_property *prop; |
309 | int err, oldlen, newlen; |
310 | |
311 | FDT_RW_PROBE(fdt); |
312 | |
313 | prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen); |
314 | if (prop) { |
315 | newlen = len + oldlen; |
316 | err = fdt_splice_struct_(fdt, prop->data, |
317 | FDT_TAGALIGN(oldlen), |
318 | FDT_TAGALIGN(newlen)); |
319 | if (err) |
320 | return err; |
321 | prop->len = cpu_to_fdt32(newlen); |
322 | memcpy(prop->data + oldlen, val, len); |
323 | } else { |
324 | err = fdt_add_property_(fdt, nodeoffset, name, len, &prop); |
325 | if (err) |
326 | return err; |
327 | memcpy(prop->data, val, len); |
328 | } |
329 | return 0; |
330 | } |
331 | |
332 | int fdt_delprop(void *fdt, int nodeoffset, const char *name) |
333 | { |
334 | struct fdt_property *prop; |
335 | int len, proplen; |
336 | |
337 | FDT_RW_PROBE(fdt); |
338 | |
339 | prop = fdt_get_property_w(fdt, nodeoffset, name, &len); |
340 | if (!prop) |
341 | return len; |
342 | |
343 | proplen = sizeof(*prop) + FDT_TAGALIGN(len); |
344 | return fdt_splice_struct_(fdt, prop, proplen, 0); |
345 | } |
346 | |
347 | int fdt_add_subnode_namelen(void *fdt, int parentoffset, |
348 | const char *name, int namelen) |
349 | { |
350 | struct fdt_node_header *nh; |
351 | int offset, nextoffset; |
352 | int nodelen; |
353 | int err; |
354 | uint32_t tag; |
355 | fdt32_t *endtag; |
356 | |
357 | FDT_RW_PROBE(fdt); |
358 | |
359 | offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen); |
360 | if (offset >= 0) |
361 | return -FDT_ERR_EXISTS; |
362 | else if (offset != -FDT_ERR_NOTFOUND) |
363 | return offset; |
364 | |
365 | /* Try to place the new node after the parent's properties */ |
366 | fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */ |
367 | do { |
368 | offset = nextoffset; |
369 | tag = fdt_next_tag(fdt, offset, &nextoffset); |
370 | } while ((tag == FDT_PROP) || (tag == FDT_NOP)); |
371 | |
372 | nh = fdt_offset_ptr_w_(fdt, offset); |
373 | nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE; |
374 | |
375 | err = fdt_splice_struct_(fdt, nh, 0, nodelen); |
376 | if (err) |
377 | return err; |
378 | |
379 | nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE); |
380 | memset(nh->name, 0, FDT_TAGALIGN(namelen+1)); |
381 | memcpy(nh->name, name, namelen); |
382 | endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE); |
383 | *endtag = cpu_to_fdt32(FDT_END_NODE); |
384 | |
385 | return offset; |
386 | } |
387 | |
388 | int fdt_add_subnode(void *fdt, int parentoffset, const char *name) |
389 | { |
390 | return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name)); |
391 | } |
392 | |
393 | int fdt_del_node(void *fdt, int nodeoffset) |
394 | { |
395 | int endoffset; |
396 | |
397 | FDT_RW_PROBE(fdt); |
398 | |
399 | endoffset = fdt_node_end_offset_(fdt, nodeoffset); |
400 | if (endoffset < 0) |
401 | return endoffset; |
402 | |
403 | return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset), |
404 | endoffset - nodeoffset, 0); |
405 | } |
406 | |
407 | static void fdt_packblocks_(const char *old, char *new, |
408 | int mem_rsv_size, int struct_size) |
409 | { |
410 | int mem_rsv_off, struct_off, strings_off; |
411 | |
412 | mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8); |
413 | struct_off = mem_rsv_off + mem_rsv_size; |
414 | strings_off = struct_off + struct_size; |
415 | |
416 | memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size); |
417 | fdt_set_off_mem_rsvmap(new, mem_rsv_off); |
418 | |
419 | memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size); |
420 | fdt_set_off_dt_struct(new, struct_off); |
421 | fdt_set_size_dt_struct(new, struct_size); |
422 | |
423 | memmove(new + strings_off, old + fdt_off_dt_strings(old), |
424 | fdt_size_dt_strings(old)); |
425 | fdt_set_off_dt_strings(new, strings_off); |
426 | fdt_set_size_dt_strings(new, fdt_size_dt_strings(old)); |
427 | } |
428 | |
429 | int fdt_open_into(const void *fdt, void *buf, int bufsize) |
430 | { |
431 | int err; |
432 | int mem_rsv_size, struct_size; |
433 | int newsize; |
434 | const char *fdtstart = fdt; |
435 | const char *fdtend = fdtstart + fdt_totalsize(fdt); |
436 | char *tmp; |
437 | |
438 | FDT_RO_PROBE(fdt); |
439 | |
440 | mem_rsv_size = (fdt_num_mem_rsv(fdt)+1) |
441 | * sizeof(struct fdt_reserve_entry); |
442 | |
443 | if (fdt_version(fdt) >= 17) { |
444 | struct_size = fdt_size_dt_struct(fdt); |
445 | } else { |
446 | struct_size = 0; |
447 | while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END) |
448 | ; |
449 | if (struct_size < 0) |
450 | return struct_size; |
451 | } |
452 | |
453 | if (!fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) { |
454 | /* no further work necessary */ |
455 | err = fdt_move(fdt, buf, bufsize); |
456 | if (err) |
457 | return err; |
458 | fdt_set_version(buf, 17); |
459 | fdt_set_size_dt_struct(buf, struct_size); |
460 | fdt_set_totalsize(buf, bufsize); |
461 | return 0; |
462 | } |
463 | |
464 | /* Need to reorder */ |
465 | newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size |
466 | + struct_size + fdt_size_dt_strings(fdt); |
467 | |
468 | if (bufsize < newsize) |
469 | return -FDT_ERR_NOSPACE; |
470 | |
471 | /* First attempt to build converted tree at beginning of buffer */ |
472 | tmp = buf; |
473 | /* But if that overlaps with the old tree... */ |
474 | if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) { |
475 | /* Try right after the old tree instead */ |
476 | tmp = (char *)(uintptr_t)fdtend; |
477 | if ((tmp + newsize) > ((char *)buf + bufsize)) |
478 | return -FDT_ERR_NOSPACE; |
479 | } |
480 | |
481 | fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size); |
482 | memmove(buf, tmp, newsize); |
483 | |
484 | fdt_set_magic(buf, FDT_MAGIC); |
485 | fdt_set_totalsize(buf, bufsize); |
486 | fdt_set_version(buf, 17); |
487 | fdt_set_last_comp_version(buf, 16); |
488 | fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt)); |
489 | |
490 | return 0; |
491 | } |
492 | |
493 | int fdt_pack(void *fdt) |
494 | { |
495 | int mem_rsv_size; |
496 | |
497 | FDT_RW_PROBE(fdt); |
498 | |
499 | mem_rsv_size = (fdt_num_mem_rsv(fdt)+1) |
500 | * sizeof(struct fdt_reserve_entry); |
501 | fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt)); |
502 | fdt_set_totalsize(fdt, fdt_data_size_(fdt)); |
503 | |
504 | return 0; |
505 | } |
506 | |