arch/mips/loongson64/loongson-3/smp.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2010, 2011, 2012, Lemote, Inc.
   4  * Author: Chen Huacai, chenhc@lemote.com
   5  */
   6
   7 #include <linux/init.h>
   8 #include <linux/cpu.h>
   9 #include <linux/sched.h>
  10 #include <linux/sched/hotplug.h>
  11 #include <linux/sched/task_stack.h>
  12 #include <linux/smp.h>
  13 #include <linux/cpufreq.h>
  14 #include <linux/kexec.h>
  15 #include <asm/processor.h>
  16 #include <asm/time.h>
  17 #include <asm/clock.h>
  18 #include <asm/tlbflush.h>
  19 #include <asm/cacheflush.h>
  20 #include <loongson.h>
  21 #include <workarounds.h>
  22
  23 #include "smp.h"
  24
  25 DEFINE_PER_CPU(int, cpu_state);
  26
  27 static void *ipi_set0_regs[16];
  28 static void *ipi_clear0_regs[16];
  29 static void *ipi_status0_regs[16];
  30 static void *ipi_en0_regs[16];
  31 static void *ipi_mailbox_buf[16];
  32 static uint32_t core0_c0count[NR_CPUS];
  33
  34 /* read a 32bit value from ipi register */
  35 #define loongson3_ipi_read32(addr) readl(addr)
  36 /* read a 64bit value from ipi register */
  37 #define loongson3_ipi_read64(addr) readq(addr)
  38 /* write a 32bit value to ipi register */
  39 #define loongson3_ipi_write32(action, addr)     \
  40         do {                                    \
  41                 writel(action, addr);           \
  42                 __wbflush();                    \
  43         } while (0)
  44 /* write a 64bit value to ipi register */
  45 #define loongson3_ipi_write64(action, addr)     \
  46         do {                                    \
  47                 writeq(action, addr);           \
  48                 __wbflush();                    \
  49         } while (0)
  50
  51 static void ipi_set0_regs_init(void)
  52 {
  53         ipi_set0_regs[0] = (void *)
  54                 (SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + SET0);
  55         ipi_set0_regs[1] = (void *)
  56                 (SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + SET0);
  57         ipi_set0_regs[2] = (void *)
  58                 (SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + SET0);
  59         ipi_set0_regs[3] = (void *)
  60                 (SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + SET0);
  61         ipi_set0_regs[4] = (void *)
  62                 (SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + SET0);
  63         ipi_set0_regs[5] = (void *)
  64                 (SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + SET0);
  65         ipi_set0_regs[6] = (void *)
  66                 (SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + SET0);
  67         ipi_set0_regs[7] = (void *)
  68                 (SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + SET0);
  69         ipi_set0_regs[8] = (void *)
  70                 (SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + SET0);
  71         ipi_set0_regs[9] = (void *)
  72                 (SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + SET0);
  73         ipi_set0_regs[10] = (void *)
  74                 (SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + SET0);
  75         ipi_set0_regs[11] = (void *)
  76                 (SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + SET0);
  77         ipi_set0_regs[12] = (void *)
  78                 (SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + SET0);
  79         ipi_set0_regs[13] = (void *)
  80                 (SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + SET0);
  81         ipi_set0_regs[14] = (void *)
  82                 (SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + SET0);
  83         ipi_set0_regs[15] = (void *)
  84                 (SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + SET0);
  85 }
  86
  87 static void ipi_clear0_regs_init(void)
  88 {
  89         ipi_clear0_regs[0] = (void *)
  90                 (SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + CLEAR0);
  91         ipi_clear0_regs[1] = (void *)
  92                 (SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + CLEAR0);
  93         ipi_clear0_regs[2] = (void *)
  94                 (SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + CLEAR0);
  95         ipi_clear0_regs[3] = (void *)
  96                 (SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + CLEAR0);
  97         ipi_clear0_regs[4] = (void *)
  98                 (SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + CLEAR0);
  99         ipi_clear0_regs[5] = (void *)
 100                 (SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + CLEAR0);
 101         ipi_clear0_regs[6] = (void *)
 102                 (SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + CLEAR0);
 103         ipi_clear0_regs[7] = (void *)
 104                 (SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + CLEAR0);
 105         ipi_clear0_regs[8] = (void *)
 106                 (SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + CLEAR0);
 107         ipi_clear0_regs[9] = (void *)
 108                 (SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + CLEAR0);
 109         ipi_clear0_regs[10] = (void *)
 110                 (SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + CLEAR0);
 111         ipi_clear0_regs[11] = (void *)
 112                 (SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + CLEAR0);
 113         ipi_clear0_regs[12] = (void *)
 114                 (SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + CLEAR0);
 115         ipi_clear0_regs[13] = (void *)
 116                 (SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + CLEAR0);
 117         ipi_clear0_regs[14] = (void *)
 118                 (SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + CLEAR0);
 119         ipi_clear0_regs[15] = (void *)
 120                 (SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + CLEAR0);
 121 }
 122
 123 static void ipi_status0_regs_init(void)
 124 {
 125         ipi_status0_regs[0] = (void *)
 126                 (SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + STATUS0);
 127         ipi_status0_regs[1] = (void *)
 128                 (SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + STATUS0);
 129         ipi_status0_regs[2] = (void *)
 130                 (SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + STATUS0);
 131         ipi_status0_regs[3] = (void *)
 132                 (SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + STATUS0);
 133         ipi_status0_regs[4] = (void *)
 134                 (SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + STATUS0);
 135         ipi_status0_regs[5] = (void *)
 136                 (SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + STATUS0);
 137         ipi_status0_regs[6] = (void *)
 138                 (SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + STATUS0);
 139         ipi_status0_regs[7] = (void *)
 140                 (SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + STATUS0);
 141         ipi_status0_regs[8] = (void *)
 142                 (SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + STATUS0);
 143         ipi_status0_regs[9] = (void *)
 144                 (SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + STATUS0);
 145         ipi_status0_regs[10] = (void *)
 146                 (SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + STATUS0);
 147         ipi_status0_regs[11] = (void *)
 148                 (SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + STATUS0);
 149         ipi_status0_regs[12] = (void *)
 150                 (SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + STATUS0);
 151         ipi_status0_regs[13] = (void *)
 152                 (SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + STATUS0);
 153         ipi_status0_regs[14] = (void *)
 154                 (SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + STATUS0);
 155         ipi_status0_regs[15] = (void *)
 156                 (SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + STATUS0);
 157 }
 158
 159 static void ipi_en0_regs_init(void)
 160 {
 161         ipi_en0_regs[0] = (void *)
 162                 (SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + EN0);
 163         ipi_en0_regs[1] = (void *)
 164                 (SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + EN0);
 165         ipi_en0_regs[2] = (void *)
 166                 (SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + EN0);
 167         ipi_en0_regs[3] = (void *)
 168                 (SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + EN0);
 169         ipi_en0_regs[4] = (void *)
 170                 (SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + EN0);
 171         ipi_en0_regs[5] = (void *)
 172                 (SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + EN0);
 173         ipi_en0_regs[6] = (void *)
 174                 (SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + EN0);
 175         ipi_en0_regs[7] = (void *)
 176                 (SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + EN0);
 177         ipi_en0_regs[8] = (void *)
 178                 (SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + EN0);
 179         ipi_en0_regs[9] = (void *)
 180                 (SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + EN0);
 181         ipi_en0_regs[10] = (void *)
 182                 (SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + EN0);
 183         ipi_en0_regs[11] = (void *)
 184                 (SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + EN0);
 185         ipi_en0_regs[12] = (void *)
 186                 (SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + EN0);
 187         ipi_en0_regs[13] = (void *)
 188                 (SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + EN0);
 189         ipi_en0_regs[14] = (void *)
 190                 (SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + EN0);
 191         ipi_en0_regs[15] = (void *)
 192                 (SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + EN0);
 193 }
 194
 195 static void ipi_mailbox_buf_init(void)
 196 {
 197         ipi_mailbox_buf[0] = (void *)
 198                 (SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + BUF);
 199         ipi_mailbox_buf[1] = (void *)
 200                 (SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + BUF);
 201         ipi_mailbox_buf[2] = (void *)
 202                 (SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + BUF);
 203         ipi_mailbox_buf[3] = (void *)
 204                 (SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + BUF);
 205         ipi_mailbox_buf[4] = (void *)
 206                 (SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + BUF);
 207         ipi_mailbox_buf[5] = (void *)
 208                 (SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + BUF);
 209         ipi_mailbox_buf[6] = (void *)
 210                 (SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + BUF);
 211         ipi_mailbox_buf[7] = (void *)
 212                 (SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + BUF);
 213         ipi_mailbox_buf[8] = (void *)
 214                 (SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + BUF);
 215         ipi_mailbox_buf[9] = (void *)
 216                 (SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + BUF);
 217         ipi_mailbox_buf[10] = (void *)
 218                 (SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + BUF);
 219         ipi_mailbox_buf[11] = (void *)
 220                 (SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + BUF);
 221         ipi_mailbox_buf[12] = (void *)
 222                 (SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + BUF);
 223         ipi_mailbox_buf[13] = (void *)
 224                 (SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + BUF);
 225         ipi_mailbox_buf[14] = (void *)
 226                 (SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + BUF);
 227         ipi_mailbox_buf[15] = (void *)
 228                 (SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + BUF);
 229 }
 230
 231 /*
 232  * Simple enough, just poke the appropriate ipi register
 233  */
 234 static void loongson3_send_ipi_single(int cpu, unsigned int action)
 235 {
 236         loongson3_ipi_write32((u32)action, ipi_set0_regs[cpu_logical_map(cpu)]);
 237 }
 238
 239 static void
 240 loongson3_send_ipi_mask(const struct cpumask *mask, unsigned int action)
 241 {
 242         unsigned int i;
 243
 244         for_each_cpu(i, mask)
 245                 loongson3_ipi_write32((u32)action, ipi_set0_regs[cpu_logical_map(i)]);
 246 }
 247
 248 #define IPI_IRQ_OFFSET 6
 249
 250 void loongson3_send_irq_by_ipi(int cpu, int irqs)
 251 {
 252         loongson3_ipi_write32(irqs << IPI_IRQ_OFFSET, ipi_set0_regs[cpu_logical_map(cpu)]);
 253 }
 254
 255 void loongson3_ipi_interrupt(struct pt_regs *regs)
 256 {
 257         int i, cpu = smp_processor_id();
 258         unsigned int action, c0count, irqs;
 259
 260         /* Load the ipi register to figure out what we're supposed to do */
 261         action = loongson3_ipi_read32(ipi_status0_regs[cpu_logical_map(cpu)]);
 262         irqs = action >> IPI_IRQ_OFFSET;
 263
 264         /* Clear the ipi register to clear the interrupt */
 265         loongson3_ipi_write32((u32)action, ipi_clear0_regs[cpu_logical_map(cpu)]);
 266
 267         if (action & SMP_RESCHEDULE_YOURSELF)
 268                 scheduler_ipi();
 269
 270         if (action & SMP_CALL_FUNCTION) {
 271                 irq_enter();
 272                 generic_smp_call_function_interrupt();
 273                 irq_exit();
 274         }
 275
 276         if (action & SMP_ASK_C0COUNT) {
 277                 BUG_ON(cpu != 0);
 278                 c0count = read_c0_count();
 279                 c0count = c0count ? c0count : 1;
 280                 for (i = 1; i < nr_cpu_ids; i++)
 281                         core0_c0count[i] = c0count;
 282                 __wbflush(); /* Let others see the result ASAP */
 283         }
 284
 285         if (irqs) {
 286                 int irq;
 287                 while ((irq = ffs(irqs))) {
 288                         do_IRQ(irq-1);
 289                         irqs &= ~(1<<(irq-1));
 290                 }
 291         }
 292 }
 293
 294 #define MAX_LOOPS 800
 295 /*
 296  * SMP init and finish on secondary CPUs
 297  */
 298 static void loongson3_init_secondary(void)
 299 {
 300         int i;
 301         uint32_t initcount;
 302         unsigned int cpu = smp_processor_id();
 303         unsigned int imask = STATUSF_IP7 | STATUSF_IP6 |
 304                              STATUSF_IP3 | STATUSF_IP2;
 305
 306         /* Set interrupt mask, but don't enable */
 307         change_c0_status(ST0_IM, imask);
 308
 309         for (i = 0; i < num_possible_cpus(); i++)
 310                 loongson3_ipi_write32(0xffffffff, ipi_en0_regs[cpu_logical_map(i)]);
 311
 312         per_cpu(cpu_state, cpu) = CPU_ONLINE;
 313         cpu_set_core(&cpu_data[cpu],
 314                      cpu_logical_map(cpu) % loongson_sysconf.cores_per_package);
 315         cpu_data[cpu].package =
 316                 cpu_logical_map(cpu) / loongson_sysconf.cores_per_package;
 317
 318         i = 0;
 319         core0_c0count[cpu] = 0;
 320         loongson3_send_ipi_single(0, SMP_ASK_C0COUNT);
 321         while (!core0_c0count[cpu]) {
 322                 i++;
 323                 cpu_relax();
 324         }
 325
 326         if (i > MAX_LOOPS)
 327                 i = MAX_LOOPS;
 328         if (cpu_data[cpu].package)
 329                 initcount = core0_c0count[cpu] + i;
 330         else /* Local access is faster for loops */
 331                 initcount = core0_c0count[cpu] + i/2;
 332
 333         write_c0_count(initcount);
 334 }
 335
 336 static void loongson3_smp_finish(void)
 337 {
 338         int cpu = smp_processor_id();
 339
 340         write_c0_compare(read_c0_count() + mips_hpt_frequency/HZ);
 341         local_irq_enable();
 342         loongson3_ipi_write64(0,
 343                         ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x0);
 344         pr_info("CPU#%d finished, CP0_ST=%x\n",
 345                         smp_processor_id(), read_c0_status());
 346 }
 347
 348 static void __init loongson3_smp_setup(void)
 349 {
 350         int i = 0, num = 0; /* i: physical id, num: logical id */
 351
 352         init_cpu_possible(cpu_none_mask);
 353
 354         /* For unified kernel, NR_CPUS is the maximum possible value,
 355          * loongson_sysconf.nr_cpus is the really present value */
 356         while (i < loongson_sysconf.nr_cpus) {
 357                 if (loongson_sysconf.reserved_cpus_mask & (1<<i)) {
 358                         /* Reserved physical CPU cores */
 359                         __cpu_number_map[i] = -1;
 360                 } else {
 361                         __cpu_number_map[i] = num;
 362                         __cpu_logical_map[num] = i;
 363                         set_cpu_possible(num, true);
 364                         num++;
 365                 }
 366                 i++;
 367         }
 368         pr_info("Detected %i available CPU(s)\n", num);
 369
 370         while (num < loongson_sysconf.nr_cpus) {
 371                 __cpu_logical_map[num] = -1;
 372                 num++;
 373         }
 374
 375         ipi_set0_regs_init();
 376         ipi_clear0_regs_init();
 377         ipi_status0_regs_init();
 378         ipi_en0_regs_init();
 379         ipi_mailbox_buf_init();
 380         cpu_set_core(&cpu_data[0],
 381                      cpu_logical_map(0) % loongson_sysconf.cores_per_package);
 382         cpu_data[0].package = cpu_logical_map(0) / loongson_sysconf.cores_per_package;
 383 }
 384
 385 static void __init loongson3_prepare_cpus(unsigned int max_cpus)
 386 {
 387         init_cpu_present(cpu_possible_mask);
 388         per_cpu(cpu_state, smp_processor_id()) = CPU_ONLINE;
 389 }
 390
 391 /*
 392  * Setup the PC, SP, and GP of a secondary processor and start it runing!
 393  */
 394 static int loongson3_boot_secondary(int cpu, struct task_struct *idle)
 395 {
 396         unsigned long startargs[4];
 397
 398         pr_info("Booting CPU#%d...\n", cpu);
 399
 400         /* startargs[] are initial PC, SP and GP for secondary CPU */
 401         startargs[0] = (unsigned long)&smp_bootstrap;
 402         startargs[1] = (unsigned long)__KSTK_TOS(idle);
 403         startargs[2] = (unsigned long)task_thread_info(idle);
 404         startargs[3] = 0;
 405
 406         pr_debug("CPU#%d, func_pc=%lx, sp=%lx, gp=%lx\n",
 407                         cpu, startargs[0], startargs[1], startargs[2]);
 408
 409         loongson3_ipi_write64(startargs[3],
 410                         ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x18);
 411         loongson3_ipi_write64(startargs[2],
 412                         ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x10);
 413         loongson3_ipi_write64(startargs[1],
 414                         ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x8);
 415         loongson3_ipi_write64(startargs[0],
 416                         ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x0);
 417         return 0;
 418 }
 419
 420 #ifdef CONFIG_HOTPLUG_CPU
 421
 422 static int loongson3_cpu_disable(void)
 423 {
 424         unsigned long flags;
 425         unsigned int cpu = smp_processor_id();
 426
 427         if (cpu == 0)
 428                 return -EBUSY;
 429
 430         set_cpu_online(cpu, false);
 431         calculate_cpu_foreign_map();
 432         local_irq_save(flags);
 433         fixup_irqs();
 434         local_irq_restore(flags);
 435         local_flush_tlb_all();
 436
 437         return 0;
 438 }
 439
 440
 441 static void loongson3_cpu_die(unsigned int cpu)
 442 {
 443         while (per_cpu(cpu_state, cpu) != CPU_DEAD)
 444                 cpu_relax();
 445
 446         mb();
 447 }
 448
 449 /* To shutdown a core in Loongson 3, the target core should go to CKSEG1 and
 450  * flush all L1 entries at first. Then, another core (usually Core 0) can
 451  * safely disable the clock of the target core. loongson3_play_dead() is
 452  * called via CKSEG1 (uncached and unmmaped) */
 453 static void loongson3a_r1_play_dead(int *state_addr)
 454 {
 455         register int val;
 456         register long cpuid, core, node, count;
 457         register void *addr, *base, *initfunc;
 458
 459         __asm__ __volatile__(
 460                 "   .set push                     \n"
 461                 "   .set noreorder                \n"
 462                 "   li %[addr], 0x80000000        \n" /* KSEG0 */
 463                 "1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
 464                 "   cache 0, 1(%[addr])           \n"
 465                 "   cache 0, 2(%[addr])           \n"
 466                 "   cache 0, 3(%[addr])           \n"
 467                 "   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
 468                 "   cache 1, 1(%[addr])           \n"
 469                 "   cache 1, 2(%[addr])           \n"
 470                 "   cache 1, 3(%[addr])           \n"
 471                 "   addiu %[sets], %[sets], -1    \n"
 472                 "   bnez  %[sets], 1b             \n"
 473                 "   addiu %[addr], %[addr], 0x20  \n"
 474                 "   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
 475                 "   sw    %[val], (%[state_addr]) \n"
 476                 "   sync                          \n"
 477                 "   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
 478                 "   .set pop                      \n"
 479                 : [addr] "=&r" (addr), [val] "=&r" (val)
 480                 : [state_addr] "r" (state_addr),
 481                   [sets] "r" (cpu_data[smp_processor_id()].dcache.sets));
 482
 483         __asm__ __volatile__(
 484                 "   .set push                         \n"
 485                 "   .set noreorder                    \n"
 486                 "   .set mips64                       \n"
 487                 "   mfc0  %[cpuid], $15, 1            \n"
 488                 "   andi  %[cpuid], 0x3ff             \n"
 489                 "   dli   %[base], 0x900000003ff01000 \n"
 490                 "   andi  %[core], %[cpuid], 0x3      \n"
 491                 "   sll   %[core], 8                  \n" /* get core id */
 492                 "   or    %[base], %[base], %[core]   \n"
 493                 "   andi  %[node], %[cpuid], 0xc      \n"
 494                 "   dsll  %[node], 42                 \n" /* get node id */
 495                 "   or    %[base], %[base], %[node]   \n"
 496                 "1: li    %[count], 0x100             \n" /* wait for init loop */
 497                 "2: bnez  %[count], 2b                \n" /* limit mailbox access */
 498                 "   addiu %[count], -1                \n"
 499                 "   ld    %[initfunc], 0x20(%[base])  \n" /* get PC via mailbox */
 500                 "   beqz  %[initfunc], 1b             \n"
 501                 "   nop                               \n"
 502                 "   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
 503                 "   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
 504                 "   ld    $a1, 0x38(%[base])          \n"
 505                 "   jr    %[initfunc]                 \n" /* jump to initial PC */
 506                 "   nop                               \n"
 507                 "   .set pop                          \n"
 508                 : [core] "=&r" (core), [node] "=&r" (node),
 509                   [base] "=&r" (base), [cpuid] "=&r" (cpuid),
 510                   [count] "=&r" (count), [initfunc] "=&r" (initfunc)
 511                 : /* No Input */
 512                 : "a1");
 513 }
 514
 515 static void loongson3a_r2r3_play_dead(int *state_addr)
 516 {
 517         register int val;
 518         register long cpuid, core, node, count;
 519         register void *addr, *base, *initfunc;
 520
 521         __asm__ __volatile__(
 522                 "   .set push                     \n"
 523                 "   .set noreorder                \n"
 524                 "   li %[addr], 0x80000000        \n" /* KSEG0 */
 525                 "1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
 526                 "   cache 0, 1(%[addr])           \n"
 527                 "   cache 0, 2(%[addr])           \n"
 528                 "   cache 0, 3(%[addr])           \n"
 529                 "   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
 530                 "   cache 1, 1(%[addr])           \n"
 531                 "   cache 1, 2(%[addr])           \n"
 532                 "   cache 1, 3(%[addr])           \n"
 533                 "   addiu %[sets], %[sets], -1    \n"
 534                 "   bnez  %[sets], 1b             \n"
 535                 "   addiu %[addr], %[addr], 0x40  \n"
 536                 "   li %[addr], 0x80000000        \n" /* KSEG0 */
 537                 "2: cache 2, 0(%[addr])           \n" /* flush L1 VCache */
 538                 "   cache 2, 1(%[addr])           \n"
 539                 "   cache 2, 2(%[addr])           \n"
 540                 "   cache 2, 3(%[addr])           \n"
 541                 "   cache 2, 4(%[addr])           \n"
 542                 "   cache 2, 5(%[addr])           \n"
 543                 "   cache 2, 6(%[addr])           \n"
 544                 "   cache 2, 7(%[addr])           \n"
 545                 "   cache 2, 8(%[addr])           \n"
 546                 "   cache 2, 9(%[addr])           \n"
 547                 "   cache 2, 10(%[addr])          \n"
 548                 "   cache 2, 11(%[addr])          \n"
 549                 "   cache 2, 12(%[addr])          \n"
 550                 "   cache 2, 13(%[addr])          \n"
 551                 "   cache 2, 14(%[addr])          \n"
 552                 "   cache 2, 15(%[addr])          \n"
 553                 "   addiu %[vsets], %[vsets], -1  \n"
 554                 "   bnez  %[vsets], 2b            \n"
 555                 "   addiu %[addr], %[addr], 0x40  \n"
 556                 "   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
 557                 "   sw    %[val], (%[state_addr]) \n"
 558                 "   sync                          \n"
 559                 "   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
 560                 "   .set pop                      \n"
 561                 : [addr] "=&r" (addr), [val] "=&r" (val)
 562                 : [state_addr] "r" (state_addr),
 563                   [sets] "r" (cpu_data[smp_processor_id()].dcache.sets),
 564                   [vsets] "r" (cpu_data[smp_processor_id()].vcache.sets));
 565
 566         __asm__ __volatile__(
 567                 "   .set push                         \n"
 568                 "   .set noreorder                    \n"
 569                 "   .set mips64                       \n"
 570                 "   mfc0  %[cpuid], $15, 1            \n"
 571                 "   andi  %[cpuid], 0x3ff             \n"
 572                 "   dli   %[base], 0x900000003ff01000 \n"
 573                 "   andi  %[core], %[cpuid], 0x3      \n"
 574                 "   sll   %[core], 8                  \n" /* get core id */
 575                 "   or    %[base], %[base], %[core]   \n"
 576                 "   andi  %[node], %[cpuid], 0xc      \n"
 577                 "   dsll  %[node], 42                 \n" /* get node id */
 578                 "   or    %[base], %[base], %[node]   \n"
 579                 "1: li    %[count], 0x100             \n" /* wait for init loop */
 580                 "2: bnez  %[count], 2b                \n" /* limit mailbox access */
 581                 "   addiu %[count], -1                \n"
 582                 "   ld    %[initfunc], 0x20(%[base])  \n" /* get PC via mailbox */
 583                 "   beqz  %[initfunc], 1b             \n"
 584                 "   nop                               \n"
 585                 "   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
 586                 "   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
 587                 "   ld    $a1, 0x38(%[base])          \n"
 588                 "   jr    %[initfunc]                 \n" /* jump to initial PC */
 589                 "   nop                               \n"
 590                 "   .set pop                          \n"
 591                 : [core] "=&r" (core), [node] "=&r" (node),
 592                   [base] "=&r" (base), [cpuid] "=&r" (cpuid),
 593                   [count] "=&r" (count), [initfunc] "=&r" (initfunc)
 594                 : /* No Input */
 595                 : "a1");
 596 }
 597
 598 static void loongson3b_play_dead(int *state_addr)
 599 {
 600         register int val;
 601         register long cpuid, core, node, count;
 602         register void *addr, *base, *initfunc;
 603
 604         __asm__ __volatile__(
 605                 "   .set push                     \n"
 606                 "   .set noreorder                \n"
 607                 "   li %[addr], 0x80000000        \n" /* KSEG0 */
 608                 "1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
 609                 "   cache 0, 1(%[addr])           \n"
 610                 "   cache 0, 2(%[addr])           \n"
 611                 "   cache 0, 3(%[addr])           \n"
 612                 "   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
 613                 "   cache 1, 1(%[addr])           \n"
 614                 "   cache 1, 2(%[addr])           \n"
 615                 "   cache 1, 3(%[addr])           \n"
 616                 "   addiu %[sets], %[sets], -1    \n"
 617                 "   bnez  %[sets], 1b             \n"
 618                 "   addiu %[addr], %[addr], 0x20  \n"
 619                 "   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
 620                 "   sw    %[val], (%[state_addr]) \n"
 621                 "   sync                          \n"
 622                 "   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
 623                 "   .set pop                      \n"
 624                 : [addr] "=&r" (addr), [val] "=&r" (val)
 625                 : [state_addr] "r" (state_addr),
 626                   [sets] "r" (cpu_data[smp_processor_id()].dcache.sets));
 627
 628         __asm__ __volatile__(
 629                 "   .set push                         \n"
 630                 "   .set noreorder                    \n"
 631                 "   .set mips64                       \n"
 632                 "   mfc0  %[cpuid], $15, 1            \n"
 633                 "   andi  %[cpuid], 0x3ff             \n"
 634                 "   dli   %[base], 0x900000003ff01000 \n"
 635                 "   andi  %[core], %[cpuid], 0x3      \n"
 636                 "   sll   %[core], 8                  \n" /* get core id */
 637                 "   or    %[base], %[base], %[core]   \n"
 638                 "   andi  %[node], %[cpuid], 0xc      \n"
 639                 "   dsll  %[node], 42                 \n" /* get node id */
 640                 "   or    %[base], %[base], %[node]   \n"
 641                 "   dsrl  %[node], 30                 \n" /* 15:14 */
 642                 "   or    %[base], %[base], %[node]   \n"
 643                 "1: li    %[count], 0x100             \n" /* wait for init loop */
 644                 "2: bnez  %[count], 2b                \n" /* limit mailbox access */
 645                 "   addiu %[count], -1                \n"
 646                 "   ld    %[initfunc], 0x20(%[base])  \n" /* get PC via mailbox */
 647                 "   beqz  %[initfunc], 1b             \n"
 648                 "   nop                               \n"
 649                 "   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
 650                 "   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
 651                 "   ld    $a1, 0x38(%[base])          \n"
 652                 "   jr    %[initfunc]                 \n" /* jump to initial PC */
 653                 "   nop                               \n"
 654                 "   .set pop                          \n"
 655                 : [core] "=&r" (core), [node] "=&r" (node),
 656                   [base] "=&r" (base), [cpuid] "=&r" (cpuid),
 657                   [count] "=&r" (count), [initfunc] "=&r" (initfunc)
 658                 : /* No Input */
 659                 : "a1");
 660 }
 661
 662 void play_dead(void)
 663 {
 664         int *state_addr;
 665         unsigned int cpu = smp_processor_id();
 666         void (*play_dead_at_ckseg1)(int *);
 667
 668         idle_task_exit();
 669         switch (read_c0_prid() & PRID_REV_MASK) {
 670         case PRID_REV_LOONGSON3A_R1:
 671         default:
 672                 play_dead_at_ckseg1 =
 673                         (void *)CKSEG1ADDR((unsigned long)loongson3a_r1_play_dead);
 674                 break;
 675         case PRID_REV_LOONGSON3A_R2_0:
 676         case PRID_REV_LOONGSON3A_R2_1:
 677         case PRID_REV_LOONGSON3A_R3_0:
 678         case PRID_REV_LOONGSON3A_R3_1:
 679                 play_dead_at_ckseg1 =
 680                         (void *)CKSEG1ADDR((unsigned long)loongson3a_r2r3_play_dead);
 681                 break;
 682         case PRID_REV_LOONGSON3B_R1:
 683         case PRID_REV_LOONGSON3B_R2:
 684                 play_dead_at_ckseg1 =
 685                         (void *)CKSEG1ADDR((unsigned long)loongson3b_play_dead);
 686                 break;
 687         }
 688         state_addr = &per_cpu(cpu_state, cpu);
 689         mb();
 690         play_dead_at_ckseg1(state_addr);
 691 }
 692
 693 static int loongson3_disable_clock(unsigned int cpu)
 694 {
 695         uint64_t core_id = cpu_core(&cpu_data[cpu]);
 696         uint64_t package_id = cpu_data[cpu].package;
 697
 698         if ((read_c0_prid() & PRID_REV_MASK) == PRID_REV_LOONGSON3A_R1) {
 699                 LOONGSON_CHIPCFG(package_id) &= ~(1 << (12 + core_id));
 700         } else {
 701                 if (!(loongson_sysconf.workarounds & WORKAROUND_CPUHOTPLUG))
 702                         LOONGSON_FREQCTRL(package_id) &= ~(1 << (core_id * 4 + 3));
 703         }
 704         return 0;
 705 }
 706
 707 static int loongson3_enable_clock(unsigned int cpu)
 708 {
 709         uint64_t core_id = cpu_core(&cpu_data[cpu]);
 710         uint64_t package_id = cpu_data[cpu].package;
 711
 712         if ((read_c0_prid() & PRID_REV_MASK) == PRID_REV_LOONGSON3A_R1) {
 713                 LOONGSON_CHIPCFG(package_id) |= 1 << (12 + core_id);
 714         } else {
 715                 if (!(loongson_sysconf.workarounds & WORKAROUND_CPUHOTPLUG))
 716                         LOONGSON_FREQCTRL(package_id) |= 1 << (core_id * 4 + 3);
 717         }
 718         return 0;
 719 }
 720
 721 static int register_loongson3_notifier(void)
 722 {
 723         return cpuhp_setup_state_nocalls(CPUHP_MIPS_SOC_PREPARE,
 724                                          "mips/loongson:prepare",
 725                                          loongson3_enable_clock,
 726                                          loongson3_disable_clock);
 727 }
 728 early_initcall(register_loongson3_notifier);
 729
 730 #endif
 731
 732 const struct plat_smp_ops loongson3_smp_ops = {
 733         .send_ipi_single = loongson3_send_ipi_single,
 734         .send_ipi_mask = loongson3_send_ipi_mask,
 735         .init_secondary = loongson3_init_secondary,
 736         .smp_finish = loongson3_smp_finish,
 737         .boot_secondary = loongson3_boot_secondary,
 738         .smp_setup = loongson3_smp_setup,
 739         .prepare_cpus = loongson3_prepare_cpus,
 740 #ifdef CONFIG_HOTPLUG_CPU
 741         .cpu_disable = loongson3_cpu_disable,
 742         .cpu_die = loongson3_cpu_die,
 743 #endif
 744 #ifdef CONFIG_KEXEC
 745         .kexec_nonboot_cpu = kexec_nonboot_cpu_jump,
 746 #endif
 747 };