port-arm/48710: Fixes for kernel ARMv7 cache invalidation routine

[jwerner%chromium.org@localhost]

>Number:         48710
>Category:       port-arm
>Synopsis:       Fixes for kernel ARMv7 cache invalidation routine
>Confidential:   no
>Severity:       non-critical
>Priority:       medium
>Responsible:    port-arm-maintainer
>State:          open
>Class:          sw-bug
>Submitter-Id:   net
>Arrival-Date:   Fri Apr 04 22:00:00 +0000 2014
>Originator:     Julius Werner
>Release:        Topmost SVN revision
>Organization:
>Environment:
I never actually built NetBSD, sorry.
>Description:
Hi,

I'm not really interested in NetBSD itself, but I recently needed some 
BSD-compatible ARMv7 cache invalidation routines for the coreboot project and 
decided to take yours (the function armv7_dcache_wbinv_all from 
src/sys/arch/arm/arm/cpufunc_asm_armv7.S). It's a nice piece of code, but it 
has some bugs in it which you probably want to fix as well:

1. The code uses R3 to store the current cache level, and counts upwards 
towards the Level of Coherency to iterate through all of them. Unfortunately, 
it already initializes R3 to LoC, so you are essentially "done" immediately and 
never really run the main loop. You want to start at 0 instead.

2. At the end of the invalidation loop, you try to extract the set from the 
way/set/level mask in R0. You shift it left to drop the way (10 highest bits), 
and then right to drop the level (4 lowest bits), but the second shift didn't 
account for the fact that the register was already shifted once. It should 
shift by 14, not 4.

3. Right after that you use conditional execution to check if the leftover set 
bits are zero or not, but in order to do that (at least in ARM mode) you need 
to use instructions that explictly set the condition code. The second shift 
should be LSRS, not just LSR.
>How-To-Repeat:

>Fix:
Here's a diff that should fix all those problems (and some whitespace issues on 
top). It also reorders the way .Lnext_level_wbinv works to reuse a bit of that 
code in the initialization part. I have tested this code in coreboot and 
confirmed that it works as it should, but I have not actually compiled the 
NetBSD version (it should apply fine, though). You probably also want similar 
fixes in armv7_dcache_inv_all and armv7_icache_inv_all, but I did not take a 
close look at them.

--- a/src/sys/arch/arm/arm/cpufunc_asm_armv7.S
+++ b/src/sys/arch/arm/arm/cpufunc_asm_armv7.S
@@ -400,15 +400,20 @@
 /* * LINTSTUB: void armv7_dcache_wbinv_all(void); */
 ENTRY_NP(armv7_dcache_wbinv_all)
+       dsb
+       mov     r3, #-2                 @ initialize level so that we start at 0
+
+.Lnext_level_wbinv:
+       add     r3, r3, #2              @ increment level
+
        mrc     p15, 1, r0, c0, c0, 1   @ read CLIDR
-       ands    r3, r0, #0x07000000
-       beq     .Ldone_wbinv
-       lsr     r3, r3, #23             @ left align loc (low 4 bits)
+       and     ip, r0, #0x07000000     @ narrow to LoC
+       lsr     ip, ip, #23             @ left align LoC (low 4 bits)
+       cmp     r3, ip                  @ compare
+       bge     .Ldone_wbinv            @ else fall through (r0 == CLIDR)

-       mov     r1, #0
-.Lstart_wbinv:
-       add     r2, r3, r3, lsr #1      @ r2 = level * 3 / 2
+       add     r2, r3, r3, lsr #1      @ r2 = (level << 1) * 3 / 2
        mov     r1, r0, lsr r2          @ r1 = cache type
-       bfc     r1, #3, #29
+       and     r1, r1, #7
        cmp     r1, #2                  @ is it data or i&d?
        blt     .Lnext_level_wbinv      @ nope, skip level

@@ -420,14 +425,14 @@ ENTRY_NP(armv7_dcache_wbinv_all)
        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 
+       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                  @
+       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
@@ -438,20 +443,12 @@ ENTRY_NP(armv7_dcache_wbinv_all)
 1:     mcr     p15, 0, r3, c7, c14, 2  @ writeback and invalidate line
        cmp     r3, #15                 @ are we done with this level (way/set 
== 0)
        bls     .Lnext_level_wbinv      @ yes, go to next level
-       lsl     r0, r3, #10             @ clear way bits leaving only set/level 
bits
-       lsr     r0, r0, #4              @ clear level bits leaving only set bits
+       lsr     r0, r3, #4              @ clear level bits leaving only way/set 
bits
+       lsls    r0, r0, #14             @ clear way bits leaving only set bits
        subne   r3, r3, r1              @ non-zero?, decrement set #
        subeq   r3, r3, r2              @ zero?, decrement way # and restore 
set count
        b       1b

-.Lnext_level_wbinv:
-       mrc     p15, 1, r0, c0, c0, 1   @ read CLIDR
-       and     ip, r0, #0x07000000     @ narrow to LoC
-       lsr     ip, ip, #23             @ left align LoC (low 4 bits)
-       add     r3, r3, #2              @ go to next level
-       cmp     r3, ip                  @ compare
-       blt     .Lstart_wbinv           @ not done, next level (r0 == CLIDR)
-
 .Ldone_wbinv:
        mov     r0, #0                  @ default back to cache level 0
        mcr     p15, 2, r0, c0, c0, 0   @ select cache level