diff --git a/src/FreeRTOS/port_cmsis.c b/src/FreeRTOS/port_cmsis.c
index 2fbec43f..d8850b52 100644
--- a/src/FreeRTOS/port_cmsis.c
+++ b/src/FreeRTOS/port_cmsis.c
@@ -354,4 +354,24 @@ static void vPortEnableVFP( void )
         configASSERT( NVIC_GetPriorityGrouping() <= ulMaxPRIGROUPValue );
     }
 
+uint32_t ulSetInterruptMaskFromISR( void )
+{
+  __asm volatile (
+  " mrs r0, PRIMASK	\n"
+  " cpsid i			\n"
+  " bx lr				  "
+  ::: "memory"
+  );
+}
+/*-----------------------------------------------------------*/
+
+void vClearInterruptMaskFromISR( __attribute__( ( unused ) ) uint32_t ulMask )
+{
+  __asm volatile (
+  " msr PRIMASK, r0	\n"
+  " bx lr				  "
+  ::: "memory"
+  );
+}
+
 #endif /* configASSERT_DEFINED */
diff --git a/src/FreeRTOS/portmacro_cmsis.h b/src/FreeRTOS/portmacro_cmsis.h
index 0497538f..e6e09158 100644
--- a/src/FreeRTOS/portmacro_cmsis.h
+++ b/src/FreeRTOS/portmacro_cmsis.h
@@ -104,8 +104,10 @@ typedef unsigned long UBaseType_t;
 /* Critical section management. */
 extern void vPortEnterCritical( void );
 extern void vPortExitCritical( void );
-#define portSET_INTERRUPT_MASK_FROM_ISR()       ulPortRaiseBASEPRI()
-#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x)    vPortSetBASEPRI(x)
+extern uint32_t ulSetInterruptMaskFromISR( void ) __attribute__( ( naked ) );
+extern void vClearInterruptMaskFromISR( uint32_t ulMask )  __attribute__( ( naked ) );
+#define portSET_INTERRUPT_MASK_FROM_ISR()       ulSetInterruptMaskFromISR()
+#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x)    vClearInterruptMaskFromISR( x )
 #define portDISABLE_INTERRUPTS()                __asm volatile 	( " cpsid i " ::: "memory" )
 #define portENABLE_INTERRUPTS()                 __asm volatile 	( " cpsie i " ::: "memory" )
 #define portENTER_CRITICAL()                    vPortEnterCritical()