diff --git a/ChangeLog b/ChangeLog
index 75d0f7480..02a225834 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,9 @@
+2013-12-23  Vladimir Serbinenko  <phcoder@gmail.com>
+
+	Fix ARM cache maintainance.
+
+	More code was converted from ASM to C for easier handling.
+
 2013-12-22  Vladimir Serbinenko  <phcoder@gmail.com>
 
 	* grub-core/kern/arm/cache.c (grub_arm_disable_caches_mmu): Use v6
diff --git a/grub-core/kern/arm/cache.S b/grub-core/kern/arm/cache.S
index 3763b7800..354a069fe 100644
--- a/grub-core/kern/arm/cache.S
+++ b/grub-core/kern/arm/cache.S
@@ -37,20 +37,16 @@
  * Simple cache maintenance functions
  */
 
-dlinesz_addr:
-	.long EXT_C(grub_arch_cache_dlinesz)
-ilinesz_addr:
-	.long EXT_C(grub_arch_cache_ilinesz)
-
 @ r0 - *beg (inclusive)
 @ r1 - *end (exclusive)
-clean_dcache_range:
+@void grub_arm_clean_dcache_range (grub_addr_t start, grub_addr_t end, grub_addr_t dlinesz)
+#ifdef ARMV6
+FUNCTION(grub_arm_clean_dcache_range_armv6)
+#else
+FUNCTION(grub_arm_clean_dcache_range_armv7)
+#endif
+	DSB
 	@ Clean data cache for range to point-of-unification
-	ldr	r2, dlinesz_addr
-	ldr	r2, [r2]
-	sub	r3, r2, #1		@ align "beg" to start of line
-	mvn	r3, r3
-	and	r0, r0, r3
 1:	cmp	r0, r1
 	bge	2f
 #ifdef ARMV6
@@ -65,13 +61,12 @@ clean_dcache_range:
 
 @ r0 - *beg (inclusive)
 @ r1 - *end (exclusive)
-invalidate_icache_range:
+#ifdef ARMV6
+FUNCTION(grub_arm_invalidate_icache_range_armv6)
+#else
+FUNCTION(grub_arm_invalidate_icache_range_armv7)
+#endif
 	@ Invalidate instruction cache for range to point-of-unification
-	ldr	r2, ilinesz_addr
-	ldr	r2, [r2]
-	sub	r3, r2, #1		@ align "beg" to start of line
-	mvn	r3, r3
-	and	r0, r0, r3
 1:	cmp	r0, r1
 	bge	2f
 	mcr	p15, 0, r0, c7, c5, 1	@ ICIMVAU
@@ -83,21 +78,6 @@ invalidate_icache_range:
 	ISB
 	bx	lr
 
-@void grub_arch_sync_caches (void *address, grub_size_t len)
-#ifdef ARMV6
-FUNCTION(grub_arch_sync_caches_armv6)
-#else
-FUNCTION(grub_arch_sync_caches_armv7)
-#endif
-	DSB
-	add	r1, r0, r1
-	push	{r0-r2, lr}
-	bl	clean_dcache_range
-	pop	{r0, r1}
-	bl	invalidate_icache_range
-	pop	{r2, lr}
-	bx	lr
-
 #ifdef ARMV6
 FUNCTION(grub_arm_disable_caches_mmu_armv6)
 #else
diff --git a/grub-core/kern/arm/cache.c b/grub-core/kern/arm/cache.c
index 5775afab7..0c0cb8816 100644
--- a/grub-core/kern/arm/cache.c
+++ b/grub-core/kern/arm/cache.c
@@ -13,12 +13,19 @@ static enum
     ARCH_ARMV7
   } type = ARCH_UNKNOWN;
 
-grub_uint32_t grub_arch_cache_dlinesz;
-grub_uint32_t grub_arch_cache_ilinesz;
+static grub_uint32_t grub_arch_cache_dlinesz;
+static grub_uint32_t grub_arch_cache_ilinesz;
+static grub_uint32_t grub_arch_cache_max_linesz;
 
 /* Prototypes for asm functions.  */
-void grub_arch_sync_caches_armv6 (void *address, grub_size_t len);
-void grub_arch_sync_caches_armv7 (void *address, grub_size_t len);
+void grub_arm_clean_dcache_range_armv6 (grub_addr_t start, grub_addr_t end,
+					grub_addr_t dlinesz);
+void grub_arm_clean_dcache_range_armv7 (grub_addr_t start, grub_addr_t end,
+					grub_addr_t dlinesz);
+void grub_arm_invalidate_icache_range_armv6 (grub_addr_t start, grub_addr_t end,
+					     grub_addr_t dlinesz);
+void grub_arm_invalidate_icache_range_armv7 (grub_addr_t start, grub_addr_t end,
+					     grub_addr_t dlinesz);
 void grub_arm_disable_caches_mmu_armv6 (void);
 void grub_arm_disable_caches_mmu_armv7 (void);
 grub_uint32_t grub_arm_main_id (void);
@@ -82,20 +89,33 @@ probe_caches (void)
     default:
       grub_fatal ("Unsupported cache type 0x%x", cache_type);
     }
+  if (grub_arch_cache_dlinesz > grub_arch_cache_ilinesz)
+    grub_arch_cache_max_linesz = grub_arch_cache_dlinesz;
+  else
+    grub_arch_cache_max_linesz = grub_arch_cache_ilinesz;
 }
 
 void
 grub_arch_sync_caches (void *address, grub_size_t len)
 {
+  grub_addr_t start = (grub_addr_t) address;
+  grub_addr_t end = start + len;
+
   if (type == ARCH_UNKNOWN)
     probe_caches ();
+  start = ALIGN_DOWN (start, grub_arch_cache_max_linesz);
+  end = ALIGN_UP (end, grub_arch_cache_max_linesz);
   switch (type)
     {
     case ARCH_ARMV6:
-      grub_arch_sync_caches_armv6 (address, len);
+      grub_arm_clean_dcache_range_armv6 (start, end, grub_arch_cache_dlinesz);
+      grub_arm_invalidate_icache_range_armv6 (start, end,
+					      grub_arch_cache_ilinesz);
       break;
     case ARCH_ARMV7:
-      grub_arch_sync_caches_armv7 (address, len);
+      grub_arm_clean_dcache_range_armv7 (start, end, grub_arch_cache_dlinesz);
+      grub_arm_invalidate_icache_range_armv7 (start, end,
+					      grub_arch_cache_ilinesz);
       break;
       /* Nothing to do.  */
     case ARCH_ARMV5_WRITE_THROUGH: