From 560581ecebfa16e8b728317abf8c0f01c7ed2139 Mon Sep 17 00:00:00 2001 From: Samuel Holland Date: Sat, 12 Aug 2017 04:07:39 -0500 Subject: [PATCH] allwinner: Add platform PSCI functions required for SMP The reset vector entry point is preserved across CPU resets, so it only needs to be set once at boot. Hotplugged CPUs are not actually powered down, but are put in a wfi with the GIC disconnected. With this commit, Linux is able to enable, hotplug and use all four CPUs. Signed-off-by: Samuel Holland Signed-off-by: Andre Przywara --- plat/allwinner/common/sunxi_pm.c | 50 ++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/plat/allwinner/common/sunxi_pm.c b/plat/allwinner/common/sunxi_pm.c index fb3842db..fcab130c 100644 --- a/plat/allwinner/common/sunxi_pm.c +++ b/plat/allwinner/common/sunxi_pm.c @@ -8,11 +8,13 @@ #include #include #include +#include #include #include #include #include #include +#include #define SUNXI_WDOG0_CTRL_REG (SUNXI_WDOG_BASE + 0x0010) #define SUNXI_WDOG0_CFG_REG (SUNXI_WDOG_BASE + 0x0014) @@ -20,6 +22,33 @@ #include "sunxi_private.h" +#define mpidr_is_valid(mpidr) ( \ + MPIDR_AFFLVL3_VAL(mpidr) == 0 && \ + MPIDR_AFFLVL2_VAL(mpidr) == 0 && \ + MPIDR_AFFLVL1_VAL(mpidr) < PLATFORM_CLUSTER_COUNT && \ + MPIDR_AFFLVL0_VAL(mpidr) < PLATFORM_MAX_CPUS_PER_CLUSTER) + +static int sunxi_pwr_domain_on(u_register_t mpidr) +{ + if (mpidr_is_valid(mpidr) == 0) + return PSCI_E_INTERN_FAIL; + + sunxi_cpu_on(MPIDR_AFFLVL1_VAL(mpidr), MPIDR_AFFLVL0_VAL(mpidr)); + + return PSCI_E_SUCCESS; +} + +static void sunxi_pwr_domain_off(const psci_power_state_t *target_state) +{ + gicv2_cpuif_disable(); +} + +static void sunxi_pwr_domain_on_finish(const psci_power_state_t *target_state) +{ + gicv2_pcpu_distif_init(); + gicv2_cpuif_enable(); +} + static void __dead2 sunxi_system_off(void) { /* Turn off all secondary CPUs */ @@ -44,9 +73,23 @@ static void __dead2 sunxi_system_reset(void) panic(); } +static int sunxi_validate_ns_entrypoint(uintptr_t ns_entrypoint) +{ + /* The non-secure entry point must be in DRAM */ + if (ns_entrypoint >= SUNXI_DRAM_BASE && + ns_entrypoint < SUNXI_DRAM_BASE + SUNXI_DRAM_SIZE) + return PSCI_E_SUCCESS; + + return PSCI_E_INVALID_ADDRESS; +} + static plat_psci_ops_t sunxi_psci_ops = { + .pwr_domain_on = sunxi_pwr_domain_on, + .pwr_domain_off = sunxi_pwr_domain_off, + .pwr_domain_on_finish = sunxi_pwr_domain_on_finish, .system_off = sunxi_system_off, .system_reset = sunxi_system_reset, + .validate_ns_entrypoint = sunxi_validate_ns_entrypoint, }; int plat_setup_psci_ops(uintptr_t sec_entrypoint, @@ -54,6 +97,13 @@ int plat_setup_psci_ops(uintptr_t sec_entrypoint, { assert(psci_ops); + for (int cpu = 0; cpu < PLATFORM_CORE_COUNT; cpu += 1) { + mmio_write_32(SUNXI_CPUCFG_RVBAR_LO_REG(cpu), + sec_entrypoint & 0xffffffff); + mmio_write_32(SUNXI_CPUCFG_RVBAR_HI_REG(cpu), + sec_entrypoint >> 32); + } + *psci_ops = &sunxi_psci_ops; return 0; -- 2.30.2