diff --git a/rtos/rtx/TARGET_CORTEX_A/rt_CMSIS.c b/rtos/rtx/TARGET_CORTEX_A/rt_CMSIS.c index 51093fa8af..8e6b80a20a 100644 --- a/rtos/rtx/TARGET_CORTEX_A/rt_CMSIS.c +++ b/rtos/rtx/TARGET_CORTEX_A/rt_CMSIS.c @@ -543,6 +543,11 @@ static __inline char __get_mode(void) { } static __inline char __exceptional_mode(void) { + // Interrupts disabled + if (__get_CPSR() & 0x80) { + return 1; + } + switch(__get_mode()) { case MODE_USR: case MODE_SYS: diff --git a/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c b/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c index bd24bca59e..e5ebbbf238 100644 --- a/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c +++ b/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c @@ -552,7 +552,7 @@ uint32_t svcKernelSysTick (void) { /// Initialize the RTOS Kernel for creating objects osStatus osKernelInitialize (void) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } if ((__get_CONTROL() & 1U) == 0U) { // Privileged mode @@ -566,7 +566,7 @@ osStatus osKernelInitialize (void) { osStatus osKernelStart (void) { uint32_t stack[8]; - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } @@ -607,7 +607,7 @@ osStatus osKernelStart (void) { /// Check if the RTOS kernel is already started int32_t osKernelRunning (void) { - if ((__get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { + if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged return (int32_t)os_running; } else { @@ -617,7 +617,7 @@ int32_t osKernelRunning (void) { /// Get the RTOS kernel system timer counter uint32_t osKernelSysTick (void) { - if (__get_IPSR() != 0U) { return 0U; } // Not allowed in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return 0U; } // Not allowed in ISR return __svcKernelSysTick(); } @@ -867,7 +867,7 @@ osThreadId osThreadCreate (const osThreadDef_t *thread_def, void *argument) { return osThreadContextCreate(thread_def, argument, NULL); } osThreadId osThreadContextCreate (const osThreadDef_t *thread_def, void *argument, void *context) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -885,7 +885,7 @@ osThreadId osThreadContextCreate (const osThreadDef_t *thread_def, void *argumen /// Return the thread ID of the current running thread osThreadId osThreadGetId (void) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } return __svcThreadGetId(); @@ -894,7 +894,7 @@ osThreadId osThreadGetId (void) { /// Terminate execution of a thread and remove it from ActiveThreads osStatus osThreadTerminate (osThreadId thread_id) { osStatus status; - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } osMutexWait(osMutexId_osThreadMutex, osWaitForever); @@ -907,7 +907,7 @@ osStatus osThreadTerminate (osThreadId thread_id) { /// Pass control to next thread that is in state READY osStatus osThreadYield (void) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcThreadYield(); @@ -915,7 +915,7 @@ osStatus osThreadYield (void) { /// Change priority of an active thread osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcThreadSetPriority(thread_id, priority); @@ -923,7 +923,7 @@ osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority) { /// Get current priority of an active thread osPriority osThreadGetPriority (osThreadId thread_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osPriorityError; // Not allowed in ISR } return __svcThreadGetPriority(thread_id); @@ -948,7 +948,7 @@ __NO_RETURN void osThreadExit (void) { uint8_t osThreadGetState (osThreadId thread_id) { P_TCB ptcb; - if (__get_IPSR() != 0U) return osErrorISR; // Not allowed in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) return osErrorISR; // Not allowed in ISR ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer if (ptcb == NULL) return INACTIVE; @@ -1040,7 +1040,7 @@ os_InRegs osEvent_type svcWait (uint32_t millisec) { /// Wait for Timeout (Time Delay) osStatus osDelay (uint32_t millisec) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcDelay(millisec); @@ -1054,7 +1054,7 @@ os_InRegs osEvent osWait (uint32_t millisec) { ret.status = osErrorOS; return ret; #else - if (__get_IPSR() != 0U) { // Not allowed in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // Not allowed in ISR ret.status = osErrorISR; return ret; } @@ -1337,7 +1337,7 @@ void sysUserTimerUpdate (uint32_t sleep_time) { /// Create timer osTimerId osTimerCreate (const osTimerDef_t *timer_def, os_timer_type type, void *argument) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -1350,7 +1350,7 @@ osTimerId osTimerCreate (const osTimerDef_t *timer_def, os_timer_type type, void /// Start or restart timer osStatus osTimerStart (osTimerId timer_id, uint32_t millisec) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcTimerStart(timer_id, millisec); @@ -1358,7 +1358,7 @@ osStatus osTimerStart (osTimerId timer_id, uint32_t millisec) { /// Stop timer osStatus osTimerStop (osTimerId timer_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcTimerStop(timer_id); @@ -1366,7 +1366,7 @@ osStatus osTimerStop (osTimerId timer_id) { /// Delete timer osStatus osTimerDelete (osTimerId timer_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcTimerDelete(timer_id); @@ -1503,7 +1503,7 @@ int32_t isrSignalSet (osThreadId thread_id, int32_t signals) { /// Set the specified Signal Flags of an active thread int32_t osSignalSet (osThreadId thread_id, int32_t signals) { - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR return isrSignalSet(thread_id, signals); } else { // in Thread return __svcSignalSet(thread_id, signals); @@ -1512,7 +1512,7 @@ int32_t osSignalSet (osThreadId thread_id, int32_t signals) { /// Clear the specified Signal Flags of an active thread int32_t osSignalClear (osThreadId thread_id, int32_t signals) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return (int32_t)0x80000000U; // Not allowed in ISR } return __svcSignalClear(thread_id, signals); @@ -1522,7 +1522,7 @@ int32_t osSignalClear (osThreadId thread_id, int32_t signals) { os_InRegs osEvent osSignalWait (int32_t signals, uint32_t millisec) { osEvent ret; - if (__get_IPSR() != 0U) { // Not allowed in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // Not allowed in ISR ret.status = osErrorISR; return ret; } @@ -1634,7 +1634,7 @@ osStatus svcMutexDelete (osMutexId mutex_id) { /// Create and Initialize a Mutex object osMutexId osMutexCreate (const osMutexDef_t *mutex_def) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -1647,7 +1647,7 @@ osMutexId osMutexCreate (const osMutexDef_t *mutex_def) { /// Wait until a Mutex becomes available osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcMutexWait(mutex_id, millisec); @@ -1655,7 +1655,7 @@ osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec) { /// Release a Mutex that was obtained with osMutexWait osStatus osMutexRelease (osMutexId mutex_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcMutexRelease(mutex_id); @@ -1663,7 +1663,7 @@ osStatus osMutexRelease (osMutexId mutex_id) { /// Delete a Mutex that was created by osMutexCreate osStatus osMutexDelete (osMutexId mutex_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcMutexDelete(mutex_id); @@ -1801,7 +1801,7 @@ osStatus isrSemaphoreRelease (osSemaphoreId semaphore_id) { /// Create and Initialize a Semaphore object osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t count) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -1814,7 +1814,7 @@ osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t /// Wait until a Semaphore becomes available int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return -1; // Not allowed in ISR } return __svcSemaphoreWait(semaphore_id, millisec); @@ -1822,16 +1822,16 @@ int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) { /// Release a Semaphore osStatus osSemaphoreRelease (osSemaphoreId semaphore_id) { - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR return isrSemaphoreRelease(semaphore_id); - } else { // in Thread + } else { // in Thread return __svcSemaphoreRelease(semaphore_id); } } /// Delete a Semaphore that was created by osSemaphoreCreate osStatus osSemaphoreDelete (osSemaphoreId semaphore_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return osErrorISR; // Not allowed in ISR } return __svcSemaphoreDelete(semaphore_id); @@ -1914,7 +1914,7 @@ osStatus sysPoolFree (osPoolId pool_id, void *block) { /// Create and Initialize memory pool osPoolId osPoolCreate (const osPoolDef_t *pool_def) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -1927,7 +1927,7 @@ osPoolId osPoolCreate (const osPoolDef_t *pool_def) { /// Allocate a memory block from a memory pool void *osPoolAlloc (osPoolId pool_id) { - if ((__get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged + if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged return sysPoolAlloc(pool_id); } else { // in Thread return __sysPoolAlloc(pool_id); @@ -1938,7 +1938,7 @@ void *osPoolAlloc (osPoolId pool_id) { void *osPoolCAlloc (osPoolId pool_id) { void *mem; - if ((__get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged + if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged mem = sysPoolAlloc(pool_id); } else { // in Thread mem = __sysPoolAlloc(pool_id); @@ -1951,7 +1951,7 @@ void *osPoolCAlloc (osPoolId pool_id) { /// Return an allocated memory block back to a specific memory pool osStatus osPoolFree (osPoolId pool_id, void *block) { - if ((__get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged + if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged return sysPoolFree(pool_id, block); } else { // in Thread return __sysPoolFree(pool_id, block); @@ -2091,7 +2091,7 @@ os_InRegs osEvent isrMessageGet (osMessageQId queue_id, uint32_t millisec) { /// Create and Initialize Message Queue osMessageQId osMessageCreate (const osMessageQDef_t *queue_def, osThreadId thread_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -2104,7 +2104,7 @@ osMessageQId osMessageCreate (const osMessageQDef_t *queue_def, osThreadId threa /// Put a Message to a Queue osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) { - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR return isrMessagePut(queue_id, info, millisec); } else { // in Thread return __svcMessagePut(queue_id, info, millisec); @@ -2113,7 +2113,7 @@ osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) /// Get a Message or Wait for a Message from a Queue os_InRegs osEvent osMessageGet (osMessageQId queue_id, uint32_t millisec) { - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR return isrMessageGet(queue_id, millisec); } else { // in Thread return __svcMessageGet(queue_id, millisec); @@ -2250,7 +2250,7 @@ osStatus sysMailFree (osMailQId queue_id, void *mail, uint32_t isr) { /// Create and Initialize mail queue osMailQId osMailCreate (const osMailQDef_t *queue_def, osThreadId thread_id) { - if (__get_IPSR() != 0U) { + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return NULL; // Not allowed in ISR } if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) { @@ -2263,7 +2263,7 @@ osMailQId osMailCreate (const osMailQDef_t *queue_def, osThreadId thread_id) { /// Allocate a memory block from a mail void *osMailAlloc (osMailQId queue_id, uint32_t millisec) { - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR return sysMailAlloc(queue_id, millisec, 1U); } else { // in Thread return __sysMailAlloc(queue_id, millisec, 0U); @@ -2275,7 +2275,7 @@ void *osMailCAlloc (osMailQId queue_id, uint32_t millisec) { void *pool; void *mem; - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR mem = sysMailAlloc(queue_id, millisec, 1U); } else { // in Thread mem = __sysMailAlloc(queue_id, millisec, 0U); @@ -2290,7 +2290,7 @@ void *osMailCAlloc (osMailQId queue_id, uint32_t millisec) { /// Free a memory block from a mail osStatus osMailFree (osMailQId queue_id, void *mail) { - if (__get_IPSR() != 0U) { // in ISR + if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR return sysMailFree(queue_id, mail, 1U); } else { // in Thread return __sysMailFree(queue_id, mail, 0U); diff --git a/rtos/rtx/TARGET_CORTEX_M/rt_HAL_CM.h b/rtos/rtx/TARGET_CORTEX_M/rt_HAL_CM.h index 29f8c84554..129c4bbc6d 100644 --- a/rtos/rtx/TARGET_CORTEX_M/rt_HAL_CM.h +++ b/rtos/rtx/TARGET_CORTEX_M/rt_HAL_CM.h @@ -53,11 +53,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 +85,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 +117,7 @@ __attribute__((always_inline)) static inline void __DMB(void) __attribute__(( always_inline)) static inline U8 __clz(U32 value) { U8 result; - + __asm volatile ("clz %0, %1" : "=r" (result) : "r" (value)); return(result); } @@ -122,6 +138,14 @@ __attribute__(( always_inline)) static inline U8 __clz(U32 value) #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 +227,22 @@ extern BIT dbg_msg; #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 +258,7 @@ __inline static U32 rt_inc_qi (U32 size, U8 *count, U8 *first) { 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 +266,9 @@ __inline static U32 rt_inc_qi (U32 size, U8 *count, U8 *first) { if (c2 == size) { c2 = 0U; } *first = (U8)c2; } - __enable_irq (); + if (!primask) { + __enable_irq (); + } #endif return (cnt); }