ARM: uniphier: add PSCI support for UniPhier ARMv7 SoCs

Currently, only the CPU_ON function is supported. Signed-off-by: Masahiro Yamada <yamada.masahiro@socionext.com>

ARM: uniphier: add PSCI support for UniPhier ARMv7 SoCs
Currently, only the CPU_ON function is supported. Signed-off-by: Masahiro Yamada <yamada.masahiro@socionext.com>
Masahiro Yamada
1 parent ee9bc77f3a
Showing 8 changed files with 275 additions and 3 deletions Side-by-side Diff
arch/arm/mach-uniphier/Kconfig
arch/arm/mach-uniphier/arm32/Makefile
arch/arm/mach-uniphier/arm32/arm-mpcore.h
arch/arm/mach-uniphier/arm32/psci.c
arch/arm/mach-uniphier/arm32/psci_smp.S
arch/arm/mach-uniphier/debug.h
arch/arm/mach-uniphier/sbc/sbc-regs.h
include/configs/uniphier.h
@@ -6,6 +6,8 @@
 config ARCH_UNIPHIER_32BIT
 	bool
 	select CPU_V7
+	select CPU_V7_HAS_NONSEC
+	select ARMV7_NONSEC
  
 config ARCH_UNIPHIER_64BIT
 	bool
@@ -8,6 +8,7 @@
 else
 obj-y += late_lowlevel_init.o
 obj-y += cache-uniphier.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o psci_smp.o
 endif
  
 obj-y += timer.o
@@ -12,6 +12,9 @@
  
 /* SCU Control Register */
 #define SCU_CTRL		0x00
