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>

diff --git a/Makefile.am b/Makefile.am
index 0918b4ecd3f3b36a6be710d95f1434cd9c3ae3fb..dd36375e6e44a8c50ed406511b9f670fc939bec1 100644 (file)
--- 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 (file)
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 c5315f2bc39bfae5253222624f452e44b1799dd2..eb3d8d713bf9d511aa3f5a4a1774e8e641f0f1a0 100644 (file)
--- 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 (file)
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 41e1ded3bec7d5e1ccc0af157af01ee08d6b33f3..b4dae949722d2eef9fc51a779e07d6b112201b57 100644 (file)
--- 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__ */