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