cpu_asm-v6.S revision 278635
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: stable/10/sys/arm/arm/cpu_asm-v6.S 278635 2015-02-12 21:10:24Z ian $ 28 */ 29 30#include <machine/acle-compat.h> 31#include <machine/asm.h> 32#include <machine/asmacros.h> 33#include <machine/armreg.h> 34#include <machine/sysreg.h> 35 36#if __ARM_ARCH >= 7 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) */ 53 beq 4f 54 551: mcr CP15_CSSELR(r0) /* set cache level */ 56 isb 57 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 58 59 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 60 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 61 clz r1, r3 /* number of bits to MSB of way */ 62 lsl r3, r3, r1 /* shift into position */ 63 mov ip, #1 64 lsl ip, ip, r1 /* ip now contains the way decr */ 65 66 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 67 add r0, r0, #4 /* apply bias */ 68 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 69 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 70 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 71 mov r1, #1 72 lsl r1, r1, r0 /* r1 now contains the set decr */ 73 mov r2, ip /* r2 now contains set way decr */ 74 75 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 762: mcr CP15_DCISW(r3) /* invalidate line */ 77 movs r0, r3 /* get current way/set */ 78 beq 3f /* at 0 means we are done */ 79 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 80 subne r3, r3, r1 /* non-zero?, decrement set */ 81 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 82 b 2b 83 843: 85 mrc CP15_CSSELR(r0) /* get cache level */ 86 add r0, r0, #2 /* next level */ 87 mrc CP15_CLIDR(r1) 88 ands r1, r1, #0x07000000 89 mov r1, r1, lsr #23 /* Get LoC (naturally aligned) */ 90 cmp r1, r0 91 bgt 1b 92 934: dsb /* wait for stores to finish */ 94 mov r0, #0 95 mcr CP15_CSSELR(r0) 96 isb 97 bx lr 98END(dcache_inv_poc_all) 99 100/* Invalidate D cache to PoU. (aka L1 cache only)*/ 101ASENTRY_NP(dcache_inv_pou_all) 102 mrc CP15_CLIDR(r0) 103 ands r0, r0, #0x07000000 104 mov r0, r0, lsr #26 /* Get LoUU (naturally aligned) */ 105 beq 4f 106 1071: mcr CP15_CSSELR(r0) /* set cache level */ 108 isb 109 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 110 111 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 112 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 113 clz r1, r3 /* number of bits to MSB of way */ 114 lsl r3, r3, r1 /* shift into position */ 115 mov ip, #1 116 lsl ip, ip, r1 /* ip now contains the way decr */ 117 118 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 119 add r0, r0, #4 /* apply bias */ 120 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 121 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 122 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 123 mov r1, #1 124 lsl r1, r1, r0 /* r1 now contains the set decr */ 125 mov r2, ip /* r2 now contains set way decr */ 126 127 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1282: mcr CP15_DCISW(r3) /* clean & invalidate line */ 129 movs r0, r3 /* get current way/set */ 130 beq 3f /* at 0 means we are done */ 131 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 132 subne r3, r3, r1 /* non-zero?, decrement set */ 133 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 134 b 2b 135 1363: 137 mrc CP15_CSSELR(r0) /* get cache level */ 138 add r0, r0, #2 /* next level */ 139 mrc CP15_CLIDR(r1) 140 ands r1, r1, #0x07000000 141 mov r1, r1, lsr #26 /* Get LoUU (naturally aligned) */ 142 cmp r1, r0 143 bgt 1b 144 1454: dsb /* wait for stores to finish */ 146 mov r0, #0 147 mcr CP15_CSSELR(r0) 148 bx lr 149END(dcache_inv_pou_all) 150 151/* Write back and Invalidate D cache to PoC. */ 152ASENTRY_NP(dcache_wbinv_poc_all) 153 mrc CP15_CLIDR(r0) 154 ands r0, r0, #0x07000000 155 mov r0, r0, lsr #23 /* Get LoC (naturally aligned) */ 156 beq 4f 157 1581: mcr CP15_CSSELR(r0) /* set cache level */ 159 isb 160 mrc CP15_CCSIDR(r0) /* read CCSIDR */ 161 162 ubfx r2, r0, #13, #15 /* get num sets - 1 from CCSIDR */ 163 ubfx r3, r0, #3, #10 /* get num ways - 1 from CCSIDR */ 164 clz r1, r3 /* number of bits to MSB of way */ 165 lsl r3, r3, r1 /* shift into position */ 166 mov ip, #1 167 lsl ip, ip, r1 /* ip now contains the way decr */ 168 169 ubfx r0, r0, #0, #3 /* get linesize from CCSIDR */ 170 add r0, r0, #4 /* apply bias */ 171 lsl r2, r2, r0 /* shift sets by log2(linesize) */ 172 add r3, r3, r2 /* merge numsets - 1 with numways - 1 */ 173 sub ip, ip, r2 /* subtract numsets - 1 from way decr */ 174 mov r1, #1 175 lsl r1, r1, r0 /* r1 now contains the set decr */ 176 mov r2, ip /* r2 now contains set way decr */ 177 178 /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 1792: mcr CP15_DCCISW(r3) /* clean & invalidate line */ 180 movs r0, r3 /* get current way/set */ 181 beq 3f /* at 0 means we are done */ 182 movs r0, r0, lsl #10 /* clear way bits leaving only set bits*/ 183 subne r3, r3, r1 /* non-zero?, decrement set */ 184 subeq r3, r3, r2 /* zero?, decrement way and restore set count */ 185 b 2b 186 1873: 188 mrc CP15_CSSELR(r0) /* get cache level */ 189 add r0, r0, #2 /* next level */ 190 mrc CP15_CLIDR(r1) 191 ands r1, r1, #0x07000000 192 mov r1, r1, lsr #23 /* Get LoC (naturally aligned) */ 193 cmp r1, r0 194 bgt 1b 195 1964: dsb /* wait for stores to finish */ 197 mov r0, #0 198 mcr CP15_CSSELR(r0) 199 bx lr 200END(dcache_wbinv_poc_all) 201 202#endif /* __ARM_ARCH >= 7 */ 203