Home | History | Annotate | Line # | Download | only in dist 
      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 
     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  */
     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  */
     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 
     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 
     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 
    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 
    141 int fdt_create(void *buf, int bufsize)
    142 {
    143 	return fdt_create_with_flags(buf, bufsize, 0);
    144 }
    145 
    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 
    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 
    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 
    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 
    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 
    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 */
    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 
    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 
    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 
    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 
    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