1 /* $NetBSD: fdt_sw.c,v 1.1.1.3 2019/12/22 12:30:38 skrll Exp $ */
2
3 // SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
4 /*
5 * libfdt - Flat Device Tree manipulation
6 * Copyright (C) 2006 David Gibson, IBM Corporation.
7 */
8 #include "libfdt_env.h"
9
10 #include <fdt.h>
11 #include <libfdt.h>
12
13 #include "libfdt_internal.h"
14
fdt_sw_probe_(void * fdt)15 static int fdt_sw_probe_(void *fdt)
16 {
17 if (fdt_magic(fdt) == FDT_MAGIC)
18 return -FDT_ERR_BADSTATE;
19 else if (fdt_magic(fdt) != FDT_SW_MAGIC)
20 return -FDT_ERR_BADMAGIC;
21 return 0;
22 }
23
24 #define FDT_SW_PROBE(fdt) \
25 { \
26 int err; \
27 if ((err = fdt_sw_probe_(fdt)) != 0) \
28 return err; \
29 }
30
31 /* 'memrsv' state: Initial state after fdt_create()
32 *
33 * Allowed functions:
34 * fdt_add_reservmap_entry()
35 * fdt_finish_reservemap() [moves to 'struct' state]
36 */
fdt_sw_probe_memrsv_(void * fdt)37 static int fdt_sw_probe_memrsv_(void *fdt)
38 {
39 int err = fdt_sw_probe_(fdt);
40 if (err)
41 return err;
42
43 if (fdt_off_dt_strings(fdt) != 0)
44 return -FDT_ERR_BADSTATE;
45 return 0;
46 }
47
48 #define FDT_SW_PROBE_MEMRSV(fdt) \
49 { \
50 int err; \
51 if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \
52 return err; \
53 }
54
55 /* 'struct' state: Enter this state after fdt_finish_reservemap()
56 *
57 * Allowed functions:
58 * fdt_begin_node()
59 * fdt_end_node()
60 * fdt_property*()
61 * fdt_finish() [moves to 'complete' state]
62 */
fdt_sw_probe_struct_(void * fdt)63 static int fdt_sw_probe_struct_(void *fdt)
64 {
65 int err = fdt_sw_probe_(fdt);
66 if (err)
67 return err;
68
69 if (fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
70 return -FDT_ERR_BADSTATE;
71 return 0;
72 }
73
74 #define FDT_SW_PROBE_STRUCT(fdt) \
75 { \
76 int err; \
77 if ((err = fdt_sw_probe_struct_(fdt)) != 0) \
78 return err; \
79 }
80
sw_flags(void * fdt)81 static inline uint32_t sw_flags(void *fdt)
82 {
83 /* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */
84 return fdt_last_comp_version(fdt);
85 }
86
87 /* 'complete' state: Enter this state after fdt_finish()
88 *
89 * Allowed functions: none
90 */
91
fdt_grab_space_(void * fdt,size_t len)92 static void *fdt_grab_space_(void *fdt, size_t len)
93 {
94 int offset = fdt_size_dt_struct(fdt);
95 int spaceleft;
96
97 spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
98 - fdt_size_dt_strings(fdt);
99
100 if ((offset + len < offset) || (offset + len > spaceleft))
101 return NULL;
102
103 fdt_set_size_dt_struct(fdt, offset + len);
104 return fdt_offset_ptr_w_(fdt, offset);
105 }
106
fdt_create_with_flags(void * buf,int bufsize,uint32_t flags)107 int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags)
108 {
109 const size_t hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
110 sizeof(struct fdt_reserve_entry));
111 void *fdt = buf;
112
113 if (bufsize < hdrsize)
114 return -FDT_ERR_NOSPACE;
115
116 if (flags & ~FDT_CREATE_FLAGS_ALL)
117 return -FDT_ERR_BADFLAGS;
118
119 memset(buf, 0, bufsize);
120
121 /*
122 * magic and last_comp_version keep intermediate state during the fdt
123 * creation process, which is replaced with the proper FDT format by
124 * fdt_finish().
125 *
126 * flags should be accessed with sw_flags().
127 */
128 fdt_set_magic(fdt, FDT_SW_MAGIC);
129 fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
130 fdt_set_last_comp_version(fdt, flags);
131
132 fdt_set_totalsize(fdt, bufsize);
133
134 fdt_set_off_mem_rsvmap(fdt, hdrsize);
135 fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
136 fdt_set_off_dt_strings(fdt, 0);
137
138 return 0;
139 }
140
fdt_create(void * buf,int bufsize)141 int fdt_create(void *buf, int bufsize)
142 {
143 return fdt_create_with_flags(buf, bufsize, 0);
144 }
145
fdt_resize(void * fdt,void * buf,int bufsize)146 int fdt_resize(void *fdt, void *buf, int bufsize)
147 {
148 size_t headsize, tailsize;
149 char *oldtail, *newtail;
150
151 FDT_SW_PROBE(fdt);
152
153 headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
154 tailsize = fdt_size_dt_strings(fdt);
155
156 if ((headsize + tailsize) > fdt_totalsize(fdt))
157 return -FDT_ERR_INTERNAL;
158
159 if ((headsize + tailsize) > bufsize)
160 return -FDT_ERR_NOSPACE;
161
162 oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
163 newtail = (char *)buf + bufsize - tailsize;
164
165 /* Two cases to avoid clobbering data if the old and new
166 * buffers partially overlap */
167 if (buf <= fdt) {
168 memmove(buf, fdt, headsize);
169 memmove(newtail, oldtail, tailsize);
170 } else {
171 memmove(newtail, oldtail, tailsize);
172 memmove(buf, fdt, headsize);
173 }
174
175 fdt_set_totalsize(buf, bufsize);
176 if (fdt_off_dt_strings(buf))
177 fdt_set_off_dt_strings(buf, bufsize);
178
179 return 0;
180 }
181
fdt_add_reservemap_entry(void * fdt,uint64_t addr,uint64_t size)182 int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
183 {
184 struct fdt_reserve_entry *re;
185 int offset;
186
187 FDT_SW_PROBE_MEMRSV(fdt);
188
189 offset = fdt_off_dt_struct(fdt);
190 if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
191 return -FDT_ERR_NOSPACE;
192
193 re = (struct fdt_reserve_entry *)((char *)fdt + offset);
194 re->address = cpu_to_fdt64(addr);
195 re->size = cpu_to_fdt64(size);
196
197 fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
198
199 return 0;
200 }
201
fdt_finish_reservemap(void * fdt)202 int fdt_finish_reservemap(void *fdt)
203 {
204 int err = fdt_add_reservemap_entry(fdt, 0, 0);
205
206 if (err)
207 return err;
208
209 fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
210 return 0;
211 }
212
fdt_begin_node(void * fdt,const char * name)213 int fdt_begin_node(void *fdt, const char *name)
214 {
215 struct fdt_node_header *nh;
216 int namelen;
217
218 FDT_SW_PROBE_STRUCT(fdt);
219
220 namelen = strlen(name) + 1;
221 nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
222 if (! nh)
223 return -FDT_ERR_NOSPACE;
224
225 nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
226 memcpy(nh->name, name, namelen);
227 return 0;
228 }
229
fdt_end_node(void * fdt)230 int fdt_end_node(void *fdt)
231 {
232 fdt32_t *en;
233
234 FDT_SW_PROBE_STRUCT(fdt);
235
236 en = fdt_grab_space_(fdt, FDT_TAGSIZE);
237 if (! en)
238 return -FDT_ERR_NOSPACE;
239
240 *en = cpu_to_fdt32(FDT_END_NODE);
241 return 0;
242 }
243
fdt_add_string_(void * fdt,const char * s)244 static int fdt_add_string_(void *fdt, const char *s)
245 {
246 char *strtab = (char *)fdt + fdt_totalsize(fdt);
247 int strtabsize = fdt_size_dt_strings(fdt);
248 int len = strlen(s) + 1;
249 int struct_top, offset;
250
251 offset = -strtabsize - len;
252 struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
253 if (fdt_totalsize(fdt) + offset < struct_top)
254 return 0; /* no more room :( */
255
256 memcpy(strtab + offset, s, len);
257 fdt_set_size_dt_strings(fdt, strtabsize + len);
258 return offset;
259 }
260
261 /* Must only be used to roll back in case of error */
fdt_del_last_string_(void * fdt,const char * s)262 static void fdt_del_last_string_(void *fdt, const char *s)
263 {
264 int strtabsize = fdt_size_dt_strings(fdt);
265 int len = strlen(s) + 1;
266
267 fdt_set_size_dt_strings(fdt, strtabsize - len);
268 }
269
fdt_find_add_string_(void * fdt,const char * s,int * allocated)270 static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
271 {
272 char *strtab = (char *)fdt + fdt_totalsize(fdt);
273 int strtabsize = fdt_size_dt_strings(fdt);
274 const char *p;
275
276 *allocated = 0;
277
278 p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
279 if (p)
280 return p - strtab;
281
282 *allocated = 1;
283
284 return fdt_add_string_(fdt, s);
285 }
286
fdt_property_placeholder(void * fdt,const char * name,int len,void ** valp)287 int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
288 {
289 struct fdt_property *prop;
290 int nameoff;
291 int allocated;
292
293 FDT_SW_PROBE_STRUCT(fdt);
294
295 /* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
296 if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
297 allocated = 1;
298 nameoff = fdt_add_string_(fdt, name);
299 } else {
300 nameoff = fdt_find_add_string_(fdt, name, &allocated);
301 }
302 if (nameoff == 0)
303 return -FDT_ERR_NOSPACE;
304
305 prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
306 if (! prop) {
307 if (allocated)
308 fdt_del_last_string_(fdt, name);
309 return -FDT_ERR_NOSPACE;
310 }
311
312 prop->tag = cpu_to_fdt32(FDT_PROP);
313 prop->nameoff = cpu_to_fdt32(nameoff);
314 prop->len = cpu_to_fdt32(len);
315 *valp = prop->data;
316 return 0;
317 }
318
fdt_property(void * fdt,const char * name,const void * val,int len)319 int fdt_property(void *fdt, const char *name, const void *val, int len)
320 {
321 void *ptr;
322 int ret;
323
324 ret = fdt_property_placeholder(fdt, name, len, &ptr);
325 if (ret)
326 return ret;
327 memcpy(ptr, val, len);
328 return 0;
329 }
330
fdt_finish(void * fdt)331 int fdt_finish(void *fdt)
332 {
333 char *p = (char *)fdt;
334 fdt32_t *end;
335 int oldstroffset, newstroffset;
336 uint32_t tag;
337 int offset, nextoffset;
338
339 FDT_SW_PROBE_STRUCT(fdt);
340
341 /* Add terminator */
342 end = fdt_grab_space_(fdt, sizeof(*end));
343 if (! end)
344 return -FDT_ERR_NOSPACE;
345 *end = cpu_to_fdt32(FDT_END);
346
347 /* Relocate the string table */
348 oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
349 newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
350 memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
351 fdt_set_off_dt_strings(fdt, newstroffset);
352
353 /* Walk the structure, correcting string offsets */
354 offset = 0;
355 while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
356 if (tag == FDT_PROP) {
357 struct fdt_property *prop =
358 fdt_offset_ptr_w_(fdt, offset);
359 int nameoff;
360
361 nameoff = fdt32_to_cpu(prop->nameoff);
362 nameoff += fdt_size_dt_strings(fdt);
363 prop->nameoff = cpu_to_fdt32(nameoff);
364 }
365 offset = nextoffset;
366 }
367 if (nextoffset < 0)
368 return nextoffset;
369
370 /* Finally, adjust the header */
371 fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
372
373 /* And fix up fields that were keeping intermediate state. */
374 fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
375 fdt_set_magic(fdt, FDT_MAGIC);
376
377 return 0;
378 }
379