Index: sys/stand/efiboot/bootarm/cache.S
===================================================================
RCS file: /cvsroot/src/sys/stand/efiboot/bootarm/cache.S,v
retrieving revision 1.1
diff -u -p -r1.1 cache.S
--- sys/stand/efiboot/bootarm/cache.S	30 Mar 2019 12:48:50 -0000	1.1
+++ sys/stand/efiboot/bootarm/cache.S	5 Apr 2019 20:50:07 -0000
@@ -55,41 +55,75 @@ END(armv7_dcache_wbinv_range)
 
 /* * LINTSTUB: void armv7_icache_inv_all(void); */
 ENTRY_NP(armv7_icache_inv_all)
-	mov	r0, #0
-	mcr	p15, 2, r0, c0, c0, 0	@ set cache level to L1
+	dsb				@ wait for stores to finish
+	mov	r0, #0			@ and ...
+	mcr	p15, 0, r0, c7, c5, 0	@ invalidate L1 cache
+	isb				@ instruction sync barrier
+	bx	lr			@ return
+END(armv7_icache_inv_all)
+
+
+/* * LINTSTUB: void armv7_dcache_wbinv_all(void); */
+ENTRY_NP(armv7_dcache_wbinv_all)
+	mrc	p15, 1, r0, c0, c0, 1	@ read CLIDR
+	tst	r0, #0x07000000
+	bxeq	lr
+	mov	r3, #0			@ start with L1
+
+.Lstart_wbinv:
+	add	r2, r3, r3, lsr #1	@ r2 = level * 3 / 2
+	mov	r1, r0, lsr r2		@ r1 = cache type
+	tst	r1, #6			@ is it unified or data?
+	beq	.Lnext_level_wbinv	@ nope, skip level
+
+	mcr	p15, 2, r3, c0, c0, 0	@ select cache level
+	isb
 	mrc	p15, 1, r0, c0, c0, 0	@ read CCSIDR
 
-	ubfx	r2, r0, #13, #15	@ get num sets - 1 from CCSIDR
-	ubfx	r3, r0, #3, #10		@ get numways - 1 from CCSIDR
-	clz	r1, r3			@ number of bits to MSB of way
-	lsl	r3, r3, r1		@ shift into position
-	mov	ip, #1			@
-	lsl	ip, ip, r1		@ ip now contains the way decr
-
-	ubfx	r0, r0, #0, #3		@ get linesize from CCSIDR
-	add	r0, r0, #4		@ apply bias
-	lsl	r2, r2, r0		@ shift sets by log2(linesize)
-	add	r3, r3, r2		@ merge numsets - 1 with numways - 1
-	sub	ip, ip, r2		@ subtract numsets - 1 from way decr
+	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, r0		@ r1 now contains the set decr
-	mov	r2, ip			@ r2 now contains set way decr
+	lsl	r1, r1, ip		@ r1 = set decr
 
-	/* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
-1:	mcr	p15, 0, r3, c7, c6, 2	@ DCISW (data cache invalidate by set/way)
-	movs	r0, r3			@ get current way/set
-	beq	2f			@ at 0 means we are done.
-	lsls	r0, r0, #10		@ clear way bits leaving only set bits
+	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, c14, 2	@ DCCISW (data cache clean and invalidate by set/way)
+	cmp	r3, #15			@ are we done with this level (way/set == 0)
+	bls	.Lnext_level_wbinv	@ yes, go to next level
+	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
 
-2:	dsb				@ wait for stores to finish
-	mov	r0, #0			@ and ...
-	mcr	p15, 0, r0, c7, c5, 0	@ invalidate L1 cache
-	isb				@ instruction sync barrier
-	bx	lr			@ return
-END(armv7_icache_inv_all)
+.Lnext_level_wbinv:
+	dsb
+	mrc	p15, 1, r0, c0, c0, 1	@ read CLIDR
+	ubfx	ip, r0, #24, #3		@ narrow to LoC
+	add	r3, r3, #2		@ go to next level
+	cmp	r3, ip, lsl #1		@ 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
+	dsb
+	isb
+	bx	lr
+END(armv7_dcache_wbinv_all)
+
 
 ENTRY_NP(armv7_exec_kernel)
 	mov	r4, r0			@ kernel entry
@@ -99,6 +133,7 @@ ENTRY_NP(armv7_exec_kernel)
 	mrc	p15, 0, r0, c1, c0, 0	@ SCTLR read
 	bic	r0, r0, #5		@ disable dcache and MMU
 	mcr	p15, 0, r0, c1, c0, 0	@ SCTLR write
+	isb
 
 	/* Invalidate TLB */
 	dsb
Index: sys/stand/efiboot/bootarm/efibootarm.c
===================================================================
RCS file: /cvsroot/src/sys/stand/efiboot/bootarm/efibootarm.c,v
retrieving revision 1.2
diff -u -p -r1.2 efibootarm.c
--- sys/stand/efiboot/bootarm/efibootarm.c	30 Mar 2019 17:41:13 -0000	1.2
+++ sys/stand/efiboot/bootarm/efibootarm.c	5 Apr 2019 20:50:07 -0000
@@ -34,6 +34,7 @@
 #include <loadfile.h>
 
 /* cache.S */
+void armv7_dcache_wbinv_all(void);
 void armv7_dcache_wbinv_range(vaddr_t, vsize_t);
 void armv7_icache_inv_all(void);
 void armv7_exec_kernel(register_t, register_t);
@@ -47,6 +48,7 @@ efi_dcache_flush(u_long start, u_long si
 void
 efi_boot_kernel(u_long marks[MARK_MAX])
 {
+#if 0
 	u_long kernel_size;
 
 	kernel_size = marks[MARK_END] - marks[MARK_START];
@@ -54,6 +56,8 @@ efi_boot_kernel(u_long marks[MARK_MAX])
 	armv7_dcache_wbinv_range(marks[MARK_START], kernel_size);
 	if (efi_fdt_size() > 0)
 		armv7_dcache_wbinv_range((u_long)efi_fdt_data(), efi_fdt_size());
+#endif
+	armv7_dcache_wbinv_all();
 	armv7_icache_inv_all();
 
 	armv7_exec_kernel((register_t)marks[MARK_ENTRY], (register_t)efi_fdt_data());