2 * This program is free software; you can redistribute it and/or modify
3 * it under the terms of the GNU General Public License as published by
4 * the Free Software Foundation; either version 2 of the License, or
5 * (at your option) any later version.
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software
14 * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
16 * Copyright Pantelis Antoniou 2006
17 * Copyright (C) IBM Corporation 2006
19 * Authors: Pantelis Antoniou <pantelis@embeddedalley.com>
20 * Hollis Blanchard <hollisb@us.ibm.com>
21 * Mark A. Greer <mgreer@mvista.com>
22 * Paul Mackerras <paulus@samba.org>
27 #include "flatdevtree.h"
28 #include "flatdevtree_env.h"
30 #define _ALIGN(x, al) (((x) + (al) - 1) & ~((al) - 1))
32 static char *ft_root_node(struct ft_cxt *cxt)
34 return cxt->rgn[FT_STRUCT].start;
37 /* Routines for keeping node ptrs returned by ft_find_device current */
38 /* First entry not used b/c it would return 0 and be taken as NULL/error */
39 static void *ft_get_phandle(struct ft_cxt *cxt, char *node)
46 for (i = 1; i < cxt->nodes_used; i++) /* already there? */
47 if (cxt->node_tbl[i] == node)
50 if (cxt->nodes_used < cxt->node_max) {
51 cxt->node_tbl[cxt->nodes_used] = node;
52 return (void *)cxt->nodes_used++;
58 static char *ft_node_ph2node(struct ft_cxt *cxt, const void *phandle)
60 unsigned int i = (unsigned int)phandle;
62 if (i < cxt->nodes_used)
63 return cxt->node_tbl[i];
67 static void ft_node_update_before(struct ft_cxt *cxt, char *addr, int shift)
74 for (i = 1; i < cxt->nodes_used; i++)
75 if (cxt->node_tbl[i] < addr)
76 cxt->node_tbl[i] += shift;
79 static void ft_node_update_after(struct ft_cxt *cxt, char *addr, int shift)
86 for (i = 1; i < cxt->nodes_used; i++)
87 if (cxt->node_tbl[i] >= addr)
88 cxt->node_tbl[i] += shift;
91 /* Struct used to return info from ft_next() */
99 /* Set ptrs to current one's info; return addr of next one */
100 static char *ft_next(struct ft_cxt *cxt, char *p, struct ft_atom *ret)
104 if (p >= cxt->rgn[FT_STRUCT].start + cxt->rgn[FT_STRUCT].size)
107 ret->tag = be32_to_cpu(*(u32 *) p);
110 switch (ret->tag) { /* Tag */
111 case OF_DT_BEGIN_NODE:
113 ret->data = (void *)(p - 4); /* start of node */
114 p += _ALIGN(strlen(p) + 1, 4);
117 ret->size = sz = be32_to_cpu(*(u32 *) p);
118 ret->name = cxt->str_anchor + be32_to_cpu(*(u32 *) (p + 4));
119 ret->data = (void *)(p + 8);
120 p += 8 + _ALIGN(sz, 4);
134 #define HDR_SIZE _ALIGN(sizeof(struct boot_param_header), 8)
135 #define EXPAND_INCR 1024 /* alloc this much extra when expanding */
137 /* See if the regions are in the standard order and non-overlapping */
138 static int ft_ordered(struct ft_cxt *cxt)
140 char *p = (char *)cxt->bph + HDR_SIZE;
143 for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) {
144 if (p > cxt->rgn[r].start)
146 p = cxt->rgn[r].start + cxt->rgn[r].size;
148 return p <= (char *)cxt->bph + cxt->max_size;
151 /* Copy the tree to a newly-allocated region and put things in order */
152 static int ft_reorder(struct ft_cxt *cxt, int nextra)
159 tot = HDR_SIZE + EXPAND_INCR;
160 for (r = FT_RSVMAP; r <= FT_STRINGS; ++r)
161 tot += cxt->rgn[r].size;
164 tot = _ALIGN(tot, 8);
168 p = cxt->realloc(NULL, tot);
172 memcpy(p, cxt->bph, sizeof(struct boot_param_header));
173 /* offsets get fixed up later */
175 cxt->bph = (struct boot_param_header *)p;
180 memcpy(p, cxt->rgn[FT_RSVMAP].start, cxt->rgn[FT_RSVMAP].size);
181 cxt->rgn[FT_RSVMAP].start = p;
182 p += cxt->rgn[FT_RSVMAP].size;
184 memcpy(p, cxt->rgn[FT_STRUCT].start, cxt->rgn[FT_STRUCT].size);
185 ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start,
186 p - cxt->rgn[FT_STRUCT].start);
187 cxt->p += p - cxt->rgn[FT_STRUCT].start;
188 cxt->rgn[FT_STRUCT].start = p;
190 p = pend - cxt->rgn[FT_STRINGS].size;
191 memcpy(p, cxt->rgn[FT_STRINGS].start, cxt->rgn[FT_STRINGS].size);
192 stroff = cxt->str_anchor - cxt->rgn[FT_STRINGS].start;
193 cxt->rgn[FT_STRINGS].start = p;
194 cxt->str_anchor = p + stroff;
200 static inline char *prev_end(struct ft_cxt *cxt, enum ft_rgn_id r)
203 return cxt->rgn[r - 1].start + cxt->rgn[r - 1].size;
204 return (char *)cxt->bph + HDR_SIZE;
207 static inline char *next_start(struct ft_cxt *cxt, enum ft_rgn_id r)
210 return cxt->rgn[r + 1].start;
211 return (char *)cxt->bph + cxt->max_size;
215 * See if we can expand region rgn by nextra bytes by using up
216 * free space after or before the region.
218 static int ft_shuffle(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn,
222 char *rgn_start, *rgn_end;
224 rgn_start = cxt->rgn[rgn].start;
225 rgn_end = rgn_start + cxt->rgn[rgn].size;
226 if (nextra <= 0 || rgn_end + nextra <= next_start(cxt, rgn)) {
227 /* move following stuff */
230 memmove(p, p - nextra, rgn_end - p + nextra);
232 memmove(p + nextra, p, rgn_end - p);
233 if (rgn == FT_STRUCT)
234 ft_node_update_after(cxt, p, nextra);
236 cxt->rgn[rgn].size += nextra;
237 if (rgn == FT_STRINGS)
238 /* assumes strings only added at beginning */
239 cxt->str_anchor += nextra;
242 if (prev_end(cxt, rgn) <= rgn_start - nextra) {
243 /* move preceding stuff */
245 memmove(rgn_start - nextra, rgn_start, p - rgn_start);
246 if (rgn == FT_STRUCT)
247 ft_node_update_before(cxt, p, -nextra);
250 cxt->rgn[rgn].start -= nextra;
251 cxt->rgn[rgn].size += nextra;
257 static int ft_make_space(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn,
260 unsigned long size, ssize, tot;
264 if (!cxt->isordered) {
265 unsigned long rgn_off = *pp - cxt->rgn[rgn].start;
267 if (!ft_reorder(cxt, nextra))
270 *pp = cxt->rgn[rgn].start + rgn_off;
272 if (ft_shuffle(cxt, pp, rgn, nextra))
275 /* See if there is space after the strings section */
276 ssize = cxt->rgn[FT_STRINGS].size;
277 if (cxt->rgn[FT_STRINGS].start + ssize
278 < (char *)cxt->bph + cxt->max_size) {
279 /* move strings up as far as possible */
280 str = (char *)cxt->bph + cxt->max_size - ssize;
281 cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start;
282 memmove(str, cxt->rgn[FT_STRINGS].start, ssize);
283 cxt->rgn[FT_STRINGS].start = str;
284 /* enough space now? */
285 if (rgn >= FT_STRUCT && ft_shuffle(cxt, pp, rgn, nextra))
289 /* how much total free space is there following this region? */
291 for (r = rgn; r < FT_STRINGS; ++r) {
292 char *r_end = cxt->rgn[r].start + cxt->rgn[r].size;
293 tot += next_start(cxt, rgn) - r_end;
296 /* cast is to shut gcc up; we know nextra >= 0 */
297 if (tot < (unsigned int)nextra) {
298 /* have to reallocate */
299 char *newp, *new_start;
304 size = _ALIGN(cxt->max_size + (nextra - tot) + EXPAND_INCR, 8);
305 newp = cxt->realloc(cxt->bph, size);
308 cxt->max_size = size;
309 shift = newp - (char *)cxt->bph;
311 if (shift) { /* realloc can return same addr */
312 cxt->bph = (struct boot_param_header *)newp;
313 ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start,
315 for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) {
316 new_start = cxt->rgn[r].start + shift;
317 cxt->rgn[r].start = new_start;
320 cxt->str_anchor += shift;
323 /* move strings up to the end */
324 str = newp + size - ssize;
325 cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start;
326 memmove(str, cxt->rgn[FT_STRINGS].start, ssize);
327 cxt->rgn[FT_STRINGS].start = str;
329 if (ft_shuffle(cxt, pp, rgn, nextra))
333 /* must be FT_RSVMAP and we need to move FT_STRUCT up */
334 if (rgn == FT_RSVMAP) {
335 next = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size
337 ssize = cxt->rgn[FT_STRUCT].size;
338 if (next + ssize >= cxt->rgn[FT_STRINGS].start)
339 return 0; /* "can't happen" */
340 memmove(next, cxt->rgn[FT_STRUCT].start, ssize);
341 ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, nextra);
342 cxt->rgn[FT_STRUCT].start = next;
344 if (ft_shuffle(cxt, pp, rgn, nextra))
348 return 0; /* "can't happen" */
351 static void ft_put_word(struct ft_cxt *cxt, u32 v)
353 *(u32 *) cxt->p = cpu_to_be32(v);
357 static void ft_put_bin(struct ft_cxt *cxt, const void *data, unsigned int sz)
359 unsigned long sza = _ALIGN(sz, 4);
361 /* zero out the alignment gap if necessary */
363 *(u32 *) (cxt->p + sza - 4) = 0;
365 /* copy in the data */
366 memcpy(cxt->p, data, sz);
371 int ft_begin_node(struct ft_cxt *cxt, const char *name)
373 unsigned long nlen = strlen(name) + 1;
374 unsigned long len = 8 + _ALIGN(nlen, 4);
376 if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len))
378 ft_put_word(cxt, OF_DT_BEGIN_NODE);
379 ft_put_bin(cxt, name, strlen(name) + 1);
383 void ft_end_node(struct ft_cxt *cxt)
385 ft_put_word(cxt, OF_DT_END_NODE);
388 void ft_nop(struct ft_cxt *cxt)
390 if (ft_make_space(cxt, &cxt->p, FT_STRUCT, 4))
391 ft_put_word(cxt, OF_DT_NOP);
394 #define NO_STRING 0x7fffffff
396 static int lookup_string(struct ft_cxt *cxt, const char *name)
400 p = cxt->rgn[FT_STRINGS].start;
401 end = p + cxt->rgn[FT_STRINGS].size;
403 if (strcmp(p, (char *)name) == 0)
404 return p - cxt->str_anchor;
411 /* lookup string and insert if not found */
412 static int map_string(struct ft_cxt *cxt, const char *name)
417 off = lookup_string(cxt, name);
418 if (off != NO_STRING)
420 p = cxt->rgn[FT_STRINGS].start;
421 if (!ft_make_space(cxt, &p, FT_STRINGS, strlen(name) + 1))
424 return p - cxt->str_anchor;
427 int ft_prop(struct ft_cxt *cxt, const char *name, const void *data,
432 off = map_string(cxt, name);
433 if (off == NO_STRING)
436 len = 12 + _ALIGN(sz, 4);
437 if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len))
440 ft_put_word(cxt, OF_DT_PROP);
441 ft_put_word(cxt, sz);
442 ft_put_word(cxt, off);
443 ft_put_bin(cxt, data, sz);
447 int ft_prop_str(struct ft_cxt *cxt, const char *name, const char *str)
449 return ft_prop(cxt, name, str, strlen(str) + 1);
452 int ft_prop_int(struct ft_cxt *cxt, const char *name, unsigned int val)
454 u32 v = cpu_to_be32((u32) val);
456 return ft_prop(cxt, name, &v, 4);
459 /* Calculate the size of the reserved map */
460 static unsigned long rsvmap_size(struct ft_cxt *cxt)
462 struct ft_reserve *res;
464 res = (struct ft_reserve *)cxt->rgn[FT_RSVMAP].start;
465 while (res->start || res->len)
467 return (char *)(res + 1) - cxt->rgn[FT_RSVMAP].start;
470 /* Calculate the size of the struct region by stepping through it */
471 static unsigned long struct_size(struct ft_cxt *cxt)
473 char *p = cxt->rgn[FT_STRUCT].start;
477 /* make check in ft_next happy */
478 if (cxt->rgn[FT_STRUCT].size == 0)
479 cxt->rgn[FT_STRUCT].size = 0xfffffffful - (unsigned long)p;
481 while ((next = ft_next(cxt, p, &atom)) != NULL)
483 return p + 4 - cxt->rgn[FT_STRUCT].start;
486 /* add `adj' on to all string offset values in the struct area */
487 static void adjust_string_offsets(struct ft_cxt *cxt, int adj)
489 char *p = cxt->rgn[FT_STRUCT].start;
494 while ((next = ft_next(cxt, p, &atom)) != NULL) {
495 if (atom.tag == OF_DT_PROP) {
496 off = be32_to_cpu(*(u32 *) (p + 8));
497 *(u32 *) (p + 8) = cpu_to_be32(off + adj);
503 /* start construction of the flat OF tree from scratch */
504 void ft_begin(struct ft_cxt *cxt, void *blob, unsigned int max_size,
505 void *(*realloc_fn) (void *, unsigned long))
507 struct boot_param_header *bph = blob;
509 struct ft_reserve *pres;
512 memset(cxt, 0, sizeof(*cxt));
515 cxt->max_size = max_size;
516 cxt->realloc = realloc_fn;
519 /* zero everything in the header area */
520 memset(bph, 0, sizeof(*bph));
522 bph->magic = cpu_to_be32(OF_DT_HEADER);
523 bph->version = cpu_to_be32(0x10);
524 bph->last_comp_version = cpu_to_be32(0x10);
527 cxt->rgn[FT_RSVMAP].start = p = blob + HDR_SIZE;
528 cxt->rgn[FT_RSVMAP].size = sizeof(struct ft_reserve);
529 pres = (struct ft_reserve *)p;
530 cxt->rgn[FT_STRUCT].start = p += sizeof(struct ft_reserve);
531 cxt->rgn[FT_STRUCT].size = 4;
532 cxt->rgn[FT_STRINGS].start = blob + max_size;
533 cxt->rgn[FT_STRINGS].size = 0;
535 /* init rsvmap and struct */
538 *(u32 *) p = cpu_to_be32(OF_DT_END);
540 cxt->str_anchor = blob;
543 /* open up an existing blob to be examined or modified */
544 int ft_open(struct ft_cxt *cxt, void *blob, unsigned int max_size,
545 unsigned int max_find_device,
546 void *(*realloc_fn) (void *, unsigned long))
548 struct boot_param_header *bph = blob;
550 /* can't cope with version < 16 */
551 if (be32_to_cpu(bph->version) < 16)
555 memset(cxt, 0, sizeof(*cxt));
557 /* alloc node_tbl to track node ptrs returned by ft_find_device */
559 cxt->node_tbl = realloc_fn(NULL, max_find_device * sizeof(char *));
562 memset(cxt->node_tbl, 0, max_find_device * sizeof(char *));
563 cxt->node_max = max_find_device;
564 cxt->nodes_used = 1; /* don't use idx 0 b/c looks like NULL */
567 cxt->max_size = max_size;
568 cxt->realloc = realloc_fn;
570 cxt->rgn[FT_RSVMAP].start = blob + be32_to_cpu(bph->off_mem_rsvmap);
571 cxt->rgn[FT_RSVMAP].size = rsvmap_size(cxt);
572 cxt->rgn[FT_STRUCT].start = blob + be32_to_cpu(bph->off_dt_struct);
573 cxt->rgn[FT_STRUCT].size = struct_size(cxt);
574 cxt->rgn[FT_STRINGS].start = blob + be32_to_cpu(bph->off_dt_strings);
575 cxt->rgn[FT_STRINGS].size = be32_to_cpu(bph->dt_strings_size);
576 /* Leave as '0' to force first ft_make_space call to do a ft_reorder
577 * and move dt to an area allocated by realloc.
578 cxt->isordered = ft_ordered(cxt);
581 cxt->p = cxt->rgn[FT_STRUCT].start;
582 cxt->str_anchor = cxt->rgn[FT_STRINGS].start;
587 /* add a reserver physical area to the rsvmap */
588 int ft_add_rsvmap(struct ft_cxt *cxt, u64 physaddr, u64 size)
591 struct ft_reserve *pres;
593 p = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size
594 - sizeof(struct ft_reserve);
595 if (!ft_make_space(cxt, &p, FT_RSVMAP, sizeof(struct ft_reserve)))
598 pres = (struct ft_reserve *)p;
599 pres->start = cpu_to_be64(physaddr);
600 pres->len = cpu_to_be64(size);
605 void ft_begin_tree(struct ft_cxt *cxt)
607 cxt->p = ft_root_node(cxt);
610 void ft_end_tree(struct ft_cxt *cxt)
612 struct boot_param_header *bph = cxt->bph;
613 char *p, *oldstr, *str, *endp;
618 return; /* we haven't touched anything */
620 /* adjust string offsets */
621 oldstr = cxt->rgn[FT_STRINGS].start;
622 adj = cxt->str_anchor - oldstr;
624 adjust_string_offsets(cxt, adj);
626 /* make strings end on 8-byte boundary */
627 ssize = cxt->rgn[FT_STRINGS].size;
628 endp = (char *)_ALIGN((unsigned long)cxt->rgn[FT_STRUCT].start
629 + cxt->rgn[FT_STRUCT].size + ssize, 8);
632 /* move strings down to end of structs */
633 memmove(str, oldstr, ssize);
634 cxt->str_anchor = str;
635 cxt->rgn[FT_STRINGS].start = str;
637 /* fill in header fields */
639 bph->totalsize = cpu_to_be32(endp - p);
640 bph->off_mem_rsvmap = cpu_to_be32(cxt->rgn[FT_RSVMAP].start - p);
641 bph->off_dt_struct = cpu_to_be32(cxt->rgn[FT_STRUCT].start - p);
642 bph->off_dt_strings = cpu_to_be32(cxt->rgn[FT_STRINGS].start - p);
643 bph->dt_strings_size = cpu_to_be32(ssize);
646 void *ft_find_device(struct ft_cxt *cxt, const char *srch_path)
650 /* require absolute path */
651 if (srch_path[0] != '/')
653 node = ft_find_descendent(cxt, ft_root_node(cxt), srch_path);
654 return ft_get_phandle(cxt, node);
657 void *ft_find_device_rel(struct ft_cxt *cxt, const void *top,
658 const char *srch_path)
662 node = ft_node_ph2node(cxt, top);
666 node = ft_find_descendent(cxt, node, srch_path);
667 return ft_get_phandle(cxt, node);
670 void *ft_find_descendent(struct ft_cxt *cxt, void *top, const char *srch_path)
678 const char *path_comp[FT_MAX_DEPTH];
684 while ((p = ft_next(cxt, p, &atom)) != NULL) {
686 case OF_DT_BEGIN_NODE:
690 cxt->genealogy[depth] = atom.data;
691 cxt->genealogy[depth + 1] = NULL;
692 if (depth && !(strncmp(atom.name, cp, cl) == 0
693 && (atom.name[cl] == '/'
694 || atom.name[cl] == '\0'
695 || atom.name[cl] == '@')))
697 path_comp[dmatch] = cp;
698 /* it matches so far, advance to next path component */
703 /* we're done if this is the end of the string */
706 /* look for end of this component */
717 if (dmatch > depth) {
719 cl = cp - path_comp[dmatch] - 1;
720 cp = path_comp[dmatch];
721 while (cl > 0 && cp[cl - 1] == '/')
731 void *ft_get_parent(struct ft_cxt *cxt, const void *phandle)
738 node = ft_node_ph2node(cxt, phandle);
742 for (d = 0; cxt->genealogy[d] != NULL; ++d)
743 if (cxt->genealogy[d] == node)
744 return cxt->genealogy[d > 0 ? d - 1 : 0];
746 /* have to do it the hard way... */
747 p = ft_root_node(cxt);
749 while ((p = ft_next(cxt, p, &atom)) != NULL) {
751 case OF_DT_BEGIN_NODE:
752 cxt->genealogy[d] = atom.data;
753 if (node == atom.data) {
755 cxt->genealogy[d + 1] = NULL;
756 return d > 0 ? cxt->genealogy[d - 1] : node;
768 int ft_get_prop(struct ft_cxt *cxt, const void *phandle, const char *propname,
769 void *buf, const unsigned int buflen)
777 node = ft_node_ph2node(cxt, phandle);
784 while ((p = ft_next(cxt, p, &atom)) != NULL) {
786 case OF_DT_BEGIN_NODE:
790 if ((depth != 1) || strcmp(atom.name, propname))
792 size = min(atom.size, buflen);
793 memcpy(buf, atom.data, size);
803 int ft_set_prop(struct ft_cxt *cxt, const void *phandle, const char *propname,
804 const void *buf, const unsigned int buflen)
811 node = ft_node_ph2node(cxt, phandle);
818 while ((next = ft_next(cxt, p, &atom)) != NULL) {
820 case OF_DT_BEGIN_NODE:
826 /* haven't found the property, insert here */
828 return ft_prop(cxt, propname, buf, buflen);
830 if ((depth != 1) || strcmp(atom.name, propname))
832 /* found an existing property, overwrite it */
833 nextra = _ALIGN(buflen, 4) - _ALIGN(atom.size, 4);
835 if (nextra && !ft_make_space(cxt, &cxt->p, FT_STRUCT,
838 *(u32 *) (cxt->p - 8) = cpu_to_be32(buflen);
839 ft_put_bin(cxt, buf, buflen);
847 int ft_del_prop(struct ft_cxt *cxt, const void *phandle, const char *propname)
854 node = ft_node_ph2node(cxt, phandle);
859 while ((next = ft_next(cxt, p, &atom)) != NULL) {
861 case OF_DT_BEGIN_NODE:
865 if (strcmp(atom.name, propname))
867 /* found the property, remove it */
868 size = 12 + -_ALIGN(atom.size, 4);
870 if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, -size))
879 void *ft_create_node(struct ft_cxt *cxt, const void *parent, const char *path)
885 p = ft_root_node(cxt);
886 while ((next = ft_next(cxt, p, &atom)) != NULL) {
888 case OF_DT_BEGIN_NODE:
890 if (depth == 1 && strcmp(atom.name, path) == 0)
891 /* duplicate node path, return error */
898 /* end of node, insert here */
900 ft_begin_node(cxt, path);