tlk: 5/22 update
[3rdparty/ote_partner/tlk.git] / arch / arm / arm / cache-ops.S
1 /*
2  * Copyright (c) 2008-2013 Travis Geiselbrecht
3  * Copyright (c) 2013-2014, NVIDIA CORPORATION. All rights reserved
4  *
5  * Permission is hereby granted, free of charge, to any person obtaining
6  * a copy of this software and associated documentation files
7  * (the "Software"), to deal in the Software without restriction,
8  * including without limitation the rights to use, copy, modify, merge,
9  * publish, distribute, sublicense, and/or sell copies of the Software,
10  * and to permit persons to whom the Software is furnished to do so,
11  * subject to the following conditions:
12  *
13  * The above copyright notice and this permission notice shall be
14  * included in all copies or substantial portions of the Software.
15  *
16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
19  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
20  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
21  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
22  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23  */
24 #include <asm.h>
25 #include <arch/ops.h>
26 #include <arch/defines.h>
27
28 .text
29
30 #if ARM_WITH_CACHE
31
32 /* low level cache routines for various cpu families */
33
34 #if ARM_CPU_ARM1136 || ARM_CPU_ARM926
35
36 /* void arch_disable_cache(uint flags) */
37 FUNCTION(arch_disable_cache)
38         mov             r12, #0                 @ zero register
39         mrs             r3, cpsr                @ save the old interrupt state
40 #if ARM_ISA_ARMv6
41         cpsid   iaf                             @ interrupts disabled
42 #else
43         orr             r3, r3, #(1<<7)
44         msr             cpsr, r3
45 #endif
46
47 .Ldcache_disable:
48         tst             r0, #DCACHE
49         beq             .Licache_disable
50         mrc             p15, 0, r1, c1, c0, 0   @ cr1
51         tst             r1, #(1<<2)             @ is the dcache already disabled?
52         beq             .Licache_disable
53
54         bic             r1, #(1<<2)
55         mcr             p15, 0, r1, c1, c0, 0   @ disable dcache
56
57 #if ARM_CPU_ARM1136
58     mcr     p15, 0, r12, c7, c14, 0             @ clean & invalidate dcache
59 #elif ARM_CPU_ARM926
60 0:
61         mrc             p15, 0, r15, c7, c14, 3 @ clean & invalidate dcache
62         bne             0b
63 #else
64 #error whut?
65 #endif
66         mcr             p15, 0, r0, c7, c10, 4  @ data sync barrier (formerly drain write buffer)
67
68 .Licache_disable:
69         tst             r0, #ICACHE
70         beq             .Ldone_disable
71
72         mrc     p15, 0, r1, c1, c0, 0           @ cr1
73         bic             r1, #(1<<12)
74         mcr             p15, 0, r1, c1, c0, 0   @ disable icache
75
76         mcr             p15, 0, r12, c7, c5, 0  @ invalidate icache
77
78 .Ldone_disable:
79         msr             cpsr, r3
80         bx              lr
81
82 /* void arch_enable_cache(uint flags) */
83 FUNCTION(arch_enable_cache)
84         mov             r12, #0                 @ zero register
85         mrs             r3, cpsr                @ save the old interrupt state
86 #if ARM_ISA_ARMv6
87         cpsid iaf                               @ interrupts disabled
88 #else
89         orr             r3, r3, #(1<<7)
90         msr             cpsr, r3
91 #endif
92
93 .Ldcache_enable:
94         tst             r0, #DCACHE
95         beq             .Licache_enable
96         mrc             p15, 0, r1, c1, c0, 0           @ cr1
97         tst             r1, #(1<<2)             @ is the dcache already enabled?
98         bne             .Licache_enable
99
100         mcr             p15, 0, r12, c7, c6, 0          @ invalidate dcache
101
102         orr             r1, #(1<<2)
103         mcr             p15, 0, r1, c1, c0, 0           @ enable dcache
104
105 .Licache_enable:
106         tst             r0, #ICACHE
107         beq             .Ldone_enable
108
109         mcr             p15, 0, r12, c7, c5, 0          @ invalidate icache
110
111         mrc             p15, 0, r1, c1, c0, 0           @ cr1
112         orr             r1, #(1<<12)
113         mcr             p15, 0, r1, c1, c0, 0           @ enable icache
114
115 .Ldone_enable:
116         msr             cpsr, r3
117         bx              lr
118
119 #elif ARM_CPU_CORTEX_A9 || ARM_CPU_CORTEX_A15
120
121 /* void arch_disable_cache(uint flags) */
122 FUNCTION(arch_disable_cache)
123         stmfd   sp!, {r4-r11, lr}
124         mov     r7, r0                          @ save flags
125         mrs     r12, cpsr                       @ save the old interrupt state
126         cpsid   iaf                             @ cpsid iaf, interrupts disabled
127
128 .Ldcache_disable:
129         tst     r7, #DCACHE
130         beq     .Licache_disable
131         mrc     p15, 0, r0, c1, c0, 0           @ system control register
132         tst     r0, #(1<<2)                     @ is the dcache already disabled?
133         beq     .Ldcache_already_disabled
134
135         bic     r0, #(1<<2)
136         mcr     p15, 0, r0, c1, c0, 0           @ disable dcache
137
138 #if ARM_CPU_CORTEX_A9
139         @ flush and invalidate the dcache
140         @ NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
141         bl      flush_invalidate_cache_v7
142 #endif
143
144         b       .Ldcache_disable_L2
145
146 .Ldcache_already_disabled:
147 #if ARM_CPU_CORTEX_A9
148         @ make sure all of the caches are invalidated
149         @ NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
150         bl      invalidate_cache_v7
151 #endif
152 .Ldcache_disable_L2:
153
154 #if ARM_WITH_L2
155         mrc     p15, 0, r0, c1, c0, 1           @ auxiliary control register
156         bic     r0, #(1<<1)
157         mcr     p15, 0, r0, c1, c0, 1           @ disable L2 dcache
158 #endif
159
160 .Licache_disable:
161         tst     r7, #ICACHE
162         beq     .Ldone_disable
163
164         mrc     p15, 0, r0, c1, c0, 0           @ system control register
165         bic     r0, #(1<<12)
166         mcr     p15, 0, r0, c1, c0, 0           @ disable icache
167
168 .Ldone_disable:
169 #if ARM_CPU_CORTEX_A9
170         mov     r0, #0
171         mcr     p15, 0, r0, c7, c5, 0           @ invalidate icache to PoU
172 #endif
173         msr     cpsr, r12
174         ldmfd   sp!, {r4-r11, pc}
175
176 /* void arch_enable_cache(uint flags) */
177 FUNCTION(arch_enable_cache)
178         stmfd   sp!, {r4-r11, lr}
179         mov     r7, r0                          @ save flags
180         mrs     r12, cpsr                       @ save the old interrupt state
181         cpsid   iaf                             @ cpsid iaf, interrupts disabled
182
183 .Ldcache_enable:
184         tst     r7, #DCACHE
185         beq     .Licache_enable
186         mrc     p15, 0, r0, c1, c0, 0           @ system control register
187         tst     r0, #(1<<2)                     @ is the dcache already enabled?
188         bne     .Licache_enable
189
190         @ invalidate L1 and L2
191         @ NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
192 #if ARM_WITH_L2
193         bl      invalidate_cache_v7
194
195         @ enable the L2, if present
196         mrc     p15, 0, r0, c1, c0, 1           @ auxiliary control register
197         orr     r0, #(1<<1)
198         mcr     p15, 0, r0, c1, c0, 1           @ enable L2 dcache
199 #endif
200
201         dmb
202         mrc     p15, 0, r0, c1, c0, 0           @ system control register
203         orr     r0, #(1<<2)
204         dsb
205         mcr     p15, 0, r0, c1, c0, 0           @ enable dcache
206
207 .Licache_enable:
208         tst     r7, #ICACHE
209         beq     .Ldone_enable
210
211 #if ARM_CPU_CORTEX_A9
212         mov     r0, #0
213         mcr     p15, 0, r0, c7, c5, 0           @ invalidate icache to PoU
214 #endif
215         mrc     p15, 0, r0, c1, c0, 0           @ system control register
216         orr     r0, #(1<<12)
217         mcr     p15, 0, r0, c1, c0, 0           @ enable icache
218
219 .Ldone_enable:
220         msr     cpsr, r12
221         ldmfd   sp!, {r4-r11, pc}
222
223 @ flush & invalidate cache routine, trashes r0-r6, r9-r11
224 flush_invalidate_cache_v7:
225 /*
226  * Clean and flush the cache to maintain consistency.
227  * From ARMv7 manual, B2-17
228  *
229  * On exit,
230  *  r1, r2, r3, r9, r10, r11, r12 corrupted
231  * This routine must preserve:
232  *  r4, r6, r7, r8
233  */
234         mrc     p15, 0, r10, c0, c1, 5  @ read ID_MMFR1
235         tst     r10, #0xf << 16         @ hierarchical cache (ARMv7)
236         mov     r10, #0
237         beq     hierarchical
238         mcr     p15, 0, r10, c7, c14, 0 @ clean+invalidate D
239         b       iflush
240 hierarchical:
241         mcr     p15, 0, r10, c7, c10, 5 @ DMB
242         stmfd   sp!, {r0-r7, r9-r11}
243         mrc     p15, 1, r0, c0, c0, 1   @ read clidr
244         ands    r3, r0, #0x7000000      @ extract loc from clidr
245         mov     r3, r3, lsr #23         @ left align loc bit field
246         beq     finished                @ if loc is 0, then no need to clean
247         mov     r10, #0                 @ start clean at cache level 0
248 loop1:
249         add     r2, r10, r10, lsr #1    @ work out 3x current cache level
250         mov     r1, r0, lsr r2          @ extract cache type bits from clidr
251         and     r1, r1, #7              @ mask of the bits for current cache only
252         cmp     r1, #2                  @ see what cache we have at this level
253         blt     skip                    @ skip if no cache, or just i-cache
254         mcr     p15, 2, r10, c0, c0, 0  @ select current cache level in cssr
255         mcr     p15, 0, r10, c7, c5, 4  @ isb to sych the new cssr&csidr
256         mrc     p15, 1, r1, c0, c0, 0   @ read the new csidr
257         and     r2, r1, #7              @ extract the length of the cache lines
258         add     r2, r2, #4              @ add 4 (line length offset)
259         ldr     r4, =0x3ff
260         ands    r4, r4, r1, lsr #3      @ find maximum number on the way size
261         clz     r5, r4                  @ find bit position of way size increment
262         ldr     r7, =0x7fff
263         ands    r7, r7, r1, lsr #13     @ extract max number of the index size
264 loop2:
265         mov     r9, r4                  @ create working copy of max way size
266 loop3:
267         orr     r11, r10, r9, lsl r5    @ factor way and cache number into r11
268         orr     r11, r11, r7, lsl r2    @ factor index number into r11
269         mcr     p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way
270         subs    r9, r9, #1              @ decrement the way
271         bge     loop3
272         subs    r7, r7, #1              @ decrement the index
273         bge     loop2
274 skip:
275         add     r10, r10, #2            @ increment cache number
276         cmp     r3, r10
277         bgt     loop1
278 finished:
279         ldmfd   sp!, {r0-r7, r9-r11}
280         mov     r10, #0                 @ swith back to cache level 0
281         mcr     p15, 2, r10, c0, c0, 0  @ select current cache level in cssr
282 iflush:
283         mcr     p15, 0, r10, c7, c10, 4 @ DSB
284         mcr     p15, 0, r10, c7, c5, 0  @ invalidate I+BTB
285         mcr     p15, 0, r10, c7, c10, 4 @ DSB
286         mcr     p15, 0, r10, c7, c5, 4  @ ISB
287         mov     pc, lr
288
289 /*
290  *      invalidate_cache_v7
291  *
292  *      Corrupted registers: r0-r7, r9-r11
293  */
294 invalidate_cache_v7:
295         dmb                                     @ ensure ordering with previous memory accesses
296         mrc     p15, 1, r0, c0, c0, 1           @ read clidr
297         ands    r3, r0, #0x7000000              @ extract loc from clidr
298         mov     r3, r3, lsr #23                 @ left align loc bit field
299         beq     1005f                           @ if loc is 0, then no need to clean
300         mov     r10, #0                         @ start clean at cache level 0
301 1001:
302         add     r2, r10, r10, lsr #1            @ work out 3x current cache level
303         mov     r1, r0, lsr r2                  @ extract cache type bits from clidr
304         and     r1, r1, #7                      @ mask of the bits for current cache only
305         cmp     r1, #2                          @ see what cache we have at this level
306         blt     1004f                           @ skip if no cache, or just i-cache
307
308         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
309         isb                                     @ isb to sych the new cssr&csidr
310         mrc     p15, 1, r1, c0, c0, 0           @ read the new csidr
311
312         and     r2, r1, #7                      @ extract the length of the cache lines
313         add     r2, r2, #4                      @ add 4 (line length offset)
314         ldr     r4, =0x3ff
315         ands    r4, r4, r1, lsr #3              @ find maximum number on the way size
316         clz     r5, r4                          @ find bit position of way size increment
317         ldr     r7, =0x7fff
318         ands    r7, r7, r1, lsr #13             @ extract max number of the index size
319 1002:
320         mov     r9, r4                          @ create working copy of max way size
321 1003:
322         orr     r11, r10, r9, lsl r5            @ factor way and cache number into r11
323         orr     r11, r11, r7, lsl r2            @ factor index number into r11
324         mcr     p15, 0, r11, c7, c14, 2         @ clean/flush by set/way
325         subs    r9, r9, #1                      @ decrement the way
326         bge     1003b
327         subs    r7, r7, #1                      @ decrement the index
328         bge     1002b
329 1004:
330         add     r10, r10, #2                    @ increment cache number
331         cmp     r3, r10
332         bgt     1001b
333 1005:
334         mov     r10, #0                         @ switch back to cache level 0
335         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
336         dsb
337         isb
338         mov     pc, lr
339
340 #else
341 #error unhandled cpu
342 #endif
343
344 #if ARM_CPU_ARM926 || ARM_CPU_ARM1136 || ARM_CPU_CORTEX_A8 || ARM_CPU_CORTEX_A9 || ARM_CPU_CORTEX_A15
345 /* shared cache flush routines */
346
347         /* void arch_flush_cache_range(addr_t start, size_t len); */
348 FUNCTION(arch_clean_cache_range)
349 #if ARM_WITH_CP15
350         add             r2, r0, r1                                      // calculate the end address
351         bic             r0, #(CACHE_LINE-1)                     // align the start with a cache line
352 0:
353         mcr             p15, 0, r0, c7, c10, 1          // clean cache to PoC by MVA
354         add             r0, r0, #CACHE_LINE
355         cmp             r0, r2
356         blo             0b
357
358         /* copied from ops.S */
359 #if defined(ARM_CPU_CORTEX_A8) || defined(ARM_CPU_CORTEX_A9) || defined(ARM_CPU_CORTEX_A15)
360         dsb             sy
361 #elif ARM_CPU_ARM1136
362         mov             r0, #0
363         mcr             p15, 0, r0, c7, c10, 4          // data sync barrier
364 #endif
365 #endif
366         bx              lr
367
368         /* void arch_flush_invalidate_cache_range(addr_t start, size_t len); */
369 FUNCTION(arch_clean_invalidate_cache_range)
370 #if ARM_WITH_CP15
371         add             r2, r0, r1                                      // calculate the end address
372         bic             r0, #(CACHE_LINE-1)                     // align the start with a cache line
373 0:
374         mcr             p15, 0, r0, c7, c14, 1          // clean & invalidate dcache to PoC by MVA
375         add             r0, r0, #CACHE_LINE
376         cmp             r0, r2
377         blo             0b
378
379         /* copied from ops.S */
380 #if defined(ARM_CPU_CORTEX_A8) || defined(ARM_CPU_CORTEX_A9) || defined(ARM_CPU_CORTEX_A15)
381         dsb             sy
382 #elif ARM_CPU_ARM1136
383         mov             r0, #0
384         mcr             p15, 0, r0, c7, c10, 4          // data sync barrier
385 #endif
386 #endif
387         bx              lr
388
389         /* void arch_invalidate_cache_range(addr_t start, size_t len); */
390 FUNCTION(arch_invalidate_cache_range)
391 #if ARM_WITH_CP15
392         add             r2, r0, r1                                      // calculate the end address
393         bic             r0, #(CACHE_LINE-1)                     // align the start with a cache line
394 0:
395         mcr             p15, 0, r0, c7, c6, 1           // invalidate dcache to PoC by MVA
396         add             r0, r0, #CACHE_LINE
397         cmp             r0, r2
398         blo             0b
399
400         /* copied from ops.S */
401 #if defined(ARM_CPU_CORTEX_A8) || defined(ARM_CPU_CORTEX_A9) || defined(ARM_CPU_CORTEX_A15)
402         dsb             sy
403 #elif ARM_CPU_ARM1136
404         mov             r0, #0
405         mcr             p15, 0, r0, c7, c10, 4          // data sync barrier
406 #endif
407 #endif
408         bx              lr
409
410         /* void arch_sync_cache_range(addr_t start, size_t len); */
411 FUNCTION(arch_sync_cache_range)
412         push    { r14 }
413         bl      arch_clean_cache_range
414
415         mov     r0, #0
416         mcr     p15, 0, r0, c7, c5, 0       // invalidate icache to PoU
417
418         pop     { pc }
419
420 #endif // ARM_CPU_...
421
422 #else
423
424 /* no cache */
425
426 FUNCTION(arch_disable_cache)
427         bx              lr
428
429 FUNCTION(arch_enable_cache)
430         bx              lr
431
432 FUNCTION(arch_clean_cache_range)
433         bx              lr
434
435 FUNCTION(arch_clean_invalidate_cache_range)
436         bx              lr
437
438 FUNCTION(arch_sync_cache_range)
439         bx              lr
440
441 #endif // ARM_WITH_CACHE
442