diff --git a/Kconfig b/Kconfig
index 5123606..57a01cb 100644
--- a/Kconfig
+++ b/Kconfig
@@ -16,7 +16,7 @@ menu "Modbus configuration"
 
     config FMB_TCP_PORT_MAX_CONN
         int "Maximum allowed connections for TCP stack"
-        range 1 8
+        range 1 LWIP_MAX_SOCKETS
         default 5
         depends on FMB_COMM_MODE_TCP_EN
         help
@@ -28,7 +28,7 @@ menu "Modbus configuration"
     config FMB_TCP_CONNECTION_TOUT_SEC
         int "Modbus TCP connection timeout"
         range 1 7200
-        default 20
+        default 2
         depends on FMB_COMM_MODE_TCP_EN
         help
                 Modbus TCP connection timeout in seconds.
diff --git a/examples/serial/mb_serial_master/main/idf_component.yml b/examples/serial/mb_serial_master/main/idf_component.yml
index 307f3db..25d9496 100644
--- a/examples/serial/mb_serial_master/main/idf_component.yml
+++ b/examples/serial/mb_serial_master/main/idf_component.yml
@@ -1,7 +1,7 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
   mb_example_common:
     path: "../../../mb_example_common"
diff --git a/examples/serial/mb_serial_slave/main/idf_component.yml b/examples/serial/mb_serial_slave/main/idf_component.yml
index 307f3db..25d9496 100644
--- a/examples/serial/mb_serial_slave/main/idf_component.yml
+++ b/examples/serial/mb_serial_slave/main/idf_component.yml
@@ -1,7 +1,7 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
   mb_example_common:
     path: "../../../mb_example_common"
diff --git a/examples/serial/mb_serial_slave/sdkconfig.defaults b/examples/serial/mb_serial_slave/sdkconfig.defaults
index 9ea03d7..57eb523 100644
--- a/examples/serial/mb_serial_slave/sdkconfig.defaults
+++ b/examples/serial/mb_serial_slave/sdkconfig.defaults
@@ -1,7 +1,8 @@
 #
 # Modbus configuration
 #
-CONFIG_MB_COMM_MODE_ASCII=y
+CONFIG_MB_COMM_MODE_ASCII=n
+CONFIG_MB_COMM_MODE_RTU=y
 CONFIG_MB_SLAVE_ADDR=1
 CONFIG_MB_UART_BAUD_RATE=115200
 CONFIG_FMB_TIMER_USE_ISR_DISPATCH_METHOD=y
diff --git a/examples/tcp/mb_tcp_master/main/idf_component.yml b/examples/tcp/mb_tcp_master/main/idf_component.yml
index 5983a02..5f3cb58 100644
--- a/examples/tcp/mb_tcp_master/main/idf_component.yml
+++ b/examples/tcp/mb_tcp_master/main/idf_component.yml
@@ -2,10 +2,10 @@ dependencies:
   idf:
      version: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
   espressif/mdns:
-    version: "^1.0.0"
+    version: "^1"
   mb_example_common:
     path: "../../../mb_example_common"
   protocol_examples_common:
diff --git a/examples/tcp/mb_tcp_slave/main/idf_component.yml b/examples/tcp/mb_tcp_slave/main/idf_component.yml
index aece724..1714d89 100644
--- a/examples/tcp/mb_tcp_slave/main/idf_component.yml
+++ b/examples/tcp/mb_tcp_slave/main/idf_component.yml
@@ -1,7 +1,7 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
   espressif/mdns:
     version: "^1.0.0"
diff --git a/examples/tcp/mb_tcp_slave/main/tcp_slave.c b/examples/tcp/mb_tcp_slave/main/tcp_slave.c
index eb25287..3c875b3 100644
--- a/examples/tcp/mb_tcp_slave/main/tcp_slave.c
+++ b/examples/tcp/mb_tcp_slave/main/tcp_slave.c
@@ -36,7 +36,7 @@
 #define MB_REG_HOLDING_START_AREA0          (HOLD_OFFSET(holding_data0))
 #define MB_REG_HOLDING_START_AREA0_SIZE     ((size_t)((HOLD_OFFSET(holding_data4) - HOLD_OFFSET(holding_data0)) << 1))
 #define MB_REG_HOLDING_START_AREA1          (HOLD_OFFSET(holding_data4))
-#define MB_REG_HOLDING_START_AREA1_SIZE     ((size_t)((HOLD_OFFSET(holding_area1_end) - HOLD_OFFSET(holding_data4)) << 1))
+#define MB_REG_HOLDING_START_AREA1_SIZE     ((size_t)((HOLD_OFFSET(holding_area1_end) - HOLD_OFFSET(holding_data4)) << 1) + 4)
 #define MB_REG_HOLDING_START_AREA2          (HOLD_OFFSET(holding_u8_a))
 #define MB_REG_HOLDING_START_AREA2_SIZE     ((size_t)((HOLD_OFFSET(holding_area2_end) - HOLD_OFFSET(holding_u8_a)) << 1))
 
@@ -327,7 +327,7 @@ static esp_err_t slave_init(mb_communication_info_t *pcomm_info)
     reg_area.type = MB_PARAM_HOLDING; // Set type of register area
     reg_area.start_offset = MB_REG_HOLDING_START_AREA1; // Offset of register area in Modbus protocol
     reg_area.address = (void*)&holding_reg_params.holding_data4; // Set pointer to storage instance
-    reg_area.size = sizeof(float) << 2; // Set the size of register storage instance
+    reg_area.size = MB_REG_HOLDING_START_AREA1_SIZE; // Set the size of register storage instance
     reg_area.access = MB_ACCESS_RW;
     err = mbc_slave_set_descriptor(slave_handle, reg_area);
     MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE,
diff --git a/idf_component.yml b/idf_component.yml
index d9482c9..e130889 100644
--- a/idf_component.yml
+++ b/idf_component.yml
@@ -1,4 +1,4 @@
-version: "2.0.2"
+version: "2.1.0"
 description: ESP-MODBUS is the official Modbus library for Espressif SoCs.
 url: https://github.com/espressif/esp-modbus
 dependencies:
