[U-Boot] [PATCH 2/3] mx7: psci: add basic psci support

[Frank Li]

add basic psci support for imx7 chip.
support cpu_on and cpu_off.
linux kernel boot at nosecure mode.
set csu allow nosecure mode kernel to access all peripherial register

Signed-off-by: Frank Li <Frank.Li at freescale.com>
---
 arch/arm/cpu/armv7/mx7/Makefile   |  4 ++
 arch/arm/cpu/armv7/mx7/psci-mx7.c | 79 +++++++++++++++++++++++++++++++++++++++
 arch/arm/cpu/armv7/mx7/psci.S     | 54 ++++++++++++++++++++++++++
 arch/arm/cpu/armv7/mx7/soc.c      |  9 +++++
 4 files changed, 146 insertions(+)
 create mode 100644 arch/arm/cpu/armv7/mx7/psci-mx7.c
 create mode 100644 arch/arm/cpu/armv7/mx7/psci.S

diff --git a/arch/arm/cpu/armv7/mx7/Makefile b/arch/arm/cpu/armv7/mx7/Makefile
index e6ecef0..f25461c 100644
--- a/arch/arm/cpu/armv7/mx7/Makefile
+++ b/arch/arm/cpu/armv7/mx7/Makefile
@@ -6,3 +6,7 @@
 #
 
 obj-y	:= soc.o clock.o clock_slice.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o psci-mx7.o
+endif
diff --git a/arch/arm/cpu/armv7/mx7/psci-mx7.c b/arch/arm/cpu/armv7/mx7/psci-mx7.c
new file mode 100644
index 0000000..6659332
--- /dev/null
+++ b/arch/arm/cpu/armv7/mx7/psci-mx7.c
@@ -0,0 +1,79 @@
+#include <common.h>
+#include <asm/psci.h>
+#include <asm/arch/imx-regs.h>
+
+#define __secure __attribute__ ((section ("._secure.text")))
+
+#define GPC_CPU_PGC_SW_PDN_REQ	0xfc
+#define GPC_CPU_PGC_SW_PUP_REQ	0xf0
+#define GPC_PGC_C1		0x840
+
+#define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7	0x2
+
+/* below is for i.MX7D */
+#define SRC_GPR1_MX7D		0x074
+#define SRC_A7RCR0		0x004
+#define SRC_A7RCR1		0x008
+
+#define BP_SRC_A7RCR0_A7_CORE_RESET0	0
+#define BP_SRC_A7RCR1_A7_CORE1_ENABLE	1
+
+static inline void psci_writel(u32 value, u32 reg)
+{
+	*(volatile u32 *)reg = value;
+}
+
+static inline int psci_readl(u32 reg)
+{
+	return *(volatile u32*)reg;
+}
+
+static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
+{
+	psci_writel(enable, GPC_IPS_BASE_ADDR + offset);
+}
+
+__secure void imx_gpcv2_set_core1_power(bool pdn)
+{
+	u32 reg = pdn ? GPC_CPU_PGC_SW_PUP_REQ : GPC_CPU_PGC_SW_PDN_REQ;
+	u32 val;
+
+	imx_gpcv2_set_m_core_pgc(true, GPC_PGC_C1);
+
+	val = psci_readl(GPC_IPS_BASE_ADDR + reg);
+	val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
+	psci_writel(val, GPC_IPS_BASE_ADDR + reg);
+
+	while ((psci_readl(GPC_IPS_BASE_ADDR + reg) & BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7) != 0)
+		;
+
+	imx_gpcv2_set_m_core_pgc(false, GPC_PGC_C1);
+}
+
+__secure void imx_enable_cpu_ca7(int cpu, bool enable)
+{
+	u32 mask, val;
+
+	mask = 1 << (BP_SRC_A7RCR1_A7_CORE1_ENABLE + cpu - 1);
+	val = psci_readl(SRC_BASE_ADDR + SRC_A7RCR1);
+	val = enable ? val | mask : val & ~mask;
+	psci_writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
+}
+
+
+__secure int imx_cpu_on(int fn, int cpu, int pc)
+{
+	psci_writel(pc, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
+	imx_gpcv2_set_core1_power(true);
+	imx_enable_cpu_ca7(cpu, true);
+	return 0;
+}
+
+__secure int imx_cpu_off(int cpu)
+{
+	imx_enable_cpu_ca7(cpu, false);
+	imx_gpcv2_set_core1_power(false);
+	psci_writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
+	return 0;
+}
+
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
new file mode 100644
index 0000000..4165cf5
--- /dev/null
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -0,0 +1,54 @@
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+	@ r1 = target CPU
+	@ r2 = target PC
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	@ r1 = target CPU
+	@ r2 = target PC
+
+.globl psci_cpu_on
+psci_cpu_on:
+	push {lr}
+
+	mov r0, r1
+	bl psci_get_cpu_stack_top
+	str r2, [r0]
+	dsb
+
+	ldr	r2, =psci_cpu_entry
+	bl imx_cpu_on
+
+	pop	{pc}
+
+.globl psci_cpu_off
+psci_cpu_off:
+
+	bl psci_cpu_off_common
+	bl psci_get_cpu_id
+	bl imx_cpu_off
+
+1: 	wfi
+	b 1b
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection
diff --git a/arch/arm/cpu/armv7/mx7/soc.c b/arch/arm/cpu/armv7/mx7/soc.c
index 8d50149..cfe2b1c 100644
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -116,10 +116,19 @@ u32 __weak get_board_rev(void)
 }
 #endif
 
+/* enable all periherial can be access in nosec mode */
+static void init_csu(void)
+{
+	int i = 0;
+	for (i = 0; i < 64; i++)
+		writel(0x00FF00FF, 0x303e0000 + i * 4);
+}
+
 int arch_cpu_init(void)
 {
 	init_aips();
 
+	init_csu();
 	/* Disable PDE bit of WMCR register */
 	imx_set_wdog_powerdown(false);
 
-- 
2.5.2