diff --git a/plat/allwinner/common/sunxi_pm.c b/plat/allwinner/common/sunxi_pm.c index fb3842dbfaf25da5a7ff7a14fbe450db9b935656..fcab130cd04e029f2e99083939bc152599648e82 100644 --- a/plat/allwinner/common/sunxi_pm.c +++ b/plat/allwinner/common/sunxi_pm.c @@ -8,11 +8,13 @@ #include <assert.h> #include <debug.h> #include <delay_timer.h> +#include <gicv2.h> #include <mmio.h> #include <platform.h> #include <platform_def.h> #include <psci.h> #include <sunxi_mmap.h> +#include <sunxi_cpucfg.h> #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;