1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright 2009 Benjamin Herrenschmidt, IBM Corp
4 * benh@kernel.crashing.org
5 *
6 * Based on parts of drivers/of/fdt.c from Linux v4.9
7 * Modifications for U-Boot
8 * Copyright (c) 2017 Google, Inc
9 */
10
11 #define LOG_CATEGORY LOGC_DT
12
13 #include <abuf.h>
14 #include <event.h>
15 #include <log.h>
16 #include <linux/libfdt.h>
17 #include <of_live.h>
18 #include <malloc.h>
19 #include <dm/of_access.h>
20 #include <linux/err.h>
21 #include <linux/sizes.h>
22
23 enum {
24 BUF_STEP = SZ_64K,
25 };
26
unflatten_dt_alloc(void ** mem,unsigned long size,unsigned long align)27 static void *unflatten_dt_alloc(void **mem, unsigned long size,
28 unsigned long align)
29 {
30 void *res;
31
32 *mem = PTR_ALIGN(*mem, align);
33 res = *mem;
34 *mem += size;
35
36 return res;
37 }
38
39 /**
40 * unflatten_dt_node() - Alloc and populate a device_node from the flat tree
41 * @blob: The parent device tree blob
42 * @mem: Memory chunk to use for allocating device nodes and properties
43 * @poffset: pointer to node in flat tree
44 * @dad: Parent struct device_node
45 * @nodepp: The device_node tree created by the call
46 * @fpsize: Size of the node path up at t05he current depth.
47 * @dryrun: If true, do not allocate device nodes but still calculate needed
48 * memory size
49 */
unflatten_dt_node(const void * blob,void * mem,int * poffset,struct device_node * dad,struct device_node ** nodepp,unsigned long fpsize,bool dryrun)50 static void *unflatten_dt_node(const void *blob, void *mem, int *poffset,
51 struct device_node *dad,
52 struct device_node **nodepp,
53 unsigned long fpsize, bool dryrun)
54 {
55 const __be32 *p;
56 struct device_node *np;
57 struct property *pp, **prev_pp = NULL;
58 const char *pathp;
59 int l;
60 unsigned int allocl;
61 static int depth;
62 int old_depth;
63 int offset;
64 int has_name = 0;
65 int new_format = 0;
66
67 pathp = fdt_get_name(blob, *poffset, &l);
68 if (!pathp)
69 return mem;
70
71 allocl = ++l;
72
73 /*
74 * version 0x10 has a more compact unit name here instead of the full
75 * path. we accumulate the full path size using "fpsize", we'll rebuild
76 * it later. We detect this because the first character of the name is
77 * not '/'.
78 */
79 if ((*pathp) != '/') {
80 new_format = 1;
81 if (fpsize == 0) {
82 /*
83 * root node: special case. fpsize accounts for path
84 * plus terminating zero. root node only has '/', so
85 * fpsize should be 2, but we want to avoid the first
86 * level nodes to have two '/' so we use fpsize 1 here
87 */
88 fpsize = 1;
89 allocl = 2;
90 l = 1;
91 pathp = "";
92 } else {
93 /*
94 * account for '/' and path size minus terminal 0
95 * already in 'l'
96 */
97 fpsize += l;
98 allocl = fpsize;
99 }
100 }
101
102 np = unflatten_dt_alloc(&mem, sizeof(struct device_node) + allocl,
103 __alignof__(struct device_node));
104 if (!dryrun) {
105 char *fn;
106
107 fn = (char *)np + sizeof(*np);
108 if (new_format) {
109 np->name = pathp;
110 has_name = 1;
111 }
112 np->full_name = fn;
113 if (new_format) {
114 /* rebuild full path for new format */
115 if (dad && dad->parent) {
116 strcpy(fn, dad->full_name);
117 #ifdef DEBUG
118 if ((strlen(fn) + l + 1) != allocl) {
119 debug("%s: p: %d, l: %d, a: %d\n",
120 pathp, (int)strlen(fn), l,
121 allocl);
122 }
123 #endif
124 fn += strlen(fn);
125 }
126 *(fn++) = '/';
127 }
128 memcpy(fn, pathp, l);
129
130 prev_pp = &np->properties;
131 if (dad != NULL) {
132 np->parent = dad;
133 np->sibling = dad->child;
134 dad->child = np;
135 }
136 }
137 /* process properties */
138 for (offset = fdt_first_property_offset(blob, *poffset);
139 (offset >= 0);
140 (offset = fdt_next_property_offset(blob, offset))) {
141 const char *pname;
142 int sz;
143
144 p = fdt_getprop_by_offset(blob, offset, &pname, &sz);
145 if (!p) {
146 offset = -FDT_ERR_INTERNAL;
147 break;
148 }
149
150 if (pname == NULL) {
151 debug("Can't find property name in list !\n");
152 break;
153 }
154 if (strcmp(pname, "name") == 0)
155 has_name = 1;
156 pp = unflatten_dt_alloc(&mem, sizeof(struct property),
157 __alignof__(struct property));
158 if (!dryrun) {
159 /*
160 * We accept flattened tree phandles either in
161 * ePAPR-style "phandle" properties, or the
162 * legacy "linux,phandle" properties. If both
163 * appear and have different values, things
164 * will get weird. Don't do that. */
165 if ((strcmp(pname, "phandle") == 0) ||
166 (strcmp(pname, "linux,phandle") == 0)) {
167 if (np->phandle == 0)
168 np->phandle = be32_to_cpup(p);
169 }
170 /*
171 * And we process the "ibm,phandle" property
172 * used in pSeries dynamic device tree
173 * stuff */
174 if (strcmp(pname, "ibm,phandle") == 0)
175 np->phandle = be32_to_cpup(p);
176 pp->name = (char *)pname;
177 pp->length = sz;
178 pp->value = (__be32 *)p;
179 *prev_pp = pp;
180 prev_pp = &pp->next;
181 }
182 }
183 /*
184 * with version 0x10 we may not have the name property, recreate
185 * it here from the unit name if absent
186 */
187 if (!has_name) {
188 const char *p1 = pathp, *ps = pathp, *pa = NULL;
189 int sz;
190
191 while (*p1) {
192 if ((*p1) == '@')
193 pa = p1;
194 if ((*p1) == '/')
195 ps = p1 + 1;
196 p1++;
197 }
198 if (pa < ps)
199 pa = p1;
200 sz = (pa - ps) + 1;
201 pp = unflatten_dt_alloc(&mem, sizeof(struct property) + sz,
202 __alignof__(struct property));
203 if (!dryrun) {
204 pp->name = "name";
205 pp->length = sz;
206 pp->value = pp + 1;
207 *prev_pp = pp;
208 prev_pp = &pp->next;
209 memcpy(pp->value, ps, sz - 1);
210 ((char *)pp->value)[sz - 1] = 0;
211 debug("fixed up name for %s -> %s\n", pathp,
212 (char *)pp->value);
213 }
214 }
215 if (!dryrun) {
216 *prev_pp = NULL;
217 if (!has_name)
218 np->name = of_get_property(np, "name", NULL);
219 np->type = of_get_property(np, "device_type", NULL);
220
221 if (!np->name)
222 np->name = "<NULL>";
223 if (!np->type)
224 np->type = "<NULL>"; }
225
226 old_depth = depth;
227 *poffset = fdt_next_node(blob, *poffset, &depth);
228 if (depth < 0)
229 depth = 0;
230 while (*poffset > 0 && depth > old_depth) {
231 mem = unflatten_dt_node(blob, mem, poffset, np, NULL,
232 fpsize, dryrun);
233 if (!mem)
234 return NULL;
235 }
236
237 if (*poffset < 0 && *poffset != -FDT_ERR_NOTFOUND) {
238 debug("unflatten: error %d processing FDT\n", *poffset);
239 return NULL;
240 }
241
242 /*
243 * Reverse the child list. Some drivers assumes node order matches .dts
244 * node order
245 */
246 if (!dryrun && np->child) {
247 struct device_node *child = np->child;
248 np->child = NULL;
249 while (child) {
250 struct device_node *next = child->sibling;
251
252 child->sibling = np->child;
253 np->child = child;
254 child = next;
255 }
256 }
257
258 if (nodepp)
259 *nodepp = np;
260
261 return mem;
262 }
263
unflatten_device_tree(const void * blob,struct device_node ** mynodes)264 int unflatten_device_tree(const void *blob, struct device_node **mynodes)
265 {
266 unsigned long size;
267 int start;
268 void *mem;
269
270 debug(" -> unflatten_device_tree()\n");
271
272 if (!blob) {
273 debug("No device tree pointer\n");
274 return -EINVAL;
275 }
276
277 debug("Unflattening device tree:\n");
278 debug("magic: %08x\n", fdt_magic(blob));
279 debug("size: %08x\n", fdt_totalsize(blob));
280 debug("version: %08x\n", fdt_version(blob));
281
282 if (fdt_check_header(blob)) {
283 debug("Invalid device tree blob header\n");
284 return -EINVAL;
285 }
286
287 /* First pass, scan for size */
288 start = 0;
289 size = (unsigned long)unflatten_dt_node(blob, NULL, &start, NULL, NULL,
290 0, true);
291 if (!size)
292 return -EFAULT;
293 size = ALIGN(size, 4);
294
295 debug(" size is %lx, allocating...\n", size);
296
297 /* Allocate memory for the expanded device tree */
298 mem = memalign(__alignof__(struct device_node), size + 4);
299 memset(mem, '\0', size);
300
301 /* Set up value for dm_test_livetree_align() */
302 *(u32 *)mem = BAD_OF_ROOT;
303
304 *(__be32 *)(mem + size) = cpu_to_be32(0xdeadbeef);
305
306 debug(" unflattening %p...\n", mem);
307
308 /* Second pass, do actual unflattening */
309 start = 0;
310 unflatten_dt_node(blob, mem, &start, NULL, mynodes, 0, false);
311 if (be32_to_cpup(mem + size) != 0xdeadbeef) {
312 debug("End of tree marker overwritten: %08x\n",
313 be32_to_cpup(mem + size));
314 return -ENOSPC;
315 }
316
317 debug(" <- unflatten_device_tree()\n");
318
319 return 0;
320 }
321
of_live_build(const void * fdt_blob,struct device_node ** rootp)322 int of_live_build(const void *fdt_blob, struct device_node **rootp)
323 {
324 int ret;
325 union event_data evt;
326
327 debug("%s: start\n", __func__);
328 ret = unflatten_device_tree(fdt_blob, rootp);
329 if (ret) {
330 debug("Failed to create live tree: err=%d\n", ret);
331 return ret;
332 }
333 ret = of_alias_scan();
334 if (ret) {
335 debug("Failed to scan live tree aliases: err=%d\n", ret);
336 return ret;
337 }
338 debug("%s: stop\n", __func__);
339
340 if (CONFIG_IS_ENABLED(EVENT)) {
341 evt.of_live_built.root = *rootp;
342 ret = event_notify(EVT_OF_LIVE_BUILT, &evt, sizeof(evt));
343 if (ret) {
344 log_debug("Failed to notify livetree build event: err=%d\n", ret);
345 return ret;
346 }
347 }
348
349 return ret;
350 }
351
of_live_free(struct device_node * root)352 void of_live_free(struct device_node *root)
353 {
354 /* the tree is stored as a contiguous block of memory */
355 free(root);
356 }
357
of_live_create_empty(struct device_node ** rootp)358 int of_live_create_empty(struct device_node **rootp)
359 {
360 struct device_node *root;
361
362 root = calloc(1, sizeof(struct device_node));
363 if (!root)
364 return -ENOMEM;
365 root->name = strdup("");
366 if (!root->name) {
367 free(root);
368 return -ENOMEM;
369 }
370 root->type = "<NULL>";
371 root->full_name = "";
372 *rootp = root;
373
374 return 0;
375 }
376
check_space(int ret,struct abuf * buf)377 static int check_space(int ret, struct abuf *buf)
378 {
379 if (ret == -FDT_ERR_NOSPACE) {
380 if (!abuf_realloc_inc(buf, BUF_STEP))
381 return log_msg_ret("spc", -ENOMEM);
382 ret = fdt_resize(abuf_data(buf), abuf_data(buf),
383 abuf_size(buf));
384 if (ret)
385 return log_msg_ret("res", -EFAULT);
386
387 return -EAGAIN;
388 }
389
390 return 0;
391 }
392
393 /**
394 * flatten_node() - Write out the node and its properties into a flat tree
395 */
flatten_node(struct abuf * buf,const struct device_node * node)396 static int flatten_node(struct abuf *buf, const struct device_node *node)
397 {
398 const struct device_node *np;
399 const struct property *pp;
400 int ret;
401
402 ret = fdt_begin_node(abuf_data(buf), node->name);
403 ret = check_space(ret, buf);
404 if (ret == -EAGAIN) {
405 ret = fdt_begin_node(abuf_data(buf), node->name);
406 if (ret) {
407 log_debug("Internal error a %d\n", ret);
408 return -EFAULT;
409 }
410 }
411 if (ret)
412 return log_msg_ret("beg", ret);
413
414 /* First write out the properties */
415 for (pp = node->properties; !ret && pp; pp = pp->next) {
416 ret = fdt_property(abuf_data(buf), pp->name, pp->value,
417 pp->length);
418 ret = check_space(ret, buf);
419 if (ret == -EAGAIN) {
420 ret = fdt_property(abuf_data(buf), pp->name, pp->value,
421 pp->length);
422 }
423 }
424
425 /* Next write out the subnodes */
426 for (np = node->child; np; np = np->sibling) {
427 ret = flatten_node(buf, np);
428 if (ret)
429 return log_msg_ret("sub", ret);
430 }
431
432 ret = fdt_end_node(abuf_data(buf));
433 ret = check_space(ret, buf);
434 if (ret == -EAGAIN) {
435 ret = fdt_end_node(abuf_data(buf));
436 if (ret) {
437 log_debug("Internal error b %d\n", ret);
438 return -EFAULT;
439 }
440 }
441 if (ret)
442 return log_msg_ret("end", ret);
443
444 return 0;
445 }
446
of_live_flatten(const struct device_node * root,struct abuf * buf)447 int of_live_flatten(const struct device_node *root, struct abuf *buf)
448 {
449 int ret;
450
451 if (!abuf_init_size(buf, BUF_STEP))
452 return log_msg_ret("ini", -ENOMEM);
453
454 ret = fdt_create(abuf_data(buf), abuf_size(buf));
455 if (!ret)
456 ret = fdt_finish_reservemap(abuf_data(buf));
457 if (ret) {
458 log_debug("Failed to start FDT (err=%d)\n", ret);
459 return log_msg_ret("sta", -EINVAL);
460 }
461
462 ret = flatten_node(buf, root);
463 if (ret)
464 return log_msg_ret("flt", ret);
465
466 ret = fdt_finish(abuf_data(buf));
467 ret = check_space(ret, buf);
468 if (ret == -EAGAIN) {
469 ret = fdt_finish(abuf_data(buf));
470 if (ret) {
471 log_debug("Internal error c %d\n", ret);
472 return -EFAULT;
473 }
474 }
475 if (ret)
476 return log_msg_ret("fin", ret);
477
478 ret = fdt_pack(abuf_data(buf));
479 if (ret) {
480 log_debug("Failed to pack (err=%d)\n", ret);
481 return log_msg_ret("pac", -EFAULT);
482 }
483
484 if (!abuf_realloc(buf, fdt_totalsize(abuf_data(buf))))
485 return log_msg_ret("abu", -EFAULT);
486
487 return 0;
488 }
489