diff --git a/components/freemodbus/Kconfig b/components/freemodbus/Kconfig
index 113202d12..cc908a5c3 100644
--- a/components/freemodbus/Kconfig
+++ b/components/freemodbus/Kconfig
@@ -113,7 +113,7 @@ menu "Modbus configuration"
         range 0 1
         default 0
         help
-            Modbus Timer group number that is used for timeout measurement.
+                Modbus Timer group number that is used for timeout measurement.
 
     config FMB_TIMER_INDEX
         int "Modbus Timer index in the group"
@@ -122,4 +122,14 @@ menu "Modbus configuration"
         help
                 Modbus Timer Index in the group that is used for timeout measurement.
 
+    config FMB_ISR_IN_IRAM
+        bool "Place interrupt handlers into IRAM"
+        default y
+        select UART_ISR_IN_IRAM
+        help
+                This option places Modbus IRQ handlers into IRAM.
+                This allows to avoid delays related to processing of non-IRAM-safe interrupts
+                during a flash write operation (NVS updating a value, or some other
+                flash API which has to perform an read/write operation and disable CPU cache).
+
 endmenu
diff --git a/components/freemodbus/common/esp_modbus_master.c b/components/freemodbus/common/esp_modbus_master.c
index 332783c03..0dafd8b96 100644
--- a/components/freemodbus/common/esp_modbus_master.c
+++ b/components/freemodbus/common/esp_modbus_master.c
@@ -45,7 +45,7 @@ esp_err_t mbc_master_init(mb_port_type_t port_type, void** handler)
     MB_MASTER_CHECK((port_handler != NULL),
                     ESP_ERR_INVALID_STATE,
                     "Master interface initialization failure, error=(0x%x), port type=(0x%x).",
-                    (uint16_t)error, (uint16_t)port_type);
+                    error, (uint16_t)port_type);
 
     if ((port_handler != NULL) && (error == ESP_OK)) {
         master_interface_ptr = (mb_master_interface_t*) port_handler;
@@ -106,9 +106,9 @@ esp_err_t mbc_master_get_parameter(uint16_t cid, char* name, uint8_t* value, uin
                     "Master interface is not correctly initialized.");
     error = master_interface_ptr->get_parameter(cid, name, value, type);
     MB_MASTER_CHECK((error == ESP_OK), 
-                    ESP_ERR_INVALID_STATE, 
-                    "SERIAL master get parameter failure error=(0x%x).", 
-                    (uint16_t)error);
+                    error,
+                    "SERIAL master get parameter failure error=(0x%x) (%s).",
+                    error, esp_err_to_name(error));
     return error;
 }
 
@@ -126,10 +126,10 @@ esp_err_t mbc_master_send_request(mb_param_request_t* request, void* data_ptr)
                     "Master interface is not correctly initialized.");
     error = master_interface_ptr->send_request(request, data_ptr);
     MB_MASTER_CHECK((error == ESP_OK), 
-                    ESP_ERR_INVALID_STATE, 
-                    "SERIAL master get parameter failure error=(0x%x).", 
-                    (uint16_t)error);
-    return error;
+                    error,
+                    "SERIAL master send request failure error=(0x%x) (%s).",
+                    error, esp_err_to_name(error));
+    return ESP_OK;
 }
 
 /**
@@ -146,10 +146,10 @@ esp_err_t mbc_master_set_descriptor(const mb_parameter_descriptor_t* descriptor,
                     "Master interface is not correctly initialized.");
     error = master_interface_ptr->set_descriptor(descriptor, num_elements);
     MB_MASTER_CHECK((error == ESP_OK), 
-                    ESP_ERR_INVALID_STATE, 
-                    "SERIAL master set descriptor failure error=(0x%x).", 
-                    (uint16_t)error);
-    return error;
+                    error,
+                    "SERIAL master set descriptor failure error=(0x%x) (%s).",
+                    error, esp_err_to_name(error));
+    return ESP_OK;
 }
 
 /**
@@ -166,10 +166,10 @@ esp_err_t mbc_master_set_parameter(uint16_t cid, char* name, uint8_t* value, uin
                     "Master interface is not correctly initialized.");
     error = master_interface_ptr->set_parameter(cid, name, value, type);
     MB_MASTER_CHECK((error == ESP_OK), 
-                    ESP_ERR_INVALID_STATE, 
-                    "SERIAL master set parameter failure error=(0x%x).", 
-                    (uint16_t)error);
-    return error;
+                    error,
+                    "SERIAL master set parameter failure error=(0x%x) (%s).",
+                    error, esp_err_to_name(error));
+    return ESP_OK;
 }
 
 /**
@@ -186,10 +186,10 @@ esp_err_t mbc_master_setup(void* comm_info)
                     "Master interface is not correctly initialized.");
     error = master_interface_ptr->setup(comm_info);
     MB_MASTER_CHECK((error == ESP_OK), 
-                    ESP_ERR_INVALID_STATE, 
-                    "SERIAL master setup failure error=(0x%x).", 
-                    (uint16_t)error);
-    return error;
+                    error,
+                    "SERIAL master setup failure error=(0x%x) (%s).",
+                    error, esp_err_to_name(error));
+    return ESP_OK;
 }
 
 /**
@@ -206,10 +206,10 @@ esp_err_t mbc_master_start(void)
                     "Master interface is not correctly initialized.");
     error = master_interface_ptr->start();
     MB_MASTER_CHECK((error == ESP_OK), 
-                    ESP_ERR_INVALID_STATE, 
-                    "SERIAL master start failure error=(0x%x).", 
-                    (uint16_t)error);
-    return error;
+                    error,
+                    "SERIAL master start failure error=(0x%x) (%s).",
+                    error, esp_err_to_name(error));
+    return ESP_OK;
 }
 
 eMBErrorCode eMBMasterRegDiscreteCB(UCHAR * pucRegBuffer, USHORT usAddress,
diff --git a/components/freemodbus/modbus/ascii/mbascii.c b/components/freemodbus/modbus/ascii/mbascii.c
index 3f0be31df..a8c6ac6af 100644
--- a/components/freemodbus/modbus/ascii/mbascii.c
+++ b/components/freemodbus/modbus/ascii/mbascii.c
@@ -113,7 +113,7 @@ eMBASCIIInit( UCHAR ucSlaveAddress, UCHAR ucPort, ULONG ulBaudRate, eMBParity eP
     ENTER_CRITICAL_SECTION(  );
     ucMBLFCharacter = MB_ASCII_DEFAULT_LF;
 
-    if( xMBPortSerialInit( ucPort, ulBaudRate, 7, eParity ) != TRUE )
+    if( xMBPortSerialInit( ucPort, ulBaudRate, 8, eParity ) != TRUE )
     {
         eStatus = MB_EPORTERR;
     }
@@ -388,27 +388,21 @@ xMBASCIITransmitFSM( void )
          * been sent. */
     case STATE_TX_NOTIFY:
         eSndState = STATE_TX_IDLE;
