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); 33DEFINE_PER_CPU(uint32_t, core0_c0count); 34 35static void *ipi_set0_regs[16]; 36static void *ipi_clear0_regs[16]; 37static void *ipi_status0_regs[16]; 38static void *ipi_en0_regs[16]; 39static void *ipi_mailbox_buf[16]; 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 smp_call_function_interrupt(); 271 272 if (action & SMP_ASK_C0COUNT) { 273 BUG_ON(cpu != 0); 274 c0count = read_c0_count(); 275 for (i = 1; i < num_possible_cpus(); i++) 276 per_cpu(core0_c0count, i) = c0count; 277 } 278} 279 280#define MAX_LOOPS 1111 281/* 282 * SMP init and finish on secondary CPUs 283 */ 284static void loongson3_init_secondary(void) 285{ 286 int i; 287 uint32_t initcount; 288 unsigned int cpu = smp_processor_id(); 289 unsigned int imask = STATUSF_IP7 | STATUSF_IP6 | 290 STATUSF_IP3 | STATUSF_IP2; 291 292 /* Set interrupt mask, but don't enable */ 293 change_c0_status(ST0_IM, imask); 294 295 for (i = 0; i < num_possible_cpus(); i++) 296 loongson3_ipi_write32(0xffffffff, ipi_en0_regs[cpu_logical_map(i)]); 297 298 per_cpu(cpu_state, cpu) = CPU_ONLINE; 299 cpu_data[cpu].core = 300 cpu_logical_map(cpu) % loongson_sysconf.cores_per_package; 301 cpu_data[cpu].package = 302 cpu_logical_map(cpu) / loongson_sysconf.cores_per_package; 303 304 i = 0; 305 __this_cpu_write(core0_c0count, 0); 306 loongson3_send_ipi_single(0, SMP_ASK_C0COUNT); 307 while (!__this_cpu_read(core0_c0count)) { 308 i++; 309 cpu_relax(); 310 } 311 312 if (i > MAX_LOOPS) 313 i = MAX_LOOPS; 314 initcount = __this_cpu_read(core0_c0count) + i; 315 write_c0_count(initcount); 316} 317 318static void loongson3_smp_finish(void) 319{ 320 int cpu = smp_processor_id(); 321 322 write_c0_compare(read_c0_count() + mips_hpt_frequency/HZ); 323 local_irq_enable(); 324 loongson3_ipi_write64(0, 325 (void *)(ipi_mailbox_buf[cpu_logical_map(cpu)]+0x0)); 326 pr_info("CPU#%d finished, CP0_ST=%x\n", 327 smp_processor_id(), read_c0_status()); 328} 329 330static void __init loongson3_smp_setup(void) 331{ 332 int i = 0, num = 0; /* i: physical id, num: logical id */ 333 334 init_cpu_possible(cpu_none_mask); 335 336 /* For unified kernel, NR_CPUS is the maximum possible value, 337 * loongson_sysconf.nr_cpus is the really present value */ 338 while (i < loongson_sysconf.nr_cpus) { 339 if (loongson_sysconf.reserved_cpus_mask & (1<<i)) { 340 /* Reserved physical CPU cores */ 341 __cpu_number_map[i] = -1; 342 } else { 343 __cpu_number_map[i] = num; 344 __cpu_logical_map[num] = i; 345 set_cpu_possible(num, true); 346 num++; 347 } 348 i++; 349 } 350 pr_info("Detected %i available CPU(s)\n", num); 351 352 while (num < loongson_sysconf.nr_cpus) { 353 __cpu_logical_map[num] = -1; 354 num++; 355 } 356 357 ipi_set0_regs_init(); 358 ipi_clear0_regs_init(); 359 ipi_status0_regs_init(); 360 ipi_en0_regs_init(); 361 ipi_mailbox_buf_init(); 362 cpu_data[0].core = cpu_logical_map(0) % loongson_sysconf.cores_per_package; 363 cpu_data[0].package = cpu_logical_map(0) / loongson_sysconf.cores_per_package; 364} 365 366static void __init loongson3_prepare_cpus(unsigned int max_cpus) 367{ 368 init_cpu_present(cpu_possible_mask); 369 per_cpu(cpu_state, smp_processor_id()) = CPU_ONLINE; 370} 371 372/* 373 * Setup the PC, SP, and GP of a secondary processor and start it runing! 374 */ 375static void loongson3_boot_secondary(int cpu, struct task_struct *idle) 376{ 377 unsigned long startargs[4]; 378 379 pr_info("Booting CPU#%d...\n", cpu); 380 381 /* startargs[] are initial PC, SP and GP for secondary CPU */ 382 startargs[0] = (unsigned long)&smp_bootstrap; 383 startargs[1] = (unsigned long)__KSTK_TOS(idle); 384 startargs[2] = (unsigned long)task_thread_info(idle); 385 startargs[3] = 0; 386 387 pr_debug("CPU#%d, func_pc=%lx, sp=%lx, gp=%lx\n", 388 cpu, startargs[0], startargs[1], startargs[2]); 389 390 loongson3_ipi_write64(startargs[3], 391 (void *)(ipi_mailbox_buf[cpu_logical_map(cpu)]+0x18)); 392 loongson3_ipi_write64(startargs[2], 393 (void *)(ipi_mailbox_buf[cpu_logical_map(cpu)]+0x10)); 394 loongson3_ipi_write64(startargs[1], 395 (void *)(ipi_mailbox_buf[cpu_logical_map(cpu)]+0x8)); 396 loongson3_ipi_write64(startargs[0], 397 (void *)(ipi_mailbox_buf[cpu_logical_map(cpu)]+0x0)); 398} 399 400#ifdef CONFIG_HOTPLUG_CPU 401 402static int loongson3_cpu_disable(void) 403{ 404 unsigned long flags; 405 unsigned int cpu = smp_processor_id(); 406 407 if (cpu == 0) 408 return -EBUSY; 409 410 set_cpu_online(cpu, false); 411 cpumask_clear_cpu(cpu, &cpu_callin_map); 412 local_irq_save(flags); 413 fixup_irqs(); 414 local_irq_restore(flags); 415 flush_cache_all(); 416 local_flush_tlb_all(); 417 418 return 0; 419} 420 421 422static void loongson3_cpu_die(unsigned int cpu) 423{ 424 while (per_cpu(cpu_state, cpu) != CPU_DEAD) 425 cpu_relax(); 426 427 mb(); 428} 429 430/* To shutdown a core in Loongson 3, the target core should go to CKSEG1 and 431 * flush all L1 entries at first. Then, another core (usually Core 0) can 432 * safely disable the clock of the target core. loongson3_play_dead() is 433 * called via CKSEG1 (uncached and unmmaped) */ 434static void loongson3a_play_dead(int *state_addr) 435{ 436 register int val; 437 register long cpuid, core, node, count; 438 register void *addr, *base, *initfunc; 439 440 __asm__ __volatile__( 441 " .set push \n" 442 " .set noreorder \n" 443 " li %[addr], 0x80000000 \n" /* KSEG0 */ 444 "1: cache 0, 0(%[addr]) \n" /* flush L1 ICache */ 445 " cache 0, 1(%[addr]) \n" 446 " cache 0, 2(%[addr]) \n" 447 " cache 0, 3(%[addr]) \n" 448 " cache 1, 0(%[addr]) \n" /* flush L1 DCache */ 449 " cache 1, 1(%[addr]) \n" 450 " cache 1, 2(%[addr]) \n" 451 " cache 1, 3(%[addr]) \n" 452 " addiu %[sets], %[sets], -1 \n" 453 " bnez %[sets], 1b \n" 454 " addiu %[addr], %[addr], 0x20 \n" 455 " li %[val], 0x7 \n" /* *state_addr = CPU_DEAD; */ 456 " sw %[val], (%[state_addr]) \n" 457 " sync \n" 458 " cache 21, (%[state_addr]) \n" /* flush entry of *state_addr */ 459 " .set pop \n" 460 : [addr] "=&r" (addr), [val] "=&r" (val) 461 : [state_addr] "r" (state_addr), 462 [sets] "r" (cpu_data[smp_processor_id()].dcache.sets)); 463 464 __asm__ __volatile__( 465 " .set push \n" 466 " .set noreorder \n" 467 " .set mips64 \n" 468 " mfc0 %[cpuid], $15, 1 \n" 469 " andi %[cpuid], 0x3ff \n" 470 " dli %[base], 0x900000003ff01000 \n" 471 " andi %[core], %[cpuid], 0x3 \n" 472 " sll %[core], 8 \n" /* get core id */ 473 " or %[base], %[base], %[core] \n" 474 " andi %[node], %[cpuid], 0xc \n" 475 " dsll %[node], 42 \n" /* get node id */ 476 " or %[base], %[base], %[node] \n" 477 "1: li %[count], 0x100 \n" /* wait for init loop */ 478 "2: bnez %[count], 2b \n" /* limit mailbox access */ 479 " addiu %[count], -1 \n" 480 " ld %[initfunc], 0x20(%[base]) \n" /* get PC via mailbox */ 481 " beqz %[initfunc], 1b \n" 482 " nop \n" 483 " ld $sp, 0x28(%[base]) \n" /* get SP via mailbox */ 484 " ld $gp, 0x30(%[base]) \n" /* get GP via mailbox */ 485 " ld $a1, 0x38(%[base]) \n" 486 " jr %[initfunc] \n" /* jump to initial PC */ 487 " nop \n" 488 " .set pop \n" 489 : [core] "=&r" (core), [node] "=&r" (node), 490 [base] "=&r" (base), [cpuid] "=&r" (cpuid), 491 [count] "=&r" (count), [initfunc] "=&r" (initfunc) 492 : /* No Input */ 493 : "a1"); 494} 495 496static void loongson3b_play_dead(int *state_addr) 497{ 498 register int val; 499 register long cpuid, core, node, count; 500 register void *addr, *base, *initfunc; 501 502 __asm__ __volatile__( 503 " .set push \n" 504 " .set noreorder \n" 505 " li %[addr], 0x80000000 \n" /* KSEG0 */ 506 "1: cache 0, 0(%[addr]) \n" /* flush L1 ICache */ 507 " cache 0, 1(%[addr]) \n" 508 " cache 0, 2(%[addr]) \n" 509 " cache 0, 3(%[addr]) \n" 510 " cache 1, 0(%[addr]) \n" /* flush L1 DCache */ 511 " cache 1, 1(%[addr]) \n" 512 " cache 1, 2(%[addr]) \n" 513 " cache 1, 3(%[addr]) \n" 514 " addiu %[sets], %[sets], -1 \n" 515 " bnez %[sets], 1b \n" 516 " addiu %[addr], %[addr], 0x20 \n" 517 " li %[val], 0x7 \n" /* *state_addr = CPU_DEAD; */ 518 " sw %[val], (%[state_addr]) \n" 519 " sync \n" 520 " cache 21, (%[state_addr]) \n" /* flush entry of *state_addr */ 521 " .set pop \n" 522 : [addr] "=&r" (addr), [val] "=&r" (val) 523 : [state_addr] "r" (state_addr), 524 [sets] "r" (cpu_data[smp_processor_id()].dcache.sets)); 525 526 __asm__ __volatile__( 527 " .set push \n" 528 " .set noreorder \n" 529 " .set mips64 \n" 530 " mfc0 %[cpuid], $15, 1 \n" 531 " andi %[cpuid], 0x3ff \n" 532 " dli %[base], 0x900000003ff01000 \n" 533 " andi %[core], %[cpuid], 0x3 \n" 534 " sll %[core], 8 \n" /* get core id */ 535 " or %[base], %[base], %[core] \n" 536 " andi %[node], %[cpuid], 0xc \n" 537 " dsll %[node], 42 \n" /* get node id */ 538 " or %[base], %[base], %[node] \n" 539 " dsrl %[node], 30 \n" /* 15:14 */ 540 " or %[base], %[base], %[node] \n" 541 "1: li %[count], 0x100 \n" /* wait for init loop */ 542 "2: bnez %[count], 2b \n" /* limit mailbox access */ 543 " addiu %[count], -1 \n" 544 " ld %[initfunc], 0x20(%[base]) \n" /* get PC via mailbox */ 545 " beqz %[initfunc], 1b \n" 546 " nop \n" 547 " ld $sp, 0x28(%[base]) \n" /* get SP via mailbox */ 548 " ld $gp, 0x30(%[base]) \n" /* get GP via mailbox */ 549 " ld $a1, 0x38(%[base]) \n" 550 " jr %[initfunc] \n" /* jump to initial PC */ 551 " nop \n" 552 " .set pop \n" 553 : [core] "=&r" (core), [node] "=&r" (node), 554 [base] "=&r" (base), [cpuid] "=&r" (cpuid), 555 [count] "=&r" (count), [initfunc] "=&r" (initfunc) 556 : /* No Input */ 557 : "a1"); 558} 559 560void play_dead(void) 561{ 562 int *state_addr; 563 unsigned int cpu = smp_processor_id(); 564 void (*play_dead_at_ckseg1)(int *); 565 566 idle_task_exit(); 567 switch (loongson_sysconf.cputype) { 568 case Loongson_3A: 569 default: 570 play_dead_at_ckseg1 = 571 (void *)CKSEG1ADDR((unsigned long)loongson3a_play_dead); 572 break; 573 case Loongson_3B: 574 play_dead_at_ckseg1 = 575 (void *)CKSEG1ADDR((unsigned long)loongson3b_play_dead); 576 break; 577 } 578 state_addr = &per_cpu(cpu_state, cpu); 579 mb(); 580 play_dead_at_ckseg1(state_addr); 581} 582 583void loongson3_disable_clock(int cpu) 584{ 585 uint64_t core_id = cpu_data[cpu].core; 586 uint64_t package_id = cpu_data[cpu].package; 587 588 if (loongson_sysconf.cputype == Loongson_3A) { 589 LOONGSON_CHIPCFG(package_id) &= ~(1 << (12 + core_id)); 590 } else if (loongson_sysconf.cputype == Loongson_3B) { 591 if (!(loongson_sysconf.workarounds & WORKAROUND_CPUHOTPLUG)) 592 LOONGSON_FREQCTRL(package_id) &= ~(1 << (core_id * 4 + 3)); 593 } 594} 595 596void loongson3_enable_clock(int cpu) 597{ 598 uint64_t core_id = cpu_data[cpu].core; 599 uint64_t package_id = cpu_data[cpu].package; 600 601 if (loongson_sysconf.cputype == Loongson_3A) { 602 LOONGSON_CHIPCFG(package_id) |= 1 << (12 + core_id); 603 } else if (loongson_sysconf.cputype == Loongson_3B) { 604 if (!(loongson_sysconf.workarounds & WORKAROUND_CPUHOTPLUG)) 605 LOONGSON_FREQCTRL(package_id) |= 1 << (core_id * 4 + 3); 606 } 607} 608 609#define CPU_POST_DEAD_FROZEN (CPU_POST_DEAD | CPU_TASKS_FROZEN) 610static int loongson3_cpu_callback(struct notifier_block *nfb, 611 unsigned long action, void *hcpu) 612{ 613 unsigned int cpu = (unsigned long)hcpu; 614 615 switch (action) { 616 case CPU_POST_DEAD: 617 case CPU_POST_DEAD_FROZEN: 618 pr_info("Disable clock for CPU#%d\n", cpu); 619 loongson3_disable_clock(cpu); 620 break; 621 case CPU_UP_PREPARE: 622 case CPU_UP_PREPARE_FROZEN: 623 pr_info("Enable clock for CPU#%d\n", cpu); 624 loongson3_enable_clock(cpu); 625 break; 626 } 627 628 return NOTIFY_OK; 629} 630 631static int register_loongson3_notifier(void) 632{ 633 hotcpu_notifier(loongson3_cpu_callback, 0); 634 return 0; 635} 636early_initcall(register_loongson3_notifier); 637 638#endif 639 640struct plat_smp_ops loongson3_smp_ops = { 641 .send_ipi_single = loongson3_send_ipi_single, 642 .send_ipi_mask = loongson3_send_ipi_mask, 643 .init_secondary = loongson3_init_secondary, 644 .smp_finish = loongson3_smp_finish, 645 .boot_secondary = loongson3_boot_secondary, 646 .smp_setup = loongson3_smp_setup, 647 .prepare_cpus = loongson3_prepare_cpus, 648#ifdef CONFIG_HOTPLUG_CPU 649 .cpu_disable = loongson3_cpu_disable, 650 .cpu_die = loongson3_cpu_die, 651#endif 652}; 653