1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright 2018 NXP
4 *
5 * Peng Fan <peng.fan@nxp.com>
6 */
7
8 #include <arm.h>
9 #include <arm32.h>
10 #include <atomic.h>
11 #include <console.h>
12 #include <drivers/imx_uart.h>
13 #include <io.h>
14 #include <imx.h>
15 #include <imx_pm.h>
16 #include <kernel/cache_helpers.h>
17 #include <kernel/boot.h>
18 #include <kernel/interrupt.h>
19 #include <kernel/misc.h>
20 #include <kernel/panic.h>
21 #include <kernel/spinlock.h>
22 #include <mm/core_mmu.h>
23 #include <mm/core_memprot.h>
24 #include <platform_config.h>
25 #include <stdint.h>
26 #include <sm/optee_smc.h>
27 #include <sm/psci.h>
28 #include <sm/pm.h>
29 #include <sm/sm.h>
30 #include <string.h>
31 #include <tee/entry_std.h>
32 #include <tee/entry_fast.h>
33 #include <util.h>
34
imx7d_cpuidle_init(void)35 int imx7d_cpuidle_init(void)
36 {
37 uint32_t lpm_idle_ocram_base =
38 core_mmu_get_va(TRUSTZONE_OCRAM_START +
39 LOWPOWER_IDLE_OCRAM_OFFSET,
40 MEM_AREA_TEE_COHERENT,
41 sizeof(struct imx7_pm_info));
42 struct imx7_pm_info *p =
43 (struct imx7_pm_info *)lpm_idle_ocram_base;
44
45 pm_imx7_iram_tbl_init();
46
47 dcache_op_level1(DCACHE_OP_CLEAN_INV);
48
49 p->va_base = lpm_idle_ocram_base;
50 p->pa_base = TRUSTZONE_OCRAM_START + LOWPOWER_IDLE_OCRAM_OFFSET;
51 p->tee_resume = (paddr_t)virt_to_phys((void *)(vaddr_t)v7_cpu_resume);
52 p->pm_info_size = sizeof(*p);
53 p->ddrc_va_base = core_mmu_get_va(DDRC_BASE, MEM_AREA_IO_SEC, 1);
54 p->ddrc_pa_base = DDRC_BASE;
55 p->ccm_va_base = core_mmu_get_va(CCM_BASE, MEM_AREA_IO_SEC, 1);
56 p->ccm_pa_base = CCM_BASE;
57 p->anatop_va_base = core_mmu_get_va(ANATOP_BASE, MEM_AREA_IO_SEC, 1);
58 p->anatop_pa_base = ANATOP_BASE;
59 p->src_va_base = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC, 1);
60 p->src_pa_base = SRC_BASE;
61 p->iomuxc_gpr_va_base = core_mmu_get_va(IOMUXC_GPR_BASE,
62 MEM_AREA_IO_SEC, 1);
63 p->iomuxc_gpr_pa_base = IOMUXC_GPR_BASE;
64 p->gpc_va_base = core_mmu_get_va(GPC_BASE, MEM_AREA_IO_SEC, 1);
65 p->gpc_pa_base = GPC_BASE;
66 p->gic_va_base = core_mmu_get_va(GIC_BASE, MEM_AREA_IO_SEC, 1);
67 p->gic_pa_base = GIC_BASE;
68
69 p->num_lpi_cpus = 0;
70 p->num_online_cpus = -1;
71
72 memcpy((void *)(lpm_idle_ocram_base + sizeof(*p)),
73 (void *)(vaddr_t)imx7d_low_power_idle,
74 LOWPOWER_IDLE_OCRAM_SIZE - sizeof(*p));
75
76 dcache_clean_range((void *)lpm_idle_ocram_base,
77 LOWPOWER_IDLE_OCRAM_SIZE);
78 /*
79 * Note that IRAM IOSEC map, if changed to MEM map,
80 * need to flush cache
81 */
82 icache_inv_all();
83
84 return 0;
85 }
86
87 static int lowpoweridle_init;
88
imx_pen_lock(uint32_t cpu)89 static void imx_pen_lock(uint32_t cpu)
90 {
91 uint32_t cpuidle_ocram_base;
92 struct imx7_pm_info *p;
93
94 cpuidle_ocram_base = core_mmu_get_va(TRUSTZONE_OCRAM_START +
95 LOWPOWER_IDLE_OCRAM_OFFSET,
96 MEM_AREA_TEE_COHERENT,
97 sizeof(struct imx7_pm_info));
98 p = (struct imx7_pm_info *)cpuidle_ocram_base;
99
100 if (cpu == 0) {
101 atomic_store_u32(&p->flag0, 1);
102 dsb();
103 atomic_store_u32(&p->val, cpu);
104 do {
105 dsb();
106 } while (atomic_load_u32(&p->flag1) == 1
107 && atomic_load_u32(&p->val) == cpu)
108 ;
109 } else {
110 atomic_store_u32(&p->flag1, 1);
111 dsb();
112 atomic_store_u32(&p->val, cpu);
113 do {
114 dsb();
115 } while (atomic_load_u32(&p->flag0) == 1
116 && atomic_load_u32(&p->val) == cpu)
117 ;
118 }
119 }
120
imx_pen_unlock(int cpu)121 static void imx_pen_unlock(int cpu)
122 {
123 uint32_t cpuidle_ocram_base;
124 struct imx7_pm_info *p;
125
126 cpuidle_ocram_base = core_mmu_get_va(TRUSTZONE_OCRAM_START +
127 LOWPOWER_IDLE_OCRAM_OFFSET,
128 MEM_AREA_TEE_COHERENT,
129 sizeof(struct imx7_pm_info));
130 p = (struct imx7_pm_info *)cpuidle_ocram_base;
131
132 dsb();
133 if (cpu == 0)
134 atomic_store_u32(&p->flag0, 0);
135 else
136 atomic_store_u32(&p->flag1, 0);
137 }
138
get_online_cpus(void)139 static uint32_t get_online_cpus(void)
140 {
141 vaddr_t src_a7rcr1 = core_mmu_get_va(SRC_BASE + SRC_A7RCR1,
142 MEM_AREA_IO_SEC, sizeof(uint32_t));
143 uint32_t val = io_read32(src_a7rcr1);
144
145 return (val & (1 << SRC_A7RCR1_A7_CORE1_ENABLE_OFFSET)) ? 2 : 1;
146 }
147
imx7d_lowpower_idle(uint32_t power_state __unused,uintptr_t entry __unused,uint32_t context_id __unused,struct sm_nsec_ctx * nsec)148 int imx7d_lowpower_idle(uint32_t power_state __unused,
149 uintptr_t entry __unused,
150 uint32_t context_id __unused,
151 struct sm_nsec_ctx *nsec)
152 {
153 struct imx7_pm_info *p;
154 uint32_t cpuidle_ocram_base;
155 static uint32_t gic_inited;
156 int ret;
157
158 uint32_t cpu_id __maybe_unused = get_core_pos();
159 uint32_t type = (power_state & PSCI_POWER_STATE_TYPE_MASK) >>
160 PSCI_POWER_STATE_TYPE_SHIFT;
161 uint32_t cpu = get_core_pos();
162
163 cpuidle_ocram_base = core_mmu_get_va(TRUSTZONE_OCRAM_START +
164 LOWPOWER_IDLE_OCRAM_OFFSET,
165 MEM_AREA_TEE_COHERENT,
166 sizeof(struct imx7_pm_info));
167 p = (struct imx7_pm_info *)cpuidle_ocram_base;
168
169 imx_pen_lock(cpu);
170
171 if (!lowpoweridle_init) {
172 imx7d_cpuidle_init();
173 lowpoweridle_init = 1;
174 }
175
176 if (type != PSCI_POWER_STATE_TYPE_POWER_DOWN)
177 panic();
178
179 p->num_online_cpus = get_online_cpus();
180 p->num_lpi_cpus++;
181
182 sm_save_unbanked_regs(&nsec->ub_regs);
183
184 ret = sm_pm_cpu_suspend((uint32_t)p, (int (*)(uint32_t))
185 (cpuidle_ocram_base + sizeof(*p)));
186
187 /*
188 * Sometimes cpu_suspend may not really suspended, we need to check
189 * it's return value to restore reg or not
190 */
191 if (ret < 0) {
192 p->num_lpi_cpus--;
193 imx_pen_unlock(cpu);
194 DMSG("=== Not suspended, GPC IRQ Pending === %d\n", cpu_id);
195 return 0;
196 }
197
198 /*
199 * Restore register of different mode in secure world
200 * When cpu powers up, after ROM init, cpu in secure SVC
201 * mode, we first need to restore monitor regs.
202 */
203 sm_restore_unbanked_regs(&nsec->ub_regs);
204
205 p->num_lpi_cpus--;
206 /* Back to Linux */
207 nsec->mon_lr = (uint32_t)entry;
208
209 if (gic_inited == 0) {
210 /*
211 * TODO: Call the Wakeup Late function to restore some
212 * HW configuration (e.g. TZASC)
213 */
214 if (!get_core_pos())
215 plat_primary_init_early();
216
217 main_init_gic();
218 gic_inited = 1;
219 DMSG("=== Back from Suspended ===\n");
220 } else {
221 main_secondary_init_gic();
222 gic_inited = 0;
223 }
224
225 imx_pen_unlock(cpu);
226
227 return 0;
228 }
229