qca: psci: Added support to enable secondary cores

Change-Id: I1211577b7bbaf3fefba3613e46d25c7724e4d555
Signed-off-by: Gokul Sriram Palanisamy <gokulsri@codeaurora.org>

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

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 <asm/system.h>
+#include <linux/linkage.h>
+
+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)
+
+

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;
 }

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

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 <common.h>
+#include <malloc.h>
+#include <asm/psci.h>
+#include <cli.h>
+#include <console.h>
+#include <linux/linkage.h>
+
+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 ...]");

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));

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