From ad3714e925f5c6b4ae0e0d31c4bab7f8c5e659db Mon Sep 17 00:00:00 2001
From: Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>
Date: Fri, 10 Aug 2012 21:55:41 +0200
Subject: [PATCH] hal/arm: fix compilation for thumb

---
 include/asm-arm/atomic.h |    3 ++
 ksrc/arch/arm/switch.S   |   84 +++++++++++++++++++++++++++------------------
 2 files changed, 53 insertions(+), 34 deletions(-)

diff --git a/include/asm-arm/atomic.h b/include/asm-arm/atomic.h
index 5d55ae8..ae96847 100644
--- a/include/asm-arm/atomic.h
+++ b/include/asm-arm/atomic.h
@@ -183,6 +183,9 @@ xnarch_atomic_cmpxchg(xnarch_atomic_t *ptr,
 		"ldrex	%1, [%3]\n"
 		"mov	%0, #0\n"
 		"teq	%1, %4\n"
+#ifdef __thumb__
+		"it	eq\n"
+#endif
 		"strexeq %0, %5, [%3]\n"
 		    : "=&r" (res), "=&r" (curval), "+Qo" (ptr->counter)
 		    : "r" (&ptr->counter), "Ir" (oldval), "r" (newval)
diff --git a/ksrc/arch/arm/switch.S b/ksrc/arch/arm/switch.S
index 83f4746..95c355f 100644
--- a/ksrc/arch/arm/switch.S
+++ b/ksrc/arch/arm/switch.S
@@ -37,6 +37,11 @@
 	.endm
 #endif
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
+#define ARM(x...)	x
+#define THUMB(x...)
+#endif
+
 #if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 37)
 #ifdef CONFIG_MMU
 #define USE_DOMAINS
@@ -83,18 +88,19 @@
  * r0 = pointer to union vfp_state, r1 = fpexc
  */
 ENTRY(rthal_vfp_save)
-    VFPFSTMIA	r0, r2			@ save the working registers
-    VFPFMRX	r2, FPSCR		@ current status
-    tst		r1, #FPEXC_EX		@ is there additional state to save?
-    beq 1f
-    VFPFMRX	r3, FPINST		@ FPINST (only if FPEXC.EX is set)
-    tst		r1, #FPEXC_FP2V		@ is there an FPINST2 to read?
-    beq 1f
-    VFPFMRX	r12, FPINST2	@ FPINST2 if needed (and present)
+	VFPFSTMIA	r0, r2		@ save the working registers
+	VFPFMRX		r2, FPSCR	@ current status
+	tst		r1, #FPEXC_EX	@ is there additional state to save?
+	beq		1f
+	VFPFMRX		r3, FPINST	@ FPINST (only if FPEXC.EX is set)
+	tst		r1, #FPEXC_FP2V	@ is there an FPINST2 to read?
+	beq		1f
+	VFPFMRX		r12, FPINST2	@ FPINST2 if needed (and present)
 1:
-    stmia	r0, {r1, r2, r3, r12}	@ save FPEXC, FPSCR, FPINST, FPINST2
-    mov		pc, lr
-
+	stmia		r0, {r1, r2, r3, r12}	@ save FPEXC, FPSCR, FPINST, FPINST2
+	mov		pc, lr
+ENDPROC(rthal_vfp_save)
+	
 /* Copied from no_old_VFP_process in arch/arm/vfp/vfphw.S
  * r0 = pointer to union vfp_state
  * r1 = current cpu
@@ -102,20 +108,21 @@ ENTRY(rthal_vfp_save)
 ENTRY(rthal_vfp_load)
 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0) \
 	|| defined(CONFIG_VFP_3_2_BACKPORT)) && defined(CONFIG_SMP)
-    str 	r1, [r0, #VFP_CPU]
+	str		r1, [r0, #VFP_CPU]
 #endif
-    VFPFLDMIA	r0, r2			@ reload the working registers while
+	VFPFLDMIA	r0, r2		@ reload the working registers while
 					@ FPEXC is in a safe state
-    ldmia	r0, {r1, r2, r3, r12}	@ load FPEXC, FPSCR, FPINST, FPINST2
-    tst		r1, #FPEXC_EX		@ is there additional state to restore?
-    beq		1f
-    VFPFMXR	FPINST, r3		@ restore FPINST (only if FPEXC.EX is set)
-    tst		r1, #FPEXC_FP2V		@ is there an FPINST2 to write?
-    beq		1f
-    VFPFMXR	FPINST2, r12	@ FPINST2 if needed (and present)
+	ldmia		r0, {r1, r2, r3, r12}	@ load FPEXC, FPSCR, FPINST, FPINST2
+	tst		r1, #FPEXC_EX	@ is there additional state to restore?
+	beq		1f
+	VFPFMXR		FPINST, r3	@ restore FPINST (only if FPEXC.EX is set)
+	tst		r1, #FPEXC_FP2V	@ is there an FPINST2 to write?
+	beq		1f
+	VFPFMXR		FPINST2, r12	@ FPINST2 if needed (and present)
 1:
-    VFPFMXR	FPSCR, r2		@ restore status
-    mov		pc, lr
+	VFPFMXR		FPSCR, r2	@ restore status
+	mov		pc, lr
+ENDPROC(rthal_vfp_load)
 #endif
 
 /*
@@ -144,28 +151,37 @@ ENTRY(rthal_vfp_load)
  * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
  */
 ENTRY(rthal_thread_switch)
-    add     ip, r1, #TI_CPU_SAVE
-    ldr     r3, [r2, #TI_TP_VALUE]
-    stmia   ip!, {r4 - sl, fp, sp, lr}      @ Store most regs on stack
+	add     ip, r1, #TI_CPU_SAVE
+	ldr     r3, [r2, #TI_TP_VALUE]
+ ARM(	stmia	ip!, {r4 - sl, fp, sp, lr} )	@ Store most regs on stack
+ THUMB(	stmia	ip!, {r4 - sl, fp}	   )	@ Store most regs on stack
+ THUMB(	str	sp, [ip], #4		   )
+ THUMB(	str	lr, [ip], #4		   )
 #ifdef USE_DOMAINS
-    ldr     r6, [r2, #TI_CPU_DOMAIN]
+	ldr     r6, [r2, #TI_CPU_DOMAIN]
 #endif
 
-    clear_exclusive_monitor
-    set_tls r3, r4, r5
+	clear_exclusive_monitor
+	set_tls r3, r4, r5
 #ifdef USE_DOMAINS
-    mcr     p15, 0, r6, c3, c0, 0           @ Set domain register
+	mcr     p15, 0, r6, c3, c0, 0           @ Set domain register
 #endif
-    fpu_switch r4
-    add	    r4, r2, #TI_CPU_SAVE
-    ldmia   r4, {r4 - sl, fp, sp, pc}       @ Load all regs saved previously
+	fpu_switch r4
+ ARM(	add	r4, r2, #TI_CPU_SAVE	   )
+ ARM(	ldmia	r4, {r4 - sl, fp, sp, pc}  )	@ Load all regs saved previously
+ THUMB(	add	ip, r2, #TI_CPU_SAVE	   )
+ THUMB(	ldmia	ip!, {r4 - sl, fp}	   )	@ Load all regs saved previously
+ THUMB(	ldr	sp, [ip], #4		   )
+ THUMB(	ldr	pc, [ip]		   )
+ENDPROC(rthal_thread_switch)
 
 /*
  * r4 = xnarch_thread_trampoline
  * r5 = xnarchtcb_t *
  */
 ENTRY(rthal_thread_trampoline)
-    mov     r0, r5
-    mov     pc, r4
+	mov	r0, r5
+	mov	pc, r4
+ENDPROC(rthal_thread_trampoline)
 
 // vim: ts=4 et sw=4 sts=4
-- 
1.7.2.5