diff --git a/src/arch/arm64/armv8/secmon/Makefile.inc b/src/arch/arm64/armv8/secmon/Makefile.inc index 1b774ff7df..5284d39f39 100644 --- a/src/arch/arm64/armv8/secmon/Makefile.inc +++ b/src/arch/arm64/armv8/secmon/Makefile.inc @@ -34,6 +34,8 @@ secmon-y += secmon_init.c secmon-y += psci.c secmon-y += smc.c secmon-y += trampoline.S +secmon-y += ../cache.c +secmon-y += ../cpu.S secmon-y += ../exception.c secmon-y += ../../cpu.c secmon-y += ../../transition_asm.S ../../transition.c diff --git a/src/arch/arm64/armv8/secmon/psci.c b/src/arch/arm64/armv8/secmon/psci.c index f87419dda8..93c5bddcb9 100644 --- a/src/arch/arm64/armv8/secmon/psci.c +++ b/src/arch/arm64/armv8/secmon/psci.c @@ -220,6 +220,15 @@ void psci_turn_on_self(const struct cpu_action *action) psci_schedule_cpu_on(e); } +void psci_cpu_entry(void) +{ + /* + * Just wait for an action to be performed. Only CPU_ON is supported + * initially. i.e. no power down then wake. + */ + secmon_wait_for_action(); +} + static void psci_cpu_on(struct psci_func *pf) { uint64_t entry; @@ -483,7 +492,7 @@ static int psci_allocate_nodes(void) return 0; } -void psci_init(void) +void psci_init(uintptr_t cpu_on_entry) { struct cpu_action action = { .run = &psci_link_cpu_info, diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c index 3405027f2f..63bd4907d9 100644 --- a/src/arch/arm64/armv8/secmon/secmon_init.c +++ b/src/arch/arm64/armv8/secmon/secmon_init.c @@ -20,6 +20,7 @@ */ #include +#include #include #include #include @@ -30,6 +31,74 @@ #include #include "secmon.h" +/* Common CPU state for all CPUs running in secmon. */ +struct cpu_resume_data { + uint64_t mair; + uint64_t tcr; + uint64_t ttbr0; + uint64_t scr; +}; + +static struct cpu_resume_data resume_data; + +static void secmon_init(struct secmon_params *params, int bsp); + +static void secmon_init_bsp(void *arg) +{ + secmon_init(arg, 1); +} + +static void secmon_init_nonbsp(void *arg) +{ + secmon_init(arg, 0); +} + +/* + * This variable holds entry point for secmon init code. Once the stacks are + * setup by the stage_entry.S, it jumps to c_entry. + */ +void (*c_entry[2])(void *) = { &secmon_init_bsp, &secmon_init_nonbsp }; + +static void cpu_resume(void *unused) +{ + uint32_t sctlr; + + /* Re-enable exception vector. */ + exception_hwinit(); + + tlbiall_el3(); + raw_write_mair_el3(resume_data.mair); + raw_write_tcr_el3(resume_data.tcr); + raw_write_ttbr0_el3(resume_data.ttbr0); + dsb(); + isb(); + + /* Enable MMU */ + sctlr = raw_read_sctlr_el3(); + sctlr |= SCTLR_C | SCTLR_M | SCTLR_I; + raw_write_sctlr_el3(sctlr); + isb(); + + raw_write_scr_el3(resume_data.scr); + isb(); + + psci_cpu_entry(); +} + +static void cpu_resume_init(void) +{ + /* Change entry points into secmon. */ + c_entry[0] = c_entry[1] = cpu_resume; + dcache_clean_by_mva(&c_entry, sizeof(c_entry)); + + /* Back up state. */ + resume_data.mair = raw_read_mair_el3(); + resume_data.tcr = raw_read_tcr_el3(); + resume_data.ttbr0 = raw_read_ttbr0_el3(); + resume_data.scr = raw_read_scr_el3(); + dcache_clean_by_mva(&resume_data, sizeof(resume_data)); +} + static void start_up_cpu(void *arg) { struct secmon_params *params = arg; @@ -81,7 +150,10 @@ static void secmon_init(struct secmon_params *params, int bsp) wait_for_all_cpus(params->online_cpus); smc_init(); - psci_init(); + psci_init((uintptr_t)arm64_cpu_startup); + + /* Initialize the resume path. */ + cpu_resume_init(); /* Make sure all non-BSP CPUs take action before the BSP. */ arch_run_on_all_cpus_but_self_async(&action); @@ -89,27 +161,11 @@ static void secmon_init(struct secmon_params *params, int bsp) start_up_cpu(params); printk(BIOS_ERR, "CPU turn on failed for BSP.\n"); - while (1) - ; + + secmon_wait_for_action(); } void secmon_wait_for_action(void) { arch_cpu_wait_for_action(); } - -static void secmon_init_bsp(void *arg) -{ - secmon_init(arg, 1); -} - -static void secmon_init_nonbsp(void *arg) -{ - secmon_init(arg, 0); -} - -/* - * This variable holds entry point for secmon init code. Once the stacks are - * setup by the stage_entry.S, it jumps to c_entry. - */ -void (*c_entry[2])(void*) = { &secmon_init_bsp, &secmon_init_nonbsp }; diff --git a/src/arch/arm64/c_entry.c b/src/arch/arm64/c_entry.c index 5e3dfe871d..de8ebcdf0f 100644 --- a/src/arch/arm64/c_entry.c +++ b/src/arch/arm64/c_entry.c @@ -70,8 +70,6 @@ static void secondary_cpu_start(void) */ void (*c_entry[2])(void) = { &arm64_init, &secondary_cpu_start }; -extern void arm64_cpu_startup(void); - void *prepare_secondary_cpu_startup(void) { return secondary_entry_point(&arm64_cpu_startup); diff --git a/src/arch/arm64/include/arch/psci.h b/src/arch/arm64/include/arch/psci.h index 32a32b1345..555333a44d 100644 --- a/src/arch/arm64/include/arch/psci.h +++ b/src/arch/arm64/include/arch/psci.h @@ -187,10 +187,13 @@ static inline void psci64_return(struct psci_func *pf, int64_t val) psci64_result(pf, 0, val); } -void psci_init(void); +void psci_init(uintptr_t cpu_on_entry); +void psci_soc_init(uintptr_t cpu_on_entry); /* Turn on the current CPU within the PSCI subsystem. */ void psci_turn_on_self(const struct cpu_action *action); int psci_turn_off_self(void); +/* Entry point for CPUs just turning on or waking up. */ +void psci_cpu_entry(void); #endif /* __ARCH_PSCI_H__ */ diff --git a/src/arch/arm64/include/arch/stages.h b/src/arch/arm64/include/arch/stages.h index fd633e484f..c3f4e6e25f 100644 --- a/src/arch/arm64/include/arch/stages.h +++ b/src/arch/arm64/include/arch/stages.h @@ -33,17 +33,4 @@ void jmp_to_elf_entry(void *entry, unsigned long buffer, unsigned long size); * be available to be used (such as console). */ void arm64_soc_init(void); -/* - * Stages and rmodules have 2 entry points: BSP and non-BSP. Provided - * a pointer the correct non-BSP entry point will be returned. The - * first instruction is for BSP and the 2nd is for non-BSP. Instructions - * are all 32-bit on arm64. - */ -static inline void *secondary_entry_point(void *e) -{ - uintptr_t nonbsp = (uintptr_t)e; - - return (void *)(nonbsp + sizeof(uint32_t)); -} - #endif diff --git a/src/arch/arm64/include/armv8/arch/cpu.h b/src/arch/arm64/include/armv8/arch/cpu.h index c6ca7d38c6..d968a06fe4 100644 --- a/src/arch/arm64/include/armv8/arch/cpu.h +++ b/src/arch/arm64/include/armv8/arch/cpu.h @@ -148,4 +148,26 @@ void arch_cpu_wait_for_action(void); */ unsigned int smp_processor_id(void); +/* + * Stages and rmodules have 2 entry points: BSP and non-BSP. Provided + * a pointer the correct non-BSP entry point will be returned. The + * first instruction is for BSP and the 2nd is for non-BSP. Instructions + * are all 32-bit on arm64. + */ +static inline void *secondary_entry_point(void *e) +{ + uintptr_t nonbsp = (uintptr_t)e; + + return (void *)(nonbsp + sizeof(uint32_t)); +} + +/* + * The arm64_cpu_startup() initializes a CPU's exception stack and regular + * stack as well initializing the C environment for the processor. It + * calls into the array of function pointers at symbol c_entry depending + * on BSP state. Note that arm64_cpu_startup contains secondary entry + * point which can be obtained by secondary_entry_point(). + */ +void arm64_cpu_startup(void); + #endif /* __ARCH_CPU_H__ */ diff --git a/src/soc/nvidia/tegra132/psci.c b/src/soc/nvidia/tegra132/psci.c index b039dfbe6f..6ba39e09b5 100644 --- a/src/soc/nvidia/tegra132/psci.c +++ b/src/soc/nvidia/tegra132/psci.c @@ -19,6 +19,10 @@ #include +void psci_soc_init(uintptr_t cpu_on_entry) +{ +} + static size_t children_at_level(int parent_level, uint64_t mpidr) { if (mpidr != 0)