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

[Li Frank]

> -----Original Message-----
> From: Peng Fan [mailto:Peng.Fan at freescale.com]
> Sent: Tuesday, October 20, 2015 1:00 AM
> To: u-boot at lists.denx.de
> Cc: Fan Peng-B51431 <Peng.Fan at freescale.com>; Li Frank-B20596
> <Frank.Li at freescale.com>; Stefano Babic <sbabic at denx.de>; Estevam Fabio-
> R49496 <Fabio.Estevam at freescale.com>
> Subject: [PATCH V2 2/3] 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 at freescale.com>
> Signed-off-by: Peng Fan <Peng.Fan at freescale.com>
> Cc: Stefano Babic <sbabic at denx.de>
> Cc: Fabio Estevam <fabio.estevam at freescale.com>
> ---
> 
> Changes V2:
>  Refine commit msg.
> 
>  arch/arm/cpu/armv7/mx7/Makefile   |  4 ++
>  arch/arm/cpu/armv7/mx7/psci-mx7.c | 78
> +++++++++++++++++++++++++++++++++++++++
>  arch/arm/cpu/armv7/mx7/psci.S     | 54 +++++++++++++++++++++++++++
>  arch/arm/cpu/armv7/mx7/soc.c      |  9 +++++
>  4 files changed, 145 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

Obj-y += psci-mx7.o psci.o
The otherwise psci_text_end will not be last one.

Best regards
Frank Li

> +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..ec9ad87
> --- /dev/null
> +++ b/arch/arm/cpu/armv7/mx7/psci-mx7.c
> @@ -0,0 +1,78 @@
> +#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..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 2ed05ea..020731d 100644
> --- a/arch/arm/cpu/armv7/mx7/soc.c
> +++ b/arch/arm/cpu/armv7/mx7/soc.c
> @@ -114,10 +114,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);
> 
> --
> 1.8.4
>