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#include "assym.inc" 28 29#include <machine/asm.h> 30#include <machine/asmacros.h> 31#include <machine/armreg.h> 32#include <machine/sysreg.h> 33 34#define GET_PCB(tmp) \ 35 mrc CP15_TPIDRPRW(tmp); \ 36 add tmp, tmp, #(TD_PCB) 37 38/* 39 * Define cache functions used by startup code, which counts on the fact that 40 * only r0-r3,r12 (ip) are modified and no stack space is used. These functions 41 * must be called with interrupts disabled. Moreover, these work only with 42 * caches integrated to CPU (accessible via CP15); systems with an external L2 43 * cache controller such as a PL310 need separate calls to that device driver 44 * to affect L2 caches. This is not a factor during early kernel startup, as 45 * any external L2 cache controller has not been enabled yet. 46 */ 47 48/* Invalidate D cache to PoC. (aka all cache levels)*/ 49ASENTRY_NP(dcache_inv_poc_all) 50 mrc CP15_CLIDR(r0) 51 ands r0, r0, #0x07000000 52 mov r0, r0, lsr #23 /* Get LoC 'naturally' aligned for */ 53 beq 4f /* use in the CSSELR register below */ 54 551: sub r0, #2 56 mcr CP15_CSSELR(r0) /* set cache level */ 57 isb 58 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 59 60 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 61 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 62 clz r1, r3 /* number of bits to MSB of way */ 63 lsl r3, r3, r1 /* shift into position */ 64 mov ip, #1 65 lsl ip, ip, r1 /* ip now contains the way decr */ 66 67 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 68 add r0, r0, #4 /* apply bias */ 69 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 70 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 71 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 72 mov r1, #1 73 lsl r1, r1, r0 /* r1 now contains the set decr */ 74 mov r2, ip /* r2 now contains set way decr */ 75 76 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 772: mcr CP15_DCISW(r3) /* invalidate line */ 78 movs r0, r3 /* get current way/set */ 79 beq 3f /* at 0 means we are done */ 80 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 81 subne r3, r3, r1 /* non-zero?, decrement set */ 82 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 83 b 2b 84 853: 86 mrc CP15_CSSELR(r0) /* get cache level */ 87 teq r0, #0 88 bne 1b 89 904: dsb /* wait for stores to finish */ 91 mov r0, #0 92 mcr CP15_CSSELR(r0) 93 isb 94 bx lr 95END(dcache_inv_poc_all) 96 97/* Invalidate D cache to PoU. (aka L1 cache only)*/ 98ASENTRY_NP(dcache_inv_pou_all) 99 mrc CP15_CLIDR(r0) 100 ands r0, r0, #0x38000000 101 mov r0, r0, lsr #26 /* Get LoUU (naturally aligned) */ 102 beq 4f 103 1041: sub r0, #2 105 mcr CP15_CSSELR(r0) /* set cache level */ 106 isb 107 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 108 109 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 110 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 111 clz r1, r3 /* number of bits to MSB of way */ 112 lsl r3, r3, r1 /* shift into position */ 113 mov ip, #1 114 lsl ip, ip, r1 /* ip now contains the way decr */ 115 116 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 117 add r0, r0, #4 /* apply bias */ 118 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 119 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 120 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 121 mov r1, #1 122 lsl r1, r1, r0 /* r1 now contains the set decr */ 123 mov r2, ip /* r2 now contains set way decr */ 124 125 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1262: mcr CP15_DCISW(r3) /* invalidate line */ 127 movs r0, r3 /* get current way/set */ 128 beq 3f /* at 0 means we are done */ 129 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 130 subne r3, r3, r1 /* non-zero?, decrement set */ 131 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 132 b 2b 133 1343: 135 mrc CP15_CSSELR(r0) /* get cache level */ 136 teq r0, #0 137 bne 1b 138 1394: dsb /* wait for stores to finish */ 140 mov r0, #0 141 mcr CP15_CSSELR(r0) 142 bx lr 143END(dcache_inv_pou_all) 144 145/* Write back and Invalidate D cache to PoC. */ 146ASENTRY_NP(dcache_wbinv_poc_all) 147 mrc CP15_CLIDR(r0) 148 ands r0, r0, #0x07000000 149 beq 4f 150 mov r0, #0 /* Clean from inner to outer levels */ 151 1521: mcr CP15_CSSELR(r0) /* set cache level */ 153 isb 154 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 155 156 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 157 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 158 clz r1, r3 /* number of bits to MSB of way */ 159 lsl r3, r3, r1 /* shift into position */ 160 mov ip, #1 161 lsl ip, ip, r1 /* ip now contains the way decr */ 162 163 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 164 add r0, r0, #4 /* apply bias */ 165 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 166 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 167 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 168 mov r1, #1 169 lsl r1, r1, r0 /* r1 now contains the set decr */ 170 mov r2, ip /* r2 now contains set way decr */ 171 172 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1732: mcr CP15_DCCISW(r3) /* clean and invalidate line */ 174 movs r0, r3 /* get current way/set */ 175 beq 3f /* at 0 means we are done */ 176 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 177 subne r3, r3, r1 /* non-zero?, decrement set */ 178 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 179 b 2b 180 1813: 182 mrc CP15_CSSELR(r0) /* get cache level */ 183 add r0, r0, #2 /* next level */ 184 mrc CP15_CLIDR(r1) 185 ands r1, r1, #0x07000000 186 mov r1, r1, lsr #23 /* Get LoC (naturally aligned) */ 187 cmp r1, r0 188 bne 1b 189 1904: dsb /* wait for stores to finish */ 191 mov r0, #0 192 mcr CP15_CSSELR(r0) 193 bx lr 194END(dcache_wbinv_poc_all) 195 196ASENTRY_NP(dcache_wb_pou_checked) 197 ldr ip, .Lcpuinfo 198 ldr ip, [ip, #DCACHE_LINE_SIZE] 199 200 GET_PCB(r2) 201 ldr r2, [r2] 202 203 adr r3, _C_LABEL(cachebailout) 204 str r3, [r2, #PCB_ONFAULT] 2051: 206 mcr CP15_DCCMVAC(r0) 207 add r0, r0, ip 208 subs r1, r1, ip 209 bhi 1b 210 DSB 211 mov r0, #0 212 str r0, [r2, #PCB_ONFAULT] 213 mov r0, #1 /* cannot be faulting address */ 214 RET 215 216.Lcpuinfo: 217 .word cpuinfo 218END(dcache_wb_pou_checked) 219 220ASENTRY_NP(icache_inv_pou_checked) 221 ldr ip, .Lcpuinfo 222 ldr ip, [ip, #ICACHE_LINE_SIZE] 223 224 GET_PCB(r2) 225 ldr r2, [r2] 226 227 adr r3, _C_LABEL(cachebailout) 228 str r3, [r2, #PCB_ONFAULT] 229 2301: 231 mcr CP15_ICIMVAU(r0) 232 add r0, r0, ip 233 subs r1, r1, ip 234 bhi 1b 235 DSB 236 ISB 237 mov r0, #0 238 str r0, [r2, #PCB_ONFAULT] 239 mov r0, #1 /* cannot be faulting address */ 240 RET 241END(icache_inv_pou_checked) 242 243/* label must be global as trap-v6.c references it */ 244 .global _C_LABEL(cachebailout) 245_C_LABEL(cachebailout): 246 DSB 247 ISB 248 mov r1, #0 249 str r1, [r2, #PCB_ONFAULT] 250 RET 251