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