diff --git a/plat/allwinner/common/include/sunxi_private.h b/plat/allwinner/common/include/sunxi_private.h index 1e1b0a4..1166879 100644 --- a/plat/allwinner/common/include/sunxi_private.h +++ b/plat/allwinner/common/include/sunxi_private.h @@ -9,9 +9,9 @@ void sunxi_configure_mmu_el3(int flags); -void sunxi_cpu_on(unsigned int cluster, unsigned int core); -void sunxi_cpu_off(unsigned int cluster, unsigned int core); -void sunxi_disable_secondary_cpus(unsigned int primary_cpu); +void sunxi_cpu_on(u_register_t mpidr); +void sunxi_cpu_off(u_register_t mpidr); +void sunxi_disable_secondary_cpus(u_register_t primary_mpidr); void __dead2 sunxi_power_down(void); int sunxi_pmic_setup(uint16_t socid, const void *fdt); diff --git a/plat/allwinner/common/sunxi_bl31_setup.c b/plat/allwinner/common/sunxi_bl31_setup.c index 7ffa658..a24527c 100644 --- a/plat/allwinner/common/sunxi_bl31_setup.c +++ b/plat/allwinner/common/sunxi_bl31_setup.c @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -101,7 +102,7 @@ SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE); /* Turn off all secondary CPUs */ - sunxi_disable_secondary_cpus(plat_my_core_pos()); + sunxi_disable_secondary_cpus(read_mpidr()); } void bl31_plat_arch_setup(void) diff --git a/plat/allwinner/common/sunxi_cpu_ops.c b/plat/allwinner/common/sunxi_cpu_ops.c index 8c08603..b4c9fcc 100644 --- a/plat/allwinner/common/sunxi_cpu_ops.c +++ b/plat/allwinner/common/sunxi_cpu_ops.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -45,9 +45,10 @@ mmio_write_32(SUNXI_CPU_POWER_CLAMP_REG(cluster, core), 0x00); } -void sunxi_cpu_off(unsigned int cluster, unsigned int core) +void sunxi_cpu_off(u_register_t mpidr) { - int corenr = cluster * PLATFORM_MAX_CPUS_PER_CLUSTER + core; + unsigned int cluster = MPIDR_AFFLVL1_VAL(mpidr); + unsigned int core = MPIDR_AFFLVL0_VAL(mpidr); VERBOSE("PSCI: Powering off cluster %d core %d\n", cluster, core); @@ -55,9 +56,9 @@ mmio_clrbits_32(SUNXI_CPUCFG_DBG_REG0, BIT(core)); /* We can't turn ourself off like this, but it works for other cores. */ - if (plat_my_core_pos() != corenr) { + if (read_mpidr() != mpidr) { /* Activate the core output clamps, but not for core 0. */ - if (corenr != 0) + if (core != 0) mmio_setbits_32(SUNXI_POWEROFF_GATING_REG(cluster), BIT(core)); /* Assert CPU power-on reset */ @@ -80,8 +81,11 @@ 0, BIT_32(core)); } -void sunxi_cpu_on(unsigned int cluster, unsigned int core) +void sunxi_cpu_on(u_register_t mpidr) { + unsigned int cluster = MPIDR_AFFLVL1_VAL(mpidr); + unsigned int core = MPIDR_AFFLVL0_VAL(mpidr); + VERBOSE("PSCI: Powering on cluster %d core %d\n", cluster, core); /* Assert CPU core reset */ @@ -102,12 +106,18 @@ mmio_setbits_32(SUNXI_CPUCFG_DBG_REG0, BIT(core)); } -void sunxi_disable_secondary_cpus(unsigned int primary_cpu) +void sunxi_disable_secondary_cpus(u_register_t primary_mpidr) { - for (unsigned int cpu = 0; cpu < PLATFORM_CORE_COUNT; cpu += 1) { - if (cpu == primary_cpu) - continue; - sunxi_cpu_off(cpu / PLATFORM_MAX_CPUS_PER_CLUSTER, - cpu % PLATFORM_MAX_CPUS_PER_CLUSTER); + unsigned int cluster; + unsigned int core; + + for (cluster = 0; cluster < PLATFORM_CLUSTER_COUNT; ++cluster) { + for (core = 0; core < PLATFORM_MAX_CPUS_PER_CLUSTER; ++core) { + u_register_t mpidr = (cluster << MPIDR_AFF1_SHIFT) | + (core << MPIDR_AFF0_SHIFT) | + BIT(31); + if (mpidr != primary_mpidr) + sunxi_cpu_off(mpidr); + } } } diff --git a/plat/allwinner/common/sunxi_pm.c b/plat/allwinner/common/sunxi_pm.c index 8cc715c..1d2dc93 100644 --- a/plat/allwinner/common/sunxi_pm.c +++ b/plat/allwinner/common/sunxi_pm.c @@ -35,7 +35,7 @@ if (mpidr_is_valid(mpidr) == 0) return PSCI_E_INTERN_FAIL; - sunxi_cpu_on(MPIDR_AFFLVL1_VAL(mpidr), MPIDR_AFFLVL0_VAL(mpidr)); + sunxi_cpu_on(mpidr); return PSCI_E_SUCCESS; } @@ -47,9 +47,7 @@ static void __dead2 sunxi_pwr_down_wfi(const psci_power_state_t *target_state) { - u_register_t mpidr = read_mpidr(); - - sunxi_cpu_off(MPIDR_AFFLVL1_VAL(mpidr), MPIDR_AFFLVL0_VAL(mpidr)); + sunxi_cpu_off(read_mpidr()); while (1) wfi(); @@ -64,7 +62,7 @@ static void __dead2 sunxi_system_off(void) { /* Turn off all secondary CPUs */ - sunxi_disable_secondary_cpus(plat_my_core_pos()); + sunxi_disable_secondary_cpus(read_mpidr()); sunxi_power_down(); }