[PATCHv1 1/3] mp: imx9: add cpu command support

[Zhiqiang Hou]

From: Tao Yang <Tao.Yang1 at nxp.com>

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

Signed-off-by: Tao Yang <Tao.Yang1 at nxp.com>
Signed-off-by: Hou Zhiqiang <Zhiqiang.Hou at nxp.com>
---
 arch/arm/mach-imx/imx9/Makefile |  4 +-
 arch/arm/mach-imx/imx9/mp.c     | 81 +++++++++++++++++++++++++++++++++
 2 files changed, 84 insertions(+), 1 deletion(-)
 create mode 100644 arch/arm/mach-imx/imx9/mp.c

diff --git a/arch/arm/mach-imx/imx9/Makefile b/arch/arm/mach-imx/imx9/Makefile
index e1b09ab534..975b6831cc 100644
--- a/arch/arm/mach-imx/imx9/Makefile
+++ b/arch/arm/mach-imx/imx9/Makefile
@@ -1,6 +1,6 @@
 # SPDX-License-Identifier: GPL-2.0+
 #
-# Copyright 2022 NXP
+# Copyright 2022, 2024 NXP
 
 obj-y += lowlevel_init.o
 obj-y += soc.o clock.o clock_root.o trdc.o
@@ -8,3 +8,5 @@ obj-y += soc.o clock.o clock_root.o trdc.o
 #ifndef CONFIG_SPL_BUILD
 obj-y += imx_bootaux.o
 #endif
+
+obj-$(CONFIG_MP) += mp.o
diff --git a/arch/arm/mach-imx/imx9/mp.c b/arch/arm/mach-imx/imx9/mp.c
new file mode 100644
index 0000000000..9ddfc7784b
--- /dev/null
+++ b/arch/arm/mach-imx/imx9/mp.c
@@ -0,0 +1,81 @@
+// 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>
+
+#define MPIDR_MT_BIT		BIT(24)
+#define MPIDR_AFF1_SHFT		(8)
+
+static u64 logical_id_to_hwid(unsigned int core)
+{
+	u64 hwid = core;
+
+	if (read_mpidr() & MPIDR_MT_BIT)
+		hwid = core << MPIDR_AFF1_SHFT;
+
+	return hwid;
+}
+
+int is_core_valid(unsigned int core)
+{
+	if (core < MAX_CPUS)
+		return 1;
+
+	return 0;
+}
+
+int cpu_reset(u32 nr)
+{
+	printf("Feature is not implemented.\n");
+
+	return 0;
+}
+
+int cpu_disable(u32 nr)
+{
+	printf("Feature is not implemented.\n");
+
+	return 0;
+}
+
+int cpu_status(u32 nr)
+{
+	printf("Feature is not implemented.\n");
+
+	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] = logical_id_to_hwid(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;
+}
-- 
2.43.0