arm: imx: imx8m: Add basic PSCI provider implementation

Implement basic PSCI provider to let OS turn CPU cores off and on,
power off and restart the system and determine PSCI version. This
is sufficient to remove the need for the ATF BL31 blob altogether.

To make use of this functionality, active the following Kconfig options:
  # CONFIG_PSCI_RESET is not set
  CONFIG_ARMV8_MULTIENTRY=y
  CONFIG_ARMV8_SET_SMPEN=y
  CONFIG_ARMV8_SPL_EXCEPTION_VECTORS=y
  CONFIG_ARMV8_EA_EL3_FIRST=y
  CONFIG_ARMV8_PSCI=y
  CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER=4
  CONFIG_ARMV8_SECURE_BASE=0x970000
  CONFIG_ARM_SMCCC=y
  CONFIG_SYS_HAS_ARMV8_SECURE_BASE=y

Signed-off-by: Marek Vasut <marex@denx.de>

3 files changed, 297 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/imx8m/Kconfig b/arch/arm/mach-imx/imx8m/Kconfig
index 6d35cda0be..c332f84994 100644
--- a/arch/arm/mach-imx/imx8m/Kconfig
+++ b/arch/arm/mach-imx/imx8m/Kconfig
@@ -27,6 +27,14 @@ config IMX8MP
 config SYS_SOC
 	default "imx8m"
 
+config SYS_HAS_ARMV8_SECURE_BASE
+	bool "Enable secure address for PSCI image"
+	depends on ARMV8_PSCI
+	help
+	  PSCI image can be re-located to secure RAM.
+	  If enabled, please also define the value for ARMV8_SECURE_BASE,
+	  for i.MX8M, it could be some address in OCRAM.
+
 choice
 	prompt "NXP i.MX8M board select"
 	optional
diff --git a/arch/arm/mach-imx/imx8m/Makefile b/arch/arm/mach-imx/imx8m/Makefile
index d9dee894aa..abd5ddc177 100644
--- a/arch/arm/mach-imx/imx8m/Makefile
+++ b/arch/arm/mach-imx/imx8m/Makefile
@@ -4,5 +4,6 @@
 
 obj-y += lowlevel_init.o
 obj-y += clock_slice.o soc.o
+obj-$(CONFIG_ARMV8_PSCI) += psci.o
 obj-$(CONFIG_IMX8MQ) += clock_imx8mq.o
 obj-$(CONFIG_IMX8MM)$(CONFIG_IMX8MN)$(CONFIG_IMX8MP) += clock_imx8mm.o
