diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 939c33c..cce951a 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -22,7 +22,7 @@
                 "USE_HAL_DRIVER",
                 "STM32F407xx"
             ],
-            "compilerPath": "D:\\mingw64\\bin\\gcc.exe",
+            "compilerPath": "D:\\gcc-arm-none-eabi\\10 2020-q4-major\\bin\\arm-none-eabi-gcc.exe",
             "cStandard": "gnu17",
             "cppStandard": "gnu++14"
         }
diff --git a/APP/mb_m_task.c b/APP/mb_m_task.c
index 6c94a16..52e6d95 100644
--- a/APP/mb_m_task.c
+++ b/APP/mb_m_task.c
@@ -11,6 +11,8 @@ void Mb_m_Task(void *argument)
     mbMasterStack.hardware.max485.dirPin = USART2_DIR_Pin;
     mbMasterStack.hardware.max485.dirPort = USART2_DIR_GPIO_Port;
     mbMasterStack.hardware.phtim = &htim3;
+    mbMasterStack.hardware.uartIRQn = USART2_IRQn;
+    mbMasterStack.hardware.timIRQn = TIM3_IRQn;
     eMBMasterInit(&mbMasterStack, MB_RTU, 2, 115200, MB_PAR_NONE);
     eMBMasterEnable(&mbMasterStack);
     while (1)
diff --git a/APP/mb_task.c b/APP/mb_task.c
index fdc1c55..eed7379 100644
--- a/APP/mb_task.c
+++ b/APP/mb_task.c
@@ -10,6 +10,8 @@ void Mb_Task(void *argument)
     mbStack.hardware.max485.dirPin = USART1_DIR_Pin;
     mbStack.hardware.max485.dirPort = USART1_DIR_GPIO_Port;
     mbStack.hardware.phtim = &htim4;
+    mbStack.hardware.uartIRQn = USART1_IRQn;
+    mbStack.hardware.timIRQn = TIM4_IRQn;
     eMBInit(&mbStack, MB_RTU, 0x01, 1, 115200, MB_PAR_NONE);
     eMBEnable(&mbStack);
     while (1)
diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c
index dd7f421..7f8d2a6 100644
--- a/Core/Src/stm32f4xx_it.c
+++ b/Core/Src/stm32f4xx_it.c
@@ -204,6 +204,10 @@ void USART1_IRQHandler(void)
   uint32_t isrflags = READ_REG(huart1.Instance->SR);
   uint32_t cr1its = READ_REG(huart1.Instance->CR1);
 
