[src/trunk]: src/sys/arch/arm/arm Provide a armv7_dcache_l1inv_all

[skrll <skrll%NetBSD.org@localhost>]

details:   https://anonhg.NetBSD.org/src/rev/266a011d4e20
branches:  trunk
changeset: 338582:266a011d4e20
user:      skrll <skrll%NetBSD.org@localhost>
date:      Sat May 30 21:23:17 2015 +0000

description:
Provide a armv7_dcache_l1inv_all

diffstat:

 sys/arch/arm/arm/cpufunc_asm_armv7.S |  47 ++++++++++++++++++++++++++++++++++++
 1 files changed, 47 insertions(+), 0 deletions(-)

diffs (57 lines):

diff -r b53cbfeadd0d -r 266a011d4e20 sys/arch/arm/arm/cpufunc_asm_armv7.S
--- a/sys/arch/arm/arm/cpufunc_asm_armv7.S      Sat May 30 21:05:18 2015 +0000
+++ b/sys/arch/arm/arm/cpufunc_asm_armv7.S      Sat May 30 21:23:17 2015 +0000
@@ -339,6 +339,53 @@
        bx      lr                      @ return
 END(armv7_icache_inv_all)
 
+/* * LINTSTUB: void armv7_dcache_l1inv_all(void); */
+ENTRY_NP(armv7_cache_l1inv_all)
+       mrc     p15, 1, r0, c0, c0, 1   @ read CLIDR
+       and     r0, r0, #0x7            @ check L1
+       bxeq    lr                      @ return if no L1 cache
+       mov     r3, #0                  @ start with L1
+       mcr     p15, 2, r3, c0, c0, 0   @ select cache level
+       isb
+       mrc     p15, 1, r0, c0, c0, 0   @ read CCSIDR
+
+       ubfx    ip, r0, #0, #3          @ get linesize from CCSIDR
+       add     ip, ip, #4              @ apply bias
+       ubfx    r2, r0, #13, #15        @ get numsets - 1 from CCSIDR
+       lsl     r2, r2, ip              @ shift to set position
+       orr     r3, r3, r2              @ merge set into way/set/level
+       mov     r1, #1
+       lsl     r1, r1, ip              @ r1 = set decr
+
+       ubfx    ip, r0, #3, #10         @ get numways - 1 from [to be discarded] CCSIDR
+       clz     r2, ip                  @ number of bits to MSB of way
+       lsl     ip, ip, r2              @ shift by that into way position
+       mov     r0, #1                  @
+       lsl     r2, r0, r2              @ r2 now contains the way decr
+       mov     r0, r3                  @ get sets/level (no way yet)
+       orr     r3, r3, ip              @ merge way into way/set/level
+       bfc     r0, #0, #4              @ clear low 4 bits (level) to get numset - 1
+       sub     r2, r2, r0              @ subtract from way decr
+
+       /* r3 = ways/sets/level, r2 = way decr, r1 = set decr, r0 and ip are free */
+1:     mcr     p15, 0, r3, c7, c6, 2   @ invalidate line
+       cmp     r3, #15                 @ are we done with this level (way/set == 0)
+       bls     .Ldone_l1inv            @ yes, we've finished
+       ubfx    r0, r3, #4, #18         @ extract set bits
+       cmp     r0, #0                  @ compare
+       subne   r3, r3, r1              @ non-zero?, decrement set #
+       subeq   r3, r3, r2              @ zero?, decrement way # and restore set count
+       b       1b
+
+.Ldone_l1inv:
+       dsb
+       mov     r0, #0                  @ default back to cache level 0
+       mcr     p15, 2, r0, c0, c0, 0   @ select cache level
+       dsb
+       isb
+       bx      lr
+END(armv7_dcache_l1inv_all)
+
 /* * LINTSTUB: void armv7_dcache_inv_all(void); */
 ENTRY_NP(armv7_dcache_inv_all)
        mrc     p15, 1, r0, c0, c0, 1   @ read CLIDR