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 (file)
--- 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 (file)
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 (file)
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 (file)
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -122,10 +122,19 @@ u32 __weak get_board_rev(void)
 }
 #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 (file)
--- a/arch/arm/include/asm/arch-mx7/imx-regs.h
+++ b/arch/arm/include/asm/arch-mx7/imx-regs.h
@@ -866,6 +866,9 @@ struct cspi_regs {
        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;