]>
Commit | Line | Data |
---|---|---|
6fb4efc6 MG |
1 | /* |
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. | |
6 | * | |
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. | |
11 | * | |
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. | |
15 | * | |
16 | * Copyright Pantelis Antoniou 2006 | |
17 | * Copyright (C) IBM Corporation 2006 | |
18 | * | |
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> | |
23 | */ | |
24 | ||
25 | #include <string.h> | |
26 | #include <stddef.h> | |
27 | #include "flatdevtree.h" | |
28 | #include "flatdevtree_env.h" | |
29 | ||
30 | #define _ALIGN(x, al) (((x) + (al) - 1) & ~((al) - 1)) | |
31 | ||
7c71c046 SW |
32 | static char *ft_root_node(struct ft_cxt *cxt) |
33 | { | |
34 | return cxt->rgn[FT_STRUCT].start; | |
35 | } | |
36 | ||
6fb4efc6 MG |
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 */ | |
1c53a496 | 39 | static void *ft_get_phandle(struct ft_cxt *cxt, char *node) |
6fb4efc6 MG |
40 | { |
41 | unsigned int i; | |
42 | ||
c8aa7263 SW |
43 | if (!node) |
44 | return NULL; | |
45 | ||
6fb4efc6 MG |
46 | for (i = 1; i < cxt->nodes_used; i++) /* already there? */ |
47 | if (cxt->node_tbl[i] == node) | |
48 | return (void *)i; | |
49 | ||
50 | if (cxt->nodes_used < cxt->node_max) { | |
51 | cxt->node_tbl[cxt->nodes_used] = node; | |
52 | return (void *)cxt->nodes_used++; | |
53 | } | |
54 | ||
55 | return NULL; | |
56 | } | |
57 | ||
58 | static char *ft_node_ph2node(struct ft_cxt *cxt, const void *phandle) | |
59 | { | |
60 | unsigned int i = (unsigned int)phandle; | |
61 | ||
62 | if (i < cxt->nodes_used) | |
63 | return cxt->node_tbl[i]; | |
64 | return NULL; | |
65 | } | |
66 | ||
67 | static void ft_node_update_before(struct ft_cxt *cxt, char *addr, int shift) | |
68 | { | |
69 | unsigned int i; | |
70 | ||
71 | if (shift == 0) | |
72 | return; | |
73 | ||
74 | for (i = 1; i < cxt->nodes_used; i++) | |
75 | if (cxt->node_tbl[i] < addr) | |
76 | cxt->node_tbl[i] += shift; | |
77 | } | |
78 | ||
79 | static void ft_node_update_after(struct ft_cxt *cxt, char *addr, int shift) | |
80 | { | |
81 | unsigned int i; | |
82 | ||
83 | if (shift == 0) | |
84 | return; | |
85 | ||
86 | for (i = 1; i < cxt->nodes_used; i++) | |
87 | if (cxt->node_tbl[i] >= addr) | |
88 | cxt->node_tbl[i] += shift; | |
89 | } | |
90 | ||
91 | /* Struct used to return info from ft_next() */ | |
92 | struct ft_atom { | |
93 | u32 tag; | |
94 | const char *name; | |
95 | void *data; | |
96 | u32 size; | |
97 | }; | |
98 | ||
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) | |
101 | { | |
102 | u32 sz; | |
103 | ||
104 | if (p >= cxt->rgn[FT_STRUCT].start + cxt->rgn[FT_STRUCT].size) | |
105 | return NULL; | |
106 | ||
107 | ret->tag = be32_to_cpu(*(u32 *) p); | |
108 | p += 4; | |
109 | ||
110 | switch (ret->tag) { /* Tag */ | |
111 | case OF_DT_BEGIN_NODE: | |
112 | ret->name = p; | |
113 | ret->data = (void *)(p - 4); /* start of node */ | |
114 | p += _ALIGN(strlen(p) + 1, 4); | |
115 | break; | |
116 | case OF_DT_PROP: | |
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); | |
121 | break; | |
122 | case OF_DT_END_NODE: | |
123 | case OF_DT_NOP: | |
124 | break; | |
125 | case OF_DT_END: | |
126 | default: | |
127 | p = NULL; | |
128 | break; | |
129 | } | |
130 | ||
131 | return p; | |
132 | } | |
133 | ||
134 | #define HDR_SIZE _ALIGN(sizeof(struct boot_param_header), 8) | |
135 | #define EXPAND_INCR 1024 /* alloc this much extra when expanding */ | |
136 | ||
6fb4efc6 MG |
137 | /* Copy the tree to a newly-allocated region and put things in order */ |
138 | static int ft_reorder(struct ft_cxt *cxt, int nextra) | |
139 | { | |
140 | unsigned long tot; | |
141 | enum ft_rgn_id r; | |
142 | char *p, *pend; | |
143 | int stroff; | |
144 | ||
145 | tot = HDR_SIZE + EXPAND_INCR; | |
146 | for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) | |
147 | tot += cxt->rgn[r].size; | |
148 | if (nextra > 0) | |
149 | tot += nextra; | |
150 | tot = _ALIGN(tot, 8); | |
151 | ||
152 | if (!cxt->realloc) | |
153 | return 0; | |
154 | p = cxt->realloc(NULL, tot); | |
155 | if (!p) | |
156 | return 0; | |
157 | ||
158 | memcpy(p, cxt->bph, sizeof(struct boot_param_header)); | |
159 | /* offsets get fixed up later */ | |
160 | ||
161 | cxt->bph = (struct boot_param_header *)p; | |
162 | cxt->max_size = tot; | |
163 | pend = p + tot; | |
164 | p += HDR_SIZE; | |
165 | ||
166 | memcpy(p, cxt->rgn[FT_RSVMAP].start, cxt->rgn[FT_RSVMAP].size); | |
167 | cxt->rgn[FT_RSVMAP].start = p; | |
168 | p += cxt->rgn[FT_RSVMAP].size; | |
169 | ||
170 | memcpy(p, cxt->rgn[FT_STRUCT].start, cxt->rgn[FT_STRUCT].size); | |
171 | ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, | |
172 | p - cxt->rgn[FT_STRUCT].start); | |
173 | cxt->p += p - cxt->rgn[FT_STRUCT].start; | |
174 | cxt->rgn[FT_STRUCT].start = p; | |
175 | ||
176 | p = pend - cxt->rgn[FT_STRINGS].size; | |
177 | memcpy(p, cxt->rgn[FT_STRINGS].start, cxt->rgn[FT_STRINGS].size); | |
178 | stroff = cxt->str_anchor - cxt->rgn[FT_STRINGS].start; | |
179 | cxt->rgn[FT_STRINGS].start = p; | |
180 | cxt->str_anchor = p + stroff; | |
181 | ||
182 | cxt->isordered = 1; | |
183 | return 1; | |
184 | } | |
185 | ||
186 | static inline char *prev_end(struct ft_cxt *cxt, enum ft_rgn_id r) | |
187 | { | |
188 | if (r > FT_RSVMAP) | |
189 | return cxt->rgn[r - 1].start + cxt->rgn[r - 1].size; | |
190 | return (char *)cxt->bph + HDR_SIZE; | |
191 | } | |
192 | ||
193 | static inline char *next_start(struct ft_cxt *cxt, enum ft_rgn_id r) | |
194 | { | |
195 | if (r < FT_STRINGS) | |
196 | return cxt->rgn[r + 1].start; | |
197 | return (char *)cxt->bph + cxt->max_size; | |
198 | } | |
199 | ||
200 | /* | |
201 | * See if we can expand region rgn by nextra bytes by using up | |
202 | * free space after or before the region. | |
203 | */ | |
204 | static int ft_shuffle(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn, | |
205 | int nextra) | |
206 | { | |
207 | char *p = *pp; | |
208 | char *rgn_start, *rgn_end; | |
209 | ||
210 | rgn_start = cxt->rgn[rgn].start; | |
211 | rgn_end = rgn_start + cxt->rgn[rgn].size; | |
212 | if (nextra <= 0 || rgn_end + nextra <= next_start(cxt, rgn)) { | |
213 | /* move following stuff */ | |
214 | if (p < rgn_end) { | |
215 | if (nextra < 0) | |
216 | memmove(p, p - nextra, rgn_end - p + nextra); | |
217 | else | |
218 | memmove(p + nextra, p, rgn_end - p); | |
219 | if (rgn == FT_STRUCT) | |
220 | ft_node_update_after(cxt, p, nextra); | |
221 | } | |
222 | cxt->rgn[rgn].size += nextra; | |
223 | if (rgn == FT_STRINGS) | |
224 | /* assumes strings only added at beginning */ | |
225 | cxt->str_anchor += nextra; | |
226 | return 1; | |
227 | } | |
228 | if (prev_end(cxt, rgn) <= rgn_start - nextra) { | |
229 | /* move preceding stuff */ | |
230 | if (p > rgn_start) { | |
231 | memmove(rgn_start - nextra, rgn_start, p - rgn_start); | |
232 | if (rgn == FT_STRUCT) | |
233 | ft_node_update_before(cxt, p, -nextra); | |
234 | } | |
9dd2c31a | 235 | *pp -= nextra; |
6fb4efc6 MG |
236 | cxt->rgn[rgn].start -= nextra; |
237 | cxt->rgn[rgn].size += nextra; | |
238 | return 1; | |
239 | } | |
240 | return 0; | |
241 | } | |
242 | ||
243 | static int ft_make_space(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn, | |
244 | int nextra) | |
245 | { | |
246 | unsigned long size, ssize, tot; | |
247 | char *str, *next; | |
248 | enum ft_rgn_id r; | |
249 | ||
78438b36 SW |
250 | if (!cxt->isordered) { |
251 | unsigned long rgn_off = *pp - cxt->rgn[rgn].start; | |
252 | ||
253 | if (!ft_reorder(cxt, nextra)) | |
254 | return 0; | |
255 | ||
256 | *pp = cxt->rgn[rgn].start + rgn_off; | |
257 | } | |
6fb4efc6 MG |
258 | if (ft_shuffle(cxt, pp, rgn, nextra)) |
259 | return 1; | |
260 | ||
261 | /* See if there is space after the strings section */ | |
262 | ssize = cxt->rgn[FT_STRINGS].size; | |
263 | if (cxt->rgn[FT_STRINGS].start + ssize | |
264 | < (char *)cxt->bph + cxt->max_size) { | |
265 | /* move strings up as far as possible */ | |
266 | str = (char *)cxt->bph + cxt->max_size - ssize; | |
267 | cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start; | |
268 | memmove(str, cxt->rgn[FT_STRINGS].start, ssize); | |
269 | cxt->rgn[FT_STRINGS].start = str; | |
270 | /* enough space now? */ | |
271 | if (rgn >= FT_STRUCT && ft_shuffle(cxt, pp, rgn, nextra)) | |
272 | return 1; | |
273 | } | |
274 | ||
275 | /* how much total free space is there following this region? */ | |
276 | tot = 0; | |
277 | for (r = rgn; r < FT_STRINGS; ++r) { | |
278 | char *r_end = cxt->rgn[r].start + cxt->rgn[r].size; | |
279 | tot += next_start(cxt, rgn) - r_end; | |
280 | } | |
281 | ||
282 | /* cast is to shut gcc up; we know nextra >= 0 */ | |
283 | if (tot < (unsigned int)nextra) { | |
284 | /* have to reallocate */ | |
285 | char *newp, *new_start; | |
286 | int shift; | |
287 | ||
288 | if (!cxt->realloc) | |
289 | return 0; | |
290 | size = _ALIGN(cxt->max_size + (nextra - tot) + EXPAND_INCR, 8); | |
291 | newp = cxt->realloc(cxt->bph, size); | |
292 | if (!newp) | |
293 | return 0; | |
294 | cxt->max_size = size; | |
295 | shift = newp - (char *)cxt->bph; | |
296 | ||
297 | if (shift) { /* realloc can return same addr */ | |
298 | cxt->bph = (struct boot_param_header *)newp; | |
299 | ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, | |
300 | shift); | |
301 | for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) { | |
302 | new_start = cxt->rgn[r].start + shift; | |
303 | cxt->rgn[r].start = new_start; | |
304 | } | |
305 | *pp += shift; | |
306 | cxt->str_anchor += shift; | |
307 | } | |
308 | ||
309 | /* move strings up to the end */ | |
310 | str = newp + size - ssize; | |
311 | cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start; | |
312 | memmove(str, cxt->rgn[FT_STRINGS].start, ssize); | |
313 | cxt->rgn[FT_STRINGS].start = str; | |
314 | ||
315 | if (ft_shuffle(cxt, pp, rgn, nextra)) | |
316 | return 1; | |
317 | } | |
318 | ||
319 | /* must be FT_RSVMAP and we need to move FT_STRUCT up */ | |
320 | if (rgn == FT_RSVMAP) { | |
321 | next = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size | |
322 | + nextra; | |
323 | ssize = cxt->rgn[FT_STRUCT].size; | |
324 | if (next + ssize >= cxt->rgn[FT_STRINGS].start) | |
325 | return 0; /* "can't happen" */ | |
326 | memmove(next, cxt->rgn[FT_STRUCT].start, ssize); | |
327 | ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, nextra); | |
328 | cxt->rgn[FT_STRUCT].start = next; | |
329 | ||
330 | if (ft_shuffle(cxt, pp, rgn, nextra)) | |
331 | return 1; | |
332 | } | |
333 | ||
334 | return 0; /* "can't happen" */ | |
335 | } | |
336 | ||
337 | static void ft_put_word(struct ft_cxt *cxt, u32 v) | |
338 | { | |
339 | *(u32 *) cxt->p = cpu_to_be32(v); | |
340 | cxt->p += 4; | |
341 | } | |
342 | ||
343 | static void ft_put_bin(struct ft_cxt *cxt, const void *data, unsigned int sz) | |
344 | { | |
345 | unsigned long sza = _ALIGN(sz, 4); | |
346 | ||
347 | /* zero out the alignment gap if necessary */ | |
348 | if (sz < sza) | |
349 | *(u32 *) (cxt->p + sza - 4) = 0; | |
350 | ||
351 | /* copy in the data */ | |
352 | memcpy(cxt->p, data, sz); | |
353 | ||
354 | cxt->p += sza; | |
355 | } | |
356 | ||
357 | int ft_begin_node(struct ft_cxt *cxt, const char *name) | |
358 | { | |
359 | unsigned long nlen = strlen(name) + 1; | |
360 | unsigned long len = 8 + _ALIGN(nlen, 4); | |
361 | ||
362 | if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len)) | |
363 | return -1; | |
364 | ft_put_word(cxt, OF_DT_BEGIN_NODE); | |
365 | ft_put_bin(cxt, name, strlen(name) + 1); | |
366 | return 0; | |
367 | } | |
368 | ||
369 | void ft_end_node(struct ft_cxt *cxt) | |
370 | { | |
371 | ft_put_word(cxt, OF_DT_END_NODE); | |
372 | } | |
373 | ||
374 | void ft_nop(struct ft_cxt *cxt) | |
375 | { | |
376 | if (ft_make_space(cxt, &cxt->p, FT_STRUCT, 4)) | |
377 | ft_put_word(cxt, OF_DT_NOP); | |
378 | } | |
379 | ||
380 | #define NO_STRING 0x7fffffff | |
381 | ||
382 | static int lookup_string(struct ft_cxt *cxt, const char *name) | |
383 | { | |
384 | char *p, *end; | |
385 | ||
386 | p = cxt->rgn[FT_STRINGS].start; | |
387 | end = p + cxt->rgn[FT_STRINGS].size; | |
388 | while (p < end) { | |
389 | if (strcmp(p, (char *)name) == 0) | |
390 | return p - cxt->str_anchor; | |
391 | p += strlen(p) + 1; | |
392 | } | |
393 | ||
394 | return NO_STRING; | |
395 | } | |
396 | ||
397 | /* lookup string and insert if not found */ | |
398 | static int map_string(struct ft_cxt *cxt, const char *name) | |
399 | { | |
400 | int off; | |
401 | char *p; | |
402 | ||
403 | off = lookup_string(cxt, name); | |
404 | if (off != NO_STRING) | |
405 | return off; | |
406 | p = cxt->rgn[FT_STRINGS].start; | |
407 | if (!ft_make_space(cxt, &p, FT_STRINGS, strlen(name) + 1)) | |
408 | return NO_STRING; | |
409 | strcpy(p, name); | |
410 | return p - cxt->str_anchor; | |
411 | } | |
412 | ||
413 | int ft_prop(struct ft_cxt *cxt, const char *name, const void *data, | |
414 | unsigned int sz) | |
415 | { | |
416 | int off, len; | |
417 | ||
5adeef52 | 418 | off = map_string(cxt, name); |
6fb4efc6 MG |
419 | if (off == NO_STRING) |
420 | return -1; | |
421 | ||
422 | len = 12 + _ALIGN(sz, 4); | |
423 | if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len)) | |
424 | return -1; | |
425 | ||
426 | ft_put_word(cxt, OF_DT_PROP); | |
427 | ft_put_word(cxt, sz); | |
428 | ft_put_word(cxt, off); | |
429 | ft_put_bin(cxt, data, sz); | |
430 | return 0; | |
431 | } | |
432 | ||
433 | int ft_prop_str(struct ft_cxt *cxt, const char *name, const char *str) | |
434 | { | |
435 | return ft_prop(cxt, name, str, strlen(str) + 1); | |
436 | } | |
437 | ||
438 | int ft_prop_int(struct ft_cxt *cxt, const char *name, unsigned int val) | |
439 | { | |
440 | u32 v = cpu_to_be32((u32) val); | |
441 | ||
442 | return ft_prop(cxt, name, &v, 4); | |
443 | } | |
444 | ||
445 | /* Calculate the size of the reserved map */ | |
446 | static unsigned long rsvmap_size(struct ft_cxt *cxt) | |
447 | { | |
448 | struct ft_reserve *res; | |
449 | ||
450 | res = (struct ft_reserve *)cxt->rgn[FT_RSVMAP].start; | |
451 | while (res->start || res->len) | |
452 | ++res; | |
453 | return (char *)(res + 1) - cxt->rgn[FT_RSVMAP].start; | |
454 | } | |
455 | ||
456 | /* Calculate the size of the struct region by stepping through it */ | |
457 | static unsigned long struct_size(struct ft_cxt *cxt) | |
458 | { | |
459 | char *p = cxt->rgn[FT_STRUCT].start; | |
460 | char *next; | |
461 | struct ft_atom atom; | |
462 | ||
463 | /* make check in ft_next happy */ | |
464 | if (cxt->rgn[FT_STRUCT].size == 0) | |
465 | cxt->rgn[FT_STRUCT].size = 0xfffffffful - (unsigned long)p; | |
466 | ||
467 | while ((next = ft_next(cxt, p, &atom)) != NULL) | |
468 | p = next; | |
469 | return p + 4 - cxt->rgn[FT_STRUCT].start; | |
470 | } | |
471 | ||
472 | /* add `adj' on to all string offset values in the struct area */ | |
473 | static void adjust_string_offsets(struct ft_cxt *cxt, int adj) | |
474 | { | |
475 | char *p = cxt->rgn[FT_STRUCT].start; | |
476 | char *next; | |
477 | struct ft_atom atom; | |
478 | int off; | |
479 | ||
480 | while ((next = ft_next(cxt, p, &atom)) != NULL) { | |
481 | if (atom.tag == OF_DT_PROP) { | |
482 | off = be32_to_cpu(*(u32 *) (p + 8)); | |
483 | *(u32 *) (p + 8) = cpu_to_be32(off + adj); | |
484 | } | |
485 | p = next; | |
486 | } | |
487 | } | |
488 | ||
489 | /* start construction of the flat OF tree from scratch */ | |
490 | void ft_begin(struct ft_cxt *cxt, void *blob, unsigned int max_size, | |
491 | void *(*realloc_fn) (void *, unsigned long)) | |
492 | { | |
493 | struct boot_param_header *bph = blob; | |
494 | char *p; | |
495 | struct ft_reserve *pres; | |
496 | ||
497 | /* clear the cxt */ | |
498 | memset(cxt, 0, sizeof(*cxt)); | |
499 | ||
500 | cxt->bph = bph; | |
501 | cxt->max_size = max_size; | |
502 | cxt->realloc = realloc_fn; | |
503 | cxt->isordered = 1; | |
504 | ||
505 | /* zero everything in the header area */ | |
506 | memset(bph, 0, sizeof(*bph)); | |
507 | ||
508 | bph->magic = cpu_to_be32(OF_DT_HEADER); | |
509 | bph->version = cpu_to_be32(0x10); | |
510 | bph->last_comp_version = cpu_to_be32(0x10); | |
511 | ||
512 | /* start pointers */ | |
513 | cxt->rgn[FT_RSVMAP].start = p = blob + HDR_SIZE; | |
514 | cxt->rgn[FT_RSVMAP].size = sizeof(struct ft_reserve); | |
515 | pres = (struct ft_reserve *)p; | |
516 | cxt->rgn[FT_STRUCT].start = p += sizeof(struct ft_reserve); | |
517 | cxt->rgn[FT_STRUCT].size = 4; | |
518 | cxt->rgn[FT_STRINGS].start = blob + max_size; | |
519 | cxt->rgn[FT_STRINGS].size = 0; | |
520 | ||
521 | /* init rsvmap and struct */ | |
522 | pres->start = 0; | |
523 | pres->len = 0; | |
524 | *(u32 *) p = cpu_to_be32(OF_DT_END); | |
525 | ||
526 | cxt->str_anchor = blob; | |
527 | } | |
528 | ||
529 | /* open up an existing blob to be examined or modified */ | |
530 | int ft_open(struct ft_cxt *cxt, void *blob, unsigned int max_size, | |
531 | unsigned int max_find_device, | |
532 | void *(*realloc_fn) (void *, unsigned long)) | |
533 | { | |
534 | struct boot_param_header *bph = blob; | |
535 | ||
536 | /* can't cope with version < 16 */ | |
537 | if (be32_to_cpu(bph->version) < 16) | |
538 | return -1; | |
539 | ||
540 | /* clear the cxt */ | |
541 | memset(cxt, 0, sizeof(*cxt)); | |
542 | ||
543 | /* alloc node_tbl to track node ptrs returned by ft_find_device */ | |
544 | ++max_find_device; | |
545 | cxt->node_tbl = realloc_fn(NULL, max_find_device * sizeof(char *)); | |
546 | if (!cxt->node_tbl) | |
547 | return -1; | |
548 | memset(cxt->node_tbl, 0, max_find_device * sizeof(char *)); | |
549 | cxt->node_max = max_find_device; | |
550 | cxt->nodes_used = 1; /* don't use idx 0 b/c looks like NULL */ | |
551 | ||
552 | cxt->bph = bph; | |
553 | cxt->max_size = max_size; | |
554 | cxt->realloc = realloc_fn; | |
555 | ||
556 | cxt->rgn[FT_RSVMAP].start = blob + be32_to_cpu(bph->off_mem_rsvmap); | |
557 | cxt->rgn[FT_RSVMAP].size = rsvmap_size(cxt); | |
558 | cxt->rgn[FT_STRUCT].start = blob + be32_to_cpu(bph->off_dt_struct); | |
559 | cxt->rgn[FT_STRUCT].size = struct_size(cxt); | |
560 | cxt->rgn[FT_STRINGS].start = blob + be32_to_cpu(bph->off_dt_strings); | |
561 | cxt->rgn[FT_STRINGS].size = be32_to_cpu(bph->dt_strings_size); | |
6fb4efc6 MG |
562 | |
563 | cxt->p = cxt->rgn[FT_STRUCT].start; | |
564 | cxt->str_anchor = cxt->rgn[FT_STRINGS].start; | |
565 | ||
566 | return 0; | |
567 | } | |
568 | ||
569 | /* add a reserver physical area to the rsvmap */ | |
570 | int ft_add_rsvmap(struct ft_cxt *cxt, u64 physaddr, u64 size) | |
571 | { | |
572 | char *p; | |
573 | struct ft_reserve *pres; | |
574 | ||
575 | p = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size | |
576 | - sizeof(struct ft_reserve); | |
577 | if (!ft_make_space(cxt, &p, FT_RSVMAP, sizeof(struct ft_reserve))) | |
578 | return -1; | |
579 | ||
580 | pres = (struct ft_reserve *)p; | |
581 | pres->start = cpu_to_be64(physaddr); | |
582 | pres->len = cpu_to_be64(size); | |
583 | ||
584 | return 0; | |
585 | } | |
586 | ||
587 | void ft_begin_tree(struct ft_cxt *cxt) | |
588 | { | |
7c71c046 | 589 | cxt->p = ft_root_node(cxt); |
6fb4efc6 MG |
590 | } |
591 | ||
592 | void ft_end_tree(struct ft_cxt *cxt) | |
593 | { | |
594 | struct boot_param_header *bph = cxt->bph; | |
595 | char *p, *oldstr, *str, *endp; | |
596 | unsigned long ssize; | |
597 | int adj; | |
598 | ||
599 | if (!cxt->isordered) | |
600 | return; /* we haven't touched anything */ | |
601 | ||
602 | /* adjust string offsets */ | |
603 | oldstr = cxt->rgn[FT_STRINGS].start; | |
604 | adj = cxt->str_anchor - oldstr; | |
605 | if (adj) | |
606 | adjust_string_offsets(cxt, adj); | |
607 | ||
608 | /* make strings end on 8-byte boundary */ | |
609 | ssize = cxt->rgn[FT_STRINGS].size; | |
610 | endp = (char *)_ALIGN((unsigned long)cxt->rgn[FT_STRUCT].start | |
611 | + cxt->rgn[FT_STRUCT].size + ssize, 8); | |
612 | str = endp - ssize; | |
613 | ||
614 | /* move strings down to end of structs */ | |
615 | memmove(str, oldstr, ssize); | |
616 | cxt->str_anchor = str; | |
617 | cxt->rgn[FT_STRINGS].start = str; | |
618 | ||
619 | /* fill in header fields */ | |
620 | p = (char *)bph; | |
621 | bph->totalsize = cpu_to_be32(endp - p); | |
622 | bph->off_mem_rsvmap = cpu_to_be32(cxt->rgn[FT_RSVMAP].start - p); | |
623 | bph->off_dt_struct = cpu_to_be32(cxt->rgn[FT_STRUCT].start - p); | |
624 | bph->off_dt_strings = cpu_to_be32(cxt->rgn[FT_STRINGS].start - p); | |
625 | bph->dt_strings_size = cpu_to_be32(ssize); | |
626 | } | |
627 | ||
628 | void *ft_find_device(struct ft_cxt *cxt, const char *srch_path) | |
629 | { | |
630 | char *node; | |
631 | ||
632 | /* require absolute path */ | |
633 | if (srch_path[0] != '/') | |
634 | return NULL; | |
7c71c046 | 635 | node = ft_find_descendent(cxt, ft_root_node(cxt), srch_path); |
1c53a496 | 636 | return ft_get_phandle(cxt, node); |
6fb4efc6 MG |
637 | } |
638 | ||
fc583411 SW |
639 | void *ft_find_device_rel(struct ft_cxt *cxt, const void *top, |
640 | const char *srch_path) | |
641 | { | |
642 | char *node; | |
643 | ||
644 | node = ft_node_ph2node(cxt, top); | |
645 | if (node == NULL) | |
646 | return NULL; | |
647 | ||
648 | node = ft_find_descendent(cxt, node, srch_path); | |
649 | return ft_get_phandle(cxt, node); | |
650 | } | |
651 | ||
6fb4efc6 MG |
652 | void *ft_find_descendent(struct ft_cxt *cxt, void *top, const char *srch_path) |
653 | { | |
654 | struct ft_atom atom; | |
655 | char *p; | |
656 | const char *cp, *q; | |
657 | int cl; | |
658 | int depth = -1; | |
659 | int dmatch = 0; | |
660 | const char *path_comp[FT_MAX_DEPTH]; | |
661 | ||
662 | cp = srch_path; | |
663 | cl = 0; | |
664 | p = top; | |
665 | ||
666 | while ((p = ft_next(cxt, p, &atom)) != NULL) { | |
667 | switch (atom.tag) { | |
668 | case OF_DT_BEGIN_NODE: | |
669 | ++depth; | |
670 | if (depth != dmatch) | |
671 | break; | |
672 | cxt->genealogy[depth] = atom.data; | |
673 | cxt->genealogy[depth + 1] = NULL; | |
674 | if (depth && !(strncmp(atom.name, cp, cl) == 0 | |
675 | && (atom.name[cl] == '/' | |
676 | || atom.name[cl] == '\0' | |
677 | || atom.name[cl] == '@'))) | |
678 | break; | |
679 | path_comp[dmatch] = cp; | |
680 | /* it matches so far, advance to next path component */ | |
681 | cp += cl; | |
682 | /* skip slashes */ | |
683 | while (*cp == '/') | |
684 | ++cp; | |
685 | /* we're done if this is the end of the string */ | |
686 | if (*cp == 0) | |
687 | return atom.data; | |
688 | /* look for end of this component */ | |
689 | q = strchr(cp, '/'); | |
690 | if (q) | |
691 | cl = q - cp; | |
692 | else | |
693 | cl = strlen(cp); | |
694 | ++dmatch; | |
695 | break; | |
696 | case OF_DT_END_NODE: | |
697 | if (depth == 0) | |
698 | return NULL; | |
699 | if (dmatch > depth) { | |
700 | --dmatch; | |
701 | cl = cp - path_comp[dmatch] - 1; | |
702 | cp = path_comp[dmatch]; | |
703 | while (cl > 0 && cp[cl - 1] == '/') | |
704 | --cl; | |
705 | } | |
706 | --depth; | |
707 | break; | |
708 | } | |
709 | } | |
710 | return NULL; | |
711 | } | |
712 | ||
a9ec7669 | 713 | void *__ft_get_parent(struct ft_cxt *cxt, void *node) |
6fb4efc6 | 714 | { |
6fb4efc6 MG |
715 | int d; |
716 | struct ft_atom atom; | |
717 | char *p; | |
718 | ||
6fb4efc6 MG |
719 | for (d = 0; cxt->genealogy[d] != NULL; ++d) |
720 | if (cxt->genealogy[d] == node) | |
a9ec7669 | 721 | return d > 0 ? cxt->genealogy[d - 1] : NULL; |
6fb4efc6 MG |
722 | |
723 | /* have to do it the hard way... */ | |
7c71c046 | 724 | p = ft_root_node(cxt); |
6fb4efc6 MG |
725 | d = 0; |
726 | while ((p = ft_next(cxt, p, &atom)) != NULL) { | |
727 | switch (atom.tag) { | |
728 | case OF_DT_BEGIN_NODE: | |
729 | cxt->genealogy[d] = atom.data; | |
730 | if (node == atom.data) { | |
731 | /* found it */ | |
732 | cxt->genealogy[d + 1] = NULL; | |
a9ec7669 | 733 | return d > 0 ? cxt->genealogy[d - 1] : NULL; |
6fb4efc6 MG |
734 | } |
735 | ++d; | |
736 | break; | |
737 | case OF_DT_END_NODE: | |
738 | --d; | |
739 | break; | |
740 | } | |
741 | } | |
742 | return NULL; | |
743 | } | |
744 | ||
a9ec7669 SW |
745 | void *ft_get_parent(struct ft_cxt *cxt, const void *phandle) |
746 | { | |
747 | void *node = ft_node_ph2node(cxt, phandle); | |
748 | if (node == NULL) | |
749 | return NULL; | |
750 | ||
751 | node = __ft_get_parent(cxt, node); | |
752 | return ft_get_phandle(cxt, node); | |
753 | } | |
754 | ||
c350038b SW |
755 | static const void *__ft_get_prop(struct ft_cxt *cxt, void *node, |
756 | const char *propname, unsigned int *len) | |
6fb4efc6 MG |
757 | { |
758 | struct ft_atom atom; | |
c350038b | 759 | int depth = 0; |
6fb4efc6 | 760 | |
c350038b | 761 | while ((node = ft_next(cxt, node, &atom)) != NULL) { |
6fb4efc6 MG |
762 | switch (atom.tag) { |
763 | case OF_DT_BEGIN_NODE: | |
764 | ++depth; | |
765 | break; | |
c350038b | 766 | |
6fb4efc6 | 767 | case OF_DT_PROP: |
c350038b | 768 | if (depth != 1 || strcmp(atom.name, propname)) |
6fb4efc6 | 769 | break; |
c350038b SW |
770 | |
771 | if (len) | |
772 | *len = atom.size; | |
773 | ||
774 | return atom.data; | |
775 | ||
6fb4efc6 MG |
776 | case OF_DT_END_NODE: |
777 | if (--depth <= 0) | |
c350038b | 778 | return NULL; |
6fb4efc6 MG |
779 | } |
780 | } | |
c350038b SW |
781 | |
782 | return NULL; | |
783 | } | |
784 | ||
785 | int ft_get_prop(struct ft_cxt *cxt, const void *phandle, const char *propname, | |
786 | void *buf, const unsigned int buflen) | |
787 | { | |
788 | const void *data; | |
789 | unsigned int size; | |
790 | ||
791 | void *node = ft_node_ph2node(cxt, phandle); | |
792 | if (!node) | |
793 | return -1; | |
794 | ||
795 | data = __ft_get_prop(cxt, node, propname, &size); | |
796 | if (data) { | |
797 | unsigned int clipped_size = min(size, buflen); | |
798 | memcpy(buf, data, clipped_size); | |
799 | return size; | |
800 | } | |
801 | ||
6fb4efc6 MG |
802 | return -1; |
803 | } | |
804 | ||
8941c0c4 SW |
805 | void *__ft_find_node_by_prop_value(struct ft_cxt *cxt, void *prev, |
806 | const char *propname, const char *propval, | |
807 | unsigned int proplen) | |
808 | { | |
809 | struct ft_atom atom; | |
810 | char *p = ft_root_node(cxt); | |
811 | char *next; | |
812 | int past_prev = prev ? 0 : 1; | |
813 | int depth = -1; | |
814 | ||
815 | while ((next = ft_next(cxt, p, &atom)) != NULL) { | |
816 | const void *data; | |
817 | unsigned int size; | |
818 | ||
819 | switch (atom.tag) { | |
820 | case OF_DT_BEGIN_NODE: | |
821 | depth++; | |
822 | ||
823 | if (prev == p) { | |
824 | past_prev = 1; | |
825 | break; | |
826 | } | |
827 | ||
828 | if (!past_prev || depth < 1) | |
829 | break; | |
830 | ||
831 | data = __ft_get_prop(cxt, p, propname, &size); | |
832 | if (!data || size != proplen) | |
833 | break; | |
834 | if (memcmp(data, propval, size)) | |
835 | break; | |
836 | ||
837 | return p; | |
838 | ||
839 | case OF_DT_END_NODE: | |
840 | if (depth-- == 0) | |
841 | return NULL; | |
842 | ||
843 | break; | |
844 | } | |
845 | ||
846 | p = next; | |
847 | } | |
848 | ||
849 | return NULL; | |
850 | } | |
851 | ||
852 | void *ft_find_node_by_prop_value(struct ft_cxt *cxt, const void *prev, | |
853 | const char *propname, const char *propval, | |
854 | int proplen) | |
855 | { | |
856 | void *node = NULL; | |
857 | ||
858 | if (prev) { | |
859 | node = ft_node_ph2node(cxt, prev); | |
860 | ||
861 | if (!node) | |
862 | return NULL; | |
863 | } | |
864 | ||
865 | node = __ft_find_node_by_prop_value(cxt, node, propname, | |
866 | propval, proplen); | |
867 | return ft_get_phandle(cxt, node); | |
868 | } | |
869 | ||
6fb4efc6 MG |
870 | int ft_set_prop(struct ft_cxt *cxt, const void *phandle, const char *propname, |
871 | const void *buf, const unsigned int buflen) | |
872 | { | |
873 | struct ft_atom atom; | |
874 | void *node; | |
875 | char *p, *next; | |
7c40542f | 876 | int nextra; |
6fb4efc6 MG |
877 | |
878 | node = ft_node_ph2node(cxt, phandle); | |
879 | if (node == NULL) | |
880 | return -1; | |
881 | ||
7c40542f DG |
882 | next = ft_next(cxt, node, &atom); |
883 | if (atom.tag != OF_DT_BEGIN_NODE) | |
884 | /* phandle didn't point to a node */ | |
885 | return -1; | |
886 | p = next; | |
6fb4efc6 MG |
887 | |
888 | while ((next = ft_next(cxt, p, &atom)) != NULL) { | |
889 | switch (atom.tag) { | |
7c40542f | 890 | case OF_DT_BEGIN_NODE: /* properties must go before subnodes */ |
6fb4efc6 | 891 | case OF_DT_END_NODE: |
6fb4efc6 MG |
892 | /* haven't found the property, insert here */ |
893 | cxt->p = p; | |
894 | return ft_prop(cxt, propname, buf, buflen); | |
895 | case OF_DT_PROP: | |
7c40542f | 896 | if (strcmp(atom.name, propname)) |
6fb4efc6 MG |
897 | break; |
898 | /* found an existing property, overwrite it */ | |
899 | nextra = _ALIGN(buflen, 4) - _ALIGN(atom.size, 4); | |
900 | cxt->p = atom.data; | |
901 | if (nextra && !ft_make_space(cxt, &cxt->p, FT_STRUCT, | |
902 | nextra)) | |
903 | return -1; | |
904 | *(u32 *) (cxt->p - 8) = cpu_to_be32(buflen); | |
905 | ft_put_bin(cxt, buf, buflen); | |
906 | return 0; | |
907 | } | |
908 | p = next; | |
909 | } | |
910 | return -1; | |
911 | } | |
912 | ||
913 | int ft_del_prop(struct ft_cxt *cxt, const void *phandle, const char *propname) | |
914 | { | |
915 | struct ft_atom atom; | |
916 | void *node; | |
917 | char *p, *next; | |
918 | int size; | |
919 | ||
920 | node = ft_node_ph2node(cxt, phandle); | |
921 | if (node == NULL) | |
922 | return -1; | |
923 | ||
924 | p = node; | |
925 | while ((next = ft_next(cxt, p, &atom)) != NULL) { | |
926 | switch (atom.tag) { | |
927 | case OF_DT_BEGIN_NODE: | |
928 | case OF_DT_END_NODE: | |
929 | return -1; | |
930 | case OF_DT_PROP: | |
931 | if (strcmp(atom.name, propname)) | |
932 | break; | |
933 | /* found the property, remove it */ | |
934 | size = 12 + -_ALIGN(atom.size, 4); | |
935 | cxt->p = p; | |
936 | if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, -size)) | |
937 | return -1; | |
938 | return 0; | |
939 | } | |
940 | p = next; | |
941 | } | |
942 | return -1; | |
943 | } | |
944 | ||
86a1b633 | 945 | void *ft_create_node(struct ft_cxt *cxt, const void *parent, const char *name) |
6fb4efc6 MG |
946 | { |
947 | struct ft_atom atom; | |
948 | char *p, *next; | |
949 | int depth = 0; | |
950 | ||
86a1b633 SW |
951 | if (parent) { |
952 | p = ft_node_ph2node(cxt, parent); | |
953 | if (!p) | |
954 | return NULL; | |
955 | } else { | |
956 | p = ft_root_node(cxt); | |
957 | } | |
958 | ||
6fb4efc6 MG |
959 | while ((next = ft_next(cxt, p, &atom)) != NULL) { |
960 | switch (atom.tag) { | |
961 | case OF_DT_BEGIN_NODE: | |
962 | ++depth; | |
86a1b633 SW |
963 | if (depth == 1 && strcmp(atom.name, name) == 0) |
964 | /* duplicate node name, return error */ | |
6fb4efc6 MG |
965 | return NULL; |
966 | break; | |
967 | case OF_DT_END_NODE: | |
968 | --depth; | |
969 | if (depth > 0) | |
970 | break; | |
971 | /* end of node, insert here */ | |
972 | cxt->p = p; | |
86a1b633 | 973 | ft_begin_node(cxt, name); |
6fb4efc6 MG |
974 | ft_end_node(cxt); |
975 | return p; | |
976 | } | |
977 | p = next; | |
978 | } | |
979 | return NULL; | |
980 | } |