diff --git a/modbus/mb_controller/tcp/mbc_tcp_master.c b/modbus/mb_controller/tcp/mbc_tcp_master.c
index 858bc6c..5f759e6 100644
--- a/modbus/mb_controller/tcp/mbc_tcp_master.c
+++ b/modbus/mb_controller/tcp/mbc_tcp_master.c
@@ -338,11 +338,12 @@ static esp_err_t mbc_tcp_master_get_parameter(void *ctx, uint16_t cid, uint8_t *
 
     error = mbc_tcp_master_set_request(ctx, cid, MB_PARAM_READ, &request, &reg_info);
     if ((error == ESP_OK) && (cid == reg_info.cid) && (request.slave_addr != MB_SLAVE_ADDR_PLACEHOLDER)) {
-        mb_uid_info_t *paddr_info = mbm_port_tcp_get_slave_info(mbm_controller_iface->mb_base->port_obj, 
+        mb_uid_info_t *paddr_info = mbm_port_tcp_get_slave_info(mbm_controller_iface->mb_base->port_obj,
                                                                         request.slave_addr, MB_SOCK_STATE_CONNECTED);
-        MB_RETURN_ON_FALSE((paddr_info), ESP_ERR_NOT_FOUND, TAG,
-                            "mb can not send request for cid #%u with uid = %d.", 
-                            (unsigned)reg_info.cid, (int)request.slave_addr);
+        if (!paddr_info) {
+            ESP_LOGW(TAG, "Try to send request for cid #%u with uid = %d, node is disconnected.",
+                                (unsigned)reg_info.cid, (int)request.slave_addr);
+        }
         MB_MASTER_ASSERT(xPortGetFreeHeapSize() > (reg_info.mb_size << 1));
         // alloc buffer to store parameter data
         pdata = calloc(1, (reg_info.mb_size << 1));
@@ -394,8 +395,10 @@ static esp_err_t mbc_tcp_master_get_parameter_with(void *ctx, uint16_t cid, uint
         // check that the requested uid is connected (call to port iface)
         mb_uid_info_t *paddr_info = mbm_port_tcp_get_slave_info(mbm_controller_iface->mb_base->port_obj, 
                                                                         uid, MB_SOCK_STATE_CONNECTED);
-        MB_RETURN_ON_FALSE((paddr_info), ESP_ERR_NOT_FOUND, TAG,
-                            "mb can not send request for cid #%u with uid=%d.", (unsigned)reg_info.cid, (int)uid);
+        if (!paddr_info) {
+            ESP_LOGW(TAG, "Try to send request for cid #%u with uid = %d, node is disconnected.",
+                                (unsigned)reg_info.cid, (int)request.slave_addr);
+        }
         if (request.slave_addr != MB_SLAVE_ADDR_PLACEHOLDER) {
             ESP_LOGD(TAG, "%s: override uid %d = %d for cid(%u)",
                             __FUNCTION__, (int)request.slave_addr, (int)uid, (unsigned)reg_info.cid);
@@ -449,11 +452,12 @@ static esp_err_t mbc_tcp_master_set_parameter(void *ctx, uint16_t cid, uint8_t *
 
     error = mbc_tcp_master_set_request(ctx, cid, MB_PARAM_WRITE, &request, &reg_info);
     if ((error == ESP_OK) && (cid == reg_info.cid) && (request.slave_addr != MB_SLAVE_ADDR_PLACEHOLDER)) {
-        mb_uid_info_t *paddr_info = mbm_port_tcp_get_slave_info(mbm_controller_iface->mb_base->port_obj, 
+        mb_uid_info_t *paddr_info = mbm_port_tcp_get_slave_info(mbm_controller_iface->mb_base->port_obj,
                                                                         request.slave_addr, MB_SOCK_STATE_CONNECTED);
-        MB_RETURN_ON_FALSE((paddr_info), ESP_ERR_NOT_FOUND, TAG,
-                            "mb can not send request for cid #%u with uid=%d.",
-                            (unsigned)reg_info.cid, (int)request.slave_addr);
+        if (!paddr_info) {
+            ESP_LOGW(TAG, "Try to send request for cid #%u with uid = %d, node is disconnected.",
+                                (unsigned)reg_info.cid, (int)request.slave_addr);
+        }
         MB_MASTER_ASSERT(xPortGetFreeHeapSize() > (reg_info.mb_size << 1));
         pdata = calloc(1, (reg_info.mb_size << 1)); // alloc parameter buffer
         if (!pdata) {
@@ -503,9 +507,10 @@ static esp_err_t mbc_tcp_master_set_parameter_with(void *ctx, uint16_t cid, uint
         // check that the requested uid is connected (call to port iface)
         mb_uid_info_t *paddr_info = mbm_port_tcp_get_slave_info(mbm_controller_iface->mb_base->port_obj, 
                                                                         uid, MB_SOCK_STATE_CONNECTED);
-        MB_RETURN_ON_FALSE((paddr_info), ESP_ERR_NOT_FOUND, TAG,
-                            "mb can not send request for cid #%d with uid=%d.",
-                            (unsigned)reg_info.cid, (int)uid);
+        if (!paddr_info) {
+            ESP_LOGW(TAG, "Try to send request for cid #%u with uid = %d, node is disconnected.",
+                                (unsigned)reg_info.cid, (int)request.slave_addr);
+        }
         if (request.slave_addr != MB_SLAVE_ADDR_PLACEHOLDER) {
             ESP_LOGD(TAG, "%s: override uid %d = %d for cid(%u)",
                             __FUNCTION__, (int)request.slave_addr, (int)uid, (unsigned)reg_info.cid);
diff --git a/modbus/mb_objects/common/mb_port_types.h b/modbus/mb_objects/common/mb_port_types.h
index 1472c10..584da29 100644
--- a/modbus/mb_objects/common/mb_port_types.h
+++ b/modbus/mb_objects/common/mb_port_types.h
@@ -61,7 +61,6 @@ typedef enum _addr_type_enum {
     MB_IPV6 = 2                     /*!< TCP IPV6 addressing */
 } mb_addr_type_t;
 
-
 struct port_common_opts_s {
     mb_mode_type_t mode;            /*!< Modbus communication mode */
     uint16_t port;                  /*!< Modbus communication port (UART) number */
diff --git a/modbus/mb_objects/mb_master.c b/modbus/mb_objects/mb_master.c
index 0e431dc..04d3690 100644
--- a/modbus/mb_objects/mb_master.c
+++ b/modbus/mb_objects/mb_master.c
@@ -547,7 +547,7 @@ mb_err_enum_t mbm_poll(mb_base_t *inst)
                 status = MB_OBJ(inst->transp_obj)->frm_send(inst->transp_obj, mbm_obj->master_dst_addr, 
                                                                 mbm_obj->snd_frame, mbm_obj->pdu_snd_len);
                 if (status != MB_ENOERR) {
-                    mb_port_event_set_err_type(MB_OBJ(inst->port_obj), EV_ERROR_RECEIVE_DATA);
+                    mb_port_event_set_err_type(MB_OBJ(inst->port_obj), EV_ERROR_RESPOND_TIMEOUT);
                     (void)mb_port_event_post(MB_OBJ(inst->port_obj), EVENT(EV_ERROR_PROCESS));
                     ESP_LOGE(TAG, MB_OBJ_FMT", frame send error. %d", MB_OBJ_PARENT(inst), (int)status);
                 }
diff --git a/modbus/mb_ports/common/port_event.c b/modbus/mb_ports/common/port_event.c
index 648f2ca..3c0f11a 100644
--- a/modbus/mb_ports/common/port_event.c
+++ b/modbus/mb_ports/common/port_event.c
@@ -200,7 +200,7 @@ mb_err_enum_t mb_port_event_wait_req_finish(mb_port_base_t *inst)
             err_status = MB_EILLFUNC;
         }
     } else {
-        ESP_LOGE(TAG, "%s, %s: incorrect event or timeout, rcv_event = 0x%x", inst->descr.parent_name, __func__, (int)bits);
+        ESP_LOGD(TAG, "%s, %s: incorrect event or timeout, rcv_event = 0x%x", inst->descr.parent_name, __func__, (int)bits);
         err_status = MB_ETIMEDOUT;
     }
     return err_status;
diff --git a/modbus/mb_ports/tcp/port_tcp_common.h b/modbus/mb_ports/tcp/port_tcp_common.h
index 36b525b..30d4358 100644
--- a/modbus/mb_ports/tcp/port_tcp_common.h
+++ b/modbus/mb_ports/tcp/port_tcp_common.h
@@ -9,6 +9,7 @@
 #include <string.h>
 #include "mb_config.h"
 #include "mb_common.h"
+#include "sdkconfig.h" // for KConfig options
 
 #ifdef __cplusplus
 extern "C" {
@@ -19,9 +20,8 @@ extern "C" {
 #define MB_TCP_PORT_MAX_CONN            (CONFIG_FMB_TCP_PORT_MAX_CONN)
 #define MB_TCP_DEFAULT_PORT             (CONFIG_FMB_TCP_PORT_DEFAULT)
 #define MB_FRAME_QUEUE_SZ               (20)
-#define MB_TCP_CONNECTION_TIMEOUT_MS    (20)        // connection timeout in mS
-#define MB_TCP_RECONNECT_TIMEOUT        (5000000)   // reconnection timeout in uS
-
+#define MB_TCP_CHECK_ALIVE_TOUT_MS      (20) // check alive timeout in mS
+#define MB_RECONNECT_TIME_MS            (CONFIG_FMB_TCP_CONNECTION_TOUT_SEC * 1000UL)
 #define MB_EVENT_SEND_RCV_TOUT_MS       (500)
 
 #define MB_TCP_MBAP_GET_FIELD(buffer, field) ((uint16_t)((buffer[field] << 8U) | buffer[field + 1]))
diff --git a/modbus/mb_ports/tcp/port_tcp_driver.c b/modbus/mb_ports/tcp/port_tcp_driver.c
index ca5cc01..5b73a77 100644
--- a/modbus/mb_ports/tcp/port_tcp_driver.c
+++ b/modbus/mb_ports/tcp/port_tcp_driver.c
@@ -202,18 +202,22 @@ static esp_err_t init_queues(mb_node_info_t *mb_node)
 
 static void delete_queues(mb_node_info_t *pmb_node)
 {
-    if (!queue_is_empty(pmb_node->rx_queue)) 
-    {
-        queue_flush(pmb_node->rx_queue);
+    if (pmb_node) {
+        if (pmb_node->rx_queue) {
+            if (!queue_is_empty(pmb_node->rx_queue)) {
+                queue_flush(pmb_node->rx_queue);
+            }
+            queue_delete(pmb_node->rx_queue);
+            pmb_node->rx_queue = NULL;
+        }
+        if (pmb_node->tx_queue) {
+            if (!queue_is_empty(pmb_node->tx_queue)) {
+                queue_flush(pmb_node->tx_queue);
+            }
+            queue_delete(pmb_node->tx_queue);
+            pmb_node->tx_queue = NULL;
+        }
     }
-    if (!queue_is_empty(pmb_node->tx_queue)) 
-    {
-        queue_flush(pmb_node->tx_queue);
-    }
-    queue_delete(pmb_node->rx_queue);
-    queue_delete(pmb_node->tx_queue);
-    pmb_node->rx_queue = NULL;
-    pmb_node->tx_queue = NULL;
 }
 
 inline void mb_drv_lock(void *ctx)
@@ -287,6 +291,7 @@ int mb_drv_open(void *ctx, mb_uid_info_t addr_info, int flags)
         pnode_info = pdrv_ctx->mb_nodes[fd];
         if (!pnode_info) {
             pnode_info = calloc(1, sizeof(mb_node_info_t));
+            mb_drv_lock(ctx);
             if (!pnode_info) {
                 goto err;
             }
@@ -297,9 +302,9 @@ int mb_drv_open(void *ctx, mb_uid_info_t addr_info, int flags)
                 goto err;
             }
             if (pdrv_ctx->mb_node_open_count > MB_MAX_FDS) {
+                ESP_LOGD(TAG, "Exceeded maximum node count: %d", pdrv_ctx->mb_node_open_count);
                 goto err;
             }
-            mb_drv_lock(ctx);
             pdrv_ctx->mb_node_open_count++;
             pnode_info->index = fd;
             pnode_info->fd = fd;
@@ -326,6 +331,7 @@ int mb_drv_open(void *ctx, mb_uid_info_t addr_info, int flags)
     }
 
 err:
+    delete_queues(pnode_info);
     free(pnode_info);
     pdrv_ctx->mb_nodes[fd] = NULL;
     mb_drv_unlock(ctx);
@@ -409,16 +415,26 @@ int mb_drv_close(void *ctx, int fd)
         errno = EBADF;
         return -1;
     }
-    
+    mb_drv_lock(ctx);
     // stop socket
     if (MB_GET_NODE_STATE(pnode_info) != MB_SOCK_STATE_CLOSED) {
-        MB_SET_NODE_STATE(pnode_info, MB_SOCK_STATE_CLOSED);
         // Do we need to close connection, if the close event is not run
+        if ((pnode_info->sock_id > 0) && (FD_ISSET(pnode_info->sock_id, &pdrv_ctx->conn_set)))
+        {
+            FD_CLR(pnode_info->sock_id, &pdrv_ctx->conn_set);
+            if (pdrv_ctx->node_conn_count)
+            {
+                pdrv_ctx->node_conn_count--;
+            }
+        }
         port_close_connection((mb_node_info_t *)pnode_info);
     }
-    mb_drv_lock(ctx);
+    MB_SET_NODE_STATE(pnode_info, MB_SOCK_STATE_CLOSED);
     FD_CLR(fd, &pdrv_ctx->open_set);
     delete_queues(pnode_info);
+    if (pdrv_ctx->mb_node_open_count) {
+        pdrv_ctx->mb_node_open_count--;
+    }
     if (pnode_info->addr_info.node_name_str != pnode_info->addr_info.ip_addr_str) {
         free((void *)pnode_info->addr_info.ip_addr_str); // node ip addr string shall be freed
     }
@@ -551,30 +567,28 @@ esp_err_t mb_drv_stop_task(void *ctx)
     return err;
 }
 
-// Todo: remove this later
-err_t mb_drv_check_node_state(void *ctx, int fd)
+err_t mb_drv_check_node_state(void *ctx, int *pfd, uint32_t timeout_ms)
 {
     port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
     mb_node_info_t *pnode = NULL;
-    err_t err = ERR_ABRT;
-    int curr_fd = (fd >= 0) ? fd : 0;
+    err_t err = ERR_TIMEOUT;
 
-    while(((pnode = mb_drv_get_next_node_from_set(ctx, &curr_fd, &pdrv_ctx->conn_set))
-                            && (curr_fd < MB_MAX_FDS))) {
-        if (FD_ISSET(pnode->sock_id, &pdrv_ctx->conn_set)) {
-            uint64_t last_read_div_us = (esp_timer_get_time() - pnode->recv_time);
-            if (last_read_div_us >= (uint64_t)(MB_RECONNECT_TIME_MS * 1000)) {
-                // ESP_LOGD(TAG, "%p, slave: %d, sock: %d, IP:%s, check connection, time = %" PRId64 ", rcv_time: %" PRId64,
-                //             ctx, (int)pnode->index, (int)pnode->sock_id, pnode->addr_info.ip_addr_str,
-                //             (esp_timer_get_time() / 1000), pnode->recv_time / 1000);
-                err = port_check_alive(pnode, 1);
-                if ((err < 0) && (err != ERR_INPROGRESS)) {
-                    ESP_LOGE(TAG, "Node #%d [%s], connection error.", pnode->index, pnode->addr_info.ip_addr_str);
-                } else {
-                    ESP_LOGD(TAG, "Node #%d [%s], connection is ok.", pnode->index, pnode->addr_info.ip_addr_str);
-                }
+    pnode = mb_drv_get_next_node_from_set(ctx, pfd, &pdrv_ctx->conn_set);
+    if (pnode && FD_ISSET(pnode->sock_id, &pdrv_ctx->conn_set)) {
+        uint64_t last_read_div_us = (esp_timer_get_time() - pnode->recv_time);
+        ESP_LOGD(TAG, "%p, node: %d, sock: %d, IP:%s, check connection timeout = %" PRId64 ", rcv_time: %" PRId64 " %" PRId32,
+                    ctx, (int)pnode->index, (int)pnode->sock_id, pnode->addr_info.ip_addr_str,
+                    (esp_timer_get_time() / 1000), pnode->recv_time / 1000, timeout_ms);
+        if (last_read_div_us >= (uint64_t)(timeout_ms * 1000)) {
+            ESP_LOGD(TAG, "%p, node: %d, sock: %d, IP:%s, check connection state, time = %" PRId64 ", rcv_time: %" PRId64,
+                        ctx, (int)pnode->index, (int)pnode->sock_id, pnode->addr_info.ip_addr_str,
+                        (esp_timer_get_time() / 1000), pnode->recv_time / 1000);
+            err = port_check_alive(pnode, 1); // minimize blocking time
+            if ((err < 0) && (err != ERR_INPROGRESS)) {
+                ESP_LOGD(TAG, "Node #%d (%s), connection error, err=(%d).", pnode->index, pnode->addr_info.ip_addr_str, (int)err);
+            } if (err == ERR_OK) {
+                ESP_LOGD(TAG, "Node #%d (%s), connection is alive, err=(%d).", pnode->index, pnode->addr_info.ip_addr_str, (int)err);
                 pnode->recv_time = esp_timer_get_time();
-                curr_fd++;
             }
         }
     }
@@ -591,7 +605,6 @@ void mb_drv_tcp_task(void *ctx)
         int ret = mb_drv_wait_fd_events(ctx, &readset, &errorset, MB_SELECT_WAIT_MS);
         if (ret == ERR_TIMEOUT) {
             // timeout occured waiting for the vfds
-            // ESP_LOGD(TAG, "%p, task select timeout.", ctx);
             DRIVER_SEND_EVENT(ctx, MB_EVENT_TIMEOUT, UNDEF_FD);
             mb_drv_check_suspend_shutdown(ctx);
         } else if (ret == -1) {
@@ -654,12 +667,13 @@ void mb_drv_tcp_task(void *ctx)
                                         (int)pnode_info->sock_id, pnode_info->addr_info.ip_addr_str);
                         } else {
                             if (ret == ERR_CONN) {
-                                ESP_LOGE(TAG, "%p, "MB_NODE_FMT(", connection lost."), ctx, (int)pnode_info->fd,
+                                ESP_LOGD(TAG, "%p, "MB_NODE_FMT(", connection lost."), ctx, (int)pnode_info->fd,
                                             (int)pnode_info->sock_id, pnode_info->addr_info.ip_addr_str);
                                 DRIVER_SEND_EVENT(ctx, MB_EVENT_ERROR, pnode_info->index);
                             } else {
-                                ESP_LOGE(TAG, "%p, "MB_NODE_FMT(", critical error=%d, errno=%u."), ctx, (int)pnode_info->fd,
+                                ESP_LOGD(TAG, "%p, "MB_NODE_FMT(", critical read error=%d, errno=%u."), ctx, (int)pnode_info->fd,
                                         (int)pnode_info->sock_id, pnode_info->addr_info.ip_addr_str, (int)ret, (unsigned)errno);
+                                DRIVER_SEND_EVENT(ctx, MB_EVENT_ERROR, pnode_info->index);
                             }
                         }
                     }
@@ -779,6 +793,12 @@ esp_err_t mb_drv_unregister(void *ctx)
         ESP_LOGE(TAG, "could not close the eventfd handle, err = %d. Already closed?", err);
     }
 
+    if (pdrv_ctx->listen_sock_fd) {
+        shutdown(pdrv_ctx->listen_sock_fd, SHUT_RDWR);
+        close(pdrv_ctx->listen_sock_fd);
+        pdrv_ctx->listen_sock_fd = UNDEF_FD;
+    }
+
     for (int i = 0; i < MB_MAX_FDS; i++) {
         mb_node_info_t *pnode_info = pdrv_ctx->mb_nodes[i];
         if (pnode_info) {
diff --git a/modbus/mb_ports/tcp/port_tcp_driver.h b/modbus/mb_ports/tcp/port_tcp_driver.h
index c2b4113..b28c6a1 100644
--- a/modbus/mb_ports/tcp/port_tcp_driver.h
+++ b/modbus/mb_ports/tcp/port_tcp_driver.h
@@ -44,8 +44,7 @@ typedef void (*mb_event_handler_fp)(void *ctx, esp_event_base_t base, int32_t id
 #define MB_PORT_TASK_AFFINITY       (CONFIG_FMB_PORT_TASK_AFFINITY)
 
 #define MB_MAX_FDS                  (MB_TCP_PORT_MAX_CONN)
-#define MB_RETRY_MAX                (2)
-#define MB_RECONNECT_TIME_MS        (1000)
+#define MB_RETRY_CNT                (2)
 #define MB_RX_QUEUE_MAX_SIZE        (CONFIG_FMB_QUEUE_LENGTH)
 #define MB_TX_QUEUE_MAX_SIZE        (CONFIG_FMB_QUEUE_LENGTH)
 #define MB_EVENT_QUEUE_SZ           (CONFIG_FMB_QUEUE_LENGTH * MB_TCP_PORT_MAX_CONN)
@@ -58,7 +57,7 @@ typedef void (*mb_event_handler_fp)(void *ctx, esp_event_base_t base, int32_t id
 #define MB_DRIVER_CONFIG_DEFAULT {              \
     .spin_lock = portMUX_INITIALIZER_UNLOCKED,  \
     .listen_sock_fd = UNDEF_FD,                 \
-    .retry_cnt = MB_RETRY_MAX,                  \
+    .retry_cnt = MB_RETRY_CNT,                  \
     .mb_tcp_task_handle = NULL,                 \
     .mb_node_open_count = 0,                    \
     .curr_node_index = 0,                       \
@@ -68,7 +67,6 @@ typedef void (*mb_event_handler_fp)(void *ctx, esp_event_base_t base, int32_t id
     .mb_nodes = NULL,                           \
     .mb_node_curr = NULL,                       \
     .close_done_sema = NULL,                    \
-    .max_conn_sd = UNDEF_FD,                    \
     .node_conn_count = 0,                       \
     .event_fd = UNDEF_FD,                       \
 }
@@ -243,7 +241,6 @@ typedef struct _port_driver {
     uint16_t curr_node_index;                   /*!< current processing slave index */
     fd_set open_set;                            /*!< file descriptor set for opened nodes */
     fd_set conn_set;                            /*!< file descriptor set for associated nodes */
-    int max_conn_sd;                            /*!< max file descriptor for associated nodes */
     int event_fd;                               /*!< eventfd descriptor for modbus event tracking */
     SemaphoreHandle_t close_done_sema;          /*!< close and done semaphore */
     EventGroupHandle_t status_flags_hdl;        /*!< status bits to control nodes states */
@@ -344,7 +341,7 @@ mb_status_flags_t mb_drv_set_status_flag(void *ctx, mb_status_flags_t mask);
 
 mb_status_flags_t mb_drv_clear_status_flag(void *ctx, mb_status_flags_t mask);
 
-err_t mb_drv_check_node_state(void *ctx, int fd);
+err_t mb_drv_check_node_state(void *ctx, int *fd, uint32_t timeout_ms);
 
 #endif
 
diff --git a/modbus/mb_ports/tcp/port_tcp_master.c b/modbus/mb_ports/tcp/port_tcp_master.c
index 28709e7..9e221d7 100644
--- a/modbus/mb_ports/tcp/port_tcp_master.c
+++ b/modbus/mb_ports/tcp/port_tcp_master.c
@@ -256,8 +256,11 @@ bool mbm_port_tcp_send_data(mb_port_base_t *inst, uint8_t address, uint8_t *pfra
     bool frame_sent = false;
     // get slave descriptor from its address
     mb_node_info_t *pinfo = (mb_node_info_t *)mb_drv_get_node_info_from_addr(port_obj->pdriver, address);
-    MB_RETURN_ON_FALSE((pinfo && (MB_GET_NODE_STATE(pinfo) >= MB_SOCK_STATE_CONNECTED)), 
-                        false, TAG, "the slave address #%d is not registered.", address);
+
+    bool all_nodes_connected = mb_drv_wait_status_flag(port_obj->pdriver, MB_FLAG_CONNECTED, pdMS_TO_TICKS(MB_RECONNECT_TIME_MS));
+
+    MB_RETURN_ON_FALSE((all_nodes_connected && pinfo && (MB_GET_NODE_STATE(pinfo) >= MB_SOCK_STATE_CONNECTED)),
+                        false, TAG, "The node UID #%d, is not connected.", address);
 
     if (pinfo && pframe) {
         // Apply TID field to the frame before send
@@ -266,7 +269,7 @@ bool mbm_port_tcp_send_data(mb_port_base_t *inst, uint8_t address, uint8_t *pfra
     }
 
     ESP_LOGD(TAG, "%p,  send fd: %d, sock_id: %d[%s], %p, len: %d", 
-                port_obj->pdriver, pinfo->fd, pinfo->sock_id, pinfo->addr_info.node_name_str, pframe, length);
+                port_obj->pdriver, pinfo->fd, pinfo->sock_id, pinfo->addr_info.ip_addr_str, pframe, length);
 
     // Write data to the modbus driver send queue of the slave 
     int write_length = mb_drv_write(port_obj->pdriver, pinfo->fd, pframe, length);
@@ -368,10 +371,11 @@ MB_EVENT_HANDLER(mbm_on_resolve)
     ESP_LOGD(TAG, "%s  %s: fd: %d", (char *)base, __func__, (int)pevent_info->opt_fd);
 
     if (MB_CHECK_FD_RANGE(pevent_info->opt_fd)) {
+        ESP_LOGD(TAG, "%p, Node: %d, resolve.", ctx, (int)pevent_info->opt_fd);
         // The mdns is not used in the main app, then can use manually defined IPs
         int fd = pevent_info->opt_fd;
         mb_node_info_t *pslave = mb_drv_get_node(pdrv_ctx, fd);
-        if (pslave && (MB_GET_NODE_STATE(pslave) == MB_SOCK_STATE_OPENED) 
+        if (pslave && (MB_GET_NODE_STATE(pslave) == MB_SOCK_STATE_OPENED)
                     && FD_ISSET(pslave->index, &pdrv_ctx->open_set)) {
             mb_status_flags_t status = mb_drv_wait_status_flag(pdrv_ctx, MB_FLAG_DISCONNECTED, 0);
             if ((status & MB_FLAG_DISCONNECTED)) {
@@ -430,30 +434,36 @@ MB_EVENT_HANDLER(mbm_on_connect)
     err_t err = ERR_CONN;
     if (MB_CHECK_FD_RANGE(pevent_info->opt_fd)) {
         pnode_info = mb_drv_get_node(pdrv_ctx, pevent_info->opt_fd);
-        if (pnode_info && (MB_GET_NODE_STATE(pnode_info) < MB_SOCK_STATE_CONNECTED)) {
+        if (pnode_info &&
+            (MB_GET_NODE_STATE(pnode_info) < MB_SOCK_STATE_CONNECTED) &&
+            (MB_GET_NODE_STATE(pnode_info) >= MB_SOCK_STATE_RESOLVED)) {
             ESP_LOGD(TAG, "%p, connection phase, slave: #%d(%d) [%s].",
                      ctx, (int)pevent_info->opt_fd, (int)pnode_info->sock_id, pnode_info->addr_info.ip_addr_str);
-            if (pnode_info->sock_id != UNDEF_FD) {
-                port_close_connection(pnode_info);
-            }
             err = port_connect(ctx, pnode_info);
             switch (err) {
                 case ERR_OK:
-                    if (!FD_ISSET(pnode_info->sock_id, &pdrv_ctx->conn_set)) {
-                        FD_SET(pnode_info->sock_id, &pdrv_ctx->conn_set);
-                        mb_drv_lock(ctx);
-                        pdrv_ctx->node_conn_count++;
-                        pdrv_ctx->max_conn_sd = (pnode_info->sock_id > pdrv_ctx->max_conn_sd) ? (int)pnode_info->sock_id : pdrv_ctx->max_conn_sd;
-                        // Update time stamp for connected slaves
-                        pnode_info->send_time = esp_timer_get_time();
-                        pnode_info->recv_time = esp_timer_get_time();
-                        mb_drv_unlock(ctx);
-                        ESP_LOGI(TAG, "%p, slave: #%d, sock:%d, IP: %s, is connected.",
-                                    ctx, (int)pevent_info->opt_fd, (int)pnode_info->sock_id, 
-                                    pnode_info->addr_info.ip_addr_str);
-                    }
+                    FD_SET(pnode_info->sock_id, &pdrv_ctx->conn_set);
+                    mb_drv_lock(ctx);
+                    pdrv_ctx->node_conn_count++;
+                    // Update time stamp for connected slaves
+                    pnode_info->send_time = esp_timer_get_time();
+                    pnode_info->recv_time = esp_timer_get_time();
+                    mb_drv_unlock(ctx);
+                    ESP_LOGI(TAG, "%p, slave: #%d, sock:%d, IP: %s, is connected.",
+                                ctx, (int)pevent_info->opt_fd, (int)pnode_info->sock_id, 
+                                pnode_info->addr_info.ip_addr_str);
                     MB_SET_NODE_STATE(pnode_info, MB_SOCK_STATE_CONNECTED);
-                    port_keep_alive(pnode_info);
+                    (void)port_keep_alive(pnode_info->sock_id);
+                    ESP_LOGD(TAG, "Opened/connected: %u, %u.",
+                                (unsigned)pdrv_ctx->mb_node_open_count, (unsigned)pdrv_ctx->node_conn_count);
+                    if (pdrv_ctx->mb_node_open_count == pdrv_ctx->node_conn_count) {
+                        if (pdrv_ctx->event_cbs.on_conn_done_cb) {
+                            pdrv_ctx->event_cbs.on_conn_done_cb(pdrv_ctx->event_cbs.arg);
+                        }
+                        ESP_LOGI(TAG, "%p, Connected: %u, %u, start polling.", 
+                                    ctx, (unsigned)pdrv_ctx->mb_node_open_count, (unsigned)pdrv_ctx->node_conn_count);
+                        mb_drv_set_status_flag(ctx, MB_FLAG_CONNECTED);
+                    }
                     break;
                 case ERR_INPROGRESS:
                     if (FD_ISSET(pnode_info->sock_id, &pdrv_ctx->conn_set)) {
@@ -466,18 +476,24 @@ MB_EVENT_HANDLER(mbm_on_connect)
                             pdrv_ctx->node_conn_count--;
                         }
                         mb_drv_unlock(ctx);
+                        DRIVER_SEND_EVENT(ctx, MB_EVENT_CLOSE, pevent_info->opt_fd);
+                        port_close_connection(pnode_info);
+                    } else {
+                        ESP_LOGD(TAG, "%p, slave: #%d, sock:%d, IP:%s, connection is in progress.",
+                                ctx, (int)pevent_info->opt_fd, (int)pnode_info->sock_id,
+                                pnode_info->addr_info.ip_addr_str);
+                        MB_SET_NODE_STATE(pnode_info, MB_SOCK_STATE_CONNECTING);
+                        vTaskDelay(MB_CONN_TICK_TIMEOUT);
+                        // try to connect to slave and check connection again if it is not connected
+                        DRIVER_SEND_EVENT(ctx, MB_EVENT_CONNECT, pevent_info->opt_fd);
                     }
-                    MB_SET_NODE_STATE(pnode_info, MB_SOCK_STATE_CONNECTING);
-                    vTaskDelay(MB_CONN_TICK_TIMEOUT);
-                    // try to connect to slave and check connection again if it is not connected
-                    DRIVER_SEND_EVENT(ctx, MB_EVENT_CONNECT, pevent_info->opt_fd);
                     break;
                 case ERR_CONN:
-                    ESP_LOGE(TAG, "Modbus connection phase, slave: %d [%s], connection error (%d).",
+                    ESP_LOGE(TAG, "Modbus connection phase, slave: %d (%s), connection error (%d).",
                             (int)pevent_info->opt_fd, pnode_info->addr_info.ip_addr_str, (int)err);
                     break;
                 default:
-                    ESP_LOGE(TAG, "Invalid error state, slave: %d [%s], error = %d.",
+                    ESP_LOGE(TAG, "Invalid error state, slave: %d (%s), error = %d.",
                             (int)pevent_info->opt_fd, pnode_info->addr_info.ip_addr_str, (int)err);
                     break;
             }
@@ -487,7 +503,9 @@ MB_EVENT_HANDLER(mbm_on_connect)
         // then perform connection phase for all resolved slaves sending the connection event
         for (int node = 0; (node < MB_TCP_PORT_MAX_CONN); node++) {
             pnode_info = mb_drv_get_node(pdrv_ctx, node);
-            if (pnode_info && (MB_GET_NODE_STATE(pnode_info) == MB_SOCK_STATE_RESOLVED)) {
+            if (pnode_info && 
+                (MB_GET_NODE_STATE(pnode_info) < MB_SOCK_STATE_CONNECTED) &&
+                (MB_GET_NODE_STATE(pnode_info) >= MB_SOCK_STATE_RESOLVED)) {
                 if (((pnode_info->sock_id < 0) || !FD_ISSET(pnode_info->sock_id, &pdrv_ctx->conn_set))
                             && FD_ISSET(node, &pdrv_ctx->open_set)) {
                     DRIVER_SEND_EVENT(ctx, MB_EVENT_CONNECT, pnode_info->index);
@@ -496,66 +514,42 @@ MB_EVENT_HANDLER(mbm_on_connect)
             mb_drv_check_suspend_shutdown(ctx);
         }
     }
-    ESP_LOGD(TAG, "Opened/connected: %u, %u.", 
-                (unsigned)pdrv_ctx->mb_node_open_count, (unsigned)pdrv_ctx->node_conn_count);
-    if (pdrv_ctx->mb_node_open_count == pdrv_ctx->node_conn_count) {
-        if (pdrv_ctx->event_cbs.on_conn_done_cb) {
-            pdrv_ctx->event_cbs.on_conn_done_cb(pdrv_ctx->event_cbs.arg);
-        }
-        ESP_LOGI(TAG, "%p, Connected: %u, %u, start polling.", 
-                    ctx, (unsigned)pdrv_ctx->mb_node_open_count, (unsigned)pdrv_ctx->node_conn_count);
-    }
 }
 
 MB_EVENT_HANDLER(mbm_on_error)
 {
-    static int curr_fd = 0;
     port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
     mb_event_info_t *pevent_info = (mb_event_info_t *)data;
     mb_node_info_t *pnode_info = NULL;
     if (MB_CHECK_FD_RANGE(pevent_info->opt_fd)) {
         mb_drv_check_suspend_shutdown(ctx);
-        mb_status_flags_t status = mb_drv_wait_status_flag(pdrv_ctx, MB_FLAG_DISCONNECTED, 0);
+        mb_status_flags_t status = mb_drv_wait_status_flag(pdrv_ctx, MB_FLAG_DISCONNECTED, 1);
         if ((status & MB_FLAG_DISCONNECTED)) {
             ESP_LOGE(TAG, "%p, node: %d, is in disconnected state.", ctx, (int)pevent_info->opt_fd);
+            mb_drv_clear_status_flag(ctx, MB_FLAG_CONNECTED);
             return;
         }
-        curr_fd = pevent_info->opt_fd;
-        pnode_info = mb_drv_get_next_node_from_set(ctx, &curr_fd, &pdrv_ctx->conn_set);
-        if (pnode_info && (status > 0)) {
-            uint64_t last_read_div_us = esp_timer_get_time() - pnode_info->recv_time;
-            ESP_LOGD(TAG, "%p, slave: %d, sock: %d, IP:%s, check connection, time = %" PRId64 ", rcv_time: %" PRId64,
-                    ctx, (int)pnode_info->index, (int)pnode_info->sock_id, pnode_info->addr_info.ip_addr_str,
-                    (esp_timer_get_time() / 1000), pnode_info->recv_time / 1000);
-            if (last_read_div_us >= (uint64_t)(MB_RECONNECT_TIME_MS * 1000)) {
-                mb_drv_check_suspend_shutdown(ctx);
-                err_t err = port_check_alive(pnode_info, MB_RECONNECT_TIME_MS);
-                if (err < 0) {
-                    ESP_LOGD(TAG, "%p, slave: %d, sock: %d, inactive for %" PRId64 " [ms], reconnect...",
-                            ctx, (int)pnode_info->index, (int)pnode_info->sock_id,
-                            (last_read_div_us / 1000));
-                    MB_SET_NODE_STATE(pnode_info, MB_SOCK_STATE_OPENED);
-                    FD_CLR(pnode_info->sock_id, &pdrv_ctx->conn_set);
-                    port_close_connection(pnode_info);
-                    mb_drv_lock(ctx);
-                    pdrv_ctx->node_conn_count--;
-                    mb_drv_unlock(ctx);
-                    DRIVER_SEND_EVENT(ctx, MB_EVENT_CONNECT, pnode_info->index);
-                } else {
-                    curr_fd++;
-                }
-            } else {
-                ESP_LOGD(TAG, "%p, slave: %d, sock: %d, inactive for %" PRId64 " [ms], wait reconnection...",
-                            ctx, (int)pnode_info->index, (int)pnode_info->sock_id,
-                            (last_read_div_us / 1000));
+        int ret = mb_drv_check_node_state(pdrv_ctx, (int *)&pevent_info->opt_fd, MB_RECONNECT_TIME_MS);
+        if ((ret != ERR_OK) && (ret != ERR_TIMEOUT)) {
+            pnode_info = mb_drv_get_node(pdrv_ctx, pevent_info->opt_fd);
+            ESP_LOGW(TAG, "%p, "MB_NODE_FMT(", error handling."), ctx, (int)pnode_info->fd,
+                                            (int)pnode_info->sock_id, pnode_info->addr_info.ip_addr_str);
+            ESP_LOGE(TAG, "Node: %d, try to repair lost connection, err= %d", (int)pevent_info->opt_fd, ret);
+            FD_CLR(pnode_info->sock_id, &pdrv_ctx->conn_set);
+            mb_drv_lock(ctx);
+            if (pdrv_ctx->node_conn_count) {
+                pdrv_ctx->node_conn_count--;
             }
+            mb_drv_unlock(ctx);
+            port_close_connection(pnode_info);
+            DRIVER_SEND_EVENT(ctx, MB_EVENT_RESOLVE, pnode_info->index);
         }
     } else if (pevent_info->opt_fd < 0) {
         // send resolve event to all slaves
         for (int fd = 0; fd < pdrv_ctx->mb_node_open_count; fd++) {
             mb_drv_check_suspend_shutdown(ctx);
             mb_node_info_t *pslave = mb_drv_get_node(pdrv_ctx, fd);
-            if (pslave && (MB_GET_NODE_STATE(pslave) == MB_SOCK_STATE_OPENED) 
+            if (pslave && (MB_GET_NODE_STATE(pslave) == MB_SOCK_STATE_OPENED)
                 && FD_ISSET(pslave->index, &pdrv_ctx->open_set)) {
                 DRIVER_SEND_EVENT(ctx, MB_EVENT_RESOLVE, pslave->index);
             }
@@ -652,31 +646,32 @@ MB_EVENT_HANDLER(mbm_on_close)
     mb_event_info_t *pevent_info = (mb_event_info_t *)data;
     ESP_LOGD(TAG, "%s  %s, fd: %d", (char *)base, __func__, (int)pevent_info->opt_fd);
     port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
+    mb_node_info_t *pnode = NULL;
     // if close all sockets event is received
     if (pevent_info->opt_fd < 0) {
+        ESP_LOGD(TAG, "%p, Close all nodes...", ctx);
         (void)mb_drv_clear_status_flag(pdrv_ctx, MB_FLAG_DISCONNECTED);
         for (int fd = 0; fd < MB_MAX_FDS; fd++) {
-            mb_node_info_t *pslave = mb_drv_get_node(pdrv_ctx, fd);
-            if (pslave && (MB_GET_NODE_STATE(pslave) >= MB_SOCK_STATE_OPENED) 
-                    && FD_ISSET(pslave->index, &pdrv_ctx->open_set)) {
-                mb_drv_lock(ctx);
-                // Check connection and unregister slave
-                if ((pslave->sock_id > 0) && (FD_ISSET(pslave->sock_id, &pdrv_ctx->conn_set)) ) {
-                    FD_CLR(pslave->sock_id, &pdrv_ctx->conn_set);
-                    if (pdrv_ctx->node_conn_count) {
-                        pdrv_ctx->node_conn_count--;
-                    }
-                }
-                FD_CLR(pslave->index, &pdrv_ctx->open_set);
-                mb_drv_unlock(ctx);
-                // close the socket connection, if active
-                (void)port_close_connection(pslave);
-                // change slave state immediately to release from select
-                MB_SET_NODE_STATE(pslave, MB_SOCK_STATE_READY);
+            mb_node_info_t *pnode = mb_drv_get_node(pdrv_ctx, fd);
+            if (pnode && (MB_GET_NODE_STATE(pnode) >= MB_SOCK_STATE_OPENED)
+                    && FD_ISSET(pnode->index, &pdrv_ctx->open_set)) {
+                // Close node immediately
+                mb_drv_close(pdrv_ctx, fd);
+                MB_SET_NODE_STATE(pnode, MB_SOCK_STATE_READY);
+                ESP_LOGD(TAG, "%p, Close node %d, sock #%d.", ctx, fd, pnode->sock_id);
             }
         }
         (void)mb_drv_set_status_flag(pdrv_ctx, MB_FLAG_DISCONNECTED);
         mb_drv_check_suspend_shutdown(ctx);
+    } else if (MB_CHECK_FD_RANGE(pevent_info->opt_fd)) {
+        pnode = mb_drv_get_node(pdrv_ctx, pevent_info->opt_fd);
+        if (pnode && (MB_GET_NODE_STATE(pnode) >= MB_SOCK_STATE_OPENED)) {
+            ESP_LOGD(TAG, "%p, Close node %d, sock #%d, intentionally.", ctx, (int)pevent_info->opt_fd, pnode->sock_id);
+            if ((pnode->sock_id < 0) && FD_ISSET(pnode->sock_id, &pdrv_ctx->open_set)) {
+                mb_drv_close(pdrv_ctx, pevent_info->opt_fd);
+            }
+        }
+        mb_drv_check_suspend_shutdown(ctx);
     }
 }
 
diff --git a/modbus/mb_ports/tcp/port_tcp_slave.c b/modbus/mb_ports/tcp/port_tcp_slave.c
index 967f63f..5f7e67f 100644
--- a/modbus/mb_ports/tcp/port_tcp_slave.c
+++ b/modbus/mb_ports/tcp/port_tcp_slave.c
@@ -358,6 +358,8 @@ MB_EVENT_HANDLER(mbs_on_ready)
     {
         mb_drv_lock(ctx);
         pdrv_ctx->listen_sock_fd = listen_sock;
+        // so, all accepted sockets will inherit the keep-alive feature
+        (void)port_keep_alive(pdrv_ctx->listen_sock_fd);
         mb_drv_unlock(ctx);
         ESP_LOGI(TAG, "%s  %s: fd: %d, bind is done", (char *)base, __func__, (int)pevent_info->opt_fd);
     }
@@ -366,7 +368,6 @@ MB_EVENT_HANDLER(mbs_on_ready)
 MB_EVENT_HANDLER(mbs_on_open)
 {
     mb_event_info_t *pevent_info = (mb_event_info_t *)data;
-    //port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
     ESP_LOGD(TAG, "%s  %s: fd: %d", (char *)base, __func__, (int)pevent_info->opt_fd);
 }
 
@@ -380,9 +381,13 @@ MB_EVENT_HANDLER(mbs_on_connect)
         ESP_LOGD(TAG, "%s %s: fd: %d, is closed.", (char *)base, __func__, (int)pevent_info->opt_fd);
         return;
     }
+    (void)port_keep_alive(pnode->sock_id);
     mb_drv_lock(ctx);
     MB_SET_NODE_STATE(pnode, MB_SOCK_STATE_CONNECTED);
     FD_SET(pnode->sock_id, &pdrv_ctx->conn_set);
+    if (pdrv_ctx->node_conn_count < MB_MAX_FDS) {
+        pdrv_ctx->node_conn_count++;
+    }
     mb_drv_unlock(ctx);
 }
 
@@ -532,22 +537,12 @@ MB_EVENT_HANDLER(mbs_on_error)
         ESP_LOGD(TAG, "%s %s: fd: %d, is closed.", (char *)base, __func__, (int)pevent_info->opt_fd);
         return;
     }
-    port_close_connection(pnode);
-    mb_drv_lock(ctx);
-    // Check connection and unregister slave
-    if ((pnode->sock_id > 0) && (FD_ISSET(pnode->sock_id, &pdrv_ctx->conn_set)))
-    {
-        FD_CLR(pnode->sock_id, &pdrv_ctx->conn_set);
-        if (pdrv_ctx->node_conn_count)
-        {
-            pdrv_ctx->node_conn_count--;
-        }
+    // Check if the node is not alive for timeout
+    int ret = mb_drv_check_node_state(pdrv_ctx, (int *)&pevent_info->opt_fd, MB_TCP_EVENT_LOOP_TICK_MS);
+    if ((ret != ERR_OK) && (ret != ERR_TIMEOUT)) {
+        ESP_LOGE(TAG, "Node: #%d is not alive, err= %d", (int)pevent_info->opt_fd, ret);
+        mb_drv_close(pdrv_ctx, pevent_info->opt_fd);
     }
-    if (pnode->index && (FD_ISSET(pnode->index, &pdrv_ctx->open_set))) {
-        FD_CLR(pnode->index, &pdrv_ctx->open_set);
-    }
-    mb_drv_unlock(ctx);
-    mb_drv_close(pdrv_ctx, pevent_info->opt_fd);
 }
 
 MB_EVENT_HANDLER(mbs_on_close)
@@ -555,6 +550,7 @@ MB_EVENT_HANDLER(mbs_on_close)
     mb_event_info_t *pevent_info = (mb_event_info_t *)data;
     ESP_LOGD(TAG, "%s  %s, fd: %d", (char *)base, __func__, (int)pevent_info->opt_fd);
     port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
+    mb_node_info_t *pnode =NULL;
     // if close all sockets event is received
     if (pevent_info->opt_fd < 0)
     {
@@ -565,37 +561,40 @@ MB_EVENT_HANDLER(mbs_on_close)
             if (pnode && (MB_GET_NODE_STATE(pnode) >= MB_SOCK_STATE_OPENED)
                       && FD_ISSET(pnode->index, &pdrv_ctx->open_set))
             {
-                mb_drv_lock(ctx);
-                // Check connection and unregister slave
-                if ((pnode->sock_id > 0) && (FD_ISSET(pnode->sock_id, &pdrv_ctx->conn_set)))
-                {
-                    FD_CLR(pnode->sock_id, &pdrv_ctx->conn_set);
-                    if (pdrv_ctx->node_conn_count) {
-                        pdrv_ctx->node_conn_count--;
-                    }
-                }
-                FD_CLR(pnode->index, &pdrv_ctx->open_set);
-                mb_drv_unlock(ctx);
-                // close the socket connection, if active
-                (void)port_close_connection(pnode);
-                // change slave state immediately to release from select
-                MB_SET_NODE_STATE(pnode, MB_SOCK_STATE_READY);
+                mb_drv_close(pdrv_ctx, fd);
             }
         }
         (void)mb_drv_set_status_flag(pdrv_ctx, MB_FLAG_DISCONNECTED);
         mb_drv_check_suspend_shutdown(ctx);
+    } else if (MB_CHECK_FD_RANGE(pevent_info->opt_fd)) {
+        pnode = mb_drv_get_node(pdrv_ctx, pevent_info->opt_fd);
+        if (pnode && (MB_GET_NODE_STATE(pnode) >= MB_SOCK_STATE_OPENED)) {
+            if ((pnode->sock_id < 0) && FD_ISSET(pnode->sock_id, &pdrv_ctx->open_set)) {
+                mb_drv_close(ctx, pevent_info->opt_fd);
+            }
+        }
+        mb_drv_check_suspend_shutdown(ctx);
     }
 }
 
 MB_EVENT_HANDLER(mbs_on_timeout)
 {
     // Slave timeout triggered
-    mb_event_info_t *pevent_info = (mb_event_info_t *)data;
-    //port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
-    ESP_LOGD(TAG, "%s  %s: fd: %d", (char *)base, __func__, (int)pevent_info->opt_fd);
-    // Todo: add network diagnostic here (ping)? Keep empty for now.
-    //mb_drv_check_node_state(pdrv_ctx, UNDEF_FD);
+    //mb_event_info_t *pevent_info = (mb_event_info_t *)data;
+    port_driver_t *pdrv_ctx = MB_GET_DRV_PTR(ctx);
+    static int curr_fd = 0;
+    ESP_LOGD(TAG, "%s  %s: fd: %d, %d", (char *)base, __func__, (int)curr_fd, pdrv_ctx->node_conn_count);
     mb_drv_check_suspend_shutdown(ctx);
+    int ret = mb_drv_check_node_state(pdrv_ctx, &curr_fd, MB_RECONNECT_TIME_MS);
+    if ((ret != ERR_OK) && (ret != ERR_TIMEOUT)) {
+        ESP_LOGE(TAG, "Node: %d, connection lost, err= %d", curr_fd, ret);
+        mb_drv_close(pdrv_ctx, curr_fd);
+    }
+    if ((curr_fd + 1) >= (pdrv_ctx->node_conn_count)) {
+        curr_fd = 0;
+    } else {
+        curr_fd++;
+    }
 }
 
 #endif
\ No newline at end of file
diff --git a/modbus/mb_ports/tcp/port_tcp_slave.h b/modbus/mb_ports/tcp/port_tcp_slave.h
index e845bf5..86c8001 100644
--- a/modbus/mb_ports/tcp/port_tcp_slave.h
+++ b/modbus/mb_ports/tcp/port_tcp_slave.h
@@ -11,14 +11,6 @@
 #include "sys/time.h"
 #include "esp_netif.h"
 
-/* ----------------------- lwIP includes ------------------------------------*/
-// #include "lwip/opt.h"
-// #include "lwip/sys.h"
-// #include "lwip/err.h"
-// #include "lwip/sockets.h"
-// #include "lwip/netdb.h"
-// #include "net/if.h"
-
 #include "mb_common.h"
 #include "mb_frame.h"
 
diff --git a/modbus/mb_ports/tcp/port_tcp_utils.c b/modbus/mb_ports/tcp/port_tcp_utils.c
index c82a2c3..0909397 100644
--- a/modbus/mb_ports/tcp/port_tcp_utils.c
+++ b/modbus/mb_ports/tcp/port_tcp_utils.c
@@ -171,18 +171,18 @@ static int port_get_buf(mb_node_info_t *pinfo, uint8_t *pdst_buf, uint16_t len,
         if (errno == EINPROGRESS || errno == EAGAIN || errno == EWOULDBLOCK) {
             // Read timeout occurred, check the timeout and return
             return 0;
-        } else if (errno == ENOTCONN) {
-            ESP_LOGE(TAG, "socket(#%d)(%s) connection closed, ret=%d, errno=%d.", 
+        } else if ((errno == ENOTCONN) || (errno == ECONNRESET)) {
+            ESP_LOGD(TAG, "socket(#%d)(%s) connection closed, ret=%d, errno=%d.", 
                             pinfo->sock_id, pinfo->addr_info.ip_addr_str, ret, (int)errno);
             // Socket connection closed
             return ERR_CONN;
         } else {
             // Other error occurred during receiving
-            ESP_LOGE(TAG, "Socket(#%d)(%s) receive error, ret = %d, errno = %d(%s)",
+            ESP_LOGD(TAG, "Socket(#%d)(%s) receive error, ret = %d, errno = %d(%s)",
                         pinfo->sock_id, pinfo->addr_info.ip_addr_str, ret, (int)errno, strerror(errno));
             return -1;
         }
-    } 
+    }
     return ret;
 }
 
@@ -217,7 +217,7 @@ int port_read_packet(mb_node_info_t *pinfo)
         // the number of bytes left to complete the current response.
         temp = MB_TCP_MBAP_GET_FIELD(ptemp_buf, MB_TCP_LEN);
         if (temp > MB_TCP_BUFF_MAX_SIZE) {
-            ESP_LOGD("RCV", "Incorrect packet length: %d", temp);
+            ESP_LOGD(TAG, "Incorrect packet length: %d", temp);
             ESP_LOG_BUFFER_HEX_LEVEL(TAG, ptemp_buf, MB_TCP_FUNC, ESP_LOG_DEBUG);
             pinfo->recv_err = ERR_BUF;
             temp = MB_TCP_BUFF_MAX_SIZE; // read all remaining data from buffer
@@ -269,10 +269,37 @@ err_t port_set_blocking(mb_node_info_t *pinfo, bool is_blocking)
     return ERR_OK;
 }
 
-void port_keep_alive(mb_node_info_t *pinfo)
+int port_keep_alive(int sock)
 {
     int optval = 1;
-    setsockopt(pinfo->sock_id, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(optval));
+    // Enable the Keepalive feature in the LWIP stack
+    int ret = setsockopt(sock, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(optval));
+    if (ret != 0) {
+        ESP_LOGE(TAG, "Sock %d, set keep alive option fail, err= (%d).", sock, ret);
+        return -1;
+    }
+    //  The interval between the last data packet sent and the first keepalive probe (seconds)
+    optval = 1;
+    ret = setsockopt(sock, IPPROTO_TCP, TCP_KEEPIDLE, &optval, sizeof(optval));
+    if (ret != 0) {
+        ESP_LOGD(TAG, "Sock %d, set keep idle time fail, err = (%d).", sock, ret);
+        return -1;
+    }
+    // The interval between keepalive probes (send every second)
+    optval = 1;
+    ret = setsockopt(sock, IPPROTO_TCP, TCP_KEEPINTVL, &optval, sizeof(optval));
+    if (ret != 0) {
+        ESP_LOGD(TAG, "Sock %d, set keep alive probes interval fail, err = (%d).", sock, ret);
+        return -1;
+    }
+    // Set count of probes before timing out
+    optval = CONFIG_FMB_TCP_CONNECTION_TOUT_SEC;
+    ret = setsockopt(sock, IPPROTO_TCP, TCP_KEEPCNT, &optval, sizeof(optval));
+    if (ret != 0) {
+        ESP_LOGD(TAG, "Sock %d, set keep alive probes count fail., err = (%d).", sock, ret);
+        return -1;
+    }
+    return 0;
 }
 
 // Check connection for timeout helper
@@ -393,12 +420,12 @@ err_t port_connect(void *ctx, mb_node_info_t *pinfo)
         err = connect(pinfo->sock_id, (struct sockaddr *)pcur_addr->ai_addr, pcur_addr->ai_addrlen);
         if ((err < 0) && (errno == EINPROGRESS || errno == EALREADY)) {
             // The unblocking connect is pending (check status later) or already connected
-            ESP_LOGV(TAG, "Socket(#%d)(%s) connection is pending, errno %d (%s).",
+            ESP_LOGD(TAG, "Socket(#%d)(%s) connection is pending, errno %d (%s).",
                         pinfo->sock_id, str, (int)errno, strerror(errno));
 
-            // Set keep alive flag in socket options
-            port_keep_alive(pinfo);
-            err = port_check_alive(pinfo, MB_TCP_CONNECTION_TIMEOUT_MS);
+            // Set keepalive option
+            (void)port_keep_alive(pinfo->sock_id);
+            err = port_check_alive(pinfo, MB_TCP_CHECK_ALIVE_TOUT_MS);
             continue;
         } else if ((err < 0) && (errno == EISCONN)) {
             // Socket already connected
@@ -591,7 +618,7 @@ esp_err_t port_start_mdns_service(char **ppdns_name, bool is_master, int uid, vo
         strncpy(temp_str, *ppdns_name, strlen(*ppdns_name));
         *ppdns_name[strlen(*ppdns_name)] = '\0';
     } else {
-        if (snprintf(temp_str, sizeof(temp_str), "%s_%02X", MB_MDNS_INST_NAME(is_master), uid) <= 0) {
+        if (snprintf(temp_str, sizeof(temp_str), "%s_%02x", MB_MDNS_INST_NAME(is_master), uid) <= 0) {
             return ESP_ERR_INVALID_STATE;
         };
     }
@@ -743,7 +770,7 @@ int port_resolve_mdns_host(const char *host_name, char **paddr_str)
             // Try to resolve using AAAA query
             err =  mdns_query_aaaa(host_name, MB_MDNS_QUERY_TIME_MS, &addr.u_addr.ip6);
             if (err == ESP_ERR_NOT_FOUND) {
-                ESP_LOGE(TAG, "Host: %s, was not resolved!", host_name);
+                ESP_LOGE(TAG, "Node: %s, unable to resolve!", host_name);
                 return -1;
             }
             addr.type = ESP_IPADDR_TYPE_V6;
@@ -760,7 +787,7 @@ int port_resolve_mdns_host(const char *host_name, char **paddr_str)
         }
     }
     if (paddr_str) {
-        ESP_LOGD(TAG, "Host: %s, was resolved with IP: %s", host_name, pstr);
+        ESP_LOGD(TAG, "Node: %s, was resolved with IP: %s", host_name, pstr);
         *paddr_str = pstr;
     }
     return strlen(pstr);
@@ -834,6 +861,7 @@ int port_bind_addr(const char *pbind_ip, mb_addr_type_t addr_type, mb_comm_mode_
             if (listen(listen_sock_fd, MB_TCP_NET_LISTEN_BACKLOG) != 0)
             {
                 ESP_LOGE(TAG, "Error occurred during listen: errno=%u", (unsigned)errno);
+                shutdown(listen_sock_fd, SHUT_RDWR);
                 close(listen_sock_fd);
                 listen_sock_fd = UNDEF_FD;
                 continue;
diff --git a/modbus/mb_ports/tcp/port_tcp_utils.h b/modbus/mb_ports/tcp/port_tcp_utils.h
index 09ce892..16c9ebb 100644
--- a/modbus/mb_ports/tcp/port_tcp_utils.h
+++ b/modbus/mb_ports/tcp/port_tcp_utils.h
@@ -92,7 +92,7 @@ int port_enqueue_packet(QueueHandle_t queue, uint8_t *pbuf, uint16_t len);
 int port_dequeue_packet(QueueHandle_t queue, frame_entry_t* pframe_info);
 int port_read_packet(mb_node_info_t* pinfo);
 err_t port_set_blocking(mb_node_info_t* pinfo, bool is_blocking);
-void port_keep_alive(mb_node_info_t* pinfo);
+int port_keep_alive(int sock);
 err_t port_check_alive(mb_node_info_t* pinfo, uint32_t timeout_ms);
 err_t port_connect(void *ctx, mb_node_info_t* pinfo);
 bool port_close_connection(mb_node_info_t* pinfo);
diff --git a/test_apps/adapter_tests/main/idf_component.yml b/test_apps/adapter_tests/main/idf_component.yml
index 77cf471..a2ec2c8 100644
--- a/test_apps/adapter_tests/main/idf_component.yml
+++ b/test_apps/adapter_tests/main/idf_component.yml
@@ -1,6 +1,6 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../"
 
diff --git a/test_apps/cpp/modbus_serial_cpp_test/main/idf_component.yml b/test_apps/cpp/modbus_serial_cpp_test/main/idf_component.yml
index e6964ae..7f1161f 100644
--- a/test_apps/cpp/modbus_serial_cpp_test/main/idf_component.yml
+++ b/test_apps/cpp/modbus_serial_cpp_test/main/idf_component.yml
@@ -1,6 +1,6 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
 
diff --git a/test_apps/physical_tests/main/idf_component.yml b/test_apps/physical_tests/main/idf_component.yml
index a3c27db..bda0ed6 100644
--- a/test_apps/physical_tests/main/idf_component.yml
+++ b/test_apps/physical_tests/main/idf_component.yml
@@ -1,7 +1,7 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../"
   espressif/mdns:
     version: "^1.0.0"
diff --git a/test_apps/unit_tests/mb_controller_common/main/idf_component.yml b/test_apps/unit_tests/mb_controller_common/main/idf_component.yml
index e6964ae..7f1161f 100644
--- a/test_apps/unit_tests/mb_controller_common/main/idf_component.yml
+++ b/test_apps/unit_tests/mb_controller_common/main/idf_component.yml
@@ -1,6 +1,6 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
 
diff --git a/test_apps/unit_tests/mb_controller_mapping/main/idf_component.yml b/test_apps/unit_tests/mb_controller_mapping/main/idf_component.yml
index e6964ae..7f1161f 100644
--- a/test_apps/unit_tests/mb_controller_mapping/main/idf_component.yml
+++ b/test_apps/unit_tests/mb_controller_mapping/main/idf_component.yml
@@ -1,6 +1,6 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"
 
diff --git a/test_apps/unit_tests/mb_ext_types/main/idf_component.yml b/test_apps/unit_tests/mb_ext_types/main/idf_component.yml
index e6964ae..7f1161f 100644
--- a/test_apps/unit_tests/mb_ext_types/main/idf_component.yml
+++ b/test_apps/unit_tests/mb_ext_types/main/idf_component.yml
@@ -1,6 +1,6 @@
 dependencies:
   idf: ">=5.0"
   espressif/esp-modbus:
-    version: "^2.0.0"
+    version: "^2"
     override_path: "../../../../"