-        xNeedPoll = xMBPortEventPost( EV_FRAME_SENT );
+        xMBPortEventPost( EV_FRAME_SENT );
+        xNeedPoll = TRUE;
 
-        /* Disable transmitter. This prevents another transmit buffer
-         * empty interrupt. */
-        vMBPortSerialEnable( TRUE, FALSE );
-        eSndState = STATE_TX_IDLE;
         break;
 
         /* We should not get a transmitter event if the transmitter is in
          * idle state.  */
     case STATE_TX_IDLE:
-        /* enable receiver/disable transmitter. */
-        vMBPortSerialEnable( TRUE, FALSE );
         break;
     }
 
     return xNeedPoll;
 }
 
-BOOL
-xMBASCIITimerT1SExpired( void )
+BOOL MB_PORT_ISR_ATTR xMBASCIITimerT1SExpired( void )
 {
     switch ( eRcvState )
     {
@@ -421,7 +415,8 @@ xMBASCIITimerT1SExpired( void )
         break;
 
     default:
-        assert( ( eRcvState == STATE_RX_RCV ) || ( eRcvState == STATE_RX_WAIT_EOF ) );
+        assert( ( eRcvState == STATE_RX_RCV ) || ( eRcvState == STATE_RX_WAIT_EOF )
+                        || (eRcvState == STATE_RX_IDLE ));
         break;
     }
     vMBPortTimersDisable(  );
diff --git a/components/freemodbus/modbus/include/mbconfig.h b/components/freemodbus/modbus/include/mbconfig.h
index ce160d827..a20bcedc2 100644
--- a/components/freemodbus/modbus/include/mbconfig.h
+++ b/components/freemodbus/modbus/include/mbconfig.h
@@ -130,6 +130,9 @@ PR_BEGIN_EXTERN_C
 /*! \brief If the <em>Read/Write Multiple Registers</em> function should be enabled. */
 #define MB_FUNC_READWRITE_HOLDING_ENABLED       (  1 )
 
+/*! \brief Check the option to place handlers into IRAM */
+#define MB_ISR_IN_IRAM                          (  CONFIG_FMB_ISR_IN_IRAM )
+
 /*! @} */
 #ifdef __cplusplus
     PR_END_EXTERN_C
diff --git a/components/freemodbus/modbus/include/mbport.h b/components/freemodbus/modbus/include/mbport.h
index 6be73ad83..3a5a7a680 100644
--- a/components/freemodbus/modbus/include/mbport.h
+++ b/components/freemodbus/modbus/include/mbport.h
@@ -37,6 +37,20 @@
 PR_BEGIN_EXTERN_C
 #endif
 
+#if CONFIG_UART_ISR_IN_IRAM
+#define MB_PORT_SERIAL_ISR_FLAG ESP_INTR_FLAG_IRAM
+#else
+#define MB_PORT_SERIAL_ISR_FLAG ESP_INTR_FLAG_LOWMED
+#endif
+
+#if MB_ISR_IN_IRAM
+#define MB_PORT_ISR_ATTR IRAM_ATTR
+#define MB_PORT_TIMER_ISR_FLAG ESP_INTR_FLAG_IRAM
+#else
+#define MB_PORT_ISR_ATTR
+#define MB_PORT_TIMER_ISR_FLAG ESP_INTR_FLAG_LOWMED
+#endif
+
 /* ----------------------- Type definitions ---------------------------------*/
 
 typedef enum
@@ -47,7 +61,7 @@ typedef enum
     EV_FRAME_SENT = 0x08               /*!< Frame sent. */
 } eMBEventType;
 
