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 <samuel@sholland.org>
Signed-off-by: Andre Przywara <andre.przywara@arm.com>

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;