| @@ -1,406 +1,441 @@ | | | @@ -1,406 +1,441 @@ |
1 | /*- | | 1 | /*- |
2 | * Copyright (c) 2010 Per Odlund <per.odlund@armagedon.se> | | 2 | * Copyright (c) 2010 Per Odlund <per.odlund@armagedon.se> |
3 | * | | 3 | * |
4 | * Redistribution and use in source and binary forms, with or without | | 4 | * Redistribution and use in source and binary forms, with or without |
5 | * modification, are permitted provided that the following conditions | | 5 | * modification, are permitted provided that the following conditions |
6 | * are met: | | 6 | * are met: |
7 | * 1. Redistributions of source code must retain the above copyright | | 7 | * 1. Redistributions of source code must retain the above copyright |
8 | * notice, this list of conditions and the following disclaimer. | | 8 | * notice, this list of conditions and the following disclaimer. |
9 | * 2. Redistributions in binary form must reproduce the above copyright | | 9 | * 2. Redistributions in binary form must reproduce the above copyright |
10 | * notice, this list of conditions and the following disclaimer in the | | 10 | * notice, this list of conditions and the following disclaimer in the |
11 | * documentation and/or other materials provided with the distribution. | | 11 | * documentation and/or other materials provided with the distribution. |
12 | * | | 12 | * |
13 | * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS | | 13 | * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS |
14 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED | | 14 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
15 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR | | 15 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
16 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS | | 16 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS |
17 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | | 17 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
18 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | | 18 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
19 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | | 19 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
20 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | | 20 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
21 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | | 21 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
22 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | | 22 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
23 | * POSSIBILITY OF SUCH DAMAGE. | | 23 | * POSSIBILITY OF SUCH DAMAGE. |
24 | */ | | 24 | */ |
25 | | | 25 | |
26 | /* ARMv7 assembly functions for manipulating caches and other core functions. | | 26 | /* ARMv7 assembly functions for manipulating caches and other core functions. |
27 | * Based on cpufuncs for v6 and xscale. | | 27 | * Based on cpufuncs for v6 and xscale. |
28 | */ | | 28 | */ |
29 | | | 29 | |
30 | #include "assym.h" | | 30 | #include "assym.h" |
31 | #include <machine/asm.h> | | 31 | #include <machine/asm.h> |
32 | #include <arm/locore.h> | | 32 | #include <arm/locore.h> |
33 | | | 33 | |
34 | .arch armv7a | | 34 | .arch armv7a |
35 | | | 35 | |
36 | ENTRY(armv7_cpu_sleep) | | 36 | ENTRY(armv7_cpu_sleep) |
37 | wfi @ wait for an interrupt | | 37 | wfi @ wait for an interrupt |
38 | b irq_idle_entry @ assume we got an interrupt | | 38 | b irq_idle_entry @ assume we got an interrupt |
39 | END(armv7_cpu_sleep) | | 39 | END(armv7_cpu_sleep) |
40 | | | 40 | |
41 | ENTRY(armv7_wait) | | 41 | ENTRY(armv7_wait) |
42 | mrc p15, 0, r0, c2, c0, 0 @ arbitrary read of CP15 | | 42 | mrc p15, 0, r0, c2, c0, 0 @ arbitrary read of CP15 |
43 | add r0, r0, #0 @ a stall | | 43 | add r0, r0, #0 @ a stall |
44 | bx lr | | 44 | bx lr |
45 | END(armv7_wait) | | 45 | END(armv7_wait) |
46 | | | 46 | |
47 | ENTRY(armv7_context_switch) | | 47 | ENTRY(armv7_context_switch) |
48 | dsb @ data synchronization barrier | | 48 | dsb @ data synchronization barrier |
49 | mrc p15, 0, r2, c0, c0, 5 @ get MPIDR | | 49 | mrc p15, 0, ip, c0, c0, 5 @ get MPIDR |
50 | cmp r2, #0 | | 50 | cmp ip, #0 |
51 | orrlt r0, r0, #0x5b @ MP, cachable (Normal WB) | | 51 | orrlt r0, r0, #0x5b @ MP, cachable (Normal WB) |
52 | orrge r0, r0, #0x1b @ Non-MP, cacheable, normal WB | | 52 | orrge r0, r0, #0x1b @ Non-MP, cacheable, normal WB |
53 | mcr p15, 0, r0, c2, c0, 0 @ set the new TTB | | 53 | mcr p15, 0, r0, c2, c0, 0 @ set the new TTB |
54 | #ifdef MULTIPROCESSOR | | 54 | #ifdef MULTIPROCESSOR |
55 | mcr p15, 0, r0, c8, c3, 0 @ flush the I+D | | 55 | mcr p15, 0, r0, c8, c3, 0 @ flush the I+D |
56 | #else | | 56 | #else |
57 | mcr p15, 0, r0, c8, c7, 0 @ flush the I+D | | 57 | mcr p15, 0, r0, c8, c7, 0 @ flush the I+D |
58 | #endif | | 58 | #endif |
59 | dsb | | 59 | dsb |
60 | isb | | 60 | isb |
61 | bx lr | | 61 | bx lr |
62 | END(armv7_context_switch) | | 62 | END(armv7_context_switch) |
63 | | | 63 | |
| | | 64 | #ifdef ARM_MMU_EXTENDED |
| | | 65 | ENTRY(armv7_tlb_flushID_ASID) |
| | | 66 | #ifdef MULTIPROCESSOR |
| | | 67 | mcr p15, 0, r0, c8, c3, 2 @ flush I+D tlb all ASID |
| | | 68 | #else |
| | | 69 | mcr p15, 0, r0, c8, c7, 2 @ flush I+D tlb all ASID |
| | | 70 | #endif |
| | | 71 | dsb @ data synchronization barrier |
| | | 72 | isb |
| | | 73 | bx lr |
| | | 74 | END(armv7_tlb_flushID_ASID) |
| | | 75 | #endif |
| | | 76 | |
| | | 77 | STRONG_ALIAS(armv7_tlb_flushD_SE, armv7_tlb_flushID_SE) |
| | | 78 | STRONG_ALIAS(armv7_tlb_flushI_SE, armv7_tlb_flushID_SE) |
64 | ENTRY(armv7_tlb_flushID_SE) | | 79 | ENTRY(armv7_tlb_flushID_SE) |
| | | 80 | #ifdef ARM_MMU_EXTENDED |
| | | 81 | bfi r0, r1, #0, #8 @ insert ASID into MVA |
| | | 82 | #endif |
65 | #ifdef MULTIPROCESSOR | | 83 | #ifdef MULTIPROCESSOR |
66 | mcr p15, 0, r0, c8, c3, 1 @ flush I+D tlb single entry | | 84 | mcr p15, 0, r0, c8, c3, 1 @ flush I+D tlb single entry |
67 | #else | | 85 | #else |
68 | mcr p15, 0, r0, c8, c7, 1 @ flush I+D tlb single entry | | 86 | mcr p15, 0, r0, c8, c7, 1 @ flush I+D tlb single entry |
69 | #endif | | 87 | #endif |
70 | dsb @ data synchronization barrier | | 88 | dsb @ data synchronization barrier |
71 | isb | | 89 | isb |
72 | bx lr | | 90 | bx lr |
73 | END(armv7_tlb_flushID_SE) | | 91 | END(armv7_tlb_flushID_SE) |
74 | | | 92 | |
| | | 93 | ENTRY(armv7_tlb_flushD) |
| | | 94 | mov r0, #0 |
| | | 95 | mcr p15, 0, r0, c8, c6, 0 @ flush entire D tlb |
| | | 96 | dsb @ data synchronization barrier |
| | | 97 | isb |
| | | 98 | bx lr |
| | | 99 | END(armv7_tlb_flushD) |
| | | 100 | |
| | | 101 | STRONG_ALIAS(armv7_tlb_flushI, armv7_tlb_flushID) |
| | | 102 | ENTRY(armv7_tlb_flushID) |
| | | 103 | mov r0, #0 |
| | | 104 | mcr p15, 0, r0, c8, c7, 0 @ flush entire I+D tlb |
| | | 105 | dsb @ data synchronization barrier |
| | | 106 | isb |
| | | 107 | bx lr |
| | | 108 | END(armv7_tlb_flushID) |
| | | 109 | |
75 | | | 110 | |
76 | ENTRY_NP(armv7_setttb) | | 111 | ENTRY_NP(armv7_setttb) |
77 | mrc p15, 0, r2, c0, c0, 5 @ get MPIDR | | 112 | mrc p15, 0, ip, c0, c0, 5 @ get MPIDR |
78 | cmp r2, #0 | | 113 | cmp ip, #0 |
79 | orrlt r0, r0, #0x5b @ MP, cachable (Normal WB) | | 114 | orrlt r0, r0, #0x5b @ MP, cachable (Normal WB) |
80 | orrge r0, r0, #0x1b @ Non-MP, cacheable, normal WB | | 115 | orrge r0, r0, #0x1b @ Non-MP, cacheable, normal WB |
81 | mcr p15, 0, r0, c2, c0, 0 @ load new TTB | | 116 | mcr p15, 0, r0, c2, c0, 0 @ load new TTB |
82 | cmp r1, #0 | | 117 | cmp r1, #0 |
83 | #ifdef MULTIPROCESSOR | | 118 | #ifdef MULTIPROCESSOR |
84 | mcrne p15, 0, r0, c8, c3, 0 @ invalidate all I+D TLBs | | 119 | mcrne p15, 0, r0, c8, c3, 0 @ invalidate all I+D TLBs |
85 | #else | | 120 | #else |
86 | mcrne p15, 0, r0, c8, c7, 0 @ invalidate all I+D TLBs | | 121 | mcrne p15, 0, r0, c8, c7, 0 @ invalidate all I+D TLBs |
87 | #endif | | 122 | #endif |
88 | dsb @ data synchronization barrier | | 123 | dsb @ data synchronization barrier |
89 | isb | | 124 | isb |
90 | bx lr | | 125 | bx lr |
91 | END(armv7_setttb) | | 126 | END(armv7_setttb) |
92 | | | 127 | |
93 | /* Other functions. */ | | 128 | /* Other functions. */ |
94 | | | 129 | |
95 | ENTRY_NP(armv7_drain_writebuf) | | 130 | ENTRY_NP(armv7_drain_writebuf) |
96 | dsb @ data synchronization barrier | | 131 | dsb @ data synchronization barrier |
97 | RET | | 132 | RET |
98 | END(armv7_drain_writebuf) | | 133 | END(armv7_drain_writebuf) |
99 | | | 134 | |
100 | /* Cache operations. */ | | 135 | /* Cache operations. */ |
101 | | | 136 | |
102 | /* LINTSTUB: void armv7_icache_sync_range(vaddr_t, vsize_t); */ | | 137 | /* LINTSTUB: void armv7_icache_sync_range(vaddr_t, vsize_t); */ |
103 | ENTRY_NP(armv7_icache_sync_range) | | 138 | ENTRY_NP(armv7_icache_sync_range) |
104 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR | | 139 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR |
105 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) | | 140 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) |
106 | mov ip, #16 @ make a bit mask | | 141 | mov ip, #16 @ make a bit mask |
107 | lsl r2, ip, r2 @ and shift into position | | 142 | lsl r2, ip, r2 @ and shift into position |
108 | sub ip, r2, #1 @ make into a mask | | 143 | sub ip, r2, #1 @ make into a mask |
109 | and r3, r0, ip @ get offset into cache line | | 144 | and r3, r0, ip @ get offset into cache line |
110 | add r1, r1, r3 @ add to length | | 145 | add r1, r1, r3 @ add to length |
111 | bic r0, r0, ip @ clear offset from start. | | 146 | bic r0, r0, ip @ clear offset from start. |
112 | 1: | | 147 | 1: |
113 | mcr p15, 0, r0, c7, c10, 1 @ wb the D-Cache line | | 148 | mcr p15, 0, r0, c7, c10, 1 @ wb the D-Cache line |
114 | mcr p15, 0, r0, c7, c5, 1 @ invalidate the I-Cache line | | 149 | mcr p15, 0, r0, c7, c5, 1 @ invalidate the I-Cache line |
115 | add r0, r0, r2 | | 150 | add r0, r0, r2 |
116 | subs r1, r1, r2 | | 151 | subs r1, r1, r2 |
117 | bhi 1b | | 152 | bhi 1b |
118 | | | 153 | |
119 | dsb @ data synchronization barrier | | 154 | dsb @ data synchronization barrier |
120 | isb | | 155 | isb |
121 | bx lr | | 156 | bx lr |
122 | END(armv7_icache_sync_range) | | 157 | END(armv7_icache_sync_range) |
123 | | | 158 | |
124 | /* LINTSTUB: void armv7_icache_sync_all(void); */ | | 159 | /* LINTSTUB: void armv7_icache_sync_all(void); */ |
125 | ENTRY_NP(armv7_icache_sync_all) | | 160 | ENTRY_NP(armv7_icache_sync_all) |
126 | /* | | 161 | /* |
127 | * We assume that the code here can never be out of sync with the | | 162 | * We assume that the code here can never be out of sync with the |
128 | * dcache, so that we can safely flush the Icache and fall through | | 163 | * dcache, so that we can safely flush the Icache and fall through |
129 | * into the Dcache cleaning code. | | 164 | * into the Dcache cleaning code. |
130 | */ | | 165 | */ |
131 | stmdb sp!, {r0, lr} | | 166 | stmdb sp!, {r0, lr} |
132 | bl _C_LABEL(armv7_idcache_wbinv_all) @clean the D cache | | 167 | bl _C_LABEL(armv7_idcache_wbinv_all) @clean the D cache |
133 | ldmia sp!, {r0, lr} | | 168 | ldmia sp!, {r0, lr} |
134 | dsb @ data synchronization barrier | | 169 | dsb @ data synchronization barrier |
135 | isb | | 170 | isb |
136 | bx lr | | 171 | bx lr |
137 | END(armv7_icache_sync_all) | | 172 | END(armv7_icache_sync_all) |
138 | | | 173 | |
139 | ENTRY(armv7_dcache_wb_range) | | 174 | ENTRY(armv7_dcache_wb_range) |
140 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR | | 175 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR |
141 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) | | 176 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) |
142 | mov ip, #16 @ make a bit mask | | 177 | mov ip, #16 @ make a bit mask |
143 | lsl r2, ip, r2 @ and shift into position | | 178 | lsl r2, ip, r2 @ and shift into position |
144 | sub ip, r2, #1 @ make into a mask | | 179 | sub ip, r2, #1 @ make into a mask |
145 | and r3, r0, ip @ get offset into cache line | | 180 | and r3, r0, ip @ get offset into cache line |
146 | add r1, r1, r3 @ add to length | | 181 | add r1, r1, r3 @ add to length |
147 | bic r0, r0, ip @ clear offset from start. | | 182 | bic r0, r0, ip @ clear offset from start. |
148 | dsb | | 183 | dsb |
149 | 1: | | 184 | 1: |
150 | mcr p15, 0, r0, c7, c10, 1 @ wb the D-Cache to PoC | | 185 | mcr p15, 0, r0, c7, c10, 1 @ wb the D-Cache to PoC |
151 | add r0, r0, r2 | | 186 | add r0, r0, r2 |
152 | subs r1, r1, r2 | | 187 | subs r1, r1, r2 |
153 | bhi 1b | | 188 | bhi 1b |
154 | dsb @ data synchronization barrier | | 189 | dsb @ data synchronization barrier |
155 | bx lr | | 190 | bx lr |
156 | END(armv7_dcache_wb_range) | | 191 | END(armv7_dcache_wb_range) |
157 | | | 192 | |
158 | /* LINTSTUB: void armv7_dcache_wbinv_range(vaddr_t, vsize_t); */ | | 193 | /* LINTSTUB: void armv7_dcache_wbinv_range(vaddr_t, vsize_t); */ |
159 | ENTRY(armv7_dcache_wbinv_range) | | 194 | ENTRY(armv7_dcache_wbinv_range) |
160 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR | | 195 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR |
161 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) | | 196 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) |
162 | mov ip, #16 @ make a bit mask | | 197 | mov ip, #16 @ make a bit mask |
163 | lsl r2, ip, r2 @ and shift into position | | 198 | lsl r2, ip, r2 @ and shift into position |
164 | sub ip, r2, #1 @ make into a mask | | 199 | sub ip, r2, #1 @ make into a mask |
165 | and r3, r0, ip @ get offset into cache line | | 200 | and r3, r0, ip @ get offset into cache line |
166 | add r1, r1, r3 @ add to length | | 201 | add r1, r1, r3 @ add to length |
167 | bic r0, r0, ip @ clear offset from start. | | 202 | bic r0, r0, ip @ clear offset from start. |
168 | dsb | | 203 | dsb |
169 | 1: | | 204 | 1: |
170 | mcr p15, 0, r0, c7, c14, 1 @ wb and inv the D-Cache line | | 205 | mcr p15, 0, r0, c7, c14, 1 @ wb and inv the D-Cache line |
171 | add r0, r0, r2 | | 206 | add r0, r0, r2 |
172 | subs r1, r1, r2 | | 207 | subs r1, r1, r2 |
173 | bhi 1b | | 208 | bhi 1b |
174 | dsb @ data synchronization barrier | | 209 | dsb @ data synchronization barrier |
175 | bx lr | | 210 | bx lr |
176 | END(armv7_dcache_wbinv_range) | | 211 | END(armv7_dcache_wbinv_range) |
177 | | | 212 | |
178 | /* * LINTSTUB: void armv7_dcache_inv_range(vaddr_t, vsize_t); */ | | 213 | /* * LINTSTUB: void armv7_dcache_inv_range(vaddr_t, vsize_t); */ |
179 | ENTRY(armv7_dcache_inv_range) | | 214 | ENTRY(armv7_dcache_inv_range) |
180 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR | | 215 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR |
181 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) | | 216 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) |
182 | mov ip, #16 @ make a bit mask | | 217 | mov ip, #16 @ make a bit mask |
183 | lsl r2, ip, r2 @ and shift into position | | 218 | lsl r2, ip, r2 @ and shift into position |
184 | sub ip, r2, #1 @ make into a mask | | 219 | sub ip, r2, #1 @ make into a mask |
185 | and r3, r0, ip @ get offset into cache line | | 220 | and r3, r0, ip @ get offset into cache line |
186 | add r1, r1, r3 @ add to length | | 221 | add r1, r1, r3 @ add to length |
187 | bic r0, r0, ip @ clear offset from start. | | 222 | bic r0, r0, ip @ clear offset from start. |
188 | 1: | | 223 | 1: |
189 | mcr p15, 0, r0, c7, c6, 1 @ invalidate the D-Cache line | | 224 | mcr p15, 0, r0, c7, c6, 1 @ invalidate the D-Cache line |
190 | add r0, r0, r2 | | 225 | add r0, r0, r2 |
191 | subs r1, r1, r2 | | 226 | subs r1, r1, r2 |
192 | bhi 1b | | 227 | bhi 1b |
193 | | | 228 | |
194 | dsb @ data synchronization barrier | | 229 | dsb @ data synchronization barrier |
195 | bx lr | | 230 | bx lr |
196 | END(armv7_dcache_inv_range) | | 231 | END(armv7_dcache_inv_range) |
197 | | | 232 | |
198 | | | 233 | |
199 | /* * LINTSTUB: void armv7_idcache_wbinv_range(vaddr_t, vsize_t); */ | | 234 | /* * LINTSTUB: void armv7_idcache_wbinv_range(vaddr_t, vsize_t); */ |
200 | ENTRY(armv7_idcache_wbinv_range) | | 235 | ENTRY(armv7_idcache_wbinv_range) |
201 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR | | 236 | mrc p15, 1, r2, c0, c0, 0 @ read CCSIDR |
202 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) | | 237 | and r2, r2, #7 @ get line size (log2(size)-4, 0=16) |
203 | mov ip, #16 @ make a bit mask | | 238 | mov ip, #16 @ make a bit mask |
204 | lsl r2, ip, r2 @ and shift into position | | 239 | lsl r2, ip, r2 @ and shift into position |
205 | sub ip, r2, #1 @ make into a mask | | 240 | sub ip, r2, #1 @ make into a mask |
206 | and r3, r0, ip @ get offset into cache line | | 241 | and r3, r0, ip @ get offset into cache line |
207 | add r1, r1, r3 @ add to length | | 242 | add r1, r1, r3 @ add to length |
208 | bic r0, r0, ip @ clear offset from start. | | 243 | bic r0, r0, ip @ clear offset from start. |
209 | dsb | | 244 | dsb |
210 | 1: | | 245 | 1: |
211 | mcr p15, 0, r0, c7, c5, 1 @ invalidate the I-Cache line | | 246 | mcr p15, 0, r0, c7, c5, 1 @ invalidate the I-Cache line |
212 | mcr p15, 0, r0, c7, c14, 1 @ wb and inv the D-Cache line | | 247 | mcr p15, 0, r0, c7, c14, 1 @ wb and inv the D-Cache line |
213 | add r0, r0, r2 | | 248 | add r0, r0, r2 |
214 | subs r1, r1, r2 | | 249 | subs r1, r1, r2 |
215 | bhi 1b | | 250 | bhi 1b |
216 | | | 251 | |
217 | dsb @ data synchronization barrier | | 252 | dsb @ data synchronization barrier |
218 | isb | | 253 | isb |
219 | bx lr | | 254 | bx lr |
220 | END(armv7_idcache_wbinv_range) | | 255 | END(armv7_idcache_wbinv_range) |
221 | | | 256 | |
222 | /* * LINTSTUB: void armv7_idcache_wbinv_all(void); */ | | 257 | /* * LINTSTUB: void armv7_idcache_wbinv_all(void); */ |
223 | ENTRY_NP(armv7_idcache_wbinv_all) | | 258 | ENTRY_NP(armv7_idcache_wbinv_all) |
224 | /* | | 259 | /* |
225 | * We assume that the code here can never be out of sync with the | | 260 | * We assume that the code here can never be out of sync with the |
226 | * dcache, so that we can safely flush the Icache and fall through | | 261 | * dcache, so that we can safely flush the Icache and fall through |
227 | * into the Dcache purging code. | | 262 | * into the Dcache purging code. |
228 | */ | | 263 | */ |
229 | dmb | | 264 | dmb |
230 | mcr p15, 0, r0, c7, c5, 0 | | 265 | mcr p15, 0, r0, c7, c5, 0 |
231 | b _C_LABEL(armv7_dcache_wbinv_all) | | 266 | b _C_LABEL(armv7_dcache_wbinv_all) |
232 | END(armv7_idcache_wbinv_all) | | 267 | END(armv7_idcache_wbinv_all) |
233 | | | 268 | |
234 | /* | | 269 | /* |
235 | * These work very hard to not push registers onto the stack and to limit themselves | | 270 | * These work very hard to not push registers onto the stack and to limit themselves |
236 | * to use r0-r3 and ip. | | 271 | * to use r0-r3 and ip. |
237 | */ | | 272 | */ |
238 | /* * LINTSTUB: void armv7_icache_inv_all(void); */ | | 273 | /* * LINTSTUB: void armv7_icache_inv_all(void); */ |
239 | ENTRY_NP(armv7_icache_inv_all) | | 274 | ENTRY_NP(armv7_icache_inv_all) |
240 | mov r0, #0 | | 275 | mov r0, #0 |
241 | mcr p15, 2, r0, c0, c0, 0 @ set cache level to L1 | | 276 | mcr p15, 2, r0, c0, c0, 0 @ set cache level to L1 |
242 | mrc p15, 1, r0, c0, c0, 0 @ read CCSIDR | | 277 | mrc p15, 1, r0, c0, c0, 0 @ read CCSIDR |
243 | | | 278 | |
244 | ubfx r2, r0, #13, #15 @ get num sets - 1 from CCSIDR | | 279 | ubfx r2, r0, #13, #15 @ get num sets - 1 from CCSIDR |
245 | ubfx r3, r0, #3, #10 @ get numways - 1 from CCSIDR | | 280 | ubfx r3, r0, #3, #10 @ get numways - 1 from CCSIDR |
246 | lsl r3, r3, #1 @ double | | 281 | lsl r3, r3, #1 @ double |
247 | sub r3, r3, #1 @ subtract one (now rounded up) | | 282 | sub r3, r3, #1 @ subtract one (now rounded up) |
248 | clz r1, r3 @ number of bits to MSB of way | | 283 | clz r1, r3 @ number of bits to MSB of way |
249 | lsl r3, r3, r1 @ shift into position | | 284 | lsl r3, r3, r1 @ shift into position |
250 | mov ip, #1 @ | | 285 | mov ip, #1 @ |
251 | lsl ip, ip, r1 @ ip now contains the way decr | | 286 | lsl ip, ip, r1 @ ip now contains the way decr |
252 | | | 287 | |
253 | ubfx r0, r0, #0, #3 @ get linesize from CCSIDR | | 288 | ubfx r0, r0, #0, #3 @ get linesize from CCSIDR |
254 | add r0, r0, #4 @ apply bias | | 289 | add r0, r0, #4 @ apply bias |
255 | lsl r2, r2, r0 @ shift sets by log2(linesize) | | 290 | lsl r2, r2, r0 @ shift sets by log2(linesize) |
256 | add r3, r3, r2 @ merge numsets - 1 with numways - 1 | | 291 | add r3, r3, r2 @ merge numsets - 1 with numways - 1 |
257 | sub ip, ip, r2 @ subtract numsets - 1 from way decr | | 292 | sub ip, ip, r2 @ subtract numsets - 1 from way decr |
258 | mov r1, #1 | | 293 | mov r1, #1 |
259 | lsl r1, r1, r0 @ r1 now contains the set decr | | 294 | lsl r1, r1, r0 @ r1 now contains the set decr |
260 | mov r2, ip @ r2 now contains set way decr | | 295 | mov r2, ip @ r2 now contains set way decr |
261 | | | 296 | |
262 | /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ | | 297 | /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ |
263 | 1: mcr p15, 0, r3, c7, c6, 2 @ invalidate line | | 298 | 1: mcr p15, 0, r3, c7, c6, 2 @ invalidate line |
264 | movs r0, r3 @ get current way/set | | 299 | movs r0, r3 @ get current way/set |
265 | beq 2f @ at 0 means we are done. | | 300 | beq 2f @ at 0 means we are done. |
266 | movs r0, r0, lsl #10 @ clear way bits leaving only set bits | | 301 | movs r0, r0, lsl #10 @ clear way bits leaving only set bits |
267 | subne r3, r3, r1 @ non-zero?, decrement set # | | 302 | subne r3, r3, r1 @ non-zero?, decrement set # |
268 | subeq r3, r3, r2 @ zero?, decrement way # and restore set count | | 303 | subeq r3, r3, r2 @ zero?, decrement way # and restore set count |
269 | b 1b | | 304 | b 1b |
270 | | | 305 | |
271 | 2: dsb @ wait for stores to finish | | 306 | 2: dsb @ wait for stores to finish |
272 | mov r0, #0 @ and ... | | 307 | mov r0, #0 @ and ... |
273 | mcr p15, 0, r0, c7, c5, 0 @ invalidate L1 cache | | 308 | mcr p15, 0, r0, c7, c5, 0 @ invalidate L1 cache |
274 | isb @ instruction sync barrier | | 309 | isb @ instruction sync barrier |
275 | bx lr @ return | | 310 | bx lr @ return |
276 | END(armv7_icache_inv_all) | | 311 | END(armv7_icache_inv_all) |
277 | | | 312 | |
278 | /* * LINTSTUB: void armv7_dcache_inv_all(void); */ | | 313 | /* * LINTSTUB: void armv7_dcache_inv_all(void); */ |
279 | ENTRY_NP(armv7_dcache_inv_all) | | 314 | ENTRY_NP(armv7_dcache_inv_all) |
280 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR | | 315 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR |
281 | ands r3, r0, #0x07000000 | | 316 | ands r3, r0, #0x07000000 |
282 | beq .Ldone_inv | | 317 | beq .Ldone_inv |
283 | lsr r3, r3, #23 @ left align loc (low 4 bits) | | 318 | lsr r3, r3, #23 @ left align loc (low 4 bits) |
284 | | | 319 | |
285 | mov r1, #0 | | 320 | mov r1, #0 |
286 | .Lstart_inv: | | 321 | .Lstart_inv: |
287 | add r2, r3, r3, lsr #1 @ r2 = level * 3 / 2 | | 322 | add r2, r3, r3, lsr #1 @ r2 = level * 3 / 2 |
288 | mov r1, r0, lsr r2 @ r1 = cache type | | 323 | mov r1, r0, lsr r2 @ r1 = cache type |
289 | and r1, r1, #7 | | 324 | and r1, r1, #7 |
290 | cmp r1, #2 @ is it data or i&d? | | 325 | cmp r1, #2 @ is it data or i&d? |
291 | blt .Lnext_level_inv @ nope, skip level | | 326 | blt .Lnext_level_inv @ nope, skip level |
292 | | | 327 | |
293 | mcr p15, 2, r3, c0, c0, 0 @ select cache level | | 328 | mcr p15, 2, r3, c0, c0, 0 @ select cache level |
294 | isb | | 329 | isb |
295 | mrc p15, 1, r0, c0, c0, 0 @ read CCSIDR | | 330 | mrc p15, 1, r0, c0, c0, 0 @ read CCSIDR |
296 | | | 331 | |
297 | ubfx ip, r0, #0, #3 @ get linesize from CCSIDR | | 332 | ubfx ip, r0, #0, #3 @ get linesize from CCSIDR |
298 | add ip, ip, #4 @ apply bias | | 333 | add ip, ip, #4 @ apply bias |
299 | ubfx r2, r0, #13, #15 @ get numsets - 1 from CCSIDR | | 334 | ubfx r2, r0, #13, #15 @ get numsets - 1 from CCSIDR |
300 | lsl r2, r2, ip @ shift to set position | | 335 | lsl r2, r2, ip @ shift to set position |
301 | orr r3, r3, r2 @ merge set into way/set/level | | 336 | orr r3, r3, r2 @ merge set into way/set/level |
302 | mov r1, #1 | | 337 | mov r1, #1 |
303 | lsl r1, r1, ip @ r1 = set decr | | 338 | lsl r1, r1, ip @ r1 = set decr |
304 | | | 339 | |
305 | ubfx ip, r0, #3, #10 @ get numways - 1 from [to be discarded] CCSIDR | | 340 | ubfx ip, r0, #3, #10 @ get numways - 1 from [to be discarded] CCSIDR |
306 | lsl ip, ip, #1 @ double | | 341 | lsl ip, ip, #1 @ double |
307 | sub ip, ip, #1 @ subtract one (now rounded up) | | 342 | sub ip, ip, #1 @ subtract one (now rounded up) |
308 | clz r2, ip @ number of bits to MSB of way | | 343 | clz r2, ip @ number of bits to MSB of way |
309 | lsl ip, ip, r2 @ shift by that into way position | | 344 | lsl ip, ip, r2 @ shift by that into way position |
310 | mov r0, #1 @ | | 345 | mov r0, #1 @ |
311 | lsl r2, r0, r2 @ r2 now contains the way decr | | 346 | lsl r2, r0, r2 @ r2 now contains the way decr |
312 | mov r0, r3 @ get sets/level (no way yet) | | 347 | mov r0, r3 @ get sets/level (no way yet) |
313 | orr r3, r3, ip @ merge way into way/set/level | | 348 | orr r3, r3, ip @ merge way into way/set/level |
314 | bfc r0, #0, #4 @ clear low 4 bits (level) to get numset - 1 | | 349 | bfc r0, #0, #4 @ clear low 4 bits (level) to get numset - 1 |
315 | sub r2, r2, r0 @ subtract from way decr | | 350 | sub r2, r2, r0 @ subtract from way decr |
316 | | | 351 | |
317 | /* r3 = ways/sets/level, r2 = way decr, r1 = set decr, r0 and ip are free */ | | 352 | /* r3 = ways/sets/level, r2 = way decr, r1 = set decr, r0 and ip are free */ |
318 | 1: mcr p15, 0, r3, c7, c6, 2 @ invalidate line | | 353 | 1: mcr p15, 0, r3, c7, c6, 2 @ invalidate line |
319 | cmp r3, #15 @ are we done with this level (way/set == 0) | | 354 | cmp r3, #15 @ are we done with this level (way/set == 0) |
320 | bls .Lnext_level_inv @ yes, go to next level | | 355 | bls .Lnext_level_inv @ yes, go to next level |
321 | lsl r0, r3, #10 @ clear way bits leaving only set/level bits | | 356 | lsl r0, r3, #10 @ clear way bits leaving only set/level bits |
322 | lsr r0, r0, #4 @ clear level bits leaving only set bits | | 357 | lsr r0, r0, #4 @ clear level bits leaving only set bits |
323 | subne r3, r3, r1 @ non-zero?, decrement set # | | 358 | subne r3, r3, r1 @ non-zero?, decrement set # |
324 | subeq r3, r3, r2 @ zero?, decrement way # and restore set count | | 359 | subeq r3, r3, r2 @ zero?, decrement way # and restore set count |
325 | b 1b | | 360 | b 1b |
326 | | | 361 | |
327 | .Lnext_level_inv: | | 362 | .Lnext_level_inv: |
328 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR | | 363 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR |
329 | and ip, r0, #0x07000000 @ narrow to LoC | | 364 | and ip, r0, #0x07000000 @ narrow to LoC |
330 | lsr ip, ip, #23 @ left align LoC (low 4 bits) | | 365 | lsr ip, ip, #23 @ left align LoC (low 4 bits) |
331 | add r3, r3, #2 @ go to next level | | 366 | add r3, r3, #2 @ go to next level |
332 | cmp r3, ip @ compare | | 367 | cmp r3, ip @ compare |
333 | blt .Lstart_inv @ not done, next level (r0 == CLIDR) | | 368 | blt .Lstart_inv @ not done, next level (r0 == CLIDR) |
334 | | | 369 | |
335 | .Ldone_inv: | | 370 | .Ldone_inv: |
336 | mov r0, #0 @ default back to cache level 0 | | 371 | mov r0, #0 @ default back to cache level 0 |
337 | mcr p15, 2, r0, c0, c0, 0 @ select cache level | | 372 | mcr p15, 2, r0, c0, c0, 0 @ select cache level |
338 | dsb | | 373 | dsb |
339 | isb | | 374 | isb |
340 | bx lr | | 375 | bx lr |
341 | END(armv7_dcache_inv_all) | | 376 | END(armv7_dcache_inv_all) |
342 | | | 377 | |
343 | /* * LINTSTUB: void armv7_dcache_wbinv_all(void); */ | | 378 | /* * LINTSTUB: void armv7_dcache_wbinv_all(void); */ |
344 | ENTRY_NP(armv7_dcache_wbinv_all) | | 379 | ENTRY_NP(armv7_dcache_wbinv_all) |
345 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR | | 380 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR |
346 | ands r3, r0, #0x07000000 | | 381 | ands r3, r0, #0x07000000 |
347 | beq .Ldone_wbinv | | 382 | beq .Ldone_wbinv |
348 | lsr r3, r3, #23 @ left align loc (low 4 bits) | | 383 | lsr r3, r3, #23 @ left align loc (low 4 bits) |
349 | | | 384 | |
350 | mov r1, #0 | | 385 | mov r1, #0 |
351 | .Lstart_wbinv: | | 386 | .Lstart_wbinv: |
352 | add r2, r3, r3, lsr #1 @ r2 = level * 3 / 2 | | 387 | add r2, r3, r3, lsr #1 @ r2 = level * 3 / 2 |
353 | mov r1, r0, lsr r2 @ r1 = cache type | | 388 | mov r1, r0, lsr r2 @ r1 = cache type |
354 | bfc r1, #3, #28 | | 389 | bfc r1, #3, #28 |
355 | cmp r1, #2 @ is it data or i&d? | | 390 | cmp r1, #2 @ is it data or i&d? |
356 | blt .Lnext_level_wbinv @ nope, skip level | | 391 | blt .Lnext_level_wbinv @ nope, skip level |
357 | | | 392 | |
358 | mcr p15, 2, r3, c0, c0, 0 @ select cache level | | 393 | mcr p15, 2, r3, c0, c0, 0 @ select cache level |
359 | isb | | 394 | isb |
360 | mrc p15, 1, r0, c0, c0, 0 @ read CCSIDR | | 395 | mrc p15, 1, r0, c0, c0, 0 @ read CCSIDR |
361 | | | 396 | |
362 | ubfx ip, r0, #0, #3 @ get linesize from CCSIDR | | 397 | ubfx ip, r0, #0, #3 @ get linesize from CCSIDR |
363 | add ip, ip, #4 @ apply bias | | 398 | add ip, ip, #4 @ apply bias |
364 | ubfx r2, r0, #13, #15 @ get numsets - 1 from CCSIDR | | 399 | ubfx r2, r0, #13, #15 @ get numsets - 1 from CCSIDR |
365 | lsl r2, r2, ip @ shift to set position | | 400 | lsl r2, r2, ip @ shift to set position |
366 | orr r3, r3, r2 @ merge set into way/set/level | | 401 | orr r3, r3, r2 @ merge set into way/set/level |
367 | mov r1, #1 | | 402 | mov r1, #1 |
368 | lsl r1, r1, ip @ r1 = set decr | | 403 | lsl r1, r1, ip @ r1 = set decr |
369 | | | 404 | |
370 | ubfx ip, r0, #3, #10 @ get numways - 1 from [to be discarded] CCSIDR | | 405 | ubfx ip, r0, #3, #10 @ get numways - 1 from [to be discarded] CCSIDR |
371 | lsl ip, ip, #1 @ double | | 406 | lsl ip, ip, #1 @ double |
372 | sub ip, ip, #1 @ subtract one (now rounded up) | | 407 | sub ip, ip, #1 @ subtract one (now rounded up) |
373 | clz r2, ip @ number of bits to MSB of way | | 408 | clz r2, ip @ number of bits to MSB of way |
374 | lsl ip, ip, r2 @ shift by that into way position | | 409 | lsl ip, ip, r2 @ shift by that into way position |
375 | mov r0, #1 @ | | 410 | mov r0, #1 @ |
376 | lsl r2, r0, r2 @ r2 now contains the way decr | | 411 | lsl r2, r0, r2 @ r2 now contains the way decr |
377 | mov r0, r3 @ get sets/level (no way yet) | | 412 | mov r0, r3 @ get sets/level (no way yet) |
378 | orr r3, r3, ip @ merge way into way/set/level | | 413 | orr r3, r3, ip @ merge way into way/set/level |
379 | bfc r0, #0, #4 @ clear low 4 bits (level) to get numset - 1 | | 414 | bfc r0, #0, #4 @ clear low 4 bits (level) to get numset - 1 |
380 | sub r2, r2, r0 @ subtract from way decr | | 415 | sub r2, r2, r0 @ subtract from way decr |
381 | | | 416 | |
382 | /* r3 = ways/sets/level, r2 = way decr, r1 = set decr, r0 and ip are free */ | | 417 | /* r3 = ways/sets/level, r2 = way decr, r1 = set decr, r0 and ip are free */ |
383 | 1: mcr p15, 0, r3, c7, c14, 2 @ writeback and invalidate line | | 418 | 1: mcr p15, 0, r3, c7, c14, 2 @ writeback and invalidate line |
384 | cmp r3, #15 @ are we done with this level (way/set == 0) | | 419 | cmp r3, #15 @ are we done with this level (way/set == 0) |
385 | bls .Lnext_level_wbinv @ yes, go to next level | | 420 | bls .Lnext_level_wbinv @ yes, go to next level |
386 | lsl r0, r3, #10 @ clear way bits leaving only set/level bits | | 421 | lsl r0, r3, #10 @ clear way bits leaving only set/level bits |
387 | lsr r0, r0, #4 @ clear level bits leaving only set bits | | 422 | lsr r0, r0, #4 @ clear level bits leaving only set bits |
388 | subne r3, r3, r1 @ non-zero?, decrement set # | | 423 | subne r3, r3, r1 @ non-zero?, decrement set # |
389 | subeq r3, r3, r2 @ zero?, decrement way # and restore set count | | 424 | subeq r3, r3, r2 @ zero?, decrement way # and restore set count |
390 | b 1b | | 425 | b 1b |
391 | | | 426 | |
392 | .Lnext_level_wbinv: | | 427 | .Lnext_level_wbinv: |
393 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR | | 428 | mrc p15, 1, r0, c0, c0, 1 @ read CLIDR |
394 | and ip, r0, #0x07000000 @ narrow to LoC | | 429 | and ip, r0, #0x07000000 @ narrow to LoC |
395 | lsr ip, ip, #23 @ left align LoC (low 4 bits) | | 430 | lsr ip, ip, #23 @ left align LoC (low 4 bits) |
396 | add r3, r3, #2 @ go to next level | | 431 | add r3, r3, #2 @ go to next level |
397 | cmp r3, ip @ compare | | 432 | cmp r3, ip @ compare |
398 | blt .Lstart_wbinv @ not done, next level (r0 == CLIDR) | | 433 | blt .Lstart_wbinv @ not done, next level (r0 == CLIDR) |
399 | | | 434 | |
400 | .Ldone_wbinv: | | 435 | .Ldone_wbinv: |
401 | mov r0, #0 @ default back to cache level 0 | | 436 | mov r0, #0 @ default back to cache level 0 |
402 | mcr p15, 2, r0, c0, c0, 0 @ select cache level | | 437 | mcr p15, 2, r0, c0, c0, 0 @ select cache level |
403 | dsb | | 438 | dsb |
404 | isb | | 439 | isb |
405 | bx lr | | 440 | bx lr |
406 | END(armv7_dcache_wbinv_all) | | 441 | END(armv7_dcache_wbinv_all) |