1 files changed, 62 insertions, 13 deletions

diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index 7abc881e45..5001d9bebb 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -19,6 +19,25 @@
 #include <rtems/asm.h>
 #include <rtems/score/cpu_asm.h>
 
+/*
+ *  function declaration macro (start body in ARM mode)
+ */
+#ifdef __thumb__
+  #define FUNC_START_ARM(_name_)              \
+    .code   16                              ;\
+    .thumb_func                             ;\
+    .globl _name_                           ;\
+_name_:                                         ;\
+     bx      pc                              ;\
+     .code   32                              ;\
+_name_ ## _ARM:
+#else
+  #define FUNC_START_ARM(_name_) \
+     .globl _name_; \
+_name_: 
+#endif
+
+        .text
 
 /*
  *  void _CPU_Context_switch( run_context, heir_context )
@@ -35,8 +54,8 @@
  *  a 16 bit data bus.
  *        
  */
-        .globl _CPU_Context_switch
-_CPU_Context_switch:
+ 
+FUNC_START_ARM(_CPU_Context_switch)
 /* Start saving context */
 	mrs	r2, cpsr
 	stmia	r0,  {r2, r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
@@ -46,8 +65,12 @@ _CPU_Context_switch:
 _restore:       
 	ldmia	r1,  {r2, r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
 	msr	cpsr, r2
+#ifdef __thumb__
+	bx	lr
+	nop
+#else
 	mov	pc, lr
-
+#endif
 /*
  *  void _CPU_Context_restore( new_context )
  *
@@ -55,16 +78,14 @@ _restore:
  *  It must match _CPU_Context_switch()
  *
  */
-        .globl _CPU_Context_restore
-_CPU_Context_restore:
+FUNC_START_ARM(_CPU_Context_restore)
         mov     r1, r0
         b       _restore
 
 
 
 /* FIXME:       _Exception_Handler_Undef_Swi is untested */
-        .globl _Exception_Handler_Undef_Swi
-_Exception_Handler_Undef_Swi:
+FUNC_START_ARM(_Exception_Handler_Undef_Swi)
 /* FIXME: This should use load and store multiple instructions */
 	sub     r13,r13,#SIZE_REGS
 	str     r4,  [r13, #REG_R4]
@@ -100,8 +121,7 @@ _go_back_1:
 	movs	pc,r14			/* return  */ 
 	
 /* FIXME:       _Exception_Handler_Abort is untested */
-        .globl _Exception_Handler_Abort
-_Exception_Handler_Abort:
+FUNC_START_ARM(_Exception_Handler_Abort)
 /* FIXME: This should use load and store multiple instructions */
 	sub     r13,r13,#SIZE_REGS
 	str     r4,  [r13, #REG_R4]
@@ -133,13 +153,18 @@ _go_back_2:
 	ldr     lr,  [r13, #REG_SP]
 	ldr     lr,  [r13, #REG_LR]
 	add     r13,r13,#SIZE_REGS
+#ifdef  __thumb__
+	subs	r11, r14,#4
+	bx	r11
+	nop
+#else
 	subs	pc,r14,#4			/* return */
+#endif
 
 #define ABORT_REGS_OFFS 32-REG_R4
 #define ABORT_SIZE_REGS SIZE_REGS+ABORT_REGS_OFFS
 	
-	.globl _exc_data_abort
-_exc_data_abort:
+FUNC_START_ARM(_exc_data_abort)
 	sub	sp, sp, #ABORT_SIZE_REGS	/* reserve register frame */
 	stmia	sp, {r0-r11}
 	add	sp, sp, #ABORT_REGS_OFFS	/* the Context_Control structure starts by CPSR, R4, ... */
@@ -151,7 +176,26 @@ _exc_data_abort:
 	ldr	r0, [r1, #-8]			/* r0 = bad instruction */
 	mrs	r1, spsr			/* r1 = spsr */
 	mov	r2, r13				/* r2 = exception frame of Context_Control type */
+#if defined(__thumb__)
+	.code 32
+	/*arm to thumb*/
+	adr	r5, to_thumb + 1
+	bx	r5
+	.code 16
+to_thumb:	
+#endif	
 	bl	do_data_abort
+#if defined(__thumb__)
+/*back to arm*/		
+	.code 16
+thumb_to_arm:
+	.align 2
+	adr r5, arm_code
+	bx	r5
+	nop
+	.code 32
+arm_code:
+#endif
 	
 	ldr	lr, [sp, #REG_LR]
 	ldr	ip, [sp, #REG_PC]		/* restore R12 (ip) */
@@ -159,6 +203,11 @@ _exc_data_abort:
 	sub	sp, sp, #ABORT_REGS_OFFS
 	ldmia	sp, {r0-r11}
 	add	sp, sp, #ABORT_SIZE_REGS
-
-	subs	pc, r14, #4			/* return to the instruction */
+#ifdef  __thumb__
+	subs	r11, r14, #4			/* return to the instruction */
+	bx      r11
+	nop
+#else
+	subs	pc, r14, #4
+#endif
 						/* _AFTER_ the aborted one */