ipq806x: Added multicore support

-Device IO commands are not supported
-CPU down is temporary supported by WFE instruction
-Need reboot to execute bootipq command

Change-Id: Ic7f1dece29e29f75b984018bcf1fc5f724282567
Signed-off-by: Santan Kumar <santank@codeaurora.org>

arch/arm/cpu/armv7/qca/common/Makefile

@@ -3,4 +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
+obj-$(CONFIG_SMP_CMD_SUPPORT) += smp.o

arch/arm/cpu/armv7/qca/common/smp.S

@@ -13,6 +13,28 @@
 #include <asm/system.h>
 #include <linux/linkage.h>
 
+ENTRY(ak_secondary_cpu_init)
+        mrc     p15, 0, r0, c0, c0, 5   /* read MPIDR */
+        and     r1, r0, #0xf           /* return CPU ID in cluster */
+	sub	r0, r1, #1
+	/* Now r1 has the cpu number,
+	 *  this will serve as index to "struct cpu_entry_arg"
+         * struct cpu_entry_arg {
+	 * void *stack_ptr;       +0 1c   38 54
+	 * volatile void *gd_ptr; +4 20   3c
+	 * void *arg_ptr;         +8 24   40
+	 * int  cpu_up;           +0xc 28 44
+	 * int cmd_complete;      +0x10 2c 48
+	 * int cmd_result;        +0x14 30 4c
+	 * void *stack_top_ptr;   +0x18 34 50
+	 *                         +0x1c (next entry in core[])
+	 * };
+         */
+	ldr r4, =globl_core_array
+	mov r3, #0x1c
+	mul r2, r0, r3
+	ldr r0, [r4]
+	add r0, r0, r2
 ENTRY(secondary_cpu_init)
 	/* For us r0 has the arg structure pointer
 		struct cpu_entry_arg {
@@ -70,5 +92,21 @@ ENTRY(secondary_cpu_init)
 self_loop:
 	b self_loop
 ENDPROC(secondary_cpu_init)
+ENDPROC(ak_secondary_cpu_init)
 
+ENTRY(send_event)
+	dsb
+	sev
+	mov pc, lr
+ENDPROC(send_event)
 
+ENTRY(wait_event)
+	dsb
+	wfe
+	dsb
+	bx r0
+ENDPROC(wait_event)
+
+.globl globl_core_array
+globl_core_array:
+        .word 0

board/qca/arm/common/Makefile

@@ -1,7 +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-$(CONFIG_SMP_CMD_SUPPORT) += 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

@@ -23,6 +23,8 @@ DECLARE_GLOBAL_DATA_PTR;
 #define SECONDARY_CORE_STACKSZ (8 * 1024)
 #define CPU_POWER_DOWN (1 << 16)
 
+extern void *globl_core_array;
+
 struct cpu_entry_arg {
 	void *stack_ptr;
 	volatile void *gd_ptr;
@@ -34,8 +36,9 @@ struct cpu_entry_arg {
 };
 
 extern void secondary_cpu_init(void);
+extern void bring_secondary_core_down(int);
 
-static struct cpu_entry_arg core[NR_CPUS - 1];
+struct cpu_entry_arg core[NR_CPUS - 1];
 
 asmlinkage void secondary_core_entry(char *argv, int *cmd_complete,
 					int *cmd_result)
@@ -72,6 +75,7 @@ int do_runmulticore(cmd_tbl_t *cmdtp,
 	/* Setting up stack for secondary cores */
 	memset(core, 0, sizeof(core));
 
