[PATCHv1 1/2] mp: imx8m: add cpu command support

[Zhiqiang Hou]

From: Hou Zhiqiang <zhiqiang....@nxp.com>

Implement the cpu command to kick cpu core to run barematel or RTOS
applications.

Signed-off-by: Hou Zhiqiang <zhiqiang....@nxp.com>
Signed-off-by: Yi Zhao <yi.z...@nxp.com>
Signed-off-by: Jiafei Pan <jiafei....@nxp.com>
---
 arch/arm/mach-imx/imx8m/Makefile |   3 +-
 arch/arm/mach-imx/imx8m/mp.c     | 100 +++++++++++++++++++++++++++++++
 2 files changed, 102 insertions(+), 1 deletion(-)
 create mode 100644 arch/arm/mach-imx/imx8m/mp.c

diff --git a/arch/arm/mach-imx/imx8m/Makefile b/arch/arm/mach-imx/imx8m/Makefile
index abd5ddc177..9e4d919159 100644
--- a/arch/arm/mach-imx/imx8m/Makefile
+++ b/arch/arm/mach-imx/imx8m/Makefile
@@ -1,9 +1,10 @@
 # SPDX-License-Identifier: GPL-2.0+
 #
-# Copyright 2017 NXP
+# Copyright 2017, 2024 NXP
 
 obj-y += lowlevel_init.o
 obj-y += clock_slice.o soc.o
+obj-$(CONFIG_MP) += mp.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/mp.c b/arch/arm/mach-imx/imx8m/mp.c
new file mode 100644
index 0000000000..33b9ccfce2
--- /dev/null
+++ b/arch/arm/mach-imx/imx8m/mp.c
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2024 NXP
+ */
+
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/ptrace.h>
+#include <asm/system.h>
+#include <config.h>
+#include <cpu_func.h>
+#include <linux/psci.h>
+#include <vsprintf.h>
+
+#ifndef MAX_CPUS
+#define MAX_CPUS 4
+#endif
+
+/* A53 Reset Control Register (SRC_A53RCR0) */
+#define SRC_A53RCR0_CORE_1_RESET_MASK          BIT(5)
+#define SRC_A53RCR0_CORE_2_RESET_MASK          BIT(6)
+#define SRC_A53RCR0_CORE_3_RESET_MASK          BIT(7)
+
+/* A53 Reset Control Register (SRC_A53RCR1) */
+#define SRC_A53RCR1_CORE_1_ENABLE_MASK         BIT(1)
+#define SRC_A53RCR1_CORE_2_ENABLE_MASK         BIT(2)
+#define SRC_A53RCR1_CORE_3_ENABLE_MASK         BIT(3)
+
+static struct src *src = (struct src *)SRC_BASE_ADDR;
+
+static u32 cpu_reset_mask[MAX_CPUS] = {
+       0, /* We don't really want to modify the cpu0 */
+       SRC_A53RCR0_CORE_1_RESET_MASK,
+       SRC_A53RCR0_CORE_2_RESET_MASK,
+       SRC_A53RCR0_CORE_3_RESET_MASK
+};
+
+static u32 cpu_ctrl_mask[MAX_CPUS] = {
+       0, /* We don't really want to modify the cpu0 */
+       SRC_A53RCR1_CORE_1_ENABLE_MASK,
+       SRC_A53RCR1_CORE_2_ENABLE_MASK,
+       SRC_A53RCR1_CORE_3_ENABLE_MASK
+};
+
+int is_core_valid(unsigned int core)
+{
+       if (core < MAX_CPUS)
+               return 1;
+
+       return 0;
+}
+
+int cpu_status(u32 nr)
+{
+       printf("core %d => %d\n", nr, !!(src->a53rcr1 & cpu_ctrl_mask[nr]));
+
+       return 0;
+}
+
+int cpu_reset(u32 nr)
+{
+       /* Software reset of the CPU N */
+       src->a53rcr |= cpu_reset_mask[nr];
+
+       return 0;
+}
+
+int cpu_release(u32 nr, int argc, char *const argv[])
+{
+       struct pt_regs regs;
+       u64 boot_addr;
+
+       if (nr >= MAX_CPUS) {
+               printf("Invalid CPU ID %d\n", nr);
+               return -1;
+       }
+
+       boot_addr = simple_strtoull(argv[0], NULL, 16);
+
+       regs.regs[0] = PSCI_0_2_FN64_CPU_ON;
+       regs.regs[1] = nr;
+       regs.regs[2] = boot_addr;
+       regs.regs[3] = 0;
+
+       smc_call(&regs);
+       if (regs.regs[0])
+               return -1;
+
+       printf("kicked cpu core #%d to address %llx\n", nr, boot_addr);
+
+       return 0;
+}
+
+int cpu_disable(u32 nr)
+{
+       /* Disable the CPU N */
+       src->a53rcr1 &= ~cpu_ctrl_mask[nr];
+
+       return 0;
+}
-- 
2.43.0