cpu_asm-v6.S revision 295920
1/*- 2 * Copyright 2014 Svatopluk Kraus <onwahe@gmail.com> 3 * Copyright 2014 Michal Meloun <meloun@miracle.cz> 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions 8 * are met: 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25 * SUCH DAMAGE. 26 * 27 * $FreeBSD: head/sys/arm/arm/cpu_asm-v6.S 295920 2016-02-23 10:12:50Z skra $ 28 */ 29#include "assym.s" 30 31#include <machine/acle-compat.h> 32#include <machine/asm.h> 33#include <machine/asmacros.h> 34#include <machine/armreg.h> 35#include <machine/sysreg.h> 36 37#define GET_PCB(tmp) \ 38 mrc CP15_TPIDRPRW(tmp); \ 39 add tmp, tmp, #(TD_PCB) 40 41/* 42 * Define cache functions used by startup code, which counts on the fact that 43 * only r0-r3,r12 (ip) are modified and no stack space is used. These functions 44 * must be called with interrupts disabled. Moreover, these work only with 45 * caches integrated to CPU (accessible via CP15); systems with an external L2 46 * cache controller such as a PL310 need separate calls to that device driver 47 * to affect L2 caches. This is not a factor during early kernel startup, as 48 * any external L2 cache controller has not been enabled yet. 49 */ 50 51/* Invalidate D cache to PoC. (aka all cache levels)*/ 52ASENTRY_NP(dcache_inv_poc_all) 53#if __ARM_ARCH == 6 54 mcr CP15_DCIALL 55 DSB 56 bx lr 57#else 58 mrc CP15_CLIDR(r0) 59 ands r0, r0, #0x07000000 60 mov r0, r0, lsr #23 /* Get LoC 'naturally' aligned for */ 61 beq 4f /* use in the CSSELR register below */ 62 631: sub r0, #2 64 mcr CP15_CSSELR(r0) /* set cache level */ 65 isb 66 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 67 68 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 69 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 70 clz r1, r3 /* number of bits to MSB of way */ 71 lsl r3, r3, r1 /* shift into position */ 72 mov ip, #1 73 lsl ip, ip, r1 /* ip now contains the way decr */ 74 75 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 76 add r0, r0, #4 /* apply bias */ 77 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 78 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 79 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 80 mov r1, #1 81 lsl r1, r1, r0 /* r1 now contains the set decr */ 82 mov r2, ip /* r2 now contains set way decr */ 83 84 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 852: mcr CP15_DCISW(r3) /* invalidate line */ 86 movs r0, r3 /* get current way/set */ 87 beq 3f /* at 0 means we are done */ 88 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 89 subne r3, r3, r1 /* non-zero?, decrement set */ 90 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 91 b 2b 92 933: 94 mrc CP15_CSSELR(r0) /* get cache level */ 95 teq r0, #0 96 bne 1b 97 984: dsb /* wait for stores to finish */ 99 mov r0, #0 100 mcr CP15_CSSELR(r0) 101 isb 102 bx lr 103#endif /* __ARM_ARCH == 6 */ 104END(dcache_inv_poc_all) 105 106/* Invalidate D cache to PoU. (aka L1 cache only)*/ 107ASENTRY_NP(dcache_inv_pou_all) 108#if __ARM_ARCH == 6 109 mcr CP15_DCIALL 110 DSB 111 bx lr 112#else 113 mrc CP15_CLIDR(r0) 114 ands r0, r0, #0x38000000 115 mov r0, r0, lsr #26 /* Get LoUU (naturally aligned) */ 116 beq 4f 117 1181: sub r0, #2 119 mcr CP15_CSSELR(r0) /* set cache level */ 120 isb 121 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 122 123 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 124 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 125 clz r1, r3 /* number of bits to MSB of way */ 126 lsl r3, r3, r1 /* shift into position */ 127 mov ip, #1 128 lsl ip, ip, r1 /* ip now contains the way decr */ 129 130 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 131 add r0, r0, #4 /* apply bias */ 132 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 133 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 134 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 135 mov r1, #1 136 lsl r1, r1, r0 /* r1 now contains the set decr */ 137 mov r2, ip /* r2 now contains set way decr */ 138 139 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1402: mcr CP15_DCISW(r3) /* invalidate line */ 141 movs r0, r3 /* get current way/set */ 142 beq 3f /* at 0 means we are done */ 143 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 144 subne r3, r3, r1 /* non-zero?, decrement set */ 145 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 146 b 2b 147 1483: 149 mrc CP15_CSSELR(r0) /* get cache level */ 150 teq r0, #0 151 bne 1b 152 1534: dsb /* wait for stores to finish */ 154 mov r0, #0 155 mcr CP15_CSSELR(r0) 156 bx lr 157#endif 158END(dcache_inv_pou_all) 159 160/* Write back and Invalidate D cache to PoC. */ 161ASENTRY_NP(dcache_wbinv_poc_all) 162#if __ARM_ARCH == 6 163 mcr CP15_DCCIALL 164 DSB 165 bx lr 166#else 167 mrc CP15_CLIDR(r0) 168 ands r0, r0, #0x07000000 169 beq 4f 170 mov r0, #0 /* Clean from inner to outer levels */ 171 1721: mcr CP15_CSSELR(r0) /* set cache level */ 173 isb 174 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 175 176 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 177 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 178 clz r1, r3 /* number of bits to MSB of way */ 179 lsl r3, r3, r1 /* shift into position */ 180 mov ip, #1 181 lsl ip, ip, r1 /* ip now contains the way decr */ 182 183 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 184 add r0, r0, #4 /* apply bias */ 185 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 186 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 187 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 188 mov r1, #1 189 lsl r1, r1, r0 /* r1 now contains the set decr */ 190 mov r2, ip /* r2 now contains set way decr */ 191 192 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1932: mcr CP15_DCCISW(r3) /* clean and invalidate line */ 194 movs r0, r3 /* get current way/set */ 195 beq 3f /* at 0 means we are done */ 196 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 197 subne r3, r3, r1 /* non-zero?, decrement set */ 198 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 199 b 2b 200 2013: 202 mrc CP15_CSSELR(r0) /* get cache level */ 203 add r0, r0, #2 /* next level */ 204 mrc CP15_CLIDR(r1) 205 ands r1, r1, #0x07000000 206 mov r1, r1, lsr #23 /* Get LoC (naturally aligned) */ 207 cmp r1, r0 208 bne 1b 209 2104: dsb /* wait for stores to finish */ 211 mov r0, #0 212 mcr CP15_CSSELR(r0) 213 bx lr 214#endif /* __ARM_ARCH == 6 */ 215END(dcache_wbinv_poc_all) 216 217ASENTRY_NP(dcache_wb_pou_checked) 218 ldr ip, .Lcpuinfo 219 ldr ip, [ip, #DCACHE_LINE_SIZE] 220 221 GET_PCB(r2) 222 ldr r2, [r2] 223 224 adr r3, _C_LABEL(cachebailout) 225 str r3, [r2, #PCB_ONFAULT] 2261: 227 mcr CP15_DCCMVAC(r0) 228 add r0, r0, ip 229 subs r1, r1, ip 230 bhi 1b 231 DSB 232 mov r0, #0 233 str r0, [r2, #PCB_ONFAULT] 234 mov r0, #1 /* cannot be faulting address */ 235 RET 236 237.Lcpuinfo: 238 .word cpuinfo 239END(dcache_wb_pou_checked) 240 241ASENTRY_NP(icache_inv_pou_checked) 242 ldr ip, .Lcpuinfo 243 ldr ip, [ip, #ICACHE_LINE_SIZE] 244 245 GET_PCB(r2) 246 ldr r2, [r2] 247 248 adr r3, _C_LABEL(cachebailout) 249 str r3, [r2, #PCB_ONFAULT] 250 2511: 252 mcr CP15_ICIMVAU(r0) 253 add r0, r0, ip 254 subs r1, r1, ip 255 bhi 1b 256 DSB 257 ISB 258 mov r0, #0 259 str r0, [r2, #PCB_ONFAULT] 260 mov r0, #1 /* cannot be faulting address */ 261 RET 262END(icache_inv_pou_checked) 263 264/* label must be global as trap-v6.c references it */ 265 .global _C_LABEL(cachebailout) 266_C_LABEL(cachebailout): 267 DSB 268 ISB 269 mov r1, #0 270 str r1, [r2, #PCB_ONFAULT] 271 RET 272