1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
|
// SPDX-License-Identifier: GPL-2.0+
/*
* Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
* Copyright 2017 NXP
*/
#include <asm/io.h>
#include <asm/psci.h>
#include <asm/secure.h>
#include <asm/arch/imx-regs.h>
#include <linux/bitops.h>
#include <common.h>
#include <fsl_wdog.h>
#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
#define SNVS_LPCR 0x38
#define BP_SNVS_LPCR_DP_EN 0x20
#define BP_SNVS_LPCR_TOP 0x40
#define CCM_CCGR_SNVS 0x4250
#define CCM_ROOT_WDOG 0xbb80
#define CCM_CCGR_WDOG1 0x49c0
#define MPIDR_AFF0 GENMASK(7, 0)
#define IMX7D_PSCI_NR_CPUS 2
#if IMX7D_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
#endif
u8 psci_state[IMX7D_PSCI_NR_CPUS] __secure_data = {
PSCI_AFFINITY_LEVEL_ON,
PSCI_AFFINITY_LEVEL_OFF};
static inline void psci_set_state(int cpu, u8 state)
{
psci_state[cpu] = state;
dsb();
isb();
}
static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
{
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 = readl(GPC_IPS_BASE_ADDR + reg);
val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
writel(val, GPC_IPS_BASE_ADDR + reg);
while ((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 = readl(SRC_BASE_ADDR + SRC_A7RCR1);
val = enable ? val | mask : val & ~mask;
writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
}
__secure void psci_arch_cpu_entry(void)
{
u32 cpu = psci_get_cpu_id();
psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);
}
__secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr, u32 ep,
u32 context_id)
{
u32 cpu = mpidr & MPIDR_AFF0;
if (mpidr & ~MPIDR_AFF0)
return ARM_PSCI_RET_INVAL;
if (cpu >= IMX7D_PSCI_NR_CPUS)
return ARM_PSCI_RET_INVAL;
if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON)
return ARM_PSCI_RET_ALREADY_ON;
if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON_PENDING)
return ARM_PSCI_RET_ON_PENDING;
psci_save(cpu, ep, context_id);
writel((u32)psci_cpu_entry, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);
imx_gpcv2_set_core1_power(true);
imx_enable_cpu_ca7(cpu, true);
return ARM_PSCI_RET_SUCCESS;
}
__secure s32 psci_cpu_off(void)
{
int cpu;
cpu = psci_get_cpu_id();
psci_cpu_off_common();
psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
imx_enable_cpu_ca7(cpu, false);
imx_gpcv2_set_core1_power(false);
writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
while (1)
wfi();
}
__secure void psci_system_reset(void)
{
struct wdog_regs *wdog = (struct wdog_regs *)WDOG1_BASE_ADDR;
/* make sure WDOG1 clock is enabled */
writel(0x1 << 28, CCM_BASE_ADDR + CCM_ROOT_WDOG);
writel(0x3, CCM_BASE_ADDR + CCM_CCGR_WDOG1);
writew(WCR_WDE, &wdog->wcr);
while (1)
wfi();
}
__secure void psci_system_off(void)
{
u32 val;
/* make sure SNVS clock is enabled */
writel(0x3, CCM_BASE_ADDR + CCM_CCGR_SNVS);
val = readl(SNVS_BASE_ADDR + SNVS_LPCR);
val |= BP_SNVS_LPCR_DP_EN | BP_SNVS_LPCR_TOP;
writel(val, SNVS_BASE_ADDR + SNVS_LPCR);
while (1)
wfi();
}
__secure u32 psci_version(void)
{
return ARM_PSCI_VER_1_0;
}
__secure s32 psci_cpu_suspend(u32 __always_unused function_id, u32 power_state,
u32 entry_point_address,
u32 context_id)
{
return ARM_PSCI_RET_INVAL;
}
__secure s32 psci_affinity_info(u32 __always_unused function_id,
u32 target_affinity,
u32 lowest_affinity_level)
{
u32 cpu = target_affinity & MPIDR_AFF0;
if (lowest_affinity_level > 0)
return ARM_PSCI_RET_INVAL;
if (target_affinity & ~MPIDR_AFF0)
return ARM_PSCI_RET_INVAL;
if (cpu >= IMX7D_PSCI_NR_CPUS)
return ARM_PSCI_RET_INVAL;
return psci_state[cpu];
}
__secure s32 psci_features(u32 __always_unused function_id, u32 psci_fid)
{
switch (psci_fid) {
case ARM_PSCI_0_2_FN_PSCI_VERSION:
case ARM_PSCI_0_2_FN_CPU_OFF:
case ARM_PSCI_0_2_FN_CPU_ON:
case ARM_PSCI_0_2_FN_AFFINITY_INFO:
case ARM_PSCI_0_2_FN_SYSTEM_OFF:
case ARM_PSCI_0_2_FN_SYSTEM_RESET:
case ARM_PSCI_1_0_FN_PSCI_FEATURES:
return 0x0;
}
return ARM_PSCI_RET_NI;
}
|