+#define SCU_ENABLE		(1 << 0)
+#define SCU_STANDBY_ENABLE	(1 << 5)
+
 /* SCU Configuration Register */
 #define SCU_CONF		0x04
 /* SCU CPU Power Status Register */
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <linux/bitops.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/psci.h>
+#include <linux/sizes.h>
+#include <asm/processor.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+
+#include "../debug.h"
+#include "../soc-info.h"
+#include "arm-mpcore.h"
+#include "cache-uniphier.h"
+
+#define UNIPHIER_SMPCTRL_ROM_RSV2	0x59801208
+
+void uniphier_smp_trampoline(void);
+void uniphier_smp_trampoline_end(void);
+u32 uniphier_smp_booted[CONFIG_ARMV7_PSCI_NR_CPUS];
+
+static int uniphier_get_nr_cpus(void)
+{
+	switch (uniphier_get_soc_type()) {
+	case SOC_UNIPHIER_SLD3:
+	case SOC_UNIPHIER_PRO4:
+	case SOC_UNIPHIER_PRO5:
+		return 2;
+	case SOC_UNIPHIER_PXS2:
+	case SOC_UNIPHIER_LD6B:
+		return 4;
+	default:
+		return 1;
+	}
+}
+
+static void uniphier_smp_kick_all_cpus(void)
+{
+	const u32 target_ways = BIT(0);
+	size_t trmp_size;
+	u32 trmp_src = (unsigned long)uniphier_smp_trampoline;
+	u32 trmp_src_end = (unsigned long)uniphier_smp_trampoline_end;
+	u32 trmp_dest, trmp_dest_end;
+	int nr_cpus, i;
+	int timeout = 1000;
+
+	nr_cpus = uniphier_get_nr_cpus();
+	if (nr_cpus == 1)
+		return;
+
+	for (i = 0; i < nr_cpus; i++)	/* lock ways for all CPUs */
+		uniphier_cache_set_active_ways(i, 0);
+	uniphier_cache_inv_way(target_ways);
+	uniphier_cache_enable();
+
+	/* copy trampoline code */
+	uniphier_cache_prefetch_range(trmp_src, trmp_src_end, target_ways);
+
+	trmp_size = trmp_src_end - trmp_src;
+
+	trmp_dest = trmp_src & (SZ_64K - 1);
+	trmp_dest += SZ_1M - SZ_64K * 2;
+
+	trmp_dest_end = trmp_dest + trmp_size;
+
+	uniphier_cache_touch_range(trmp_dest, trmp_dest_end, target_ways);
+
+	writel(trmp_dest, UNIPHIER_SMPCTRL_ROM_RSV2);
+
+	asm("dsb	ishst\n" /* Ensure the write to ROM_RSV2 is visible */
+	    "sev"); /* Bring up all secondary CPUs from Boot ROM into U-Boot */
+
+	while (--timeout) {
+		int all_booted = 1;
+
+		for (i = 1; i < nr_cpus; i++)
+			if (!uniphier_smp_booted[i])
+				all_booted = 0;
+		if (all_booted)
+			break;
+		udelay(1);
+
+		/* barrier here because uniphier_smp_booted[] may be updated */
+		cpu_relax();
+	}
+
+	if (!timeout)
+		printf("warning: some of secondary CPUs may not boot\n");
+
+	uniphier_cache_disable();
+}
+
+void psci_board_init(void)
+{
+	unsigned long scu_base;
+	u32 scu_ctrl, tmp;
+
+	asm("mrc p15, 4, %0, c15, c0, 0" : "=r" (scu_base));
+
+	scu_ctrl = readl(scu_base + 0x30);
+	if (!(scu_ctrl & 1))
+		writel(scu_ctrl | 0x1, scu_base + 0x30);
+
+	scu_ctrl = readl(scu_base + SCU_CTRL);
+	scu_ctrl |= SCU_ENABLE | SCU_STANDBY_ENABLE;
+	writel(scu_ctrl, scu_base + SCU_CTRL);
+
+	tmp = readl(scu_base + SCU_SNSAC);
+	tmp |= 0xfff;
+	writel(tmp, scu_base + SCU_SNSAC);
+
+	uniphier_smp_kick_all_cpus();
+}
+
+void psci_arch_init(void)
+{
+	u32 actlr;
+
+	asm("mrc p15, 0, %0, c1, c0, 1" : "=r" (actlr));
+	actlr |= 0x41;		/* set SMP and FW bits */
+	asm("mcr p15, 0, %0, c1, c0, 1" : : "r" (actlr));
+}
+
+u32 uniphier_psci_holding_pen_release __secure_data = 0xffffffff;
+
+int __secure psci_cpu_on(u32 function_id, u32 cpuid, u32 entry_point)
+{
+	u32 cpu = cpuid & 0xff;
+
+	debug_puts("[U-Boot PSCI]  psci_cpu_on: cpuid=");
+	debug_puth(cpuid);
+	debug_puts(", entry_point=");
+	debug_puth(entry_point);
+	debug_puts("\n");
+
+	psci_save_target_pc(cpu, entry_point);
+
+	/* We assume D-cache is off, so do not call flush_dcache() here */
+	uniphier_psci_holding_pen_release = cpu;
+
+	/* Send an event to wake up the secondary CPU. */
+	asm("dsb	ishst\n"
+	    "sev");
+
+	return PSCI_RET_SUCCESS;
+}
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <linux/linkage.h>
+#include <asm/system.h>
+
+	.section ._secure.text, "ax"
+
+ENTRY(uniphier_smp_trampoline)
+	ldr	r0, 0f
+	mrc	p15, 0, r1, c1, c0, 0	@ SCTLR (System Control Register)
+	orr	r1, r1, #CR_I		@ Enable ICache
+	bic	r1, r1, #(CR_C | CR_M)	@ Disable MMU and Dcache
+	mcr	p15, 0, r1, c1, c0, 0
+
+	bx	r0
+0:	.word	uniphier_secondary_startup
+	.globl	uniphier_smp_trampoline_end
+uniphier_smp_trampoline_end:
+ENDPROC(uniphier_smp_trampoline)
+
+LENTRY(uniphier_secondary_startup)
+	mrc	p15, 0, r1, c0, c0, 5	@ MPIDR (Multiprocessor Affinity Reg)
+	and	r1, r1, #0xff
+
+	ldr	r2, =uniphier_smp_booted
+	mov	r0, #1
+	str	r0, [r2, r1, lsl #2]
+
+	ldr	r2, =uniphier_psci_holding_pen_release
+pen:	ldr	r0, [r2]
+	cmp	r0, r1
+	beq	psci_cpu_entry
+	wfe
+	b	pen
+ENDPROC(uniphier_secondary_startup)
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __DEBUG_H__
+#define __DEBUG_H__
+
+#include <linux/io.h>
+#include <linux/serial_reg.h>
+
+#define DEBUG_UART_BASE		0x54006800
+#define UART_SHIFT 2
+
+#define UNIPHIER_UART_TX		0
+#define UNIPHIER_UART_LSR		(5 * 4)
+
+/* All functions are inline so that they can be called from .secure section. */
+
+#ifdef DEBUG
+static inline void debug_putc(int c)
+{
+	void __iomem *base = (void __iomem *)DEBUG_UART_BASE;
+
+	while (!(readl(base + UNIPHIER_UART_LSR) & UART_LSR_THRE))
+		;
+
+	writel(c, base + UNIPHIER_UART_TX);
+}
+
+static inline void debug_puts(const char *s)
+{
+	while (*s) {
+		if (*s == '\n')
+			debug_putc('\r');
+
+		debug_putc(*s++);
+	}
+}
+
+static inline void debug_puth(unsigned long val)
+{
+	int i;
+	unsigned char c;
+
+	for (i = 8; i--; ) {
+		c = ((val >> (i * 4)) & 0xf);
+		c += (c >= 10) ? 'a' - 10 : '0';
+		debug_putc(c);
+	}
+}
+#else
+static inline void debug_putc(int c)
+{
+}
+
+static inline void debug_puts(const char *s)
+{
+}
+
+static inline void debug_puth(unsigned long val)
+{
+}
+#endif
+
+#endif /* __DEBUG_H__ */
 /*
  * UniPhier SBC (System Bus Controller) registers
  *
- * Copyright (C) 2011-2015 Masahiro Yamada <yamada.masahiro@socionext.com>
+ * Copyright (C) 2011-2014 Panasonic Corporation
+ * Copyright (C) 2015-2016 Socionext Inc.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -75,7 +76,6 @@
 #define	SBCTRL74		(SBCTRL_BASE + 0x170)
  
 #define PC0CTRL				0x598000c0
-#define ROM_BOOT_ROMRSV2		0x59801208
  
 #ifndef __ASSEMBLY__
 #include <linux/io.h>
 /*
- * Copyright (C) 2012-2015 Masahiro Yamada <yamada.masahiro@socionext.com>
+ * Copyright (C) 2012-2015 Panasonic Corporation
+ * Copyright (C) 2015-2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -8,6 +10,9 @@
  
 #ifndef __CONFIG_UNIPHIER_COMMON_H__
 #define __CONFIG_UNIPHIER_COMMON_H__
+
+#define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_NR_CPUS   4
  
 #define CONFIG_SYS_EEPROM_PAGE_WRITE_DELAY_MS  10
...	...	@@ -6,6 +6,8 @@
6	6	config ARCH_UNIPHIER_32BIT
7	7	bool
8	8	select CPU_V7
	9	+ select CPU_V7_HAS_NONSEC
	10	+ select ARMV7_NONSEC
9	11
10	12	config ARCH_UNIPHIER_64BIT
11	13	bool
...	...	@@ -8,6 +8,7 @@
8	8	else
9	9	obj-y += late_lowlevel_init.o
10	10	obj-y += cache-uniphier.o
	11	+obj-$(CONFIG_ARMV7_PSCI) += psci.o psci_smp.o
11	12	endif
12	13
13	14	obj-y += timer.o
...	...	@@ -12,6 +12,9 @@
12	12
13	13	/* SCU Control Register */
14	14	#define SCU_CTRL 0x00
	15	+#define SCU_ENABLE (1 << 0)
	16	+#define SCU_STANDBY_ENABLE (1 << 5)
	17	+
15	18	/* SCU Configuration Register */
16	19	#define SCU_CONF 0x04
17	20	/* SCU CPU Power Status Register */
	1	+/*
	2	+ * Copyright (C) 2016 Socionext Inc.
	3	+ * Author: Masahiro Yamada <yamada.masahiro@socionext.com>
	4	+ *
	5	+ * SPDX-License-Identifier: GPL-2.0+
	6	+ */
	7	+
	8	+#include <common.h>
	9	+#include <linux/bitops.h>
	10	+#include <linux/io.h>
	11	+#include <linux/kernel.h>
	12	+#include <linux/psci.h>
	13	+#include <linux/sizes.h>
	14	+#include <asm/processor.h>
	15	+#include <asm/psci.h>
	16	+#include <asm/secure.h>
	17	+
	18	+#include "../debug.h"
	19	+#include "../soc-info.h"
	20	+#include "arm-mpcore.h"
	21	+#include "cache-uniphier.h"
	22	+
	23	+#define UNIPHIER_SMPCTRL_ROM_RSV2 0x59801208
	24	+
	25	+void uniphier_smp_trampoline(void);
	26	+void uniphier_smp_trampoline_end(void);
	27	+u32 uniphier_smp_booted[CONFIG_ARMV7_PSCI_NR_CPUS];
	28	+
	29	+static int uniphier_get_nr_cpus(void)
	30	+{
	31	+ switch (uniphier_get_soc_type()) {
	32	+ case SOC_UNIPHIER_SLD3:
	33	+ case SOC_UNIPHIER_PRO4:
	34	+ case SOC_UNIPHIER_PRO5:
	35	+ return 2;
	36	+ case SOC_UNIPHIER_PXS2:
	37	+ case SOC_UNIPHIER_LD6B:
	38	+ return 4;
	39	+ default:
	40	+ return 1;
	41	+ }
	42	+}
	43	+
	44	+static void uniphier_smp_kick_all_cpus(void)
	45	+{
	46	+ const u32 target_ways = BIT(0);
	47	+ size_t trmp_size;
	48	+ u32 trmp_src = (unsigned long)uniphier_smp_trampoline;
	49	+ u32 trmp_src_end = (unsigned long)uniphier_smp_trampoline_end;
	50	+ u32 trmp_dest, trmp_dest_end;
	51	+ int nr_cpus, i;
	52	+ int timeout = 1000;
	53	+
	54	+ nr_cpus = uniphier_get_nr_cpus();
	55	+ if (nr_cpus == 1)
	56	+ return;
	57	+
	58	+ for (i = 0; i < nr_cpus; i++) /* lock ways for all CPUs */
	59	+ uniphier_cache_set_active_ways(i, 0);
	60	+ uniphier_cache_inv_way(target_ways);
	61	+ uniphier_cache_enable();
	62	+
	63	+ /* copy trampoline code */
	64	+ uniphier_cache_prefetch_range(trmp_src, trmp_src_end, target_ways);
	65	+
	66	+ trmp_size = trmp_src_end - trmp_src;
	67	+
	68	+ trmp_dest = trmp_src & (SZ_64K - 1);
	69	+ trmp_dest += SZ_1M - SZ_64K * 2;
	70	+
	71	+ trmp_dest_end = trmp_dest + trmp_size;
	72	+
	73	+ uniphier_cache_touch_range(trmp_dest, trmp_dest_end, target_ways);
	74	+
	75	+ writel(trmp_dest, UNIPHIER_SMPCTRL_ROM_RSV2);
	76	+
	77	+ asm("dsb ishst\n" /* Ensure the write to ROM_RSV2 is visible */
	78	+ "sev"); /* Bring up all secondary CPUs from Boot ROM into U-Boot */
	79	+
	80	+ while (--timeout) {
	81	+ int all_booted = 1;
	82	+
	83	+ for (i = 1; i < nr_cpus; i++)
	84	+ if (!uniphier_smp_booted[i])
	85	+ all_booted = 0;
	86	+ if (all_booted)
	87	+ break;
	88	+ udelay(1);
	89	+
	90	+ /* barrier here because uniphier_smp_booted[] may be updated */
	91	+ cpu_relax();
	92	+ }
	93	+
	94	+ if (!timeout)
	95	+ printf("warning: some of secondary CPUs may not boot\n");
	96	+
	97	+ uniphier_cache_disable();
	98	+}
	99	+
	100	+void psci_board_init(void)
	101	+{
	102	+ unsigned long scu_base;
	103	+ u32 scu_ctrl, tmp;
	104	+
	105	+ asm("mrc p15, 4, %0, c15, c0, 0" : "=r" (scu_base));
	106	+
	107	+ scu_ctrl = readl(scu_base + 0x30);
	108	+ if (!(scu_ctrl & 1))
	109	+ writel(scu_ctrl \| 0x1, scu_base + 0x30);
	110	+
	111	+ scu_ctrl = readl(scu_base + SCU_CTRL);
	112	+ scu_ctrl \|= SCU_ENABLE \| SCU_STANDBY_ENABLE;
	113	+ writel(scu_ctrl, scu_base + SCU_CTRL);
	114	+
	115	+ tmp = readl(scu_base + SCU_SNSAC);
	116	+ tmp \|= 0xfff;
	117	+ writel(tmp, scu_base + SCU_SNSAC);
	118	+
	119	+ uniphier_smp_kick_all_cpus();
	120	+}
	121	+
	122	+void psci_arch_init(void)
	123	+{
	124	+ u32 actlr;
	125	+
	126	+ asm("mrc p15, 0, %0, c1, c0, 1" : "=r" (actlr));
	127	+ actlr \|= 0x41; /* set SMP and FW bits */
	128	+ asm("mcr p15, 0, %0, c1, c0, 1" : : "r" (actlr));
	129	+}
	130	+
	131	+u32 uniphier_psci_holding_pen_release __secure_data = 0xffffffff;
	132	+
	133	+int __secure psci_cpu_on(u32 function_id, u32 cpuid, u32 entry_point)
	134	+{
	135	+ u32 cpu = cpuid & 0xff;
	136	+
	137	+ debug_puts("[U-Boot PSCI] psci_cpu_on: cpuid=");
	138	+ debug_puth(cpuid);
	139	+ debug_puts(", entry_point=");
	140	+ debug_puth(entry_point);
	141	+ debug_puts("\n");
	142	+
	143	+ psci_save_target_pc(cpu, entry_point);
	144	+
	145	+ /* We assume D-cache is off, so do not call flush_dcache() here */
	146	+ uniphier_psci_holding_pen_release = cpu;
	147	+
	148	+ /* Send an event to wake up the secondary CPU. */
	149	+ asm("dsb ishst\n"
	150	+ "sev");
	151	+
	152	+ return PSCI_RET_SUCCESS;
	153	+}
1	1	/*
2	2	* UniPhier SBC (System Bus Controller) registers
3	3	*
4		- * Copyright (C) 2011-2015 Masahiro Yamada <yamada.masahiro@socionext.com>
	4	+ * Copyright (C) 2011-2014 Panasonic Corporation
	5	+ * Copyright (C) 2015-2016 Socionext Inc.
5	6	*
6	7	* SPDX-License-Identifier: GPL-2.0+
7	8	*/
...	...	@@ -75,7 +76,6 @@
75	76	#define SBCTRL74 (SBCTRL_BASE + 0x170)
76	77
77	78	#define PC0CTRL 0x598000c0
78		-#define ROM_BOOT_ROMRSV2 0x59801208
79	79
80	80	#ifndef __ASSEMBLY__
81	81	#include <linux/io.h>
1	1	/*
2		- * Copyright (C) 2012-2015 Masahiro Yamada <yamada.masahiro@socionext.com>
	2	+ * Copyright (C) 2012-2015 Panasonic Corporation
	3	+ * Copyright (C) 2015-2016 Socionext Inc.
	4	+ * Author: Masahiro Yamada <yamada.masahiro@socionext.com>
3	5	*
4	6	* SPDX-License-Identifier: GPL-2.0+
5	7	*/
...	...	@@ -8,6 +10,9 @@
8	10
9	11	#ifndef __CONFIG_UNIPHIER_COMMON_H__
10	12	#define __CONFIG_UNIPHIER_COMMON_H__
	13	+
	14	+#define CONFIG_ARMV7_PSCI
	15	+#define CONFIG_ARMV7_PSCI_NR_CPUS 4
11	16
12	17	#define CONFIG_SYS_EEPROM_PAGE_WRITE_DELAY_MS 10
13	18