src/arch/arm/armv7/cpu.S

   1 /* SPDX-License-Identifier: BSD-3-Clause */
   2 /*
   3  * Optimized assembly for low-level CPU operations on ARMv7 processors.
   4  *
   5  * Cache flushing code based off sys/arch/arm/arm/cpufunc_asm_armv7.S in NetBSD
   6  */
   7
   8 #include <arch/asm.h>
   9
  10 /*
  11  * Dcache invalidations by set/way work by passing a [way:sbz:set:sbz:level:0]
  12  * bitfield in a register to the appropriate MCR instruction. This algorithm
  13  * works by initializing a bitfield with the highest-numbered set and way, and
  14  * generating a "set decrement" and a "way decrement". The former just contains
  15  * the LSB of the set field, but the latter contains the LSB of the way field
  16  * minus the highest valid set field... such that when you subtract it from a
  17  * [way:0:level] field you end up with a [way - 1:highest_set:level] field
  18  * through the magic of double subtraction. It's quite ingenious, really.
  19  * Takes care to only use r0-r3 and ip so it's pefectly ABI-compatible without
  20  * needing to write to memory.
  21  *
  22  * THIS FUNCTION MUST PRESERVE THE VALUE OF r10
  23  */
  24
  25 #if ENV_USER_SPACE
  26 /*
  27  * Empty macro for code running in userspace.  Trying to manipulate the
  28  * cache from userspace hangs the system. To run code at a privileged level,
  29  * the userspace code needs to execute an API call to the privileged mode
  30  * code.
  31  */
  32 .macro  dcache_apply_all crm
  33         bx      lr
  34 .endm
  35
  36 #else
  37
  38 .macro  dcache_apply_all crm
  39         dsb
  40         mov     r3, #-2                 @ initialize level so that we start at 0
  41
  42 1:      @next_level
  43         add     r3, r3, #2              @ increment level
  44
  45         mrc     p15, 1, r0, c0, c0, 1   @ read CLIDR
  46         and     ip, r0, #0x07000000     @ narrow to LoC
  47         lsr     ip, ip, #23             @ left align LoC (low 4 bits)
  48         cmp     r3, ip                  @ compare
  49         bge     3f @done                @ else fall through (r0 == CLIDR)
  50
  51         add     r2, r3, r3, lsr #1      @ r2 = (level << 1) * 3 / 2
  52         mov     r1, r0, lsr r2          @ r1 = cache type
  53         and     r1, r1, #7
  54         cmp     r1, #2                  @ is it data or i&d?
  55         blt     1b @next_level          @ nope, skip level
  56
  57         mcr     p15, 2, r3, c0, c0, 0   @ select cache level
  58         isb
  59         mrc     p15, 1, r0, c0, c0, 0   @ read CCSIDR
  60
  61         ubfx    ip, r0, #0, #3          @ get linesize from CCSIDR
  62         add     ip, ip, #4              @ apply bias
  63         ubfx    r2, r0, #13, #15        @ get numsets - 1 from CCSIDR
  64         lsl     r2, r2, ip              @ shift to set position
  65         orr     r3, r3, r2              @ merge set into way/set/level
  66         mov     r1, #1
  67         lsl     r1, r1, ip              @ r1 = set decr
  68
  69         ubfx    ip, r0, #3, #10         @ get numways - 1 from [to be discarded] CCSIDR
  70         clz     r2, ip                  @ number of bits to MSB of way
  71         lsl     ip, ip, r2              @ shift by that into way position
  72         mov     r0, #1
  73         lsl     r2, r0, r2              @ r2 now contains the way decr
  74         mov     r0, r3                  @ get sets/level (no way yet)
  75         orr     r3, r3, ip              @ merge way into way/set/level
  76         bfc     r0, #0, #4              @ clear low 4 bits (level) to get numset - 1
  77         sub     r2, r2, r0              @ subtract from way decr
  78
  79         /* r3 = ways/sets/level, r2 = way decr, r1 = set decr, r0 and ip are free */
  80 2:      mcr     p15, 0, r3, c7, \crm, 2 @ writeback and/or invalidate line
  81         cmp     r3, #15                 @ are we done with this level (way/set == 0)
  82         bls     1b @next_level          @ yes, go to next level
  83         lsr     r0, r3, #4              @ clear level bits leaving only way/set bits
  84         lsls    r0, r0, #14             @ clear way bits leaving only set bits
  85         subne   r3, r3, r1              @ non-zero?, decrement set #
  86         subeq   r3, r3, r2              @ zero?, decrement way # and restore set count
  87         b       2b
  88
  89 3:      @done
  90         mov     r0, #0                  @ default back to cache level 0
  91         mcr     p15, 2, r0, c0, c0, 0   @ select cache level
  92         dsb
  93         isb
  94         bx      lr
  95 .endm
  96
  97 #endif /* ENV_USER_SPACE */
  98
  99 /*
 100  * Bring an ARM processor we just gained control of (e.g. from IROM) into a
 101  * known state regarding caches/SCTLR. Completely cleans and invalidates
 102  * icache/dcache, disables MMU and dcache (if active), and enables unaligned
 103  * accesses, icache and branch prediction (if inactive). Clobbers r4 and r5.
 104  *
 105  * THIS FUNCTION MUST PRESERVE THE VALUE OF r10
 106  */
 107 ENTRY(arm_init_caches)
 108         /* r4: SCTLR, return address: r5 (stay valid for the whole function) */
 109         mov     r5, lr
 110         mrc     p15, 0, r4, c1, c0, 0
 111
 112         /* Activate ICache (12) and Branch Prediction (11) already for speed */
 113         orr     r4, # (1 << 11) | (1 << 12)
 114         mcr     p15, 0, r4, c1, c0, 0
 115
 116         /* Flush and invalidate dcache in ascending order */
 117         bl      dcache_invalidate_all
 118
 119 #if ENV_ARMV7_A
 120         /* Deactivate MMU (0), Alignment Check (1) and DCache (2) */
 121         and     r4, # ~(1 << 0) & ~(1 << 1) & ~(1 << 2)
 122         mcr     p15, 0, r4, c1, c0, 0
 123
 124         /* Invalidate icache and TLB for good measure */
 125         mcr     p15, 0, r0, c7, c5, 0
 126         mcr     p15, 0, r0, c8, c7, 0
 127 #endif
 128
 129 #if ENV_ARMV7_R
 130         /* Deactivate Alignment Check (1) and DCache (2) */
 131         and     r4, # ~(1 << 1) & ~(1 << 2)
 132         mcr     p15, 0, r4, c1, c0, 0
 133
 134         /* Invalidate icache for good measure */
 135         mcr     p15, 0, r0, c7, c5, 0
 136 #endif
 137         dsb
 138         isb
 139
 140         bx      r5
 141 ENDPROC(arm_init_caches)
 142
 143 ENTRY(dcache_invalidate_all)
 144         dcache_apply_all crm=c6
 145 ENDPROC(dcache_invalidate_all)
 146
 147 ENTRY(dcache_clean_all)
 148         dcache_apply_all crm=c10
 149 ENDPROC(dcache_clean_all)
 150
 151 ENTRY(dcache_clean_invalidate_all)
 152         dcache_apply_all crm=c14
 153 ENDPROC(dcache_clean_invalidate_all)