arch/arm/mach-tegra/cortex-a9.S

   1 /*
   2  * arch/arm/mach-tegra/cortex-a9.S
   3  *
   4  * CPU state save & restore routines for CPU hotplug
   5  *
   6  * Copyright (c) 2010, NVIDIA Corporation.
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; either version 2 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful, but WITHOUT
  14  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  15  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  16  * more details.
  17  *
  18  * You should have received a copy of the GNU General Public License along
  19  * with this program; if not, write to the Free Software Foundation, Inc.,
  20  * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21  */
  22
  23 #include <linux/linkage.h>
  24 #include <linux/init.h>
  25
  26 #include <asm/assembler.h>
  27 #include <asm/domain.h>
  28 #include <asm/ptrace.h>
  29 #include <asm/cache.h>
  30 #include <asm/vfpmacros.h>
  31 #include <asm/hardware/cache-l2x0.h>
  32
  33 #include <mach/iomap.h>
  34 #include <mach/io.h>
  35
  36 /*      .section ".cpuinit.text", "ax"*/
  37
  38 #define TTB_FLAGS 0x6A  @ IRGN_WBWA, OC_RGN_WBWA, S, NOS
  39
  40 /*
  41  * spooled CPU context is 1KB / CPU
  42  */
  43 #define CTX_SP          0
  44 #define CTX_CPSR        4
  45 #define CTX_SPSR        8
  46 #define CTX_CPACR       12
  47 #define CTX_CSSELR      16
  48 #define CTX_SCTLR       20
  49 #define CTX_ACTLR       24
  50 #define CTX_PCTLR       28
  51
  52 #define CTX_FPEXC       32
  53 #define CTX_FPSCR       36
  54 #define CTX_DIAGNOSTIC  40
  55
  56 #define CTX_TTBR0       48
  57 #define CTX_TTBR1       52
  58 #define CTX_TTBCR       56
  59 #define CTX_DACR        60
  60 #define CTX_PAR         64
  61 #define CTX_PRRR        68
  62 #define CTX_NMRR        72
  63 #define CTX_VBAR        76
  64 #define CTX_CONTEXTIDR  80
  65 #define CTX_TPIDRURW    84
  66 #define CTX_TPIDRURO    88
  67 #define CTX_TPIDRPRW    92
  68
  69 #define CTX_SVC_SP      0
  70 #define CTX_SVC_LR      -1      @ stored on stack
  71 #define CTX_SVC_SPSR    8
  72
  73 #define CTX_SYS_SP      96
  74 #define CTX_SYS_LR      100
  75
  76 #define CTX_ABT_SPSR    112
  77 #define CTX_ABT_SP      116
  78 #define CTX_ABT_LR      120
  79
  80 #define CTX_UND_SPSR    128
  81 #define CTX_UND_SP      132
  82 #define CTX_UND_LR      136
  83
  84 #define CTX_IRQ_SPSR    144
  85 #define CTX_IRQ_SP      148
  86 #define CTX_IRQ_LR      152
  87
  88 #define CTX_FIQ_SPSR    160
  89 #define CTX_FIQ_R8      164
  90 #define CTX_FIQ_R9      168
  91 #define CTX_FIQ_R10     172
  92 #define CTX_FIQ_R11     178
  93 #define CTX_FIQ_R12     180
  94 #define CTX_FIQ_SP      184
  95 #define CTX_FIQ_LR      188
  96
  97 /* context only relevant for master cpu */
  98 #ifdef CONFIG_CACHE_L2X0
  99 #define CTX_L2_CTRL     224
 100 #define CTX_L2_AUX      228
 101 #define CTX_L2_TAG_CTRL 232
 102 #define CTX_L2_DAT_CTRL 236
 103 #define CTX_L2_PREFETCH 240
 104 #endif
 105
 106 #define CTX_VFP_REGS    256
 107 #define CTX_VFP_SIZE    (32 * 8)
 108
 109 #define CTX_CP14_REGS   512
 110 #define CTS_CP14_DSCR   512
 111 #define CTX_CP14_WFAR   516
 112 #define CTX_CP14_VCR    520
 113 #define CTX_CP14_CLAIM  524
 114
 115 /* Each of the folowing is 2 32-bit registers */
 116 #define CTS_CP14_BKPT_0 528
 117 #define CTS_CP14_BKPT_1 536
 118 #define CTS_CP14_BKPT_2 544
 119 #define CTS_CP14_BKPT_3 552
 120 #define CTS_CP14_BKPT_4 560
 121 #define CTS_CP14_BKPT_5 568
 122
 123 /* Each of the folowing is 2 32-bit registers */
 124 #define CTS_CP14_WPT_0  576
 125 #define CTS_CP14_WPT_1  584
 126 #define CTS_CP14_WPT_2  592
 127 #define CTS_CP14_WPT_3  600
 128
 129 #include "power.h"
 130 #include "power-macros.S"
 131
 132 .macro  ctx_ptr, rd, tmp
 133         cpu_id  \tmp
 134         mov32   \rd, tegra_context_area
 135         ldr     \rd, [\rd]
 136         add     \rd, \rd, \tmp, lsl #(CONTEXT_SIZE_BYTES_SHIFT)
 137 .endm
 138
 139 .macro  translate, pa, va, tmp
 140         mov     \tmp, #0x1000
 141         sub     \tmp, \tmp, #1
 142         bic     \pa, \va, \tmp
 143         mcr     p15, 0, \pa, c7, c8, 1
 144         mrc     p15, 0, \pa, c7, c4, 0
 145         bic     \pa, \pa, \tmp
 146         and     \tmp, \va, \tmp
 147         orr     \pa, \pa, \tmp
 148 .endm
 149
 150 /*
 151  *      __cortex_a9_save(unsigned int mode)
 152  *
 153  *       spools out the volatile processor state to memory, so that
 154  *       the CPU may be safely powered down. does not preserve:
 155  *       - CP15 c0 registers (except cache size select 2,c0/c0,0)
 156  *       - CP15 c1 secure registers (c1/c1, 0-3)
 157  *       - CP15 c5 fault status registers (c5/c0 0&1, c5/c1 0&1)
 158  *       - CP15 c6 fault address registers (c6/c0 0&2)
 159  *       - CP15 c9 performance monitor registers (c9/c12 0-5,
 160  *           c9/c13 0-2, c9/c14 0-2)
 161  *       - CP15 c10 TLB lockdown register (c10/c0, 0)
 162  *       - CP15 c12 MVBAR (c12/c0, 1)
 163  *       - CP15 c15 TLB lockdown registers
 164  */
 165         .align L1_CACHE_SHIFT
 166 ENTRY(__cortex_a9_save)
 167         mrs     r3, cpsr
 168         cps     0x13                    @ save off svc registers
 169         mov     r1, sp
 170         stmfd   sp!, {r3-r12, lr}
 171
 172         bic     r2, sp, #(L1_CACHE_BYTES-1)
 173
 174 1:      mcr     p15, 0, r2, c7, c14, 1  @ clean out dirty stack cachelines
 175         add     r2, r2, #L1_CACHE_BYTES
 176         cmp     r2, r1
 177         ble     1b
 178         dsb
 179
 180         ctx_ptr r8, r9
 181         mov     r12, r0
 182
 183         /* zero-out context area */
 184         mov     r9, r8
 185         add     r10, r8, #(CONTEXT_SIZE_BYTES)
 186         mov     r0, #0
 187         mov     r1, #0
 188         mov     r2, #0
 189         mov     r3, #0
 190         mov     r4, #0
 191         mov     r5, #0
 192         mov     r6, #0
 193         mov     r7, #0
 194 2:      stmia   r9!, {r0-r7}
 195         cmp     r9, r10
 196         blo     2b
 197
 198         mov     r0, sp
 199         mov     sp, r12                 @ sp holds the power mode
 200         mrs     r1, cpsr
 201         mrs     r2, spsr
 202
 203         mrc     p15, 0, r3, c1, c0, 2   @ cpacr
 204         stmia   r8, {r0-r3}
 205         mrc     p15, 2, r0, c0, c0, 0   @ csselr
 206         mrc     p15, 0, r1, c1, c0, 0   @ sctlr
 207         mrc     p15, 0, r2, c1, c0, 1   @ actlr
 208         mrc     p15, 0, r4, c15, c0, 0  @ pctlr
 209         add     r9, r8, #CTX_CSSELR
 210         stmia   r9, {r0-r2, r4}
 211
 212 #ifdef CONFIG_VFPv3
 213         orr     r2, r3, #0xF00000
 214         mcr     p15, 0, r2, c1, c0, 2   @ enable access to FPU
 215         VFPFMRX r2, FPEXC
 216         str     r2, [r8, #CTX_FPEXC]
 217         mov     r1, #0x40000000         @ enable access to FPU
 218         VFPFMXR FPEXC, r1
 219         VFPFMRX r1, FPSCR
 220         str     r1, [r8, #CTX_FPSCR]
 221         isb
 222         add     r9, r8, #CTX_VFP_REGS
 223
 224         VFPFSTMIA r9, r12       @ save out (16 or 32)*8B of FPU registers
 225         VFPFMXR FPEXC, r2
 226         mrc     p15, 0, r3, c1, c0, 2   @ restore original FPEXC/CPACR
 227 #endif
 228         mrc     p15, 0, r0, c15, c0, 1  @ diag
 229         str     r0, [r8, #CTX_DIAGNOSTIC]
 230
 231         add     r9, r8, #CTX_TTBR0
 232         mrc     p15, 0, r0, c2, c0, 0   @ TTBR0
 233         mrc     p15, 0, r1, c2, c0, 1   @ TTBR1
 234         mrc     p15, 0, r2, c2, c0, 2   @ TTBCR
 235         mrc     p15, 0, r3, c3, c0, 0   @ domain access control reg
 236         mrc     p15, 0, r4, c7, c4, 0   @ PAR
 237         mrc     p15, 0, r5, c10, c2, 0  @ PRRR
 238         mrc     p15, 0, r6, c10, c2, 1  @ NMRR
 239         mrc     p15, 0, r7, c12, c0, 0  @ VBAR
 240         stmia   r9!, {r0-r7}
 241         mrc     p15, 0, r0, c13, c0, 1  @ CONTEXTIDR
 242         mrc     p15, 0, r1, c13, c0, 2  @ TPIDRURW
 243         mrc     p15, 0, r2, c13, c0, 3  @ TPIDRURO
 244         mrc     p15, 0, r3, c13, c0, 4  @ TPIDRPRW
 245         stmia   r9, {r0-r3}
 246
 247         cps     0x1f                    @ SYS mode
 248         add     r9, r8, #CTX_SYS_SP
 249         stmia   r9, {sp,lr}
 250
 251         cps     0x17                    @ Abort mode
 252         mrs     r12, spsr
 253         add     r9, r8, #CTX_ABT_SPSR
 254         stmia   r9, {r12,sp,lr}
 255
 256         cps     0x12                    @ IRQ mode
 257         mrs     r12, spsr
 258         add     r9, r8, #CTX_IRQ_SPSR
 259         stmia   r9, {r12,sp,lr}
 260
 261         cps     0x1b                    @ Undefined mode
 262         mrs     r12, spsr
 263         add     r9, r8, #CTX_UND_SPSR
 264         stmia   r9, {r12,sp,lr}
 265
 266         mov     r0, r8
 267         add     r1, r8, #CTX_FIQ_SPSR
 268         cps     0x11                    @ FIQ mode
 269         mrs     r7, spsr
 270         stmia   r1, {r7-r12,sp,lr}
 271
 272         cps     0x13                    @ back to SVC
 273         mov     r8, r0
 274
 275         /* Save CP14 debug controller context */
 276         add     r9, r8, #CTX_CP14_REGS
 277         mrc     p14, 0, r0, c0, c1, 0   @ DSCR
 278         mrc     p14, 0, r1, c0, c6, 0   @ WFAR
 279         mrc     p14, 0, r2, c0, c7, 0   @ VCR
 280         mrc     p14, 0, r3, c7, c9, 6   @ CLAIM
 281         stmia   r9, {r0-r3}
 282
 283         add     r9, r8, #CTS_CP14_BKPT_0
 284         mrc     p14, 0, r2, c0, c0, 4
 285         mrc     p14, 0, r3, c0, c0, 5
 286         stmia   r9!, {r2-r3}            @ BRKPT_0
 287         mrc     p14, 0, r2, c0, c1, 4
 288         mrc     p14, 0, r3, c0, c1, 5
 289         stmia   r9!, {r2-r3}            @ BRKPT_0
 290         mrc     p14, 0, r2, c0, c2, 4
 291         mrc     p14, 0, r3, c0, c2, 5
 292         stmia   r9!, {r2-r3}            @ BRKPT_0
 293         mrc     p14, 0, r2, c0, c3, 4
 294         mrc     p14, 0, r3, c0, c3, 5
 295         stmia   r9!, {r2-r3}            @ BRKPT_0
 296         mrc     p14, 0, r2, c0, c4, 4
 297         mrc     p14, 0, r3, c0, c4, 5
 298         stmia   r9!, {r2-r3}            @ BRKPT_0
 299         mrc     p14, 0, r2, c0, c5, 4
 300         mrc     p14, 0, r3, c0, c5, 5
 301         stmia   r9!, {r2-r3}            @ BRKPT_0
 302
 303         add     r9, r8, #CTS_CP14_WPT_0
 304         mrc     p14, 0, r2, c0, c0, 6
 305         mrc     p14, 0, r3, c0, c0, 7
 306         stmia   r9!, {r2-r3}            @ WPT_0
 307         mrc     p14, 0, r2, c0, c1, 6
 308         mrc     p14, 0, r3, c0, c1, 7
 309         stmia   r9!, {r2-r3}            @ WPT_0
 310         mrc     p14, 0, r2, c0, c2, 6
 311         mrc     p14, 0, r3, c0, c2, 7
 312         stmia   r9!, {r2-r3}            @ WPT_0
 313         mrc     p14, 0, r2, c0, c3, 6
 314         mrc     p14, 0, r3, c0, c3, 7
 315         stmia   r9!, {r2-r3}            @ WPT_0
 316
 317 #ifdef CONFIG_CACHE_L2X0
 318         cpu_id  r4
 319         cmp     r4, #0
 320         bne     __cortex_a9_save_clean_cache
 321         mov32   r4, (TEGRA_ARM_PL310_BASE-IO_CPU_PHYS+IO_CPU_VIRT)
 322         add     r9, r8, #CTX_L2_CTRL
 323         ldr     r0, [r4, #L2X0_CTRL]
 324         ldr     r1, [r4, #L2X0_AUX_CTRL]
 325         ldr     r2, [r4, #L2X0_TAG_LATENCY_CTRL]
 326         ldr     r3, [r4, #L2X0_DATA_LATENCY_CTRL]
 327         ldr     r4, [r4, #L2X0_PREFETCH_OFFSET]
 328         stmia   r9, {r0-r4}
 329 #endif
 330
 331
 332 __cortex_a9_save_clean_cache:
 333         mov     r10, r8
 334         add     r9, r10, #(CONTEXT_SIZE_BYTES)
 335         add     r9, r9, #(L1_CACHE_BYTES-1)
 336         bic     r10, r10, #(L1_CACHE_BYTES-1)
 337         bic     r9, r9, #(L1_CACHE_BYTES-1)
 338
 339 3:      mcr     p15, 0, r10, c7, c10, 1
 340         add     r10, r10, #L1_CACHE_BYTES
 341         cmp     r10, r9
 342         blo     3b
 343         dsb
 344
 345         translate r10, r8, r1
 346
 347         mov     r0, #0
 348         mcr     p15, 0, r0, c1, c0, 1   @ exit coherency
 349         isb
 350         cpu_id  r0
 351         mov32   r1, (TEGRA_ARM_PERIF_BASE-IO_CPU_PHYS+IO_CPU_VIRT+0xC)
 352         mov     r3, r0, lsl #2
 353         mov     r2, #0xf
 354         mov     r2, r2, lsl r3
 355         str     r2, [r1]                @ invalidate SCU tags for CPU
 356
 357         cmp     r0, #0
 358         bne     __put_cpu_in_reset
 359         mov     r8, r10
 360         b       __tear_down_master
 361 ENDPROC(__cortex_a9_save)
 362
 363 /*
 364  *      __cortex_a9_restore
 365  *
 366  *       reloads the volatile CPU state from the context area
 367  *       the MMU should already be enabled using the secondary_data
 368  *       page tables for cpu_up before this function is called, and the
 369  *       CPU should be coherent with the SMP complex
 370  */
 371         .align L1_CACHE_SHIFT
 372 ENTRY(__cortex_a9_restore)
 373         cps     0x13
 374         ctx_ptr r0, r9
 375
 376         cps     0x11                    @ FIQ mode
 377         add     r1, r0, #CTX_FIQ_SPSR
 378         ldmia   r1, {r7-r12,sp,lr}
 379         msr     spsr_fsxc, r7
 380
 381         cps     0x12                    @ IRQ mode
 382         add     r1, r0, #CTX_IRQ_SPSR
 383         ldmia   r1, {r12, sp, lr}
 384         msr     spsr_fsxc, r12
 385
 386         cps     0x17                    @ abort mode
 387         add     r1, r0, #CTX_ABT_SPSR
 388         ldmia   r1, {r12, sp, lr}
 389         msr     spsr_fsxc, r12
 390
 391         cps     0x1f                    @ SYS mode
 392         add     r1, r0, #CTX_SYS_SP
 393         ldmia   r1, {sp, lr}
 394
 395         cps     0x1b                    @ Undefined mode
 396         add     r1, r0, #CTX_UND_SPSR
 397         ldmia   r1, {r12, sp, lr}
 398         msr     spsr_fsxc, r12
 399
 400         cps     0x13                    @ back to SVC
 401         mov     r8, r0
 402
 403         add     r9, r8, #CTX_CSSELR
 404         ldmia   r9, {r0-r3}
 405
 406         mcr     p15, 2, r0, c0, c0, 0   @ csselr
 407         mcr     p15, 0, r1, c1, c0, 0   @ sctlr
 408         mcr     p15, 0, r2, c1, c0, 1   @ actlr
 409         mcr     p15, 0, r3, c15, c0, 0  @ pctlr
 410
 411         add     r9, r8, #CTX_TTBR0
 412         ldmia   r9!, {r0-r7}
 413
 414         mcr     p15, 0, r4, c7, c4, 0   @ PAR
 415         mcr     p15, 0, r7, c12, c0, 0  @ VBAR
 416         mcr     p15, 0, r3, c3, c0, 0   @ domain access control reg
 417         isb
 418         mcr     p15, 0, r2, c2, c0, 2   @ TTBCR
 419         isb
 420         mcr     p15, 0, r5, c10, c2, 0  @ PRRR
 421         isb
 422         mcr     p15, 0, r6, c10, c2, 1  @ NMRR
 423         isb
 424
 425         ldmia   r9, {r4-r7}
 426
 427         mcr     p15, 0, r5, c13, c0, 2  @ TPIDRURW
 428         mcr     p15, 0, r6, c13, c0, 3  @ TPIDRURO
 429         mcr     p15, 0, r7, c13, c0, 4  @ TPIDRPRW
 430
 431         ldmia   r8, {r5-r7, lr}
 432
 433         /* perform context switch to previous context */
 434         mov     r9, #0
 435         mcr     p15, 0, r9, c13, c0, 1  @ set reserved context
 436         isb
 437         mcr     p15, 0, r0, c2, c0, 0   @ TTBR0
 438         isb
 439         mcr     p15, 0, r4, c13, c0, 1  @ CONTEXTIDR
 440         isb
 441         mcr     p15, 0, r1, c2, c0, 1   @ TTBR1
 442         isb
 443
 444         mov     r4, #0
 445         mcr     p15, 0, r4, c8, c3, 0   @ invalidate TLB
 446         mcr     p15, 0, r4, c7, c5, 6   @ flush BTAC
 447         mcr     p15, 0, r4, c7, c5, 0   @ flush instruction cache
 448         dsb
 449         isb
 450
 451         mov     sp, r5
 452         msr     cpsr_cxsf, r6
 453         msr     spsr_cxsf, r7
 454
 455         /* Restore CP14 debug controller context */
 456         add     r9, r8, #CTX_CP14_REGS
 457         ldmia   r9, {r0-r3}
 458         mcr     p14, 0, r1, c0, c6, 0   @ WFAR
 459         mcr     p14, 0, r2, c0, c7, 0   @ VCR
 460         mcr     p14, 0, r3, c7, c8, 6   @ CLAIM
 461
 462         add     r9, r8, #CTS_CP14_BKPT_0
 463         ldmia   r9!, {r2-r3}            @ BRKPT_0
 464         mcr     p14, 0, r2, c0, c0, 4
 465         mcr     p14, 0, r3, c0, c0, 5
 466         ldmia   r9!, {r2-r3}            @ BRKPT_0
 467         mcr     p14, 0, r2, c0, c1, 4
 468         mcr     p14, 0, r3, c0, c1, 5
 469         ldmia   r9!, {r2-r3}            @ BRKPT_0
 470         mcr     p14, 0, r2, c0, c2, 4
 471         mcr     p14, 0, r3, c0, c2, 5
 472         ldmia   r9!, {r2-r3}            @ BRKPT_0
 473         mcr     p14, 0, r2, c0, c3, 4
 474         mcr     p14, 0, r3, c0, c3, 5
 475         ldmia   r9!, {r2-r3}            @ BRKPT_0
 476         mcr     p14, 0, r2, c0, c4, 4
 477         mcr     p14, 0, r3, c0, c4, 5
 478         ldmia   r9!, {r2-r3}            @ BRKPT_0
 479         mcr     p14, 0, r2, c0, c5, 4
 480         mcr     p14, 0, r3, c0, c5, 5
 481
 482         add     r9, r8, #CTS_CP14_WPT_0
 483         ldmia   r9!, {r2-r3}            @ WPT_0
 484         mcr     p14, 0, r2, c0, c0, 6
 485         mcr     p14, 0, r3, c0, c0, 7
 486         ldmia   r9!, {r2-r3}            @ WPT_0
 487         mcr     p14, 0, r2, c0, c1, 6
 488         mcr     p14, 0, r3, c0, c1, 7
 489         ldmia   r9!, {r2-r3}            @ WPT_0
 490         mcr     p14, 0, r2, c0, c2, 6
 491         mcr     p14, 0, r3, c0, c2, 7
 492         ldmia   r9!, {r2-r3}            @ WPT_0
 493         mcr     p14, 0, r2, c0, c3, 6
 494         mcr     p14, 0, r3, c0, c3, 7
 495         isb
 496         mcr     p14, 0, r0, c0, c2, 2   @ DSCR
 497         isb
 498
 499 #ifdef CONFIG_VFPv3
 500         orr     r4, lr, #0xF00000
 501         mcr     p15, 0, r4, c1, c0, 2   @ enable coproc access
 502         mov     r5, #0x40000000
 503         VFPFMXR FPEXC, r5               @ enable FPU access
 504         add     r9, r8, #CTX_VFP_REGS
 505         add     r7, r8, #CTX_FPEXC
 506         VFPFLDMIA r9, r10
 507         ldmia   r7, {r0, r4}
 508         VFPFMXR FPSCR, r4
 509         VFPFMXR FPEXC, r0
 510 #endif
 511         mcr     p15, 0, lr, c1, c0, 2   @ cpacr (loaded before VFP)
 512
 513         ldr     r9, [r8, #CTX_DIAGNOSTIC]
 514         mcr     p15, 0, r9, c15, c0, 1  @ diag
 515
 516         /* finally, restore the stack and return */
 517         ldmfd   sp!, {r3-r12, lr}
 518         msr     cpsr_fsxc, r3           @ restore original processor mode
 519         isb
 520         mov     pc, lr
 521 ENDPROC(__cortex_a9_restore)
 522
 523 /*
 524  *      __cortex_a9_l2x0_restart(bool invalidate)
 525  *
 526  *       Reconfigures the L2 cache following a power event.
 527  */
 528         .align L1_CACHE_SHIFT
 529 ENTRY(__cortex_a9_l2x0_restart)
 530 #ifdef CONFIG_CACHE_L2X0
 531         ctx_ptr r8, r9
 532         mov32   r9, (TEGRA_ARM_PL310_BASE-IO_CPU_PHYS+IO_CPU_VIRT)
 533         add     r10, r8, #CTX_L2_CTRL
 534         ldmia   r10, {r3-r7}
 535         str     r5, [r9, #L2X0_TAG_LATENCY_CTRL]
 536         str     r6, [r9, #L2X0_DATA_LATENCY_CTRL]
 537         str     r7, [r9, #L2X0_PREFETCH_OFFSET]
 538         str     r4, [r9, #L2X0_AUX_CTRL]
 539         cmp     r0, #0
 540
 541         beq     __reenable_l2x0
 542
 543         mov     r0, #0xff
 544         str     r0, [r9, #L2X0_INV_WAY]
 545 1:      ldr     r1, [r9, #L2X0_INV_WAY]
 546         tst     r1, r0
 547         bne     1b
 548         mov     r0, #0
 549         str     r0, [r9, #L2X0_CACHE_SYNC]
 550 __reenable_l2x0:
 551         mov     r5, #0
 552         mcr     p15, 0, r5, c8, c3, 0   @ invalidate TLB
 553         mcr     p15, 0, r5, c7, c5, 6   @ flush BTAC
 554         mcr     p15, 0, r5, c7, c5, 0   @ flush instruction cache
 555         dsb
 556         isb
 557         str     r3, [r9, #L2X0_CTRL]
 558 #endif
 559         b       __cortex_a9_restore
 560
 561
 562         .align L1_CACHE_SHIFT
 563 ENTRY(__shut_off_mmu)
 564         mrc     p15, 0, r3, c1, c0, 0
 565         movw    r2, #(1<<12) | (1<<11) | (1<<2) | (1<<0)
 566         bic     r3, r3, r2
 567         dsb
 568         mcr     p15, 0, r3, c1, c0, 0
 569         isb
 570         bx      r9
 571 ENDPROC(__shut_off_mmu)
 572
 573 /*
 574  *      __invalidate_l1
 575  *
 576  *        Invalidates the L1 data cache (no clean) during initial boot of
 577  *        a secondary processor
 578  *
 579  *        Corrupted registers: r0-r6
 580  */
 581 __invalidate_l1:
 582         mov     r0, #0
 583         mcr     p15, 2, r0, c0, c0, 0
 584         mrc     p15, 1, r0, c0, c0, 0
 585
 586         movw    r1, #0x7fff
 587         and     r2, r1, r0, lsr #13
 588
 589         movw    r1, #0x3ff
 590
 591         and     r3, r1, r0, lsr #3  @ NumWays - 1
 592         add     r2, r2, #1      @ NumSets
 593
 594         and     r0, r0, #0x7
 595         add     r0, r0, #4      @ SetShift
 596
 597         clz     r1, r3          @ WayShift
 598         add     r4, r3, #1      @ NumWays
 599 1:      sub     r2, r2, #1      @ NumSets--
 600         mov     r3, r4          @ Temp = NumWays
 601 2:      subs    r3, r3, #1      @ Temp--
 602         mov     r5, r3, lsl r1
 603         mov     r6, r2, lsl r0
 604         orr     r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
 605         mcr     p15, 0, r5, c7, c6, 2
 606         bgt     2b
 607         cmp     r2, #0
 608         bgt     1b
 609         dsb
 610         isb
 611         bx      lr
 612 ENDPROC(__invalidate_l1)
 613
 614 /*
 615  *      __invalidate_cpu_state
 616  *
 617  *       Invalidates volatile CPU state (SCU tags, caches, branch address
 618  *       arrays, exclusive monitor, etc.) so that they can be safely enabled
 619  *       instruction caching and branch predicition enabled as early as
 620  *       possible to improve performance
 621  */
 622 ENTRY(__invalidate_cpu_state)
 623         clrex
 624         mov     r0, #0
 625         mcr     p15, 0, r0, c1, c0, 1   @ disable SMP, prefetch, broadcast
 626         isb
 627         mcr     p15, 0, r0, c7, c5, 0   @ invalidate BTAC, i-cache
 628         mcr     p15, 0, r0, c7, c5, 6   @ invalidate branch pred array
 629         mcr     p15, 0, r0, c8, c7, 0   @ invalidate unified TLB
 630         dsb
 631         isb
 632
 633         cpu_id  r0
 634         cmp     r0, #0
 635         mov32   r1, (TEGRA_ARM_PERIF_BASE + 0xC)
 636         movne   r0, r0, lsl #2
 637         movne   r2, #0xf
 638         movne   r2, r2, lsl r0
 639         strne   r2, [r1]                @ invalidate SCU tags for CPU
 640
 641         dsb
 642         mov     r0, #0x1800
 643         mcr     p15, 0, r0, c1, c0, 0   @ enable branch prediction, i-cache
 644         isb
 645         b       __invalidate_l1         @ invalidate data cache
 646 ENDPROC(__invalidate_cpu_state)
 647
 648 /*
 649  *      __return_to_virtual(unsigned long pgdir, void (*ctx_restore)(void))
 650  *
 651  *        Restores a CPU to the world of virtual addressing, using the
 652  *        specified page tables (which must ensure that a VA=PA mapping
 653  *        exists for the __enable_mmu function), and then jumps to
 654  *        ctx_restore to restore CPU context and return control to the OS
 655  */
 656         .align L1_CACHE_SHIFT
 657 ENTRY(__return_to_virtual)
 658         orr     r8, r0, #TTB_FLAGS
 659         mov     lr, r1                  @ "return" to ctx_restore
 660         mov     r3, #0
 661         mcr     p15, 0, r3, c2, c0, 2   @ TTB control register
 662
 663         mcr     p15, 0, r8, c2, c0, 1   @ load TTBR1
 664
 665         mov     r0, #0x1f
 666         mcr     p15, 0, r0, c3, c0, 0   @ domain access register
 667
 668         mov32   r0, 0xff0a89a8
 669 #ifdef CONFIG_SMP
 670         mov32   r1, 0xc0e0c4e0
 671 #else
 672         mov32   r1, 0x40e044e0
 673 #endif
 674         mcr     p15, 0, r0, c10, c2, 0  @ PRRR
 675         mcr     p15, 0, r1, c10, c2, 1  @ NMRR
 676         mrc     p15, 0, r0, c1, c0, 0
 677         mov32   r1, 0x0120c302
 678         bic     r0, r0, r1
 679         mov32   r1, 0x10c03c7d
 680         orr     r0, r0, r1
 681
 682 #ifdef CONFIG_ALIGNMENT_TRAP
 683         orr     r0, r0, #0x2
 684 #else
 685         bic     r0, r0, #0x2
 686 #endif
 687         mov     r1, #(domain_val(DOMAIN_USER, DOMAIN_MANAGER) | \
 688                       domain_val(DOMAIN_KERNEL, DOMAIN_MANAGER) | \
 689                       domain_val(DOMAIN_TABLE, DOMAIN_MANAGER) | \
 690                       domain_val(DOMAIN_IO, DOMAIN_CLIENT))
 691         mcr     p15, 0, r1, c3, c0, 0   @ domain access register
 692         mcr     p15, 0, r8, c2, c0, 0   @ TTBR0
 693         b       __turn_mmu_on_again
 694         andeq   r0, r0, r0
 695         andeq   r0, r0, r0
 696         andeq   r0, r0, r0
 697         andeq   r0, r0, r0
 698 ENDPROC(__return_to_virtual)
 699
 700 /*
 701  *      __turn_mmu_on_again
 702  *
 703  *        does exactly what it advertises: turns the MMU on, again
 704  *        jumps to the *virtual* address lr after the MMU is enabled.
 705  */
 706         .align  L1_CACHE_SHIFT
 707 __turn_mmu_on_again:
 708         mov     r0, r0
 709         mcr     p15, 0, r0, c1, c0, 0
 710         mrc     p15, 0, r3, c0, c0, 0
 711         mov     r3, r3
 712         mov     r3, lr
 713         bx      lr
 714 ENDPROC(__turn_mmu_on_again)