arch/arm/mach-imx/mx7/psci-mx7.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
   4  * Copyright 2017 NXP
   5  */
   6
   7 #include <asm/io.h>
   8 #include <asm/psci.h>
   9 #include <asm/secure.h>
  10 #include <asm/arch/imx-regs.h>
  11 #include <asm/armv7.h>
  12 #include <asm/gic.h>
  13 #include <linux/bitops.h>
  14 #include <common.h>
  15 #include <fsl_wdog.h>
  16
  17 #define GPC_LPCR_A7_BSC 0x0
  18 #define GPC_LPCR_A7_AD          0x4
  19 #define GPC_SLPCR               0x14
  20 #define GPC_PGC_ACK_SEL_A7      0x24
  21 #define GPC_IMR1_CORE0          0x30
  22 #define GPC_SLOT0_CFG           0xb0
  23 #define GPC_CPU_PGC_SW_PUP_REQ  0xf0
  24 #define GPC_CPU_PGC_SW_PDN_REQ  0xfc
  25 #define GPC_PGC_C0              0x800
  26 #define GPC_PGC_C0              0x800
  27 #define GPC_PGC_C1              0x840
  28 #define GPC_PGC_SCU             0x880
  29
  30 #define BM_LPCR_A7_BSC_CPU_CLK_ON_LPM           0x4000
  31 #define BM_LPCR_A7_BSC_LPM1                     0xc
  32 #define BM_LPCR_A7_BSC_LPM0                     0x3
  33 #define BP_LPCR_A7_BSC_LPM0                     0
  34 #define BM_SLPCR_EN_DSM                         0x80000000
  35 #define BM_SLPCR_RBC_EN                         0x40000000
  36 #define BM_SLPCR_REG_BYPASS_COUNT               0x3f000000
  37 #define BM_SLPCR_VSTBY                          0x4
  38 #define BM_SLPCR_SBYOS                          0x2
  39 #define BM_SLPCR_BYPASS_PMIC_READY              0x1
  40 #define BM_LPCR_A7_AD_L2PGE                     0x10000
  41 #define BM_LPCR_A7_AD_EN_C1_PUP                 0x800
  42 #define BM_LPCR_A7_AD_EN_C0_PUP                 0x200
  43 #define BM_LPCR_A7_AD_EN_PLAT_PDN               0x10
  44 #define BM_LPCR_A7_AD_EN_C1_PDN                 0x8
  45 #define BM_LPCR_A7_AD_EN_C0_PDN                 0x2
  46
  47 #define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE0_A7      0x1
  48 #define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7      0x2
  49
  50 #define BM_GPC_PGC_ACK_SEL_A7_PD_DUMMY_ACK      0x8000
  51 #define BM_GPC_PGC_ACK_SEL_A7_PU_DUMMY_ACK      0x80000000
  52
  53 #define MAX_SLOT_NUMBER                         10
  54 #define A7_LPM_WAIT                             0x5
  55 #define A7_LPM_STOP                             0xa
  56
  57 #define BM_SYS_COUNTER_CNTCR_FCR1 0x200
  58 #define BM_SYS_COUNTER_CNTCR_FCR0 0x100
  59
  60 #define REG_SET         0x4
  61 #define REG_CLR         0x8
  62
  63 #define ANADIG_ARM_PLL          0x60
  64 #define ANADIG_DDR_PLL          0x70
  65 #define ANADIG_SYS_PLL          0xb0
  66 #define ANADIG_ENET_PLL         0xe0
  67 #define ANADIG_AUDIO_PLL        0xf0
  68 #define ANADIG_VIDEO_PLL        0x130
  69 #define BM_ANATOP_ARM_PLL_OVERRIDE      BIT(20)
  70 #define BM_ANATOP_DDR_PLL_OVERRIDE      BIT(19)
  71 #define BM_ANATOP_SYS_PLL_OVERRIDE      (0x1ff << 17)
  72 #define BM_ANATOP_ENET_PLL_OVERRIDE     BIT(13)
  73 #define BM_ANATOP_AUDIO_PLL_OVERRIDE    BIT(24)
  74 #define BM_ANATOP_VIDEO_PLL_OVERRIDE    BIT(24)
  75
  76 #define DDRC_STAT       0x4
  77 #define DDRC_PWRCTL     0x30
  78 #define DDRC_PSTAT      0x3fc
  79
  80 #define SRC_GPR1_MX7D           0x074
  81 #define SRC_GPR2_MX7D           0x078
  82 #define SRC_A7RCR0              0x004
  83 #define SRC_A7RCR1              0x008
  84
  85 #define BP_SRC_A7RCR0_A7_CORE_RESET0    0
  86 #define BP_SRC_A7RCR1_A7_CORE1_ENABLE   1
  87
  88 #define SNVS_LPCR               0x38
  89 #define BP_SNVS_LPCR_DP_EN      0x20
  90 #define BP_SNVS_LPCR_TOP        0x40
  91
  92 #define CCM_CCGR_SNVS           0x4250
  93
  94 #define CCM_ROOT_WDOG           0xbb80
  95 #define CCM_CCGR_WDOG1          0x49c0
  96
  97 #define MPIDR_AFF0              GENMASK(7, 0)
  98
  99 #define IMX7D_PSCI_NR_CPUS      2
 100 #if IMX7D_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
 101 #error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
 102 #endif
 103
 104 #define imx_cpu_gpr_entry_offset(cpu) \
 105         (SRC_BASE_ADDR + SRC_GPR1_MX7D + cpu * 8)
 106 #define imx_cpu_gpr_para_offset(cpu) \
 107         (imx_cpu_gpr_entry_offset(cpu) + 4)
 108
 109 #define IMX_CPU_SYNC_OFF        ~0
 110 #define IMX_CPU_SYNC_ON         0
 111
 112 u8 psci_state[IMX7D_PSCI_NR_CPUS] __secure_data = {
 113          PSCI_AFFINITY_LEVEL_ON,
 114          PSCI_AFFINITY_LEVEL_OFF};
 115
 116 enum imx_gpc_slot {
 117         CORE0_A7,
 118         CORE1_A7,
 119         SCU_A7,
 120         FAST_MEGA_MIX,
 121         MIPI_PHY,
 122         PCIE_PHY,
 123         USB_OTG1_PHY,
 124         USB_OTG2_PHY,
 125         USB_HSIC_PHY,
 126         CORE0_M4,
 127 };
 128
 129 enum mxc_cpu_pwr_mode {
 130         RUN,
 131         WAIT,
 132         STOP,
 133 };
 134
 135 extern void psci_system_resume(void);
 136
 137 static inline void psci_set_state(int cpu, u8 state)
 138 {
 139         psci_state[cpu] = state;
 140         dsb();
 141         isb();
 142 }
 143
 144 static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
 145 {
 146         writel(enable, GPC_IPS_BASE_ADDR + offset);
 147 }
 148
 149 __secure void imx_gpcv2_set_core_power(int cpu, bool pdn)
 150 {
 151         u32 reg = pdn ? GPC_CPU_PGC_SW_PUP_REQ : GPC_CPU_PGC_SW_PDN_REQ;
 152         u32 pgc = cpu ? GPC_PGC_C1 : GPC_PGC_C0;
 153         u32 pdn_pup_req = cpu ? BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7 :
 154                                 BM_CPU_PGC_SW_PDN_PUP_REQ_CORE0_A7;
 155         u32 val;
 156
 157         imx_gpcv2_set_m_core_pgc(true, pgc);
 158
 159         val = readl(GPC_IPS_BASE_ADDR + reg);
 160         val |= pdn_pup_req;
 161         writel(val, GPC_IPS_BASE_ADDR + reg);
 162
 163         while ((readl(GPC_IPS_BASE_ADDR + reg) & pdn_pup_req) != 0)
 164                 ;
 165
 166         imx_gpcv2_set_m_core_pgc(false, pgc);
 167 }
 168
 169 __secure void imx_enable_cpu_ca7(int cpu, bool enable)
 170 {
 171         u32 mask, val;
 172
 173         mask = 1 << (BP_SRC_A7RCR1_A7_CORE1_ENABLE + cpu - 1);
 174         val = readl(SRC_BASE_ADDR + SRC_A7RCR1);
 175         val = enable ? val | mask : val & ~mask;
 176         writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
 177 }
 178
 179 __secure void psci_arch_cpu_entry(void)
 180 {
 181         u32 cpu = psci_get_cpu_id();
 182
 183         psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);
 184 }
 185
 186 __secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr, u32 ep,
 187                          u32 context_id)
 188 {
 189         u32 cpu = mpidr & MPIDR_AFF0;
 190
 191         if (mpidr & ~MPIDR_AFF0)
 192                 return ARM_PSCI_RET_INVAL;
 193
 194         if (cpu >= IMX7D_PSCI_NR_CPUS)
 195                 return ARM_PSCI_RET_INVAL;
 196
 197         if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON)
 198                 return ARM_PSCI_RET_ALREADY_ON;
 199
 200         if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON_PENDING)
 201                 return ARM_PSCI_RET_ON_PENDING;
 202
 203         psci_save(cpu, ep, context_id);
 204
 205         writel((u32)psci_cpu_entry, imx_cpu_gpr_entry_offset(cpu));
 206
 207         psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);
 208
 209         imx_gpcv2_set_core_power(cpu, true);
 210         imx_enable_cpu_ca7(cpu, true);
 211
 212         return ARM_PSCI_RET_SUCCESS;
 213 }
 214
 215 __secure s32 psci_cpu_off(void)
 216 {
 217         int cpu;
 218
 219         cpu = psci_get_cpu_id();
 220
 221         psci_cpu_off_common();
 222         psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
 223
 224         imx_enable_cpu_ca7(cpu, false);
 225         imx_gpcv2_set_core_power(cpu, false);
 226         /*
 227          * We use the cpu jumping argument register to sync with
 228          * psci_affinity_info() which is running on cpu0 to kill the cpu.
 229          */
 230         writel(IMX_CPU_SYNC_OFF, imx_cpu_gpr_para_offset(cpu));
 231
 232         while (1)
 233                 wfi();
 234 }
 235
 236 __secure void psci_system_reset(void)
 237 {
 238         struct wdog_regs *wdog = (struct wdog_regs *)WDOG1_BASE_ADDR;
 239
 240         /* make sure WDOG1 clock is enabled */
 241         writel(0x1 << 28, CCM_BASE_ADDR + CCM_ROOT_WDOG);
 242         writel(0x3, CCM_BASE_ADDR + CCM_CCGR_WDOG1);
 243         writew(WCR_WDE, &wdog->wcr);
 244
 245         while (1)
 246                 wfi();
 247 }
 248
 249 __secure void psci_system_off(void)
 250 {
 251         u32 val;
 252
 253         /* make sure SNVS clock is enabled */
 254         writel(0x3, CCM_BASE_ADDR + CCM_CCGR_SNVS);
 255
 256         val = readl(SNVS_BASE_ADDR + SNVS_LPCR);
 257         val |= BP_SNVS_LPCR_DP_EN | BP_SNVS_LPCR_TOP;
 258         writel(val, SNVS_BASE_ADDR + SNVS_LPCR);
 259
 260         while (1)
 261                 wfi();
 262 }
 263
 264 __secure u32 psci_version(void)
 265 {
 266         return ARM_PSCI_VER_1_0;
 267 }
 268
 269 __secure s32 psci_cpu_suspend(u32 __always_unused function_id, u32 power_state,
 270                               u32 entry_point_address,
 271                               u32 context_id)
 272 {
 273         return ARM_PSCI_RET_INVAL;
 274 }
 275
 276 __secure s32 psci_affinity_info(u32 __always_unused function_id,
 277                                 u32 target_affinity,
 278                                 u32 lowest_affinity_level)
 279 {
 280         u32 cpu = target_affinity & MPIDR_AFF0;
 281
 282         if (lowest_affinity_level > 0)
 283                 return ARM_PSCI_RET_INVAL;
 284
 285         if (target_affinity & ~MPIDR_AFF0)
 286                 return ARM_PSCI_RET_INVAL;
 287
 288         if (cpu >= IMX7D_PSCI_NR_CPUS)
 289                 return ARM_PSCI_RET_INVAL;
 290
 291         /* CPU is waiting for killed */
 292         if (readl(imx_cpu_gpr_para_offset(cpu)) == IMX_CPU_SYNC_OFF) {
 293                 imx_enable_cpu_ca7(cpu, false);
 294                 imx_gpcv2_set_core_power(cpu, false);
 295                 writel(IMX_CPU_SYNC_ON, imx_cpu_gpr_para_offset(cpu));
 296         }
 297
 298         return psci_state[cpu];
 299 }
 300
 301 __secure s32 psci_migrate_info_type(u32 function_id)
 302 {
 303         /* Trusted OS is either not present or does not require migration */
 304         return 2;
 305 }
 306
 307 __secure s32 psci_features(u32 __always_unused function_id, u32 psci_fid)
 308 {
 309         switch (psci_fid) {
 310         case ARM_PSCI_0_2_FN_PSCI_VERSION:
 311         case ARM_PSCI_0_2_FN_CPU_OFF:
 312         case ARM_PSCI_0_2_FN_CPU_ON:
 313         case ARM_PSCI_0_2_FN_AFFINITY_INFO:
 314         case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
 315         case ARM_PSCI_0_2_FN_SYSTEM_OFF:
 316         case ARM_PSCI_0_2_FN_SYSTEM_RESET:
 317         case ARM_PSCI_1_0_FN_PSCI_FEATURES:
 318         case ARM_PSCI_1_0_FN_SYSTEM_SUSPEND:
 319                 return 0x0;
 320         }
 321         return ARM_PSCI_RET_NI;
 322 }
 323
 324 static __secure void imx_gpcv2_set_lpm_mode(enum mxc_cpu_pwr_mode mode)
 325 {
 326         u32 val1, val2, val3;
 327
 328         val1 = readl(GPC_IPS_BASE_ADDR + GPC_LPCR_A7_BSC);
 329         val2 = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR);
 330
 331         /* all cores' LPM settings must be same */
 332         val1 &= ~(BM_LPCR_A7_BSC_LPM0 | BM_LPCR_A7_BSC_LPM1);
 333         val1 |= BM_LPCR_A7_BSC_CPU_CLK_ON_LPM;
 334
 335         val2 &= ~(BM_SLPCR_EN_DSM | BM_SLPCR_VSTBY | BM_SLPCR_RBC_EN |
 336                 BM_SLPCR_SBYOS | BM_SLPCR_BYPASS_PMIC_READY);
 337         /*
 338          * GPC: When improper low-power sequence is used,
 339          * the SoC enters low power mode before the ARM core executes WFI.
 340          *
 341          * Software workaround:
 342          * 1) Software should trigger IRQ #32 (IOMUX) to be always pending
 343          *    by setting IOMUX_GPR1_IRQ.
 344          * 2) Software should then unmask IRQ #32 in GPC before setting GPC
 345          *    Low-Power mode.
 346          * 3) Software should mask IRQ #32 right after GPC Low-Power mode
 347          *    is set.
 348          */
 349         switch (mode) {
 350         case RUN:
 351                 val3 = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0);
 352                 val3 &= ~0x1;
 353                 writel(val3, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0);
 354                 break;
 355         case WAIT:
 356                 val1 |= A7_LPM_WAIT << BP_LPCR_A7_BSC_LPM0;
 357                 val1 &= ~BM_LPCR_A7_BSC_CPU_CLK_ON_LPM;
 358                 val3 = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0);
 359                 val3 &= ~0x1;
 360                 writel(val3, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0);
 361                 break;
 362         case STOP:
 363                 val1 |= A7_LPM_STOP << BP_LPCR_A7_BSC_LPM0;
 364                 val1 &= ~BM_LPCR_A7_BSC_CPU_CLK_ON_LPM;
 365                 val2 |= BM_SLPCR_EN_DSM;
 366                 val2 |= BM_SLPCR_SBYOS;
 367                 val2 |= BM_SLPCR_VSTBY;
 368                 val2 |= BM_SLPCR_BYPASS_PMIC_READY;
 369                 val3 = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0);
 370                 val3 |= 0x1;
 371                 writel(val3, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0);
 372                 break;
 373         default:
 374                 return;
 375         }
 376         writel(val1, GPC_IPS_BASE_ADDR + GPC_LPCR_A7_BSC);
 377         writel(val2, GPC_IPS_BASE_ADDR + GPC_SLPCR);
 378 }
 379
 380 static __secure void imx_gpcv2_set_plat_power_gate_by_lpm(bool pdn)
 381 {
 382         u32 val = readl(GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD);
 383
 384         val &= ~(BM_LPCR_A7_AD_EN_PLAT_PDN | BM_LPCR_A7_AD_L2PGE);
 385         if (pdn)
 386                 val |= BM_LPCR_A7_AD_EN_PLAT_PDN | BM_LPCR_A7_AD_L2PGE;
 387
 388         writel(val, GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD);
 389 }
 390
 391 static __secure void imx_gpcv2_set_cpu_power_gate_by_lpm(u32 cpu, bool pdn)
 392 {
 393         u32 val;
 394
 395         val = readl(GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD);
 396         if (cpu == 0) {
 397                 if (pdn)
 398                         val |= BM_LPCR_A7_AD_EN_C0_PDN |
 399                                 BM_LPCR_A7_AD_EN_C0_PUP;
 400                 else
 401                         val &= ~(BM_LPCR_A7_AD_EN_C0_PDN |
 402                                 BM_LPCR_A7_AD_EN_C0_PUP);
 403         }
 404         if (cpu == 1) {
 405                 if (pdn)
 406                         val |= BM_LPCR_A7_AD_EN_C1_PDN |
 407                                 BM_LPCR_A7_AD_EN_C1_PUP;
 408                 else
 409                         val &= ~(BM_LPCR_A7_AD_EN_C1_PDN |
 410                                 BM_LPCR_A7_AD_EN_C1_PUP);
 411         }
 412         writel(val, GPC_IPS_BASE_ADDR + GPC_LPCR_A7_AD);
 413 }
 414
 415 static __secure void imx_gpcv2_set_slot_ack(u32 index, enum imx_gpc_slot m_core,
 416                                             bool mode, bool ack)
 417 {
 418         u32 val;
 419
 420         if (index >= MAX_SLOT_NUMBER)
 421                 return;
 422
 423         /* set slot */
 424         writel(readl(GPC_IPS_BASE_ADDR + GPC_SLOT0_CFG + index * 4) |
 425                 ((mode + 1) << (m_core * 2)),
 426                 GPC_IPS_BASE_ADDR + GPC_SLOT0_CFG + index * 4);
 427
 428         if (ack) {
 429                 /* set ack */
 430                 val = readl(GPC_IPS_BASE_ADDR + GPC_PGC_ACK_SEL_A7);
 431                 /* clear dummy ack */
 432                 val &= ~(mode ? BM_GPC_PGC_ACK_SEL_A7_PU_DUMMY_ACK :
 433                         BM_GPC_PGC_ACK_SEL_A7_PD_DUMMY_ACK);
 434                 val |= 1 << (m_core + (mode ? 16 : 0));
 435                 writel(val, GPC_IPS_BASE_ADDR + GPC_PGC_ACK_SEL_A7);
 436         }
 437 }
 438
 439 static __secure void imx_system_counter_resume(void)
 440 {
 441         u32 val;
 442
 443         val = readl(SYSCNT_CTRL_IPS_BASE_ADDR);
 444         val &= ~BM_SYS_COUNTER_CNTCR_FCR1;
 445         val |= BM_SYS_COUNTER_CNTCR_FCR0;
 446         writel(val, SYSCNT_CTRL_IPS_BASE_ADDR);
 447 }
 448
 449 static __secure void imx_system_counter_suspend(void)
 450 {
 451         u32 val;
 452
 453         val = readl(SYSCNT_CTRL_IPS_BASE_ADDR);
 454         val &= ~BM_SYS_COUNTER_CNTCR_FCR0;
 455         val |= BM_SYS_COUNTER_CNTCR_FCR1;
 456         writel(val, SYSCNT_CTRL_IPS_BASE_ADDR);
 457 }
 458
 459 static __secure void gic_resume(void)
 460 {
 461         u32 itlinesnr, i;
 462         u32 gic_dist_addr = GIC400_ARB_BASE_ADDR + GIC_DIST_OFFSET;
 463
 464         /* enable the GIC distributor */
 465         writel(readl(gic_dist_addr + GICD_CTLR) | 0x03,
 466                gic_dist_addr + GICD_CTLR);
 467
 468         /* TYPER[4:0] contains an encoded number of available interrupts */
 469         itlinesnr = readl(gic_dist_addr + GICD_TYPER) & 0x1f;
 470
 471         /* set all bits in the GIC group registers to one to allow access
 472          * from non-secure state. The first 32 interrupts are private per
 473          * CPU and will be set later when enabling the GIC for each core
 474          */
 475         for (i = 1; i <= itlinesnr; i++)
 476                 writel((u32)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i);
 477 }
 478
 479 static inline void imx_pll_suspend(void)
 480 {
 481         writel(BM_ANATOP_ARM_PLL_OVERRIDE,
 482                ANATOP_BASE_ADDR + ANADIG_ARM_PLL + REG_SET);
 483         writel(BM_ANATOP_DDR_PLL_OVERRIDE,
 484                ANATOP_BASE_ADDR + ANADIG_DDR_PLL + REG_SET);
 485         writel(BM_ANATOP_SYS_PLL_OVERRIDE,
 486                ANATOP_BASE_ADDR + ANADIG_SYS_PLL + REG_SET);
 487         writel(BM_ANATOP_ENET_PLL_OVERRIDE,
 488                ANATOP_BASE_ADDR + ANADIG_ENET_PLL + REG_SET);
 489         writel(BM_ANATOP_AUDIO_PLL_OVERRIDE,
 490                ANATOP_BASE_ADDR + ANADIG_AUDIO_PLL + REG_SET);
 491         writel(BM_ANATOP_VIDEO_PLL_OVERRIDE,
 492                ANATOP_BASE_ADDR + ANADIG_VIDEO_PLL + REG_SET);
 493 }
 494
 495 static inline void imx_pll_resume(void)
 496 {
 497         writel(BM_ANATOP_ARM_PLL_OVERRIDE,
 498                ANATOP_BASE_ADDR + ANADIG_ARM_PLL + REG_CLR);
 499         writel(BM_ANATOP_DDR_PLL_OVERRIDE,
 500                ANATOP_BASE_ADDR + ANADIG_DDR_PLL + REG_CLR);
 501         writel(BM_ANATOP_SYS_PLL_OVERRIDE,
 502                ANATOP_BASE_ADDR + ANADIG_SYS_PLL + REG_CLR);
 503         writel(BM_ANATOP_ENET_PLL_OVERRIDE,
 504                ANATOP_BASE_ADDR + ANADIG_ENET_PLL + REG_CLR);
 505         writel(BM_ANATOP_AUDIO_PLL_OVERRIDE,
 506                ANATOP_BASE_ADDR + ANADIG_AUDIO_PLL + REG_CLR);
 507         writel(BM_ANATOP_VIDEO_PLL_OVERRIDE,
 508                ANATOP_BASE_ADDR + ANADIG_VIDEO_PLL + REG_CLR);
 509 }
 510
 511 static inline void imx_udelay(u32 usec)
 512 {
 513         u32 freq;
 514         u64 start, end;
 515
 516         asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (freq));
 517         asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (start));
 518         do {
 519                 asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (end));
 520                 if ((end - start) > usec * (freq / 1000000))
 521                         break;
 522         } while (1);
 523 }
 524
 525 static inline void imx_ddrc_enter_self_refresh(void)
 526 {
 527         writel(0, DDRC_IPS_BASE_ADDR + DDRC_PWRCTL);
 528         while (readl(DDRC_IPS_BASE_ADDR + DDRC_PSTAT) & 0x10001)
 529                 ;
 530
 531         writel(0x20, DDRC_IPS_BASE_ADDR + DDRC_PWRCTL);
 532         while ((readl(DDRC_IPS_BASE_ADDR + DDRC_STAT) & 0x23) != 0x23)
 533                 ;
 534         writel(readl(DDRC_IPS_BASE_ADDR + DDRC_PWRCTL) | 0x8,
 535                DDRC_IPS_BASE_ADDR + DDRC_PWRCTL);
 536 }
 537
 538 static inline void imx_ddrc_exit_self_refresh(void)
 539 {
 540         writel(0, DDRC_IPS_BASE_ADDR + DDRC_PWRCTL);
 541         while ((readl(DDRC_IPS_BASE_ADDR + DDRC_STAT) & 0x3) == 0x3)
 542                 ;
 543         writel(readl(DDRC_IPS_BASE_ADDR + DDRC_PWRCTL) | 0x1,
 544                DDRC_IPS_BASE_ADDR + DDRC_PWRCTL);
 545 }
 546
 547 __secure void imx_system_resume(void)
 548 {
 549         unsigned int i, val, imr[4], entry;
 550
 551         entry = psci_get_target_pc(0);
 552         imx_ddrc_exit_self_refresh();
 553         imx_system_counter_resume();
 554         imx_gpcv2_set_lpm_mode(RUN);
 555         imx_gpcv2_set_cpu_power_gate_by_lpm(0, false);
 556         imx_gpcv2_set_plat_power_gate_by_lpm(false);
 557         imx_gpcv2_set_m_core_pgc(false, GPC_PGC_C0);
 558         imx_gpcv2_set_m_core_pgc(false, GPC_PGC_SCU);
 559
 560         /*
 561          * need to mask all interrupts in GPC before
 562          * operating RBC configurations
 563          */
 564         for (i = 0; i < 4; i++) {
 565                 imr[i] = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4);
 566                 writel(~0, GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4);
 567         }
 568
 569         /* configure RBC enable bit */
 570         val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR);
 571         val &= ~BM_SLPCR_RBC_EN;
 572         writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR);
 573
 574         /* configure RBC count */
 575         val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR);
 576         val &= ~BM_SLPCR_REG_BYPASS_COUNT;
 577         writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR);
 578
 579         /*
 580          * need to delay at least 2 cycles of CKIL(32K)
 581          * due to hardware design requirement, which is
 582          * ~61us, here we use 65us for safe
 583          */
 584         imx_udelay(65);
 585
 586         /* restore GPC interrupt mask settings */
 587         for (i = 0; i < 4; i++)
 588                 writel(imr[i], GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4);
 589
 590         /* initialize gic distributor */
 591         gic_resume();
 592         _nonsec_init();
 593
 594         /* save cpu0 entry */
 595         psci_save(0, entry, 0);
 596         psci_cpu_entry();
 597 }
 598
 599 __secure void psci_system_suspend(u32 __always_unused function_id,
 600                                   u32 ep, u32 context_id)
 601 {
 602         u32 gpc_mask[4];
 603         u32 i, val;
 604
 605         psci_save(0, ep, context_id);
 606         /* overwrite PLL to be controlled by low power mode */
 607         imx_pll_suspend();
 608         imx_system_counter_suspend();
 609         /* set CA7 platform to enter STOP mode */
 610         imx_gpcv2_set_lpm_mode(STOP);
 611         /* enable core0/scu power down/up with low power mode */
 612         imx_gpcv2_set_cpu_power_gate_by_lpm(0, true);
 613         imx_gpcv2_set_plat_power_gate_by_lpm(true);
 614         /* time slot settings for core0 and scu */
 615         imx_gpcv2_set_slot_ack(0, CORE0_A7, false, false);
 616         imx_gpcv2_set_slot_ack(1, SCU_A7, false, true);
 617         imx_gpcv2_set_slot_ack(5, SCU_A7, true, false);
 618         imx_gpcv2_set_slot_ack(6, CORE0_A7, true, true);
 619         imx_gpcv2_set_m_core_pgc(true, GPC_PGC_C0);
 620         imx_gpcv2_set_m_core_pgc(true, GPC_PGC_SCU);
 621         psci_v7_flush_dcache_all();
 622
 623         imx_ddrc_enter_self_refresh();
 624
 625         /*
 626          * e10133: ARM: Boot failure after A7 enters into
 627          * low-power idle mode
 628          *
 629          * Workaround:
 630          * If both CPU0/CPU1 are IDLE, the last IDLE CPU should
 631          * disable GIC first, then REG_BYPASS_COUNTER is used
 632          * to mask wakeup INT, and then execute “wfi” is used to
 633          * bring the system into power down processing safely.
 634          * The counter must be enabled as close to the “wfi” state
 635          * as possible. The following equation can be used to
 636          * determine the RBC counter value:
 637          * RBC_COUNT * (1/32K RTC frequency) >=
 638          * (46 + PDNSCR_SW + PDNSCR_SW2ISO ) ( 1/IPG_CLK frequency ).
 639          */
 640
 641         /* disable GIC distributor */
 642         writel(0, GIC400_ARB_BASE_ADDR + GIC_DIST_OFFSET);
 643
 644         for (i = 0; i < 4; i++)
 645                 gpc_mask[i] = readl(GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4);
 646
 647         /*
 648          * enable the RBC bypass counter here
 649          * to hold off the interrupts. RBC counter
 650          * = 8 (240us). With this setting, the latency
 651          * from wakeup interrupt to ARM power up
 652          * is ~250uS.
 653          */
 654         val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR);
 655         val &= ~(0x3f << 24);
 656         val |= (0x8 << 24);
 657         writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR);
 658
 659         /* enable the counter. */
 660         val = readl(GPC_IPS_BASE_ADDR + GPC_SLPCR);
 661         val |= (1 << 30);
 662         writel(val, GPC_IPS_BASE_ADDR + GPC_SLPCR);
 663
 664         /* unmask all the GPC interrupts. */
 665         for (i = 0; i < 4; i++)
 666                 writel(gpc_mask[i], GPC_IPS_BASE_ADDR + GPC_IMR1_CORE0 + i * 4);
 667
 668         /*
 669          * now delay for a short while (3usec)
 670          * ARM is at 1GHz at this point
 671          * so a short loop should be enough.
 672          * this delay is required to ensure that
 673          * the RBC counter can start counting in
 674          * case an interrupt is already pending
 675          * or in case an interrupt arrives just
 676          * as ARM is about to assert DSM_request.
 677          */
 678         imx_udelay(3);
 679
 680         /* save resume entry and sp in CPU0 GPR registers */
 681         asm volatile("mov %0, sp" : "=r" (val));
 682         writel((u32)psci_system_resume, SRC_BASE_ADDR + SRC_GPR1_MX7D);
 683         writel(val, SRC_BASE_ADDR + SRC_GPR2_MX7D);
 684
 685         /* sleep */
 686         while (1)
 687                 wfi();
 688 }