1/* 2 * Copyright (C) 2013 Imagination Technologies 3 * Author: Paul Burton <paul.burton@imgtec.com> 4 * 5 * This program is free software; you can redistribute it and/or modify it 6 * under the terms of the GNU General Public License as published by the 7 * Free Software Foundation; either version 2 of the License, or (at your 8 * option) any later version. 9 */ 10 11#include <asm/addrspace.h> 12#include <asm/asm.h> 13#include <asm/asm-offsets.h> 14#include <asm/asmmacro.h> 15#include <asm/cacheops.h> 16#include <asm/eva.h> 17#include <asm/mipsregs.h> 18#include <asm/mipsmtregs.h> 19#include <asm/pm.h> 20 21#define GCR_CL_COHERENCE_OFS 0x2008 22#define GCR_CL_ID_OFS 0x2028 23 24.extern mips_cm_base 25 26.set noreorder 27 28 /* 29 * Set dest to non-zero if the core supports the MT ASE, else zero. If 30 * MT is not supported then branch to nomt. 31 */ 32 .macro has_mt dest, nomt 33 mfc0 \dest, CP0_CONFIG 34 bgez \dest, \nomt 35 mfc0 \dest, CP0_CONFIG, 1 36 bgez \dest, \nomt 37 mfc0 \dest, CP0_CONFIG, 2 38 bgez \dest, \nomt 39 mfc0 \dest, CP0_CONFIG, 3 40 andi \dest, \dest, MIPS_CONF3_MT 41 beqz \dest, \nomt 42 .endm 43 44.section .text.cps-vec 45.balign 0x1000 46 47LEAF(mips_cps_core_entry) 48 /* 49 * These first 12 bytes will be patched by cps_smp_setup to load the 50 * base address of the CM GCRs into register v1 and the CCA to use into 51 * register s0. 52 */ 53 .quad 0 54 .word 0 55 56 /* Check whether we're here due to an NMI */ 57 mfc0 k0, CP0_STATUS 58 and k0, k0, ST0_NMI 59 beqz k0, not_nmi 60 nop 61 62 /* This is an NMI */ 63 la k0, nmi_handler 64 jr k0 65 nop 66 67not_nmi: 68 /* Setup Cause */ 69 li t0, CAUSEF_IV 70 mtc0 t0, CP0_CAUSE 71 72 /* Setup Status */ 73 li t0, ST0_CU1 | ST0_CU0 74 mtc0 t0, CP0_STATUS 75 76 /* 77 * Clear the bits used to index the caches. Note that the architecture 78 * dictates that writing to any of TagLo or TagHi selects 0 or 2 should 79 * be valid for all MIPS32 CPUs, even those for which said writes are 80 * unnecessary. 81 */ 82 mtc0 zero, CP0_TAGLO, 0 83 mtc0 zero, CP0_TAGHI, 0 84 mtc0 zero, CP0_TAGLO, 2 85 mtc0 zero, CP0_TAGHI, 2 86 ehb 87 88 /* Primary cache configuration is indicated by Config1 */ 89 mfc0 v0, CP0_CONFIG, 1 90 91 /* Detect I-cache line size */ 92 _EXT t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ 93 beqz t0, icache_done 94 li t1, 2 95 sllv t0, t1, t0 96 97 /* Detect I-cache size */ 98 _EXT t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ 99 xori t2, t1, 0x7 100 beqz t2, 1f 101 li t3, 32 102 addi t1, t1, 1 103 sllv t1, t3, t1 1041: /* At this point t1 == I-cache sets per way */ 105 _EXT t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ 106 addi t2, t2, 1 107 mul t1, t1, t0 108 mul t1, t1, t2 109 110 li a0, KSEG0 111 add a1, a0, t1 1121: cache Index_Store_Tag_I, 0(a0) 113 add a0, a0, t0 114 bne a0, a1, 1b 115 nop 116icache_done: 117 118 /* Detect D-cache line size */ 119 _EXT t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ 120 beqz t0, dcache_done 121 li t1, 2 122 sllv t0, t1, t0 123 124 /* Detect D-cache size */ 125 _EXT t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ 126 xori t2, t1, 0x7 127 beqz t2, 1f 128 li t3, 32 129 addi t1, t1, 1 130 sllv t1, t3, t1 1311: /* At this point t1 == D-cache sets per way */ 132 _EXT t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ 133 addi t2, t2, 1 134 mul t1, t1, t0 135 mul t1, t1, t2 136 137 li a0, KSEG0 138 addu a1, a0, t1 139 subu a1, a1, t0 1401: cache Index_Store_Tag_D, 0(a0) 141 bne a0, a1, 1b 142 add a0, a0, t0 143dcache_done: 144 145 /* Set Kseg0 CCA to that in s0 */ 146 mfc0 t0, CP0_CONFIG 147 ori t0, 0x7 148 xori t0, 0x7 149 or t0, t0, s0 150 mtc0 t0, CP0_CONFIG 151 ehb 152 153 /* Enter the coherent domain */ 154 li t0, 0xff 155 sw t0, GCR_CL_COHERENCE_OFS(v1) 156 ehb 157 158 /* Jump to kseg0 */ 159 la t0, 1f 160 jr t0 161 nop 162 163 /* 164 * We're up, cached & coherent. Perform any further required core-level 165 * initialisation. 166 */ 1671: jal mips_cps_core_init 168 nop 169 170 /* Do any EVA initialization if necessary */ 171 eva_init 172 173 /* 174 * Boot any other VPEs within this core that should be online, and 175 * deactivate this VPE if it should be offline. 176 */ 177 jal mips_cps_boot_vpes 178 nop 179 180 /* Off we go! */ 181 lw t1, VPEBOOTCFG_PC(v0) 182 lw gp, VPEBOOTCFG_GP(v0) 183 lw sp, VPEBOOTCFG_SP(v0) 184 jr t1 185 nop 186 END(mips_cps_core_entry) 187 188.org 0x200 189LEAF(excep_tlbfill) 190 b . 191 nop 192 END(excep_tlbfill) 193 194.org 0x280 195LEAF(excep_xtlbfill) 196 b . 197 nop 198 END(excep_xtlbfill) 199 200.org 0x300 201LEAF(excep_cache) 202 b . 203 nop 204 END(excep_cache) 205 206.org 0x380 207LEAF(excep_genex) 208 b . 209 nop 210 END(excep_genex) 211 212.org 0x400 213LEAF(excep_intex) 214 b . 215 nop 216 END(excep_intex) 217 218.org 0x480 219LEAF(excep_ejtag) 220 la k0, ejtag_debug_handler 221 jr k0 222 nop 223 END(excep_ejtag) 224 225LEAF(mips_cps_core_init) 226#ifdef CONFIG_MIPS_MT 227 /* Check that the core implements the MT ASE */ 228 has_mt t0, 3f 229 nop 230 231 .set push 232 .set mips32r2 233 .set mt 234 235 /* Only allow 1 TC per VPE to execute... */ 236 dmt 237 238 /* ...and for the moment only 1 VPE */ 239 dvpe 240 la t1, 1f 241 jr.hb t1 242 nop 243 244 /* Enter VPE configuration state */ 2451: mfc0 t0, CP0_MVPCONTROL 246 ori t0, t0, MVPCONTROL_VPC 247 mtc0 t0, CP0_MVPCONTROL 248 249 /* Retrieve the number of VPEs within the core */ 250 mfc0 t0, CP0_MVPCONF0 251 srl t0, t0, MVPCONF0_PVPE_SHIFT 252 andi t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT) 253 addi t7, t0, 1 254 255 /* If there's only 1, we're done */ 256 beqz t0, 2f 257 nop 258 259 /* Loop through each VPE within this core */ 260 li t5, 1 261 2621: /* Operate on the appropriate TC */ 263 mtc0 t5, CP0_VPECONTROL 264 ehb 265 266 /* Bind TC to VPE (1:1 TC:VPE mapping) */ 267 mttc0 t5, CP0_TCBIND 268 269 /* Set exclusive TC, non-active, master */ 270 li t0, VPECONF0_MVP 271 sll t1, t5, VPECONF0_XTC_SHIFT 272 or t0, t0, t1 273 mttc0 t0, CP0_VPECONF0 274 275 /* Set TC non-active, non-allocatable */ 276 mttc0 zero, CP0_TCSTATUS 277 278 /* Set TC halted */ 279 li t0, TCHALT_H 280 mttc0 t0, CP0_TCHALT 281 282 /* Next VPE */ 283 addi t5, t5, 1 284 slt t0, t5, t7 285 bnez t0, 1b 286 nop 287 288 /* Leave VPE configuration state */ 2892: mfc0 t0, CP0_MVPCONTROL 290 xori t0, t0, MVPCONTROL_VPC 291 mtc0 t0, CP0_MVPCONTROL 292 2933: .set pop 294#endif 295 jr ra 296 nop 297 END(mips_cps_core_init) 298 299LEAF(mips_cps_boot_vpes) 300 /* Retrieve CM base address */ 301 la t0, mips_cm_base 302 lw t0, 0(t0) 303 304 /* Calculate a pointer to this cores struct core_boot_config */ 305 lw t0, GCR_CL_ID_OFS(t0) 306 li t1, COREBOOTCFG_SIZE 307 mul t0, t0, t1 308 la t1, mips_cps_core_bootcfg 309 lw t1, 0(t1) 310 addu t0, t0, t1 311 312 /* Calculate this VPEs ID. If the core doesn't support MT use 0 */ 313 has_mt t6, 1f 314 li t9, 0 315 316 /* Find the number of VPEs present in the core */ 317 mfc0 t1, CP0_MVPCONF0 318 srl t1, t1, MVPCONF0_PVPE_SHIFT 319 andi t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT 320 addi t1, t1, 1 321 322 /* Calculate a mask for the VPE ID from EBase.CPUNum */ 323 clz t1, t1 324 li t2, 31 325 subu t1, t2, t1 326 li t2, 1 327 sll t1, t2, t1 328 addiu t1, t1, -1 329 330 /* Retrieve the VPE ID from EBase.CPUNum */ 331 mfc0 t9, $15, 1 332 and t9, t9, t1 333 3341: /* Calculate a pointer to this VPEs struct vpe_boot_config */ 335 li t1, VPEBOOTCFG_SIZE 336 mul v0, t9, t1 337 lw t7, COREBOOTCFG_VPECONFIG(t0) 338 addu v0, v0, t7 339 340#ifdef CONFIG_MIPS_MT 341 342 /* If the core doesn't support MT then return */ 343 bnez t6, 1f 344 nop 345 jr ra 346 nop 347 348 .set push 349 .set mips32r2 350 .set mt 351 3521: /* Enter VPE configuration state */ 353 dvpe 354 la t1, 1f 355 jr.hb t1 356 nop 3571: mfc0 t1, CP0_MVPCONTROL 358 ori t1, t1, MVPCONTROL_VPC 359 mtc0 t1, CP0_MVPCONTROL 360 ehb 361 362 /* Loop through each VPE */ 363 lw t6, COREBOOTCFG_VPEMASK(t0) 364 move t8, t6 365 li t5, 0 366 367 /* Check whether the VPE should be running. If not, skip it */ 3681: andi t0, t6, 1 369 beqz t0, 2f 370 nop 371 372 /* Operate on the appropriate TC */ 373 mfc0 t0, CP0_VPECONTROL 374 ori t0, t0, VPECONTROL_TARGTC 375 xori t0, t0, VPECONTROL_TARGTC 376 or t0, t0, t5 377 mtc0 t0, CP0_VPECONTROL 378 ehb 379 380 /* Skip the VPE if its TC is not halted */ 381 mftc0 t0, CP0_TCHALT 382 beqz t0, 2f 383 nop 384 385 /* Calculate a pointer to the VPEs struct vpe_boot_config */ 386 li t0, VPEBOOTCFG_SIZE 387 mul t0, t0, t5 388 addu t0, t0, t7 389 390 /* Set the TC restart PC */ 391 lw t1, VPEBOOTCFG_PC(t0) 392 mttc0 t1, CP0_TCRESTART 393 394 /* Set the TC stack pointer */ 395 lw t1, VPEBOOTCFG_SP(t0) 396 mttgpr t1, sp 397 398 /* Set the TC global pointer */ 399 lw t1, VPEBOOTCFG_GP(t0) 400 mttgpr t1, gp 401 402 /* Copy config from this VPE */ 403 mfc0 t0, CP0_CONFIG 404 mttc0 t0, CP0_CONFIG 405 406 /* Ensure no software interrupts are pending */ 407 mttc0 zero, CP0_CAUSE 408 mttc0 zero, CP0_STATUS 409 410 /* Set TC active, not interrupt exempt */ 411 mftc0 t0, CP0_TCSTATUS 412 li t1, ~TCSTATUS_IXMT 413 and t0, t0, t1 414 ori t0, t0, TCSTATUS_A 415 mttc0 t0, CP0_TCSTATUS 416 417 /* Clear the TC halt bit */ 418 mttc0 zero, CP0_TCHALT 419 420 /* Set VPE active */ 421 mftc0 t0, CP0_VPECONF0 422 ori t0, t0, VPECONF0_VPA 423 mttc0 t0, CP0_VPECONF0 424 425 /* Next VPE */ 4262: srl t6, t6, 1 427 addi t5, t5, 1 428 bnez t6, 1b 429 nop 430 431 /* Leave VPE configuration state */ 432 mfc0 t1, CP0_MVPCONTROL 433 xori t1, t1, MVPCONTROL_VPC 434 mtc0 t1, CP0_MVPCONTROL 435 ehb 436 evpe 437 438 /* Check whether this VPE is meant to be running */ 439 li t0, 1 440 sll t0, t0, t9 441 and t0, t0, t8 442 bnez t0, 2f 443 nop 444 445 /* This VPE should be offline, halt the TC */ 446 li t0, TCHALT_H 447 mtc0 t0, CP0_TCHALT 448 la t0, 1f 4491: jr.hb t0 450 nop 451 4522: .set pop 453 454#endif /* CONFIG_MIPS_MT */ 455 456 /* Return */ 457 jr ra 458 nop 459 END(mips_cps_boot_vpes) 460 461#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM) 462 463 /* Calculate a pointer to this CPUs struct mips_static_suspend_state */ 464 .macro psstate dest 465 .set push 466 .set noat 467 lw $1, TI_CPU(gp) 468 sll $1, $1, LONGLOG 469 la \dest, __per_cpu_offset 470 addu $1, $1, \dest 471 lw $1, 0($1) 472 la \dest, cps_cpu_state 473 addu \dest, \dest, $1 474 .set pop 475 .endm 476 477LEAF(mips_cps_pm_save) 478 /* Save CPU state */ 479 SUSPEND_SAVE_REGS 480 psstate t1 481 SUSPEND_SAVE_STATIC 482 jr v0 483 nop 484 END(mips_cps_pm_save) 485 486LEAF(mips_cps_pm_restore) 487 /* Restore CPU state */ 488 psstate t1 489 RESUME_RESTORE_STATIC 490 RESUME_RESTORE_REGS_RETURN 491 END(mips_cps_pm_restore) 492 493#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */ 494