diff options
| -rw-r--r-- | spm/cactus/aarch64/cactus_entrypoint.S | 43 | ||||
| -rw-r--r-- | spm/cactus/cactus_main.c | 44 | ||||
| -rw-r--r-- | spm/cactus/plat/arm/fvp/include/cactus_platform_def.h | 1 |
3 files changed, 63 insertions, 25 deletions
diff --git a/spm/cactus/aarch64/cactus_entrypoint.S b/spm/cactus/aarch64/cactus_entrypoint.S index b0f89d4..7e63856 100644 --- a/spm/cactus/aarch64/cactus_entrypoint.S +++ b/spm/cactus/aarch64/cactus_entrypoint.S @@ -7,25 +7,35 @@ #include <arch.h> #include <asm_macros.S> #include <cactus_def.h> -#include <platform_def.h> +#include <cactus_platform_def.h> .globl cactus_entrypoint + .globl secondary_cold_entry +/* Provision one stack per Execution Context (or vCPU) */ .section .bss.stacks .balign CACHE_WRITEBACK_GRANULE - .fill CACTUS_STACKS_SIZE + .fill CACTUS_STACKS_SIZE * PLAT_CACTUS_CORE_COUNT stacks_end: func cactus_entrypoint + /* Entry reason is primary EC cold boot */ + mov x19, #1 +secondary_cold_entry: + /* Entry reason is secondary EC cold boot */ + mrs x0, mpidr_el1 + bl platform_get_core_pos /* Setup the stack pointer. */ - adr x0, stacks_end - mov sp, x0 + adr x1, stacks_end + mov x2, #CACTUS_STACKS_SIZE + mul x2, x0, x2 + sub sp, x1, x2 /* Enable I-Cache */ - mrs x0, sctlr_el1 - orr x0, x0, #SCTLR_I_BIT - msr sctlr_el1, x0 + mrs x1, sctlr_el1 + orr x1, x1, #SCTLR_I_BIT + msr sctlr_el1, x1 isb /* @@ -38,6 +48,15 @@ func cactus_entrypoint msr cpacr_el1, x0 isb + /* Set up exceptions vector table */ + adrp x1, tftf_vector + add x1, x1, :lo12:tftf_vector + msr vbar_el1, x1 + isb + + /* Skip to main if warm boot */ + cbz x19, 0f + /* Relocate symbols */ pie_fixup: ldr x0, =pie_fixup @@ -46,13 +65,7 @@ pie_fixup: add x1, x1, x0 bl fixup_gdt_reloc - /* Set up exceptions vector table */ - adrp x0, tftf_vector - add x0, x0, :lo12:tftf_vector - msr vbar_el1, x0 - isb - - /* And jump to the C entrypoint. */ + /* Jump to the C entrypoint (it does not return) */ +0: mov x0, x19 b cactus_main - endfunc cactus_entrypoint diff --git a/spm/cactus/cactus_main.c b/spm/cactus/cactus_main.c index 7e3f40b..e54f3b0 100644 --- a/spm/cactus/cactus_main.c +++ b/spm/cactus/cactus_main.c @@ -13,6 +13,7 @@ #include <drivers/arm/pl011.h> #include <drivers/console.h> #include <lib/aarch64/arch_helpers.h> +#include <lib/tftf_lib.h> #include <lib/xlat_tables/xlat_mmu_helpers.h> #include <lib/xlat_tables/xlat_tables_v2.h> #include <plat_arm.h> @@ -29,6 +30,8 @@ extern const char build_message[]; extern const char version_string[]; +extern void secondary_cold_entry(void); + /* * * Message loop function @@ -147,6 +150,16 @@ static void cactus_plat_configure_mmu(unsigned int vm_id) init_xlat_tables(); } +static void register_secondary_entrypoint(void) +{ + smc_args args; + + args.fid = FFA_SECONDARY_EP_REGISTER_SMC64; + args.arg1 = (u_register_t)&secondary_cold_entry; + + tftf_smc(&args); +} + int tftf_irq_handler_dispatcher(void) { ERROR("%s\n", __func__); @@ -154,31 +167,39 @@ int tftf_irq_handler_dispatcher(void) return 0; } -void __dead2 cactus_main(void) +void __dead2 cactus_main(bool primary_cold_boot) { assert(IS_IN_EL1() != 0); struct mailbox_buffers mb; - /* Clear BSS */ - memset((void *)CACTUS_BSS_START, - 0, CACTUS_BSS_END - CACTUS_BSS_START); - /* Get current FFA id */ smc_ret_values ffa_id_ret = ffa_id_get(); if (ffa_func_id(ffa_id_ret) != FFA_SUCCESS_SMC32) { ERROR("FFA_ID_GET failed.\n"); panic(); } - ffa_vm_id_t ffa_id = ffa_id_ret.ret2 & 0xffff; - mb.send = (void *) get_sp_tx_start(ffa_id); - mb.recv = (void *) get_sp_rx_start(ffa_id); - /* Configure and enable Stage-1 MMU, enable D-Cache */ - cactus_plat_configure_mmu(ffa_id); + if (primary_cold_boot == true) { + /* Clear BSS */ + memset((void *)CACTUS_BSS_START, + 0, CACTUS_BSS_END - CACTUS_BSS_START); + + + mb.send = (void *) get_sp_tx_start(ffa_id); + mb.recv = (void *) get_sp_rx_start(ffa_id); + + /* Configure and enable Stage-1 MMU, enable D-Cache */ + cactus_plat_configure_mmu(ffa_id); + } + enable_mmu_el1(0); + if (primary_cold_boot == false) { + goto msg_loop; + } + if (ffa_id == SPM_VM_ID_FIRST) { console_init(CACTUS_PL011_UART_BASE, CACTUS_PL011_UART_CLK_IN_HZ, @@ -210,9 +231,12 @@ void __dead2 cactus_main(void) INFO("FF-A id: %x\n", ffa_id); cactus_print_memory_layout(ffa_id); + register_secondary_entrypoint(); + /* Invoking Tests */ ffa_tests(&mb); +msg_loop: /* End up to message loop */ message_loop(ffa_id, &mb); diff --git a/spm/cactus/plat/arm/fvp/include/cactus_platform_def.h b/spm/cactus/plat/arm/fvp/include/cactus_platform_def.h index e879002..2e5240e 100644 --- a/spm/cactus/plat/arm/fvp/include/cactus_platform_def.h +++ b/spm/cactus/plat/arm/fvp/include/cactus_platform_def.h @@ -16,6 +16,7 @@ #define CACTUS_PL011_UART_CLK_IN_HZ PL011_UART2_CLK_IN_HZ #define PLAT_CACTUS_RX_BASE ULL(0x7300000) +#define PLAT_CACTUS_CORE_COUNT (8U) #define CACTUS_PRIMARY_EC_COUNT (8U) #define CACTUS_SECONDARY_EC_COUNT (8U) |