+  uint32_t errorflags = 0x00U;
+  /* If no error occurs */
+  errorflags = (isrflags & (uint32_t)(USART_SR_PE | USART_SR_FE | USART_SR_ORE | USART_SR_NE));
+
   /* UART in mode Receiver -------------------------------------------------*/
   if (((isrflags & USART_SR_RXNE) != RESET) && ((cr1its & USART_CR1_RXNEIE) != RESET))
   {
@@ -213,8 +217,11 @@ void USART1_IRQHandler(void)
   {
     mbStack.peMBFrameCBTransmitterEmptyCur((void *)&mbStack);
   }
-  /* USER CODE END USART1_IRQn 0 */
-  HAL_UART_IRQHandler(&huart1);
+
+  if (errorflags == SET)
+  {
+    __HAL_UART_CLEAR_PEFLAG(&huart1);
+  }
   /* USER CODE BEGIN USART1_IRQn 1 */
 
   /* USER CODE END USART1_IRQn 1 */
@@ -230,6 +237,10 @@ void USART2_IRQHandler(void)
   uint32_t isrflags = READ_REG(huart2.Instance->SR);
   uint32_t cr1its = READ_REG(huart2.Instance->CR1);
 
+  uint32_t errorflags = 0x00U;
+  /* If no error occurs */
+  errorflags = (isrflags & (uint32_t)(USART_SR_PE | USART_SR_FE | USART_SR_ORE | USART_SR_NE));
+
   /* UART in mode Receiver -------------------------------------------------*/
   if (((isrflags & USART_SR_RXNE) != RESET) && ((cr1its & USART_CR1_RXNEIE) != RESET))
   {
@@ -239,9 +250,11 @@ void USART2_IRQHandler(void)
   {
     mbMasterStack.peMBMasterFrameCBTransmitterEmptyCur((void *)&mbMasterStack);
   }
-  
-  /* USER CODE END USART2_IRQn 0 */
-  HAL_UART_IRQHandler(&huart2);
+
+  if (errorflags == SET)
+  {
+    __HAL_UART_CLEAR_PEFLAG(&huart1);
+  }
   /* USER CODE BEGIN USART2_IRQn 1 */
 
   /* USER CODE END USART2_IRQn 1 */
diff --git a/FreeModbus/modbus/include/mb.h b/FreeModbus/modbus/include/mb.h
index 90c0878..2ac0c8a 100644
--- a/FreeModbus/modbus/include/mb.h
+++ b/FreeModbus/modbus/include/mb.h
@@ -242,29 +242,6 @@ eMBErrorCode    eMBSetSlaveID( UCHAR ucSlaveID, BOOL xIsRunning,
                                UCHAR const *pucAdditional,
                                USHORT usAdditionalLen );
 
-/*! \ingroup modbus
- * \brief Registers a callback handler for a given function code.
- *
- * This function registers a new callback handler for a given function code.
- * The callback handler supplied is responsible for interpreting the Modbus PDU and
- * the creation of an appropriate response. In case of an error it should return
- * one of the possible Modbus exceptions which results in a Modbus exception frame
- * sent by the protocol stack.
- *
- * \param ucFunctionCode The Modbus function code for which this handler should
- *   be registers. Valid function codes are in the range 1 to 127.
- * \param pxHandler The function handler which should be called in case
- *   such a frame is received. If \c NULL a previously registered function handler
- *   for this function code is removed.
- *
- * \return eMBErrorCode::MB_ENOERR if the handler has been installed. If no
- *   more resources are available it returns eMBErrorCode::MB_ENORES. In this
- *   case the values in mbconfig.h should be adjusted. If the argument was not
- *   valid it returns eMBErrorCode::MB_EINVAL.
- */
-eMBErrorCode    eMBRegisterCB( UCHAR ucFunctionCode,
-                               pxMBFunctionHandler pxHandler );
-
 /* ----------------------- Callback -----------------------------------------*/
 
 /*! \defgroup modbus_registers Modbus Registers
diff --git a/FreeModbus/modbus/mb.c b/FreeModbus/modbus/mb.c
index 3101cb6..06afbfa 100644
--- a/FreeModbus/modbus/mb.c
+++ b/FreeModbus/modbus/mb.c
@@ -194,53 +194,6 @@ eMBTCPInit( USHORT ucTCPPort )
 }
 #endif
 
-eMBErrorCode
-eMBRegisterCB( UCHAR ucFunctionCode, pxMBFunctionHandler pxHandler )
-{
-    int             i;
-    eMBErrorCode    eStatus;
-
-    if( ( 0 < ucFunctionCode ) && ( ucFunctionCode <= 127 ) )
-    {
-        ENTER_CRITICAL_SECTION(  );
-        if( pxHandler != NULL )
-        {
-            for( i = 0; i < MB_FUNC_HANDLERS_MAX; i++ )
-            {
-                if( ( xFuncHandlers[i].pxHandler == NULL ) ||
-                    ( xFuncHandlers[i].pxHandler == pxHandler ) )
-                {
-                    xFuncHandlers[i].ucFunctionCode = ucFunctionCode;
-                    xFuncHandlers[i].pxHandler = pxHandler;
-                    break;
-                }
-            }
-            eStatus = ( i != MB_FUNC_HANDLERS_MAX ) ? MB_ENOERR : MB_ENORES;
-        }
-        else
-        {
-            for( i = 0; i < MB_FUNC_HANDLERS_MAX; i++ )
-            {
-                if( xFuncHandlers[i].ucFunctionCode == ucFunctionCode )
-                {
-                    xFuncHandlers[i].ucFunctionCode = 0;
-                    xFuncHandlers[i].pxHandler = NULL;
-                    break;
-                }
-            }
-            /* Remove can't fail. */
-            eStatus = MB_ENOERR;
-        }
-        EXIT_CRITICAL_SECTION(  );
-    }
-    else
-    {
-        eStatus = MB_EINVAL;
-    }
-    return eStatus;
-}
-
-
 eMBErrorCode
 eMBClose( void * this )
 {
diff --git a/FreeModbus/modbus/rtu/mbrtu.c b/FreeModbus/modbus/rtu/mbrtu.c
index ce01516..76a77c8 100644
--- a/FreeModbus/modbus/rtu/mbrtu.c
+++ b/FreeModbus/modbus/rtu/mbrtu.c
@@ -64,7 +64,7 @@ eMBRTUInit( void * this, UCHAR ucSlaveAddress, UCHAR ucPort, ULONG ulBaudRate, e
     pMB_StackTypeDef p = (pMB_StackTypeDef)this;
 
     ( void )ucSlaveAddress;
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
 
     /* Modbus RTU uses 8 Databits. */
     if( xMBPortSerialInit( (void *)&(p->hardware.max485), ucPort, ulBaudRate, 8, eParity ) != TRUE )
@@ -97,7 +97,7 @@ eMBRTUInit( void * this, UCHAR ucSlaveAddress, UCHAR ucPort, ULONG ulBaudRate, e
             eStatus = MB_EPORTERR;
         }
     }
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
 
     return eStatus;
 }
