commit b7d1ae79887456cb1901d648497ad60d36411db1
parent 9eb96ea3f960237a04270b7e359bbd8d17803cc0
Author: Roberto Vargas <roberto.vargas@arm.com>
Date: Thu, 15 Nov 2018 14:40:25 +0000
Merge branch 'master' of ssh://gerrit.oss.arm.com/trusted-firmware/rcode
Change-Id: Ic71197d1e4489a385ccfe289386377a08996a232
Diffstat:
8 files changed, 116 insertions(+), 5 deletions(-)
diff --git a/arch/amd64/crt-netbsd.s b/arch/amd64/crt-netbsd.s
@@ -9,4 +9,4 @@
.ascii "NetBSD\0\0"
.long 800000000
- .include "crt-posix.s"
+ .include "crt-posix.s"
diff --git a/arch/arm64/Makefile b/arch/arm64/Makefile
@@ -13,6 +13,7 @@ ROMOBJS = rom-crt-$(SYS).o \
$(DLANG) \
$(DRVDIR)/uart.o \
$(SRCDIR)/romfw/builtin.o \
+ cache.o \
RAMOBJS = ram-crt-$(SYS).o \
ram-$(SYS).o \
@@ -30,6 +31,7 @@ ram-crt-none.o rom-crt-none.o: crt-none.s
ram-crt-linux.o rom-crt-linux.o: crt-linux.s
rom-rmode.o: sysreg.h
+cache.o: sysreg.h
sysreg.h: sysreg.lst
./gensysreg.sh -h sysreg.lst
diff --git a/arch/arm64/arch.h b/arch/arm64/arch.h
@@ -6,3 +6,6 @@ enum barrier_type {
extern void wsysreg(enum sysreg, unsigned long long v);
extern unsigned long long rsysreg(enum sysreg);
extern void barrier(enum barrier_type type);
+extern void invdcachesetway(void *addr);
+extern void invdcache(void);
+extern void invicache(void);
diff --git a/arch/arm64/arch.s b/arch/arm64/arch.s
@@ -4,6 +4,8 @@
.globl panic,dohalt,enaint,dopanic
.globl barrier,vectbl,doswtch,inm8,inm16,inm32
.globl outm8,outm16,outm32
+ .globl invdcachesetway,invicache,vectbl,doswtch
+ .globl inm8,inm16,inm32,outm8,outm16,outm32
enaint:
msr daifclr,#15
@@ -21,6 +23,14 @@ barrier:
dsb sy
ret
+invdcachesetway:
+ dc isw,x0
+ ret
+
+invicache:
+ ic iallu
+ ret
+
badinst:
adr x0,badimsg
b panic
diff --git a/arch/arm64/cache.c b/arch/arm64/cache.c
@@ -0,0 +1,89 @@
+#include <rcode.h>
+
+#include "sysreg.h"
+#include "arch.h"
+
+#define NR_LEVELS 7
+
+/* clidr_el1 */
+#define LOC_SHIFT 24
+#define LOC_MASK 0x7 /* level of coherence */
+#define CTYPE_MASK 0x7 /* cache type */
+
+/* ccsidr_el1 */
+#define NSETS_SHIFT 13
+#define NSETS_MASK 0x7fff /* (number of sets in cache) - 1 */
+#define NWAYS_SHIFT 3
+#define NWAYS_MASK 0x3ff /* (associativty of cache) - 1 */
+#define LINE_MASK 0x7 /* log2(cache line in bytes) - 4 */
+
+/* csselr_el1 */
+#define LEVEL_SHIFT 1 /* cache level required */
+
+static unsigned
+log2(unsigned long long v, unsigned width)
+{
+ unsigned nbits, i;
+
+ for (nbits = 0, i = width - 1; i >= 0; i--) {
+ if ((v & (1ull << i)) != 0)
+ break;
+ nbits++;
+ }
+ return width - nbits - 1;
+}
+
+static void
+invdcachelvl(unsigned l)
+{
+ unsigned long long ccsidr;
+ unsigned nsets, nways, size, shift, i, j;
+
+ wsysreg(CSSELR_EL1, l << LEVEL_SHIFT);
+ barrier(ISB);
+
+ ccsidr = rsysreg(CCSIDR_EL1);
+ nsets = ((ccsidr >> NSETS_SHIFT) & NSETS_MASK) + 1;
+ nways = ((ccsidr >> NWAYS_SHIFT) & NWAYS_MASK) + 1;
+ size = (ccsidr & LINE_MASK) + 4;
+ shift = 32 - log2(nways, 32);
+
+ for (i = 0; i < nways; i++) {
+ for (j = 0; j < nsets; j++) {
+ uintptr_t addr;
+
+ addr = (uintptr_t)i << shift | j << size | l << 1;
+ invdcachesetway((void *)addr);
+ }
+ }
+ barrier(DSB_SY);
+}
+
+void
+invdcache(void)
+{
+ unsigned long long clidr;
+ unsigned l;
+ enum cachetype {
+ NOCACHE,
+ ICACHE,
+ DCACHE,
+ HARVARD,
+ UNIFIED
+ } type;
+
+ barrier(DSB_SY);
+
+ clidr = rsysreg(CLIDR_EL1);
+ if (((clidr >> LOC_SHIFT) & LOC_MASK) == 0)
+ return;
+
+ for (l = 0; l < NR_LEVELS; l++) {
+ type = clidr & CTYPE_MASK;
+ if (type == NOCACHE || type == ICACHE)
+ continue;
+
+ invdcachelvl(l);
+ clidr >>= 3;
+ }
+}
diff --git a/arch/arm64/rom-rmode.c b/arch/arm64/rom-rmode.c
@@ -22,14 +22,18 @@ main(void *text, void *ram, size_t ramsiz)
bss->backtrace = 1;
bss->dumpstack = 1;
bss->uartbase = (void *)UARTBASE;
- uartinit(UARTCLK, UARTBAUDRATE);
- enaint();
- barrier(ISB);
frame.sp = bp + ramsiz;
frame.elr = (void *) rsysreg(RVBAR_EL3);
frame.spsr = 0xf << 6 | 0xd;
+ uartinit(UARTCLK, UARTBAUDRATE);
+ enaint();
+ barrier(ISB);
+
+ invicache();
+ invdcache();
+
dbg("romfw: text = %p, ram = %p, ramsiz = 0x%llx\n",
text, ram, (unsigned long long)ramsiz);
diff --git a/arch/arm64/sysreg.lst b/arch/arm64/sysreg.lst
@@ -4,3 +4,6 @@ RCR_R S3_6_C1_C1_6
RDSCR_R S3_6_C6_C15_6
SCTLR_R S3_6_C1_C0_6
RVBAR_EL3 S3_6_C12_C0_1
+CLIDR_EL1 S3_1_C0_C0_1
+CCSIDR_EL1 S3_1_C0_C0_0
+CSSELR_EL1 S3_2_C0_C0_0
diff --git a/include/rcode.h b/include/rcode.h
@@ -158,7 +158,7 @@ extern uint16_t outm16(uint16_t, void *addr);
extern uint32_t outm32(uint32_t, void *addr);
/* global constant variables */
-extern const struct rowidx rowidx[];
+extern const struct rowidx rowidx[];
extern const void *const handler[];
extern const char *const ecstr[];
