diff --git a/arch/arm/mach-imx/mx7/Makefile b/arch/arm/mach-imx/mx7/Makefile index 94a0e64..f1436e2 100644 --- a/arch/arm/mach-imx/mx7/Makefile +++ b/arch/arm/mach-imx/mx7/Makefile @@ -4,4 +4,4 @@ # obj-y := soc.o clock.o clock_slice.o ddr.o snvs.o -obj-$(CONFIG_ARMV7_PSCI) += psci-mx7.o +obj-$(CONFIG_ARMV7_PSCI) += psci-mx7.o psci-suspend.o diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c index d6c4263..34ba0a9 100644 --- a/arch/arm/mach-imx/mx7/psci-mx7.c +++ b/arch/arm/mach-imx/mx7/psci-mx7.c @@ -8,20 +8,77 @@ #include #include #include +#include +#include #include #include #include -#define GPC_CPU_PGC_SW_PDN_REQ 0xfc +#define GPC_LPCR_A7_BSC 0x0 +#define GPC_LPCR_A7_AD 0x4 +#define GPC_SLPCR 0x14 +#define GPC_PGC_ACK_SEL_A7 0x24 +#define GPC_IMR1_CORE0 0x30 +#define GPC_SLOT0_CFG 0xb0 #define GPC_CPU_PGC_SW_PUP_REQ 0xf0 +#define GPC_CPU_PGC_SW_PDN_REQ 0xfc +#define GPC_PGC_C0 0x800 #define GPC_PGC_C0 0x800 #define GPC_PGC_C1 0x840 +#define GPC_PGC_SCU 0x880 + +#define BM_LPCR_A7_BSC_CPU_CLK_ON_LPM 0x4000 +#define BM_LPCR_A7_BSC_LPM1 0xc +#define BM_LPCR_A7_BSC_LPM0 0x3 +#define BP_LPCR_A7_BSC_LPM0 0 +#define BM_SLPCR_EN_DSM 0x80000000 +#define BM_SLPCR_RBC_EN 0x40000000 +#define BM_SLPCR_REG_BYPASS_COUNT 0x3f000000 +#define BM_SLPCR_VSTBY 0x4 +#define BM_SLPCR_SBYOS 0x2 +#define BM_SLPCR_BYPASS_PMIC_READY 0x1 +#define BM_LPCR_A7_AD_L2PGE 0x10000 +#define BM_LPCR_A7_AD_EN_C1_PUP 0x800 +#define BM_LPCR_A7_AD_EN_C0_PUP 0x200 +#define BM_LPCR_A7_AD_EN_PLAT_PDN 0x10 +#define BM_LPCR_A7_AD_EN_C1_PDN 0x8 +#define BM_LPCR_A7_AD_EN_C0_PDN 0x2 #define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE0_A7 0x1 #define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7 0x2 -/* below is for i.MX7D */ +#define BM_GPC_PGC_ACK_SEL_A7_PD_DUMMY_ACK 0x8000 +#define BM_GPC_PGC_ACK_SEL_A7_PU_DUMMY_ACK 0x80000000 + +#define MAX_SLOT_NUMBER 10 +#define A7_LPM_WAIT 0x5 +#define A7_LPM_STOP 0xa + +#define BM_SYS_COUNTER_CNTCR_FCR1 0x200 +#define BM_SYS_COUNTER_CNTCR_FCR0 0x100 + +#define REG_SET 0x4 +#define REG_CLR 0x8 + +#define ANADIG_ARM_PLL 0x60 +#define ANADIG_DDR_PLL 0x70 +#define ANADIG_SYS_PLL 0xb0 +#define ANADIG_ENET_PLL 0xe0 +#define ANADIG_AUDIO_PLL 0xf0 +#define ANADIG_VIDEO_PLL 0x130 +#define BM_ANATOP_ARM_PLL_OVERRIDE BIT(20) +#define BM_ANATOP_DDR_PLL_OVERRIDE BIT(19) +#define BM_ANATOP_SYS_PLL_OVERRIDE (0x1ff << 17) +#define BM_ANATOP_ENET_PLL_OVERRIDE BIT(13) +#define BM_ANATOP_AUDIO_PLL_OVERRIDE BIT(24) +#define BM_ANATOP_VIDEO_PLL_OVERRIDE BIT(24) + +#define DDRC_STAT 0x4 +#define DDRC_PWRCTL 0x30 +#define DDRC_PSTAT 0x3fc + #define SRC_GPR1_MX7D 0x074 +#define SRC_GPR2_MX7D 0x078 #define SRC_A7RCR0 0x004 #define SRC_A7RCR1 0x008 @@ -56,6 +113,27 @@ u8 psci_state[IMX7D_PSCI_NR_CPUS] __secure_data = { PSCI_AFFINITY_LEVEL_ON, PSCI_AFFINITY_LEVEL_OFF}; +enum imx_gpc_slot { + CORE0_A7, + CORE1_A7, + SCU_A7, + FAST_MEGA_MIX, + MIPI_PHY, + PCIE_PHY, + USB_OTG1_PHY, + USB_OTG2_PHY, + USB_HSIC_PHY, + CORE0_M4, +}; + +enum mxc_cpu_pwr_mode { + RUN, + WAIT, + STOP, +}; + +extern void psci_system_resume(void); + static inline void psci_set_state(int cpu, u8 state) { psci_state[cpu] = state; @@ -237,7 +315,374 @@ __secure s32 psci_features(u32 __always_unused function_id, u32 psci_fid) case ARM_PSCI_0_2_FN_SYSTEM_OFF: case ARM_PSCI_0_2_FN_SYSTEM_RESET: case ARM_PSCI_1_0_FN_PSCI_FEATURES: + case ARM_PSCI_1_0_FN_SYSTEM_SUSPEND: return 0x0; } return ARM_PSCI_RET_NI; } + +static __secure void imx_gpcv2_set_lpm_mode(enum mxc_cpu_pwr_mode mode) +{ + u32 val1, val2, val3; + + val1 = readl(GPC_IPS_BASE_ADDR + GPC_LPCR_A7_BSC); + val2 = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR); + + /* all cores' LPM settings must be same */ + val1 &= ~(BM_LPCR_A7_BSC_LPM0 | BM_LPCR_A7_BSC_LPM1); + val1 |= BM_LPCR_A7_BSC_CPU_CLK_ON_LPM; + + val2 &= ~(BM_SLPCR_EN_DSM | BM_SLPCR_VSTBY | BM_SLPCR_RBC_EN | + BM_SLPCR_SBYOS | BM_SLPCR_BYPASS_PMIC_READY); + /* + * GPC: When improper low-power sequence is used, + * the SoC enters low power mode before the ARM core executes WFI. + * + * Software workaround: + * 1) Software should trigger IRQ #32 (IOMUX) to be always pending + * by setting IOMUX_GPR1_IRQ. + * 2) Software should then unmask IRQ #32 in GPC before setting GPC + * Low-Power mode. + * 3) Software should mask IRQ #32 right after GPC Low-Power mode + * is set. + */ + switch (mode) { + case RUN: + val3 = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0); + val3 &= ~0x1; + writel(val3, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0); + break; + case WAIT: + val1 |= A7_LPM_WAIT << BP_LPCR_A7_BSC_LPM0; + val1 &= ~BM_LPCR_A7_BSC_CPU_CLK_ON_LPM; + val3 = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0); + val3 &= ~0x1; + writel(val3, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0); + break; + case STOP: + val1 |= A7_LPM_STOP << BP_LPCR_A7_BSC_LPM0; + val1 &= ~BM_LPCR_A7_BSC_CPU_CLK_ON_LPM; + val2 |= BM_SLPCR_EN_DSM; + val2 |= BM_SLPCR_SBYOS; + val2 |= BM_SLPCR_VSTBY; + val2 |= BM_SLPCR_BYPASS_PMIC_READY; + val3 = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0); + val3 |= 0x1; + writel(val3, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0); + break; + default: + return; + } + writel(val1, GPC_IPS_BASE_ADDR + GPC_LPCR_A7_BSC); + writel(val2, GPC_IPS_BASE_ADDR + GPC_SLPCR); +} + +static __secure void imx_gpcv2_set_plat_power_gate_by_lpm(bool pdn) +{ + u32 val = readl(GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD); + + val &= ~(BM_LPCR_A7_AD_EN_PLAT_PDN | BM_LPCR_A7_AD_L2PGE); + if (pdn) + val |= BM_LPCR_A7_AD_EN_PLAT_PDN | BM_LPCR_A7_AD_L2PGE; + + writel(val, GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD); +} + +static __secure void imx_gpcv2_set_cpu_power_gate_by_lpm(u32 cpu, bool pdn) +{ + u32 val; + + val = readl(GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD); + if (cpu == 0) { + if (pdn) + val |= BM_LPCR_A7_AD_EN_C0_PDN | + BM_LPCR_A7_AD_EN_C0_PUP; + else + val &= ~(BM_LPCR_A7_AD_EN_C0_PDN | + BM_LPCR_A7_AD_EN_C0_PUP); + } + if (cpu == 1) { + if (pdn) + val |= BM_LPCR_A7_AD_EN_C1_PDN | + BM_LPCR_A7_AD_EN_C1_PUP; + else + val &= ~(BM_LPCR_A7_AD_EN_C1_PDN | + BM_LPCR_A7_AD_EN_C1_PUP); + } + writel(val, GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD); +} + +static __secure void imx_gpcv2_set_slot_ack(u32 index, enum imx_gpc_slot m_core, + bool mode, bool ack) +{ + u32 val; + + if (index >= MAX_SLOT_NUMBER) + return; + + /* set slot */ + writel(readl(GPC_IPS_BASE_ADDR + GPC_SLOT0_CFG + index * 4) | + ((mode + 1) << (m_core * 2)), + GPC_IPS_BASE_ADDR + GPC_SLOT0_CFG + index * 4); + + if (ack) { + /* set ack */ + val = readl(GPC_IPS_BASE_ADDR + GPC_PGC_ACK_SEL_A7); + /* clear dummy ack */ + val &= ~(mode ? BM_GPC_PGC_ACK_SEL_A7_PU_DUMMY_ACK : + BM_GPC_PGC_ACK_SEL_A7_PD_DUMMY_ACK); + val |= 1 << (m_core + (mode ? 16 : 0)); + writel(val, GPC_IPS_BASE_ADDR + GPC_PGC_ACK_SEL_A7); + } +} + +static __secure void imx_system_counter_resume(void) +{ + u32 val; + + val = readl(SYSCNT_CTRL_IPS_BASE_ADDR); + val &= ~BM_SYS_COUNTER_CNTCR_FCR1; + val |= BM_SYS_COUNTER_CNTCR_FCR0; + writel(val, SYSCNT_CTRL_IPS_BASE_ADDR); +} + +static __secure void imx_system_counter_suspend(void) +{ + u32 val; + + val = readl(SYSCNT_CTRL_IPS_BASE_ADDR); + val &= ~BM_SYS_COUNTER_CNTCR_FCR0; + val |= BM_SYS_COUNTER_CNTCR_FCR1; + writel(val, SYSCNT_CTRL_IPS_BASE_ADDR); +} + +static __secure void gic_resume(void) +{ + u32 itlinesnr, i; + u32 gic_dist_addr = GIC400_ARB_BASE_ADDR + GIC_DIST_OFFSET; + + /* enable the GIC distributor */ + writel(readl(gic_dist_addr + GICD_CTLR) | 0x03, + gic_dist_addr + GICD_CTLR); + + /* TYPER[4:0] contains an encoded number of available interrupts */ + itlinesnr = readl(gic_dist_addr + GICD_TYPER) & 0x1f; + + /* set all bits in the GIC group registers to one to allow access + * from non-secure state. The first 32 interrupts are private per + * CPU and will be set later when enabling the GIC for each core + */ + for (i = 1; i <= itlinesnr; i++) + writel((u32)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i); +} + +static inline void imx_pll_suspend(void) +{ + writel(BM_ANATOP_ARM_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_ARM_PLL + REG_SET); + writel(BM_ANATOP_DDR_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_DDR_PLL + REG_SET); + writel(BM_ANATOP_SYS_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_SYS_PLL + REG_SET); + writel(BM_ANATOP_ENET_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_ENET_PLL + REG_SET); + writel(BM_ANATOP_AUDIO_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_AUDIO_PLL + REG_SET); + writel(BM_ANATOP_VIDEO_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_VIDEO_PLL + REG_SET); +} + +static inline void imx_pll_resume(void) +{ + writel(BM_ANATOP_ARM_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_ARM_PLL + REG_CLR); + writel(BM_ANATOP_DDR_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_DDR_PLL + REG_CLR); + writel(BM_ANATOP_SYS_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_SYS_PLL + REG_CLR); + writel(BM_ANATOP_ENET_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_ENET_PLL + REG_CLR); + writel(BM_ANATOP_AUDIO_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_AUDIO_PLL + REG_CLR); + writel(BM_ANATOP_VIDEO_PLL_OVERRIDE, + ANATOP_BASE_ADDR + ANADIG_VIDEO_PLL + REG_CLR); +} + +static inline void imx_udelay(u32 usec) +{ + u32 freq; + u64 start, end; + + asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (freq)); + asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (start)); + do { + asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (end)); + if ((end - start) > usec * (freq / 1000000)) + break; + } while (1); +} + +static inline void imx_ddrc_enter_self_refresh(void) +{ + writel(0, DDRC_IPS_BASE_ADDR + DDRC_PWRCTL); + while (readl(DDRC_IPS_BASE_ADDR + DDRC_PSTAT) & 0x10001) + ; + + writel(0x20, DDRC_IPS_BASE_ADDR + DDRC_PWRCTL); + while ((readl(DDRC_IPS_BASE_ADDR + DDRC_STAT) & 0x23) != 0x23) + ; + writel(readl(DDRC_IPS_BASE_ADDR + DDRC_PWRCTL) | 0x8, + DDRC_IPS_BASE_ADDR + DDRC_PWRCTL); +} + +static inline void imx_ddrc_exit_self_refresh(void) +{ + writel(0, DDRC_IPS_BASE_ADDR + DDRC_PWRCTL); + while ((readl(DDRC_IPS_BASE_ADDR + DDRC_STAT) & 0x3) == 0x3) + ; + writel(readl(DDRC_IPS_BASE_ADDR + DDRC_PWRCTL) | 0x1, + DDRC_IPS_BASE_ADDR + DDRC_PWRCTL); +} + +__secure void imx_system_resume(void) +{ + unsigned int i, val, imr[4], entry; + + entry = psci_get_target_pc(0); + imx_ddrc_exit_self_refresh(); + imx_system_counter_resume(); + imx_gpcv2_set_lpm_mode(RUN); + imx_gpcv2_set_cpu_power_gate_by_lpm(0, false); + imx_gpcv2_set_plat_power_gate_by_lpm(false); + imx_gpcv2_set_m_core_pgc(false, GPC_PGC_C0); + imx_gpcv2_set_m_core_pgc(false, GPC_PGC_SCU); + + /* + * need to mask all interrupts in GPC before + * operating RBC configurations + */ + for (i = 0; i < 4; i++) { + imr[i] = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4); + writel(~0, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4); + } + + /* configure RBC enable bit */ + val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR); + val &= ~BM_SLPCR_RBC_EN; + writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR); + + /* configure RBC count */ + val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR); + val &= ~BM_SLPCR_REG_BYPASS_COUNT; + writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR); + + /* + * need to delay at least 2 cycles of CKIL(32K) + * due to hardware design requirement, which is + * ~61us, here we use 65us for safe + */ + imx_udelay(65); + + /* restore GPC interrupt mask settings */ + for (i = 0; i < 4; i++) + writel(imr[i], GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4); + + /* initialize gic distributor */ + gic_resume(); + _nonsec_init(); + + /* save cpu0 entry */ + psci_save(0, entry, 0); + psci_cpu_entry(); +} + +__secure void psci_system_suspend(u32 __always_unused function_id, + u32 ep, u32 context_id) +{ + u32 gpc_mask[4]; + u32 i, val; + + psci_save(0, ep, context_id); + /* overwrite PLL to be controlled by low power mode */ + imx_pll_suspend(); + imx_system_counter_suspend(); + /* set CA7 platform to enter STOP mode */ + imx_gpcv2_set_lpm_mode(STOP); + /* enable core0/scu power down/up with low power mode */ + imx_gpcv2_set_cpu_power_gate_by_lpm(0, true); + imx_gpcv2_set_plat_power_gate_by_lpm(true); + /* time slot settings for core0 and scu */ + imx_gpcv2_set_slot_ack(0, CORE0_A7, false, false); + imx_gpcv2_set_slot_ack(1, SCU_A7, false, true); + imx_gpcv2_set_slot_ack(5, SCU_A7, true, false); + imx_gpcv2_set_slot_ack(6, CORE0_A7, true, true); + imx_gpcv2_set_m_core_pgc(true, GPC_PGC_C0); + imx_gpcv2_set_m_core_pgc(true, GPC_PGC_SCU); + psci_v7_flush_dcache_all(); + + imx_ddrc_enter_self_refresh(); + + /* + * e10133: ARM: Boot failure after A7 enters into + * low-power idle mode + * + * Workaround: + * If both CPU0/CPU1 are IDLE, the last IDLE CPU should + * disable GIC first, then REG_BYPASS_COUNTER is used + * to mask wakeup INT, and then execute “wfi” is used to + * bring the system into power down processing safely. + * The counter must be enabled as close to the “wfi” state + * as possible. The following equation can be used to + * determine the RBC counter value: + * RBC_COUNT * (1/32K RTC frequency) >= + * (46 + PDNSCR_SW + PDNSCR_SW2ISO ) ( 1/IPG_CLK frequency ). + */ + + /* disable GIC distributor */ + writel(0, GIC400_ARB_BASE_ADDR + GIC_DIST_OFFSET); + + for (i = 0; i < 4; i++) + gpc_mask[i] = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4); + + /* + * enable the RBC bypass counter here + * to hold off the interrupts. RBC counter + * = 8 (240us). With this setting, the latency + * from wakeup interrupt to ARM power up + * is ~250uS. + */ + val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR); + val &= ~(0x3f << 24); + val |= (0x8 << 24); + writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR); + + /* enable the counter. */ + val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR); + val |= (1 << 30); + writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR); + + /* unmask all the GPC interrupts. */ + for (i = 0; i < 4; i++) + writel(gpc_mask[i], GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4); + + /* + * now delay for a short while (3usec) + * ARM is at 1GHz at this point + * so a short loop should be enough. + * this delay is required to ensure that + * the RBC counter can start counting in + * case an interrupt is already pending + * or in case an interrupt arrives just + * as ARM is about to assert DSM_request. + */ + imx_udelay(3); + + /* save resume entry and sp in CPU0 GPR registers */ + asm volatile("mov %0, sp" : "=r" (val)); + writel((u32)psci_system_resume, SRC_BASE_ADDR + SRC_GPR1_MX7D); + writel(val, SRC_BASE_ADDR + SRC_GPR2_MX7D); + + /* sleep */ + while (1) + wfi(); +} diff --git a/arch/arm/mach-imx/mx7/psci-suspend.S b/arch/arm/mach-imx/mx7/psci-suspend.S new file mode 100644 index 0000000..a21403f --- /dev/null +++ b/arch/arm/mach-imx/mx7/psci-suspend.S @@ -0,0 +1,67 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * Copyright 2018 NXP + */ + +#include +#include + +#include +#include + + .pushsection ._secure.text, "ax" + + .arch_extension sec + +.globl v7_invalidate_l1 +v7_invalidate_l1: + mov r0, #0 + mcr p15, 2, r0, c0, c0, 0 + mrc p15, 1, r0, c0, c0, 0 + + movw r1, #0x7fff + and r2, r1, r0, lsr #13 + + movw r1, #0x3ff + + and r3, r1, r0, lsr #3 @ NumWays - 1 + add r2, r2, #1 @ NumSets + + and r0, r0, #0x7 + add r0, r0, #4 @ SetShift + + clz r1, r3 @ WayShift + add r4, r3, #1 @ NumWays +1: + sub r2, r2, #1 @ NumSets-- + mov r3, r4 @ Temp = NumWays +2: + subs r3, r3, #1 @ Temp-- + mov r5, r3, lsl r1 + mov r6, r2, lsl r0 + orr r5, r5, r6 @ Reg = (Temp<