Diff: rtx/TARGET_CORTEX_M/rt_HAL_CM.h

Revision:

123:58563e6cba1e

Parent:

119:19af2d39a542

--- a/rtx/TARGET_CORTEX_M/rt_HAL_CM.h	Wed Nov 09 12:22:14 2016 -0600
+++ b/rtx/TARGET_CORTEX_M/rt_HAL_CM.h	Mon Nov 14 17:14:42 2016 -0600
@@ -1,3 +1,6 @@
+
+/** \addtogroup rtos */
+/** @{*/
 /*----------------------------------------------------------------------------
  *      CMSIS-RTOS  -  RTX
  *----------------------------------------------------------------------------
@@ -53,11 +56,19 @@
 #endif
 
 #ifndef __CMSIS_GENERIC
+
+__attribute__((always_inline)) static inline U32 __get_PRIMASK(void)
+{
+    register U32 primask __asm("primask");
+    return primask;
+}
+
 #define __DMB() do {\
                    __schedule_barrier();\
                    __dmb(0xF);\
                    __schedule_barrier();\
                 } while (0)
+
 #endif
 
 #elif defined (__GNUC__)        /* GNU Compiler */
@@ -77,6 +88,14 @@
 
 #ifndef __CMSIS_GENERIC
 
+__attribute__((always_inline)) static inline U32 __get_PRIMASK(void)
+{
+  U32 result;
+
+  __asm volatile ("mrs %0, primask" : "=r" (result));
+  return result;
+}
+
 __attribute__((always_inline)) static inline void __enable_irq(void)
 {
   __asm volatile ("cpsie i");
@@ -101,7 +120,7 @@
 __attribute__(( always_inline)) static inline U8 __clz(U32 value)
 {
   U8 result;
-  
+
   __asm volatile ("clz %0, %1" : "=r" (result) : "r" (value));
   return(result);
 }
@@ -122,6 +141,14 @@
 
 #ifndef __CMSIS_GENERIC
 
+static inline U32 __get_PRIMASK(void)
+{
+  U32 result;
+  
+  __asm volatile ("mrs %0, primask" : "=r" (result));
+  return result;
+}
+
 static inline void __enable_irq(void)
 {
   __asm volatile ("cpsie i");
@@ -203,8 +230,22 @@
  #define rt_inc(p)     while(__strex((__ldrex(p)+1U),p))
  #define rt_dec(p)     while(__strex((__ldrex(p)-1U),p))
 #else
- #define rt_inc(p)     __disable_irq();(*p)++;__enable_irq();
- #define rt_dec(p)     __disable_irq();(*p)--;__enable_irq();
+ #define rt_inc(p) do {\
+                     U32 primask = __get_PRIMASK();\
+                     __disable_irq();\
+                     (*p)++;\
+                     if (!primask) {\
+                       __enable_irq();\
+                     }\
+                   } while (0)
+ #define rt_dec(p) do {\
+                     U32 primask = __get_PRIMASK();\
+                     __disable_irq();\
+                     (*p)--;\
+                     if (!primask) {\
+                       __enable_irq();\
+                     }\
+                   } while (0)
 #endif
 
 __inline static U32 rt_inc_qi (U32 size, U8 *count, U8 *first) {
@@ -220,6 +261,7 @@
     if (c2 == size) { c2 = 0U; }
   } while (__strex(c2, first));
 #else
+  U32 primask = __get_PRIMASK();
   __disable_irq();
   if ((cnt = *count) < size) {
     *count = (U8)(cnt+1U);
@@ -227,7 +269,9 @@
     if (c2 == size) { c2 = 0U; }
     *first = (U8)c2; 
   }
-  __enable_irq ();
+  if (!primask) {
+    __enable_irq ();
+  }
 #endif
   return (cnt);
 }
@@ -296,3 +340,5 @@
 /*----------------------------------------------------------------------------
  * end of file
  *---------------------------------------------------------------------------*/
+
+/** @}*/