From 8a07b55a041c3dccbacdf4f6433e46fb124f4099 Mon Sep 17 00:00:00 2001
From: Leif Lindholm <leif.lindholm@linaro.org>
Date: Tue, 19 Nov 2013 10:05:23 +0000
Subject: [PATCH] arm64: cache maintenance code rework + bugfix

---
 grub-core/kern/arm64/cache.c       | 45 ++++++++++++------------------
 grub-core/kern/arm64/cache_flush.S | 30 ++++++--------------
 2 files changed, 27 insertions(+), 48 deletions(-)

diff --git a/grub-core/kern/arm64/cache.c b/grub-core/kern/arm64/cache.c
index c13ad2163..c4f02b868 100644
--- a/grub-core/kern/arm64/cache.c
+++ b/grub-core/kern/arm64/cache.c
@@ -19,11 +19,14 @@
 #include <grub/cache.h>
 #include <grub/misc.h>
 
-grub_int64_t grub_arch_cache_dlinesz;
-grub_int64_t grub_arch_cache_ilinesz;
+static grub_int64_t dlinesz;
+static grub_int64_t ilinesz;
 
 /* Prototypes for asm functions. */
-void grub_arch_sync_caches_real (grub_uint64_t address, grub_uint64_t end);
+void grub_arch_clean_dcache_range (grub_addr_t beg, grub_addr_t end,
+				   grub_uint64_t line_size);
+void grub_arch_invalidate_icache_range (grub_addr_t beg, grub_addr_t end,
+					grub_uint64_t line_size);
 
 static void
 probe_caches (void)
@@ -33,40 +36,28 @@ probe_caches (void)
   /* Read Cache Type Register */
   asm volatile ("mrs 	%0, ctr_el0": "=r"(cache_type));
 
-  grub_arch_cache_dlinesz = 8 << ((cache_type >> 16) & 0xf);
-  grub_arch_cache_ilinesz = 8 << (cache_type & 0xf);
+  dlinesz = 4 << ((cache_type >> 16) & 0xf);
+  ilinesz = 4 << (cache_type & 0xf);
 
-  grub_dprintf("cache", "D$ line size: %lld\n",
-	       (long long) grub_arch_cache_dlinesz);
-  grub_dprintf("cache", "I$ line size: %lld\n",
-	       (long long) grub_arch_cache_ilinesz);
+  grub_dprintf("cache", "D$ line size: %lld\n", (long long) dlinesz);
+  grub_dprintf("cache", "I$ line size: %lld\n", (long long) ilinesz);
 }
 
 void
 grub_arch_sync_caches (void *address, grub_size_t len)
 {
-  grub_uint64_t start, end;
+  grub_uint64_t start, end, max;
 
-  if (grub_arch_cache_dlinesz == 0)
+  if (dlinesz == 0)
     probe_caches();
-  if (grub_arch_cache_dlinesz == 0)
+  if (dlinesz == 0)
     grub_fatal ("Unknown cache line size!");
 
-  grub_dprintf("cache", "syncing caches for %p-%lx\n",
-	       address, (grub_addr_t) address + len);
+  max = dlinesz > ilinesz ? dlinesz : ilinesz;
 
-  /* Align here to both cache lines. Saves a tiny bit of asm complexity and
-     most of potential problems with different line sizes.  */
-  start = (grub_uint64_t) address;
-  end = (grub_uint64_t) address + len;
-  start = ALIGN_DOWN (start, grub_arch_cache_dlinesz);
-  start = ALIGN_DOWN (start, grub_arch_cache_ilinesz);
+  start = ALIGN_DOWN ((grub_uint64_t) address, max);
+  end = ALIGN_UP ((grub_uint64_t) address + len, max);
 
-  end = ALIGN_UP (end, grub_arch_cache_dlinesz);
-  end = ALIGN_UP (end, grub_arch_cache_ilinesz);
-
-  grub_dprintf("cache", "aligned to: %lx-%lx\n",
-	       start, end);
-
-  grub_arch_sync_caches_real (start, end);
+  grub_arch_clean_dcache_range (start, end, dlinesz);
+  grub_arch_invalidate_icache_range (start, end, ilinesz);
 }
diff --git a/grub-core/kern/arm64/cache_flush.S b/grub-core/kern/arm64/cache_flush.S
index 1a35120cf..589e8eb7a 100644
--- a/grub-core/kern/arm64/cache_flush.S
+++ b/grub-core/kern/arm64/cache_flush.S
@@ -25,26 +25,25 @@
  * Simple cache maintenance functions
  */
 
-// r0 - *beg (inclusive)
-// r1 - *end (exclusive)
-clean_dcache_range:
+// x0 - *beg (inclusive)
+// x1 - *end (exclusive)
+// x2 - line size
+FUNCTION(grub_arch_clean_dcache_range)
 	// Clean data cache for range to point-of-unification
-	ldr	x2, =EXT_C(grub_arch_cache_dlinesz)
-	ldr	x2, [x2]
 1:	cmp	x0, x1
 	bge	2f
 	dc	cvau, x0		// Clean Virtual Address to PoU
 	add	x0, x0, x2		// Next line
 	b	1b
 2:	dsb	ish
+	isb
 	ret
 
-// r0 - *beg (inclusive)
-// r1 - *end (exclusive)
-invalidate_icache_range:
+// x0 - *beg (inclusive)
+// x1 - *end (exclusive)
+// x2 - line size
+FUNCTION(grub_arch_invalidate_icache_range)
 	// Invalidate instruction cache for range to point-of-unification
-	ldr	x2, =EXT_C(grub_arch_cache_ilinesz)
-	ldr	x2, [x2]
 1:	cmp	x0, x1
 	bge	2f
 	ic	ivau, x0		// Invalidate Virtual Address to PoU
@@ -54,14 +53,3 @@ invalidate_icache_range:
 2:	dsb	ish
 	isb
 	ret
-
-// void grub_arch_sync_caches_real (void *address, grub_size_t len)
-FUNCTION(grub_arch_sync_caches_real)
-	dsb	ish
-	stp	x0, x30, [sp, #-16]!
-	stp	x0, x1, [sp, #-16]!
-	bl	clean_dcache_range
-	ldp	x0, x1, [sp], #16
-	bl	invalidate_icache_range
-	ldp	x0, x30, [sp], #16
-	ret