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