diff options
Diffstat (limited to 'common')
| -rw-r--r-- | common/aarch32/debug.S | 161 | ||||
| -rw-r--r-- | common/aarch64/debug.S | 39 | ||||
| -rw-r--r-- | common/backtrace/backtrace.c | 55 | ||||
| -rw-r--r-- | common/bl_common.c | 6 | ||||
| -rw-r--r-- | common/desc_image_load.c | 73 | ||||
| -rw-r--r-- | common/fdt_fixup.c | 233 | ||||
| -rw-r--r-- | common/fdt_wrappers.c | 405 |
7 files changed, 759 insertions, 213 deletions
diff --git a/common/aarch32/debug.S b/common/aarch32/debug.S index f50635691..9d410df07 100644 --- a/common/aarch32/debug.S +++ b/common/aarch32/debug.S @@ -1,71 +1,25 @@ /* - * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ #include <arch.h> #include <asm_macros.S> +#include <common/debug.h> + .globl asm_print_str + .globl asm_print_hex + .globl asm_print_hex_bits .globl asm_assert .globl do_panic .globl report_exception /* Since the max decimal input number is 65536 */ #define MAX_DEC_DIVISOR 10000 - /* The offset to add to get ascii for numerals '0 - 9' */ #define ASCII_OFFSET_NUM '0' - .section .rodata.panic_str, "aS" -panic_msg: - .asciz "PANIC at PC : 0x" -panic_end: - .asciz "\r\n" - - /*********************************************************** - * The common implementation of do_panic for all BL stages - ***********************************************************/ -func do_panic - /* Have LR copy point to PC at the time of panic */ - sub r6, lr, #4 - - /* Initialize crash console and verify success */ - bl plat_crash_console_init - cmp r0, #0 - beq 1f - - /* Print panic message */ - ldr r4, =panic_msg - bl asm_print_str - - /* Print LR in hex */ - mov r4, r6 - bl asm_print_hex - - /* Print new line */ - ldr r4, =panic_end - bl asm_print_str - - bl plat_crash_console_flush - -1: - mov lr, r6 - b plat_panic_handler -endfunc do_panic - - /*********************************************************** - * This function is called from the vector table for - * unhandled exceptions. It reads the current mode and - * passes it to platform. - ***********************************************************/ -func report_exception - mrs r0, cpsr - and r0, #MODE32_MASK - bl plat_report_exception - no_ret plat_panic_handler -endfunc report_exception - #if ENABLE_ASSERTIONS .section .rodata.assert_str, "aS" assert_msg1: @@ -79,6 +33,26 @@ assert_msg2: .asciz " Line 0x" #else .asciz " Line " + + /* + * This macro is intended to be used to print the + * line number in decimal. Used by asm_assert macro. + * The max number expected is 65536. + * In: r4 = the decimal to print. + * Clobber: lr, r0, r1, r2, r5, r6 + */ + .macro asm_print_line_dec + mov r6, #10 /* Divide by 10 after every loop iteration */ + ldr r5, =MAX_DEC_DIVISOR +dec_print_loop: + udiv r0, r4, r5 /* Get the quotient */ + mls r4, r0, r5, r4 /* Find the remainder */ + add r0, r0, #ASCII_OFFSET_NUM /* Convert to ascii */ + bl plat_crash_console_putc + udiv r5, r5, r6 /* Reduce divisor */ + cmp r5, #0 + bne dec_print_loop + .endm #endif /* --------------------------------------------------------------------------- @@ -100,25 +74,25 @@ func asm_assert mov r5, r0 mov r6, r1 - /* Initialize crash console and verify success */ + /* Ensure the console is initialized */ bl plat_crash_console_init + + /* Check if the console is initialized */ cmp r0, #0 - beq 1f + beq _assert_loop - /* Print file name */ + /* The console is initialized */ ldr r4, =assert_msg1 bl asm_print_str mov r4, r5 bl asm_print_str - - /* Print line number string */ ldr r4, =assert_msg2 bl asm_print_str - /* Test for maximum supported line number */ + /* Check if line number higher than max permitted */ ldr r4, =~0xffff tst r6, r4 - bne 1f + bne _assert_loop mov r4, r6 #if ARM_ARCH_MAJOR == 7 && !defined(ARMV7_SUPPORTS_VIRTUALIZATION) @@ -128,22 +102,10 @@ func asm_assert ******************************************************************/ bl asm_print_hex #else - /* Print line number in decimal */ - mov r6, #10 /* Divide by 10 after every loop iteration */ - ldr r5, =MAX_DEC_DIVISOR -dec_print_loop: - udiv r0, r4, r5 /* Quotient */ - mls r4, r0, r5, r4 /* Remainder */ - add r0, r0, #ASCII_OFFSET_NUM /* Convert to ASCII */ - bl plat_crash_console_putc - udiv r5, r5, r6 /* Reduce divisor */ - cmp r5, #0 - bne dec_print_loop + asm_print_line_dec #endif - bl plat_crash_console_flush - -1: +_assert_loop: #endif /* LOG_LEVEL >= LOG_LEVEL_INFO */ no_ret plat_panic_handler endfunc asm_assert @@ -171,8 +133,11 @@ endfunc asm_print_str * Clobber: lr, r0 - r3, r5 */ func asm_print_hex - mov r3, lr mov r5, #32 /* No of bits to convert to ascii */ + + /* Convert to ascii number of bits in r5 */ +asm_print_hex_bits: + mov r3, lr 1: sub r5, r5, #4 lsr r0, r4, r5 @@ -190,3 +155,53 @@ func asm_print_hex bne 1b bx r3 endfunc asm_print_hex + + /*********************************************************** + * The common implementation of do_panic for all BL stages + ***********************************************************/ + +.section .rodata.panic_str, "aS" + panic_msg: .asciz "PANIC at PC : 0x" + panic_end: .asciz "\r\n" + +func do_panic + /* Have LR copy point to PC at the time of panic */ + sub r6, lr, #4 + + /* Initialize crash console and verify success */ + bl plat_crash_console_init + + /* Check if the console is initialized */ + cmp r0, #0 + beq _panic_handler + + /* The console is initialized */ + ldr r4, =panic_msg + bl asm_print_str + + /* Print LR in hex */ + mov r4, r6 + bl asm_print_hex + + /* Print new line */ + ldr r4, =panic_end + bl asm_print_str + + bl plat_crash_console_flush + +_panic_handler: + mov lr, r6 + b plat_panic_handler +endfunc do_panic + + /*********************************************************** + * This function is called from the vector table for + * unhandled exceptions. It reads the current mode and + * passes it to platform. + ***********************************************************/ +func report_exception + mrs r0, cpsr + and r0, #MODE32_MASK + bl plat_report_exception + no_ret plat_panic_handler +endfunc report_exception diff --git a/common/aarch64/debug.S b/common/aarch64/debug.S index e6e329853..ad6acd9d2 100644 --- a/common/aarch64/debug.S +++ b/common/aarch64/debug.S @@ -1,5 +1,5 @@ /* - * Copyright (c) 2014-2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -38,11 +38,11 @@ assert_msg2: mov x6, #10 /* Divide by 10 after every loop iteration */ mov x5, #MAX_DEC_DIVISOR dec_print_loop: - udiv x0, x4, x5 /* Get the quotient */ - msub x4, x0, x5, x4 /* Find the remainder */ - add x0, x0, #ASCII_OFFSET_NUM /* Convert to ascii */ + udiv x0, x4, x5 /* Get the quotient */ + msub x4, x0, x5, x4 /* Find the remainder */ + add x0, x0, #ASCII_OFFSET_NUM /* Convert to ascii */ bl plat_crash_console_putc - udiv x5, x5, x6 /* Reduce divisor */ + udiv x5, x5, x6 /* Reduce divisor */ cbnz x5, dec_print_loop .endm @@ -64,10 +64,13 @@ func asm_assert */ mov x5, x0 mov x6, x1 + /* Ensure the console is initialized */ bl plat_crash_console_init + /* Check if the console is initialized */ cbz x0, _assert_loop + /* The console is initialized */ adr x4, assert_msg1 bl asm_print_str @@ -75,6 +78,7 @@ func asm_assert bl asm_print_str adr x4, assert_msg2 bl asm_print_str + /* Check if line number higher than max permitted */ tst x6, #~0xffff b.ne _assert_loop @@ -156,16 +160,32 @@ endfunc asm_print_newline /* This is for the non el3 BL stages to compile through */ .weak el3_panic + .weak elx_panic func do_panic #if CRASH_REPORTING str x0, [sp, #-0x10]! mrs x0, currentel - ubfx x0, x0, #2, #2 - cmp x0, #0x3 + ubfx x0, x0, #MODE_EL_SHIFT, #MODE_EL_WIDTH + cmp x0, #MODE_EL3 +#if !HANDLE_EA_EL3_FIRST ldr x0, [sp], #0x10 b.eq el3_panic -#endif +#else + b.ne to_panic_common + + /* Check EL the exception taken from */ + mrs x0, spsr_el3 + ubfx x0, x0, #SPSR_EL_SHIFT, #SPSR_EL_WIDTH + cmp x0, #MODE_EL3 + b.ne elx_panic + ldr x0, [sp], #0x10 + b el3_panic + +to_panic_common: + ldr x0, [sp], #0x10 +#endif /* HANDLE_EA_EL3_FIRST */ +#endif /* CRASH_REPORTING */ panic_common: /* @@ -175,12 +195,15 @@ panic_common: el3_panic: mov x6, x30 bl plat_crash_console_init + /* Check if the console is initialized */ cbz x0, _panic_handler + /* The console is initialized */ adr x4, panic_msg bl asm_print_str mov x4, x6 + /* The panic location is lr -4 */ sub x4, x4, #4 bl asm_print_hex diff --git a/common/backtrace/backtrace.c b/common/backtrace/backtrace.c index 506d4a482..25e2c707b 100644 --- a/common/backtrace/backtrace.c +++ b/common/backtrace/backtrace.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -37,48 +37,7 @@ struct frame_record { uintptr_t return_addr; }; -/* - * Strip the Pointer Authentication Code (PAC) from the address to retrieve the - * original one. - * - * The PAC field is stored on the high bits of the address and defined as: - * - PAC field = Xn[54:bottom_PAC_bit], when address tagging is used. - * - PAC field = Xn[63:56, 54:bottom_PAC_bit], without address tagging. - * - * With bottom_PAC_bit = 64 - TCR_ELx.TnSZ - */ -#if ENABLE_PAUTH -static uintptr_t demangle_address(uintptr_t addr) -{ - unsigned int el, t0sz, bottom_pac_bit; - uint64_t tcr, pac_mask; - - /* - * Different virtual address space size can be defined for each EL. - * Ensure that we use the proper one by reading the corresponding - * TCR_ELx register. - */ - el = get_current_el(); - - if (el == 3U) { - tcr = read_tcr_el3(); - } else if (el == 2U) { - tcr = read_tcr_el2(); - } else { - tcr = read_tcr_el1(); - } - - /* T0SZ = TCR_ELx[5:0] */ - t0sz = tcr & 0x1f; - bottom_pac_bit = 64 - t0sz; - pac_mask = (1ULL << bottom_pac_bit) - 1; - - /* demangle the address with the computed mask */ - return (addr & pac_mask); -} -#endif /* ENABLE_PAUTH */ - -static const char *get_el_str(unsigned int el) +const char *get_el_str(unsigned int el) { if (el == 3U) { return "EL3"; @@ -104,15 +63,14 @@ static bool is_address_readable(uintptr_t addr) * stack contains a PAC. It must be stripped to retrieve the return * address. */ - addr = demangle_address(addr); + xpaci(addr); #endif - if (el == 3U) { ats1e3r(addr); } else if (el == 2U) { ats1e2r(addr); } else { - ats1e1r(addr); + AT(ats1e1r, addr); } isb(); @@ -257,9 +215,8 @@ static void unwind_stack(struct frame_record *fr, uintptr_t current_pc, * the stack contains a PAC. It must be stripped to retrieve the * return address. */ - call_site = demangle_address(call_site); + xpaci(call_site); #endif - /* * If the address is invalid it means that the frame record is * probably corrupted. @@ -304,7 +261,7 @@ void backtrace(const char *cookie) struct frame_record *fr = __builtin_frame_address(0U); /* Printing the backtrace may crash the system, flush before starting */ - (void)console_flush(); + console_flush(); fr = adjust_frame_record(fr); diff --git a/common/bl_common.c b/common/bl_common.c index b74225b13..f17afcb11 100644 --- a/common/bl_common.c +++ b/common/bl_common.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -50,8 +50,8 @@ static int dyn_is_auth_disabled(void) uintptr_t page_align(uintptr_t value, unsigned dir) { /* Round up the limit to the next page boundary */ - if ((value & (PAGE_SIZE - 1U)) != 0U) { - value &= ~(PAGE_SIZE - 1U); + if ((value & PAGE_SIZE_MASK) != 0U) { + value &= ~PAGE_SIZE_MASK; if (dir == UP) value += PAGE_SIZE; } diff --git a/common/desc_image_load.c b/common/desc_image_load.c index f2e8f6054..30b97e050 100644 --- a/common/desc_image_load.c +++ b/common/desc_image_load.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -214,8 +214,9 @@ void populate_next_bl_params_config(bl_params_t *bl2_to_next_bl_params) { bl_params_node_t *params_node; unsigned int fw_config_id; - uintptr_t hw_config_base = 0, fw_config_base; + uintptr_t fw_config_base; bl_mem_params_node_t *mem_params; + uintptr_t hw_config_base = 0; assert(bl2_to_next_bl_params != NULL); @@ -224,6 +225,7 @@ void populate_next_bl_params_config(bl_params_t *bl2_to_next_bl_params) * if available. */ mem_params = get_bl_mem_params_node(HW_CONFIG_ID); + if (mem_params != NULL) hw_config_base = mem_params->image_info.image_base; @@ -237,8 +239,16 @@ void populate_next_bl_params_config(bl_params_t *bl2_to_next_bl_params) fw_config_id = SOC_FW_CONFIG_ID; break; case BL32_IMAGE_ID: + /* + * At the moment, OPTEE cannot accept a DTB in secure memory, + * so fall back and use NT_FW_CONFIG instead. + * This MUST be fixed as soon as OPTEE has support to + * receive DTBs in secure memory. + */ +#ifndef SPD_opteed fw_config_id = TOS_FW_CONFIG_ID; break; +#endif case BL33_IMAGE_ID: fw_config_id = NT_FW_CONFIG_ID; break; @@ -249,31 +259,50 @@ void populate_next_bl_params_config(bl_params_t *bl2_to_next_bl_params) if (fw_config_id != INVALID_IMAGE_ID) { mem_params = get_bl_mem_params_node(fw_config_id); - if (mem_params != NULL) + if (mem_params != NULL) { fw_config_base = mem_params->image_info.image_base; + } } +#ifdef SPD_opteed /* - * Pass hw and tb_fw config addresses to next images. NOTE - for - * EL3 runtime images (BL31 for AArch64 and BL32 for AArch32), - * arg0 is already used by generic code. Take care of not - * overwriting the previous initialisations. + * If SPD_opteed is enabled, arg[0,2] are populated by + * parse_optee_header(), which is called by + * arm_bl2_handle_post_image_load(). The meaning of the + * arguments are: + * arg0 <-- MODE_RW + * arg1 <-- Paged image base + * arg2 <-- Paged image size */ - if (params_node == bl2_to_next_bl_params->head) { - if (params_node->ep_info->args.arg1 == 0U) - params_node->ep_info->args.arg1 = + if (params_node->image_id == BL32_IMAGE_ID) { + params_node->ep_info->args.arg3 = fw_config_base; + } else { +#endif + /* + * Pass hw and tb_fw config addresses to next images. + * NOTE - for EL3 runtime images (BL31 for AArch64 + * and BL32 for AArch32), arg0 is already used by + * generic code. Take care of not overwriting the + * previous initialisations. + */ + if (params_node == bl2_to_next_bl_params->head) { + if (params_node->ep_info->args.arg1 == 0U) + params_node->ep_info->args.arg1 = fw_config_base; - if (params_node->ep_info->args.arg2 == 0U) - params_node->ep_info->args.arg2 = + if (params_node->ep_info->args.arg2 == 0U) + params_node->ep_info->args.arg2 = hw_config_base; - } else { - if (params_node->ep_info->args.arg0 == 0U) - params_node->ep_info->args.arg0 = + } else { + if (params_node->ep_info->args.arg0 == 0U) + params_node->ep_info->args.arg0 = fw_config_base; - if (params_node->ep_info->args.arg1 == 0U) - params_node->ep_info->args.arg1 = + if (params_node->ep_info->args.arg1 == 0U) + params_node->ep_info->args.arg1 = hw_config_base; + } +#ifdef SPD_opteed } +#endif } } @@ -301,9 +330,9 @@ void bl31_params_parse_helper(u_register_t param, image_info_t *bl33_image_info; } *v1 = (void *)(uintptr_t)param; assert(v1->h.type == PARAM_BL31); - if (bl32_ep_info_out) + if (bl32_ep_info_out != NULL) *bl32_ep_info_out = *v1->bl32_ep_info; - if (bl33_ep_info_out) + if (bl33_ep_info_out != NULL) *bl33_ep_info_out = *v1->bl33_ep_info; return; } @@ -311,12 +340,12 @@ void bl31_params_parse_helper(u_register_t param, assert(v2->h.version == PARAM_VERSION_2); assert(v2->h.type == PARAM_BL_PARAMS); - for (node = v2->head; node; node = node->next_params_info) { + for (node = v2->head; node != NULL; node = node->next_params_info) { if (node->image_id == BL32_IMAGE_ID) - if (bl32_ep_info_out) + if (bl32_ep_info_out != NULL) *bl32_ep_info_out = *node->ep_info; if (node->image_id == BL33_IMAGE_ID) - if (bl33_ep_info_out) + if (bl33_ep_info_out != NULL) *bl33_ep_info_out = *node->ep_info; } } diff --git a/common/fdt_fixup.c b/common/fdt_fixup.c index d518eb2a4..e88a55008 100644 --- a/common/fdt_fixup.c +++ b/common/fdt_fixup.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -14,15 +14,20 @@ * that. */ +#include <errno.h> +#include <stdio.h> #include <string.h> #include <libfdt.h> +#include <arch.h> #include <common/debug.h> +#include <common/fdt_fixup.h> +#include <common/fdt_wrappers.h> #include <drivers/console.h> #include <lib/psci/psci.h> +#include <plat/common/platform.h> -#include <common/fdt_fixup.h> static int append_psci_compatible(void *fdt, int offs, const char *str) { @@ -210,3 +215,227 @@ int fdt_add_reserved_memory(void *dtb, const char *node_name, return 0; } + +/******************************************************************************* + * fdt_add_cpu() Add a new CPU node to the DT + * @dtb: Pointer to the device tree blob in memory + * @parent: Offset of the parent node + * @mpidr: MPIDR for the current CPU + * + * Create and add a new cpu node to a DTB. + * + * Return the offset of the new node or a negative value in case of error + ******************************************************************************/ + +static int fdt_add_cpu(void *dtb, int parent, u_register_t mpidr) +{ + int cpu_offs; + int err; + char snode_name[15]; + uint64_t reg_prop; + + reg_prop = mpidr & MPID_MASK & ~MPIDR_MT_MASK; + + snprintf(snode_name, sizeof(snode_name), "cpu@%x", + (unsigned int)reg_prop); + + cpu_offs = fdt_add_subnode(dtb, parent, snode_name); + if (cpu_offs < 0) { + ERROR ("FDT: add subnode \"%s\" failed: %i\n", + snode_name, cpu_offs); + return cpu_offs; + } + + err = fdt_setprop_string(dtb, cpu_offs, "compatible", "arm,armv8"); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "compatible", cpu_offs); + return err; + } + + err = fdt_setprop_u64(dtb, cpu_offs, "reg", reg_prop); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "reg", cpu_offs); + return err; + } + + err = fdt_setprop_string(dtb, cpu_offs, "device_type", "cpu"); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "device_type", cpu_offs); + return err; + } + + err = fdt_setprop_string(dtb, cpu_offs, "enable-method", "psci"); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "enable-method", cpu_offs); + return err; + } + + return cpu_offs; +} + +/****************************************************************************** + * fdt_add_cpus_node() - Add the cpus node to the DTB + * @dtb: pointer to the device tree blob in memory + * @afflv0: Maximum number of threads per core (affinity level 0). + * @afflv1: Maximum number of CPUs per cluster (affinity level 1). + * @afflv2: Maximum number of clusters (affinity level 2). + * + * Iterate over all the possible MPIDs given the maximum affinity levels and + * add a cpus node to the DTB with all the valid CPUs on the system. + * If there is already a /cpus node, exit gracefully + * + * A system with two CPUs would generate a node equivalent or similar to: + * + * cpus { + * #address-cells = <2>; + * #size-cells = <0>; + * + * cpu0: cpu@0 { + * compatible = "arm,armv8"; + * reg = <0x0 0x0>; + * device_type = "cpu"; + * enable-method = "psci"; + * }; + * cpu1: cpu@10000 { + * compatible = "arm,armv8"; + * reg = <0x0 0x100>; + * device_type = "cpu"; + * enable-method = "psci"; + * }; + * }; + * + * Full documentation about the CPU bindings can be found at: + * https://www.kernel.org/doc/Documentation/devicetree/bindings/arm/cpus.txt + * + * Return the offset of the node or a negative value on error. + ******************************************************************************/ + +int fdt_add_cpus_node(void *dtb, unsigned int afflv0, + unsigned int afflv1, unsigned int afflv2) +{ + int offs; + int err; + unsigned int i, j, k; + u_register_t mpidr; + int cpuid; + + if (fdt_path_offset(dtb, "/cpus") >= 0) { + return -EEXIST; + } + + offs = fdt_add_subnode(dtb, 0, "cpus"); + if (offs < 0) { + ERROR ("FDT: add subnode \"cpus\" node to parent node failed"); + return offs; + } + + err = fdt_setprop_u32(dtb, offs, "#address-cells", 2); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "#address-cells", offs); + return err; + } + + err = fdt_setprop_u32(dtb, offs, "#size-cells", 0); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "#size-cells", offs); + return err; + } + + /* + * Populate the node with the CPUs. + * As libfdt prepends subnodes within a node, reverse the index count + * so the CPU nodes would be better ordered. + */ + for (i = afflv2; i > 0U; i--) { + for (j = afflv1; j > 0U; j--) { + for (k = afflv0; k > 0U; k--) { + mpidr = ((i - 1) << MPIDR_AFF2_SHIFT) | + ((j - 1) << MPIDR_AFF1_SHIFT) | + ((k - 1) << MPIDR_AFF0_SHIFT) | + (read_mpidr_el1() & MPIDR_MT_MASK); + + cpuid = plat_core_pos_by_mpidr(mpidr); + if (cpuid >= 0) { + /* Valid MPID found */ + err = fdt_add_cpu(dtb, offs, mpidr); + if (err < 0) { + ERROR ("FDT: %s 0x%08x\n", + "error adding CPU", + (uint32_t)mpidr); + return err; + } + } + } + } + } + + return offs; +} + +/** + * fdt_adjust_gic_redist() - Adjust GICv3 redistributor size + * @dtb: Pointer to the DT blob in memory + * @nr_cores: Number of CPU cores on this system. + * @gicr_frame_size: Size of the GICR frame per core + * + * On a GICv3 compatible interrupt controller, the redistributor provides + * a number of 64k pages per each supported core. So with a dynamic topology, + * this size cannot be known upfront and thus can't be hardcoded into the DTB. + * + * Find the DT node describing the GICv3 interrupt controller, and adjust + * the size of the redistributor to match the number of actual cores on + * this system. + * A GICv4 compatible redistributor uses four 64K pages per core, whereas GICs + * without support for direct injection of virtual interrupts use two 64K pages. + * The @gicr_frame_size parameter should be 262144 and 131072, respectively. + * + * Return: 0 on success, negative error value otherwise. + */ +int fdt_adjust_gic_redist(void *dtb, unsigned int nr_cores, + unsigned int gicr_frame_size) +{ + int offset = fdt_node_offset_by_compatible(dtb, 0, "arm,gic-v3"); + uint64_t redist_size_64; + uint32_t redist_size_32; + void *val; + int parent; + int ac, sc; + + if (offset < 0) { + return offset; + } + + parent = fdt_parent_offset(dtb, offset); + if (parent < 0) { + return parent; + } + ac = fdt_address_cells(dtb, parent); + sc = fdt_size_cells(dtb, parent); + if (ac < 0 || sc < 0) { + return -EINVAL; + } + + if (sc == 1) { + redist_size_32 = cpu_to_fdt32(nr_cores * gicr_frame_size); + val = &redist_size_32; + } else { + redist_size_64 = cpu_to_fdt64(nr_cores * + (uint64_t)gicr_frame_size); + val = &redist_size_64; + } + + /* + * The redistributor is described in the second "reg" entry. + * So we have to skip one address and one size cell, then another + * address cell to get to the second size cell. + */ + return fdt_setprop_inplace_namelen_partial(dtb, offset, "reg", 3, + (ac + sc + ac) * 4, + val, sc * 4); +} diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c index ca5b4556d..5aad14e38 100644 --- a/common/fdt_wrappers.c +++ b/common/fdt_wrappers.c @@ -15,90 +15,72 @@ #include <common/fdt_wrappers.h> /* - * Read cells from a given property of the given node. At most 2 cells of the - * property are read, and pointer is updated. Returns 0 on success, and -1 upon - * error + * 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 fdtw_read_cells(const void *dtb, int node, const char *prop, - unsigned int cells, void *value) +int fdt_read_uint32_array(const void *dtb, int node, const char *prop_name, + unsigned int cells, uint32_t *value) { - const uint32_t *value_ptr; - uint32_t hi = 0, lo; + const fdt32_t *prop; int value_len; assert(dtb != NULL); - assert(prop != NULL); + assert(prop_name != NULL); assert(value != NULL); assert(node >= 0); - /* We expect either 1 or 2 cell property */ - assert(cells <= 2U); - /* Access property and obtain its length (in bytes) */ - value_ptr = fdt_getprop_namelen(dtb, node, prop, (int)strlen(prop), - &value_len); - if (value_ptr == NULL) { - WARN("Couldn't find property %s in dtb\n", prop); - return -1; + prop = fdt_getprop(dtb, node, prop_name, &value_len); + if (prop == NULL) { + WARN("Couldn't find property %s in dtb\n", prop_name); + return -FDT_ERR_NOTFOUND; } - /* Verify that property length accords with cell length */ - if (NCELLS((unsigned int)value_len) != cells) { + /* Verify that property length can fill the entire array. */ + if (NCELLS((unsigned int)value_len) < cells) { WARN("Property length mismatch\n"); - return -1; + return -FDT_ERR_BADVALUE; } - if (cells == 2U) { - hi = fdt32_to_cpu(*value_ptr); - value_ptr++; + for (unsigned int i = 0U; i < cells; i++) { + value[i] = fdt32_to_cpu(prop[i]); } - lo = fdt32_to_cpu(*value_ptr); - - if (cells == 2U) - *((uint64_t *) value) = ((uint64_t) hi << 32) | lo; - else - *((uint32_t *) value) = lo; - return 0; } -/* - * Read cells from a given property of the given node. Any number of 32-bit - * cells of the property can be read. The fdt pointer is updated. Returns 0 on - * success, and -1 on error. - */ -int fdtw_read_array(const void *dtb, int node, const char *prop, - unsigned int cells, void *value) +int fdt_read_uint32(const void *dtb, int node, const char *prop_name, + uint32_t *value) { - const uint32_t *value_ptr; - int value_len; + return fdt_read_uint32_array(dtb, node, prop_name, 1, value); +} - assert(dtb != NULL); - assert(prop != NULL); - assert(value != NULL); - assert(node >= 0); +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); - /* Access property and obtain its length (in bytes) */ - value_ptr = fdt_getprop_namelen(dtb, node, prop, (int)strlen(prop), - &value_len); - if (value_ptr == NULL) { - WARN("Couldn't find property %s in dtb\n", prop); - return -1; + if (err < 0) { + return dflt_value; } - /* Verify that property length accords with cell length */ - if (NCELLS((unsigned int)value_len) != cells) { - WARN("Property length mismatch\n"); - return -1; - } + return ret; +} - uint32_t *dst = value; +int fdt_read_uint64(const void *dtb, int node, const char *prop_name, + uint64_t *value) +{ + uint32_t array[2] = {0, 0}; + int ret; - for (unsigned int i = 0U; i < cells; i++) { - dst[i] = fdt32_to_cpu(value_ptr[i]); + 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; } @@ -244,3 +226,314 @@ int fdtw_write_inplace_bytes(void *dtb, int node, const char *prop, 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 %llx mapped to %llx with range %llx\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 %llx mapped to %llx 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 %llx 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); +} |