diff --git a/arch/arm/cpu/armv7/qca/common/Makefile b/arch/arm/cpu/armv7/qca/common/Makefile index 253ef6bb2c..48903e4832 100644 --- a/arch/arm/cpu/armv7/qca/common/Makefile +++ b/arch/arm/cpu/armv7/qca/common/Makefile @@ -3,3 +3,4 @@ ccflags-y += -I$(srctree)/board/qca/arm/common/ obj-y := smem.o obj-y += timer.o obj-y += scm.o +obj-$(CONFIG_SMP_PSCI_CMD) += smp.o diff --git a/arch/arm/cpu/armv7/qca/common/smp.S b/arch/arm/cpu/armv7/qca/common/smp.S new file mode 100644 index 0000000000..658b217a75 --- /dev/null +++ b/arch/arm/cpu/armv7/qca/common/smp.S @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include + +ENTRY(secondary_cpu_init) + /* For us r0 has the arg structure pointer + struct cpu_entry_arg { + void *stack_ptr; + void *gd_ptr; + void *arg_ptr; + int cpu_up; + int cmd_complete; + int cmd_result; + }; + */ + ldr sp, [r0] /* stack_ptr */ + bic sp, sp, #7 /* 8-byte alignment required for stack */ + stmfd sp!, {r0} + + /* + * disable interrupts (FIQ and IRQ), also set the cpu to SVC32 mode + */ + mrs r0, cpsr + bic r0, r0, #0x1f @ clear all mode bits + orr r0, r0, #0x13 @ set SVC mode + orr r0, r0, #0xc0 @ disable FIQ and IRQ + msr cpsr,r0 + + /* Enable barrier instructions */ + mrc p15, 0, r0, c1, c0, 0 @Read SCTLR to r0 + orr r0, r0, #0x20 @set the cp15 barrier enable bit + mcr p15, 0, r0, c1, c0, 0 @write back to SCTLR + + /* Vector address setup - going to be same as core0 for secondary cpus */ + mrc p15, 0, r0, c1, c0, 0 @ Read CP15 SCTLR Register + bic r0, #CR_V @ V = 0 + mcr p15, 0, r0, c1, c0, 0 @ Write CP15 SCTLR Register + + /* Set vector address in CP15 VBAR register */ + ldr r0, =_start + mcr p15, 0, r0, c12, c0, 0 @Set VBAR + + bl cpu_init_cp15 + + ldmfd sp!, {r0} + + /* gd address is always in r9, but if u-boot changes Tomorrow, + * we have to change here. Note :- --> ARM: use r9 for gd + */ + ldr r9, [r0, #0x4] + /* store success in cpu_up */ + mov r3, #1 + str r3, [r0, #0xc] + add r1, r0, #0x10 + add r2, r0, #0x14 + ldr r0, [r0, #0x8] + + bl secondary_core_entry +self_loop: + b self_loop +ENDPROC(secondary_cpu_init) + + diff --git a/arch/arm/lib/cache-cp15.c b/arch/arm/lib/cache-cp15.c index e8e26db869..2a1153033d 100644 --- a/arch/arm/lib/cache-cp15.c +++ b/arch/arm/lib/cache-cp15.c @@ -22,7 +22,7 @@ __weak void arm_init_domains(void) { } -static void cp_delay (void) +void cp_delay (void) { volatile int i; @@ -84,7 +84,7 @@ __weak void dram_bank_mmu_setup(int bank) } /* to activate the MMU we need to set up virtual memory: use 1M areas */ -static inline void mmu_setup(void) +void mmu_setup(void) { int i; u32 reg; @@ -132,7 +132,7 @@ __weak void dram_bank_mmu_setup(int bank) } /* to activate the MMU we need to set up virtual memory: use 1M areas */ -static inline void mmu_setup(void) +void mmu_setup(void) { int i; u32 reg; @@ -176,7 +176,7 @@ static inline void mmu_setup(void) } #endif -static int mmu_enabled(void) +int mmu_enabled(void) { return get_cr() & CR_M; } diff --git a/board/qca/arm/common/Makefile b/board/qca/arm/common/Makefile index faef6d943c..0e182e367e 100644 --- a/board/qca/arm/common/Makefile +++ b/board/qca/arm/common/Makefile @@ -1,6 +1,7 @@ obj-y := cmd_bootqca.o obj-y += cmd_blowsecfuse.o obj-y += cmd_exectzt.o +obj-$(CONFIG_SMP_PSCI_CMD) += cmd_runmulticore.o obj-y += fdt_info.o obj-y += board_init.o ifndef CONFIG_ENV_IS_NOWHERE diff --git a/board/qca/arm/common/cmd_runmulticore.c b/board/qca/arm/common/cmd_runmulticore.c new file mode 100644 index 0000000000..d6ec16c92d --- /dev/null +++ b/board/qca/arm/common/cmd_runmulticore.c @@ -0,0 +1,212 @@ +/* + * Copyright (c) 2017, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include +#include +#include +#include +#include + +DECLARE_GLOBAL_DATA_PTR; + +#define SECONDARY_CORE_STACKSZ (8 * 1024) +#define CPU_POWER_DOWN (1 << 16) + +#define ARM_PSCI_TZ_FN_BASE 0x84000000 +#define ARM_PSCI_TZ_FN(n) (ARM_PSCI_TZ_FN_BASE + (n)) + +#define ARM_PSCI_TZ_FN_CPU_OFF ARM_PSCI_TZ_FN(2) +#define ARM_PSCI_TZ_FN_CPU_ON ARM_PSCI_TZ_FN(3) +#define ARM_PSCI_TZ_FN_AFFINITY_INFO ARM_PSCI_TZ_FN(4) + +struct cpu_entry_arg { + void *stack_ptr; + volatile void *gd_ptr; + void *arg_ptr; + int cpu_up; + int cmd_complete; + int cmd_result; + void *stack_top_ptr; +}; + +extern unsigned int __invoke_psci_fn_smc(unsigned int, unsigned int, + unsigned int, unsigned int); +extern void secondary_cpu_init(void); + +static struct cpu_entry_arg core[NR_CPUS - 1]; + +void psci_cpu_off(unsigned int state) +{ + __invoke_psci_fn_smc(ARM_PSCI_TZ_FN_CPU_OFF, state, 0, 0); +} + +asmlinkage void secondary_core_entry(char *argv, int *cmd_complete, + int *cmd_result) +{ + unsigned int state = 0; + + if (!mmu_enabled()) { + mmu_setup(); + cp_delay(); + } + + /* Update here as ncessary - secondary entry point */ + *cmd_result = cli_simple_run_command(argv, 0); + *cmd_complete = 1; + + state = CPU_POWER_DOWN; + psci_cpu_off(state); +} + +int psci_cpu_on(unsigned int cpuid, unsigned int entry, unsigned int arg) +{ + int err; + + err = __invoke_psci_fn_smc(ARM_PSCI_TZ_FN_CPU_ON, cpuid, entry, arg); + if (err) { + printf("Enabling CPU%d via psci failed!\n", cpuid); + return -1; + } + + printf("Enabled CPU%d via psci successfully!\n", cpuid); + return 0; +} + +int is_psci_cpu_off(unsigned int cpuid) +{ + int err; + + err = __invoke_psci_fn_smc(ARM_PSCI_TZ_FN_AFFINITY_INFO, cpuid, 0, 0); + + return err; +} + +static int do_runmulticore(cmd_tbl_t *cmdtp, + int flag, int argc, char *const argv[]) +{ + int j; + int i; + int ret; + int delay = 0; + int core_status = 0; + int core_on_status = 0; + char *ptr = NULL; + + if ((argc <= 1) || (argc > 4)) + return CMD_RET_USAGE; + + /* Setting up stack for secondary cores */ + memset(core, 0, sizeof(core)); + + for (i = 1; i < argc; i++) { + ptr = malloc(SECONDARY_CORE_STACKSZ); + if (NULL == ptr) { + j = i - 1; + while (j >= 0) { + free(core[i - 1].stack_ptr); + j--; + } + printf("Memory allocation failure\n"); + return CMD_RET_FAILURE; + } + /* 0xf0 is the padding length */ + core[i - 1].stack_top_ptr = ptr; + core[i - 1].stack_ptr = (ptr + (SECONDARY_CORE_STACKSZ) - 0xf0); + + core[i - 1].gd_ptr = gd; + core[i - 1].arg_ptr = argv[i]; + } + + if (!mmu_enabled()) { + mmu_setup(); + cp_delay(); + } + + /* Bringing up the secondary cores */ + for (i = 1; i < argc; i++) { + printf("Scheduling Core %d\n", i); + delay = 0; + ret = psci_cpu_on(i, (unsigned int)secondary_cpu_init, + (unsigned int)&(core[i - 1])); + if (ret) { + panic("PSCI failed for core %d\n",i); + } + + while ((delay < 5) && (!(core[i - 1].cpu_up))) { + mdelay(1000); + delay++; + } + if (!(core[i - 1].cpu_up)) { + panic("Can't bringup core %d\n",i); + } + + core_status |= (BIT(i - 1)); + core_on_status |= (BIT(i - 1)); + } + + /* Waiting for secondary cores to complete the task */ + while (core_status) { + for (i = 1; i < argc; i++) { + if ((core_status & (BIT(i - 1))) && + (core[i - 1].cmd_complete)) { + printf("Command on core %d is %s\n", i, + core[i - 1].cmd_complete ? + ((core[i - 1].cmd_result == -1) ? + "FAIL" : "PASS"): + "INCOMPLETE"); + core_status &= (~BIT((i - 1))); + } + } + if (ctrlc()) { + run_command("reset", 0); + } + } + + for (i = 1; (core_status && (i < argc)); i++) { + if (core_status & (BIT(i - 1))) { + printf("Command on core %d is %s\n", i, + core[i - 1].cmd_complete ? + ((core[i - 1].cmd_result == -1) ? + "FAIL" : "PASS"): "INCOMPLETE"); + } + } + + /* Waiting for cores to powerdown */ + delay = 0; + while (core_on_status) { + for (i = 1; i < argc; i++) { + if (core_on_status & (BIT(i - 1))) { + if (is_psci_cpu_off(i) == 1) { + printf("core %d powered off\n", i); + core_on_status &= (~BIT((i - 1))); + } + } + } + mdelay(1000); + delay++; + if (delay > 5) + panic("Some cores can't be powered off\n"); + } + + /* Free up all the stack */ + for (i = 1; i < argc; i++) { + free(core[i - 1].stack_top_ptr); + } + + return CMD_RET_SUCCESS; +} + +U_BOOT_CMD(runmulticore, 4, 0, do_runmulticore, + "Enable and schedule secondary cores", + "runmulticore <\"command to core1\"> [core2 core3 ...]"); diff --git a/include/common.h b/include/common.h index b6865aad09..a786ff56c5 100644 --- a/include/common.h +++ b/include/common.h @@ -231,6 +231,11 @@ int run_command_repeatable(const char *cmd, int flag); */ int run_command_list(const char *cmd, int len, int flag); +extern void mmu_setup(void); +extern int mmu_enabled(void); +extern void cp_delay (void); +void secondary_core_entry(char *argv, int *cmd_complete, int *cmd_result); + /* arch/$(ARCH)/lib/board.c */ void board_init_f(ulong); void board_init_r(gd_t *, ulong) __attribute__ ((noreturn)); diff --git a/include/configs/ipq807x.h b/include/configs/ipq807x.h index d922bce1f6..5f2c4087a6 100644 --- a/include/configs/ipq807x.h +++ b/include/configs/ipq807x.h @@ -106,6 +106,12 @@ #define CONFIG_IPQ_NO_MACS 6 #define CONFIG_ENV_IS_IN_SPI_FLASH 1 #define CONFIG_ENV_SECT_SIZE (64 * 1024) + +#define CONFIG_SMP_PSCI_CMD + +#ifdef CONFIG_SMP_PSCI_CMD +#define NR_CPUS 4 +#endif /* * IPQ_TFTP_MIN_ADDR: Starting address of Linux HLOS region. * CONFIG_TZ_END_ADDR: Ending address of Trust Zone and starting