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         /*
  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
  47 LEAF(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
  67 not_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
 104 1:      /* 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
 112 1:      cache   Index_Store_Tag_I, 0(a0)
 113         add     a0, a0, t0
 114         bne     a0, a1, 1b
 115          nop
 116 icache_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
 131 1:      /* 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
 140 1:      cache   Index_Store_Tag_D, 0(a0)
 141         bne     a0, a1, 1b
 142          add    a0, a0, t0
 143 dcache_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          */
 167 1:      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
 189 LEAF(excep_tlbfill)
 190         b       .
 191          nop
 192         END(excep_tlbfill)
 193
 194 .org 0x280
 195 LEAF(excep_xtlbfill)
 196         b       .
 197          nop
 198         END(excep_xtlbfill)
 199
 200 .org 0x300
 201 LEAF(excep_cache)
 202         b       .
 203          nop
 204         END(excep_cache)
 205
 206 .org 0x380
 207 LEAF(excep_genex)
 208         b       .
 209          nop
 210         END(excep_genex)
 211
 212 .org 0x400
 213 LEAF(excep_intex)
 214         b       .
 215          nop
 216         END(excep_intex)
 217
 218 .org 0x480
 219 LEAF(excep_ejtag)
 220         la      k0, ejtag_debug_handler
 221         jr      k0
 222          nop
 223         END(excep_ejtag)
 224
 225 LEAF(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    mt
 233
 234         /* Only allow 1 TC per VPE to execute... */
 235         dmt
 236
 237         /* ...and for the moment only 1 VPE */
 238         dvpe
 239         la      t1, 1f
 240         jr.hb   t1
 241          nop
 242
 243         /* Enter VPE configuration state */
 244 1:      mfc0    t0, CP0_MVPCONTROL
 245         ori     t0, t0, MVPCONTROL_VPC
 246         mtc0    t0, CP0_MVPCONTROL
 247
 248         /* Retrieve the number of VPEs within the core */
 249         mfc0    t0, CP0_MVPCONF0
 250         srl     t0, t0, MVPCONF0_PVPE_SHIFT
 251         andi    t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
 252         addi    t7, t0, 1
 253
 254         /* If there's only 1, we're done */
 255         beqz    t0, 2f
 256          nop
 257
 258         /* Loop through each VPE within this core */
 259         li      t5, 1
 260
 261 1:      /* Operate on the appropriate TC */
 262         mtc0    t5, CP0_VPECONTROL
 263         ehb
 264
 265         /* Bind TC to VPE (1:1 TC:VPE mapping) */
 266         mttc0   t5, CP0_TCBIND
 267
 268         /* Set exclusive TC, non-active, master */
 269         li      t0, VPECONF0_MVP
 270         sll     t1, t5, VPECONF0_XTC_SHIFT
 271         or      t0, t0, t1
 272         mttc0   t0, CP0_VPECONF0
 273
 274         /* Set TC non-active, non-allocatable */
 275         mttc0   zero, CP0_TCSTATUS
 276
 277         /* Set TC halted */
 278         li      t0, TCHALT_H
 279         mttc0   t0, CP0_TCHALT
 280
 281         /* Next VPE */
 282         addi    t5, t5, 1
 283         slt     t0, t5, t7
 284         bnez    t0, 1b
 285          nop
 286
 287         /* Leave VPE configuration state */
 288 2:      mfc0    t0, CP0_MVPCONTROL
 289         xori    t0, t0, MVPCONTROL_VPC
 290         mtc0    t0, CP0_MVPCONTROL
 291
 292 3:      .set    pop
 293 #endif
 294         jr      ra
 295          nop
 296         END(mips_cps_core_init)
 297
 298 LEAF(mips_cps_boot_vpes)
 299         /* Retrieve CM base address */
 300         la      t0, mips_cm_base
 301         lw      t0, 0(t0)
 302
 303         /* Calculate a pointer to this cores struct core_boot_config */
 304         lw      t0, GCR_CL_ID_OFS(t0)
 305         li      t1, COREBOOTCFG_SIZE
 306         mul     t0, t0, t1
 307         la      t1, mips_cps_core_bootcfg
 308         lw      t1, 0(t1)
 309         addu    t0, t0, t1
 310
 311         /* Calculate this VPEs ID. If the core doesn't support MT use 0 */
 312         has_mt  t6, 1f
 313          li     t9, 0
 314
 315         /* Find the number of VPEs present in the core */
 316         mfc0    t1, CP0_MVPCONF0
 317         srl     t1, t1, MVPCONF0_PVPE_SHIFT
 318         andi    t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
 319         addi    t1, t1, 1
 320
 321         /* Calculate a mask for the VPE ID from EBase.CPUNum */
 322         clz     t1, t1
 323         li      t2, 31
 324         subu    t1, t2, t1
 325         li      t2, 1
 326         sll     t1, t2, t1
 327         addiu   t1, t1, -1
 328
 329         /* Retrieve the VPE ID from EBase.CPUNum */
 330         mfc0    t9, $15, 1
 331         and     t9, t9, t1
 332
 333 1:      /* Calculate a pointer to this VPEs struct vpe_boot_config */
 334         li      t1, VPEBOOTCFG_SIZE
 335         mul     v0, t9, t1
 336         lw      t7, COREBOOTCFG_VPECONFIG(t0)
 337         addu    v0, v0, t7
 338
 339 #ifdef CONFIG_MIPS_MT
 340
 341         /* If the core doesn't support MT then return */
 342         bnez    t6, 1f
 343          nop
 344         jr      ra
 345          nop
 346
 347         .set    push
 348         .set    mt
 349
 350 1:      /* Enter VPE configuration state */
 351         dvpe
 352         la      t1, 1f
 353         jr.hb   t1
 354          nop
 355 1:      mfc0    t1, CP0_MVPCONTROL
 356         ori     t1, t1, MVPCONTROL_VPC
 357         mtc0    t1, CP0_MVPCONTROL
 358         ehb
 359
 360         /* Loop through each VPE */
 361         lw      t6, COREBOOTCFG_VPEMASK(t0)
 362         move    t8, t6
 363         li      t5, 0
 364
 365         /* Check whether the VPE should be running. If not, skip it */
 366 1:      andi    t0, t6, 1
 367         beqz    t0, 2f
 368          nop
 369
 370         /* Operate on the appropriate TC */
 371         mfc0    t0, CP0_VPECONTROL
 372         ori     t0, t0, VPECONTROL_TARGTC
 373         xori    t0, t0, VPECONTROL_TARGTC
 374         or      t0, t0, t5
 375         mtc0    t0, CP0_VPECONTROL
 376         ehb
 377
 378         /* Skip the VPE if its TC is not halted */
 379         mftc0   t0, CP0_TCHALT
 380         beqz    t0, 2f
 381          nop
 382
 383         /* Calculate a pointer to the VPEs struct vpe_boot_config */
 384         li      t0, VPEBOOTCFG_SIZE
 385         mul     t0, t0, t5
 386         addu    t0, t0, t7
 387
 388         /* Set the TC restart PC */
 389         lw      t1, VPEBOOTCFG_PC(t0)
 390         mttc0   t1, CP0_TCRESTART
 391
 392         /* Set the TC stack pointer */
 393         lw      t1, VPEBOOTCFG_SP(t0)
 394         mttgpr  t1, sp
 395
 396         /* Set the TC global pointer */
 397         lw      t1, VPEBOOTCFG_GP(t0)
 398         mttgpr  t1, gp
 399
 400         /* Copy config from this VPE */
 401         mfc0    t0, CP0_CONFIG
 402         mttc0   t0, CP0_CONFIG
 403
 404         /* Ensure no software interrupts are pending */
 405         mttc0   zero, CP0_CAUSE
 406         mttc0   zero, CP0_STATUS
 407
 408         /* Set TC active, not interrupt exempt */
 409         mftc0   t0, CP0_TCSTATUS
 410         li      t1, ~TCSTATUS_IXMT
 411         and     t0, t0, t1
 412         ori     t0, t0, TCSTATUS_A
 413         mttc0   t0, CP0_TCSTATUS
 414
 415         /* Clear the TC halt bit */
 416         mttc0   zero, CP0_TCHALT
 417
 418         /* Set VPE active */
 419         mftc0   t0, CP0_VPECONF0
 420         ori     t0, t0, VPECONF0_VPA
 421         mttc0   t0, CP0_VPECONF0
 422
 423         /* Next VPE */
 424 2:      srl     t6, t6, 1
 425         addi    t5, t5, 1
 426         bnez    t6, 1b
 427          nop
 428
 429         /* Leave VPE configuration state */
 430         mfc0    t1, CP0_MVPCONTROL
 431         xori    t1, t1, MVPCONTROL_VPC
 432         mtc0    t1, CP0_MVPCONTROL
 433         ehb
 434         evpe
 435
 436         /* Check whether this VPE is meant to be running */
 437         li      t0, 1
 438         sll     t0, t0, t9
 439         and     t0, t0, t8
 440         bnez    t0, 2f
 441          nop
 442
 443         /* This VPE should be offline, halt the TC */
 444         li      t0, TCHALT_H
 445         mtc0    t0, CP0_TCHALT
 446         la      t0, 1f
 447 1:      jr.hb   t0
 448          nop
 449
 450 2:      .set    pop
 451
 452 #endif /* CONFIG_MIPS_MT */
 453
 454         /* Return */
 455         jr      ra
 456          nop
 457         END(mips_cps_boot_vpes)
 458
 459 #if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)
 460
 461         /* Calculate a pointer to this CPUs struct mips_static_suspend_state */
 462         .macro  psstate dest
 463         .set    push
 464         .set    noat
 465         lw      $1, TI_CPU(gp)
 466         sll     $1, $1, LONGLOG
 467         la      \dest, __per_cpu_offset
 468         addu    $1, $1, \dest
 469         lw      $1, 0($1)
 470         la      \dest, cps_cpu_state
 471         addu    \dest, \dest, $1
 472         .set    pop
 473         .endm
 474
 475 LEAF(mips_cps_pm_save)
 476         /* Save CPU state */
 477         SUSPEND_SAVE_REGS
 478         psstate t1
 479         SUSPEND_SAVE_STATIC
 480         jr      v0
 481          nop
 482         END(mips_cps_pm_save)
 483
 484 LEAF(mips_cps_pm_restore)
 485         /* Restore CPU state */
 486         psstate t1
 487         RESUME_RESTORE_STATIC
 488         RESUME_RESTORE_REGS_RETURN
 489         END(mips_cps_pm_restore)
 490
 491 #endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */