1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2014 Imagination Technologies
4 * Author: Paul Burton <paul.burton@mips.com>
5 */
6
7 #include <linux/cpuhotplug.h>
8 #include <linux/init.h>
9 #include <linux/percpu.h>
10 #include <linux/slab.h>
11 #include <linux/suspend.h>
12
13 #include <asm/asm-offsets.h>
14 #include <asm/cacheflush.h>
15 #include <asm/cacheops.h>
16 #include <asm/idle.h>
17 #include <asm/mips-cps.h>
18 #include <asm/mipsmtregs.h>
19 #include <asm/pm.h>
20 #include <asm/pm-cps.h>
21 #include <asm/regdef.h>
22 #include <asm/smp-cps.h>
23 #include <asm/uasm.h>
24
25 /*
26 * cps_nc_entry_fn - type of a generated non-coherent state entry function
27 * @online: the count of online coupled VPEs
28 * @nc_ready_count: pointer to a non-coherent mapping of the core ready_count
29 *
30 * The code entering & exiting non-coherent states is generated at runtime
31 * using uasm, in order to ensure that the compiler cannot insert a stray
32 * memory access at an unfortunate time and to allow the generation of optimal
33 * core-specific code particularly for cache routines. If coupled_coherence
34 * is non-zero and this is the entry function for the CPS_PM_NC_WAIT state,
35 * returns the number of VPEs that were in the wait state at the point this
36 * VPE left it. Returns garbage if coupled_coherence is zero or this is not
37 * the entry function for CPS_PM_NC_WAIT.
38 */
39 typedef unsigned (*cps_nc_entry_fn)(unsigned online, u32 *nc_ready_count);
40
41 /*
42 * The entry point of the generated non-coherent idle state entry/exit
43 * functions. Actually per-core rather than per-CPU.
44 */
45 static DEFINE_PER_CPU_READ_MOSTLY(cps_nc_entry_fn[CPS_PM_STATE_COUNT],
46 nc_asm_enter);
47
48 /* Bitmap indicating which states are supported by the system */
49 static DECLARE_BITMAP(state_support, CPS_PM_STATE_COUNT);
50
51 /*
52 * Indicates the number of coupled VPEs ready to operate in a non-coherent
53 * state. Actually per-core rather than per-CPU.
54 */
55 static DEFINE_PER_CPU_ALIGNED(u32*, ready_count);
56
57 /* Indicates online CPUs coupled with the current CPU */
58 static DEFINE_PER_CPU_ALIGNED(cpumask_t, online_coupled);
59
60 /* Used to synchronize entry to deep idle states */
61 static DEFINE_PER_CPU_ALIGNED(atomic_t, pm_barrier);
62
63 /* Saved CPU state across the CPS_PM_POWER_GATED state */
64 DEFINE_PER_CPU_ALIGNED(struct mips_static_suspend_state, cps_cpu_state);
65
66 /* A somewhat arbitrary number of labels & relocs for uasm */
67 static struct uasm_label labels[32];
68 static struct uasm_reloc relocs[32];
69
cps_pm_support_state(enum cps_pm_state state)70 bool cps_pm_support_state(enum cps_pm_state state)
71 {
72 return test_bit(state, state_support);
73 }
74
coupled_barrier(atomic_t * a,unsigned online)75 static void coupled_barrier(atomic_t *a, unsigned online)
76 {
77 /*
78 * This function is effectively the same as
79 * cpuidle_coupled_parallel_barrier, which can't be used here since
80 * there's no cpuidle device.
81 */
82
83 if (!coupled_coherence)
84 return;
85
86 smp_mb__before_atomic();
87 atomic_inc(a);
88
89 while (atomic_read(a) < online)
90 cpu_relax();
91
92 if (atomic_inc_return(a) == online * 2) {
93 atomic_set(a, 0);
94 return;
95 }
96
97 while (atomic_read(a) > online)
98 cpu_relax();
99 }
100
cps_pm_enter_state(enum cps_pm_state state)101 int cps_pm_enter_state(enum cps_pm_state state)
102 {
103 unsigned cpu = smp_processor_id();
104 unsigned core = cpu_core(¤t_cpu_data);
105 unsigned online, left;
106 cpumask_t *coupled_mask = this_cpu_ptr(&online_coupled);
107 u32 *core_ready_count, *nc_core_ready_count;
108 void *nc_addr;
109 cps_nc_entry_fn entry;
110 struct core_boot_config *core_cfg;
111 struct vpe_boot_config *vpe_cfg;
112 atomic_t *barrier;
113
114 /* Check that there is an entry function for this state */
115 entry = per_cpu(nc_asm_enter, cpu)[state];
116 if (!entry)
117 return -EINVAL;
118
119 /* Calculate which coupled CPUs (VPEs) are online */
120 #if defined(CONFIG_MIPS_MT) || defined(CONFIG_CPU_MIPSR6)
121 if (cpu_online(cpu)) {
122 cpumask_and(coupled_mask, cpu_online_mask,
123 &cpu_sibling_map[cpu]);
124 online = cpumask_weight(coupled_mask);
125 cpumask_clear_cpu(cpu, coupled_mask);
126 } else
127 #endif
128 {
129 cpumask_clear(coupled_mask);
130 online = 1;
131 }
132
133 /* Setup the VPE to run mips_cps_pm_restore when started again */
134 if (IS_ENABLED(CONFIG_CPU_PM) && state == CPS_PM_POWER_GATED) {
135 /* Power gating relies upon CPS SMP */
136 if (!mips_cps_smp_in_use())
137 return -EINVAL;
138
139 core_cfg = &mips_cps_core_bootcfg[core];
140 vpe_cfg = &core_cfg->vpe_config[cpu_vpe_id(¤t_cpu_data)];
141 vpe_cfg->pc = (unsigned long)mips_cps_pm_restore;
142 vpe_cfg->gp = (unsigned long)current_thread_info();
143 vpe_cfg->sp = 0;
144 }
145
146 /* Indicate that this CPU might not be coherent */
147 cpumask_clear_cpu(cpu, &cpu_coherent_mask);
148 smp_mb__after_atomic();
149
150 /* Create a non-coherent mapping of the core ready_count */
151 core_ready_count = per_cpu(ready_count, cpu);
152 nc_addr = kmap_noncoherent(virt_to_page(core_ready_count),
153 (unsigned long)core_ready_count);
154 nc_addr += ((unsigned long)core_ready_count & ~PAGE_MASK);
155 nc_core_ready_count = nc_addr;
156
157 /* Ensure ready_count is zero-initialised before the assembly runs */
158 WRITE_ONCE(*nc_core_ready_count, 0);
159 barrier = &per_cpu(pm_barrier, cpumask_first(&cpu_sibling_map[cpu]));
160 coupled_barrier(barrier, online);
161
162 /* Run the generated entry code */
163 left = entry(online, nc_core_ready_count);
164
165 /* Remove the non-coherent mapping of ready_count */
166 kunmap_noncoherent();
167
168 /* Indicate that this CPU is definitely coherent */
169 cpumask_set_cpu(cpu, &cpu_coherent_mask);
170
171 /*
172 * If this VPE is the first to leave the non-coherent wait state then
173 * it needs to wake up any coupled VPEs still running their wait
174 * instruction so that they return to cpuidle, which can then complete
175 * coordination between the coupled VPEs & provide the governor with
176 * a chance to reflect on the length of time the VPEs were in the
177 * idle state.
178 */
179 if (coupled_coherence && (state == CPS_PM_NC_WAIT) && (left == online))
180 arch_send_call_function_ipi_mask(coupled_mask);
181
182 return 0;
183 }
184
cps_gen_cache_routine(u32 ** pp,struct uasm_label ** pl,struct uasm_reloc ** pr,const struct cache_desc * cache,unsigned op,int lbl)185 static void cps_gen_cache_routine(u32 **pp, struct uasm_label **pl,
186 struct uasm_reloc **pr,
187 const struct cache_desc *cache,
188 unsigned op, int lbl)
189 {
190 unsigned cache_size = cache->ways << cache->waybit;
191 unsigned i;
192 const unsigned unroll_lines = 32;
193
194 /* If the cache isn't present this function has it easy */
195 if (cache->flags & MIPS_CACHE_NOT_PRESENT)
196 return;
197
198 /* Load base address */
199 UASM_i_LA(pp, GPR_T0, (long)CKSEG0);
200
201 /* Calculate end address */
202 if (cache_size < 0x8000)
203 uasm_i_addiu(pp, GPR_T1, GPR_T0, cache_size);
204 else
205 UASM_i_LA(pp, GPR_T1, (long)(CKSEG0 + cache_size));
206
207 /* Start of cache op loop */
208 uasm_build_label(pl, *pp, lbl);
209
210 /* Generate the cache ops */
211 for (i = 0; i < unroll_lines; i++) {
212 if (cpu_has_mips_r6) {
213 uasm_i_cache(pp, op, 0, GPR_T0);
214 uasm_i_addiu(pp, GPR_T0, GPR_T0, cache->linesz);
215 } else {
216 uasm_i_cache(pp, op, i * cache->linesz, GPR_T0);
217 }
218 }
219
220 if (!cpu_has_mips_r6)
221 /* Update the base address */
222 uasm_i_addiu(pp, GPR_T0, GPR_T0, unroll_lines * cache->linesz);
223
224 /* Loop if we haven't reached the end address yet */
225 uasm_il_bne(pp, pr, GPR_T0, GPR_T1, lbl);
226 uasm_i_nop(pp);
227 }
228
cps_gen_flush_fsb(u32 ** pp,struct uasm_label ** pl,struct uasm_reloc ** pr,const struct cpuinfo_mips * cpu_info,int lbl)229 static int cps_gen_flush_fsb(u32 **pp, struct uasm_label **pl,
230 struct uasm_reloc **pr,
231 const struct cpuinfo_mips *cpu_info,
232 int lbl)
233 {
234 unsigned i, fsb_size = 8;
235 unsigned num_loads = (fsb_size * 3) / 2;
236 unsigned line_stride = 2;
237 unsigned line_size = cpu_info->dcache.linesz;
238 unsigned perf_counter, perf_event;
239 unsigned revision = cpu_info->processor_id & PRID_REV_MASK;
240
241 /*
242 * Determine whether this CPU requires an FSB flush, and if so which
243 * performance counter/event reflect stalls due to a full FSB.
244 */
245 switch (__get_cpu_type(cpu_info->cputype)) {
246 case CPU_INTERAPTIV:
247 perf_counter = 1;
248 perf_event = 51;
249 break;
250
251 case CPU_PROAPTIV:
252 /* Newer proAptiv cores don't require this workaround */
253 if (revision >= PRID_REV_ENCODE_332(1, 1, 0))
254 return 0;
255
256 /* On older ones it's unavailable */
257 return -1;
258
259 default:
260 /* Assume that the CPU does not need this workaround */
261 return 0;
262 }
263
264 /*
265 * Ensure that the fill/store buffer (FSB) is not holding the results
266 * of a prefetch, since if it is then the CPC sequencer may become
267 * stuck in the D3 (ClrBus) state whilst entering a low power state.
268 */
269
270 /* Preserve perf counter setup */
271 uasm_i_mfc0(pp, GPR_T2, 25, (perf_counter * 2) + 0); /* PerfCtlN */
272 uasm_i_mfc0(pp, GPR_T3, 25, (perf_counter * 2) + 1); /* PerfCntN */
273
274 /* Setup perf counter to count FSB full pipeline stalls */
275 uasm_i_addiu(pp, GPR_T0, GPR_ZERO, (perf_event << 5) | 0xf);
276 uasm_i_mtc0(pp, GPR_T0, 25, (perf_counter * 2) + 0); /* PerfCtlN */
277 uasm_i_ehb(pp);
278 uasm_i_mtc0(pp, GPR_ZERO, 25, (perf_counter * 2) + 1); /* PerfCntN */
279 uasm_i_ehb(pp);
280
281 /* Base address for loads */
282 UASM_i_LA(pp, GPR_T0, (long)CKSEG0);
283
284 /* Start of clear loop */
285 uasm_build_label(pl, *pp, lbl);
286
287 /* Perform some loads to fill the FSB */
288 for (i = 0; i < num_loads; i++)
289 uasm_i_lw(pp, GPR_ZERO, i * line_size * line_stride, GPR_T0);
290
291 /*
292 * Invalidate the new D-cache entries so that the cache will need
293 * refilling (via the FSB) if the loop is executed again.
294 */
295 for (i = 0; i < num_loads; i++) {
296 uasm_i_cache(pp, Hit_Invalidate_D,
297 i * line_size * line_stride, GPR_T0);
298 uasm_i_cache(pp, Hit_Writeback_Inv_SD,
299 i * line_size * line_stride, GPR_T0);
300 }
301
302 /* Barrier ensuring previous cache invalidates are complete */
303 uasm_i_sync(pp, __SYNC_full);
304 uasm_i_ehb(pp);
305
306 /* Check whether the pipeline stalled due to the FSB being full */
307 uasm_i_mfc0(pp, GPR_T1, 25, (perf_counter * 2) + 1); /* PerfCntN */
308
309 /* Loop if it didn't */
310 uasm_il_beqz(pp, pr, GPR_T1, lbl);
311 uasm_i_nop(pp);
312
313 /* Restore perf counter 1. The count may well now be wrong... */
314 uasm_i_mtc0(pp, GPR_T2, 25, (perf_counter * 2) + 0); /* PerfCtlN */
315 uasm_i_ehb(pp);
316 uasm_i_mtc0(pp, GPR_T3, 25, (perf_counter * 2) + 1); /* PerfCntN */
317 uasm_i_ehb(pp);
318
319 return 0;
320 }
321
cps_gen_set_top_bit(u32 ** pp,struct uasm_label ** pl,struct uasm_reloc ** pr,unsigned r_addr,int lbl)322 static void cps_gen_set_top_bit(u32 **pp, struct uasm_label **pl,
323 struct uasm_reloc **pr,
324 unsigned r_addr, int lbl)
325 {
326 uasm_i_lui(pp, GPR_T0, uasm_rel_hi(0x80000000));
327 uasm_build_label(pl, *pp, lbl);
328 uasm_i_ll(pp, GPR_T1, 0, r_addr);
329 uasm_i_or(pp, GPR_T1, GPR_T1, GPR_T0);
330 uasm_i_sc(pp, GPR_T1, 0, r_addr);
331 uasm_il_beqz(pp, pr, GPR_T1, lbl);
332 uasm_i_nop(pp);
333 }
334
cps_gen_entry_code(unsigned cpu,enum cps_pm_state state)335 static void *cps_gen_entry_code(unsigned cpu, enum cps_pm_state state)
336 {
337 struct uasm_label *l = labels;
338 struct uasm_reloc *r = relocs;
339 u32 *buf, *p;
340 const unsigned r_online = GPR_A0;
341 const unsigned r_nc_count = GPR_A1;
342 const unsigned r_pcohctl = GPR_T8;
343 const unsigned max_instrs = 256;
344 unsigned cpc_cmd;
345 int err;
346 enum {
347 lbl_incready = 1,
348 lbl_poll_cont,
349 lbl_secondary_hang,
350 lbl_disable_coherence,
351 lbl_flush_fsb,
352 lbl_invicache,
353 lbl_flushdcache,
354 lbl_hang,
355 lbl_set_cont,
356 lbl_secondary_cont,
357 lbl_decready,
358 };
359
360 /* Allocate a buffer to hold the generated code */
361 p = buf = kcalloc(max_instrs, sizeof(u32), GFP_KERNEL);
362 if (!buf)
363 return NULL;
364
365 /* Clear labels & relocs ready for (re)use */
366 memset(labels, 0, sizeof(labels));
367 memset(relocs, 0, sizeof(relocs));
368
369 if (IS_ENABLED(CONFIG_CPU_PM) && state == CPS_PM_POWER_GATED) {
370 /* Power gating relies upon CPS SMP */
371 if (!mips_cps_smp_in_use())
372 goto out_err;
373
374 /*
375 * Save CPU state. Note the non-standard calling convention
376 * with the return address placed in v0 to avoid clobbering
377 * the ra register before it is saved.
378 */
379 UASM_i_LA(&p, GPR_T0, (long)mips_cps_pm_save);
380 uasm_i_jalr(&p, GPR_V0, GPR_T0);
381 uasm_i_nop(&p);
382 }
383
384 /*
385 * Load addresses of required CM & CPC registers. This is done early
386 * because they're needed in both the enable & disable coherence steps
387 * but in the coupled case the enable step will only run on one VPE.
388 */
389 UASM_i_LA(&p, r_pcohctl, (long)addr_gcr_cl_coherence());
390
391 if (coupled_coherence) {
392 /* Increment ready_count */
393 uasm_i_sync(&p, __SYNC_mb);
394 uasm_build_label(&l, p, lbl_incready);
395 uasm_i_ll(&p, GPR_T1, 0, r_nc_count);
396 uasm_i_addiu(&p, GPR_T2, GPR_T1, 1);
397 uasm_i_sc(&p, GPR_T2, 0, r_nc_count);
398 uasm_il_beqz(&p, &r, GPR_T2, lbl_incready);
399 uasm_i_addiu(&p, GPR_T1, GPR_T1, 1);
400
401 /* Barrier ensuring all CPUs see the updated r_nc_count value */
402 uasm_i_sync(&p, __SYNC_mb);
403
404 /*
405 * If this is the last VPE to become ready for non-coherence
406 * then it should branch below.
407 */
408 uasm_il_beq(&p, &r, GPR_T1, r_online, lbl_disable_coherence);
409 uasm_i_nop(&p);
410
411 if (state < CPS_PM_POWER_GATED) {
412 /*
413 * Otherwise this is not the last VPE to become ready
414 * for non-coherence. It needs to wait until coherence
415 * has been disabled before proceeding, which it will do
416 * by polling for the top bit of ready_count being set.
417 */
418 uasm_i_addiu(&p, GPR_T1, GPR_ZERO, -1);
419 uasm_build_label(&l, p, lbl_poll_cont);
420 uasm_i_lw(&p, GPR_T0, 0, r_nc_count);
421 uasm_il_bltz(&p, &r, GPR_T0, lbl_secondary_cont);
422 uasm_i_ehb(&p);
423 if (cpu_has_mipsmt)
424 uasm_i_yield(&p, GPR_ZERO, GPR_T1);
425 uasm_il_b(&p, &r, lbl_poll_cont);
426 uasm_i_nop(&p);
427 } else {
428 /*
429 * The core will lose power & this VPE will not continue
430 * so it can simply halt here.
431 */
432 if (cpu_has_mipsmt) {
433 /* Halt the VPE via C0 tchalt register */
434 uasm_i_addiu(&p, GPR_T0, GPR_ZERO, TCHALT_H);
435 uasm_i_mtc0(&p, GPR_T0, 2, 4);
436 } else if (cpu_has_vp) {
437 /* Halt the VP via the CPC VP_STOP register */
438 unsigned int vpe_id;
439
440 vpe_id = cpu_vpe_id(&cpu_data[cpu]);
441 uasm_i_addiu(&p, GPR_T0, GPR_ZERO, 1 << vpe_id);
442 UASM_i_LA(&p, GPR_T1, (long)addr_cpc_cl_vp_stop());
443 uasm_i_sw(&p, GPR_T0, 0, GPR_T1);
444 } else {
445 BUG();
446 }
447 uasm_build_label(&l, p, lbl_secondary_hang);
448 uasm_il_b(&p, &r, lbl_secondary_hang);
449 uasm_i_nop(&p);
450 }
451 }
452
453 /*
454 * This is the point of no return - this VPE will now proceed to
455 * disable coherence. At this point we *must* be sure that no other
456 * VPE within the core will interfere with the L1 dcache.
457 */
458 uasm_build_label(&l, p, lbl_disable_coherence);
459
460 /* Invalidate the L1 icache */
461 cps_gen_cache_routine(&p, &l, &r, &cpu_data[cpu].icache,
462 Index_Invalidate_I, lbl_invicache);
463
464 /* Writeback & invalidate the L1 dcache */
465 cps_gen_cache_routine(&p, &l, &r, &cpu_data[cpu].dcache,
466 Index_Writeback_Inv_D, lbl_flushdcache);
467
468 /* Barrier ensuring previous cache invalidates are complete */
469 uasm_i_sync(&p, __SYNC_full);
470 uasm_i_ehb(&p);
471
472 if (mips_cm_revision() < CM_REV_CM3) {
473 /*
474 * Disable all but self interventions. The load from COHCTL is
475 * defined by the interAptiv & proAptiv SUMs as ensuring that the
476 * operation resulting from the preceding store is complete.
477 */
478 uasm_i_addiu(&p, GPR_T0, GPR_ZERO, 1 << cpu_core(&cpu_data[cpu]));
479 uasm_i_sw(&p, GPR_T0, 0, r_pcohctl);
480 uasm_i_lw(&p, GPR_T0, 0, r_pcohctl);
481
482 /* Barrier to ensure write to coherence control is complete */
483 uasm_i_sync(&p, __SYNC_full);
484 uasm_i_ehb(&p);
485 }
486
487 /* Disable coherence */
488 uasm_i_sw(&p, GPR_ZERO, 0, r_pcohctl);
489 uasm_i_lw(&p, GPR_T0, 0, r_pcohctl);
490
491 if (state >= CPS_PM_CLOCK_GATED) {
492 err = cps_gen_flush_fsb(&p, &l, &r, &cpu_data[cpu],
493 lbl_flush_fsb);
494 if (err)
495 goto out_err;
496
497 /* Determine the CPC command to issue */
498 switch (state) {
499 case CPS_PM_CLOCK_GATED:
500 cpc_cmd = CPC_Cx_CMD_CLOCKOFF;
501 break;
502 case CPS_PM_POWER_GATED:
503 cpc_cmd = CPC_Cx_CMD_PWRDOWN;
504 break;
505 default:
506 BUG();
507 goto out_err;
508 }
509
510 /* Issue the CPC command */
511 UASM_i_LA(&p, GPR_T0, (long)addr_cpc_cl_cmd());
512 uasm_i_addiu(&p, GPR_T1, GPR_ZERO, cpc_cmd);
513 uasm_i_sw(&p, GPR_T1, 0, GPR_T0);
514
515 if (state == CPS_PM_POWER_GATED) {
516 /* If anything goes wrong just hang */
517 uasm_build_label(&l, p, lbl_hang);
518 uasm_il_b(&p, &r, lbl_hang);
519 uasm_i_nop(&p);
520
521 /*
522 * There's no point generating more code, the core is
523 * powered down & if powered back up will run from the
524 * reset vector not from here.
525 */
526 goto gen_done;
527 }
528
529 /* Barrier to ensure write to CPC command is complete */
530 uasm_i_sync(&p, __SYNC_full);
531 uasm_i_ehb(&p);
532 }
533
534 if (state == CPS_PM_NC_WAIT) {
535 /*
536 * At this point it is safe for all VPEs to proceed with
537 * execution. This VPE will set the top bit of ready_count
538 * to indicate to the other VPEs that they may continue.
539 */
540 if (coupled_coherence)
541 cps_gen_set_top_bit(&p, &l, &r, r_nc_count,
542 lbl_set_cont);
543
544 /*
545 * VPEs which did not disable coherence will continue
546 * executing, after coherence has been disabled, from this
547 * point.
548 */
549 uasm_build_label(&l, p, lbl_secondary_cont);
550
551 /* Now perform our wait */
552 uasm_i_wait(&p, 0);
553 }
554
555 /*
556 * Re-enable coherence. Note that for CPS_PM_NC_WAIT all coupled VPEs
557 * will run this. The first will actually re-enable coherence & the
558 * rest will just be performing a rather unusual nop.
559 */
560 uasm_i_addiu(&p, GPR_T0, GPR_ZERO, mips_cm_revision() < CM_REV_CM3
561 ? CM_GCR_Cx_COHERENCE_COHDOMAINEN
562 : CM3_GCR_Cx_COHERENCE_COHEN);
563
564 uasm_i_sw(&p, GPR_T0, 0, r_pcohctl);
565 uasm_i_lw(&p, GPR_T0, 0, r_pcohctl);
566
567 /* Barrier to ensure write to coherence control is complete */
568 uasm_i_sync(&p, __SYNC_full);
569 uasm_i_ehb(&p);
570
571 if (coupled_coherence && (state == CPS_PM_NC_WAIT)) {
572 /* Decrement ready_count */
573 uasm_build_label(&l, p, lbl_decready);
574 uasm_i_sync(&p, __SYNC_mb);
575 uasm_i_ll(&p, GPR_T1, 0, r_nc_count);
576 uasm_i_addiu(&p, GPR_T2, GPR_T1, -1);
577 uasm_i_sc(&p, GPR_T2, 0, r_nc_count);
578 uasm_il_beqz(&p, &r, GPR_T2, lbl_decready);
579 uasm_i_andi(&p, GPR_V0, GPR_T1, (1 << fls(smp_num_siblings)) - 1);
580
581 /* Barrier ensuring all CPUs see the updated r_nc_count value */
582 uasm_i_sync(&p, __SYNC_mb);
583 }
584
585 if (coupled_coherence && (state == CPS_PM_CLOCK_GATED)) {
586 /*
587 * At this point it is safe for all VPEs to proceed with
588 * execution. This VPE will set the top bit of ready_count
589 * to indicate to the other VPEs that they may continue.
590 */
591 cps_gen_set_top_bit(&p, &l, &r, r_nc_count, lbl_set_cont);
592
593 /*
594 * This core will be reliant upon another core sending a
595 * power-up command to the CPC in order to resume operation.
596 * Thus an arbitrary VPE can't trigger the core leaving the
597 * idle state and the one that disables coherence might as well
598 * be the one to re-enable it. The rest will continue from here
599 * after that has been done.
600 */
601 uasm_build_label(&l, p, lbl_secondary_cont);
602
603 /* Barrier ensuring all CPUs see the updated r_nc_count value */
604 uasm_i_sync(&p, __SYNC_mb);
605 }
606
607 /* The core is coherent, time to return to C code */
608 uasm_i_jr(&p, GPR_RA);
609 uasm_i_nop(&p);
610
611 gen_done:
612 /* Ensure the code didn't exceed the resources allocated for it */
613 BUG_ON((p - buf) > max_instrs);
614 BUG_ON((l - labels) > ARRAY_SIZE(labels));
615 BUG_ON((r - relocs) > ARRAY_SIZE(relocs));
616
617 /* Patch branch offsets */
618 uasm_resolve_relocs(relocs, labels);
619
620 /* Flush the icache */
621 local_flush_icache_range((unsigned long)buf, (unsigned long)p);
622
623 return buf;
624 out_err:
625 kfree(buf);
626 return NULL;
627 }
628
cps_pm_online_cpu(unsigned int cpu)629 static int cps_pm_online_cpu(unsigned int cpu)
630 {
631 unsigned int sibling, core;
632 void *entry_fn, *core_rc;
633 enum cps_pm_state state;
634
635 core = cpu_core(&cpu_data[cpu]);
636
637 for (state = CPS_PM_NC_WAIT; state < CPS_PM_STATE_COUNT; state++) {
638 if (per_cpu(nc_asm_enter, cpu)[state])
639 continue;
640 if (!test_bit(state, state_support))
641 continue;
642
643 entry_fn = cps_gen_entry_code(cpu, state);
644 if (!entry_fn) {
645 pr_err("Failed to generate core %u state %u entry\n",
646 core, state);
647 clear_bit(state, state_support);
648 }
649
650 for_each_cpu(sibling, &cpu_sibling_map[cpu])
651 per_cpu(nc_asm_enter, sibling)[state] = entry_fn;
652 }
653
654 if (!per_cpu(ready_count, cpu)) {
655 core_rc = kmalloc(sizeof(u32), GFP_KERNEL);
656 if (!core_rc) {
657 pr_err("Failed allocate core %u ready_count\n", core);
658 return -ENOMEM;
659 }
660
661 for_each_cpu(sibling, &cpu_sibling_map[cpu])
662 per_cpu(ready_count, sibling) = core_rc;
663 }
664
665 return 0;
666 }
667
cps_pm_power_notifier(struct notifier_block * this,unsigned long event,void * ptr)668 static int cps_pm_power_notifier(struct notifier_block *this,
669 unsigned long event, void *ptr)
670 {
671 unsigned int stat;
672
673 switch (event) {
674 case PM_SUSPEND_PREPARE:
675 stat = read_cpc_cl_stat_conf();
676 /*
677 * If we're attempting to suspend the system and power down all
678 * of the cores, the JTAG detect bit indicates that the CPC will
679 * instead put the cores into clock-off state. In this state
680 * a connected debugger can cause the CPU to attempt
681 * interactions with the powered down system. At best this will
682 * fail. At worst, it can hang the NoC, requiring a hard reset.
683 * To avoid this, just block system suspend if a JTAG probe
684 * is detected.
685 */
686 if (stat & CPC_Cx_STAT_CONF_EJTAG_PROBE) {
687 pr_warn("JTAG probe is connected - abort suspend\n");
688 return NOTIFY_BAD;
689 }
690 return NOTIFY_DONE;
691 default:
692 return NOTIFY_DONE;
693 }
694 }
695
cps_pm_init(void)696 static int __init cps_pm_init(void)
697 {
698 /* A CM is required for all non-coherent states */
699 if (!mips_cm_present()) {
700 pr_warn("pm-cps: no CM, non-coherent states unavailable\n");
701 return 0;
702 }
703
704 /*
705 * If interrupts were enabled whilst running a wait instruction on a
706 * non-coherent core then the VPE may end up processing interrupts
707 * whilst non-coherent. That would be bad.
708 */
709 if (cpu_wait == r4k_wait_irqoff)
710 set_bit(CPS_PM_NC_WAIT, state_support);
711 else
712 pr_warn("pm-cps: non-coherent wait unavailable\n");
713
714 /* Detect whether a CPC is present */
715 if (mips_cpc_present()) {
716 /* Detect whether clock gating is implemented */
717 if (read_cpc_cl_stat_conf() & CPC_Cx_STAT_CONF_CLKGAT_IMPL)
718 set_bit(CPS_PM_CLOCK_GATED, state_support);
719 else
720 pr_warn("pm-cps: CPC does not support clock gating\n");
721
722 /* Power gating is available with CPS SMP & any CPC */
723 if (mips_cps_smp_in_use())
724 set_bit(CPS_PM_POWER_GATED, state_support);
725 else
726 pr_warn("pm-cps: CPS SMP not in use, power gating unavailable\n");
727 } else {
728 pr_warn("pm-cps: no CPC, clock & power gating unavailable\n");
729 }
730
731 pm_notifier(cps_pm_power_notifier, 0);
732
733 return cpuhp_setup_state(CPUHP_AP_ONLINE_DYN, "mips/cps_pm:online",
734 cps_pm_online_cpu, NULL);
735 }
736 arch_initcall(cps_pm_init);
737