diff --git a/arch/arm/mach-imx/imx8m/psci.c b/arch/arm/mach-imx/imx8m/psci.c
new file mode 100644
index 0000000000..62f0b768cf
--- /dev/null
+++ b/arch/arm/mach-imx/imx8m/psci.c
@@ -0,0 +1,288 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file implements basic PSCI support for i.MX8M
+ *
+ * Copyright (C) 2022 Marek Vasut <marex@denx.de>
+ */
+#include <asm/arch/imx-regs.h>
+#include <asm/cache.h>
+#include <asm/gic.h>
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <common.h>
+#include <cpu_func.h>
+#include <debug_uart.h>
+#include <fsl_wdog.h>
+#include <linux/bitops.h>
+
+#define SNVS_LPCR			0x38
+#define SNVS_LPCR_TOP			BIT(6)
+#define SNVS_LPCR_DP_EN			BIT(5)
+#define SNVS_LPCR_SRTC_ENV		BIT(0)
+
+#define MPIDR_AFF0			GENMASK(7, 0)
+
+#define GPC_LPCR_A53_AD			0x4
+#define EN_Cn_WFI_PDN(cpu)		BIT(((((cpu) & 1) * 2) + (((cpu) & 2) * 8)))
+#define GPC_PGC_nCTRL(cpu)		(0x800 + ((cpu) * 0x40))
+#define PGC_PCR				BIT(0)
+#define GPC_CPU_PGC_SW_PUP_REQ		(IS_ENABLED(CONFIG_IMX8MP) ? 0xd0 : 0xf0)
+#define COREn_A53_SW_PUP_REQ(cpu)	BIT(cpu)
+
+#define SRC_A53RCR1			0x8
+#define A53_COREn_ENABLE(n)		BIT(n)
+#define SRC_GPR(n)			(0x74 + ((n) * 4))
+
+/*
+ * Helper code
+ */
+static u8 psci_state[CONFIG_ARMV8_PSCI_NR_CPUS] __secure_data = {
+	PSCI_AFFINITY_LEVEL_ON,
+	PSCI_AFFINITY_LEVEL_OFF,
+	PSCI_AFFINITY_LEVEL_OFF,
+	PSCI_AFFINITY_LEVEL_OFF
+};
+
+int psci_update_dt(void *fdt)
+{
+	return 0;
+}
+
+__secure static void psci_set_state(int cpu, u8 state)
+{
+	psci_state[cpu] = state;
+	dsb();
+	isb();
+}
+
+__secure static s32 psci_cpu_on_validate_mpidr(u64 mpidr, u32 *cpu)
+{
+	*cpu = mpidr & MPIDR_AFF0;
+
+	if (mpidr & ~MPIDR_AFF0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (*cpu >= CONFIG_ARMV8_PSCI_NR_CPUS)
+		return ARM_PSCI_RET_INVAL;
+
+	if (psci_state[*cpu] == PSCI_AFFINITY_LEVEL_ON)
+		return ARM_PSCI_RET_ALREADY_ON;
+
+	if (psci_state[*cpu] == PSCI_AFFINITY_LEVEL_ON_PENDING)
+		return ARM_PSCI_RET_ON_PENDING;
+
+	return ARM_PSCI_RET_SUCCESS;
+}
+
+__secure static void psci_cpu_on_write_entry_point(const u32 cpu, u64 entry_point)
+{
+	const u64 ep = CONFIG_SPL_TEXT_BASE;
+
+	/* Trampoline target */
+	writeq(entry_point, CPU_RELEASE_ADDR);
+	/* RVBAR address HI */
+	writel((u32)(ep >> 24) & 0xffff,
+	       (void *)SRC_BASE_ADDR + SRC_GPR(cpu * 2));
+	/* RVBAR address LO */
+	writel((u32)(ep >> 2) & 0x3fffff,
+	       (void *)SRC_BASE_ADDR + SRC_GPR(cpu * 2 + 1));
+}
+
+__secure static void psci_cpu_on_power_on(const u32 cpu)
+{
+	int i;
+
+	clrbits_le32((void *)GPC_BASE_ADDR + GPC_LPCR_A53_AD, EN_Cn_WFI_PDN(cpu));
+	clrbits_le32((void *)SRC_BASE_ADDR + SRC_A53RCR1, A53_COREn_ENABLE(cpu));
+	setbits_le32((void *)GPC_BASE_ADDR + GPC_PGC_nCTRL(cpu), PGC_PCR);
+	setbits_le32((void *)GPC_BASE_ADDR + GPC_CPU_PGC_SW_PUP_REQ, COREn_A53_SW_PUP_REQ(cpu));
+
+	/* If we fail here, the core gets power cycled, hang is OK */
+	while (readl(GPC_BASE_ADDR + GPC_CPU_PGC_SW_PUP_REQ) & COREn_A53_SW_PUP_REQ(cpu))
+			;
+
+	clrbits_le32((void *)GPC_BASE_ADDR + GPC_PGC_nCTRL(cpu), PGC_PCR);
+	setbits_le32((void *)SRC_BASE_ADDR + SRC_A53RCR1, A53_COREn_ENABLE(cpu));
+
+	/* Give the core a bit of time to boot and start executing code */
+	for (i = 0; i < 100000; i++)
+		asm volatile("nop");
+}
+
+__secure static void psci_cpu_on_power_off(const u32 cpu)
+{
+	setbits_le32((void *)GPC_BASE_ADDR + GPC_LPCR_A53_AD, EN_Cn_WFI_PDN(cpu));
+	setbits_le32((void *)GPC_BASE_ADDR + GPC_PGC_nCTRL(cpu), PGC_PCR);
+}
+
+/*
+ * Common PSCI code
+ */
+/* Return supported PSCI version */
+__secure u32 psci_version(void)
+{
+	return ARM_PSCI_VER_1_0;
+}
+
+/*
+ * 64bit PSCI code
+ */
+__secure s32 psci_cpu_on_64(u32 __always_unused function_id, u64 mpidr,
+			    u64 entry_point_address, u64 context_id)
+{
+	u32 cpu = 0;
+	int ret;
+
+	ret = psci_cpu_on_validate_mpidr(mpidr, &cpu);
+	if (ret != ARM_PSCI_RET_SUCCESS)
+		return ret;
+
+	psci_cpu_on_write_entry_point(cpu, entry_point_address);
+
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);
+
+	psci_cpu_on_power_on(cpu);
+
+	smp_kick_all_cpus();
+
+	return ARM_PSCI_RET_SUCCESS;
+}
+
+__secure s32 psci_affinity_info_64(u32 __always_unused function_id,
+				   u64 target_affinity, u32 lowest_affinity_level)
+{
+	u32 cpu = target_affinity & MPIDR_AFF0;
+
+	if (lowest_affinity_level > 0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (target_affinity & ~MPIDR_AFF0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (cpu >= CONFIG_ARMV8_PSCI_NR_CPUS)
+		return ARM_PSCI_RET_INVAL;
+
+	return psci_state[cpu];
+}
+
+__secure s32 psci_system_reset2_64(u32 __always_unused function_id,
+				   u32 reset_type, u64 cookie)
+{
+	psci_system_reset();
+	return 0;	/* Not reached */
+}
+
+/*
+ * 32bit PSCI code
+ */
+__secure s32 psci_affinity_info(u32 __always_unused function_id,
+				u32 target_affinity, u32 lowest_affinity_level)
+{
+	return psci_affinity_info_64(function_id, target_affinity, lowest_affinity_level);
+}
+
+__secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr,
+			 u32 entry_point_address, u32 context_id)
+{
+	return psci_cpu_on_64(function_id, mpidr, entry_point_address, context_id);
+}
+
+__secure s32 psci_cpu_off(void)
+{
+	u32 cpu = psci_get_cpu_id();
+
+	psci_cpu_on_power_off(cpu);
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
+
+	while (1)
+		wfi();
+}
+
+__secure u32 psci_migrate_info_type(void)
+{
+	/* Trusted OS is either not present or does not require migration */
+	return 2;
+}
+
+__secure void psci_system_reset(void)
+{
+	struct wdog_regs *wdog = (struct wdog_regs *)WDOG1_BASE_ADDR;
+	bool ext_reset = true;
+
+	u16 wcr = WCR_WDE;
+
+	if (ext_reset)
+		wcr |= WCR_SRS; /* do not assert internal reset */
+	else
+		wcr |= WCR_WDA; /* do not assert external reset */
+
+	/* Write 3 times to ensure it works, due to IMX6Q errata ERR004346 */
+	writew(wcr, &wdog->wcr);
+	writew(wcr, &wdog->wcr);
+	writew(wcr, &wdog->wcr);
+
+	while (1)
+		wfi();
+}
+
+__secure void psci_system_off(void)
+{
+	writel(SNVS_LPCR_TOP | SNVS_LPCR_DP_EN | SNVS_LPCR_SRTC_ENV,
+	       SNVS_BASE_ADDR + SNVS_LPCR);
+
+	while (1)
+		wfi();
+}
+
+/*
+ * PSCI jump table
+ */
+__secure s32 psci_features(u32 __always_unused function_id, u32 psci_fid)
+{
+	switch (psci_fid) {
+	case ARM_PSCI_0_2_FN_PSCI_VERSION:
+	case ARM_PSCI_0_2_FN_CPU_OFF:
+	case ARM_PSCI_0_2_FN_CPU_ON:
+	case ARM_PSCI_0_2_FN_AFFINITY_INFO:
+	case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
+	case ARM_PSCI_0_2_FN_SYSTEM_OFF:
+	case ARM_PSCI_0_2_FN_SYSTEM_RESET:
+	case ARM_PSCI_0_2_FN64_CPU_ON:
+	case ARM_PSCI_0_2_FN64_AFFINITY_INFO:
+
+	/* PSCI 1.0 interface */
+	case ARM_PSCI_1_0_FN_PSCI_FEATURES:
+
+	/* PSCI 1.1 interface */
+	case ARM_PSCI_1_1_FN64_SYSTEM_RESET2:
+		return 0x0;
+
+	/*
+	 * Not implemented:
+	 * ARM_PSCI_0_2_FN_CPU_SUSPEND
+	 * ARM_PSCI_1_0_FN_CPU_FREEZE
+	 * ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND
+	 * ARM_PSCI_1_0_FN_NODE_HW_STATE
+	 * ARM_PSCI_1_0_FN_SYSTEM_SUSPEND
+	 * ARM_PSCI_1_0_FN_SET_SUSPEND_MODE
+	 * ARM_PSCI_1_0_FN_STAT_RESIDENCY
+	 * ARM_PSCI_1_0_FN_STAT_COUNT
+	 * ARM_PSCI_0_2_FN64_CPU_SUSPEND
+	 * ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND
+	 * ARM_PSCI_1_0_FN64_NODE_HW_STATE
+	 * ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND
+	 * ARM_PSCI_1_0_FN64_STAT_RESIDENCY
+	 * ARM_PSCI_1_0_FN64_STAT_COUNT
+	 */
+
+	/* Not required, ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE returns 2 */
+	case ARM_PSCI_0_2_FN_MIGRATE:
+	case ARM_PSCI_0_2_FN64_MIGRATE:
+	/* Not required */
+	case ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU:
+	case ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU:
+	default:
+		return ARM_PSCI_RET_NI;
+	}
+}