1 /*
2  * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 /* Helper functions to offer easier navigation of Device Tree Blob */
8 
9 #include <assert.h>
10 #include <errno.h>
11 #include <inttypes.h>
12 #include <stdint.h>
13 #include <string.h>
14 
15 #include <libfdt.h>
16 
17 #include <common/debug.h>
18 #include <common/fdt_wrappers.h>
19 #include <common/uuid.h>
20 
21 /*
22  * Read cells from a given property of the given node. Any number of 32-bit
23  * cells of the property can be read. Returns 0 on success, or a negative
24  * FDT error value otherwise.
25  */
fdt_read_uint32_array(const void * dtb,int node,const char * prop_name,unsigned int cells,uint32_t * value)26 int fdt_read_uint32_array(const void *dtb, int node, const char *prop_name,
27 			  unsigned int cells, uint32_t *value)
28 {
29 	const fdt32_t *prop;
30 	int value_len;
31 
32 	assert(dtb != NULL);
33 	assert(prop_name != NULL);
34 	assert(value != NULL);
35 	assert(node >= 0);
36 
37 	/* Access property and obtain its length (in bytes) */
38 	prop = fdt_getprop(dtb, node, prop_name, &value_len);
39 	if (prop == NULL) {
40 		VERBOSE("Couldn't find property %s in dtb\n", prop_name);
41 		return -FDT_ERR_NOTFOUND;
42 	}
43 
44 	/* Verify that property length can fill the entire array. */
45 	if (NCELLS((unsigned int)value_len) < cells) {
46 		WARN("Property length mismatch\n");
47 		return -FDT_ERR_BADVALUE;
48 	}
49 
50 	for (unsigned int i = 0U; i < cells; i++) {
51 		value[i] = fdt32_to_cpu(prop[i]);
52 	}
53 
54 	return 0;
55 }
56 
fdt_read_uint32(const void * dtb,int node,const char * prop_name,uint32_t * value)57 int fdt_read_uint32(const void *dtb, int node, const char *prop_name,
58 		    uint32_t *value)
59 {
60 	return fdt_read_uint32_array(dtb, node, prop_name, 1, value);
61 }
62 
fdt_read_uint32_default(const void * dtb,int node,const char * prop_name,uint32_t dflt_value)63 uint32_t fdt_read_uint32_default(const void *dtb, int node,
64 				 const char *prop_name, uint32_t dflt_value)
65 {
66 	uint32_t ret = dflt_value;
67 	int err = fdt_read_uint32(dtb, node, prop_name, &ret);
68 
69 	if (err < 0) {
70 		return dflt_value;
71 	}
72 
73 	return ret;
74 }
75 
fdt_read_uint64(const void * dtb,int node,const char * prop_name,uint64_t * value)76 int fdt_read_uint64(const void *dtb, int node, const char *prop_name,
77 		    uint64_t *value)
78 {
79 	uint32_t array[2] = {0, 0};
80 	int ret;
81 
82 	ret = fdt_read_uint32_array(dtb, node, prop_name, 2, array);
83 	if (ret < 0) {
84 		return ret;
85 	}
86 
87 	*value = ((uint64_t)array[0] << 32) | array[1];
88 	return 0;
89 }
90 
91 /*
92  * Read bytes from a given property of the given node. Any number of
93  * bytes of the property can be read. The fdt pointer is updated.
94  * Returns 0 on success, and -1 on error.
95  */
fdtw_read_bytes(const void * dtb,int node,const char * prop,unsigned int length,void * value)96 int fdtw_read_bytes(const void *dtb, int node, const char *prop,
97 		    unsigned int length, void *value)
98 {
99 	const void *ptr;
100 	int value_len;
101 
102 	assert(dtb != NULL);
103 	assert(prop != NULL);
104 	assert(value != NULL);
105 	assert(node >= 0);
106 
107 	/* Access property and obtain its length (in bytes) */
108 	ptr = fdt_getprop_namelen(dtb, node, prop, (int)strlen(prop),
109 					&value_len);
110 	if (ptr == NULL) {
111 		WARN("Couldn't find property %s in dtb\n", prop);
112 		return -1;
113 	}
114 
115 	/* Verify that property length is not less than number of bytes */
116 	if ((unsigned int)value_len < length) {
117 		WARN("Property length mismatch\n");
118 		return -1;
119 	}
120 
121 	(void)memcpy(value, ptr, length);
122 
123 	return 0;
124 }
125 
126 /*
127  * Read string from a given property of the given node. Up to 'size - 1'
128  * characters are read, and a NUL terminator is added. Returns 0 on success,
129  * and -1 upon error.
130  */
fdtw_read_string(const void * dtb,int node,const char * prop,char * str,size_t size)131 int fdtw_read_string(const void *dtb, int node, const char *prop,
132 		char *str, size_t size)
133 {
134 	const char *ptr;
135 	size_t len;
136 
137 	assert(dtb != NULL);
138 	assert(node >= 0);
139 	assert(prop != NULL);
140 	assert(str != NULL);
141 	assert(size > 0U);
142 
143 	ptr = fdt_getprop_namelen(dtb, node, prop, (int)strlen(prop), NULL);
144 	if (ptr == NULL) {
145 		WARN("Couldn't find property %s in dtb\n", prop);
146 		return -1;
147 	}
148 
149 	len = strlcpy(str, ptr, size);
150 	if (len >= size) {
151 		WARN("String of property %s in dtb has been truncated\n", prop);
152 		return -1;
153 	}
154 
155 	return 0;
156 }
157 
158 /*
159  * Read UUID from a given property of the given node. Returns 0 on success,
160  * and a negative value upon error.
161  */
fdtw_read_uuid(const void * dtb,int node,const char * prop,unsigned int length,uint8_t * uuid)162 int fdtw_read_uuid(const void *dtb, int node, const char *prop,
163 		   unsigned int length, uint8_t *uuid)
164 {
165 	/* Buffer for UUID string (plus NUL terminator) */
166 	char uuid_string[UUID_STRING_LENGTH + 1U];
167 	int err;
168 
169 	assert(dtb != NULL);
170 	assert(prop != NULL);
171 	assert(uuid != NULL);
172 	assert(node >= 0);
173 
174 	if (length < UUID_BYTES_LENGTH) {
175 		return -EINVAL;
176 	}
177 
178 	err = fdtw_read_string(dtb, node, prop, uuid_string,
179 			       UUID_STRING_LENGTH + 1U);
180 	if (err != 0) {
181 		return err;
182 	}
183 
184 	if (read_uuid(uuid, uuid_string) != 0) {
185 		return -FDT_ERR_BADVALUE;
186 	}
187 
188 	return 0;
189 }
190 
191 /*
192  * Write cells in place to a given property of the given node. At most 2 cells
193  * of the property are written. Returns 0 on success, and -1 upon error.
194  */
fdtw_write_inplace_cells(void * dtb,int node,const char * prop,unsigned int cells,void * value)195 int fdtw_write_inplace_cells(void *dtb, int node, const char *prop,
196 		unsigned int cells, void *value)
197 {
198 	int err, len;
199 
200 	assert(dtb != NULL);
201 	assert(prop != NULL);
202 	assert(value != NULL);
203 	assert(node >= 0);
204 
205 	/* We expect either 1 or 2 cell property */
206 	assert(cells <= 2U);
207 
208 	if (cells == 2U)
209 		*(uint64_t *)value = cpu_to_fdt64(*(uint64_t *)value);
210 	else
211 		*(uint32_t *)value = cpu_to_fdt32(*(uint32_t *)value);
212 
213 	len = (int)cells * 4;
214 
215 	/* Set property value in place */
216 	err = fdt_setprop_inplace(dtb, node, prop, value, len);
217 	if (err != 0) {
218 		WARN("Modify property %s failed with error %d\n", prop, err);
219 		return -1;
220 	}
221 
222 	return 0;
223 }
224 
225 /*
226  * Write bytes in place to a given property of the given node.
227  * Any number of bytes of the property can be written.
228  * Returns 0 on success, and < 0 on error.
229  */
fdtw_write_inplace_bytes(void * dtb,int node,const char * prop,unsigned int length,const void * data)230 int fdtw_write_inplace_bytes(void *dtb, int node, const char *prop,
231 			     unsigned int length, const void *data)
232 {
233 	const void *ptr;
234 	int namelen, value_len, err;
235 
236 	assert(dtb != NULL);
237 	assert(prop != NULL);
238 	assert(data != NULL);
239 	assert(node >= 0);
240 
241 	namelen = (int)strlen(prop);
242 
243 	/* Access property and obtain its length in bytes */
244 	ptr = fdt_getprop_namelen(dtb, node, prop, namelen, &value_len);
245 	if (ptr == NULL) {
246 		WARN("Couldn't find property %s in dtb\n", prop);
247 		return -1;
248 	}
249 
250 	/* Verify that property length is not less than number of bytes */
251 	if ((unsigned int)value_len < length) {
252 		WARN("Property length mismatch\n");
253 		return -1;
254 	}
255 
256 	/* Set property value in place */
257 	err = fdt_setprop_inplace_namelen_partial(dtb, node, prop,
258 						  namelen, 0,
259 						  data, (int)length);
260 	if (err != 0) {
261 		WARN("Set property %s failed with error %d\n", prop, err);
262 	}
263 
264 	return err;
265 }
266 
fdt_read_prop_cells(const fdt32_t * prop,int nr_cells)267 static uint64_t fdt_read_prop_cells(const fdt32_t *prop, int nr_cells)
268 {
269 	uint64_t reg = fdt32_to_cpu(prop[0]);
270 
271 	if (nr_cells > 1) {
272 		reg = (reg << 32) | fdt32_to_cpu(prop[1]);
273 	}
274 
275 	return reg;
276 }
277 
fdt_get_reg_props_by_index(const void * dtb,int node,int index,uintptr_t * base,size_t * size)278 int fdt_get_reg_props_by_index(const void *dtb, int node, int index,
279 			       uintptr_t *base, size_t *size)
280 {
281 	const fdt32_t *prop;
282 	int parent, len;
283 	int ac, sc;
284 	int cell;
285 
286 	parent = fdt_parent_offset(dtb, node);
287 	if (parent < 0) {
288 		return -FDT_ERR_BADOFFSET;
289 	}
290 
291 	ac = fdt_address_cells(dtb, parent);
292 	sc = fdt_size_cells(dtb, parent);
293 
294 	cell = index * (ac + sc);
295 
296 	prop = fdt_getprop(dtb, node, "reg", &len);
297 	if (prop == NULL) {
298 		WARN("Couldn't find \"reg\" property in dtb\n");
299 		return -FDT_ERR_NOTFOUND;
300 	}
301 
302 	if (((cell + ac + sc) * (int)sizeof(uint32_t)) > len) {
303 		return -FDT_ERR_BADVALUE;
304 	}
305 
306 	if (base != NULL) {
307 		*base = (uintptr_t)fdt_read_prop_cells(&prop[cell], ac);
308 	}
309 
310 	if (size != NULL) {
311 		*size = (size_t)fdt_read_prop_cells(&prop[cell + ac], sc);
312 	}
313 
314 	return 0;
315 }
316 
317 /*******************************************************************************
318  * This function fills reg node info (base & size) with an index found by
319  * checking the reg-names node.
320  * Returns 0 on success and a negative FDT error code on failure.
321  ******************************************************************************/
fdt_get_reg_props_by_name(const void * dtb,int node,const char * name,uintptr_t * base,size_t * size)322 int fdt_get_reg_props_by_name(const void *dtb, int node, const char *name,
323 			      uintptr_t *base, size_t *size)
324 {
325 	int index;
326 
327 	index = fdt_stringlist_search(dtb, node, "reg-names", name);
328 	if (index < 0) {
329 		return index;
330 	}
331 
332 	return fdt_get_reg_props_by_index(dtb, node, index, base, size);
333 }
334 
335 /*******************************************************************************
336  * This function gets the stdout path node.
337  * It reads the value indicated inside the device tree.
338  * Returns node offset on success and a negative FDT error code on failure.
339  ******************************************************************************/
fdt_get_stdout_node_offset(const void * dtb)340 int fdt_get_stdout_node_offset(const void *dtb)
341 {
342 	int node;
343 	const char *prop, *path;
344 	int len;
345 
346 	/* The /secure-chosen node takes precedence over the standard one. */
347 	node = fdt_path_offset(dtb, "/secure-chosen");
348 	if (node < 0) {
349 		node = fdt_path_offset(dtb, "/chosen");
350 		if (node < 0) {
351 			return -FDT_ERR_NOTFOUND;
352 		}
353 	}
354 
355 	prop = fdt_getprop(dtb, node, "stdout-path", NULL);
356 	if (prop == NULL) {
357 		return -FDT_ERR_NOTFOUND;
358 	}
359 
360 	/* Determine the actual path length, as a colon terminates the path. */
361 	path = strchr(prop, ':');
362 	if (path == NULL) {
363 		len = strlen(prop);
364 	} else {
365 		len = path - prop;
366 	}
367 
368 	/* Aliases cannot start with a '/', so it must be the actual path. */
369 	if (prop[0] == '/') {
370 		return fdt_path_offset_namelen(dtb, prop, len);
371 	}
372 
373 	/* Lookup the alias, as this contains the actual path. */
374 	path = fdt_get_alias_namelen(dtb, prop, len);
375 	if (path == NULL) {
376 		return -FDT_ERR_NOTFOUND;
377 	}
378 
379 	return fdt_path_offset(dtb, path);
380 }
381 
382 
383 /*******************************************************************************
384  * Only devices which are direct children of root node use CPU address domain.
385  * All other devices use addresses that are local to the device node and cannot
386  * directly used by CPU. Device tree provides an address translation mechanism
387  * through "ranges" property which provides mappings from local address space to
388  * parent address space. Since a device could be a child of a child node to the
389  * root node, there can be more than one level of address translation needed to
390  * map the device local address space to CPU address space.
391  * fdtw_translate_address() API performs address translation of a local address
392  * to a global address with help of various helper functions.
393  ******************************************************************************/
394 
fdtw_xlat_hit(const uint32_t * value,int child_addr_size,int parent_addr_size,int range_size,uint64_t base_address,uint64_t * translated_addr)395 static bool fdtw_xlat_hit(const uint32_t *value, int child_addr_size,
396 		int parent_addr_size, int range_size, uint64_t base_address,
397 		uint64_t *translated_addr)
398 {
399 	uint64_t local_address, parent_address, addr_range;
400 
401 	local_address = fdt_read_prop_cells(value, child_addr_size);
402 	parent_address = fdt_read_prop_cells(value + child_addr_size,
403 				parent_addr_size);
404 	addr_range = fdt_read_prop_cells(value + child_addr_size +
405 				parent_addr_size,
406 				range_size);
407 	VERBOSE("DT: Address %" PRIx64 " mapped to %" PRIx64 " with range %" PRIx64 "\n",
408 		local_address, parent_address, addr_range);
409 
410 	/* Perform range check */
411 	if ((base_address < local_address) ||
412 		(base_address >= local_address + addr_range)) {
413 		return false;
414 	}
415 
416 	/* Found hit for the addr range that needs to be translated */
417 	*translated_addr = parent_address + (base_address - local_address);
418 	VERBOSE("DT: child address %" PRIx64 "mapped to %" PRIx64 " in parent bus\n",
419 		local_address, parent_address);
420 	return true;
421 }
422 
423 #define ILLEGAL_ADDR	ULL(~0)
424 
fdtw_search_all_xlat_entries(const void * dtb,const struct fdt_property * ranges_prop,int local_bus,uint64_t base_address)425 static uint64_t fdtw_search_all_xlat_entries(const void *dtb,
426 				const struct fdt_property *ranges_prop,
427 				int local_bus, uint64_t base_address)
428 {
429 	uint64_t translated_addr;
430 	const uint32_t *next_entry;
431 	int parent_bus_node, nxlat_entries, length;
432 	int self_addr_cells, parent_addr_cells, self_size_cells, ncells_xlat;
433 
434 	/*
435 	 * The number of cells in one translation entry in ranges is the sum of
436 	 * the following values:
437 	 * self#address-cells + parent#address-cells + self#size-cells
438 	 * Ex: the iofpga ranges property has one translation entry with 4 cells
439 	 * They represent iofpga#addr-cells + motherboard#addr-cells + iofpga#size-cells
440 	 *              = 1                 + 2                      + 1
441 	 */
442 
443 	parent_bus_node = fdt_parent_offset(dtb, local_bus);
444 	self_addr_cells = fdt_address_cells(dtb, local_bus);
445 	self_size_cells = fdt_size_cells(dtb, local_bus);
446 	parent_addr_cells = fdt_address_cells(dtb, parent_bus_node);
447 
448 	/* Number of cells per translation entry i.e., mapping */
449 	ncells_xlat = self_addr_cells + parent_addr_cells + self_size_cells;
450 
451 	assert(ncells_xlat > 0);
452 
453 	/*
454 	 * Find the number of translations(mappings) specified in the current
455 	 * `ranges` property. Note that length represents number of bytes and
456 	 * is stored in big endian mode.
457 	 */
458 	length = fdt32_to_cpu(ranges_prop->len);
459 	nxlat_entries = (length/sizeof(uint32_t))/ncells_xlat;
460 
461 	assert(nxlat_entries > 0);
462 
463 	next_entry = (const uint32_t *)ranges_prop->data;
464 
465 	/* Iterate over the entries in the "ranges" */
466 	for (int i = 0; i < nxlat_entries; i++) {
467 		if (fdtw_xlat_hit(next_entry, self_addr_cells,
468 				parent_addr_cells, self_size_cells, base_address,
469 				&translated_addr)){
470 			return translated_addr;
471 		}
472 		next_entry = next_entry + ncells_xlat;
473 	}
474 
475 	INFO("DT: No translation found for address %" PRIx64 " in node %s\n",
476 	     base_address, fdt_get_name(dtb, local_bus, NULL));
477 	return ILLEGAL_ADDR;
478 }
479 
480 
481 /*******************************************************************************
482  * address mapping needs to be done recursively starting from current node to
483  * root node through all intermediate parent nodes.
484  * Sample device tree is shown here:
485 
486 smb@0,0 {
487 	compatible = "simple-bus";
488 
489 	#address-cells = <2>;
490 	#size-cells = <1>;
491 	ranges = <0 0 0 0x08000000 0x04000000>,
492 		 <1 0 0 0x14000000 0x04000000>,
493 		 <2 0 0 0x18000000 0x04000000>,
494 		 <3 0 0 0x1c000000 0x04000000>,
495 		 <4 0 0 0x0c000000 0x04000000>,
496 		 <5 0 0 0x10000000 0x04000000>;
497 
498 	motherboard {
499 		arm,v2m-memory-map = "rs1";
500 		compatible = "arm,vexpress,v2m-p1", "simple-bus";
501 		#address-cells = <2>;
502 		#size-cells = <1>;
503 		ranges;
504 
505 		iofpga@3,00000000 {
506 			compatible = "arm,amba-bus", "simple-bus";
507 			#address-cells = <1>;
508 			#size-cells = <1>;
509 			ranges = <0 3 0 0x200000>;
510 			v2m_serial1: uart@a0000 {
511 				compatible = "arm,pl011", "arm,primecell";
512 				reg = <0x0a0000 0x1000>;
513 				interrupts = <0 6 4>;
514 				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
515 				clock-names = "uartclk", "apb_pclk";
516 		};
517 	};
518 };
519 
520  * As seen above, there are 3 levels of address translations needed. An empty
521  * `ranges` property denotes identity mapping (as seen in `motherboard` node).
522  * Each ranges property can map a set of child addresses to parent bus. Hence
523  * there can be more than 1 (translation) entry in the ranges property as seen
524  * in the `smb` node which has 6 translation entries.
525  ******************************************************************************/
526 
527 /* Recursive implementation */
fdtw_translate_address(const void * dtb,int node,uint64_t base_address)528 uint64_t fdtw_translate_address(const void *dtb, int node,
529 				uint64_t base_address)
530 {
531 	int length, local_bus_node;
532 	const char *node_name;
533 	uint64_t global_address;
534 
535 	local_bus_node = fdt_parent_offset(dtb, node);
536 	node_name = fdt_get_name(dtb, local_bus_node, NULL);
537 
538 	/*
539 	 * In the example given above, starting from the leaf node:
540 	 * uart@a000 represents the current node
541 	 * iofpga@3,00000000 represents the local bus
542 	 * motherboard represents the parent bus
543 	 */
544 
545 	/* Read the ranges property */
546 	const struct fdt_property *property = fdt_get_property(dtb,
547 					local_bus_node, "ranges", &length);
548 
549 	if (property == NULL) {
550 		if (local_bus_node == 0) {
551 			/*
552 			 * root node doesn't have range property as addresses
553 			 * are in CPU address space.
554 			 */
555 			return base_address;
556 		}
557 		INFO("DT: Couldn't find ranges property in node %s\n",
558 			node_name);
559 		return ILLEGAL_ADDR;
560 	} else if (length == 0) {
561 		/* empty ranges indicates identity map to parent bus */
562 		return fdtw_translate_address(dtb, local_bus_node, base_address);
563 	}
564 
565 	VERBOSE("DT: Translation lookup in node %s at offset %d\n", node_name,
566 		local_bus_node);
567 	global_address = fdtw_search_all_xlat_entries(dtb, property,
568 				local_bus_node, base_address);
569 
570 	if (global_address == ILLEGAL_ADDR) {
571 		return ILLEGAL_ADDR;
572 	}
573 
574 	/* Translate the local device address recursively */
575 	return fdtw_translate_address(dtb, local_bus_node, global_address);
576 }
577 
578 /*
579  * For every CPU node (`/cpus/cpu@n`) in an FDT, execute a callback passing a
580  * pointer to the FDT and the offset of the CPU node. If the return value of the
581  * callback is negative, it is treated as an error and the loop is aborted. In
582  * this situation, the value of the callback is returned from the function.
583  *
584  * Returns `0` on success, or a negative integer representing an error code.
585  */
fdtw_for_each_cpu(const void * dtb,int (* callback)(const void * dtb,int node,uintptr_t mpidr))586 int fdtw_for_each_cpu(const void *dtb,
587 		      int (*callback)(const void *dtb, int node, uintptr_t mpidr))
588 {
589 	int ret = 0;
590 	int parent, node = 0;
591 
592 	parent = fdt_path_offset(dtb, "/cpus");
593 	if (parent < 0) {
594 		return parent;
595 	}
596 
597 	fdt_for_each_subnode(node, dtb, parent) {
598 		const char *name;
599 		int len;
600 
601 		uintptr_t mpidr = 0U;
602 
603 		name = fdt_get_name(dtb, node, &len);
604 		if (strncmp(name, "cpu@", 4) != 0) {
605 			continue;
606 		}
607 
608 		ret = fdt_get_reg_props_by_index(dtb, node, 0, &mpidr, NULL);
609 		if (ret < 0) {
610 			break;
611 		}
612 
613 		ret = callback(dtb, node, mpidr);
614 		if (ret < 0) {
615 			break;
616 		}
617 	}
618 
619 	return ret;
620 }
621 
622 /*
623  * Find a given node in device tree. If not present, add it.
624  * Returns offset of node found/added on success, and < 0 on error.
625  */
fdtw_find_or_add_subnode(void * fdt,int parentoffset,const char * name)626 int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name)
627 {
628 	int offset;
629 
630 	offset = fdt_subnode_offset(fdt, parentoffset, name);
631 
632 	if (offset == -FDT_ERR_NOTFOUND) {
633 		offset = fdt_add_subnode(fdt, parentoffset, name);
634 	}
635 
636 	if (offset < 0) {
637 		ERROR("%s: %s: %s\n", __func__, name, fdt_strerror(offset));
638 	}
639 
640 	return offset;
641 }
642