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