From 102b0d2daa97dae68d3eed54d8fe37a9cc38a892 Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Sun, 28 Apr 2024 11:13:47 +0200 Subject: Adding upstream version 2.8.0+dfsg. Signed-off-by: Daniel Baumann --- common/fdt_wrappers.c | 641 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 641 insertions(+) create mode 100644 common/fdt_wrappers.c (limited to 'common/fdt_wrappers.c') diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c new file mode 100644 index 0000000..1b065b1 --- /dev/null +++ b/common/fdt_wrappers.c @@ -0,0 +1,641 @@ +/* + * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +/* Helper functions to offer easier navigation of Device Tree Blob */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +/* + * Read cells from a given property of the given node. Any number of 32-bit + * cells of the property can be read. Returns 0 on success, or a negative + * FDT error value otherwise. + */ +int fdt_read_uint32_array(const void *dtb, int node, const char *prop_name, + unsigned int cells, uint32_t *value) +{ + const fdt32_t *prop; + int value_len; + + assert(dtb != NULL); + assert(prop_name != NULL); + assert(value != NULL); + assert(node >= 0); + + /* Access property and obtain its length (in bytes) */ + prop = fdt_getprop(dtb, node, prop_name, &value_len); + if (prop == NULL) { + VERBOSE("Couldn't find property %s in dtb\n", prop_name); + return -FDT_ERR_NOTFOUND; + } + + /* Verify that property length can fill the entire array. */ + if (NCELLS((unsigned int)value_len) < cells) { + WARN("Property length mismatch\n"); + return -FDT_ERR_BADVALUE; + } + + for (unsigned int i = 0U; i < cells; i++) { + value[i] = fdt32_to_cpu(prop[i]); + } + + return 0; +} + +int fdt_read_uint32(const void *dtb, int node, const char *prop_name, + uint32_t *value) +{ + return fdt_read_uint32_array(dtb, node, prop_name, 1, value); +} + +uint32_t fdt_read_uint32_default(const void *dtb, int node, + const char *prop_name, uint32_t dflt_value) +{ + uint32_t ret = dflt_value; + int err = fdt_read_uint32(dtb, node, prop_name, &ret); + + if (err < 0) { + return dflt_value; + } + + return ret; +} + +int fdt_read_uint64(const void *dtb, int node, const char *prop_name, + uint64_t *value) +{ + uint32_t array[2] = {0, 0}; + int ret; + + ret = fdt_read_uint32_array(dtb, node, prop_name, 2, array); + if (ret < 0) { + return ret; + } + + *value = ((uint64_t)array[0] << 32) | array[1]; + return 0; +} + +/* + * Read bytes from a given property of the given node. Any number of + * bytes of the property can be read. The fdt pointer is updated. + * Returns 0 on success, and -1 on error. + */ +int fdtw_read_bytes(const void *dtb, int node, const char *prop, + unsigned int length, void *value) +{ + const void *ptr; + int value_len; + + assert(dtb != NULL); + assert(prop != NULL); + assert(value != NULL); + assert(node >= 0); + + /* Access property and obtain its length (in bytes) */ + ptr = fdt_getprop_namelen(dtb, node, prop, (int)strlen(prop), + &value_len); + if (ptr == NULL) { + WARN("Couldn't find property %s in dtb\n", prop); + return -1; + } + + /* Verify that property length is not less than number of bytes */ + if ((unsigned int)value_len < length) { + WARN("Property length mismatch\n"); + return -1; + } + + (void)memcpy(value, ptr, length); + + return 0; +} + +/* + * Read string from a given property of the given node. Up to 'size - 1' + * characters are read, and a NUL terminator is added. Returns 0 on success, + * and -1 upon error. + */ +int fdtw_read_string(const void *dtb, int node, const char *prop, + char *str, size_t size) +{ + const char *ptr; + size_t len; + + assert(dtb != NULL); + assert(node >= 0); + assert(prop != NULL); + assert(str != NULL); + assert(size > 0U); + + ptr = fdt_getprop_namelen(dtb, node, prop, (int)strlen(prop), NULL); + if (ptr == NULL) { + WARN("Couldn't find property %s in dtb\n", prop); + return -1; + } + + len = strlcpy(str, ptr, size); + if (len >= size) { + WARN("String of property %s in dtb has been truncated\n", prop); + return -1; + } + + return 0; +} + +/* + * Read UUID from a given property of the given node. Returns 0 on success, + * and a negative value upon error. + */ +int fdtw_read_uuid(const void *dtb, int node, const char *prop, + unsigned int length, uint8_t *uuid) +{ + /* Buffer for UUID string (plus NUL terminator) */ + char uuid_string[UUID_STRING_LENGTH + 1U]; + int err; + + assert(dtb != NULL); + assert(prop != NULL); + assert(uuid != NULL); + assert(node >= 0); + + if (length < UUID_BYTES_LENGTH) { + return -EINVAL; + } + + err = fdtw_read_string(dtb, node, prop, uuid_string, + UUID_STRING_LENGTH + 1U); + if (err != 0) { + return err; + } + + if (read_uuid(uuid, uuid_string) != 0) { + return -FDT_ERR_BADVALUE; + } + + return 0; +} + +/* + * Write cells in place to a given property of the given node. At most 2 cells + * of the property are written. Returns 0 on success, and -1 upon error. + */ +int fdtw_write_inplace_cells(void *dtb, int node, const char *prop, + unsigned int cells, void *value) +{ + int err, len; + + assert(dtb != NULL); + assert(prop != NULL); + assert(value != NULL); + assert(node >= 0); + + /* We expect either 1 or 2 cell property */ + assert(cells <= 2U); + + if (cells == 2U) + *(uint64_t *)value = cpu_to_fdt64(*(uint64_t *)value); + else + *(uint32_t *)value = cpu_to_fdt32(*(uint32_t *)value); + + len = (int)cells * 4; + + /* Set property value in place */ + err = fdt_setprop_inplace(dtb, node, prop, value, len); + if (err != 0) { + WARN("Modify property %s failed with error %d\n", prop, err); + return -1; + } + + return 0; +} + +/* + * Write bytes in place to a given property of the given node. + * Any number of bytes of the property can be written. + * Returns 0 on success, and < 0 on error. + */ +int fdtw_write_inplace_bytes(void *dtb, int node, const char *prop, + unsigned int length, const void *data) +{ + const void *ptr; + int namelen, value_len, err; + + assert(dtb != NULL); + assert(prop != NULL); + assert(data != NULL); + assert(node >= 0); + + namelen = (int)strlen(prop); + + /* Access property and obtain its length in bytes */ + ptr = fdt_getprop_namelen(dtb, node, prop, namelen, &value_len); + if (ptr == NULL) { + WARN("Couldn't find property %s in dtb\n", prop); + return -1; + } + + /* Verify that property length is not less than number of bytes */ + if ((unsigned int)value_len < length) { + WARN("Property length mismatch\n"); + return -1; + } + + /* Set property value in place */ + err = fdt_setprop_inplace_namelen_partial(dtb, node, prop, + namelen, 0, + data, (int)length); + if (err != 0) { + WARN("Set property %s failed with error %d\n", prop, err); + } + + return err; +} + +static uint64_t fdt_read_prop_cells(const fdt32_t *prop, int nr_cells) +{ + uint64_t reg = fdt32_to_cpu(prop[0]); + + if (nr_cells > 1) { + reg = (reg << 32) | fdt32_to_cpu(prop[1]); + } + + return reg; +} + +int fdt_get_reg_props_by_index(const void *dtb, int node, int index, + uintptr_t *base, size_t *size) +{ + const fdt32_t *prop; + int parent, len; + int ac, sc; + int cell; + + parent = fdt_parent_offset(dtb, node); + if (parent < 0) { + return -FDT_ERR_BADOFFSET; + } + + ac = fdt_address_cells(dtb, parent); + sc = fdt_size_cells(dtb, parent); + + cell = index * (ac + sc); + + prop = fdt_getprop(dtb, node, "reg", &len); + if (prop == NULL) { + WARN("Couldn't find \"reg\" property in dtb\n"); + return -FDT_ERR_NOTFOUND; + } + + if (((cell + ac + sc) * (int)sizeof(uint32_t)) > len) { + return -FDT_ERR_BADVALUE; + } + + if (base != NULL) { + *base = (uintptr_t)fdt_read_prop_cells(&prop[cell], ac); + } + + if (size != NULL) { + *size = (size_t)fdt_read_prop_cells(&prop[cell + ac], sc); + } + + return 0; +} + +/******************************************************************************* + * This function fills reg node info (base & size) with an index found by + * checking the reg-names node. + * Returns 0 on success and a negative FDT error code on failure. + ******************************************************************************/ +int fdt_get_reg_props_by_name(const void *dtb, int node, const char *name, + uintptr_t *base, size_t *size) +{ + int index; + + index = fdt_stringlist_search(dtb, node, "reg-names", name); + if (index < 0) { + return index; + } + + return fdt_get_reg_props_by_index(dtb, node, index, base, size); +} + +/******************************************************************************* + * This function gets the stdout path node. + * It reads the value indicated inside the device tree. + * Returns node offset on success and a negative FDT error code on failure. + ******************************************************************************/ +int fdt_get_stdout_node_offset(const void *dtb) +{ + int node; + const char *prop, *path; + int len; + + /* The /secure-chosen node takes precedence over the standard one. */ + node = fdt_path_offset(dtb, "/secure-chosen"); + if (node < 0) { + node = fdt_path_offset(dtb, "/chosen"); + if (node < 0) { + return -FDT_ERR_NOTFOUND; + } + } + + prop = fdt_getprop(dtb, node, "stdout-path", NULL); + if (prop == NULL) { + return -FDT_ERR_NOTFOUND; + } + + /* Determine the actual path length, as a colon terminates the path. */ + path = strchr(prop, ':'); + if (path == NULL) { + len = strlen(prop); + } else { + len = path - prop; + } + + /* Aliases cannot start with a '/', so it must be the actual path. */ + if (prop[0] == '/') { + return fdt_path_offset_namelen(dtb, prop, len); + } + + /* Lookup the alias, as this contains the actual path. */ + path = fdt_get_alias_namelen(dtb, prop, len); + if (path == NULL) { + return -FDT_ERR_NOTFOUND; + } + + return fdt_path_offset(dtb, path); +} + + +/******************************************************************************* + * Only devices which are direct children of root node use CPU address domain. + * All other devices use addresses that are local to the device node and cannot + * directly used by CPU. Device tree provides an address translation mechanism + * through "ranges" property which provides mappings from local address space to + * parent address space. Since a device could be a child of a child node to the + * root node, there can be more than one level of address translation needed to + * map the device local address space to CPU address space. + * fdtw_translate_address() API performs address translation of a local address + * to a global address with help of various helper functions. + ******************************************************************************/ + +static bool 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) +{ + uint64_t local_address, parent_address, addr_range; + + local_address = fdt_read_prop_cells(value, child_addr_size); + parent_address = fdt_read_prop_cells(value + child_addr_size, + parent_addr_size); + addr_range = fdt_read_prop_cells(value + child_addr_size + + parent_addr_size, + range_size); + VERBOSE("DT: Address %" PRIx64 " mapped to %" PRIx64 " with range %" PRIx64 "\n", + local_address, parent_address, addr_range); + + /* Perform range check */ + if ((base_address < local_address) || + (base_address >= local_address + addr_range)) { + return false; + } + + /* Found hit for the addr range that needs to be translated */ + *translated_addr = parent_address + (base_address - local_address); + VERBOSE("DT: child address %" PRIx64 "mapped to %" PRIx64 " in parent bus\n", + local_address, parent_address); + return true; +} + +#define ILLEGAL_ADDR ULL(~0) + +static uint64_t fdtw_search_all_xlat_entries(const void *dtb, + const struct fdt_property *ranges_prop, + int local_bus, uint64_t base_address) +{ + uint64_t translated_addr; + const uint32_t *next_entry; + int parent_bus_node, nxlat_entries, length; + int self_addr_cells, parent_addr_cells, self_size_cells, ncells_xlat; + + /* + * The number of cells in one translation entry in ranges is the sum of + * the following values: + * self#address-cells + parent#address-cells + self#size-cells + * Ex: the iofpga ranges property has one translation entry with 4 cells + * They represent iofpga#addr-cells + motherboard#addr-cells + iofpga#size-cells + * = 1 + 2 + 1 + */ + + parent_bus_node = fdt_parent_offset(dtb, local_bus); + self_addr_cells = fdt_address_cells(dtb, local_bus); + self_size_cells = fdt_size_cells(dtb, local_bus); + parent_addr_cells = fdt_address_cells(dtb, parent_bus_node); + + /* Number of cells per translation entry i.e., mapping */ + ncells_xlat = self_addr_cells + parent_addr_cells + self_size_cells; + + assert(ncells_xlat > 0); + + /* + * Find the number of translations(mappings) specified in the current + * `ranges` property. Note that length represents number of bytes and + * is stored in big endian mode. + */ + length = fdt32_to_cpu(ranges_prop->len); + nxlat_entries = (length/sizeof(uint32_t))/ncells_xlat; + + assert(nxlat_entries > 0); + + next_entry = (const uint32_t *)ranges_prop->data; + + /* Iterate over the entries in the "ranges" */ + for (int i = 0; i < nxlat_entries; i++) { + if (fdtw_xlat_hit(next_entry, self_addr_cells, + parent_addr_cells, self_size_cells, base_address, + &translated_addr)){ + return translated_addr; + } + next_entry = next_entry + ncells_xlat; + } + + INFO("DT: No translation found for address %" PRIx64 " in node %s\n", + base_address, fdt_get_name(dtb, local_bus, NULL)); + return ILLEGAL_ADDR; +} + + +/******************************************************************************* + * address mapping needs to be done recursively starting from current node to + * root node through all intermediate parent nodes. + * Sample device tree is shown here: + +smb@0,0 { + compatible = "simple-bus"; + + #address-cells = <2>; + #size-cells = <1>; + ranges = <0 0 0 0x08000000 0x04000000>, + <1 0 0 0x14000000 0x04000000>, + <2 0 0 0x18000000 0x04000000>, + <3 0 0 0x1c000000 0x04000000>, + <4 0 0 0x0c000000 0x04000000>, + <5 0 0 0x10000000 0x04000000>; + + motherboard { + arm,v2m-memory-map = "rs1"; + compatible = "arm,vexpress,v2m-p1", "simple-bus"; + #address-cells = <2>; + #size-cells = <1>; + ranges; + + iofpga@3,00000000 { + compatible = "arm,amba-bus", "simple-bus"; + #address-cells = <1>; + #size-cells = <1>; + ranges = <0 3 0 0x200000>; + v2m_serial1: uart@a0000 { + compatible = "arm,pl011", "arm,primecell"; + reg = <0x0a0000 0x1000>; + interrupts = <0 6 4>; + clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>; + clock-names = "uartclk", "apb_pclk"; + }; + }; +}; + + * As seen above, there are 3 levels of address translations needed. An empty + * `ranges` property denotes identity mapping (as seen in `motherboard` node). + * Each ranges property can map a set of child addresses to parent bus. Hence + * there can be more than 1 (translation) entry in the ranges property as seen + * in the `smb` node which has 6 translation entries. + ******************************************************************************/ + +/* Recursive implementation */ +uint64_t fdtw_translate_address(const void *dtb, int node, + uint64_t base_address) +{ + int length, local_bus_node; + const char *node_name; + uint64_t global_address; + + local_bus_node = fdt_parent_offset(dtb, node); + node_name = fdt_get_name(dtb, local_bus_node, NULL); + + /* + * In the example given above, starting from the leaf node: + * uart@a000 represents the current node + * iofpga@3,00000000 represents the local bus + * motherboard represents the parent bus + */ + + /* Read the ranges property */ + const struct fdt_property *property = fdt_get_property(dtb, + local_bus_node, "ranges", &length); + + if (property == NULL) { + if (local_bus_node == 0) { + /* + * root node doesn't have range property as addresses + * are in CPU address space. + */ + return base_address; + } + INFO("DT: Couldn't find ranges property in node %s\n", + node_name); + return ILLEGAL_ADDR; + } else if (length == 0) { + /* empty ranges indicates identity map to parent bus */ + return fdtw_translate_address(dtb, local_bus_node, base_address); + } + + VERBOSE("DT: Translation lookup in node %s at offset %d\n", node_name, + local_bus_node); + global_address = fdtw_search_all_xlat_entries(dtb, property, + local_bus_node, base_address); + + if (global_address == ILLEGAL_ADDR) { + return ILLEGAL_ADDR; + } + + /* Translate the local device address recursively */ + return fdtw_translate_address(dtb, local_bus_node, global_address); +} + +/* + * For every CPU node (`/cpus/cpu@n`) in an FDT, execute a callback passing a + * pointer to the FDT and the offset of the CPU node. If the return value of the + * callback is negative, it is treated as an error and the loop is aborted. In + * this situation, the value of the callback is returned from the function. + * + * Returns `0` on success, or a negative integer representing an error code. + */ +int fdtw_for_each_cpu(const void *dtb, + int (*callback)(const void *dtb, int node, uintptr_t mpidr)) +{ + int ret = 0; + int parent, node = 0; + + parent = fdt_path_offset(dtb, "/cpus"); + if (parent < 0) { + return parent; + } + + fdt_for_each_subnode(node, dtb, parent) { + const char *name; + int len; + + uintptr_t mpidr = 0U; + + name = fdt_get_name(dtb, node, &len); + if (strncmp(name, "cpu@", 4) != 0) { + continue; + } + + ret = fdt_get_reg_props_by_index(dtb, node, 0, &mpidr, NULL); + if (ret < 0) { + break; + } + + ret = callback(dtb, node, mpidr); + if (ret < 0) { + break; + } + } + + return ret; +} + +/* + * Find a given node in device tree. If not present, add it. + * Returns offset of node found/added on success, and < 0 on error. + */ +int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name) +{ + int offset; + + offset = fdt_subnode_offset(fdt, parentoffset, name); + + if (offset == -FDT_ERR_NOTFOUND) { + offset = fdt_add_subnode(fdt, parentoffset, name); + } + + if (offset < 0) { + ERROR("%s: %s: %s\n", __func__, name, fdt_strerror(offset)); + } + + return offset; +} -- cgit v1.2.3