@@ -106,7 +106,7 @@ void
 eMBRTUStart( void * this )
 {
     pMB_StackTypeDef p = (pMB_StackTypeDef)this;
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
     /* Initially the receiver is in the state STATE_RX_INIT. we start
      * the timer and if no character is received within t3.5 we change
      * to STATE_RX_IDLE. This makes sure that we delay startup of the
@@ -116,17 +116,17 @@ eMBRTUStart( void * this )
     vMBPortSerialEnable( (void *)&(p->hardware.max485), TRUE, FALSE );
     vMBPortTimersEnable( p->hardware.phtim );
 
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
 }
 
 void
 eMBRTUStop( void * this )
 {
     pMB_StackTypeDef p = (pMB_StackTypeDef)this;
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
     vMBPortSerialEnable( (void *)&(p->hardware.max485), FALSE, FALSE );
     vMBPortTimersDisable( p->hardware.phtim );
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
 }
 
 eMBErrorCode
@@ -135,7 +135,7 @@ eMBRTUReceive( void * this, UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * p
     eMBErrorCode    eStatus = MB_ENOERR;
     pMB_StackTypeDef p = (pMB_StackTypeDef)this;
 
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
     assert_param( p->usRcvBufferPos < MB_SER_PDU_SIZE_MAX );
 
     /* Length and CRC check */
@@ -160,7 +160,7 @@ eMBRTUReceive( void * this, UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * p
         eStatus = MB_EIO;
     }
 
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
     return eStatus;
 }
 
@@ -171,7 +171,7 @@ eMBRTUSend( void * this, UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT us
     USHORT          usCRC16;
     pMB_StackTypeDef p = (pMB_StackTypeDef)this;
 
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
 
     /* Check if the receiver is still in idle state. If not we where to
      * slow with processing the received frame and the master sent another
@@ -200,7 +200,7 @@ eMBRTUSend( void * this, UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT us
     {
         eStatus = MB_EIO;
     }
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
     return eStatus;
 }
 
diff --git a/FreeModbus/modbus/rtu/mbrtu_m.c b/FreeModbus/modbus/rtu/mbrtu_m.c
index 5efebda..901811d 100644
--- a/FreeModbus/modbus/rtu/mbrtu_m.c
+++ b/FreeModbus/modbus/rtu/mbrtu_m.c
@@ -61,7 +61,7 @@ eMBMasterRTUInit(void * this, UCHAR ucPort, ULONG ulBaudRate, eMBParity eParity
     ULONG           usTimerT35_50us;
     pMB_M_StackTypeDef p = (pMB_M_StackTypeDef)this;
 
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
 
     /* Modbus RTU uses 8 Databits. */
     if( xMBMasterPortSerialInit( (void *)&(p->hardware.max485), ucPort, ulBaudRate, 8, eParity ) != TRUE )
@@ -94,7 +94,7 @@ eMBMasterRTUInit(void * this, UCHAR ucPort, ULONG ulBaudRate, eMBParity eParity
             eStatus = MB_EPORTERR;
         }
     }
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
 
     return eStatus;
 }
