Add code to clean and invalidate caches

Linux expects to be handed a system with caches disabled and
invalidated. While the model currently brings CPUs up with caches
invalidated, we'll need to invalidate caches when leaving the
bootwrapper if we enable them within the bootwrapper (e.g. for the use
of exclusive operations), as lines may be allocated.

This patch adds code to invalidate the dcaches and icaches. It is not
yet called.

Signed-off-by: Mark Rutland <mark.rutland@arm.com>

3 files changed, 85 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index 827dc7c..50dca3b 100644
--- a/Makefile
+++ b/Makefile
@@ -66,9 +66,9 @@ DTC		:= $(if $(wildcard ./dtc), ./dtc, $(shell which dtc))
 all: $(IMAGE)
 
 clean:
-	rm -f $(IMAGE) boot.o gic.o ns.o $(BOOTMETHOD) model.lds fdt.dtb
+	rm -f $(IMAGE) boot.o cache.o gic.o ns.o $(BOOTMETHOD) model.lds fdt.dtb
 
-$(IMAGE): boot.o gic.o ns.o $(BOOTMETHOD) model.lds fdt.dtb $(KERNEL) $(FILESYSTEM)
+$(IMAGE): boot.o cache.o gic.o ns.o $(BOOTMETHOD) model.lds fdt.dtb $(KERNEL) $(FILESYSTEM)
 	$(LD) -o $@ --script=model.lds
 
 %.o: %.S Makefile
diff --git a/cache.S b/cache.S
new file mode 100644
index 0000000..ce928eb
--- /dev/null
+++ b/cache.S
@@ -0,0 +1,81 @@
+/*
+ * cache.S - simple cache clean+invalidate code for stand-alone Linux booting
+ *
+ * Copyright (C) 2013 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+	.text
+
+	.globl flush_caches
+
+flush_caches:
+	mrs	x0, clidr_el1
+
+	/* find what out the max cache level to flush */
+	lsr	x1, x0, #24
+	and	x1, x0, #(0x7)
+	cbz	x1, dcaches_done
+
+	mov	x2, #0			/* level 1 (represented 1-off) */
+
+1:	cmp	x2, x1			/* gone over all levels */
+	b.eq	dcaches_done
+
+	/* find out if we have a cache at this level */
+	add	x3, x2, x2, lsl 1	/* amount to shift for CtypeN */
+	lsr	x4, x0, x3
+	and	x4, x4, #0x7
+
+	cmp	x4, #1
+	b.eq	5f			/* no dcache at this level */
+
+	lsl	x3, x2, #1
+	msr	csselr_el1, x3
+	isb
+	mrs	x3, ccsidr_el1
+	and	x4, x3, #0x7
+	add	x4, x4, #4		/* log2 line size, corrected for offset */
+	ubfx	x6, x3, #3, #10		/* ways */
+	clz	w5, w6			/* log2 ways, rounded down */
+	ubfx	x7, x3, #13, #15	/* sets */
+
+	/* loop over ways */
+2:	mov	x8, x7			/* temporary (sets) */
+
+	/* loop over sets */
+	/* build the set/way command */
+3:	lsl	x9, x2, #1		/* cache level (-1) */
+	lsl	x10, x8, x5		/* way << shift */
+	orr	x9, x9, x10
+	lsl	x10, x6, x4		/* set << line size */
+	orr	x9, x9, x10
+
+	dc	cisw, x9
+	dsb	sy
+
+	cbz	x8, 4f
+	sub	x8, x8, #1
+	b	3b
+
+4:	/* completed all sets for this way */
+	cbz	x6, 5f
+	sub	x6, x6, #1
+	b	2b
+
+5:	/* finished this level, try the next */
+	dsb	sy
+	add	x2, x2, #1
+	b	1b
+
+dcaches_done:
+
+	dsb	sy
+	ic	iallu
+	dsb	sy
+	isb
+	ret
+
+	.ltorg
+	.org 0x100
diff --git a/model.lds.S b/model.lds.S
index 3931857..53d0f7f 100644
--- a/model.lds.S
+++ b/model.lds.S
@@ -12,6 +12,7 @@ OUTPUT_ARCH(aarch64)
 TARGET(binary)
 
 INPUT(./boot.o)
+INPUT(./cache.o)
 INPUT(./gic.o)
 INPUT(./ns.o)
 INPUT(./BOOTMETHOD)
@@ -26,6 +27,7 @@ SECTIONS
 {
 	. = PHYS_OFFSET;
 	.text : { boot.o }
+	.text : { cache.o }
 	.text : { gic.o }
 	.text : { ns.o }
 	.text : { BOOTMETHOD }