mx7: psci: add basic psci support
1. add basic psci support for imx7 chip.
2. support cpu_on and cpu_off.
3. switch to non-secure mode when boot linux kernel.
4. set csu allow accessing all peripherial register in non-secure mode.
Signed-off-by: Frank Li <Frank.Li@freescale.com>
Signed-off-by: Peng Fan <Peng.Fan@freescale.com>
Cc: Stefano Babic <sbabic@denx.de>
Cc: Fabio Estevam <fabio.estevam@freescale.com>
diff --git a/arch/arm/cpu/armv7/mx7/Makefile b/arch/arm/cpu/armv7/mx7/Makefile
index e6ecef0..d21f87f 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-mx7.o psci.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..9a33047
--- /dev/null
+++ b/arch/arm/cpu/armv7/mx7/psci-mx7.c
@@ -0,0 +1,69 @@
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/arch/imx-regs.h>
+#include <common.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 imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
+{
+ 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 = readl(GPC_IPS_BASE_ADDR + reg);
+ val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
+ writel(val, GPC_IPS_BASE_ADDR + reg);
+
+ while ((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 = readl(SRC_BASE_ADDR + SRC_A7RCR1);
+ val = enable ? val | mask : val & ~mask;
+ writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
+}
+
+__secure int imx_cpu_on(int fn, int cpu, int pc)
+{
+ 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);
+ 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..34c6ab3
--- /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 a6224af..c777922 100644
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -122,10 +122,19 @@
}
#endif
+/* enable all periherial can be accessed in nosec mode */
+static void init_csu(void)
+{
+ int i = 0;
+ for (i = 0; i < CSU_NUM_REGS; i++)
+ writel(CSU_INIT_SEC_LEVEL0, CSU_IPS_BASE_ADDR + i * 4);
+}
+
int arch_cpu_init(void)
{
init_aips();
+ init_csu();
/* Disable PDE bit of WMCR register */
imx_set_wdog_powerdown(false);
diff --git a/arch/arm/include/asm/arch-mx7/imx-regs.h b/arch/arm/include/asm/arch-mx7/imx-regs.h
index 4dc11ee..9213374 100644
--- a/arch/arm/include/asm/arch-mx7/imx-regs.h
+++ b/arch/arm/include/asm/arch-mx7/imx-regs.h
@@ -866,6 +866,9 @@
ECSPI3_BASE_ADDR, \
ECSPI4_BASE_ADDR
+#define CSU_INIT_SEC_LEVEL0 0x00FF00FF
+#define CSU_NUM_REGS 64
+
struct ocotp_regs {
u32 ctrl;
u32 ctrl_set;