diff --git a/ChangeLog b/ChangeLog
index 7e09c4d12..9b0a85937 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,10 @@
+2013-11-16  Vladimir Serbinenko  <phcoder@gmail.com>
+
+	* grub-core/kern/arm/cache.S: Don't switch back to ARM mode when
+	compiling to thumb2.
+	* grub-core/kern/arm/cache_armv7.S: Likewise.
+	* grub-core/lib/arm/setjmp.S: Likewise.
+
 2013-11-16  Leif Lindholm <leif.lindholm@linaro.org>
 
 	* grub-core/kern/arm/uboot/startup.S: delete superflouous save of r8
diff --git a/grub-core/kern/arm/cache.S b/grub-core/kern/arm/cache.S
index 8522d2470..2ad774d90 100644
--- a/grub-core/kern/arm/cache.S
+++ b/grub-core/kern/arm/cache.S
@@ -21,7 +21,12 @@
 	.file	"cache.S"
 	.text
 	.syntax	unified
+#if !defined (__thumb2__) || !defined (ARMV7)
 	.arm
+#else
+	.thumb
+#endif
+
 #if !defined (ARMV6) && !defined (ARMV7)
 # error Unsupported architecture version!
 #endif
diff --git a/grub-core/kern/arm/cache_armv7.S b/grub-core/kern/arm/cache_armv7.S
index 0c16b1047..aa42fad81 100644
--- a/grub-core/kern/arm/cache_armv7.S
+++ b/grub-core/kern/arm/cache_armv7.S
@@ -21,8 +21,13 @@
 	.file	"cache_armv7.S"
 	.text
 	.syntax	unified
-	.arm
+#if !defined (__thumb2__)
 	.arch	armv7a
+	.arm
+#else
+	.arch	armv7
+	.thumb
+#endif
 # define DMB	dmb
 # define DSB	dsb
 # define ISB	isb
@@ -58,11 +63,17 @@ clean_invalidate_dcache:
 	@ read current cache information
 	mrc	p15, 1, r8, c0, c0, 0	@ Read CCSIDR
 	lsr	r3, r8, #13		@ Number of sets -1
-	ldr	r9, =0x3fff
-	and	r3, r3, r9
+
+	@ Keep only 14 bits of r3
+	lsl     r3, r3, #18
+	lsr     r3, r3, #18
+
 	lsr	r4, r8, #3		@ Number of ways -1
-	ldr	r9, =0x1ff
-	and	r4, r4, r9
+
+	@ Keep only 9  bits of r4
+	lsl     r4, r4, #23
+	lsr     r4, r4, #23
+
 	and	r7, r8, #7		@ log2(line size in words) - 2
 	add	r7, r7, #2		@  adjust
 	mov	r8, #1
diff --git a/grub-core/lib/arm/setjmp.S b/grub-core/lib/arm/setjmp.S
index 4f1567946..a5373d3a9 100644
--- a/grub-core/lib/arm/setjmp.S
+++ b/grub-core/lib/arm/setjmp.S
@@ -24,7 +24,11 @@
 GRUB_MOD_LICENSE "GPLv3+"
 
 	.syntax	unified
+#if !defined (__thumb2__)
 	.arm
+#else
+	.thumb
+#endif
 
 	.text
 
@@ -32,7 +36,8 @@ GRUB_MOD_LICENSE "GPLv3+"
  * int grub_setjmp (grub_jmp_buf env)
  */
 FUNCTION(grub_setjmp)
-	stm	r0, { r4-r11, sp, lr }
+	mov	r12, sp
+	stm	r0, { r4-r12, lr }
 	mov	r0, #0
 	bx	lr
 
@@ -40,7 +45,9 @@ FUNCTION(grub_setjmp)
  * int grub_longjmp (grub_jmp_buf env, int val)
  */
 FUNCTION(grub_longjmp)
- 	ldm	r0, { r4-r11, sp, lr }
+	ldm	r0, { r4-r12, lr }
+	mov	sp, r12
 	movs	r0, r1
+	it	eq
 	moveq	r0, #1
 	bx	lr