+	globl_core_array = core;
 	for (i = 1; i < argc; i++) {
 		ptr = malloc(SECONDARY_CORE_STACKSZ);
 		if (NULL == ptr) {
@@ -87,6 +91,9 @@ int do_runmulticore(cmd_tbl_t *cmdtp,
 		core[i - 1].stack_top_ptr = ptr;
 		core[i - 1].stack_ptr = (ptr + (SECONDARY_CORE_STACKSZ) - 0xf0);
 
+		core[i - 1].cpu_up = 0;
+		core[i - 1].cmd_complete = 0;
+		core[i - 1].cmd_result = -1;
 		core[i - 1].gd_ptr = gd;
 		core[i - 1].arg_ptr = argv[i];
 	}
@@ -105,7 +112,6 @@ int do_runmulticore(cmd_tbl_t *cmdtp,
 		if (ret) {
 			panic("Some problem to getting core %d up\n", i);
 		}
-
 		while ((delay < 5) && (!(core[i - 1].cpu_up))) {
 			mdelay(1000);
 			delay++;

board/qca/arm/ipq806x/ipq806x.c

@@ -30,6 +30,8 @@
 #include "ipq806x.h"
 #include "qca_common.h"
 #include <asm/arch-qca-common/scm.h>
+#include <asm/arch-qca-common/iomap.h>
+#include <asm/io.h>
 
 #define DLOAD_MAGIC_COOKIE_1 0xE47B337D
 #define DLOAD_MAGIC_COOKIE_2 0x0501CAB0
@@ -920,7 +922,8 @@ int ipq_get_tz_version(char *version_name, int buf_size)
 	return 0;
 }
 
-void forever(void) { while (1); }
+extern void ak_secondary_cpu_init(void);
+extern void send_event(void);
 /*
  * Set the cold/warm boot address for one of the CPU cores.
  */
@@ -932,7 +935,7 @@ int scm_set_boot_addr(void)
 		unsigned long addr;
 	} cmd;
 
-	cmd.addr = (unsigned long)forever;
+	cmd.addr = (unsigned long)ak_secondary_cpu_init;
 	cmd.flags = SCM_FLAG_COLDBOOT_CPU1;
 
 	ret = scm_call(SCM_SVC_BOOT, SCM_BOOT_ADDR,
@@ -960,6 +963,8 @@ void clear_l2cache_err(void)
 #endif
 }
 
+static int dcache_old_status;
+
 void enable_caches(void)
 {
 	icache_enable();
@@ -979,3 +984,63 @@ int set_uuid_bootargs(char *boot_args, char *part_name, int buflen, bool gpt_fla
 {
 	return 0;
 }
+int is_secondary_core_off(unsigned int cpuid)
+{
+	if (dcache_old_status)
+		dcache_enable();
+	return 1;
+}
+
+static int secondary_core_already_reset;
+extern void wait_event(void (*)(void));
+
+void bring_secondary_core_down(unsigned int state)
+{
+	wait_event(ak_secondary_cpu_init);
+}
+
+static int krait_release_secondary(void)
+{
+	dcache_disable();
+	writel(0xa4, CPU1_APCS_SAW2_VCTL);
+	barrier();
+	udelay(512);
+
+	writel(0x109, CPU1_APCS_CPU_PWR_CTL);
+	writel(0x101, CPU1_APCS_CPU_PWR_CTL);
+	barrier();
+	udelay(1);
+
+	writel(0x121, CPU1_APCS_CPU_PWR_CTL);
+	barrier();
+	udelay(2);
+
+	writel(0x120, CPU1_APCS_CPU_PWR_CTL);
+	barrier();
+	udelay(2);
+
+	writel(0x100, CPU1_APCS_CPU_PWR_CTL);
+	barrier();
+	udelay(100);
+
+	writel(0x180, CPU1_APCS_CPU_PWR_CTL);
+	barrier();
+
+	return 0;
+}
+int bring_sec_core_up(unsigned int cpuid, unsigned int entry, unsigned int arg)
+{
+	int err = 0;
+	dcache_old_status = dcache_status();
+	if (!secondary_core_already_reset) {
+		secondary_core_already_reset = 1;
+		if (scm_set_boot_addr() == 0) {
+			/* Pull Core-1 out of reset, iff scm call succeeds */
+			krait_release_secondary();
+		}
+	} else {
+		dcache_disable();
+		send_event();
+	}
+	return 0;
+}

common/command.c

@@ -510,7 +510,7 @@ enum command_ret_t cmd_process(int flag, int argc, char * const argv[],
 		return 1;
 	}
 
-#if defined(CONFIG_SMP_PSCI_CMD)
+#if defined(CONFIG_SMP_CMD_SUPPORT)
 	if ((flag & CMD_FLAG_SEC_CORE) &&
 	    (cmdtp->cmd == do_runmulticore)) {
 		printf("Restricted command '%s' for secondary core\n", argv[0]);

include/command.h

@@ -111,7 +111,7 @@ extern int common_diskboot(cmd_tbl_t *cmdtp, const char *intf, int argc,
 
 extern int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[]);
 
-#if defined(CONFIG_SMP_PSCI_CMD)
+#if defined(CONFIG_SMP_CMD_SUPPORT)
 extern int do_runmulticore(cmd_tbl_t *cmdtp,
 			   int flag, int argc, char *const argv[]);
 #endif

include/configs/ipq806x.h

@@ -34,6 +34,10 @@
 #endif /* !DO_DEPS_ONLY */
 
 #define CONFIG_IPQ806X
+#define CONFIG_SMP_CMD_SUPPORT
+#ifdef CONFIG_SMP_CMD_SUPPORT
+#define NR_CPUS				2
+#endif
 
 #define CONFIG_SYS_NO_FLASH
 #define CONFIG_SYS_CACHELINE_SIZE   64

include/configs/ipq807x.h

@@ -107,9 +107,9 @@
 #define CONFIG_ENV_IS_IN_SPI_FLASH	1
 #define CONFIG_ENV_SECT_SIZE        	(64 * 1024)
 
-#define CONFIG_SMP_PSCI_CMD
+#define CONFIG_SMP_CMD_SUPPORT
 
-#ifdef CONFIG_SMP_PSCI_CMD
+#ifdef CONFIG_SMP_CMD_SUPPORT
 #define NR_CPUS				4
 #endif
 /*