1 /*
2 * Copyright (c) 2006-2022, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2022-10-19 GuEe-GUI first version
9 */
10
11 #include <drivers/ofw_raw.h>
12
fdt_add_subnode_possible(void * fdt,int parentoffset,const char * name)13 int fdt_add_subnode_possible(void *fdt, int parentoffset, const char *name)
14 {
15 int nodeoffset;
16
17 if ((nodeoffset = fdt_add_subnode(fdt, parentoffset, name)) < 0)
18 {
19 fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + FDT_PADDING_SIZE);
20 nodeoffset = fdt_add_subnode(fdt, parentoffset, name);
21 }
22
23 return nodeoffset;
24 }
25
fdt_add_mem_rsv_possible(void * fdt,size_t addr,size_t size)26 int fdt_add_mem_rsv_possible(void *fdt, size_t addr, size_t size)
27 {
28 int err = 0;
29
30 if (fdt_add_mem_rsv(fdt, addr, size) < 0)
31 {
32 fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + FDT_PADDING_SIZE);
33 err = fdt_add_mem_rsv(fdt, addr, size);
34 }
35
36 return err;
37 }
38
fdt_setprop_uxx(void * fdt,int nodeoffset,const char * name,uint64_t val,bool is_u64)39 int fdt_setprop_uxx(void *fdt, int nodeoffset, const char *name, uint64_t val, bool is_u64)
40 {
41 int err;
42
43 if (is_u64)
44 {
45 err = fdt_setprop_u64(fdt, nodeoffset, name, val);
46 }
47 else
48 {
49 err = fdt_setprop_u32(fdt, nodeoffset, name, (uint32_t)val);
50 }
51
52 return err;
53 }
54
55 #define FDT_RAW_GET_VAL_FLAG(std_type, s, sz) \
56 int fdt_getprop_##std_type##sz(void *fdt, int nodeoffset, \
57 const char *name, s##int##sz##_t *out_value, int *lenp) \
58 { \
59 int err = -FDT_ERR_NOTFOUND; \
60 if (fdt && nodeoffset >= 0 && name && out_value) \
61 { \
62 const fdt##sz##_t *ptr; \
63 if ((ptr = fdt_getprop(fdt, nodeoffset, name, lenp))) \
64 { \
65 *out_value = fdt##sz##_to_cpu(*ptr); \
66 err = 0; \
67 } \
68 } \
69 return err; \
70 }
71
72 #define FDT_RAW_GET_VAL(size) \
73 FDT_RAW_GET_VAL_FLAG(u, u, size) \
74 FDT_RAW_GET_VAL_FLAG(s, , size)
75
76 FDT_RAW_GET_VAL(64)
77 FDT_RAW_GET_VAL(32)
78 FDT_RAW_GET_VAL(16)
79 FDT_RAW_GET_VAL(8)
80
81 #undef FDT_RAW_GET_VAL
82 #undef FDT_RAW_GET_VAL_FLAG
83
fdt_io_addr_cells(void * fdt,int nodeoffset)84 int fdt_io_addr_cells(void *fdt, int nodeoffset)
85 {
86 int cells = -1;
87 int parentoffset = fdt_parent_offset(fdt, nodeoffset);
88
89 for (; parentoffset >= 0 ; parentoffset = fdt_parent_offset(fdt, parentoffset))
90 {
91 const fdt32_t *cells_tmp = fdt_getprop(fdt, parentoffset, "#address-cells", NULL);
92
93 if (cells_tmp)
94 {
95 cells = fdt32_to_cpu(*cells_tmp);
96 }
97 }
98
99 if (cells < 0)
100 {
101 cells = fdt_address_cells(fdt, nodeoffset);
102 }
103
104 return cells;
105 }
106
fdt_io_size_cells(void * fdt,int nodeoffset)107 int fdt_io_size_cells(void *fdt, int nodeoffset)
108 {
109 int cells = -1;
110 int parentoffset = fdt_parent_offset(fdt, nodeoffset);
111
112 for (; parentoffset >= 0 ; parentoffset = fdt_parent_offset(fdt, parentoffset))
113 {
114 const fdt32_t *cells_tmp = fdt_getprop(fdt, parentoffset, "#size-cells", NULL);
115
116 if (cells_tmp)
117 {
118 cells = fdt32_to_cpu(*cells_tmp);
119 }
120 }
121
122 if (cells < 0)
123 {
124 cells = fdt_size_cells(fdt, nodeoffset);
125 }
126
127 return cells;
128 }
129
fdt_install_initrd(void * fdt,char * os_name,size_t initrd_addr,size_t initrd_size)130 int fdt_install_initrd(void *fdt, char *os_name, size_t initrd_addr, size_t initrd_size)
131 {
132 int err = -FDT_ERR_NOTFOUND;
133 int chosen_offset = -1, root_off = fdt_path_offset(fdt, "/");
134
135 if (root_off >= 0)
136 {
137 chosen_offset = fdt_subnode_offset(fdt, root_off, "chosen");
138
139 if (chosen_offset == -FDT_ERR_NOTFOUND)
140 {
141 chosen_offset = fdt_add_subnode_possible(fdt, root_off, "chosen");
142 }
143 }
144
145 if (chosen_offset >= 0)
146 {
147 uint64_t addr, size;
148
149 err = 0;
150
151 /* Update the entry */
152 for (int i = fdt_num_mem_rsv(fdt) - 1; i >= 0; --i)
153 {
154 fdt_get_mem_rsv(fdt, i, &addr, &size);
155
156 if (addr == initrd_addr)
157 {
158 fdt_del_mem_rsv(fdt, i);
159 break;
160 }
161 }
162
163 /* Add the memory */
164 if (fdt_add_mem_rsv(fdt, initrd_addr, initrd_size) < 0)
165 {
166 /* Move the memory */
167 fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + FDT_PADDING_SIZE);
168
169 if (fdt_add_mem_rsv(fdt, initrd_addr, initrd_size) < 0)
170 {
171 err = -FDT_ERR_NOSPACE;
172 }
173 }
174
175 if (!err)
176 {
177 size_t name_len;
178 char initrd_name[64];
179 bool is_u64 = (fdt_io_addr_cells(fdt, root_off) == 2);
180
181 if (!os_name)
182 {
183 os_name = "rt-thread";
184 }
185
186 name_len = strlen(initrd_name);
187
188 strncpy(&initrd_name[name_len], ",initrd-start", sizeof(initrd_name) - name_len);
189 fdt_setprop_uxx(fdt, chosen_offset, initrd_name, initrd_addr, is_u64);
190
191 strncpy(&initrd_name[name_len], ",initrd-end", sizeof(initrd_name) - name_len);
192 fdt_setprop_uxx(fdt, chosen_offset, initrd_name, initrd_addr + initrd_size, is_u64);
193 }
194 }
195
196 return err;
197 }
198