Rewrite cache flush in C

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Signed-off-by: Mark Rutland <mark.rutland@arm.com>

5 files changed, 93 insertions, 82 deletions

diff --git a/Makefile.am b/Makefile.am
index 0918b4e..dd36375 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -90,8 +90,8 @@ CPPFLAGS	+= $(INITRD_FLAGS)
 CFLAGS		+= -Iinclude/ -I$(ARCH_SRC)/include/
 CFLAGS		+= -Wall -fomit-frame-pointer
 
-OFILES		+= boot_common.o ns.o $(GIC)
-OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o mmu.o $(BOOTMETHOD) utils.o)
+OFILES		+= boot_common.o ns.o $(GIC) cache.o
+OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o mmu.o $(BOOTMETHOD) utils.o)
 
 all: $(IMAGE)
 
diff --git a/arch/aarch64/cache.S b/arch/aarch64/cache.S
deleted file mode 100644
index d8f2336..0000000
--- a/arch/aarch64/cache.S
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
- * arch/aarch64/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, x1, #(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		/* max way index */
-	clz	w5, w6			/* 32 - log2 ways */
-	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, x6, x5		/* way << shift */
-	orr	x9, x9, x10
-	lsl	x10, x8, 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
diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index c5315f2..eb3d8d7 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -50,6 +50,37 @@ static inline uint64_t read_id_aa64pfr0(void)
 	return val;
 }
 
+static inline uint32_t read_clidr(void)
+{
+	uint32_t val;
+
+	asm volatile ("mrs	%0, clidr_el1" : "=r" (val));
+	return val;
+}
+
+static inline uint32_t read_ccsidr(void)
+{
+	uint32_t val;
+
+	asm volatile ("mrs	%0, ccsidr_el1" : "=r" (val));
+	return val;
+}
+
+static inline void write_csselr(uint32_t val)
+{
+	asm volatile ("msr	csselr_el1, %0" : : "r" (val));
+}
+
+static inline void dccisw(uint32_t val)
+{
+	asm volatile ("dc	cisw, %0" : : "r" (val));
+}
+
+static inline void iciallu(void)
+{
+	asm volatile ("ic	iallu");
+}
+
 static inline int has_gicv3_sysreg(void)
 {
 	return !!((read_id_aa64pfr0() >> 24) & 0xf);
diff --git a/cache.c b/cache.c
new file mode 100644
index 0000000..9d71248
--- /dev/null
+++ b/cache.c
@@ -0,0 +1,58 @@
+/*
+ * cache.c - simple cache clean+invalidate code
+ *
+ * Copyright (C) 2015 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.
+ */
+
+#include <cpu.h>
+
+void flush_caches(void)
+{
+	unsigned int level;
+	uint32_t clidr = read_clidr();
+	unsigned int max_level = (clidr >> 24) & 0x7;
+
+	uint32_t ccsidr;
+
+	if (max_level == 0)
+		return;
+
+	for (level = 0; level < max_level; level++) {
+		uint32_t cache_type = (clidr >> (level * 3)) & 0x7;
+		unsigned int line_size, num_ways, num_sets, way_shift;
+		unsigned int way, set;
+
+		if (cache_type == 1)
+			/* No dcache at this level */
+			continue;
+
+		write_csselr(level << 1);
+		isb();
+		ccsidr = read_ccsidr();
+
+		line_size = (ccsidr & 0x7) + 4; /* log2 line size */
+		num_ways = ((ccsidr >> 3) & 0x3ff) + 1;
+		num_sets = ((ccsidr >> 13) & 0x7fff) + 1;
+
+		way_shift = clz(num_ways - 1);
+		for (way = 0; way < num_ways; way++) {
+			for (set = 0; set < num_sets; set++) {
+				uint32_t command = level << 1;
+				command |= way << way_shift;
+				command |= set << line_size;
+
+				dccisw(command);
+				dsb(sy);
+			}
+		}
+
+		dsb(sy);
+	}
+	dsb(sy);
+	iciallu();
+	dsb(sy);
+	isb();
+}
diff --git a/include/cpu.h b/include/cpu.h
index 41e1ded..b4dae94 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -20,6 +20,8 @@
 #define sev()		asm volatile ("sev\n" : : : "memory")
 #define wfe()		asm volatile ("wfe\n" : : : "memory")
 
+#define clz(val)	__builtin_clz(val)
+
 unsigned int find_logical_id(unsigned long mpidr);
 
 #endif /* !__ASSEMBLY__ */