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