feat(msm8916): allow booting secondary CPU cores

Add support for the PSCI CPU_ON call to allow booting secondary CPU
cores. On cold boot they need to be booted with a special register
sequence. Also, the "boot remapper" needs to be configured to point to
the BL31_BASE, so the CPUs actually start executing BL31 after reset.

Change-Id: I406c508070ccb046bfdefd51554f12e1db671fd4
Signed-off-by: Stephan Gerhold <stephan@gerhold.net>

plat/qti/msm8916/msm8916_bl31_setup.c

@@ -134,6 +134,9 @@ static void msm8916_configure_timer(void)
  */
 #define APCS_GLB_SECURE_STS_NS		BIT_32(0)
 #define APCS_GLB_SECURE_PWR_NS		BIT_32(1)
+#define APCS_BOOT_START_ADDR_SEC	(APCS_CFG + 0x04)
+#define REMAP_EN			BIT_32(0)
+#define APCS_AA64NAA32_REG		(APCS_CFG + 0x0c)
 
 static void msm8916_configure_cpu_pm(void)
 {
@@ -158,6 +161,11 @@ static void msm8916_configure_cpu_pm(void)
 		mmio_write_32(APCS_ALIAS_ACS(cpu), 0);
 		mmio_write_32(APCS_ALIAS_SAW2(cpu), 0);
 	}
+
+	/* Make sure all further warm boots end up in BL31 and aarch64 state */
+	CASSERT((BL31_BASE & 0xffff) == 0, assert_bl31_base_64k_aligned);
+	mmio_write_32(APCS_BOOT_START_ADDR_SEC, BL31_BASE | REMAP_EN);
+	mmio_write_32(APCS_AA64NAA32_REG, 1);
 }
 
 /*

plat/qti/msm8916/msm8916_cpu_boot.c

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2021, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <msm8916_mmap.h>
+#include "msm8916_pm.h"
+
+#define CPU_PWR_CTL			0x4
+#define APC_PWR_GATE_CTL		0x14
+
+#define CPU_PWR_CTL_CLAMP		BIT_32(0)
+#define CPU_PWR_CTL_CORE_MEM_CLAMP	BIT_32(1)
+#define CPU_PWR_CTL_L1_RST_DIS		BIT_32(2)
+#define CPU_PWR_CTL_CORE_MEM_HS		BIT_32(3)
+#define CPU_PWR_CTL_CORE_RST		BIT_32(4)
+#define CPU_PWR_CTL_COREPOR_RST		BIT_32(5)
+#define CPU_PWR_CTL_GATE_CLK		BIT_32(6)
+#define CPU_PWR_CTL_CORE_PWRD_UP	BIT_32(7)
+
+#define APC_PWR_GATE_CTL_GHDS_EN	BIT_32(0)
+#define APC_PWR_GATE_CTL_GHDS_CNT(cnt)	((cnt) << 24)
+
+/* Boot a secondary CPU core for the first time. */
+void msm8916_cpu_boot(unsigned int core)
+{
+	uintptr_t acs = APCS_ALIAS_ACS(core);
+	uint32_t pwr_ctl;
+
+	pwr_ctl = CPU_PWR_CTL_CLAMP | CPU_PWR_CTL_CORE_MEM_CLAMP |
+		  CPU_PWR_CTL_CORE_RST | CPU_PWR_CTL_COREPOR_RST;
+	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
+	dsb();
+
+	mmio_write_32(acs + APC_PWR_GATE_CTL, APC_PWR_GATE_CTL_GHDS_EN |
+		      APC_PWR_GATE_CTL_GHDS_CNT(16));
+	dsb();
+	udelay(2);
+
+	pwr_ctl &= ~CPU_PWR_CTL_CORE_MEM_CLAMP;
+	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
+	dsb();
+
+	pwr_ctl |= CPU_PWR_CTL_CORE_MEM_HS;
+	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
+	dsb();
+	udelay(2);
+
+	pwr_ctl &= ~CPU_PWR_CTL_CLAMP;
+	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
+	dsb();
+	udelay(2);
+
+	pwr_ctl &= ~(CPU_PWR_CTL_CORE_RST | CPU_PWR_CTL_COREPOR_RST);
+	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
+	dsb();
+
+	pwr_ctl |= CPU_PWR_CTL_CORE_PWRD_UP;
+	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
+	dsb();
+}

plat/qti/msm8916/msm8916_pm.c

@@ -6,12 +6,30 @@
 
 #include <arch.h>
 #include <common/debug.h>
+#include <drivers/arm/gicv2.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
 #include <plat/common/platform.h>
 
 #include <msm8916_mmap.h>
+#include "msm8916_pm.h"
+
+static int msm8916_pwr_domain_on(u_register_t mpidr)
+{
+	unsigned int core = MPIDR_AFFLVL0_VAL(mpidr);
+
+	VERBOSE("PSCI: Booting CPU %d\n", core);
+	msm8916_cpu_boot(core);
+
+	return PSCI_E_SUCCESS;
+}
+
+static void msm8916_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	gicv2_pcpu_distif_init();
+	gicv2_cpuif_enable();
+}
 
 static void __dead2 msm8916_system_reset(void)
 {
@@ -23,6 +41,8 @@ static void __dead2 msm8916_system_reset(void)
 }
 
 static const plat_psci_ops_t msm8916_psci_ops = {
+	.pwr_domain_on			= msm8916_pwr_domain_on,
+	.pwr_domain_on_finish		= msm8916_pwr_domain_on_finish,
 	.system_off			= msm8916_system_reset,
 	.system_reset			= msm8916_system_reset,
 };

plat/qti/msm8916/msm8916_pm.h

@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MSM8916_PM_H
+#define MSM8916_PM_H
+
+void msm8916_cpu_boot(unsigned int core);
+
+#endif /* MSM8916_PM_H */

plat/qti/msm8916/platform.mk

@@ -19,6 +19,7 @@ BL31_SOURCES	+=	${GICV2_SOURCES}				\
 			plat/common/plat_gicv2.c			\
 			plat/common/plat_psci_common.c			\
 			plat/qti/msm8916/msm8916_bl31_setup.c		\
+			plat/qti/msm8916/msm8916_cpu_boot.c		\
 			plat/qti/msm8916/msm8916_gicv2.c		\
 			plat/qti/msm8916/msm8916_pm.c			\
 			plat/qti/msm8916/msm8916_topology.c		\