git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@666 35acf78f-673a-0410-8e92-d51de3d6d3f4

2 files changed, 34 insertions, 24 deletions

diff --git a/ports/ARM7/chcore.h b/ports/ARM7/chcore.h
index 1e99f7e16..41efb7f7e 100644
--- a/ports/ARM7/chcore.h
+++ b/ports/ARM7/chcore.h
@@ -197,7 +197,10 @@ struct context {
  * Disables the IRQ sources and keeps the FIQ sources enabled.

  */

 #ifdef THUMB

-#define port_lock() _port_lock_thumb()

+//#define port_lock() _port_lock_thumb()

+#define port_lock() {                                                   \

+  asm volatile ("bl     _port_lock_thumb" : : : "r3", "lr");            \

+}

 #else /* THUMB */

 #define port_lock() asm volatile ("msr     CPSR_c, #0x9F")

 #endif /* !THUMB */

@@ -206,7 +209,10 @@ struct context {
  * Enables both the IRQ and FIQ sources.

  */

 #ifdef THUMB

-#define port_unlock() _port_unlock_thumb()

+//#define port_unlock() _port_unlock_thumb()

+#define port_unlock() {                                                 \

+  asm volatile ("bl     _port_unlock_thumb" : : : "r3", "lr");          \

+}

 #else /* THUMB */

 #define port_unlock() asm volatile ("msr     CPSR_c, #0x1F")

 #endif /* !THUMB */

@@ -227,7 +233,10 @@ struct context {
  *       LPC214x datasheet.

  */

 #ifdef THUMB

-#define port_disable() _port_disable_thumb()

+//#define port_disable() _port_disable_thumb()

+#define port_disable() {                                                \

+  asm volatile ("bl     _port_disable_thumb" : : : "r3", "lr");         \

+}

 #else /* THUMB */

 #define port_disable() {                                                \

   asm volatile ("mrs     r3, CPSR                       \n\t"           \

@@ -242,7 +251,9 @@ struct context {
  * Disables the IRQ sources and enables the FIQ sources.

  */

 #ifdef THUMB

-#define port_suspend() _port_suspend_thumb()

+#define port_suspend() {                                                \

+  asm volatile ("bl     _port_suspend_thumb" : : : "r3", "lr");         \

+}

 #else /* THUMB */

 #define port_suspend() asm volatile ("msr     CPSR_c, #0x9F")

 #endif /* !THUMB */

@@ -251,7 +262,9 @@ struct context {
  * Enables both the IRQ and FIQ sources.

  */

 #ifdef THUMB

-#define port_enable() _port_enable_thumb()

+#define port_enable() {                                                 \

+  asm volatile ("bl     _port_enable_thumb" : : : "r3", "lr");          \

+}

 #else /* THUMB */

 #define port_enable() asm volatile ("msr     CPSR_c, #0x1F")

 #endif /* !THUMB */

@@ -273,11 +286,6 @@ extern "C" {
   void port_puts(char *msg);

   void port_halt(void);

 #ifdef THUMB

-  void _port_lock_thumb(void);

-  void _port_unlock_thumb(void);

-  void _port_disable_thumb(void);

-  void _port_suspend_thumb(void);

-  void _port_enable_thumb(void);

   void _port_switch_thumb(Thread *otp, Thread *ntp);

 #else /* THUMB */

   void _port_switch_arm(Thread *otp, Thread *ntp);

diff --git a/ports/ARM7/chcoreasm.s b/ports/ARM7/chcoreasm.s
index adbe1c622..e8fd2881d 100644
--- a/ports/ARM7/chcoreasm.s
+++ b/ports/ARM7/chcoreasm.s
@@ -44,27 +44,28 @@
 .balign 16

 .code 16

 .thumb_func

-.global _port_disable_all_thumb

-_port_disable_all_thumb:

-        mov     r0, pc

-        bx      r0

+.global _port_disable_thumb

+_port_disable_thumb:

+        mov     r3, pc

+        bx      r3

 .code 32

-        mrs     r0, CPSR

-        orr     r0, #I_BIT

-        msr     CPSR_c, r0

-        orr     r0, #F_BIT

-        msr     CPSR_c, r0

+        mrs     r3, CPSR

+        orr     r3, #I_BIT

+        msr     CPSR_c, r3

+        orr     r3, #F_BIT

+        msr     CPSR_c, r3

         bx      lr

 

 .balign 16

 .code 16

 .thumb_func

 .global _port_suspend_thumb

-_port_disable_thumb:

+_port_suspend_thumb:

+.thumb_func

 .global _port_lock_thumb

 _port_lock_thumb:

-        mov     r0, pc

-        bx      r0

+        mov     r3, pc

+        bx      r3

 .code 32

         msr     CPSR_c, #MODE_SYS | I_BIT

         bx      lr

@@ -74,10 +75,11 @@ _port_lock_thumb:
 .thumb_func

 .global _port_enable_thumb

 _port_enable_thumb:

+.thumb_func

 .global _port_unlock_thumb

 _port_unlock_thumb:

-        mov     r0, pc

-        bx      r0

+        mov     r3, pc

+        bx      r3

 .code 32

         msr     CPSR_c, #MODE_SYS

         bx      lr