arm: Fix up new cache flush algorithm and replace dcache_*_all() with it

This patch fixes the remaining few bugs in our shiny new cache iteration
by set/way/level algorithm to actually make it work: It makes it start
from cache level 0 (previously it would always start at LoC and be
"done" instantly), fixes up the two shifts that isolate the set bits at
the end (which didn't seem to account for the fact that the first shift
affects the second), and throws an S bit on that last shift so that it
actually affects the conditionals after it.

In addition, also moves the next_level block to the top so that we can
share (and thus eliminate) some code at initialization, and turns the
whole thing into a thrice-instantiated macro to create functions that
fit our existing interface.

Change-Id: I1338a589cbb37d74ea6e7a3d4f67ff827e24edbe
Signed-off-by: Julius Werner <jwerner@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/183879
Reviewed-by: Stefan Reinauer <reinauer@google.com>
Reviewed-by: David Hendricks <dhendrix@chromium.org>
(cherry picked from commit 6d94f8330191c316fe093ddb5288329453da8a4b)
Signed-off-by: Isaac Christensen <isaac.christensen@se-eng.com>
Reviewed-on: http://review.coreboot.org/6932
Tested-by: build bot (Jenkins)
Reviewed-by: Patrick Georgi <patrick@georgi-clan.de>

1 files changed, 45 insertions, 27 deletions

diff --git a/src/arch/arm/armv7/cpu.S b/src/arch/arm/armv7/cpu.S
index 42e2354d44..29a19e76df 100644
--- a/src/arch/arm/armv7/cpu.S
+++ b/src/arch/arm/armv7/cpu.S
@@ -30,25 +30,39 @@
  * SUCH DAMAGE.
  */
 
+#include <arch/asm.h>
+
 /*
- * These work very hard to not push registers onto the stack and to limit themselves
- * to use r0-r3 and ip.
+ * Dcache invalidations by set/way work by passing a [way:sbz:set:sbz:level:0]
+ * bitfield in a register to the appropriate MCR instruction. This algorithm
+ * works by initializing a bitfield with the highest-numbered set and way, and
+ * generating a "set decrement" and a "way decrement". The former just contains
+ * the LSB of the set field, but the latter contains the LSB of the way field
+ * minus the highest valid set field... such that when you subtract it from a
+ * [way:0:level] field you end up with a [way - 1:highest_set:level] field
+ * through the magic of double subtraction. It's quite ingenius, really.
+ * Takes care to only use r0-r3 and ip so it's pefectly ABI-compatible without
+ * needing to write to memory.
  */
 
-/* * LINTSTUB: void armv7_dcache_wbinv_all(void); */
-ENTRY_NP(armv7_dcache_wbinv_all)
+.macro	dcache_apply_all crm
+	dsb
+	mov	r3, #-2			@ initialize level so that we start at 0
+
+1:	@next_level
+	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	3f @done		@ 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, #28
 	cmp	r1, #2			@ is it data or i&d?
-	blt	.Lnext_level_wbinv	@ nope, skip level
+	blt	1b @next_level		@ nope, skip level
 
 	mcr	p15, 2, r3, c0, c0, 0	@ select cache level
 	isb
@@ -65,7 +79,7 @@ ENTRY_NP(armv7_dcache_wbinv_all)
 	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
@@ -73,27 +87,31 @@ ENTRY_NP(armv7_dcache_wbinv_all)
 	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, c14, 2	@ writeback and invalidate line
+2:	mcr	p15, 0, r3, c7, \crm, 2	@ writeback and/or 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
+	bls	1b @next_level		@ yes, go to next level
+	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
+	b	2b
 
-.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:
+3:	@done
 	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_wbinv_all)
+.endm
+
+ENTRY(dcache_invalidate_all)
+	dcache_apply_all crm=c6
+ENDPROC(dcache_invalidate_all)
+
+ENTRY(dcache_clean_all)
+	dcache_apply_all crm=c10
+ENDPROC(dcache_clean_all)
+
+ENTRY(dcache_clean_invalidate_all)
+	dcache_apply_all crm=c14
+ENDPROC(dcache_clean_invalidate_all)