arch/mips/kernel/cps-vec.S

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