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
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
|
/*
* Power Management Service Unit(PMSU) support for Armada 370/XP platforms.
*
* Copyright (C) 2012 Marvell
*
* Yehuda Yitschak <yehuday@marvell.com>
* Gregory Clement <gregory.clement@free-electrons.com>
* Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
*
* This file is licensed under the terms of the GNU General Public
* License version 2. This program is licensed "as is" without any
* warranty of any kind, whether express or implied.
*
* The Armada 370 and Armada XP SOCs have a power management service
* unit which is responsible for powering down and waking up CPUs and
* other SOC units
*/
#define pr_fmt(fmt) "mvebu-pmsu: " fmt
#include <linux/cpu_pm.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/of_address.h>
#include <linux/io.h>
#include <linux/platform_device.h>
#include <linux/smp.h>
#include <linux/resource.h>
#include <asm/cacheflush.h>
#include <asm/cp15.h>
#include <asm/smp_plat.h>
#include <asm/suspend.h>
#include <asm/tlbflush.h>
#include "common.h"
static void __iomem *pmsu_mp_base;
#define PMSU_BASE_OFFSET 0x100
#define PMSU_REG_SIZE 0x1000
/* PMSU MP registers */
#define PMSU_CONTROL_AND_CONFIG(cpu) ((cpu * 0x100) + 0x104)
#define PMSU_CONTROL_AND_CONFIG_DFS_REQ BIT(18)
#define PMSU_CONTROL_AND_CONFIG_PWDDN_REQ BIT(16)
#define PMSU_CONTROL_AND_CONFIG_L2_PWDDN BIT(20)
#define PMSU_CPU_POWER_DOWN_CONTROL(cpu) ((cpu * 0x100) + 0x108)
#define PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP BIT(0)
#define PMSU_STATUS_AND_MASK(cpu) ((cpu * 0x100) + 0x10c)
#define PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT BIT(16)
#define PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT BIT(17)
#define PMSU_STATUS_AND_MASK_IRQ_WAKEUP BIT(20)
#define PMSU_STATUS_AND_MASK_FIQ_WAKEUP BIT(21)
#define PMSU_STATUS_AND_MASK_DBG_WAKEUP BIT(22)
#define PMSU_STATUS_AND_MASK_IRQ_MASK BIT(24)
#define PMSU_STATUS_AND_MASK_FIQ_MASK BIT(25)
#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu) ((cpu * 0x100) + 0x124)
/* PMSU fabric registers */
#define L2C_NFABRIC_PM_CTL 0x4
#define L2C_NFABRIC_PM_CTL_PWR_DOWN BIT(20)
extern void ll_disable_coherency(void);
extern void ll_enable_coherency(void);
static struct platform_device armada_xp_cpuidle_device = {
.name = "cpuidle-armada-370-xp",
};
static struct of_device_id of_pmsu_table[] = {
{ .compatible = "marvell,armada-370-pmsu", },
{ .compatible = "marvell,armada-370-xp-pmsu", },
{ .compatible = "marvell,armada-380-pmsu", },
{ /* end of list */ },
};
void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr)
{
writel(virt_to_phys(boot_addr), pmsu_mp_base +
PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
}
static int __init armada_370_xp_pmsu_init(void)
{
struct device_node *np;
struct resource res;
int ret = 0;
np = of_find_matching_node(NULL, of_pmsu_table);
if (!np)
return 0;
pr_info("Initializing Power Management Service Unit\n");
if (of_address_to_resource(np, 0, &res)) {
pr_err("unable to get resource\n");
ret = -ENOENT;
goto out;
}
if (of_device_is_compatible(np, "marvell,armada-370-xp-pmsu")) {
pr_warn(FW_WARN "deprecated pmsu binding\n");
res.start = res.start - PMSU_BASE_OFFSET;
res.end = res.start + PMSU_REG_SIZE - 1;
}
if (!request_mem_region(res.start, resource_size(&res),
np->full_name)) {
pr_err("unable to request region\n");
ret = -EBUSY;
goto out;
}
pmsu_mp_base = ioremap(res.start, resource_size(&res));
if (!pmsu_mp_base) {
pr_err("unable to map registers\n");
release_mem_region(res.start, resource_size(&res));
ret = -ENOMEM;
goto out;
}
out:
of_node_put(np);
return ret;
}
static void armada_370_xp_pmsu_enable_l2_powerdown_onidle(void)
{
u32 reg;
if (pmsu_mp_base == NULL)
return;
/* Enable L2 & Fabric powerdown in Deep-Idle mode - Fabric */
reg = readl(pmsu_mp_base + L2C_NFABRIC_PM_CTL);
reg |= L2C_NFABRIC_PM_CTL_PWR_DOWN;
writel(reg, pmsu_mp_base + L2C_NFABRIC_PM_CTL);
}
static void armada_370_xp_cpu_resume(void)
{
asm volatile("bl ll_add_cpu_to_smp_group\n\t"
"bl ll_enable_coherency\n\t"
"b cpu_resume\n\t");
}
/* No locking is needed because we only access per-CPU registers */
void armada_370_xp_pmsu_idle_prepare(bool deepidle)
{
unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
u32 reg;
if (pmsu_mp_base == NULL)
return;
/*
* Adjust the PMSU configuration to wait for WFI signal, enable
* IRQ and FIQ as wakeup events, set wait for snoop queue empty
* indication and mask IRQ and FIQ from CPU
*/
reg = readl(pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
reg |= PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT |
PMSU_STATUS_AND_MASK_IRQ_WAKEUP |
PMSU_STATUS_AND_MASK_FIQ_WAKEUP |
PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT |
PMSU_STATUS_AND_MASK_IRQ_MASK |
PMSU_STATUS_AND_MASK_FIQ_MASK;
writel(reg, pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
reg = readl(pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
/* ask HW to power down the L2 Cache if needed */
if (deepidle)
reg |= PMSU_CONTROL_AND_CONFIG_L2_PWDDN;
/* request power down */
reg |= PMSU_CONTROL_AND_CONFIG_PWDDN_REQ;
writel(reg, pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
/* Disable snoop disable by HW - SW is taking care of it */
reg = readl(pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
reg |= PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP;
writel(reg, pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
}
static noinline int do_armada_370_xp_cpu_suspend(unsigned long deepidle)
{
armada_370_xp_pmsu_idle_prepare(deepidle);
v7_exit_coherency_flush(all);
ll_disable_coherency();
dsb();
wfi();
/* If we are here, wfi failed. As processors run out of
* coherency for some time, tlbs might be stale, so flush them
*/
local_flush_tlb_all();
ll_enable_coherency();
/* Test the CR_C bit and set it if it was cleared */
asm volatile(
"mrc p15, 0, %0, c1, c0, 0 \n\t"
"tst %0, #(1 << 2) \n\t"
"orreq %0, %0, #(1 << 2) \n\t"
"mcreq p15, 0, %0, c1, c0, 0 \n\t"
"isb "
: : "r" (0));
pr_warn("Failed to suspend the system\n");
return 0;
}
static int armada_370_xp_cpu_suspend(unsigned long deepidle)
{
return cpu_suspend(deepidle, do_armada_370_xp_cpu_suspend);
}
/* No locking is needed because we only access per-CPU registers */
static noinline void armada_370_xp_pmsu_idle_restore(void)
{
unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
u32 reg;
if (pmsu_mp_base == NULL)
return;
/* cancel ask HW to power down the L2 Cache if possible */
reg = readl(pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
reg &= ~PMSU_CONTROL_AND_CONFIG_L2_PWDDN;
writel(reg, pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
/* cancel Enable wakeup events and mask interrupts */
reg = readl(pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
reg &= ~(PMSU_STATUS_AND_MASK_IRQ_WAKEUP | PMSU_STATUS_AND_MASK_FIQ_WAKEUP);
reg &= ~PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT;
reg &= ~PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT;
reg &= ~(PMSU_STATUS_AND_MASK_IRQ_MASK | PMSU_STATUS_AND_MASK_FIQ_MASK);
writel(reg, pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
}
static int armada_370_xp_cpu_pm_notify(struct notifier_block *self,
unsigned long action, void *hcpu)
{
if (action == CPU_PM_ENTER) {
unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
mvebu_pmsu_set_cpu_boot_addr(hw_cpu, armada_370_xp_cpu_resume);
} else if (action == CPU_PM_EXIT) {
armada_370_xp_pmsu_idle_restore();
}
return NOTIFY_OK;
}
static struct notifier_block armada_370_xp_cpu_pm_notifier = {
.notifier_call = armada_370_xp_cpu_pm_notify,
};
int __init armada_370_xp_cpu_pm_init(void)
{
struct device_node *np;
/*
* Check that all the requirements are available to enable
* cpuidle. So far, it is only supported on Armada XP, cpuidle
* needs the coherency fabric and the PMSU enabled
*/
if (!of_machine_is_compatible("marvell,armadaxp"))
return 0;
np = of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric");
if (!np)
return 0;
of_node_put(np);
np = of_find_matching_node(NULL, of_pmsu_table);
if (!np)
return 0;
of_node_put(np);
armada_370_xp_pmsu_enable_l2_powerdown_onidle();
armada_xp_cpuidle_device.dev.platform_data = armada_370_xp_cpu_suspend;
platform_device_register(&armada_xp_cpuidle_device);
cpu_pm_register_notifier(&armada_370_xp_cpu_pm_notifier);
return 0;
}
arch_initcall(armada_370_xp_cpu_pm_init);
early_initcall(armada_370_xp_pmsu_init);
|
