1 files changed, 22 insertions, 2 deletions

diff --git a/arch/arm/lib/copy_template.S b/arch/arm/lib/copy_template.S
index 139cce646055..805e3f8fb007 100644
--- a/arch/arm/lib/copy_template.S
+++ b/arch/arm/lib/copy_template.S
@@ -57,6 +57,13 @@
  *
  *      Restore registers with the values previously saved with the
  *      'preserv' macro. Called upon code termination.
+ *
+ * LDR1W_SHIFT
+ * STR1W_SHIFT
+ *
+ *      Correction to be applied to the "ip" register when branching into
+ *      the ldr1w or str1w instructions (some of these macros may expand to
+ *      than one 32bit instruction in Thumb-2)
  */
 
 
@@ -99,9 +106,15 @@
 
 5:              ands    ip, r2, #28
                 rsb     ip, ip, #32
+#if LDR1W_SHIFT > 0
+                lsl     ip, ip, #LDR1W_SHIFT
+#endif
                 addne   pc, pc, ip              @ C is always clear here
                 b       7f
-6:              nop
+6:
+                .rept   (1 << LDR1W_SHIFT)
+                W(nop)
+                .endr
                 ldr1w   r1, r3, abort=20f
                 ldr1w   r1, r4, abort=20f
                 ldr1w   r1, r5, abort=20f
@@ -110,9 +123,16 @@
                 ldr1w   r1, r8, abort=20f
                 ldr1w   r1, lr, abort=20f
 
+#if LDR1W_SHIFT < STR1W_SHIFT
+                lsl     ip, ip, #STR1W_SHIFT - LDR1W_SHIFT
+#elif LDR1W_SHIFT > STR1W_SHIFT
+                lsr     ip, ip, #LDR1W_SHIFT - STR1W_SHIFT
+#endif
                 add     pc, pc, ip
                 nop
-                nop
+                .rept   (1 << STR1W_SHIFT)
+                W(nop)
+                .endr
                 str1w   r0, r3, abort=20f
                 str1w   r0, r4, abort=20f
                 str1w   r0, r5, abort=20f