| From 3c1140c29c39561848056fb4b9a03042b00279f3 Mon Sep 17 00:00:00 2001 |
| From: Jaxson Han <jaxson.han@arm.com> |
| Date: Wed, 29 Dec 2021 15:17:38 +0800 |
| Subject: [PATCH] common: Provide firmware info using libfdt |
| |
| Boot-wrapper uses libfdt to provide more info in device tree. |
| We add a new node to include those new firmware relevant infomation. |
| The new node defined as follows: |
| fw-shared-info { |
| compatible = "firmware,shared_info"; |
| |
| #address-cells = <0x02>; |
| #size-cells = <0x02>; |
| |
| version = "1.0"; |
| regions = <START_ADDR_HIGH START_ADDR_LOW SIZE_HIGH SIZE_LOW |
| 0x0 0x80000000 0x0 0x400000 |
| 0x0 0x90000000 0x0 0x400000 |
| 0x0 0xA0000000 0x0 0x400000>; |
| regions-permission = "RX", "R", "RWX", "RW"; |
| regions-cache = "Cache", "NCache", "Cache", "Device" |
| |
| function_entry = <ENTRY_ADDR_HIGH ENRTY_ADDR_LOW>; |
| }; |
| The node path is /fw-shared-info. |
| For boot-wrapper, in real case, it will be: |
| fw-shared-info { |
| compatible = "firmware,shared_info"; |
| |
| #address-cells = <0x02>; |
| #size-cells = <0x02>; |
| |
| version = "1.0"; |
| regions = <0x0 firmware_start 0x0 firmware_code_size |
| 0x0 firmware_data 0x0 firmware_data_size>; |
| regions-permission = "RX", "RW"; |
| regions-cache = "Cache", "Cache"; |
| |
| function_entry = <0x0 smc_fn_entry>; |
| }; |
| |
| Issue-Id: SCM-3816 |
| Upstream-Status: Inappropriate [other] |
| Implementation pending further discussion |
| Signed-off-by: Jaxson Han <jaxson.han@arm.com> |
| Change-Id: I6ebc59ce2bd3939b0fe066720d57821eaa1bed27 |
| --- |
| common/device_tree.c | 271 ++++++++++++++++++++++++++++++++++++++++++- |
| 1 file changed, 270 insertions(+), 1 deletion(-) |
| |
| diff --git a/common/device_tree.c b/common/device_tree.c |
| index 4d0876c..7f7befc 100644 |
| --- a/common/device_tree.c |
| +++ b/common/device_tree.c |
| @@ -8,13 +8,225 @@ |
| */ |
| #include <libfdt.h> |
| |
| +#define DEVICE_TREE_DEBUG 1 |
| + |
| +#define FW_NODE_NAME "/fw-shared-info" |
| +#define FW_COMPAT "firmware,shared_info" |
| +#define FW_INFO_VER "1.0" |
| + |
| +#ifdef BOOTWRAPPER_32 |
| +#define CELL_NUM 1 |
| +#define VAL_TYPE uint32_t |
| +#else |
| +#define CELL_NUM 2 |
| +#define VAL_TYPE uint64_t |
| +#endif |
| + |
| +#define ALIGN(x) (((x) + (FDT_TAGSIZE) - 1) & ~((FDT_TAGSIZE) - 1)) |
| + |
| extern unsigned long dtb; |
| -extern char firmware_start[], firmware_end[]; |
| +extern char firmware_start[], firmware_data[], firmware_end[]; |
| + |
| +extern long smc_fn_entry(unsigned long, unsigned long, unsigned long); |
| |
| extern void print_string(const char *str); |
| +extern void print_hex(unsigned int val); |
| |
| static void *blob; |
| |
| +static char *realloc_node(char *fdt, const char *name) |
| +{ |
| + int delta; |
| + int new_sz; |
| + /* FDT_BEGIN_NODE, node name in off_struct and FDT_END_NODE */ |
| + delta = sizeof(struct fdt_node_header) + ALIGN(strlen(name) + 1) |
| + + FDT_TAGSIZE; |
| + new_sz = fdt_totalsize(fdt) + delta; |
| + fdt_open_into(fdt, fdt, new_sz); |
| + return fdt; |
| +} |
| + |
| +static int create_node(const char *node_name) |
| +{ |
| + int node = 0; |
| + char *p; |
| + |
| + p = strrchr(node_name, '/'); |
| + if (!p) { |
| + print_string("node name without '/'\r\n"); |
| + return -1; |
| + } |
| + *p = '\0'; |
| + |
| + blob = realloc_node(blob, p + 1); |
| + |
| + if (p > node_name) { |
| + node = fdt_path_offset(blob, node_name); |
| + if (node < 0) { |
| + print_string("no node name\r\n"); |
| + return -1; |
| + } |
| + } |
| + |
| + node = fdt_add_subnode(blob, node, p + 1); |
| + if (node < 0) { |
| + print_string("add subnode err\r\n"); |
| + return -1; |
| + } |
| + |
| + return node; |
| +} |
| + |
| +static int dt_create_fw_node(void) { |
| + int fw_node; |
| + |
| + fw_node = fdt_path_offset(blob, FW_NODE_NAME); |
| + |
| + if(fw_node < 0) { |
| + fw_node = create_node(FW_NODE_NAME); |
| + } |
| + |
| + return fw_node; |
| +} |
| + |
| +static char *realloc_property(char *fdt, int nodeoffset, const char *name, |
| + int newlen) |
| +{ |
| + int delta = 0; |
| + int oldlen = 0; |
| + int new_sz; |
| + |
| + if (!fdt_get_property(fdt, nodeoffset, name, &oldlen)) |
| + delta = sizeof(struct fdt_property) + strlen(name) + 1; |
| + |
| + if (newlen > oldlen) |
| + delta += ALIGN(newlen) - ALIGN(oldlen); |
| + |
| + new_sz = fdt_totalsize(fdt) + delta; |
| + fdt_open_into(fdt, fdt, new_sz); |
| + return fdt; |
| +} |
| + |
| +static void dt_set_prop(int node, char *property, void *buf, int len) |
| +{ |
| + int err; |
| + |
| + err = fdt_setprop(blob, node, property, buf, len); |
| + if (err == -FDT_ERR_NOSPACE) { |
| + blob = realloc_property(blob, node, property, len); |
| + err = fdt_setprop(blob, node, property, buf, len); |
| + } |
| + if (err) { |
| + print_string("fdt error\n\r"); |
| + } |
| +} |
| + |
| +static void dt_set_prop_u32(int node, char *property, uint32_t val) |
| +{ |
| + fdt32_t fdt_val = cpu_to_fdt32(val); |
| + int len = sizeof(fdt32_t); |
| + |
| + dt_set_prop(node, property, (void*)&fdt_val, len); |
| +} |
| + |
| +static void dt_set_prop_u64(int node, char *property, uint64_t val) |
| +{ |
| + fdt64_t fdt_val = cpu_to_fdt64(val); |
| + int len = sizeof(fdt64_t); |
| + |
| + dt_set_prop(node, property, (void*)&fdt_val, len); |
| +} |
| + |
| +/* This dt_set_prop_u32_array maybe unused according to the BOOTWRAPPER_32 */ |
| +__attribute__((unused)) |
| +static void dt_set_prop_u32_array(int node, char *property, uint32_t *vals, |
| + int size) |
| +{ |
| + fdt32_t *fdt_vals = (fdt32_t*)vals; |
| + int len = sizeof(fdt32_t) * size; |
| + |
| + for (int i = 0; i < size; i++) { |
| + fdt_vals[i] = cpu_to_fdt32(vals[i]); |
| + } |
| + |
| + dt_set_prop(node, property, (void*)fdt_vals, len); |
| +} |
| + |
| +static void dt_set_prop_u64_array(int node, char *property, uint64_t *vals, |
| + int size) |
| +{ |
| + fdt64_t *fdt_vals = (fdt64_t*)vals; |
| + int len = sizeof(fdt64_t) * size; |
| + |
| + for (int i = 0; i < size; i++) { |
| + fdt_vals[i] = cpu_to_fdt64(vals[i]); |
| + } |
| + |
| + dt_set_prop(node, property, (void*)fdt_vals, len); |
| +} |
| + |
| +#if DEVICE_TREE_DEBUG |
| +static void dt_dump_string(const void *s, int len) |
| +{ |
| + char *sub = (char*)s; |
| + int sublen; |
| + while(*sub && ((uint64_t)sub - (uint64_t)s) < len) { |
| + sublen = strlen(sub) + 1; |
| + print_string(sub); |
| + print_string(" "); |
| + sub += sublen; |
| + } |
| + print_string("\n\r"); |
| +} |
| + |
| +static void dt_dump_fdt32_array(const void *vals, int len) |
| +{ |
| + fdt32_t *fdt_vals = (fdt32_t*)vals; |
| + len = len / sizeof(fdt32_t); |
| + for (int i = 0; i < len; i++) { |
| + print_hex(fdt32_to_cpu(fdt_vals[i])); |
| + print_string(" "); |
| + } |
| + print_string("\n\r"); |
| +} |
| + |
| +static void dt_dump(int node, char *property, char type) |
| +{ |
| + const void *val; |
| + int len; |
| + |
| + val = fdt_getprop(blob, node, property, &len); |
| + print_string(property); |
| + print_string(": "); |
| + |
| + if (type == 's') { |
| + /* string type */ |
| + dt_dump_string(val, len); |
| + return; |
| + } |
| + |
| + /* uint type */ |
| + dt_dump_fdt32_array(val, len); |
| +} |
| + |
| +void dt_dump_all(int node) |
| +{ |
| + if (node >= 0) { |
| + print_string(FW_NODE_NAME" info:\r\n"); |
| + dt_dump(node, "compatible", 's'); |
| + dt_dump(node, "version", 's'); |
| + dt_dump(node, "function_entry", 'i'); |
| + dt_dump(node, "address-cells", 'i'); |
| + dt_dump(node, "size-cells", 'i'); |
| + dt_dump(node, "regions", 'i'); |
| + dt_dump(node, "regions-permission", 's'); |
| + dt_dump(node, "regions-cache", 's'); |
| + print_string("\r\n"); |
| + } |
| +} |
| +#else |
| +void dt_dump_all(int node) { (void*)node; return; } |
| +#endif |
| |
| void dt_add_memreserve(void) |
| { |
| @@ -32,3 +244,60 @@ void dt_add_memreserve(void) |
| print_string("reserve mem add err\n\r"); |
| } |
| } |
| + |
| +void dt_fw_node_init(int enable) |
| +{ |
| + int fw_node; |
| + |
| + VAL_TYPE regions[] = { |
| + /* code region: start, end, ro, x, cachable */ |
| + (VAL_TYPE)firmware_start, |
| + (VAL_TYPE)(firmware_data - firmware_start), |
| + /* data region: start, end, rw, xn, cachable */ |
| + (VAL_TYPE)firmware_data, |
| + (VAL_TYPE)(firmware_end - firmware_data), |
| + }; |
| + int regions_num = sizeof(regions) / sizeof(VAL_TYPE); |
| + char regions_permission[] = "RX\0RW"; |
| + char regions_cache[] = "Cache\0Cache"; |
| + |
| + if (!enable) |
| + return; |
| + |
| + print_string("Prepare "FW_NODE_NAME" node\n\r"); |
| + |
| + blob = (void*)&dtb; |
| + |
| + if(fdt_path_offset(blob, "/psci") < 0) { |
| + print_string("/psci node not found\n\r"); |
| + return; |
| + } |
| + |
| + fw_node = dt_create_fw_node(); |
| + |
| + if(fw_node < 0) { |
| + print_string(FW_NODE_NAME" node create err\n\r"); |
| + } |
| + |
| + dt_set_prop(fw_node, "compatible", FW_COMPAT, sizeof(FW_COMPAT)); |
| + dt_set_prop(fw_node, "version", FW_INFO_VER, sizeof(FW_INFO_VER)); |
| + |
| + dt_set_prop_u32(fw_node, "address-cells", CELL_NUM); |
| + dt_set_prop_u32(fw_node, "size-cells", CELL_NUM); |
| + dt_set_prop(fw_node, "regions-permission", regions_permission, |
| + sizeof(regions_permission)); |
| + dt_set_prop(fw_node, "regions-cache", regions_cache, |
| + sizeof(regions_cache)); |
| + |
| +#ifdef BOOTWRAPPER_32 |
| + dt_set_prop_u32_array(fw_node, "regions", regions, regions_num); |
| + dt_set_prop_u32(fw_node, "function_entry", (VAL_TYPE)smc_fn_entry); |
| +#else |
| + dt_set_prop_u64_array(fw_node, "regions", regions, regions_num); |
| + dt_set_prop_u64(fw_node, "function_entry", (VAL_TYPE)smc_fn_entry); |
| +#endif |
| + |
| + fdt_pack(blob); |
| + |
| + dt_dump_all(fw_node); |
| +} |