@@ -103,7 +103,7 @@ void
 eMBMasterRTUStart( void * this )
 {
     pMB_M_StackTypeDef p = (pMB_M_StackTypeDef)this;
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
     /* Initially the receiver is in the state STATE_M_RX_INIT. we start
      * the timer and if no character is received within t3.5 we change
      * to STATE_M_RX_IDLE. This makes sure that we delay startup of the
@@ -113,17 +113,17 @@ eMBMasterRTUStart( void * this )
     vMBMasterPortSerialEnable( (void *)&(p->hardware.max485), TRUE, FALSE );
     vMBMasterPortTimersT35Enable( this );
 
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
 }
 
 void
 eMBMasterRTUStop( void * this )
 {
     pMB_M_StackTypeDef p = (pMB_M_StackTypeDef)this;
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
     vMBMasterPortSerialEnable( (void*)&(p->hardware.max485), FALSE, FALSE );
     vMBMasterPortTimersDisable( (void *)(p->hardware.phtim) );
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
 }
 
 eMBErrorCode
@@ -132,7 +132,7 @@ eMBMasterRTUReceive( void * this, UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHO
     eMBErrorCode    eStatus = MB_ENOERR;
     pMB_M_StackTypeDef p = (pMB_M_StackTypeDef)this;
 
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
     assert_param( p->usMasterRcvBufferPos < MB_SER_PDU_SIZE_MAX );
 
     /* Length and CRC check */
@@ -157,7 +157,7 @@ eMBMasterRTUReceive( void * this, UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHO
         eStatus = MB_EIO;
     }
 
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
     return eStatus;
 }
 
@@ -170,7 +170,7 @@ eMBMasterRTUSend( void * this, UCHAR ucSlaveAddress, const UCHAR * pucFrame, USH
 
     if ( ucSlaveAddress > MB_MASTER_TOTAL_SLAVE_NUM ) return MB_EINVAL;
 
-    ENTER_CRITICAL_SECTION(  );
+    ENTER_CRITICAL_SECTION( p );
 
     /* Check if the receiver is still in idle state. If not we where to
      * slow with processing the received frame and the master sent another
@@ -199,7 +199,7 @@ eMBMasterRTUSend( void * this, UCHAR ucSlaveAddress, const UCHAR * pucFrame, USH
     {
         eStatus = MB_EIO;
     }
-    EXIT_CRITICAL_SECTION(  );
+    EXIT_CRITICAL_SECTION( p );
     return eStatus;
 }
 
diff --git a/FreeModbus/port/port.h b/FreeModbus/port/port.h
index e3b0f5d..5c5e5f8 100644
--- a/FreeModbus/port/port.h
+++ b/FreeModbus/port/port.h
@@ -31,8 +31,19 @@
 
 #define INLINE inline
 
-#define ENTER_CRITICAL_SECTION() vPortEnterCritical()
-#define EXIT_CRITICAL_SECTION() vPortExitCritical()
+#define ENTER_CRITICAL_SECTION(x)                    \
+    do                                               \
+    {                                                \
+        HAL_NVIC_DisableIRQ((x)->hardware.uartIRQn); \
+        HAL_NVIC_DisableIRQ((x)->hardware.timIRQn);  \
+    } while (0)
+
+#define EXIT_CRITICAL_SECTION(x)                     \
+    do                                              \
+    {                                               \
+        HAL_NVIC_EnableIRQ((x)->hardware.uartIRQn); \
+        HAL_NVIC_EnableIRQ((x)->hardware.timIRQn);  \
+    } while (0)
 
 typedef uint8_t BOOL;
 
@@ -64,9 +75,8 @@ typedef struct
 {
     Max485TypeDef max485;
     TIM_HandleTypeDef *phtim;
+    IRQn_Type uartIRQn;
+    IRQn_Type timIRQn;
 } MB_RTU_Hardware, *pMB_RTU_Hardware;
 
-void vPortEnterCritical(void);
-void vPortExitCritical(void);
-
 #endif