From: Tao Yang <tao.ya...@nxp.com> Implement the cpu command to kick cpu core to run barematel or RTOS applications.
Signed-off-by: Tao Yang <tao.ya...@nxp.com> Signed-off-by: Hou Zhiqiang <zhiqiang....@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(®s); + if (regs.regs[0]) + return -1; + + printf("kicked cpu core #%d to address %llx\n", nr, boot_addr); + + return 0; +} -- 2.43.0