1 /*
2 * Copyright (c) 2006-2024, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2022-08-25 GuEe-GUI first version
9 */
10
11 #include <rthw.h>
12 #include <rtthread.h>
13
14 #include <drivers/ofw_fdt.h>
15 #include <drivers/ofw_raw.h>
16 #include <drivers/core/dm.h>
17
18 #include <mm_memblock.h>
19
20 #define DBG_TAG "rtdm.ofw"
21 #define DBG_LVL DBG_INFO
22 #include <rtdbg.h>
23
24 #include "ofw_internal.h"
25
26 struct rt_fdt_earlycon fdt_earlycon;
27
28 RT_OFW_SYMBOL_TYPE_RANGE(earlycon, struct rt_fdt_earlycon_id, _earlycon_start = {}, _earlycon_end = {});
29
30 #ifndef ARCH_INIT_MEMREGION_NR
31 #define ARCH_INIT_MEMREGION_NR 128
32 #endif
33
34 static void *_fdt = RT_NULL;
35 static rt_phandle _phandle_min;
36 static rt_phandle _phandle_max;
37 static rt_size_t _root_size_cells;
38 static rt_size_t _root_addr_cells;
39
40 #ifdef ARCH_CPU_64BIT
41 #define MIN_BIT 16
42 #else
43 #define MIN_BIT 8
44 #endif
45
rt_fdt_node_name(const char * full_name)46 const char *rt_fdt_node_name(const char *full_name)
47 {
48 const char *node_name = strrchr(full_name, '/');
49
50 return node_name ? node_name + 1 : full_name;
51 }
52
rt_fdt_read_number(const fdt32_t * cell,int size)53 rt_uint64_t rt_fdt_read_number(const fdt32_t *cell, int size)
54 {
55 rt_uint64_t val = 0;
56
57 for (; size--; ++cell)
58 {
59 val = (val << 32) | fdt32_to_cpu(*cell);
60 }
61
62 return val;
63 }
64
rt_fdt_next_cell(const fdt32_t ** cellptr,int size)65 rt_uint64_t rt_fdt_next_cell(const fdt32_t **cellptr, int size)
66 {
67 const fdt32_t *ptr = *cellptr;
68
69 *cellptr = ptr + size;
70
71 return rt_fdt_read_number(ptr, size);
72 }
73
rt_fdt_translate_address(void * fdt,int nodeoffset,rt_uint64_t address)74 rt_uint64_t rt_fdt_translate_address(void *fdt, int nodeoffset, rt_uint64_t address)
75 {
76 rt_uint64_t ret = address;
77
78 if (fdt && nodeoffset >= 0)
79 {
80 struct
81 {
82 rt_uint64_t addr;
83 rt_size_t size;
84 int addr_cells;
85 int size_cells;
86 } local, cpu;
87 int parent, length = 0, group_len;
88 const fdt32_t *ranges = RT_NULL;
89
90 parent = fdt_parent_offset(fdt, nodeoffset);
91
92 if (parent >= 0)
93 {
94 ranges = fdt_getprop(fdt, parent, "ranges", &length);
95 }
96
97 if (ranges && length > 0)
98 {
99 local.addr_cells = fdt_address_cells(fdt, parent);
100 local.size_cells = fdt_size_cells(fdt, parent);
101 cpu.addr_cells = fdt_io_addr_cells(fdt, parent);
102 cpu.size_cells = fdt_io_size_cells(fdt, parent);
103
104 group_len = local.addr_cells + cpu.addr_cells + local.size_cells;
105
106 while (length > 0)
107 {
108 local.addr = rt_fdt_next_cell(&ranges, local.addr_cells);
109 cpu.addr = rt_fdt_next_cell(&ranges, cpu.addr_cells);
110 local.size = rt_fdt_next_cell(&ranges, local.size_cells);
111
112 if (local.addr <= address && local.addr + local.size > address)
113 {
114 ret = address - local.addr + cpu.addr;
115 break;
116 }
117
118 length -= group_len;
119 }
120 }
121 }
122
123 return ret;
124 }
125
rt_fdt_device_is_available(void * fdt,int nodeoffset)126 rt_bool_t rt_fdt_device_is_available(void *fdt, int nodeoffset)
127 {
128 rt_bool_t ret;
129
130 const char *status = fdt_getprop(fdt, nodeoffset, "status", RT_NULL);
131
132 if (!status)
133 {
134 ret = RT_TRUE;
135 }
136 else if (!rt_strcmp(status, "ok") || !rt_strcmp(status, "okay"))
137 {
138 ret = RT_TRUE;
139 }
140 else
141 {
142 ret = RT_FALSE;
143 }
144
145 return ret;
146 }
147
rt_fdt_prefetch(void * fdt)148 rt_err_t rt_fdt_prefetch(void *fdt)
149 {
150 rt_err_t err = -RT_ERROR;
151
152 if (fdt)
153 {
154 _fdt = fdt;
155
156 if (!fdt_check_header(_fdt))
157 {
158 err = rt_fdt_scan_root();
159 }
160 else
161 {
162 err = -RT_EINVAL;
163 }
164 }
165
166 return err;
167 }
168
rt_fdt_scan_root(void)169 rt_err_t rt_fdt_scan_root(void)
170 {
171 rt_err_t err = RT_EOK;
172 int root = fdt_path_offset(_fdt, "/");
173
174 if (root >= 0)
175 {
176 const fdt32_t *prop;
177
178 _root_addr_cells = OFW_ROOT_NODE_ADDR_CELLS_DEFAULT;
179 _root_size_cells = OFW_ROOT_NODE_SIZE_CELLS_DEFAULT;
180
181 if ((prop = fdt_getprop(_fdt, root, "#address-cells", RT_NULL)))
182 {
183 _root_addr_cells = fdt32_to_cpu(*prop);
184 }
185
186 if ((prop = fdt_getprop(_fdt, root, "#size-cells", RT_NULL)))
187 {
188 _root_size_cells = fdt32_to_cpu(*prop);
189 }
190 }
191 else
192 {
193 err = -RT_EEMPTY;
194 }
195
196 return err;
197 }
198
fdt_reserved_mem_check_root(int nodeoffset)199 static rt_err_t fdt_reserved_mem_check_root(int nodeoffset)
200 {
201 rt_err_t err = RT_EOK;
202 const fdt32_t *prop = fdt_getprop(_fdt, nodeoffset, "#size-cells", RT_NULL);
203
204 if (!prop || fdt32_to_cpu(*prop) != _root_size_cells)
205 {
206 err = -RT_EINVAL;
207 }
208
209 if (!err)
210 {
211 prop = fdt_getprop(_fdt, nodeoffset, "#address-cells", RT_NULL);
212
213 if (!prop || fdt32_to_cpu(*prop) != _root_addr_cells)
214 {
215 err = -RT_EINVAL;
216 }
217 }
218
219 if (!err && !(prop = fdt_getprop(_fdt, nodeoffset, "ranges", RT_NULL)))
220 {
221 err = -RT_EINVAL;
222 }
223
224 return err;
225 }
226
fdt_reserved_memory_reg(int nodeoffset,const char * uname)227 static rt_err_t fdt_reserved_memory_reg(int nodeoffset, const char *uname)
228 {
229 rt_err_t err = RT_EOK;
230
231 rt_ubase_t base, size;
232 const fdt32_t *prop;
233 int len, t_len = (_root_addr_cells + _root_size_cells) * sizeof(fdt32_t);
234
235 if ((prop = fdt_getprop(_fdt, nodeoffset, "reg", &len)))
236 {
237 if (len && len % t_len != 0)
238 {
239 LOG_E("Reserved memory: invalid reg property in '%s', skipping node", uname);
240 err = -RT_EINVAL;
241 }
242 else
243 {
244 for (; len >= t_len; len -= t_len)
245 {
246 base = rt_fdt_next_cell(&prop, _root_addr_cells);
247 size = rt_fdt_next_cell(&prop, _root_size_cells);
248
249 if (!size)
250 {
251 continue;
252 }
253
254 rt_bool_t is_nomap = fdt_getprop(_fdt, nodeoffset, "no-map", RT_NULL) ? RT_TRUE : RT_FALSE;
255 base = rt_fdt_translate_address(_fdt, nodeoffset, base);
256
257 rt_memblock_reserve_memory(fdt_get_name(_fdt, nodeoffset, RT_NULL),
258 base, base + size, is_nomap);
259 }
260 }
261 }
262 else
263 {
264 err = -RT_EEMPTY;
265 }
266
267 return err;
268 }
269
fdt_scan_reserved_memory(void)270 static void fdt_scan_reserved_memory(void)
271 {
272 int nodeoffset, child;
273
274 nodeoffset = fdt_path_offset(_fdt, "/reserved-memory");
275
276 if (nodeoffset >= 0)
277 {
278 if (!fdt_reserved_mem_check_root(nodeoffset))
279 {
280 fdt_for_each_subnode(child, _fdt, nodeoffset)
281 {
282 rt_err_t err;
283 const char *uname;
284
285 if (!rt_fdt_device_is_available(_fdt, child))
286 {
287 continue;
288 }
289
290 uname = fdt_get_name(_fdt, child, RT_NULL);
291 err = fdt_reserved_memory_reg(child, uname);
292
293 if (err == -RT_EEMPTY && fdt_getprop(_fdt, child, "size", RT_NULL))
294 {
295 LOG_E("Allocating reserved memory in setup is not yet supported");
296 }
297 }
298 }
299 else
300 {
301 LOG_E("Reserved memory: unsupported node format, ignoring");
302 }
303 }
304 }
305
fdt_scan_memory(void)306 static rt_err_t fdt_scan_memory(void)
307 {
308 int nodeoffset, no;
309 rt_uint64_t base, size;
310 rt_err_t err = -RT_EEMPTY;
311
312 /* Process header /memreserve/ fields */
313 for (no = 0; ; ++no)
314 {
315 fdt_get_mem_rsv(_fdt, no, &base, &size);
316
317 if (!size)
318 {
319 break;
320 }
321
322 rt_memblock_reserve_memory("memreserve", base, base + size, MEMBLOCK_NONE);
323 }
324
325 fdt_for_each_subnode(nodeoffset, _fdt, 0)
326 {
327 int len;
328 const fdt32_t *reg, *endptr;
329 const char *name = fdt_get_name(_fdt, nodeoffset, RT_NULL);
330 const char *type = fdt_getprop(_fdt, nodeoffset, "device_type", RT_NULL);
331
332 if (!type || rt_strcmp(type, "memory"))
333 {
334 continue;
335 }
336
337 if (!rt_fdt_device_is_available(_fdt, nodeoffset))
338 {
339 continue;
340 }
341
342 reg = fdt_getprop(_fdt, nodeoffset, "reg", &len);
343
344 if (!reg)
345 {
346 continue;
347 }
348
349 endptr = reg + (len / sizeof(fdt32_t));
350 name = name ? name : "memory";
351
352 while ((endptr - reg) >= (_root_addr_cells + _root_size_cells))
353 {
354 base = rt_fdt_next_cell(®, _root_addr_cells);
355 size = rt_fdt_next_cell(®, _root_size_cells);
356
357 if (!size)
358 {
359 continue;
360 }
361
362 bool is_hotpluggable = fdt_getprop(_fdt, nodeoffset, "hotpluggable", RT_NULL) ? RT_TRUE : RT_FALSE;
363 err = rt_memblock_add_memory(name, base, base + size, is_hotpluggable);
364
365 if (!err)
366 {
367 LOG_I("Memory node(%d) ranges: 0x%.*lx - 0x%.*lx%s", no, MIN_BIT, base, MIN_BIT, base + size, "");
368 }
369 else
370 {
371 LOG_W("Memory node(%d) ranges: 0x%.*lx - 0x%.*lx%s", no, MIN_BIT, base, MIN_BIT, base + size, " unable to record");
372 }
373 }
374 }
375
376 if (!err)
377 {
378 fdt_scan_reserved_memory();
379 }
380
381 return err;
382 }
383
rt_fdt_scan_memory(void)384 rt_err_t rt_fdt_scan_memory(void)
385 {
386 rt_err_t err = -RT_EEMPTY;
387
388 if (_fdt)
389 {
390 err = fdt_scan_memory();
391 }
392
393 return err;
394 }
395
fdt_scan_initrd(rt_uint64_t * ranges,const char * name,const char * oem)396 static rt_err_t fdt_scan_initrd(rt_uint64_t *ranges, const char *name, const char *oem)
397 {
398 char tmp_name[32];
399 rt_err_t err = -RT_EEMPTY;
400
401 if (_fdt && ranges)
402 {
403 int offset = fdt_path_offset(_fdt, "/chosen");
404
405 if (offset >= 0)
406 {
407 int s_len, e_len;
408 const fdt32_t *start = RT_NULL, *end = RT_NULL;
409
410 rt_snprintf(tmp_name, sizeof(tmp_name), "%s,%s-start", oem, name);
411 start = fdt_getprop(_fdt, offset, tmp_name, &s_len);
412 rt_snprintf(tmp_name, sizeof(tmp_name), "%s,%s-end", oem, name);
413 end = fdt_getprop(_fdt, offset, tmp_name, &e_len);
414
415 if (start && end)
416 {
417 s_len /= sizeof(*start);
418 e_len /= sizeof(*end);
419
420 ranges[0] = rt_fdt_read_number(start, s_len);
421 ranges[1] = rt_fdt_read_number(end, e_len);
422
423 err = RT_EOK;
424 }
425 }
426
427 if (err)
428 {
429 int len;
430 const char *options, *bootargs = fdt_getprop(_fdt, offset, "bootargs", &len);
431
432 rt_snprintf(tmp_name, sizeof(tmp_name), "%s=", name);
433
434 if (bootargs && (options = rt_strstr(bootargs, tmp_name)))
435 {
436 rt_uint64_t value;
437
438 options += rt_strlen(tmp_name) + sizeof("0x") - 1;
439 err = RT_EOK;
440
441 for (int i = 0; i < 2 && !err; ++i)
442 {
443 value = 0;
444
445 while (*options && *options != ',' && *options != ' ')
446 {
447 /* To lowercase or keep number */
448 char ch = *options | ' ';
449
450 value *= 16;
451
452 if (ch >= '0' && ch <= '9')
453 {
454 value += ch - '0';
455 }
456 else if (ch >= 'a' && ch <= 'f')
457 {
458 value += ch - 'a' + 10;
459 }
460 else
461 {
462 err = -RT_EINVAL;
463 break;
464 }
465
466 ++options;
467 }
468
469 ranges[i] = value;
470 options += sizeof(",0x") - 1;
471 }
472
473 /* This is initrd's size, convert to initrd's end */
474 ranges[1] += ranges[0];
475 }
476 }
477
478 if (!err)
479 {
480 rt_memblock_reserve_memory("initrd", ranges[0], ranges[1], MEMBLOCK_NONE);
481 }
482 }
483 else if (!ranges)
484 {
485 err = -RT_EINVAL;
486 }
487
488 return err;
489 }
490
rt_fdt_scan_initrd(rt_uint64_t * ranges)491 rt_err_t rt_fdt_scan_initrd(rt_uint64_t *ranges)
492 {
493 rt_err_t err;
494
495 err = fdt_scan_initrd(ranges, "cromfs", "rt-thread");
496
497 if (err && err == -RT_EEMPTY)
498 {
499 err = fdt_scan_initrd(ranges, "initrd", "linux");
500 }
501
502 return err;
503 }
504
505
rt_fdt_model_dump(void)506 rt_err_t rt_fdt_model_dump(void)
507 {
508 rt_err_t err = RT_EOK;
509 int root = fdt_path_offset(_fdt, "/");
510
511 if (root >= 0)
512 {
513 const char *mach_model = fdt_getprop(_fdt, root, "model", RT_NULL);
514
515 if (!mach_model)
516 {
517 mach_model = fdt_getprop(_fdt, root, "compatible", RT_NULL);
518 }
519
520 LOG_I("Machine model: %s", mach_model ? mach_model : "<undefined>");
521 }
522 else
523 {
524 err = -RT_EEMPTY;
525 }
526
527 return err;
528 }
529
rt_fdt_boot_dump(void)530 rt_weak rt_err_t rt_fdt_boot_dump(void)
531 {
532 LOG_I("Booting RT-Thread on physical CPU 0x%x", rt_hw_cpu_id());
533
534 return RT_EOK;
535 }
536
rt_fdt_earlycon_output(const char * str)537 void rt_fdt_earlycon_output(const char *str)
538 {
539 if (fdt_earlycon.console_putc)
540 {
541 while (*str)
542 {
543 fdt_earlycon.console_putc(fdt_earlycon.data, *str);
544
545 if (*str == '\n')
546 {
547 /* Make sure return */
548 fdt_earlycon.console_putc(fdt_earlycon.data, '\r');
549 }
550
551 ++str;
552 }
553 }
554 else
555 {
556 /* We need a byte to save '\0' */
557 while (*str && fdt_earlycon.msg_idx < sizeof(fdt_earlycon.msg) - 1)
558 {
559 fdt_earlycon.msg[fdt_earlycon.msg_idx++] = *str;
560
561 ++str;
562 }
563 fdt_earlycon.msg[fdt_earlycon.msg_idx] = '\0';
564 }
565 }
566
rt_fdt_earlycon_kick(int why)567 void rt_fdt_earlycon_kick(int why)
568 {
569 if (fdt_earlycon.console_kick)
570 {
571 fdt_earlycon.console_kick(&fdt_earlycon, why);
572 }
573
574 if (why == FDT_EARLYCON_KICK_COMPLETED)
575 {
576 fdt_earlycon.console_putc = RT_NULL;
577
578 if (fdt_earlycon.msg_idx)
579 {
580 fdt_earlycon.msg_idx = 0;
581
582 /* Dump old messages */
583 rt_kputs(fdt_earlycon.msg);
584 }
585 }
586 }
587
rt_fdt_scan_chosen_stdout(void)588 rt_err_t rt_fdt_scan_chosen_stdout(void)
589 {
590 rt_err_t err = RT_EOK;
591
592 int offset;
593 int len, options_len = 0;
594 const char *options = RT_NULL, *con_type = RT_NULL;
595
596 rt_memset(&fdt_earlycon, 0, rt_offsetof(struct rt_fdt_earlycon, msg_idx));
597 fdt_earlycon.nodeoffset = -1;
598
599 offset = fdt_path_offset(_fdt, "/chosen");
600
601 if (offset >= 0)
602 {
603 const char *stdout_path = RT_NULL;
604 const char *bootargs = fdt_getprop(_fdt, offset, "bootargs", &len);
605
606 if (bootargs && (options = rt_strstr(bootargs, "earlycon")))
607 {
608 options += sizeof("earlycon") - 1;
609
610 if (*options == '\0' || *options == ' ')
611 {
612 stdout_path = fdt_getprop(_fdt, offset, "stdout-path", &len);
613
614 if (stdout_path && len)
615 {
616 const char *path_split = strchrnul(stdout_path, ':');
617
618 if (*path_split != '\0')
619 {
620 options = path_split + 1;
621 }
622
623 len = path_split - stdout_path;
624
625 /*
626 * Will try 2 styles:
627 * 1: stdout-path = "serialN:bbbbpnf";
628 * 2: stdout-path = "/serial-path";
629 */
630 offset = fdt_path_offset_namelen(_fdt, stdout_path, len);
631
632 if (offset < 0)
633 {
634 stdout_path = RT_NULL;
635 }
636 }
637 else if (*options == '=')
638 {
639 ++options;
640 }
641 else
642 {
643 /* Maybe is error in bootargs or it is a new arg */
644 options = RT_NULL;
645 }
646
647 if (!stdout_path)
648 {
649 /* We couldn't know how to setup the earlycon */
650 options = RT_NULL;
651 }
652 }
653 else
654 {
655 offset = -1;
656 }
657
658 if (options)
659 {
660 int type_len = 0;
661 struct rt_fdt_earlycon_id *earlycon_id, *earlycon_id_end, *best_earlycon_id = RT_NULL;
662
663 earlycon_id = (struct rt_fdt_earlycon_id *)&_earlycon_start;
664 earlycon_id_end = (struct rt_fdt_earlycon_id *)&_earlycon_end;
665
666 err = -RT_ENOSYS;
667
668 /* Only "earlycon" in bootargs */
669 if (stdout_path)
670 {
671 const fdt32_t *reg;
672 options = RT_NULL;
673
674 if ((reg = fdt_getprop(_fdt, offset, "reg", RT_NULL)))
675 {
676 rt_uint64_t address;
677 int addr_cells = fdt_io_addr_cells(_fdt, offset);
678 int size_cells = fdt_io_size_cells(_fdt, offset);
679
680 address = rt_fdt_read_number(reg, addr_cells);
681 fdt_earlycon.mmio = rt_fdt_translate_address(_fdt, offset, address);
682 fdt_earlycon.size = rt_fdt_read_number(reg + addr_cells, size_cells);
683 }
684 }
685 else
686 {
687 /* Pass split */
688 while (*options && (*options == '=' || *options == ' '))
689 {
690 ++options;
691 }
692
693 if (*options)
694 {
695 type_len = strchrnul(options, ',') - options;
696 }
697 }
698
699 if (options && *options && *options != ' ')
700 {
701 options_len = strchrnul(options, ' ') - options;
702
703 rt_strncpy(fdt_earlycon.options, options, options_len);
704 }
705
706 /* console > stdout-path */
707 for (int max_score = 0; earlycon_id < earlycon_id_end; ++earlycon_id)
708 {
709 int score = 0;
710
711 if (type_len && earlycon_id->type)
712 {
713 if (!rt_strncmp(earlycon_id->type, options, type_len))
714 {
715 score += 1;
716 }
717 }
718
719 if (stdout_path && earlycon_id->compatible)
720 {
721 if (!fdt_node_check_compatible(_fdt, offset, earlycon_id->compatible))
722 {
723 score += 2;
724 }
725 }
726
727 if (score > max_score)
728 {
729 max_score = score;
730 best_earlycon_id = earlycon_id;
731
732 if (score == 3)
733 {
734 break;
735 }
736 }
737 }
738
739 if (best_earlycon_id && best_earlycon_id->setup)
740 {
741 const char earlycon_magic[] = { 'O', 'F', 'W', '\0' };
742
743 if (!con_type)
744 {
745 con_type = best_earlycon_id->type;
746 }
747 fdt_earlycon.fdt = _fdt;
748 fdt_earlycon.nodeoffset = offset;
749
750 options = &fdt_earlycon.options[options_len + 1];
751 rt_strncpy((void *)options, earlycon_magic, RT_ARRAY_SIZE(earlycon_magic));
752
753 err = best_earlycon_id->setup(&fdt_earlycon, fdt_earlycon.options);
754
755 if (rt_strncmp(options, earlycon_magic, RT_ARRAY_SIZE(earlycon_magic)))
756 {
757 const char *option_start = options - 1;
758
759 while (option_start[-1] != '\0')
760 {
761 --option_start;
762 }
763
764 rt_memmove(fdt_earlycon.options, option_start, options - option_start);
765 }
766 else
767 {
768 fdt_earlycon.options[0] = '\0';
769 }
770 }
771 }
772 }
773 else
774 {
775 err = -RT_EEMPTY;
776 }
777 }
778 else
779 {
780 err = -RT_EEMPTY;
781 }
782
783 if (fdt_earlycon.msg_idx)
784 {
785 fdt_earlycon.msg_idx = 0;
786
787 rt_kputs(fdt_earlycon.msg);
788 }
789
790 rt_fdt_boot_dump();
791 rt_fdt_model_dump();
792
793 if (fdt_earlycon.mmio)
794 {
795 LOG_I("Earlycon: %s at MMIO/PIO %p (options '%s')",
796 con_type, fdt_earlycon.mmio, fdt_earlycon.options);
797 }
798
799 return err;
800 }
801
rt_fdt_bootargs_select(const char * key,int index,const char ** out_result)802 rt_err_t rt_fdt_bootargs_select(const char *key, int index, const char **out_result)
803 {
804 rt_err_t err;
805
806 if (key && index >= 0 && out_result)
807 {
808 int offset = fdt_path_offset(_fdt, "/chosen");
809
810 if (offset >= 0)
811 {
812 int len, key_len = rt_strlen(key);
813 const char *bootargs = fdt_getprop(_fdt, offset, "bootargs", &len), *end;
814
815 end = bootargs + len;
816 err = -RT_EEMPTY;
817
818 for (int i = 0; bootargs < end; ++i)
819 {
820 bootargs = rt_strstr(bootargs, key);
821
822 if (!bootargs)
823 {
824 break;
825 }
826
827 bootargs += key_len;
828
829 if (i == index)
830 {
831 *out_result = bootargs;
832
833 err = -RT_EOK;
834 break;
835 }
836 }
837 }
838 else
839 {
840 err = -RT_ERROR;
841 }
842 }
843 else
844 {
845 err = -RT_EINVAL;
846 }
847
848 return err;
849 }
850
system_node_init_flag(struct rt_ofw_node * np)851 static void system_node_init_flag(struct rt_ofw_node *np)
852 {
853 if (np)
854 {
855 rt_ofw_node_set_flag(np, RT_OFW_F_READLY);
856 rt_ofw_node_set_flag(np, RT_OFW_F_SYSTEM);
857 }
858 }
859
rt_fdt_unflatten(void)860 rt_err_t rt_fdt_unflatten(void)
861 {
862 rt_err_t err = RT_EOK;
863
864 if (_fdt)
865 {
866 _phandle_min = OFW_PHANDLE_MAX;
867 _phandle_max = OFW_PHANDLE_MIN;
868
869 ofw_node_root = rt_fdt_unflatten_single(_fdt);
870
871 if (ofw_node_root)
872 {
873 ofw_node_cpus = rt_ofw_find_node_by_path("/cpus");
874 ofw_node_chosen = rt_ofw_find_node_by_path("/chosen");
875 ofw_node_aliases = rt_ofw_find_node_by_path("/aliases");
876 ofw_node_reserved_memory = rt_ofw_find_node_by_path("/reserved-memory");
877
878 RT_ASSERT(ofw_node_cpus != RT_NULL);
879
880 system_node_init_flag(ofw_node_root);
881 system_node_init_flag(ofw_node_cpus);
882 system_node_init_flag(ofw_node_chosen);
883 system_node_init_flag(ofw_node_aliases);
884 system_node_init_flag(ofw_node_reserved_memory);
885
886 if (ofw_node_aliases)
887 {
888 err = ofw_alias_scan();
889 }
890
891 err = err ? : ofw_phandle_hash_reset(_phandle_min, _phandle_max);
892 }
893 }
894 else
895 {
896 err = -RT_ERROR;
897 }
898
899 return err;
900 }
901
fdt_unflatten_props(struct rt_ofw_node * np,int node_off)902 static rt_err_t fdt_unflatten_props(struct rt_ofw_node *np, int node_off)
903 {
904 rt_err_t err = RT_EOK;
905 struct rt_ofw_prop *prop;
906 int prop_off = fdt_first_property_offset(_fdt, node_off);
907
908 if (prop_off >= 0)
909 {
910 np->props = rt_malloc(sizeof(struct rt_ofw_prop));
911 }
912
913 prop = np->props;
914
915 while (prop_off >= 0)
916 {
917 if (!prop)
918 {
919 err = -RT_ENOMEM;
920 break;
921 }
922
923 prop->value = (void *)fdt_getprop_by_offset(_fdt, prop_off, &prop->name, &prop->length);
924
925 if (prop->name && !rt_strcmp(prop->name, "name"))
926 {
927 np->name = prop->value;
928 }
929
930 prop_off = fdt_next_property_offset(_fdt, prop_off);
931
932 if (prop_off < 0)
933 {
934 prop->next = RT_NULL;
935 break;
936 }
937
938 prop->next = rt_malloc(sizeof(struct rt_ofw_prop));
939 prop = prop->next;
940 }
941
942 return err;
943 }
944
fdt_unflatten_single(struct rt_ofw_node * np,int node_off)945 static rt_err_t fdt_unflatten_single(struct rt_ofw_node *np, int node_off)
946 {
947 int depth = 0;
948 rt_err_t err = RT_EOK;
949 struct rt_ofw_node *np_stack[OFW_NODE_MAX_DEPTH], *parent = RT_NULL;
950
951 do {
952 if (!np)
953 {
954 err = -RT_ENOMEM;
955 break;
956 }
957
958 np->name = "<NULL>";
959 np->full_name = fdt_get_name(_fdt, node_off, RT_NULL);
960 np->phandle = fdt_get_phandle(_fdt, node_off);
961
962 if (np->phandle >= OFW_PHANDLE_MIN)
963 {
964 if (np->phandle < _phandle_min)
965 {
966 _phandle_min = np->phandle;
967 }
968
969 if (np->phandle > _phandle_max)
970 {
971 _phandle_max = np->phandle;
972 }
973 }
974
975 if ((err = fdt_unflatten_props(np, node_off)))
976 {
977 break;
978 }
979
980 np->parent = parent;
981
982 rt_ref_init(&np->ref);
983 np->flags = 0;
984
985 if (!np->child)
986 {
987 /* Save node offset temp */
988 rt_ofw_data(np) = (void *)(rt_ubase_t)node_off;
989
990 /* Check children */
991 node_off = fdt_first_subnode(_fdt, node_off);
992
993 if (node_off >= 0)
994 {
995 parent = np;
996
997 np_stack[depth++] = np;
998
999 np->child = rt_calloc(1, sizeof(struct rt_ofw_node));
1000 np = np->child;
1001
1002 continue;
1003 }
1004 }
1005
1006 while (depth >= 0)
1007 {
1008 /* Restore node offset temp */
1009 node_off = (long)rt_ofw_data(np);
1010 rt_ofw_data(np) = RT_NULL;
1011
1012 /* Next step */
1013 node_off = fdt_next_subnode(_fdt, node_off);
1014
1015 if (node_off < 0)
1016 {
1017 np->sibling = RT_NULL;
1018
1019 np = np_stack[--depth];
1020 }
1021 else
1022 {
1023 parent = np->parent;
1024
1025 np->sibling = rt_calloc(1, sizeof(struct rt_ofw_node));
1026 np = np->sibling;
1027
1028 break;
1029 }
1030 }
1031 } while (depth >= 0);
1032
1033 return err;
1034 }
1035
rt_fdt_unflatten_single(void * fdt)1036 struct rt_ofw_node *rt_fdt_unflatten_single(void *fdt)
1037 {
1038 int root_off;
1039 struct fdt_info *header;
1040 struct rt_ofw_node *root = RT_NULL;
1041
1042 if (fdt && (root_off = fdt_path_offset(fdt, "/")) >= 0)
1043 {
1044 root = rt_calloc(1, sizeof(struct fdt_info) + sizeof(struct rt_ofw_node));
1045 }
1046
1047 if (root)
1048 {
1049 header = (void *)root + sizeof(struct rt_ofw_node);
1050
1051 rt_strncpy(header->name, "/", sizeof("/"));
1052
1053 header->fdt = fdt;
1054
1055 header->rsvmap = (struct fdt_reserve_entry *)((void *)fdt + fdt_off_mem_rsvmap(fdt));
1056 header->rsvmap_nr = fdt_num_mem_rsv(fdt);
1057
1058 if (!fdt_unflatten_single(root, root_off))
1059 {
1060 root->name = (const char *)header;
1061 }
1062 else
1063 {
1064 rt_ofw_node_destroy(root);
1065
1066 root = RT_NULL;
1067 }
1068 }
1069
1070 return root;
1071 }
1072