arch/mips/kernel/smtc.c

   1 /* Copyright (C) 2004 Mips Technologies, Inc */
   2
   3 #include <linux/kernel.h>
   4 #include <linux/sched.h>
   5 #include <linux/cpumask.h>
   6 #include <linux/interrupt.h>
   7 #include <linux/kernel_stat.h>
   8 #include <linux/module.h>
   9
  10 #include <asm/cpu.h>
  11 #include <asm/processor.h>
  12 #include <asm/atomic.h>
  13 #include <asm/system.h>
  14 #include <asm/hardirq.h>
  15 #include <asm/hazards.h>
  16 #include <asm/irq.h>
  17 #include <asm/mmu_context.h>
  18 #include <asm/smp.h>
  19 #include <asm/mipsregs.h>
  20 #include <asm/cacheflush.h>
  21 #include <asm/time.h>
  22 #include <asm/addrspace.h>
  23 #include <asm/smtc.h>
  24 #include <asm/smtc_ipi.h>
  25 #include <asm/smtc_proc.h>
  26
  27 /*
  28  * SMTC Kernel needs to manipulate low-level CPU interrupt mask
  29  * in do_IRQ. These are passed in setup_irq_smtc() and stored
  30  * in this table.
  31  */
  32 unsigned long irq_hwmask[NR_IRQS];
  33
  34 #define LOCK_MT_PRA() \
  35         local_irq_save(flags); \
  36         mtflags = dmt()
  37
  38 #define UNLOCK_MT_PRA() \
  39         emt(mtflags); \
  40         local_irq_restore(flags)
  41
  42 #define LOCK_CORE_PRA() \
  43         local_irq_save(flags); \
  44         mtflags = dvpe()
  45
  46 #define UNLOCK_CORE_PRA() \
  47         evpe(mtflags); \
  48         local_irq_restore(flags)
  49
  50 /*
  51  * Data structures purely associated with SMTC parallelism
  52  */
  53
  54
  55 /*
  56  * Table for tracking ASIDs whose lifetime is prolonged.
  57  */
  58
  59 asiduse smtc_live_asid[MAX_SMTC_TLBS][MAX_SMTC_ASIDS];
  60
  61 /*
  62  * Clock interrupt "latch" buffers, per "CPU"
  63  */
  64
  65 unsigned int ipi_timer_latch[NR_CPUS];
  66
  67 /*
  68  * Number of InterProcessor Interupt (IPI) message buffers to allocate
  69  */
  70
  71 #define IPIBUF_PER_CPU 4
  72
  73 static struct smtc_ipi_q IPIQ[NR_CPUS];
  74 static struct smtc_ipi_q freeIPIq;
  75
  76
  77 /* Forward declarations */
  78
  79 void ipi_decode(struct smtc_ipi *);
  80 static void post_direct_ipi(int cpu, struct smtc_ipi *pipi);
  81 static void setup_cross_vpe_interrupts(unsigned int nvpe);
  82 void init_smtc_stats(void);
  83
  84 /* Global SMTC Status */
  85
  86 unsigned int smtc_status = 0;
  87
  88 /* Boot command line configuration overrides */
  89
  90 static int ipibuffers = 0;
  91 static int nostlb = 0;
  92 static int asidmask = 0;
  93 unsigned long smtc_asid_mask = 0xff;
  94
  95 static int __init ipibufs(char *str)
  96 {
  97         get_option(&str, &ipibuffers);
  98         return 1;
  99 }
 100
 101 static int __init stlb_disable(char *s)
 102 {
 103         nostlb = 1;
 104         return 1;
 105 }
 106
 107 static int __init asidmask_set(char *str)
 108 {
 109         get_option(&str, &asidmask);
 110         switch (asidmask) {
 111         case 0x1:
 112         case 0x3:
 113         case 0x7:
 114         case 0xf:
 115         case 0x1f:
 116         case 0x3f:
 117         case 0x7f:
 118         case 0xff:
 119                 smtc_asid_mask = (unsigned long)asidmask;
 120                 break;
 121         default:
 122                 printk("ILLEGAL ASID mask 0x%x from command line\n", asidmask);
 123         }
 124         return 1;
 125 }
 126
 127 __setup("ipibufs=", ipibufs);
 128 __setup("nostlb", stlb_disable);
 129 __setup("asidmask=", asidmask_set);
 130
 131 #ifdef CONFIG_SMTC_IDLE_HOOK_DEBUG
 132
 133 static int hang_trig = 0;
 134
 135 static int __init hangtrig_enable(char *s)
 136 {
 137         hang_trig = 1;
 138         return 1;
 139 }
 140
 141
 142 __setup("hangtrig", hangtrig_enable);
 143
 144 #define DEFAULT_BLOCKED_IPI_LIMIT 32
 145
 146 static int timerq_limit = DEFAULT_BLOCKED_IPI_LIMIT;
 147
 148 static int __init tintq(char *str)
 149 {
 150         get_option(&str, &timerq_limit);
 151         return 1;
 152 }
 153
 154 __setup("tintq=", tintq);
 155
 156 static int imstuckcount[2][8];
 157 /* vpemask represents IM/IE bits of per-VPE Status registers, low-to-high */
 158 static int vpemask[2][8] = {
 159         {0, 0, 1, 0, 0, 0, 0, 1},
 160         {0, 0, 0, 0, 0, 0, 0, 1}
 161 };
 162 int tcnoprog[NR_CPUS];
 163 static atomic_t idle_hook_initialized = {0};
 164 static int clock_hang_reported[NR_CPUS];
 165
 166 #endif /* CONFIG_SMTC_IDLE_HOOK_DEBUG */
 167
 168 /* Initialize shared TLB - the should probably migrate to smtc_setup_cpus() */
 169
 170 void __init sanitize_tlb_entries(void)
 171 {
 172         printk("Deprecated sanitize_tlb_entries() invoked\n");
 173 }
 174
 175
 176 /*
 177  * Configure shared TLB - VPC configuration bit must be set by caller
 178  */
 179
 180 static void smtc_configure_tlb(void)
 181 {
 182         int i,tlbsiz,vpes;
 183         unsigned long mvpconf0;
 184         unsigned long config1val;
 185
 186         /* Set up ASID preservation table */
 187         for (vpes=0; vpes<MAX_SMTC_TLBS; vpes++) {
 188             for(i = 0; i < MAX_SMTC_ASIDS; i++) {
 189                 smtc_live_asid[vpes][i] = 0;
 190             }
 191         }
 192         mvpconf0 = read_c0_mvpconf0();
 193
 194         if ((vpes = ((mvpconf0 & MVPCONF0_PVPE)
 195                         >> MVPCONF0_PVPE_SHIFT) + 1) > 1) {
 196             /* If we have multiple VPEs, try to share the TLB */
 197             if ((mvpconf0 & MVPCONF0_TLBS) && !nostlb) {
 198                 /*
 199                  * If TLB sizing is programmable, shared TLB
 200                  * size is the total available complement.
 201                  * Otherwise, we have to take the sum of all
 202                  * static VPE TLB entries.
 203                  */
 204                 if ((tlbsiz = ((mvpconf0 & MVPCONF0_PTLBE)
 205                                 >> MVPCONF0_PTLBE_SHIFT)) == 0) {
 206                     /*
 207                      * If there's more than one VPE, there had better
 208                      * be more than one TC, because we need one to bind
 209                      * to each VPE in turn to be able to read
 210                      * its configuration state!
 211                      */
 212                     settc(1);
 213                     /* Stop the TC from doing anything foolish */
 214                     write_tc_c0_tchalt(TCHALT_H);
 215                     mips_ihb();
 216                     /* No need to un-Halt - that happens later anyway */
 217                     for (i=0; i < vpes; i++) {
 218                         write_tc_c0_tcbind(i);
 219                         /*
 220                          * To be 100% sure we're really getting the right
 221                          * information, we exit the configuration state
 222                          * and do an IHB after each rebinding.
 223                          */
 224                         write_c0_mvpcontrol(
 225                                 read_c0_mvpcontrol() & ~ MVPCONTROL_VPC );
 226                         mips_ihb();
 227                         /*
 228                          * Only count if the MMU Type indicated is TLB
 229                          */
 230                         if (((read_vpe_c0_config() & MIPS_CONF_MT) >> 7) == 1) {
 231                                 config1val = read_vpe_c0_config1();
 232                                 tlbsiz += ((config1val >> 25) & 0x3f) + 1;
 233                         }
 234
 235                         /* Put core back in configuration state */
 236                         write_c0_mvpcontrol(
 237                                 read_c0_mvpcontrol() | MVPCONTROL_VPC );
 238                         mips_ihb();
 239                     }
 240                 }
 241                 write_c0_mvpcontrol(read_c0_mvpcontrol() | MVPCONTROL_STLB);
 242                 ehb();
 243
 244                 /*
 245                  * Setup kernel data structures to use software total,
 246                  * rather than read the per-VPE Config1 value. The values
 247                  * for "CPU 0" gets copied to all the other CPUs as part
 248                  * of their initialization in smtc_cpu_setup().
 249                  */
 250
 251                 /* MIPS32 limits TLB indices to 64 */
 252                 if (tlbsiz > 64)
 253                         tlbsiz = 64;
 254                 cpu_data[0].tlbsize = current_cpu_data.tlbsize = tlbsiz;
 255                 smtc_status |= SMTC_TLB_SHARED;
 256                 local_flush_tlb_all();
 257
 258                 printk("TLB of %d entry pairs shared by %d VPEs\n",
 259                         tlbsiz, vpes);
 260             } else {
 261                 printk("WARNING: TLB Not Sharable on SMTC Boot!\n");
 262             }
 263         }
 264 }
 265
 266
 267 /*
 268  * Incrementally build the CPU map out of constituent MIPS MT cores,
 269  * using the specified available VPEs and TCs.  Plaform code needs
 270  * to ensure that each MIPS MT core invokes this routine on reset,
 271  * one at a time(!).
 272  *
 273  * This version of the build_cpu_map and prepare_cpus routines assumes
 274  * that *all* TCs of a MIPS MT core will be used for Linux, and that
 275  * they will be spread across *all* available VPEs (to minimise the
 276  * loss of efficiency due to exception service serialization).
 277  * An improved version would pick up configuration information and
 278  * possibly leave some TCs/VPEs as "slave" processors.
 279  *
 280  * Use c0_MVPConf0 to find out how many TCs are available, setting up
 281  * phys_cpu_present_map and the logical/physical mappings.
 282  */
 283
 284 int __init mipsmt_build_cpu_map(int start_cpu_slot)
 285 {
 286         int i, ntcs;
 287
 288         /*
 289          * The CPU map isn't actually used for anything at this point,
 290          * so it's not clear what else we should do apart from set
 291          * everything up so that "logical" = "physical".
 292          */
 293         ntcs = ((read_c0_mvpconf0() & MVPCONF0_PTC) >> MVPCONF0_PTC_SHIFT) + 1;
 294         for (i=start_cpu_slot; i<NR_CPUS && i<ntcs; i++) {
 295                 cpu_set(i, phys_cpu_present_map);
 296                 __cpu_number_map[i] = i;
 297                 __cpu_logical_map[i] = i;
 298         }
 299         /* Initialize map of CPUs with FPUs */
 300         cpus_clear(mt_fpu_cpumask);
 301
 302         /* One of those TC's is the one booting, and not a secondary... */
 303         printk("%i available secondary CPU TC(s)\n", i - 1);
 304
 305         return i;
 306 }
 307
 308 /*
 309  * Common setup before any secondaries are started
 310  * Make sure all CPU's are in a sensible state before we boot any of the
 311  * secondaries.
 312  *
 313  * For MIPS MT "SMTC" operation, we set up all TCs, spread as evenly
 314  * as possible across the available VPEs.
 315  */
 316
 317 static void smtc_tc_setup(int vpe, int tc, int cpu)
 318 {
 319         settc(tc);
 320         write_tc_c0_tchalt(TCHALT_H);
 321         mips_ihb();
 322         write_tc_c0_tcstatus((read_tc_c0_tcstatus()
 323                         & ~(TCSTATUS_TKSU | TCSTATUS_DA | TCSTATUS_IXMT))
 324                         | TCSTATUS_A);
 325         write_tc_c0_tccontext(0);
 326         /* Bind tc to vpe */
 327         write_tc_c0_tcbind(vpe);
 328         /* In general, all TCs should have the same cpu_data indications */
 329         memcpy(&cpu_data[cpu], &cpu_data[0], sizeof(struct cpuinfo_mips));
 330         /* For 34Kf, start with TC/CPU 0 as sole owner of single FPU context */
 331         if (cpu_data[0].cputype == CPU_34K)
 332                 cpu_data[cpu].options &= ~MIPS_CPU_FPU;
 333         cpu_data[cpu].vpe_id = vpe;
 334         cpu_data[cpu].tc_id = tc;
 335 }
 336
 337
 338 void mipsmt_prepare_cpus(void)
 339 {
 340         int i, vpe, tc, ntc, nvpe, tcpervpe, slop, cpu;
 341         unsigned long flags;
 342         unsigned long val;
 343         int nipi;
 344         struct smtc_ipi *pipi;
 345
 346         /* disable interrupts so we can disable MT */
 347         local_irq_save(flags);
 348         /* disable MT so we can configure */
 349         dvpe();
 350         dmt();
 351
 352         spin_lock_init(&freeIPIq.lock);
 353
 354         /*
 355          * We probably don't have as many VPEs as we do SMP "CPUs",
 356          * but it's possible - and in any case we'll never use more!
 357          */
 358         for (i=0; i<NR_CPUS; i++) {
 359                 IPIQ[i].head = IPIQ[i].tail = NULL;
 360                 spin_lock_init(&IPIQ[i].lock);
 361                 IPIQ[i].depth = 0;
 362                 ipi_timer_latch[i] = 0;
 363         }
 364
 365         /* cpu_data index starts at zero */
 366         cpu = 0;
 367         cpu_data[cpu].vpe_id = 0;
 368         cpu_data[cpu].tc_id = 0;
 369         cpu++;
 370
 371         /* Report on boot-time options */
 372         mips_mt_set_cpuoptions ();
 373         if (vpelimit > 0)
 374                 printk("Limit of %d VPEs set\n", vpelimit);
 375         if (tclimit > 0)
 376                 printk("Limit of %d TCs set\n", tclimit);
 377         if (nostlb) {
 378                 printk("Shared TLB Use Inhibited - UNSAFE for Multi-VPE Operation\n");
 379         }
 380         if (asidmask)
 381                 printk("ASID mask value override to 0x%x\n", asidmask);
 382
 383         /* Temporary */
 384 #ifdef CONFIG_SMTC_IDLE_HOOK_DEBUG
 385         if (hang_trig)
 386                 printk("Logic Analyser Trigger on suspected TC hang\n");
 387 #endif /* CONFIG_SMTC_IDLE_HOOK_DEBUG */
 388
 389         /* Put MVPE's into 'configuration state' */
 390         write_c0_mvpcontrol( read_c0_mvpcontrol() | MVPCONTROL_VPC );
 391
 392         val = read_c0_mvpconf0();
 393         nvpe = ((val & MVPCONF0_PVPE) >> MVPCONF0_PVPE_SHIFT) + 1;
 394         if (vpelimit > 0 && nvpe > vpelimit)
 395                 nvpe = vpelimit;
 396         ntc = ((val & MVPCONF0_PTC) >> MVPCONF0_PTC_SHIFT) + 1;
 397         if (ntc > NR_CPUS)
 398                 ntc = NR_CPUS;
 399         if (tclimit > 0 && ntc > tclimit)
 400                 ntc = tclimit;
 401         tcpervpe = ntc / nvpe;
 402         slop = ntc % nvpe;      /* Residual TCs, < NVPE */
 403
 404         /* Set up shared TLB */
 405         smtc_configure_tlb();
 406
 407         for (tc = 0, vpe = 0 ; (vpe < nvpe) && (tc < ntc) ; vpe++) {
 408                 /*
 409                  * Set the MVP bits.
 410                  */
 411                 settc(tc);
 412                 write_vpe_c0_vpeconf0(read_vpe_c0_vpeconf0() | VPECONF0_MVP);
 413                 if (vpe != 0)
 414                         printk(", ");
 415                 printk("VPE %d: TC", vpe);
 416                 for (i = 0; i < tcpervpe; i++) {
 417                         /*
 418                          * TC 0 is bound to VPE 0 at reset,
 419                          * and is presumably executing this
 420                          * code.  Leave it alone!
 421                          */
 422                         if (tc != 0) {
 423                                 smtc_tc_setup(vpe,tc, cpu);
 424                                 cpu++;
 425                         }
 426                         printk(" %d", tc);
 427                         tc++;
 428                 }
 429                 if (slop) {
 430                         if (tc != 0) {
 431                                 smtc_tc_setup(vpe,tc, cpu);
 432                                 cpu++;
 433                         }
 434                         printk(" %d", tc);
 435                         tc++;
 436                         slop--;
 437                 }
 438                 if (vpe != 0) {
 439                         /*
 440                          * Clear any stale software interrupts from VPE's Cause
 441                          */
 442                         write_vpe_c0_cause(0);
 443
 444                         /*
 445                          * Clear ERL/EXL of VPEs other than 0
 446                          * and set restricted interrupt enable/mask.
 447                          */
 448                         write_vpe_c0_status((read_vpe_c0_status()
 449                                 & ~(ST0_BEV | ST0_ERL | ST0_EXL | ST0_IM))
 450                                 | (STATUSF_IP0 | STATUSF_IP1 | STATUSF_IP7
 451                                 | ST0_IE));
 452                         /*
 453                          * set config to be the same as vpe0,
 454                          *  particularly kseg0 coherency alg
 455                          */
 456                         write_vpe_c0_config(read_c0_config());
 457                         /* Clear any pending timer interrupt */
 458                         write_vpe_c0_compare(0);
 459                         /* Propagate Config7 */
 460                         write_vpe_c0_config7(read_c0_config7());
 461                         write_vpe_c0_count(read_c0_count());
 462                 }
 463                 /* enable multi-threading within VPE */
 464                 write_vpe_c0_vpecontrol(read_vpe_c0_vpecontrol() | VPECONTROL_TE);
 465                 /* enable the VPE */
 466                 write_vpe_c0_vpeconf0(read_vpe_c0_vpeconf0() | VPECONF0_VPA);
 467         }
 468
 469         /*
 470          * Pull any physically present but unused TCs out of circulation.
 471          */
 472         while (tc < (((val & MVPCONF0_PTC) >> MVPCONF0_PTC_SHIFT) + 1)) {
 473                 cpu_clear(tc, phys_cpu_present_map);
 474                 cpu_clear(tc, cpu_present_map);
 475                 tc++;
 476         }
 477
 478         /* release config state */
 479         write_c0_mvpcontrol( read_c0_mvpcontrol() & ~ MVPCONTROL_VPC );
 480
 481         printk("\n");
 482
 483         /* Set up coprocessor affinity CPU mask(s) */
 484
 485         for (tc = 0; tc < ntc; tc++) {
 486                 if (cpu_data[tc].options & MIPS_CPU_FPU)
 487                         cpu_set(tc, mt_fpu_cpumask);
 488         }
 489
 490         /* set up ipi interrupts... */
 491
 492         /* If we have multiple VPEs running, set up the cross-VPE interrupt */
 493
 494         setup_cross_vpe_interrupts(nvpe);
 495
 496         /* Set up queue of free IPI "messages". */
 497         nipi = NR_CPUS * IPIBUF_PER_CPU;
 498         if (ipibuffers > 0)
 499                 nipi = ipibuffers;
 500
 501         pipi = kmalloc(nipi *sizeof(struct smtc_ipi), GFP_KERNEL);
 502         if (pipi == NULL)
 503                 panic("kmalloc of IPI message buffers failed\n");
 504         else
 505                 printk("IPI buffer pool of %d buffers\n", nipi);
 506         for (i = 0; i < nipi; i++) {
 507                 smtc_ipi_nq(&freeIPIq, pipi);
 508                 pipi++;
 509         }
 510
 511         /* Arm multithreading and enable other VPEs - but all TCs are Halted */
 512         emt(EMT_ENABLE);
 513         evpe(EVPE_ENABLE);
 514         local_irq_restore(flags);
 515         /* Initialize SMTC /proc statistics/diagnostics */
 516         init_smtc_stats();
 517 }
 518
 519
 520 /*
 521  * Setup the PC, SP, and GP of a secondary processor and start it
 522  * running!
 523  * smp_bootstrap is the place to resume from
 524  * __KSTK_TOS(idle) is apparently the stack pointer
 525  * (unsigned long)idle->thread_info the gp
 526  *
 527  */
 528 void __cpuinit smtc_boot_secondary(int cpu, struct task_struct *idle)
 529 {
 530         extern u32 kernelsp[NR_CPUS];
 531         long flags;
 532         int mtflags;
 533
 534         LOCK_MT_PRA();
 535         if (cpu_data[cpu].vpe_id != cpu_data[smp_processor_id()].vpe_id) {
 536                 dvpe();
 537         }
 538         settc(cpu_data[cpu].tc_id);
 539
 540         /* pc */
 541         write_tc_c0_tcrestart((unsigned long)&smp_bootstrap);
 542
 543         /* stack pointer */
 544         kernelsp[cpu] = __KSTK_TOS(idle);
 545         write_tc_gpr_sp(__KSTK_TOS(idle));
 546
 547         /* global pointer */
 548         write_tc_gpr_gp((unsigned long)task_thread_info(idle));
 549
 550         smtc_status |= SMTC_MTC_ACTIVE;
 551         write_tc_c0_tchalt(0);
 552         if (cpu_data[cpu].vpe_id != cpu_data[smp_processor_id()].vpe_id) {
 553                 evpe(EVPE_ENABLE);
 554         }
 555         UNLOCK_MT_PRA();
 556 }
 557
 558 void smtc_init_secondary(void)
 559 {
 560         /*
 561          * Start timer on secondary VPEs if necessary.
 562          * plat_timer_setup has already have been invoked by init/main
 563          * on "boot" TC.  Like per_cpu_trap_init() hack, this assumes that
 564          * SMTC init code assigns TCs consdecutively and in ascending order
 565          * to across available VPEs.
 566          */
 567         if (((read_c0_tcbind() & TCBIND_CURTC) != 0) &&
 568             ((read_c0_tcbind() & TCBIND_CURVPE)
 569             != cpu_data[smp_processor_id() - 1].vpe_id)){
 570                 write_c0_compare (read_c0_count() + mips_hpt_frequency/HZ);
 571         }
 572
 573         local_irq_enable();
 574 }
 575
 576 void smtc_smp_finish(void)
 577 {
 578         printk("TC %d going on-line as CPU %d\n",
 579                 cpu_data[smp_processor_id()].tc_id, smp_processor_id());
 580 }
 581
 582 void smtc_cpus_done(void)
 583 {
 584 }
 585
 586 /*
 587  * Support for SMTC-optimized driver IRQ registration
 588  */
 589
 590 /*
 591  * SMTC Kernel needs to manipulate low-level CPU interrupt mask
 592  * in do_IRQ. These are passed in setup_irq_smtc() and stored
 593  * in this table.
 594  */
 595
 596 int setup_irq_smtc(unsigned int irq, struct irqaction * new,
 597                         unsigned long hwmask)
 598 {
 599 #ifdef CONFIG_SMTC_IDLE_HOOK_DEBUG
 600         unsigned int vpe = current_cpu_data.vpe_id;
 601
 602         vpemask[vpe][irq - MIPS_CPU_IRQ_BASE] = 1;
 603 #endif
 604         irq_hwmask[irq] = hwmask;
 605
 606         return setup_irq(irq, new);
 607 }
 608
 609 /*
 610  * IPI model for SMTC is tricky, because interrupts aren't TC-specific.
 611  * Within a VPE one TC can interrupt another by different approaches.
 612  * The easiest to get right would probably be to make all TCs except
 613  * the target IXMT and set a software interrupt, but an IXMT-based
 614  * scheme requires that a handler must run before a new IPI could
 615  * be sent, which would break the "broadcast" loops in MIPS MT.
 616  * A more gonzo approach within a VPE is to halt the TC, extract
 617  * its Restart, Status, and a couple of GPRs, and program the Restart
 618  * address to emulate an interrupt.
 619  *
 620  * Within a VPE, one can be confident that the target TC isn't in
 621  * a critical EXL state when halted, since the write to the Halt
 622  * register could not have issued on the writing thread if the
 623  * halting thread had EXL set. So k0 and k1 of the target TC
 624  * can be used by the injection code.  Across VPEs, one can't
 625  * be certain that the target TC isn't in a critical exception
 626  * state. So we try a two-step process of sending a software
 627  * interrupt to the target VPE, which either handles the event
 628  * itself (if it was the target) or injects the event within
 629  * the VPE.
 630  */
 631
 632 static void smtc_ipi_qdump(void)
 633 {
 634         int i;
 635
 636         for (i = 0; i < NR_CPUS ;i++) {
 637                 printk("IPIQ[%d]: head = 0x%x, tail = 0x%x, depth = %d\n",
 638                         i, (unsigned)IPIQ[i].head, (unsigned)IPIQ[i].tail,
 639                         IPIQ[i].depth);
 640         }
 641 }
 642
 643 /*
 644  * The standard atomic.h primitives don't quite do what we want
 645  * here: We need an atomic add-and-return-previous-value (which
 646  * could be done with atomic_add_return and a decrement) and an
 647  * atomic set/zero-and-return-previous-value (which can't really
 648  * be done with the atomic.h primitives). And since this is
 649  * MIPS MT, we can assume that we have LL/SC.
 650  */
 651 static __inline__ int atomic_postincrement(unsigned int *pv)
 652 {
 653         unsigned long result;
 654
 655         unsigned long temp;
 656
 657         __asm__ __volatile__(
 658         "1:     ll      %0, %2                                  \n"
 659         "       addu    %1, %0, 1                               \n"
 660         "       sc      %1, %2                                  \n"
 661         "       beqz    %1, 1b                                  \n"
 662         "       sync                                            \n"
 663         : "=&r" (result), "=&r" (temp), "=m" (*pv)
 664         : "m" (*pv)
 665         : "memory");
 666
 667         return result;
 668 }
 669
 670 void smtc_send_ipi(int cpu, int type, unsigned int action)
 671 {
 672         int tcstatus;
 673         struct smtc_ipi *pipi;
 674         long flags;
 675         int mtflags;
 676
 677         if (cpu == smp_processor_id()) {
 678                 printk("Cannot Send IPI to self!\n");
 679                 return;
 680         }
 681         /* Set up a descriptor, to be delivered either promptly or queued */
 682         pipi = smtc_ipi_dq(&freeIPIq);
 683         if (pipi == NULL) {
 684                 bust_spinlocks(1);
 685                 mips_mt_regdump(dvpe());
 686                 panic("IPI Msg. Buffers Depleted\n");
 687         }
 688         pipi->type = type;
 689         pipi->arg = (void *)action;
 690         pipi->dest = cpu;
 691         if (cpu_data[cpu].vpe_id != cpu_data[smp_processor_id()].vpe_id) {
 692                 /* If not on same VPE, enqueue and send cross-VPE interupt */
 693                 smtc_ipi_nq(&IPIQ[cpu], pipi);
 694                 LOCK_CORE_PRA();
 695                 settc(cpu_data[cpu].tc_id);
 696                 write_vpe_c0_cause(read_vpe_c0_cause() | C_SW1);
 697                 UNLOCK_CORE_PRA();
 698         } else {
 699                 /*
 700                  * Not sufficient to do a LOCK_MT_PRA (dmt) here,
 701                  * since ASID shootdown on the other VPE may
 702                  * collide with this operation.
 703                  */
 704                 LOCK_CORE_PRA();
 705                 settc(cpu_data[cpu].tc_id);
 706                 /* Halt the targeted TC */
 707                 write_tc_c0_tchalt(TCHALT_H);
 708                 mips_ihb();
 709
 710                 /*
 711                  * Inspect TCStatus - if IXMT is set, we have to queue
 712                  * a message. Otherwise, we set up the "interrupt"
 713                  * of the other TC
 714                  */
 715                 tcstatus = read_tc_c0_tcstatus();
 716
 717                 if ((tcstatus & TCSTATUS_IXMT) != 0) {
 718                         /*
 719                          * Spin-waiting here can deadlock,
 720                          * so we queue the message for the target TC.
 721                          */
 722                         write_tc_c0_tchalt(0);
 723                         UNLOCK_CORE_PRA();
 724                         /* Try to reduce redundant timer interrupt messages */
 725                         if (type == SMTC_CLOCK_TICK) {
 726                             if (atomic_postincrement(&ipi_timer_latch[cpu])!=0){
 727                                 smtc_ipi_nq(&freeIPIq, pipi);
 728                                 return;
 729                             }
 730                         }
 731                         smtc_ipi_nq(&IPIQ[cpu], pipi);
 732                 } else {
 733                         post_direct_ipi(cpu, pipi);
 734                         write_tc_c0_tchalt(0);
 735                         UNLOCK_CORE_PRA();
 736                 }
 737         }
 738 }
 739
 740 /*
 741  * Send IPI message to Halted TC, TargTC/TargVPE already having been set
 742  */
 743 static void post_direct_ipi(int cpu, struct smtc_ipi *pipi)
 744 {
 745         struct pt_regs *kstack;
 746         unsigned long tcstatus;
 747         unsigned long tcrestart;
 748         extern u32 kernelsp[NR_CPUS];
 749         extern void __smtc_ipi_vector(void);
 750
 751         /* Extract Status, EPC from halted TC */
 752         tcstatus = read_tc_c0_tcstatus();
 753         tcrestart = read_tc_c0_tcrestart();
 754         /* If TCRestart indicates a WAIT instruction, advance the PC */
 755         if ((tcrestart & 0x80000000)
 756             && ((*(unsigned int *)tcrestart & 0xfe00003f) == 0x42000020)) {
 757                 tcrestart += 4;
 758         }
 759         /*
 760          * Save on TC's future kernel stack
 761          *
 762          * CU bit of Status is indicator that TC was
 763          * already running on a kernel stack...
 764          */
 765         if (tcstatus & ST0_CU0)  {
 766                 /* Note that this "- 1" is pointer arithmetic */
 767                 kstack = ((struct pt_regs *)read_tc_gpr_sp()) - 1;
 768         } else {
 769                 kstack = ((struct pt_regs *)kernelsp[cpu]) - 1;
 770         }
 771
 772         kstack->cp0_epc = (long)tcrestart;
 773         /* Save TCStatus */
 774         kstack->cp0_tcstatus = tcstatus;
 775         /* Pass token of operation to be performed kernel stack pad area */
 776         kstack->pad0[4] = (unsigned long)pipi;
 777         /* Pass address of function to be called likewise */
 778         kstack->pad0[5] = (unsigned long)&ipi_decode;
 779         /* Set interrupt exempt and kernel mode */
 780         tcstatus |= TCSTATUS_IXMT;
 781         tcstatus &= ~TCSTATUS_TKSU;
 782         write_tc_c0_tcstatus(tcstatus);
 783         ehb();
 784         /* Set TC Restart address to be SMTC IPI vector */
 785         write_tc_c0_tcrestart(__smtc_ipi_vector);
 786 }
 787
 788 static void ipi_resched_interrupt(void)
 789 {
 790         /* Return from interrupt should be enough to cause scheduler check */
 791 }
 792
 793
 794 static void ipi_call_interrupt(void)
 795 {
 796         /* Invoke generic function invocation code in smp.c */
 797         smp_call_function_interrupt();
 798 }
 799
 800 void ipi_decode(struct smtc_ipi *pipi)
 801 {
 802         void *arg_copy = pipi->arg;
 803         int type_copy = pipi->type;
 804         int dest_copy = pipi->dest;
 805
 806         smtc_ipi_nq(&freeIPIq, pipi);
 807         switch (type_copy) {
 808         case SMTC_CLOCK_TICK:
 809                 irq_enter();
 810                 kstat_this_cpu.irqs[MIPS_CPU_IRQ_BASE + cp0_compare_irq]++;
 811                 /* Invoke Clock "Interrupt" */
 812                 ipi_timer_latch[dest_copy] = 0;
 813 #ifdef CONFIG_SMTC_IDLE_HOOK_DEBUG
 814                 clock_hang_reported[dest_copy] = 0;
 815 #endif /* CONFIG_SMTC_IDLE_HOOK_DEBUG */
 816                 local_timer_interrupt(0, NULL);
 817                 irq_exit();
 818                 break;
 819         case LINUX_SMP_IPI:
 820                 switch ((int)arg_copy) {
 821                 case SMP_RESCHEDULE_YOURSELF:
 822                         ipi_resched_interrupt();
 823                         break;
 824                 case SMP_CALL_FUNCTION:
 825                         ipi_call_interrupt();
 826                         break;
 827                 default:
 828                         printk("Impossible SMTC IPI Argument 0x%x\n",
 829                                 (int)arg_copy);
 830                         break;
 831                 }
 832                 break;
 833         default:
 834                 printk("Impossible SMTC IPI Type 0x%x\n", type_copy);
 835                 break;
 836         }
 837 }
 838
 839 void deferred_smtc_ipi(void)
 840 {
 841         struct smtc_ipi *pipi;
 842         unsigned long flags;
 843 /* DEBUG */
 844         int q = smp_processor_id();
 845
 846         /*
 847          * Test is not atomic, but much faster than a dequeue,
 848          * and the vast majority of invocations will have a null queue.
 849          */
 850         if (IPIQ[q].head != NULL) {
 851                 while((pipi = smtc_ipi_dq(&IPIQ[q])) != NULL) {
 852                         /* ipi_decode() should be called with interrupts off */
 853                         local_irq_save(flags);
 854                         ipi_decode(pipi);
 855                         local_irq_restore(flags);
 856                 }
 857         }
 858 }
 859
 860 /*
 861  * Send clock tick to all TCs except the one executing the funtion
 862  */
 863
 864 void smtc_timer_broadcast(void)
 865 {
 866         int cpu;
 867         int myTC = cpu_data[smp_processor_id()].tc_id;
 868         int myVPE = cpu_data[smp_processor_id()].vpe_id;
 869
 870         smtc_cpu_stats[smp_processor_id()].timerints++;
 871
 872         for_each_online_cpu(cpu) {
 873                 if (cpu_data[cpu].vpe_id == myVPE &&
 874                     cpu_data[cpu].tc_id != myTC)
 875                         smtc_send_ipi(cpu, SMTC_CLOCK_TICK, 0);
 876         }
 877 }
 878
 879 /*
 880  * Cross-VPE interrupts in the SMTC prototype use "software interrupts"
 881  * set via cross-VPE MTTR manipulation of the Cause register. It would be
 882  * in some regards preferable to have external logic for "doorbell" hardware
 883  * interrupts.
 884  */
 885
 886 static int cpu_ipi_irq = MIPS_CPU_IRQ_BASE + MIPS_CPU_IPI_IRQ;
 887
 888 static irqreturn_t ipi_interrupt(int irq, void *dev_idm)
 889 {
 890         int my_vpe = cpu_data[smp_processor_id()].vpe_id;
 891         int my_tc = cpu_data[smp_processor_id()].tc_id;
 892         int cpu;
 893         struct smtc_ipi *pipi;
 894         unsigned long tcstatus;
 895         int sent;
 896         long flags;
 897         unsigned int mtflags;
 898         unsigned int vpflags;
 899
 900         /*
 901          * So long as cross-VPE interrupts are done via
 902          * MFTR/MTTR read-modify-writes of Cause, we need
 903          * to stop other VPEs whenever the local VPE does
 904          * anything similar.
 905          */
 906         local_irq_save(flags);
 907         vpflags = dvpe();
 908         clear_c0_cause(0x100 << MIPS_CPU_IPI_IRQ);
 909         set_c0_status(0x100 << MIPS_CPU_IPI_IRQ);
 910         irq_enable_hazard();
 911         evpe(vpflags);
 912         local_irq_restore(flags);
 913
 914         /*
 915          * Cross-VPE Interrupt handler: Try to directly deliver IPIs
 916          * queued for TCs on this VPE other than the current one.
 917          * Return-from-interrupt should cause us to drain the queue
 918          * for the current TC, so we ought not to have to do it explicitly here.
 919          */
 920
 921         for_each_online_cpu(cpu) {
 922                 if (cpu_data[cpu].vpe_id != my_vpe)
 923                         continue;
 924
 925                 pipi = smtc_ipi_dq(&IPIQ[cpu]);
 926                 if (pipi != NULL) {
 927                         if (cpu_data[cpu].tc_id != my_tc) {
 928                                 sent = 0;
 929                                 LOCK_MT_PRA();
 930                                 settc(cpu_data[cpu].tc_id);
 931                                 write_tc_c0_tchalt(TCHALT_H);
 932                                 mips_ihb();
 933                                 tcstatus = read_tc_c0_tcstatus();
 934                                 if ((tcstatus & TCSTATUS_IXMT) == 0) {
 935                                         post_direct_ipi(cpu, pipi);
 936                                         sent = 1;
 937                                 }
 938                                 write_tc_c0_tchalt(0);
 939                                 UNLOCK_MT_PRA();
 940                                 if (!sent) {
 941                                         smtc_ipi_req(&IPIQ[cpu], pipi);
 942                                 }
 943                         } else {
 944                                 /*
 945                                  * ipi_decode() should be called
 946                                  * with interrupts off
 947                                  */
 948                                 local_irq_save(flags);
 949                                 ipi_decode(pipi);
 950                                 local_irq_restore(flags);
 951                         }
 952                 }
 953         }
 954
 955         return IRQ_HANDLED;
 956 }
 957
 958 static void ipi_irq_dispatch(void)
 959 {
 960         do_IRQ(cpu_ipi_irq);
 961 }
 962
 963 static struct irqaction irq_ipi = {
 964         .handler        = ipi_interrupt,
 965         .flags          = IRQF_DISABLED,
 966         .name           = "SMTC_IPI",
 967         .flags          = IRQF_PERCPU
 968 };
 969
 970 static void setup_cross_vpe_interrupts(unsigned int nvpe)
 971 {
 972         if (nvpe < 1)
 973                 return;
 974
 975         if (!cpu_has_vint)
 976                 panic("SMTC Kernel requires Vectored Interupt support");
 977
 978         set_vi_handler(MIPS_CPU_IPI_IRQ, ipi_irq_dispatch);
 979
 980         setup_irq_smtc(cpu_ipi_irq, &irq_ipi, (0x100 << MIPS_CPU_IPI_IRQ));
 981
 982         set_irq_handler(cpu_ipi_irq, handle_percpu_irq);
 983 }
 984
 985 /*
 986  * SMTC-specific hacks invoked from elsewhere in the kernel.
 987  *
 988  * smtc_ipi_replay is called from raw_local_irq_restore which is only ever
 989  * called with interrupts disabled.  We do rely on interrupts being disabled
 990  * here because using spin_lock_irqsave()/spin_unlock_irqrestore() would
 991  * result in a recursive call to raw_local_irq_restore().
 992  */
 993
 994 static void __smtc_ipi_replay(void)
 995 {
 996         unsigned int cpu = smp_processor_id();
 997
 998         /*
 999          * To the extent that we've ever turned interrupts off,
1000          * we may have accumulated deferred IPIs.  This is subtle.
1001          * If we use the smtc_ipi_qdepth() macro, we'll get an
1002          * exact number - but we'll also disable interrupts
1003          * and create a window of failure where a new IPI gets
1004          * queued after we test the depth but before we re-enable
1005          * interrupts. So long as IXMT never gets set, however,
1006          * we should be OK:  If we pick up something and dispatch
1007          * it here, that's great. If we see nothing, but concurrent
1008          * with this operation, another TC sends us an IPI, IXMT
1009          * is clear, and we'll handle it as a real pseudo-interrupt
1010          * and not a pseudo-pseudo interrupt.
1011          */
1012         if (IPIQ[cpu].depth > 0) {
1013                 while (1) {
1014                         struct smtc_ipi_q *q = &IPIQ[cpu];
1015                         struct smtc_ipi *pipi;
1016                         extern void self_ipi(struct smtc_ipi *);
1017
1018                         spin_lock(&q->lock);
1019                         pipi = __smtc_ipi_dq(q);
1020                         spin_unlock(&q->lock);
1021                         if (!pipi)
1022                                 break;
1023
1024                         self_ipi(pipi);
1025                         smtc_cpu_stats[cpu].selfipis++;
1026                 }
1027         }
1028 }
1029
1030 void smtc_ipi_replay(void)
1031 {
1032         raw_local_irq_disable();
1033         __smtc_ipi_replay();
1034 }
1035
1036 EXPORT_SYMBOL(smtc_ipi_replay);
1037
1038 void smtc_idle_loop_hook(void)
1039 {
1040 #ifdef CONFIG_SMTC_IDLE_HOOK_DEBUG
1041         int im;
1042         int flags;
1043         int mtflags;
1044         int bit;
1045         int vpe;
1046         int tc;
1047         int hook_ntcs;
1048         /*
1049          * printk within DMT-protected regions can deadlock,
1050          * so buffer diagnostic messages for later output.
1051          */
1052         char *pdb_msg;
1053         char id_ho_db_msg[768]; /* worst-case use should be less than 700 */
1054
1055         if (atomic_read(&idle_hook_initialized) == 0) { /* fast test */
1056                 if (atomic_add_return(1, &idle_hook_initialized) == 1) {
1057                         int mvpconf0;
1058                         /* Tedious stuff to just do once */
1059                         mvpconf0 = read_c0_mvpconf0();
1060                         hook_ntcs = ((mvpconf0 & MVPCONF0_PTC) >> MVPCONF0_PTC_SHIFT) + 1;
1061                         if (hook_ntcs > NR_CPUS)
1062                                 hook_ntcs = NR_CPUS;
1063                         for (tc = 0; tc < hook_ntcs; tc++) {
1064                                 tcnoprog[tc] = 0;
1065                                 clock_hang_reported[tc] = 0;
1066                         }
1067                         for (vpe = 0; vpe < 2; vpe++)
1068                                 for (im = 0; im < 8; im++)
1069                                         imstuckcount[vpe][im] = 0;
1070                         printk("Idle loop test hook initialized for %d TCs\n", hook_ntcs);
1071                         atomic_set(&idle_hook_initialized, 1000);
1072                 } else {
1073                         /* Someone else is initializing in parallel - let 'em finish */
1074                         while (atomic_read(&idle_hook_initialized) < 1000)
1075                                 ;
1076                 }
1077         }
1078
1079         /* Have we stupidly left IXMT set somewhere? */
1080         if (read_c0_tcstatus() & 0x400) {
1081                 write_c0_tcstatus(read_c0_tcstatus() & ~0x400);
1082                 ehb();
1083                 printk("Dangling IXMT in cpu_idle()\n");
1084         }
1085
1086         /* Have we stupidly left an IM bit turned off? */
1087 #define IM_LIMIT 2000
1088         local_irq_save(flags);
1089         mtflags = dmt();
1090         pdb_msg = &id_ho_db_msg[0];
1091         im = read_c0_status();
1092         vpe = current_cpu_data.vpe_id;
1093         for (bit = 0; bit < 8; bit++) {
1094                 /*
1095                  * In current prototype, I/O interrupts
1096                  * are masked for VPE > 0
1097                  */
1098                 if (vpemask[vpe][bit]) {
1099                         if (!(im & (0x100 << bit)))
1100                                 imstuckcount[vpe][bit]++;
1101                         else
1102                                 imstuckcount[vpe][bit] = 0;
1103                         if (imstuckcount[vpe][bit] > IM_LIMIT) {
1104                                 set_c0_status(0x100 << bit);
1105                                 ehb();
1106                                 imstuckcount[vpe][bit] = 0;
1107                                 pdb_msg += sprintf(pdb_msg,
1108                                         "Dangling IM %d fixed for VPE %d\n", bit,
1109                                         vpe);
1110                         }
1111                 }
1112         }
1113
1114         /*
1115          * Now that we limit outstanding timer IPIs, check for hung TC
1116          */
1117         for (tc = 0; tc < NR_CPUS; tc++) {
1118                 /* Don't check ourself - we'll dequeue IPIs just below */
1119                 if ((tc != smp_processor_id()) &&
1120                     ipi_timer_latch[tc] > timerq_limit) {
1121                     if (clock_hang_reported[tc] == 0) {
1122                         pdb_msg += sprintf(pdb_msg,
1123                                 "TC %d looks hung with timer latch at %d\n",
1124                                 tc, ipi_timer_latch[tc]);
1125                         clock_hang_reported[tc]++;
1126                         }
1127                 }
1128         }
1129         emt(mtflags);
1130         local_irq_restore(flags);
1131         if (pdb_msg != &id_ho_db_msg[0])
1132                 printk("CPU%d: %s", smp_processor_id(), id_ho_db_msg);
1133 #endif /* CONFIG_SMTC_IDLE_HOOK_DEBUG */
1134
1135         /*
1136          * Replay any accumulated deferred IPIs. If "Instant Replay"
1137          * is in use, there should never be any.
1138          */
1139 #ifndef CONFIG_MIPS_MT_SMTC_INSTANT_REPLAY
1140         {
1141                 unsigned long flags;
1142
1143                 local_irq_save(flags);
1144                 __smtc_ipi_replay();
1145                 local_irq_restore(flags);
1146         }
1147 #endif /* CONFIG_MIPS_MT_SMTC_INSTANT_REPLAY */
1148 }
1149
1150 void smtc_soft_dump(void)
1151 {
1152         int i;
1153
1154         printk("Counter Interrupts taken per CPU (TC)\n");
1155         for (i=0; i < NR_CPUS; i++) {
1156                 printk("%d: %ld\n", i, smtc_cpu_stats[i].timerints);
1157         }
1158         printk("Self-IPI invocations:\n");
1159         for (i=0; i < NR_CPUS; i++) {
1160                 printk("%d: %ld\n", i, smtc_cpu_stats[i].selfipis);
1161         }
1162         smtc_ipi_qdump();
1163         printk("Timer IPI Backlogs:\n");
1164         for (i=0; i < NR_CPUS; i++) {
1165                 printk("%d: %d\n", i, ipi_timer_latch[i]);
1166         }
1167         printk("%d Recoveries of \"stolen\" FPU\n",
1168                atomic_read(&smtc_fpu_recoveries));
1169 }
1170
1171
1172 /*
1173  * TLB management routines special to SMTC
1174  */
1175
1176 void smtc_get_new_mmu_context(struct mm_struct *mm, unsigned long cpu)
1177 {
1178         unsigned long flags, mtflags, tcstat, prevhalt, asid;
1179         int tlb, i;
1180
1181         /*
1182          * It would be nice to be able to use a spinlock here,
1183          * but this is invoked from within TLB flush routines
1184          * that protect themselves with DVPE, so if a lock is
1185          * held by another TC, it'll never be freed.
1186          *
1187          * DVPE/DMT must not be done with interrupts enabled,
1188          * so even so most callers will already have disabled
1189          * them, let's be really careful...
1190          */
1191
1192         local_irq_save(flags);
1193         if (smtc_status & SMTC_TLB_SHARED) {
1194                 mtflags = dvpe();
1195                 tlb = 0;
1196         } else {
1197                 mtflags = dmt();
1198                 tlb = cpu_data[cpu].vpe_id;
1199         }
1200         asid = asid_cache(cpu);
1201
1202         do {
1203                 if (!((asid += ASID_INC) & ASID_MASK) ) {
1204                         if (cpu_has_vtag_icache)
1205                                 flush_icache_all();
1206                         /* Traverse all online CPUs (hack requires contigous range) */
1207                         for (i = 0; i < num_online_cpus(); i++) {
1208                                 /*
1209                                  * We don't need to worry about our own CPU, nor those of
1210                                  * CPUs who don't share our TLB.
1211                                  */
1212                                 if ((i != smp_processor_id()) &&
1213                                     ((smtc_status & SMTC_TLB_SHARED) ||
1214                                      (cpu_data[i].vpe_id == cpu_data[cpu].vpe_id))) {
1215                                         settc(cpu_data[i].tc_id);
1216                                         prevhalt = read_tc_c0_tchalt() & TCHALT_H;
1217                                         if (!prevhalt) {
1218                                                 write_tc_c0_tchalt(TCHALT_H);
1219                                                 mips_ihb();
1220                                         }
1221                                         tcstat = read_tc_c0_tcstatus();
1222                                         smtc_live_asid[tlb][(tcstat & ASID_MASK)] |= (asiduse)(0x1 << i);
1223                                         if (!prevhalt)
1224                                                 write_tc_c0_tchalt(0);
1225                                 }
1226                         }
1227                         if (!asid)              /* fix version if needed */
1228                                 asid = ASID_FIRST_VERSION;
1229                         local_flush_tlb_all();  /* start new asid cycle */
1230                 }
1231         } while (smtc_live_asid[tlb][(asid & ASID_MASK)]);
1232
1233         /*
1234          * SMTC shares the TLB within VPEs and possibly across all VPEs.
1235          */
1236         for (i = 0; i < num_online_cpus(); i++) {
1237                 if ((smtc_status & SMTC_TLB_SHARED) ||
1238                     (cpu_data[i].vpe_id == cpu_data[cpu].vpe_id))
1239                         cpu_context(i, mm) = asid_cache(i) = asid;
1240         }
1241
1242         if (smtc_status & SMTC_TLB_SHARED)
1243                 evpe(mtflags);
1244         else
1245                 emt(mtflags);
1246         local_irq_restore(flags);
1247 }
1248
1249 /*
1250  * Invoked from macros defined in mmu_context.h
1251  * which must already have disabled interrupts
1252  * and done a DVPE or DMT as appropriate.
1253  */
1254
1255 void smtc_flush_tlb_asid(unsigned long asid)
1256 {
1257         int entry;
1258         unsigned long ehi;
1259
1260         entry = read_c0_wired();
1261
1262         /* Traverse all non-wired entries */
1263         while (entry < current_cpu_data.tlbsize) {
1264                 write_c0_index(entry);
1265                 ehb();
1266                 tlb_read();
1267                 ehb();
1268                 ehi = read_c0_entryhi();
1269                 if ((ehi & ASID_MASK) == asid) {
1270                     /*
1271                      * Invalidate only entries with specified ASID,
1272                      * makiing sure all entries differ.
1273                      */
1274                     write_c0_entryhi(CKSEG0 + (entry << (PAGE_SHIFT + 1)));
1275                     write_c0_entrylo0(0);
1276                     write_c0_entrylo1(0);
1277                     mtc0_tlbw_hazard();
1278                     tlb_write_indexed();
1279                 }
1280                 entry++;
1281         }
1282         write_c0_index(PARKED_INDEX);
1283         tlbw_use_hazard();
1284 }
1285
1286 /*
1287  * Support for single-threading cache flush operations.
1288  */
1289
1290 static int halt_state_save[NR_CPUS];
1291
1292 /*
1293  * To really, really be sure that nothing is being done
1294  * by other TCs, halt them all.  This code assumes that
1295  * a DVPE has already been done, so while their Halted
1296  * state is theoretically architecturally unstable, in
1297  * practice, it's not going to change while we're looking
1298  * at it.
1299  */
1300
1301 void smtc_cflush_lockdown(void)
1302 {
1303         int cpu;
1304
1305         for_each_online_cpu(cpu) {
1306                 if (cpu != smp_processor_id()) {
1307                         settc(cpu_data[cpu].tc_id);
1308                         halt_state_save[cpu] = read_tc_c0_tchalt();
1309                         write_tc_c0_tchalt(TCHALT_H);
1310                 }
1311         }
1312         mips_ihb();
1313 }
1314
1315 /* It would be cheating to change the cpu_online states during a flush! */
1316
1317 void smtc_cflush_release(void)
1318 {
1319         int cpu;
1320
1321         /*
1322          * Start with a hazard barrier to ensure
1323          * that all CACHE ops have played through.
1324          */
1325         mips_ihb();
1326
1327         for_each_online_cpu(cpu) {
1328                 if (cpu != smp_processor_id()) {
1329                         settc(cpu_data[cpu].tc_id);
1330                         write_tc_c0_tchalt(halt_state_save[cpu]);
1331                 }
1332         }
1333         mips_ihb();
1334 }