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