src/arch/arm64/armv8/cpu.S

   1 /* SPDX-License-Identifier: GPL-2.0-only */
   2 /*
   3  * Optimized assembly for low-level CPU operations on ARM64 processors.
   4  */
   5
   6 #include <arch/asm.h>
   7 #include <arch/cache.h>
   8
   9 .macro  dcache_apply_all crm
  10         dsb     sy
  11         mrs     x0, clidr_el1                   // read CLIDR
  12         and     w3, w0, #0x07000000             // narrow to LoC
  13         lsr     w3, w3, #23                     // left align LoC (low 4 bits)
  14         cbz     w3, 5f //done
  15
  16         mov     w10, #0                         // w10 = 2 * cache level
  17         mov     w8, #1                          // w8 = constant 0b1
  18
  19         mrs     x12, id_aa64mmfr2_el1           // read ID_AA64MMFR2_EL1
  20         ubfx    x12, x12, #20, #4               // [23:20] - CCIDX support
  21
  22 1:      //next_level
  23         add     w2, w10, w10, lsr #1            // calculate 3 * cache level
  24         lsr     w1, w0, w2                      // extract 3-bit cache type for this level
  25         and     w1, w1, #0x7                    // w1 = cache type
  26         cmp     w1, #2                          // is it data or i&d?
  27         b.lt    4f //skip
  28         msr     csselr_el1, x10                 // select current cache level
  29         isb                                     // sync change of csselr
  30         mrs     x1, ccsidr_el1                  // w1 = read ccsidr
  31         and     w2, w1, #7                      // w2 = log2(linelen_bytes) - 4
  32         add     w2, w2, #4                      // w2 = log2(linelen_bytes)
  33
  34         cbz     x12, 11f                        // check FEAT_CCIDX for associativity
  35                                                 // branch to 11 if FEAT_CCIDX is not implemented
  36         ubfx    x4, x1, #3, #21                 // x4 = associativity CCSIDR_EL1[23:3]
  37         b       12f
  38 11:
  39         ubfx    x4, x1, #3, #10                 // x4 = associativity CCSIDR_EL1[12:3]
  40 12:
  41         clz     w5, w4                          // w5 = 32 - log2(ways)
  42                                                 // (bit position of way in DC)
  43         lsl     w9, w4, w5                      // w9 = max way number
  44                                                 // (aligned for DC)
  45         lsl     w16, w8, w5                     // w16 = amount to decrement (way
  46                                                 // number per iteration)
  47 2:      //next_way
  48         cbz     x12, 21f                        // check FEAT_CCIDX for numsets
  49                                                 // branch to 21 if FEAT_CCIDX is not implemented
  50         ubfx    x7, x1, #32, #24                // x7(w7) = numsets CCSIDR_EL1[55:32]
  51         b       22f
  52 21:
  53         ubfx    w7, w1, #13, #15                // w7 = numsets CCSIDR_EL1[27:13]
  54 22:
  55         lsl     w7, w7, w2                      // w7 = max set #, DC aligned
  56         lsl     w17, w8, w2                     // w17 = amount to decrement (set
  57                                                 // number per iteration)
  58
  59 3:      //next_set
  60         orr     w11, w10, w9                    // w11 = combine way # & cache #
  61         orr     w11, w11, w7                    // ... and set #
  62         dc      \crm, x11                       // clean and/or invalidate line
  63         subs    w7, w7, w17                     // decrement set number
  64         b.ge    3b //next_set
  65         subs    x9, x9, x16                     // decrement way number
  66         b.ge    2b //next_way
  67
  68 4:      //skip
  69         add     w10, w10, #2                    // increment 2 *cache level
  70         cmp     w3, w10                         // Went beyond LoC?
  71         b.gt    1b //next_level
  72
  73 5:      //done
  74         dsb     sy
  75         isb
  76         ret
  77 .endm
  78
  79 ENTRY(dcache_invalidate_all)
  80         dcache_apply_all crm=isw
  81 ENDPROC(dcache_invalidate_all)
  82
  83 ENTRY(dcache_clean_all)
  84         dcache_apply_all crm=csw
  85 ENDPROC(dcache_clean_all)
  86
  87 ENTRY(dcache_clean_invalidate_all)
  88         dcache_apply_all crm=cisw
  89 ENDPROC(dcache_clean_invalidate_all)
  90
  91 /* This must be implemented in assembly to ensure there are no accesses to
  92    memory (e.g. the stack) in between disabling and flushing the cache. */
  93 ENTRY(mmu_disable)
  94         str     x30, [sp, #-0x8]
  95         mrs     x0, CURRENT_EL(sctlr)
  96         mov     x1, #~(SCTLR_C | SCTLR_M)
  97         and     x0, x0, x1
  98         msr     CURRENT_EL(sctlr), x0
  99         isb
 100         bl      dcache_clean_invalidate_all
 101         ldr     x30, [sp, #-0x8]
 102         ret
 103 ENDPROC(mmu_disable)
 104
 105 /*
 106  * Bring an ARMv8 processor we just gained control of (e.g. from IROM) into a
 107  * known state regarding caches/SCTLR/SCR/PSTATE. Completely invalidates
 108  * icache/dcache, disables MMU and dcache (if active), and enables unaligned
 109  * accesses, icache. Seeds stack and initializes SP_EL0. Clobbers R22 and R23.
 110  */
 111 ENTRY(arm64_init_cpu)
 112         /* Initialize PSTATE (mask all exceptions, select SP_EL0). */
 113         msr     SPSel, #0
 114         msr     DAIFSet, #0xf
 115
 116         /* TODO: This is where we'd put non-boot CPUs into WFI if needed. */
 117
 118         /* x22: SCTLR, return address: x23 (callee-saved by subroutine) */
 119         mov     x23, x30
 120         mrs     x22, CURRENT_EL(sctlr)
 121
 122         /* Activate ICache already for speed during cache flush below. */
 123         orr     x22, x22, #SCTLR_I
 124         msr     CURRENT_EL(sctlr), x22
 125         isb
 126
 127         /* Invalidate dcache */
 128         bl      dcache_invalidate_all
 129
 130         /* Reinitialize SCTLR from scratch to known-good state.
 131            This may disable MMU or DCache. */
 132         ldr     w22, =(SCTLR_RES1 | SCTLR_I | SCTLR_SA)
 133         msr     CURRENT_EL(sctlr), x22
 134
 135 #if CONFIG_ARM64_CURRENT_EL == EL3
 136         /* Initialize SCR to unmask all interrupts (so that if we get a spurious
 137            IRQ/SError we'll see it when it happens, not hang in BL31). This will
 138            only have an effect after we DAIFClr in exception_init(). */
 139         mov     x22, #SCR_RES1 | SCR_IRQ | SCR_FIQ | SCR_EA
 140         msr     scr_el3, x22
 141 #endif
 142
 143         /* Invalidate icache and TLB for good measure */
 144         ic      iallu
 145         tlbi    alle3
 146         dsb     sy
 147         isb
 148
 149         /* Initialize stack with sentinel value to later check overflow. */
 150         ldr     x2, =0xdeadbeefdeadbeef
 151         ldr     x0, =_stack
 152         ldr     x1, =_estack
 153 1:
 154         stp     x2, x2, [x0], #16
 155         cmp     x0, x1
 156         bne     1b
 157
 158         /* Leave a line of beef dead for easier visibility in stack dumps. */
 159         sub     sp, x0, #16
 160
 161         ret     x23
 162 ENDPROC(arm64_init_cpu)