-#if MB_MASTER_RTU_ENABLED > 0 || MB_MASTER_ASCII_ENABLED > 0
+#if MB_MASTER_RTU_ENABLED || MB_MASTER_ASCII_ENABLED
 typedef enum {
     EV_MASTER_NO_EVENT = 0x0000,
     EV_MASTER_READY = 0x0001,                   /*!< Startup finished. */
diff --git a/components/freemodbus/modbus/mb_m.c b/components/freemodbus/modbus/mb_m.c
index 3f2a97e4c..04c0efcfb 100644
--- a/components/freemodbus/modbus/mb_m.c
+++ b/components/freemodbus/modbus/mb_m.c
@@ -48,7 +48,7 @@
 #include "mbrtu.h"
 #endif
 #if MB_MASTER_ASCII_ENABLED == 1
-#include "mbascii.h"
+#include "mbascii_m.h"
 #endif
 #if MB_MASTER_TCP_ENABLED == 1
 #include "mbtcp.h"
@@ -56,7 +56,6 @@
 
 #if MB_MASTER_RTU_ENABLED > 0 || MB_MASTER_ASCII_ENABLED > 0
 
-
 #ifndef MB_PORT_HAS_CLOSE
 #define MB_PORT_HAS_CLOSE 1
 #endif
@@ -269,15 +268,13 @@ eMBMasterPoll( void )
     eMBMasterErrorEventType errorType;
 
     /* Check if the protocol stack is ready. */
-    if( eMBState != STATE_ENABLED )
-    {
+    if( eMBState != STATE_ENABLED ) {
         return MB_EILLSTATE;
     }
 
     /* Check if there is a event available. If not return control to caller.
      * Otherwise we will handle the event. */
-    if( xMBMasterPortEventGet( &eEvent ) == TRUE )
-    {
+    if( xMBMasterPortEventGet( &eEvent ) == TRUE ) {
         switch ( eEvent )
         {
         case EV_MASTER_NO_EVENT:
@@ -303,13 +300,10 @@ eMBMasterPoll( void )
         case EV_MASTER_FRAME_RECEIVED:
             eStatus = peMBMasterFrameReceiveCur( &ucRcvAddress, &ucMBFrame, &usLength );
             // Check if the frame is for us. If not ,send an error process event.
-            if ( ( eStatus == MB_ENOERR ) && ( ucRcvAddress == ucMBMasterGetDestAddress() ) )
-            {
+            if ( ( eStatus == MB_ENOERR ) && ( ucRcvAddress == ucMBMasterGetDestAddress() ) ) {
                 ESP_LOGD(MB_PORT_TAG, "%s: Packet data received successfully (%u).", __func__, eStatus);
                 ( void ) xMBMasterPortEventPost( EV_MASTER_EXECUTE );
-            }
-            else
-            {
+            } else {
                 vMBMasterSetErrorType(EV_ERROR_RECEIVE_DATA);
                 ESP_LOGD(MB_PORT_TAG, "%s: Packet data receive failed (addr=%u)(%u).", __func__, ucRcvAddress, eStatus);
                 ( void ) xMBMasterPortEventPost( EV_MASTER_ERROR_PROCESS );
@@ -322,13 +316,10 @@ eMBMasterPoll( void )
             /* If receive frame has exception. The receive function code highest bit is 1.*/
             if(ucFunctionCode >> 7) {
                 eException = (eMBException)ucMBFrame[MB_PDU_DATA_OFF];
-            }
-            else
-            {
-                for (i = 0; i < MB_FUNC_HANDLERS_MAX; i++)
-                {
+            } else {
+                for (i = 0; i < MB_FUNC_HANDLERS_MAX; i++) {
                     /* No more function handlers registered. Abort. */
-                    if (xMasterFuncHandlers[i].ucFunctionCode == 0)    {
+                    if (xMasterFuncHandlers[i].ucFunctionCode == 0) {
                         break;
                     }
                     else if (xMasterFuncHandlers[i].ucFunctionCode == ucFunctionCode) {
@@ -342,8 +333,7 @@ eMBMasterPoll( void )
                                 vMBMasterSetDestAddress(j);
                                 eException = xMasterFuncHandlers[i].pxHandler(ucMBFrame, &usLength);
                             }
-                        }
-                        else {
+                        } else {
                             eException = xMasterFuncHandlers[i].pxHandler(ucMBFrame, &usLength);
                         }
                         vMBMasterSetCBRunInMasterMode(FALSE);
@@ -355,8 +345,7 @@ eMBMasterPoll( void )
             if (eException != MB_EX_NONE) {
                 vMBMasterSetErrorType(EV_ERROR_EXECUTE_FUNCTION);
                 ( void ) xMBMasterPortEventPost( EV_MASTER_ERROR_PROCESS );
-            }
-            else {
+            } else {
                 vMBMasterCBRequestSuccess( );
                 vMBMasterRunResRelease( );
             }
@@ -366,8 +355,7 @@ eMBMasterPoll( void )
             /* Master is busy now. */
             vMBMasterGetPDUSndBuf( &ucMBFrame );
             eStatus = peMBMasterFrameSendCur( ucMBMasterGetDestAddress(), ucMBFrame, usMBMasterGetPDUSndLength() );
-            if (eStatus != MB_ENOERR)
-            {
+            if (eStatus != MB_ENOERR) {
                 ESP_LOGD(MB_PORT_TAG, "%s:Frame send error. %d", __func__, eStatus);
             }
 
@@ -438,7 +426,7 @@ eMBMasterErrorEventType eMBMasterGetErrorType( void )
 }
 
 // Set Modbus Master current error event type.
-void vMBMasterSetErrorType( eMBMasterErrorEventType errorType )
+void IRAM_ATTR vMBMasterSetErrorType( eMBMasterErrorEventType errorType )
 {
     eMBMasterCurErrorType = errorType;
 }
diff --git a/components/freemodbus/modbus/rtu/mbrtu.c b/components/freemodbus/modbus/rtu/mbrtu.c
index 0a3bb87ad..0fbebcbe2 100644
--- a/components/freemodbus/modbus/rtu/mbrtu.c
+++ b/components/freemodbus/modbus/rtu/mbrtu.c
@@ -249,7 +249,7 @@ xMBRTUReceiveFSM( void )
 
         /* In the idle state we wait for a new character. If a character
          * is received the t1.5 and t3.5 timers are started and the
-         * receiver is in the state STATE_RX_RECEIVCE.
+         * receiver is in the state STATE_RX_RCV.
          */
     case STATE_RX_IDLE:
         usRcvBufferPos = 0;
@@ -292,8 +292,6 @@ xMBRTUTransmitFSM( void )
         /* We should not get a transmitter event if the transmitter is in
          * idle state.  */
     case STATE_TX_IDLE:
-        /* enable receiver/disable transmitter. */
-        vMBPortSerialEnable( TRUE, FALSE );
         break;
 
     case STATE_TX_XMIT:
@@ -306,11 +304,10 @@ xMBRTUTransmitFSM( void )
         }
         else
         {
-            xNeedPoll = xMBPortEventPost( EV_FRAME_SENT );
-            /* Disable transmitter. This prevents another transmit buffer
-             * empty interrupt. */
-            vMBPortSerialEnable( TRUE, FALSE );
+            xMBPortEventPost( EV_FRAME_SENT );
+            xNeedPoll = TRUE;
             eSndState = STATE_TX_IDLE;
+            vMBPortTimersEnable(  );
         }
         break;
     }
@@ -318,7 +315,7 @@ xMBRTUTransmitFSM( void )
     return xNeedPoll;
 }
 
-BOOL
+BOOL MB_PORT_ISR_ATTR
 xMBRTUTimerT35Expired( void )
 {
     BOOL            xNeedPoll = FALSE;
@@ -342,8 +339,7 @@ xMBRTUTimerT35Expired( void )
 
         /* Function called in an illegal state. */
     default:
-        assert( ( eRcvState == STATE_RX_INIT ) ||
-                ( eRcvState == STATE_RX_RCV ) || ( eRcvState == STATE_RX_ERROR ) );
+        assert( ( eRcvState == STATE_RX_IDLE ) || ( eRcvState == STATE_RX_ERROR ) );
     }
 
     vMBPortTimersDisable(  );
diff --git a/components/freemodbus/modbus/rtu/mbrtu_m.c b/components/freemodbus/modbus/rtu/mbrtu_m.c
index 3d80e6e8f..129c2d540 100644
--- a/components/freemodbus/modbus/rtu/mbrtu_m.c
+++ b/components/freemodbus/modbus/rtu/mbrtu_m.c
@@ -69,12 +69,15 @@ typedef enum
     STATE_M_TX_XFWR,              /*!< Transmitter is in transfer finish and wait receive state. */
 } eMBMasterSndState;
 
+/*------------------------ Shared variables ---------------------------------*/
+
+volatile UCHAR  ucMasterRTUSndBuf[MB_PDU_SIZE_MAX];
+volatile UCHAR  ucMasterRTURcvBuf[MB_SER_PDU_SIZE_MAX];
+
 /* ----------------------- Static variables ---------------------------------*/
 static volatile eMBMasterSndState eSndState;
 static volatile eMBMasterRcvState eRcvState;
 
-static volatile UCHAR  ucMasterRTUSndBuf[MB_PDU_SIZE_MAX];
-static volatile UCHAR  ucMasterRTURcvBuf[MB_SER_PDU_SIZE_MAX];
 static volatile USHORT usMasterSendPDULength;
 
 static volatile UCHAR *pucMasterSndBufferCur;
@@ -260,7 +263,7 @@ xMBMasterRTUReceiveFSM( void )
 
         /* In the idle state we wait for a new character. If a character
          * is received the t1.5 and t3.5 timers are started and the
-         * receiver is in the state STATE_RX_RECEIVCE and disable early
+         * receiver is in the state STATE_M_RX_RCV and disable early
          * the timer of respond timeout .
          */
     case STATE_M_RX_IDLE:
@@ -310,11 +313,10 @@ xMBMasterRTUTransmitFSM( void )
         /* We should not get a transmitter event if the transmitter is in
          * idle state.  */
     case STATE_M_TX_XFWR:
+        xNeedPoll = TRUE;
         break;
 
     case STATE_M_TX_IDLE:
-        /* enable receiver/disable transmitter. */
-        vMBMasterPortSerialEnable( TRUE, FALSE );
         break;
 
     case STATE_M_TX_XMIT:
@@ -328,9 +330,6 @@ xMBMasterRTUTransmitFSM( void )
         else
         {
             xFrameIsBroadcast = ( ucMasterRTUSndBuf[MB_SER_PDU_ADDR_OFF] == MB_ADDRESS_BROADCAST ) ? TRUE : FALSE;
-            /* Disable transmitter. This prevents another transmit buffer
-             * empty interrupt. */
-            vMBMasterPortSerialEnable( TRUE, FALSE );
             eSndState = STATE_M_TX_XFWR;
             /* If the frame is broadcast ,master will enable timer of convert delay,
              * else master will enable timer of respond timeout. */
@@ -342,7 +341,6 @@ xMBMasterRTUTransmitFSM( void )
             {
             	vMBMasterPortTimersRespondTimeoutEnable( );
             }
-            xNeedPoll = TRUE;
         }
         break;
     }
@@ -350,8 +348,7 @@ xMBMasterRTUTransmitFSM( void )
     return xNeedPoll;
 }
 
-BOOL
-xMBMasterRTUTimerExpired(void)
+BOOL MB_PORT_ISR_ATTR xMBMasterRTUTimerExpired(void)
 {
 	BOOL xNeedPoll = FALSE;
 
@@ -395,8 +392,7 @@ xMBMasterRTUTimerExpired(void)
 		break;
 		/* Function called in an illegal state. */
 	default:
-		assert(
-				( eSndState == STATE_M_TX_XFWR ) || ( eSndState == STATE_M_TX_IDLE ));
+		assert(( eSndState == STATE_M_TX_XFWR ) || ( eSndState == STATE_M_TX_IDLE ));
 		break;
 	}
 	eSndState = STATE_M_TX_IDLE;
diff --git a/components/freemodbus/port/port.h b/components/freemodbus/port/port.h
index ccba5b3d1..f405898bb 100644
--- a/components/freemodbus/port/port.h
+++ b/components/freemodbus/port/port.h
@@ -30,7 +30,7 @@
 #define MB_PORT_CHECK(a, ret_val, str, ...) \
     if (!(a)) { \
         ESP_LOGE(MB_PORT_TAG, "%s(%u): " str, __FUNCTION__, __LINE__, ##__VA_ARGS__); \
-        return (ret_val); \
+        return ret_val; \
     }
 
 #ifdef __cplusplus
diff --git a/components/freemodbus/port/portevent.c b/components/freemodbus/port/portevent.c
index 2c6597eef..e9988bb58 100644
--- a/components/freemodbus/port/portevent.c
+++ b/components/freemodbus/port/portevent.c
@@ -56,7 +56,7 @@
 /* ----------------------- Variables ----------------------------------------*/
 static xQueueHandle xQueueHdl;
 
-#define MB_EVENT_QUEUE_SIZE     (1)
+#define MB_EVENT_QUEUE_SIZE     (6)
 #define MB_EVENT_QUEUE_TIMEOUT  (pdMS_TO_TICKS(CONFIG_FMB_EVENT_QUEUE_TIMEOUT))
 
 /* ----------------------- Start implementation -----------------------------*/
@@ -82,21 +82,27 @@ vMBPortEventClose( void )
     }
 }
 
-BOOL
+BOOL MB_PORT_ISR_ATTR
 xMBPortEventPost( eMBEventType eEvent )
 {
-    BOOL bStatus = TRUE;
+    BaseType_t xStatus, xHigherPriorityTaskWoken = pdFALSE;
     assert(xQueueHdl != NULL);
     
     if( (BOOL)xPortInIsrContext() == TRUE )
     {
-        xQueueSendFromISR(xQueueHdl, (const void*)&eEvent, pdFALSE);
+        xStatus = xQueueSendFromISR(xQueueHdl, (const void*)&eEvent, &xHigherPriorityTaskWoken);
+        MB_PORT_CHECK((xStatus == pdTRUE), FALSE, "%s: Post message failure.", __func__);
+        if ( xHigherPriorityTaskWoken )
+        {
+            portYIELD_FROM_ISR();
+        }
     }
     else
     {
-        xQueueSend(xQueueHdl, (const void*)&eEvent, MB_EVENT_QUEUE_TIMEOUT);
+        xStatus = xQueueSend(xQueueHdl, (const void*)&eEvent, MB_EVENT_QUEUE_TIMEOUT);
+        MB_PORT_CHECK((xStatus == pdTRUE), FALSE, "%s: Post message failure.", __func__);
     }
-    return bStatus;
+    return TRUE;
 }
 
 BOOL
diff --git a/components/freemodbus/port/portevent_m.c b/components/freemodbus/port/portevent_m.c
index 21fdf77be..a0d44b95b 100644
--- a/components/freemodbus/port/portevent_m.c
+++ b/components/freemodbus/port/portevent_m.c
@@ -79,7 +79,7 @@ xMBMasterPortEventInit( void )
     return TRUE;
 }
 
-BOOL
+BOOL MB_PORT_ISR_ATTR
 xMBMasterPortEventPost( eMBMasterEventType eEvent )
 {
     BOOL bStatus = FALSE;
@@ -140,10 +140,7 @@ xMBMasterPortEventGet( eMBMasterEventType * eEvent)
 void vMBMasterOsResInit( void )
 {
     xSemaphorMasterHdl = xSemaphoreCreateBinary();
-    if (xSemaphorMasterHdl == NULL)
-    {
-        ESP_LOGE(MB_PORT_TAG,"%s: OS semaphore create error.", __func__);
-    }
+    MB_PORT_CHECK((xSemaphorMasterHdl != NULL), ; , "%s: OS semaphore create error.", __func__);
 }
 
 /**
@@ -156,15 +153,13 @@ void vMBMasterOsResInit( void )
  */
 BOOL xMBMasterRunResTake( LONG lTimeOut )
 {
-    BOOL xResult = FALSE;
     BaseType_t xStatus = pdTRUE;
 
     // If waiting time is -1. It will wait forever
     xStatus = xSemaphoreTake(xSemaphorMasterHdl, lTimeOut );
-    if (xStatus == pdTRUE) {
-        xResult = TRUE;
-    }
-    return xResult;
+    MB_PORT_CHECK((xStatus == pdTRUE), FALSE , "%s:Take resource failure.", __func__);
+    ESP_LOGV(MB_PORT_TAG,"%s:Take resource (%lu ticks).", __func__, lTimeOut);
+    return TRUE;
 }
 
 /**
@@ -173,11 +168,9 @@ BOOL xMBMasterRunResTake( LONG lTimeOut )
  */
 void vMBMasterRunResRelease( void )
 {
-    BaseType_t xStatus = pdTRUE;
+    BaseType_t xStatus = pdFALSE;
     xStatus = xSemaphoreGive(xSemaphorMasterHdl);
-    if (xStatus != pdTRUE) {
-        ESP_LOGE(MB_PORT_TAG,"%s: resource release failure.", __func__);
-    }
+    MB_PORT_CHECK((xStatus == pdTRUE), ; , "%s: resource release failure.", __func__);
 }
 
 /**
@@ -192,9 +185,7 @@ void vMBMasterRunResRelease( void )
 void vMBMasterErrorCBRespondTimeout(UCHAR ucDestAddress, const UCHAR* pucPDUData, USHORT ucPDULength) 
 {
     BOOL ret = xMBMasterPortEventPost(EV_MASTER_ERROR_RESPOND_TIMEOUT);
-    if(ret != TRUE) {
-        ESP_LOGE(MB_PORT_TAG, "Post event 'EV_MASTER_ERROR_RESPOND_TIMEOUT' failed!!!");
-    }
+    MB_PORT_CHECK((ret == TRUE), ; , "%s: Post event 'EV_MASTER_ERROR_RESPOND_TIMEOUT' failed!", __func__);
     ESP_LOGD(MB_PORT_TAG,"%s:Callback respond timeout.", __func__);
 }
 
@@ -209,9 +200,7 @@ void vMBMasterErrorCBRespondTimeout(UCHAR ucDestAddress, const UCHAR* pucPDUData
 void vMBMasterErrorCBReceiveData(UCHAR ucDestAddress, const UCHAR* pucPDUData, USHORT ucPDULength) 
 {
     BOOL ret = xMBMasterPortEventPost(EV_MASTER_ERROR_RECEIVE_DATA);
-    if(ret != TRUE) {
-        ESP_LOGE(MB_PORT_TAG,"xMBMasterPortEventPost event 'EV_MASTER_ERROR_RECEIVE_DATA' failed!!!");
-    }
+    MB_PORT_CHECK((ret == TRUE), ; , "%s: Post event 'EV_MASTER_ERROR_RECEIVE_DATA' failed!", __func__);
     ESP_LOGD(MB_PORT_TAG,"%s:Callback receive data timeout failure.", __func__);
 }
 
@@ -228,9 +217,8 @@ void vMBMasterErrorCBReceiveData(UCHAR ucDestAddress, const UCHAR* pucPDUData, U
 void vMBMasterErrorCBExecuteFunction(UCHAR ucDestAddress, const UCHAR* pucPDUData, USHORT ucPDULength) 
 {
     BOOL ret = xMBMasterPortEventPost(EV_MASTER_ERROR_EXECUTE_FUNCTION);
-    if(ret != TRUE) {
-        ESP_LOGE(MB_PORT_TAG,"xMBMasterPortEventPost event 'EV_MASTER_ERROR_EXECUTE_FUNCTION' failed!!!");
-    }
+    MB_PORT_CHECK((ret == TRUE), ; , "%s: Post event 'EV_MASTER_ERROR_EXECUTE_FUNCTION' failed!", __func__);
+    ESP_LOGD(MB_PORT_TAG,"%s:Callback execute data timeout failure.", __func__);
 }
 
 /**
@@ -244,9 +232,8 @@ void vMBMasterCBRequestSuccess( void ) {
      * If you don't use OS, you can change it.
      */
     BOOL ret = xMBMasterPortEventPost(EV_MASTER_PROCESS_SUCCESS);
-    if (ret != TRUE) {
-        ESP_LOGE(MB_PORT_TAG,"xMBMasterPortEventPost event 'EV_MASTER_PROCESS_SUCCESS' failed!!!");
-    }
+    MB_PORT_CHECK((ret == TRUE), ; , "%s: Post event 'EV_MASTER_PROCESS_SUCCESS' failed!", __func__);
+    ESP_LOGD(MB_PORT_TAG,"%s: Callback request success.", __func__);
 }
 
 /**
@@ -284,7 +271,7 @@ eMBMasterReqErrCode eMBMasterWaitRequestFinish( void ) {
             eErrStatus = MB_MRE_EXE_FUN;
         }
     } else {
-        ESP_LOGE(MB_PORT_TAG,"%s: Incorrect event or timeout xRecvedEvent = 0x%x", __func__, xRecvedEvent);
+        ESP_LOGE(MB_PORT_TAG,"%s: Incorrect event or timeout xRecvedEvent = 0x%x", __func__, uxBits);
         assert(0);
     }
     return eErrStatus;
diff --git a/components/freemodbus/port/portserial.c b/components/freemodbus/port/portserial.c
index 879c14ee8..73e72807f 100644
--- a/components/freemodbus/port/portserial.c
+++ b/components/freemodbus/port/portserial.c
@@ -67,6 +67,8 @@
 #define MB_SERIAL_TASK_STACK_SIZE   (CONFIG_FMB_SERIAL_TASK_STACK_SIZE)
 #define MB_SERIAL_TOUT              (3) // 3.5*8 = 28 ticks, TOUT=3 -> ~24..33 ticks
 
+#define MB_SERIAL_TX_TOUT_MS        (100)
+#define MB_SERIAL_TX_TOUT_TICKS     pdMS_TO_TICKS(MB_SERIAL_TX_TOUT_MS) // timeout for transmission
 // Set buffer size for transmission
 #define MB_SERIAL_BUF_SIZE          (CONFIG_FMB_SERIAL_BUF_SIZE)
 
@@ -125,14 +127,13 @@ static void vMBPortSerialRxPoll(size_t xEventSize)
             // Let the stack know that T3.5 time is expired and data is received
             (void)pxMBPortCBTimerExpired(); // calls callback xMBRTUTimerT35Expired();
 #endif
-            ESP_LOGD(TAG, "RX_T35_timeout: %d(bytes in buffer)\n", (uint32_t)usLength);
+            ESP_LOGD(TAG, "Receive: %d(bytes in buffer)\n", (uint32_t)usLength);
         }
     }
 }
 
 BOOL xMBPortSerialTxPoll(void)
 {
-    BOOL bStatus = FALSE;
     USHORT usCount = 0;
     BOOL bNeedPoll = FALSE;
 
@@ -142,10 +143,14 @@ BOOL xMBPortSerialTxPoll(void)
             // Calls the modbus stack callback function to let it fill the UART transmit buffer.
             bNeedPoll = pxMBFrameCBTransmitterEmpty( ); // calls callback xMBRTUTransmitFSM();
         }
-        ESP_LOGD(TAG, "MB_TX_buffer sent: (%d) bytes\n", (uint16_t)usCount);
-        bStatus = TRUE;
+        ESP_LOGD(TAG, "MB_TX_buffer send: (%d) bytes\n", (uint16_t)usCount);
+        // Waits while UART sending the packet
+        esp_err_t xTxStatus = uart_wait_tx_done(ucUartNumber, MB_SERIAL_TX_TOUT_TICKS);
+        vMBPortSerialEnable(TRUE, FALSE);
+        MB_PORT_CHECK((xTxStatus == ESP_OK), FALSE, "mb serial sent buffer failure.");
+        return TRUE;
     }
-    return bStatus;
+    return FALSE;
 }
 
 static void vUartTask(void *pvParameters)
@@ -154,7 +159,6 @@ static void vUartTask(void *pvParameters)
     for(;;) {
         if (xQueueReceive(xMbUartQueue, (void*)&xEvent, portMAX_DELAY) == pdTRUE) {
             ESP_LOGD(TAG, "MB_uart[%d] event:", ucUartNumber);
-            //vMBPortTimersEnable();
             switch(xEvent.type) {
                 //Event of UART receving data
                 case UART_DATA:
@@ -246,7 +250,7 @@ BOOL xMBPortSerialInit(UCHAR ucPORT, ULONG ulBaudRate,
             FALSE, "mb config failure, uart_param_config() returned (0x%x).", (uint32_t)xErr);
     // Install UART driver, and get the queue.
     xErr = uart_driver_install(ucUartNumber, MB_SERIAL_BUF_SIZE, MB_SERIAL_BUF_SIZE,
-            MB_QUEUE_LENGTH, &xMbUartQueue, ESP_INTR_FLAG_LEVEL3);
+                                    MB_QUEUE_LENGTH, &xMbUartQueue, MB_PORT_SERIAL_ISR_FLAG);
     MB_PORT_CHECK((xErr == ESP_OK), FALSE,
             "mb serial driver failure, uart_driver_install() returned (0x%x).", (uint32_t)xErr);
 #ifndef MB_TIMER_PORT_ENABLED
diff --git a/components/freemodbus/port/portserial_m.c b/components/freemodbus/port/portserial_m.c
index 2c70479e5..15f4d9f00 100644
--- a/components/freemodbus/port/portserial_m.c
+++ b/components/freemodbus/port/portserial_m.c
@@ -114,7 +114,7 @@ static void vMBMasterPortSerialRxPoll(size_t xEventSize)
             }
             // The buffer is transferred into Modbus stack and is not needed here any more
             uart_flush_input(ucUartNumber);
-            ESP_LOGD(TAG, "RX_T35_timeout: %d(bytes in buffer)\n", (uint32_t)usLength);
+            ESP_LOGD(TAG, "Receive: %d(bytes in buffer)\n", (uint32_t)usLength);
         }
     } else {
         ESP_LOGE(TAG, "%s: bRxState disabled but junk data (%d bytes) received. ", __func__, (uint16_t)xEventSize);
@@ -123,7 +123,6 @@ static void vMBMasterPortSerialRxPoll(size_t xEventSize)
 
 BOOL xMBMasterPortSerialTxPoll(void)
 {
-    BOOL bStatus = FALSE;
     USHORT usCount = 0;
     BOOL bNeedPoll = FALSE;
 
@@ -137,10 +136,11 @@ BOOL xMBMasterPortSerialTxPoll(void)
         // Waits while UART sending the packet
         esp_err_t xTxStatus = uart_wait_tx_done(ucUartNumber, MB_SERIAL_TX_TOUT_TICKS);
         bTxStateEnabled = FALSE;
+        vMBMasterPortSerialEnable( TRUE, FALSE );
         MB_PORT_CHECK((xTxStatus == ESP_OK), FALSE, "mb serial sent buffer failure.");
-        bStatus = TRUE;
+        return TRUE;
     }
-    return bStatus;
+    return FALSE;
 }
 
 // UART receive event task
@@ -241,7 +241,7 @@ BOOL xMBMasterPortSerialInit( UCHAR ucPORT, ULONG ulBaudRate, UCHAR ucDataBits,
             FALSE, "mb config failure, uart_param_config() returned (0x%x).", (uint32_t)xErr);
     // Install UART driver, and get the queue.
     xErr = uart_driver_install(ucUartNumber, MB_SERIAL_BUF_SIZE, MB_SERIAL_BUF_SIZE,
-            MB_QUEUE_LENGTH, &xMbUartQueue, ESP_INTR_FLAG_LEVEL3);
+                                    MB_QUEUE_LENGTH, &xMbUartQueue, MB_PORT_SERIAL_ISR_FLAG);
     MB_PORT_CHECK((xErr == ESP_OK), FALSE,
             "mb serial driver failure, uart_driver_install() returned (0x%x).", (uint32_t)xErr);
     // Set timeout for TOUT interrupt (T3.5 modbus time)
diff --git a/components/freemodbus/port/porttimer.c b/components/freemodbus/port/porttimer.c
index 95ecf0f3a..4d1afcb31 100644
--- a/components/freemodbus/port/porttimer.c
+++ b/components/freemodbus/port/porttimer.c
@@ -112,7 +112,7 @@ BOOL xMBPortTimersInit(USHORT usTim1Timerout50us)
                     (uint32_t)xErr);
     // Register ISR for timer
     xErr = timer_isr_register(usTimerGroupIndex, usTimerIndex, vTimerGroupIsr,
-                                (void*)(uint32_t)usTimerIndex, ESP_INTR_FLAG_LOWMED, &xTimerIntHandle);
+                                (void*)(uint32_t)usTimerIndex, MB_PORT_TIMER_ISR_FLAG, &xTimerIntHandle);
     MB_PORT_CHECK((xErr == ESP_OK), FALSE,
                     "timer set value failure, timer_isr_register() returned (0x%x).",
                     (uint32_t)xErr);
@@ -130,13 +130,18 @@ void vMBPortTimersEnable(void)
 #endif
 }
 
-void vMBPortTimersDisable(void)
+void MB_PORT_ISR_ATTR
+vMBPortTimersDisable(void)
 {
 #ifdef CONFIG_FMB_TIMER_PORT_ENABLED
-    ESP_ERROR_CHECK(timer_pause(usTimerGroupIndex, usTimerIndex));
-    ESP_ERROR_CHECK(timer_set_counter_value(usTimerGroupIndex, usTimerIndex, 0ULL));
-    // Disable timer interrupt
-    ESP_ERROR_CHECK(timer_disable_intr(usTimerGroupIndex, usTimerIndex));
+    if( (BOOL)xPortInIsrContext() ) {
+            timer_group_set_counter_enable_in_isr(usTimerGroupIndex, usTimerIndex, TIMER_PAUSE);
+    } else {
+        ESP_ERROR_CHECK(timer_pause(usTimerGroupIndex, usTimerIndex));
+        ESP_ERROR_CHECK(timer_set_counter_value(usTimerGroupIndex, usTimerIndex, 0ULL));
+        // Disable timer interrupt
+        ESP_ERROR_CHECK(timer_disable_intr(usTimerGroupIndex, usTimerIndex));
+    }
 #endif
 }
 
diff --git a/components/freemodbus/port/porttimer_m.c b/components/freemodbus/port/porttimer_m.c
index 7a4897d10..2bf47c331 100644
--- a/components/freemodbus/port/porttimer_m.c
+++ b/components/freemodbus/port/porttimer_m.c
@@ -109,7 +109,7 @@ BOOL xMBMasterPortTimersInit(USHORT usTimeOut50us)
                     (uint32_t)xErr);
     // Register ISR for timer
     xErr = timer_isr_register(usTimerGroupIndex, usTimerIndex,
-                                vTimerGroupIsr, (void*)(uint32_t)usTimerIndex, ESP_INTR_FLAG_LOWMED, &xTimerIntHandle);
+                                vTimerGroupIsr, (void*)(uint32_t)usTimerIndex, MB_PORT_TIMER_ISR_FLAG, &xTimerIntHandle);
     MB_PORT_CHECK((xErr == ESP_OK), FALSE,
                     "timer set value failure, timer_isr_register() returned (0x%x).",
                     (uint32_t)xErr);
@@ -179,13 +179,18 @@ void vMBMasterPortTimersRespondTimeoutEnable(void)
     (void)xMBMasterPortTimersEnable(usTimerTicks);
 }
 
-void vMBMasterPortTimersDisable(void)
+void MB_PORT_ISR_ATTR
+vMBMasterPortTimersDisable()
 {
-    // Stop timer and then reload timer counter value
-    ESP_ERROR_CHECK(timer_pause(usTimerGroupIndex, usTimerIndex));
-    ESP_ERROR_CHECK(timer_set_counter_value(usTimerGroupIndex, usTimerIndex, 0ULL));
-    // Disable timer interrupt
-    ESP_ERROR_CHECK(timer_disable_intr(usTimerGroupIndex, usTimerIndex));
+    if( (BOOL)xPortInIsrContext() ) {
+        timer_group_set_counter_enable_in_isr(usTimerGroupIndex, usTimerIndex, TIMER_PAUSE);
+    } else {
+        // Stop timer and then reload timer counter value
+        ESP_ERROR_CHECK(timer_pause(usTimerGroupIndex, usTimerIndex));
+        ESP_ERROR_CHECK(timer_set_counter_value(usTimerGroupIndex, usTimerIndex, 0ULL));
+        // Disable timer interrupt
+        ESP_ERROR_CHECK(timer_disable_intr(usTimerGroupIndex, usTimerIndex));
+    }
 }
 
 void vMBMasterPortTimerClose(void)
diff --git a/components/freemodbus/serial_master/modbus_controller/mbc_serial_master.c b/components/freemodbus/serial_master/modbus_controller/mbc_serial_master.c
index b162312f9..6900af59a 100644
--- a/components/freemodbus/serial_master/modbus_controller/mbc_serial_master.c
+++ b/components/freemodbus/serial_master/modbus_controller/mbc_serial_master.c
@@ -36,10 +36,17 @@
 extern BOOL xMBMasterPortSerialTxPoll(void);
 
 /*-----------------------Master mode use these variables----------------------*/
+#define MODE_RTU
+#ifdef MODE_RTU
+#define MB_DT_SIZE(size) ((size << 1) + 8)
+#else
+#define MB_DT_SIZE(size) ((size << 2) + 20)
+#endif
 
 // The response time is average processing time + data transmission (higher on lower speeds)
 // ~resp_time_ms = min_pcocessing_time_ms + ((2 packets * (header_size + packet_bytes)) * 11 bits in byte * 1000 ms_in_sec) / transmit_speed))
-#define MB_RESPONSE_TIMEOUT(size) pdMS_TO_TICKS(30 + (2 * ((size << 1) + 8) * 11 * 1000 / mb_speed))
+#define MB_RESPONSE_TIMEOUT(mb_speed, size) pdMS_TO_TICKS( 30 + (2 * MB_DT_SIZE(size) * 11 * 1000 / mb_speed))
+#define MB_RESPONSE_TICS                    pdMS_TO_TICKS(CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND)
 
 static mb_master_interface_t* mbm_interface_ptr = NULL; //&default_interface_inst;
 
@@ -190,8 +197,8 @@ static esp_err_t mbc_serial_master_send_request(mb_param_request_t* request, voi
     uint16_t mb_size = request->reg_size;
     uint32_t mb_speed = mbm_opts->mbm_comm.baudrate;
 
-    // Timeout value for packet processing
-    uint32_t timeout = 0;
+    // Timeout value for last processed packet
+    static uint32_t timeout = MB_RESPONSE_TICS;
     size_t pack_length = 0;
 
     // Set the buffer for callback function processing of received data
@@ -203,53 +210,53 @@ static esp_err_t mbc_serial_master_send_request(mb_param_request_t* request, voi
     {
         case MB_FUNC_READ_COILS:
             pack_length = (mb_size >= 8) ? (mb_size >> 3) : 1;
-            timeout = MB_RESPONSE_TIMEOUT(pack_length);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, pack_length);
             mb_error = eMBMasterReqReadCoils((UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                (USHORT)mb_size , (LONG)timeout );
             break;
         case MB_FUNC_WRITE_SINGLE_COIL:
-            timeout = MB_RESPONSE_TIMEOUT(1);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, 1);
             mb_error = eMBMasterReqWriteCoil((UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                 *(USHORT*)data_ptr, (LONG)timeout );
             break;
         case MB_FUNC_WRITE_MULTIPLE_COILS:
             pack_length = (mb_size >= 8) ? (mb_size >> 3) : 1;
-            timeout = MB_RESPONSE_TIMEOUT(pack_length);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, pack_length);
             mb_error = eMBMasterReqWriteMultipleCoils((UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                             (USHORT)mb_size, (UCHAR*)data_ptr, (LONG)timeout);
             break;
         case MB_FUNC_READ_DISCRETE_INPUTS:
             pack_length = (mb_size >= 8) ? (mb_size >> 3) : 1;
-            timeout = MB_RESPONSE_TIMEOUT(pack_length);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, pack_length);
             mb_error = eMBMasterReqReadDiscreteInputs((UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                         (USHORT)mb_size, (LONG)timeout );
             break;
         case MB_FUNC_READ_HOLDING_REGISTER:
-            timeout = MB_RESPONSE_TIMEOUT(mb_size);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, mb_size);
             mb_error = eMBMasterReqReadHoldingRegister((UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                             (USHORT)mb_size, (LONG)timeout );
             break;
         case MB_FUNC_WRITE_REGISTER:
-            timeout = MB_RESPONSE_TIMEOUT(1);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, 1);
             mb_error = eMBMasterReqWriteHoldingRegister( (UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                             *(USHORT*)data_ptr, (LONG)timeout );
             break;
 
         case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
-            timeout = MB_RESPONSE_TIMEOUT(mb_size);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, mb_size);
             mb_error = eMBMasterReqWriteMultipleHoldingRegister( (UCHAR)mb_slave_addr,
                                                                     (USHORT)mb_offset, (USHORT)mb_size,
                                                                     (USHORT*)data_ptr, (LONG)timeout );
             break;
         case MB_FUNC_READWRITE_MULTIPLE_REGISTERS:
-            timeout = MB_RESPONSE_TIMEOUT(mb_size << 1);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, mb_size << 1);
             mb_error = eMBMasterReqReadWriteMultipleHoldingRegister( (UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                                        (USHORT)mb_size, (USHORT*)data_ptr,
                                                                        (USHORT)mb_offset, (USHORT)mb_size,
                                                                        (LONG)timeout );
             break;
         case MB_FUNC_READ_INPUT_REGISTER:
-            timeout = MB_RESPONSE_TIMEOUT(mb_size);
+            timeout = MB_RESPONSE_TIMEOUT(mb_speed, mb_size);
             mb_error = eMBMasterReqReadInputRegister( (UCHAR)mb_slave_addr, (USHORT)mb_offset,
                                                         (USHORT)mb_size, (LONG) timeout );
             break;
@@ -268,18 +275,25 @@ static esp_err_t mbc_serial_master_send_request(mb_param_request_t* request, voi
             break;
 
         case MB_MRE_NO_REG:
-            error = ESP_ERR_NOT_SUPPORTED;
+            error = ESP_ERR_NOT_SUPPORTED; // Invalid register request
             break;
 
         case MB_MRE_TIMEDOUT:
-            error = ESP_ERR_TIMEOUT;
+            error = ESP_ERR_TIMEOUT; // Slave did not send response
             break;
 
         case MB_MRE_EXE_FUN:
         case MB_MRE_REV_DATA:
-            error = ESP_ERR_INVALID_RESPONSE;
+            error = ESP_ERR_INVALID_RESPONSE; // Invalid response from slave
             break;
+
+        case MB_MRE_MASTER_BUSY:
+            error = ESP_ERR_INVALID_STATE; // Master is busy (previous request is pending
+            break;
+
         default:
+            ESP_LOGE(MB_MASTER_TAG, "%s: Incorrect return code (%x) ",
+                                                                __FUNCTION__, mb_error);
             error = ESP_FAIL;
             break;
     }
diff --git a/components/freemodbus/serial_master/port/port_serial_master.h b/components/freemodbus/serial_master/port/port_serial_master.h
index 6e1a24883..c639743aa 100644
--- a/components/freemodbus/serial_master/port/port_serial_master.h
+++ b/components/freemodbus/serial_master/port/port_serial_master.h
@@ -55,7 +55,7 @@
 #ifdef __cplusplus
 PR_BEGIN_EXTERN_C
 #endif /* __cplusplus */
-   
+
 void vMBPortSetMode( UCHAR ucMode );
 
 #ifdef __cplusplus
diff --git a/components/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c b/components/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c
index 6a125d997..da34d629b 100644
--- a/components/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c
+++ b/components/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c
@@ -48,7 +48,11 @@ static void modbus_slave_task(void *pvParameters)
         // Check if stack started then poll for data
         if (status & MB_EVENT_STACK_STARTED) {
             (void)eMBPoll(); // allow stack to process data
-            (void)xMBPortSerialTxPoll(); // Send response buffer if ready
+            // Send response buffer
+            BOOL xSentState = xMBPortSerialTxPoll(); 
+            if (xSentState) {
+                (void)xMBPortEventPost( EV_FRAME_SENT );
+            }
         }
     }
 }