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$ 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#if __ARM_ARCH >= 6 38#define GET_PCB(tmp) \ 39 mrc CP15_TPIDRPRW(tmp); \ 40 add tmp, tmp, #(TD_PCB) 41#else 42.Lcurpcb: 43 .word _C_LABEL(__pcpu) + PC_CURPCB 44#define GET_PCB(tmp) \ 45 ldr tmp, .Lcurpcb 46#endif 47 48/* 49 * Define cache functions used by startup code, which counts on the fact that 50 * only r0-r3,r12 (ip) are modified and no stack space is used. These functions 51 * must be called with interrupts disabled. Moreover, these work only with 52 * caches integrated to CPU (accessible via CP15); systems with an external L2 53 * cache controller such as a PL310 need separate calls to that device driver 54 * to affect L2 caches. This is not a factor during early kernel startup, as 55 * any external L2 cache controller has not been enabled yet. 56 */ 57 58/* Invalidate D cache to PoC. (aka all cache levels)*/ 59ASENTRY_NP(dcache_inv_poc_all) 60#if __ARM_ARCH == 6 61 mcr CP15_DCIALL 62 DSB 63 bx lr 64#else 65 mrc CP15_CLIDR(r0) 66 ands r0, r0, #0x07000000 67 mov r0, r0, lsr #23 /* Get LoC 'naturally' aligned for */ 68 beq 4f /* use in the CSSELR register below */ 69 701: sub r0, #2 71 mcr CP15_CSSELR(r0) /* set cache level */ 72 isb 73 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 74 75 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 76 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 77 clz r1, r3 /* number of bits to MSB of way */ 78 lsl r3, r3, r1 /* shift into position */ 79 mov ip, #1 80 lsl ip, ip, r1 /* ip now contains the way decr */ 81 82 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 83 add r0, r0, #4 /* apply bias */ 84 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 85 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 86 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 87 mov r1, #1 88 lsl r1, r1, r0 /* r1 now contains the set decr */ 89 mov r2, ip /* r2 now contains set way decr */ 90 91 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 922: mcr CP15_DCISW(r3) /* invalidate line */ 93 movs r0, r3 /* get current way/set */ 94 beq 3f /* at 0 means we are done */ 95 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 96 subne r3, r3, r1 /* non-zero?, decrement set */ 97 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 98 b 2b 99 1003: 101 mrc CP15_CSSELR(r0) /* get cache level */ 102 teq r0, #0 103 bne 1b 104 1054: dsb /* wait for stores to finish */ 106 mov r0, #0 107 mcr CP15_CSSELR(r0) 108 isb 109 bx lr 110#endif /* __ARM_ARCH == 6 */ 111END(dcache_inv_poc_all) 112 113/* Invalidate D cache to PoU. (aka L1 cache only)*/ 114ASENTRY_NP(dcache_inv_pou_all) 115#if __ARM_ARCH == 6 116 mcr CP15_DCIALL 117 DSB 118 bx lr 119#else 120 mrc CP15_CLIDR(r0) 121 ands r0, r0, #0x38000000 122 mov r0, r0, lsr #26 /* Get LoUU (naturally aligned) */ 123 beq 4f 124 1251: sub r0, #2 126 mcr CP15_CSSELR(r0) /* set cache level */ 127 isb 128 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 129 130 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 131 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 132 clz r1, r3 /* number of bits to MSB of way */ 133 lsl r3, r3, r1 /* shift into position */ 134 mov ip, #1 135 lsl ip, ip, r1 /* ip now contains the way decr */ 136 137 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 138 add r0, r0, #4 /* apply bias */ 139 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 140 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 141 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 142 mov r1, #1 143 lsl r1, r1, r0 /* r1 now contains the set decr */ 144 mov r2, ip /* r2 now contains set way decr */ 145 146 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1472: mcr CP15_DCISW(r3) /* invalidate line */ 148 movs r0, r3 /* get current way/set */ 149 beq 3f /* at 0 means we are done */ 150 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 151 subne r3, r3, r1 /* non-zero?, decrement set */ 152 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 153 b 2b 154 1553: 156 mrc CP15_CSSELR(r0) /* get cache level */ 157 teq r0, #0 158 bne 1b 159 1604: dsb /* wait for stores to finish */ 161 mov r0, #0 162 mcr CP15_CSSELR(r0) 163 bx lr 164#endif 165END(dcache_inv_pou_all) 166 167/* Write back and Invalidate D cache to PoC. */ 168ASENTRY_NP(dcache_wbinv_poc_all) 169#if __ARM_ARCH == 6 170 mcr CP15_DCCIALL 171 DSB 172 bx lr 173#else 174 mrc CP15_CLIDR(r0) 175 ands r0, r0, #0x07000000 176 beq 4f 177 mov r0, #0 /* Clean from inner to outer levels */ 178 1791: mcr CP15_CSSELR(r0) /* set cache level */ 180 isb 181 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 182 183 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 184 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 185 clz r1, r3 /* number of bits to MSB of way */ 186 lsl r3, r3, r1 /* shift into position */ 187 mov ip, #1 188 lsl ip, ip, r1 /* ip now contains the way decr */ 189 190 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 191 add r0, r0, #4 /* apply bias */ 192 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 193 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 194 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 195 mov r1, #1 196 lsl r1, r1, r0 /* r1 now contains the set decr */ 197 mov r2, ip /* r2 now contains set way decr */ 198 199 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 2002: mcr CP15_DCCISW(r3) /* clean and invalidate line */ 201 movs r0, r3 /* get current way/set */ 202 beq 3f /* at 0 means we are done */ 203 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 204 subne r3, r3, r1 /* non-zero?, decrement set */ 205 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 206 b 2b 207 2083: 209 mrc CP15_CSSELR(r0) /* get cache level */ 210 add r0, r0, #2 /* next level */ 211 mrc CP15_CLIDR(r1) 212 ands r1, r1, #0x07000000 213 mov r1, r1, lsr #23 /* Get LoC (naturally aligned) */ 214 cmp r1, r0 215 bne 1b 216 2174: dsb /* wait for stores to finish */ 218 mov r0, #0 219 mcr CP15_CSSELR(r0) 220 bx lr 221#endif /* __ARM_ARCH == 6 */ 222END(dcache_wbinv_poc_all) 223 224ASENTRY_NP(dcache_wb_pou_checked) 225 ldr ip, .Lcpuinfo 226 ldr ip, [ip, #DCACHE_LINE_SIZE] 227 228 GET_PCB(r2) 229 ldr r2, [r2] 230 231 adr r3, _C_LABEL(cachebailout) 232 str r3, [r2, #PCB_ONFAULT] 2331: 234 mcr CP15_DCCMVAC(r0) 235 add r0, r0, ip 236 subs r1, r1, ip 237 bhi 1b 238 DSB 239 mov r0, #0 240 str r0, [r2, #PCB_ONFAULT] 241 mov r0, #1 /* cannot be faulting address */ 242 RET 243 244.Lcpuinfo: 245 .word cpuinfo 246END(dcache_wb_pou_checked) 247 248ASENTRY_NP(icache_inv_pou_checked) 249 ldr ip, .Lcpuinfo 250 ldr ip, [ip, #ICACHE_LINE_SIZE] 251 252 GET_PCB(r2) 253 ldr r2, [r2] 254 255 adr r3, _C_LABEL(cachebailout) 256 str r3, [r2, #PCB_ONFAULT] 257 2581: 259 mcr CP15_ICIMVAU(r0) 260 add r0, r0, ip 261 subs r1, r1, ip 262 bhi 1b 263 DSB 264 ISB 265 mov r0, #0 266 str r0, [r2, #PCB_ONFAULT] 267 mov r0, #1 /* cannot be faulting address */ 268 RET 269END(icache_inv_pou_checked) 270 271/* label must be global as trap-v6.c references it */ 272 .global _C_LABEL(cachebailout) 273_C_LABEL(cachebailout): 274 DSB 275 ISB 276 mov r1, #0 277 str r1, [r2, #PCB_ONFAULT] 278 RET 279