LXR linux/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
  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        addiu   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        addiu   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        addiu   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        addiu   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        addiu   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        addiu   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        addiu   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        addiu   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