freemodbus: check/fix reinitialization issues (tcp master and slave examples)

components/freemodbus/modbus/tcp/mbtcp_m.c

@@ -91,8 +91,6 @@ eMBMasterTCPStart( void )
 void
 eMBMasterTCPStop( void )
 {
-    /* Make sure that no more clients are connected. */
-    vMBMasterTCPPortDisable( );
 }
 
 eMBErrorCode

components/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.c

@@ -37,6 +37,7 @@
 
 // The response time is average processing time + data transmission
 #define MB_RESPONSE_TIMEOUT pdMS_TO_TICKS(CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND)
+#define MB_TCP_CONNECTION_TOUT pdMS_TO_TICKS(CONFIG_FMB_TCP_CONNECTION_TOUT_SEC * 1000)
 
 static mb_master_interface_t* mbm_interface_ptr = NULL;
 
@@ -116,15 +117,23 @@ static esp_err_t mbc_tcp_master_start(void)
         result = (BOOL)xMBTCPPortMasterAddSlaveIp(*comm_ip_table);
         MB_MASTER_CHECK(result, ESP_ERR_INVALID_STATE, "mb stack add slave IP failed: %s.", *comm_ip_table);
     }
-    // Add end of list condition
+    // Init polling event handlers and wait before start polling
-    (void)xMBTCPPortMasterAddSlaveIp(NULL);
+    xMBTCPPortMasterWaitEvent(mbm_opts->mbm_event_group, (EventBits_t)MB_EVENT_STACK_STARTED, 1);
 
     status = eMBMasterEnable();
     MB_MASTER_CHECK((status == MB_ENOERR), ESP_ERR_INVALID_STATE,
             "mb stack set slave ID failure, eMBMasterEnable() returned (0x%x).", (uint32_t)status);
 
-    bool start = (bool)xMBTCPPortMasterWaitEvent(mbm_opts->mbm_event_group, (EventBits_t)MB_EVENT_STACK_STARTED);
+    // Send end of list condition to start connection phase
-    MB_MASTER_CHECK((start), ESP_ERR_INVALID_STATE, "mb stack start failed.");
+    (void)xMBTCPPortMasterAddSlaveIp(NULL);
+
+    // Wait for connection done event
+    bool start = (bool)xMBTCPPortMasterWaitEvent(mbm_opts->mbm_event_group,
+                                                    (EventBits_t)MB_EVENT_STACK_STARTED, MB_TCP_CONNECTION_TOUT);
+    MB_MASTER_CHECK((start), ESP_ERR_INVALID_STATE,
+                            "mb stack could not connect to slaves for %d seconds.",
+                            CONFIG_FMB_TCP_CONNECTION_TOUT_SEC);
+
     return ESP_OK;
 }
 
@@ -136,17 +145,19 @@ static esp_err_t mbc_tcp_master_destroy(void)
     MB_MASTER_CHECK((mbm_opts != NULL), ESP_ERR_INVALID_ARG, "mb incorrect options pointer.");
     eMBErrorCode mb_error = MB_ENOERR;
 
-    // Stop polling by clearing correspondent bit in the event group
-    xEventGroupClearBits(mbm_opts->mbm_event_group,
-                                    (EventBits_t)MB_EVENT_STACK_STARTED);
     // Disable and then destroy the Modbus stack
     mb_error = eMBMasterDisable();
     MB_MASTER_CHECK((mb_error == MB_ENOERR), ESP_ERR_INVALID_STATE, "mb stack disable failure.");
-    (void)vTaskDelete(mbm_opts->mbm_task_handle);
-    (void)vEventGroupDelete(mbm_opts->mbm_event_group);
     mb_error = eMBMasterClose();
     MB_MASTER_CHECK((mb_error == MB_ENOERR), ESP_ERR_INVALID_STATE,
             "mb stack close failure returned (0x%x).", (uint32_t)mb_error);
+    // Stop polling by clearing correspondent bit in the event group
+    xEventGroupClearBits(mbm_opts->mbm_event_group,
+                                    (EventBits_t)MB_EVENT_STACK_STARTED);
+    (void)vTaskDelete(mbm_opts->mbm_task_handle);
+    mbm_opts->mbm_task_handle = NULL;
+    (void)vEventGroupDelete(mbm_opts->mbm_event_group);
+    mbm_opts->mbm_event_group = NULL;
     free(mbm_interface_ptr); // free the memory allocated for options
     vMBPortSetMode((UCHAR)MB_PORT_INACTIVE);
     mbm_interface_ptr = NULL;

components/freemodbus/tcp_master/port/port_tcp_master.c

@@ -64,7 +64,7 @@
 
 #define MB_EVENT_REQ_ERR_MASK           ( EV_MASTER_PROCESS_SUCCESS )
 
-#define MB_EVENT_WAIT_TOUT_MS           ( 2000 )
+#define MB_EVENT_WAIT_TOUT_MS           ( 3000 )
 
 #define MB_TCP_READ_TICK_MS             ( 1 )
 #define MB_TCP_READ_BUF_RETRY_CNT       ( 4 )
@@ -76,6 +76,7 @@ void vMBPortEventClose( void );
 /* ----------------------- Static variables ---------------------------------*/
 static MbPortConfig_t xMbPortConfig;
 static EventGroupHandle_t xMasterEventHandle = NULL;
+static SemaphoreHandle_t xShutdownSemaphore = NULL;
 static EventBits_t xMasterEvent = 0;
 
 /* ----------------------- Static functions ---------------------------------*/
@@ -84,7 +85,7 @@ static void vMBTCPPortMasterTask(void *pvParameters);
 /* ----------------------- Begin implementation -----------------------------*/
 
 // Waits for stack start event to start Modbus event processing
-BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEvent)
+BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEvent, USHORT usTimeout)
 {
     xMasterEventHandle = xEventHandle;
     xMasterEvent = xEvent;
@@ -92,7 +93,7 @@ BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEve
                                                (BaseType_t)(xEvent),
                                                pdFALSE, // do not clear start bit
                                                pdFALSE,
-                                               portMAX_DELAY);
+                                               usTimeout);
     return (BOOL)(status & xEvent);
 }
 
@@ -156,6 +157,8 @@ static void vMBTCPPortMasterStartPoll(void)
         if (!(xFlags & xMasterEvent)) {
             ESP_LOGE(MB_TCP_MASTER_PORT_TAG, "Fail to start TCP stack.");
         }
+    } else {
+        ESP_LOGE(MB_TCP_MASTER_PORT_TAG, "Fail to start polling. Incorrect event handle...");
     }
 }
 
@@ -169,6 +172,8 @@ static void vMBTCPPortMasterStopPoll(void)
         if (!(xFlags & xMasterEvent)) {
             ESP_LOGE(MB_TCP_MASTER_PORT_TAG, "Fail to stop polling.");
         }
+    } else {
+        ESP_LOGE(MB_TCP_MASTER_PORT_TAG, "Fail to stop polling. Incorrect event handle...");
     }
 }
 
@@ -185,6 +190,14 @@ static void vMBTCPPortMasterMStoTimeVal(USHORT usTimeoutMs, struct timeval *tv)
     tv->tv_usec = (usTimeoutMs - (tv->tv_sec * 1000)) * 1000;
 }
 
+static void xMBTCPPortMasterCheckShutdown(void) {
+    // First check if the task is not flagged for shutdown
+    if (xShutdownSemaphore) {
+        xSemaphoreGive(xShutdownSemaphore);
+        vTaskDelete(NULL);
+    }
+}
+
 static BOOL xMBTCPPortMasterCloseConnection(MbSlaveInfo_t* pxInfo)
 {
     if (!pxInfo) {
@@ -256,6 +269,7 @@ static int xMBTCPPortMasterGetBuf(MbSlaveInfo_t* pxInfo, UCHAR* pucDstBuf, USHOR
 
     // Receive data from connected client
     while (usBytesLeft > 0) {
+        xMBTCPPortMasterCheckShutdown();
         // none blocking read from socket with timeout
         xLength = recv(pxInfo->xSockId, pucBuf, usBytesLeft, MSG_DONTWAIT);
         if (xLength < 0) {
@@ -341,6 +355,9 @@ static int vMBTCPPortMasterReadPacket(MbSlaveInfo_t* pxInfo)
 
 static err_t xMBTCPPortMasterSetNonBlocking(MbSlaveInfo_t* pxInfo)
 {
+    if (!pxInfo) {
+        return ERR_CONN;
+    }
     // Set non blocking attribute for socket
     ULONG ulFlags = fcntl(pxInfo->xSockId, F_GETFL);
     if (fcntl(pxInfo->xSockId, F_SETFL, ulFlags | O_NONBLOCK) == -1) {
@@ -465,6 +482,10 @@ BOOL xMBTCPPortMasterAddSlaveIp(const CHAR* pcIpStr)
 // Unblocking connect function
 static err_t xMBTCPPortMasterConnect(MbSlaveInfo_t* pxInfo)
 {
+    if (!pxInfo) {
+        return ERR_CONN;
+    }
+
     err_t xErr = ERR_OK;
     CHAR cStr[128];
     CHAR* pcStr = NULL;
@@ -623,7 +644,8 @@ static void vMBTCPPortMasterTask(void *pvParameters)
 
     // Register each slave in the connection info structure
     while (1) {
-        BaseType_t xStatus = xQueueReceive(xMbPortConfig.xConnectQueue, (void*)&pcAddrStr, portMAX_DELAY);
+        BaseType_t xStatus = xQueueReceive(xMbPortConfig.xConnectQueue, (void*)&pcAddrStr, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS));
+        xMBTCPPortMasterCheckShutdown();
         if (xStatus != pdTRUE) {
             ESP_LOGE(MB_TCP_MASTER_PORT_TAG, "Fail to register slave IP.");
         } else {
@@ -724,10 +746,14 @@ static void vMBTCPPortMasterTask(void *pvParameters)
                                                             pxInfo->pcIpAddr, xErr);
                         break;
                 }
+                if (pxInfo) {
                     pxInfo->xError = xErr;
                 }
+                xMBTCPPortMasterCheckShutdown();
+            }
         }
         ESP_LOGI(MB_TCP_MASTER_PORT_TAG, "Connected %d slaves, start polling...", usSlaveConnCnt);
+
         vMBTCPPortMasterStartPoll(); // Send event to start stack
 
         // Slave receive data loop
@@ -745,6 +771,7 @@ static void vMBTCPPortMasterTask(void *pvParameters)
                 ESP_LOGE(MB_TCP_MASTER_PORT_TAG, "Incorrect connection options for slave index: %d.",
                                             xMbPortConfig.ucCurSlaveIndex);
                 vMBTCPPortMasterStopPoll();
+                xMBTCPPortMasterCheckShutdown();
                 break; // incorrect slave descriptor, reconnect.
             }
             xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
@@ -760,6 +787,7 @@ static void vMBTCPPortMasterTask(void *pvParameters)
                 xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
                 // Wait completion of last transaction
                 xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime + 1));
+                xMBTCPPortMasterCheckShutdown();
                 continue;
             } else if (xRes < 0) {
                 // Select error (slave connection or r/w failure).
@@ -770,6 +798,7 @@ static void vMBTCPPortMasterTask(void *pvParameters)
                 xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime));
                 // Stop polling process
                 vMBTCPPortMasterStopPoll();
+                xMBTCPPortMasterCheckShutdown();
                 // Check disconnected slaves, do not need a result just to print information.
                 xMBTCPPortMasterCheckConnState(&xConnSet);
                 break;
@@ -802,6 +831,7 @@ static void vMBTCPPortMasterTask(void *pvParameters)
                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr, xErr);
                         // Stop polling process
                         vMBTCPPortMasterStopPoll();
+                        xMBTCPPortMasterCheckShutdown();
                         // Check disconnected slaves, do not need a result just to print information.
                         xMBTCPPortMasterCheckConnState(&xConnSet);
                         break;
@@ -818,6 +848,7 @@ static void vMBTCPPortMasterTask(void *pvParameters)
                                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr, xTime);
                 }
             }
+            xMBTCPPortMasterCheckShutdown();
         } // while(usMbSlaveInfoCount)
     } // while (1)
     vTaskDelete(NULL);
@@ -826,18 +857,6 @@ static void vMBTCPPortMasterTask(void *pvParameters)
 extern void vMBMasterPortEventClose(void);
 extern void vMBMasterPortTimerClose(void);
 
-void
-vMBMasterTCPPortClose(void)
-{
-    (void)vTaskDelete(xMbPortConfig.xMbTcpTaskHandle);
-    (void)vMBMasterTCPPortDisable();
-    free(xMbPortConfig.pxMbSlaveInfo);
-    vQueueDelete(xMbPortConfig.xConnectQueue);
-    vMBMasterPortTimerClose();
-    // Release resources for the event queue.
-    vMBMasterPortEventClose();
-}
-
 void
 vMBMasterTCPPortDisable(void)
 {
@@ -854,6 +873,29 @@ vMBMasterTCPPortDisable(void)
     }
 }
 
+void
+vMBMasterTCPPortClose(void)
+{
+    // Try to exit the task gracefully, so select could release its internal callbacks
+    // that were allocated on the stack of the task we're going to delete
+    xShutdownSemaphore = xSemaphoreCreateBinary();
+    // if no semaphore (alloc issues) or couldn't acquire it, just delete the task
+    if (xShutdownSemaphore == NULL || xSemaphoreTake(xShutdownSemaphore, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS)) != pdTRUE) {
+        ESP_LOGW(MB_TCP_MASTER_PORT_TAG, "Modbus port task couldn't exit gracefully within timeout -> abruptly deleting the task.");
+        vTaskDelete(xMbPortConfig.xMbTcpTaskHandle);
+    }
+    if (xShutdownSemaphore) {
+        vSemaphoreDelete(xShutdownSemaphore);
+        xShutdownSemaphore = NULL;
+    }
+    vMBMasterTCPPortDisable();
+    free(xMbPortConfig.pxMbSlaveInfo);
+    vQueueDelete(xMbPortConfig.xConnectQueue);
+    vMBMasterPortTimerClose();
+    // Release resources for the event queue.
+    vMBMasterPortEventClose();
+}
+
 BOOL
 xMBMasterTCPPortGetRequest( UCHAR ** ppucMBTCPFrame, USHORT * usTCPLength )
 {

components/freemodbus/tcp_master/port/port_tcp_master.h

@@ -104,10 +104,11 @@ BOOL xMBTCPPortMasterAddSlaveIp(const CHAR* pcIpStr);
  *
  * @param xEventHandle Master event handle
  * @param xEvent event mask to start Modbus stack FSM
+ * @param usTimeout - timeout in ticks to wait for stack to start
  *
  * @return TRUE if stack started, else FALSE
  */
-BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEvent);
+BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEvent, USHORT usTimeout);
 
 /**
  * Set network options for Master port

examples/protocols/modbus/tcp/mb_tcp_master/main/tcp_master.c

@@ -365,6 +365,7 @@ static void master_destroy_slave_list(char** table)
     for (int i = 0; ((i < MB_DEVICE_COUNT) && table[i] != NULL); i++) {
         if (table[i]) {
             free(table[i]);
+            table[i] = NULL;
         }
     }
 }
@@ -500,48 +501,47 @@ static void master_operation_func(void *arg)
     }
     ESP_LOGI(MASTER_TAG, "Destroy master...");
     vTaskDelay(100);
-    ESP_ERROR_CHECK(mbc_master_destroy());
 }
 
-// Modbus master initialization
+static esp_err_t init_services(mb_tcp_addr_type_t ip_addr_type)
-static esp_err_t master_init(void)
 {
     esp_err_t result = nvs_flash_init();
     if (result == ESP_ERR_NVS_NO_FREE_PAGES || result == ESP_ERR_NVS_NEW_VERSION_FOUND) {
       ESP_ERROR_CHECK(nvs_flash_erase());
       result = nvs_flash_init();
     }
-    ESP_ERROR_CHECK(result);
+    MASTER_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
-    esp_netif_init();
+                            "nvs_flash_init fail, returns(0x%x).",
-    ESP_ERROR_CHECK(esp_event_loop_create_default());
+                            (uint32_t)result);
-
+    result = esp_netif_init();
+    MASTER_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                            "esp_netif_init fail, returns(0x%x).",
+                            (uint32_t)result);
+    result = esp_event_loop_create_default();
+    MASTER_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                            "esp_event_loop_create_default fail, returns(0x%x).",
+                            (uint32_t)result);
 #if CONFIG_MB_MDNS_IP_RESOLVER
     // Start mdns service and register device
     master_start_mdns_service();
 #endif
-
     // This helper function configures Wi-Fi or Ethernet, as selected in menuconfig.
     // Read "Establishing Wi-Fi or Ethernet Connection" section in
     // examples/protocols/README.md for more information about this function.
-    ESP_ERROR_CHECK(example_connect());
+    result = example_connect();
-
+    MASTER_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
-    ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
+                                "example_connect fail, returns(0x%x).",
-
+                                (uint32_t)result);
-    mb_communication_info_t comm_info = { 0 };
+#if CONFIG_EXAMPLE_CONNECT_WIFI
-    comm_info.ip_port = MB_TCP_PORT;
+   result = esp_wifi_set_ps(WIFI_PS_NONE);
-#if !CONFIG_EXAMPLE_CONNECT_IPV6
+   MASTER_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
-    comm_info.ip_addr_type = MB_IPV4;
+                                   "esp_wifi_set_ps fail, returns(0x%x).",
-#else
+                                   (uint32_t)result);
-    comm_info.ip_addr_type = MB_IPV6;
 #endif
-    comm_info.ip_mode = MB_MODE_TCP;
-    comm_info.ip_addr = (void*)slave_ip_address_table;
-    comm_info.ip_netif_ptr = (void*)get_example_netif();
-
 #if CONFIG_MB_MDNS_IP_RESOLVER
     int res = 0;
     for (int retry = 0; (res < num_device_parameters) && (retry < 10); retry++) {
-        res = master_query_slave_service("_modbus", "_tcp", comm_info.ip_addr_type);
+        res = master_query_slave_service("_modbus", "_tcp", ip_addr_type);
     }
     if (res < num_device_parameters) {
         ESP_LOGE(MASTER_TAG, "Could not resolve one or more slave IP addresses, resolved: %d out of %d.", res, num_device_parameters );
@@ -555,9 +555,40 @@ static esp_err_t master_init(void)
         ESP_LOGI(MASTER_TAG, "Configured %d IP addresse(s).", ip_cnt);
     } else {
         ESP_LOGE(MASTER_TAG, "Fail to get IP address from stdin. Continue.");
+        return ESP_ERR_NOT_FOUND;
     }
 #endif
+    return ESP_OK;
+}
 
+static esp_err_t destroy_services(void)
+{
+    esp_err_t err = ESP_OK;
+#if CONFIG_MB_MDNS_IP_RESOLVER
+    master_destroy_slave_list(slave_ip_address_table);
+#endif
+    err = example_disconnect();
+    MASTER_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                   "example_disconnect fail, returns(0x%x).",
+                                   (uint32_t)err);
+    err = esp_event_loop_delete_default();
+    MASTER_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                       "esp_event_loop_delete_default fail, returns(0x%x).",
+                                       (uint32_t)err);
+    err = esp_netif_deinit();
+    MASTER_CHECK((err == ESP_OK || err == ESP_ERR_NOT_SUPPORTED), ESP_ERR_INVALID_STATE,
+                                        "esp_netif_deinit fail, returns(0x%x).",
+                                        (uint32_t)err);
+    err = nvs_flash_deinit();
+    MASTER_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                "nvs_flash_deinit fail, returns(0x%x).",
+                                (uint32_t)err);
+    return err;
+}
+
+// Modbus master initialization
+static esp_err_t master_init(mb_communication_info_t* comm_info)
+{
     void* master_handler = NULL;
 
     esp_err_t err = mbc_master_init_tcp(&master_handler);
@@ -567,7 +598,7 @@ static esp_err_t master_init(void)
                             "mb controller initialization fail, returns(0x%x).",
                             (uint32_t)err);
 
-    err = mbc_master_setup((void*)&comm_info);
+    err = mbc_master_setup((void*)comm_info);
     MASTER_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
                             "mb controller setup fail, returns(0x%x).",
                             (uint32_t)err);
@@ -586,15 +617,37 @@ static esp_err_t master_init(void)
     return err;
 }
 
+static esp_err_t master_destroy(void)
+{
+    esp_err_t err = mbc_master_destroy();
+    MASTER_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                "mbc_master_destroy fail, returns(0x%x).",
+                                (uint32_t)err);
+    ESP_LOGI(MASTER_TAG, "Modbus master stack destroy...");
+    return err;
+}
+
 void app_main(void)
 {
-    // Initialization of device peripheral and objects
+    mb_tcp_addr_type_t ip_addr_type;
-    ESP_ERROR_CHECK(master_init());
+#if !CONFIG_EXAMPLE_CONNECT_IPV6
-    vTaskDelay(10);
+    ip_addr_type = MB_IPV4;
+#else
+    ip_addr_type = MB_IPV6;
+#endif
+    ESP_ERROR_CHECK(init_services(ip_addr_type));
+
+    mb_communication_info_t comm_info = { 0 };
+    comm_info.ip_port = MB_TCP_PORT;
+    comm_info.ip_addr_type = ip_addr_type;
+    comm_info.ip_mode = MB_MODE_TCP;
+    comm_info.ip_addr = (void*)slave_ip_address_table;
+    comm_info.ip_netif_ptr = (void*)get_example_netif();
+
+    ESP_ERROR_CHECK(master_init(&comm_info));
+    vTaskDelay(50);
 
     master_operation_func(NULL);
-#if CONFIG_MB_MDNS_IP_RESOLVER
+    ESP_ERROR_CHECK(master_destroy());
-    master_destroy_slave_list(slave_ip_address_table);
+    ESP_ERROR_CHECK(destroy_services());
-#endif
-
 }

examples/protocols/modbus/tcp/mb_tcp_slave/main/tcp_slave.c

@@ -49,6 +49,12 @@
 
 #define SLAVE_TAG "SLAVE_TEST"
 
+#define SLAVE_CHECK(a, ret_val, str, ...) \
+    if (!(a)) { \
+        ESP_LOGE(SLAVE_TAG, "%s(%u): " str, __FUNCTION__, __LINE__, ##__VA_ARGS__); \
+        return (ret_val); \
+    }
+
 static portMUX_TYPE param_lock = portMUX_INITIALIZER_UNLOCKED;
 
 #if CONFIG_MB_MDNS_IP_RESOLVER
@@ -87,7 +93,7 @@ static inline char* gen_host_name_str(char* service_name, char* name)
     return name;
 }
 
-static void start_mdns_service()
+static void start_mdns_service(void)
 {
     char temp_str[32] = {0};
     uint8_t sta_mac[6] = {0};
@@ -115,6 +121,11 @@ static void start_mdns_service()
     ESP_ERROR_CHECK( mdns_service_txt_item_set("_modbus", "_tcp", "mb_id", gen_id_str("\0", temp_str)));
 }
 
+static void stop_mdns_service(void)
+{
+    mdns_free();
+}
+
 #endif
 
 // Set register values into known state
@@ -152,104 +163,9 @@ static void setup_reg_data(void)
     input_reg_params.input_data7 = 4.78;
 }
 
-// An example application of Modbus slave. It is based on freemodbus stack.
+static void slave_operation_func(void *arg)
-// See deviceparams.h file for more information about assigned Modbus parameters.
-// These parameters can be accessed from main application and also can be changed
-// by external Modbus master host.
-void app_main(void)
 {
-    esp_err_t result = nvs_flash_init();
-    if (result == ESP_ERR_NVS_NO_FREE_PAGES || result == ESP_ERR_NVS_NEW_VERSION_FOUND) {
-      ESP_ERROR_CHECK(nvs_flash_erase());
-      result = nvs_flash_init();
-    }
-    ESP_ERROR_CHECK(result);
-    esp_netif_init();
-    ESP_ERROR_CHECK(esp_event_loop_create_default());
-
-#if CONFIG_MB_MDNS_IP_RESOLVER
-    start_mdns_service();
-#endif
-
-    /* This helper function configures Wi-Fi or Ethernet, as selected in menuconfig.
-     * Read "Establishing Wi-Fi or Ethernet Connection" section in
-     * examples/protocols/README.md for more information about this function.
-     */
-    ESP_ERROR_CHECK(example_connect());
-
-    ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
-
-    // Set UART log level
-    esp_log_level_set(SLAVE_TAG, ESP_LOG_INFO);
-    void* mbc_slave_handler = NULL;
-
-    ESP_ERROR_CHECK(mbc_slave_init_tcp(&mbc_slave_handler)); // Initialization of Modbus controller
-
     mb_param_info_t reg_info; // keeps the Modbus registers access information
-    mb_register_area_descriptor_t reg_area; // Modbus register area descriptor structure
-
-    mb_communication_info_t comm_info = { 0 };
-    comm_info.ip_port = MB_TCP_PORT_NUMBER;
-#if !CONFIG_EXAMPLE_CONNECT_IPV6
-    comm_info.ip_addr_type = MB_IPV4;
-#else
-    comm_info.ip_addr_type = MB_IPV6;
-#endif
-    comm_info.ip_mode = MB_MODE_TCP;
-    comm_info.ip_addr = NULL;
-    comm_info.ip_netif_ptr = (void*)get_example_netif();
-    // Setup communication parameters and start stack
-    ESP_ERROR_CHECK(mbc_slave_setup((void*)&comm_info));
-
-    // The code below initializes Modbus register area descriptors
-    // for Modbus Holding Registers, Input Registers, Coils and Discrete Inputs
-    // Initialization should be done for each supported Modbus register area according to register map.
-    // When external master trying to access the register in the area that is not initialized
-    // by mbc_slave_set_descriptor() API call then Modbus stack
-    // will send exception response for this register area.
-    reg_area.type = MB_PARAM_HOLDING; // Set type of register area
-    reg_area.start_offset = MB_REG_HOLDING_START_AREA0; // Offset of register area in Modbus protocol
-    reg_area.address = (void*)&holding_reg_params.holding_data0; // Set pointer to storage instance
-    reg_area.size = sizeof(float) << 2; // Set the size of register storage instance
-    ESP_ERROR_CHECK(mbc_slave_set_descriptor(reg_area));
-
-    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
-    ESP_ERROR_CHECK(mbc_slave_set_descriptor(reg_area));
-
-    // Initialization of Input Registers area
-    reg_area.type = MB_PARAM_INPUT;
-    reg_area.start_offset = MB_REG_INPUT_START_AREA0;
-    reg_area.address = (void*)&input_reg_params.input_data0;
-    reg_area.size = sizeof(float) << 2;
-    ESP_ERROR_CHECK(mbc_slave_set_descriptor(reg_area));
-    // Initialization of Input Registers area
-    reg_area.type = MB_PARAM_INPUT;
-    reg_area.start_offset = MB_REG_INPUT_START_AREA1;
-    reg_area.address = (void*)&input_reg_params.input_data4;
-    reg_area.size = sizeof(float) << 2;
-    ESP_ERROR_CHECK(mbc_slave_set_descriptor(reg_area));
-
-    // Initialization of Coils register area
-    reg_area.type = MB_PARAM_COIL;
-    reg_area.start_offset = MB_REG_COILS_START;
-    reg_area.address = (void*)&coil_reg_params;
-    reg_area.size = sizeof(coil_reg_params);
-    ESP_ERROR_CHECK(mbc_slave_set_descriptor(reg_area));
-
-    // Initialization of Discrete Inputs register area
-    reg_area.type = MB_PARAM_DISCRETE;
-    reg_area.start_offset = MB_REG_DISCRETE_INPUT_START;
-    reg_area.address = (void*)&discrete_reg_params;
-    reg_area.size = sizeof(discrete_reg_params);
-    ESP_ERROR_CHECK(mbc_slave_set_descriptor(reg_area));
-
-    setup_reg_data(); // Set values into known state
-
-    // Starts of modbus controller and stack
-    ESP_ERROR_CHECK(mbc_slave_start());
 
     ESP_LOGI(SLAVE_TAG, "Modbus slave stack initialized.");
     ESP_LOGI(SLAVE_TAG, "Start modbus test...");
@@ -310,8 +226,202 @@ void app_main(void)
     // Destroy of Modbus controller on alarm
     ESP_LOGI(SLAVE_TAG,"Modbus controller destroyed.");
     vTaskDelay(100);
-    ESP_ERROR_CHECK(mbc_slave_destroy());
+}
-#if CONFIG_MB_MDNS_IP_RESOLVER
+
-    mdns_free();
+static esp_err_t init_services(void)
-#endif
+{
+    esp_err_t result = nvs_flash_init();
+    if (result == ESP_ERR_NVS_NO_FREE_PAGES || result == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+      ESP_ERROR_CHECK(nvs_flash_erase());
+      result = nvs_flash_init();
+    }
+    SLAVE_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                            "nvs_flash_init fail, returns(0x%x).",
+                            (uint32_t)result);
+    result = esp_netif_init();
+    SLAVE_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                            "esp_netif_init fail, returns(0x%x).",
+                            (uint32_t)result);
+    result = esp_event_loop_create_default();
+    SLAVE_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                            "esp_event_loop_create_default fail, returns(0x%x).",
+                            (uint32_t)result);
+#if CONFIG_MB_MDNS_IP_RESOLVER
+    // Start mdns service and register device
+    start_mdns_service();
+#endif
+    // This helper function configures Wi-Fi or Ethernet, as selected in menuconfig.
+    // Read "Establishing Wi-Fi or Ethernet Connection" section in
+    // examples/protocols/README.md for more information about this function.
+    result = example_connect();
+    SLAVE_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                                "example_connect fail, returns(0x%x).",
+                                (uint32_t)result);
+#if CONFIG_EXAMPLE_CONNECT_WIFI
+    result = esp_wifi_set_ps(WIFI_PS_NONE);
+    SLAVE_CHECK((result == ESP_OK), ESP_ERR_INVALID_STATE,
+                                   "esp_wifi_set_ps fail, returns(0x%x).",
+                                   (uint32_t)result);
+#endif
+    return ESP_OK;
+}
+
+static esp_err_t destroy_services(void)
+{
+    esp_err_t err = ESP_OK;
+
+    err = example_disconnect();
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                   "example_disconnect fail, returns(0x%x).",
+                                   (uint32_t)err);
+    err = esp_event_loop_delete_default();
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                       "esp_event_loop_delete_default fail, returns(0x%x).",
+                                       (uint32_t)err);
+    err = esp_netif_deinit();
+    SLAVE_CHECK((err == ESP_OK || err == ESP_ERR_NOT_SUPPORTED), ESP_ERR_INVALID_STATE,
+                                        "esp_netif_deinit fail, returns(0x%x).",
+                                        (uint32_t)err);
+    err = nvs_flash_deinit();
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                "nvs_flash_deinit fail, returns(0x%x).",
+                                (uint32_t)err);
+#if CONFIG_MB_MDNS_IP_RESOLVER
+    stop_mdns_service();
+#endif
+    return err;
+}
+
+// Modbus slave initialization
+static esp_err_t slave_init(mb_communication_info_t* comm_info)
+{
+    mb_register_area_descriptor_t reg_area; // Modbus register area descriptor structure
+
+    void* slave_handler = NULL;
+
+    // Initialization of Modbus controller
+    esp_err_t err = mbc_slave_init_tcp(&slave_handler);
+    SLAVE_CHECK((err == ESP_OK && slave_handler != NULL), ESP_ERR_INVALID_STATE,
+                                "mb controller initialization fail.");
+
+    comm_info->ip_addr = NULL; // Bind to any address
+    comm_info->ip_netif_ptr = (void*)get_example_netif();
+
+    // Setup communication parameters and start stack
+    err = mbc_slave_setup((void*)comm_info);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                        "mbc_slave_setup fail, returns(0x%x).",
+                                        (uint32_t)err);
+
+    // The code below initializes Modbus register area descriptors
+    // for Modbus Holding Registers, Input Registers, Coils and Discrete Inputs
+    // Initialization should be done for each supported Modbus register area according to register map.
+    // When external master trying to access the register in the area that is not initialized
+    // by mbc_slave_set_descriptor() API call then Modbus stack
+    // will send exception response for this register area.
+    reg_area.type = MB_PARAM_HOLDING; // Set type of register area
+    reg_area.start_offset = MB_REG_HOLDING_START_AREA0; // Offset of register area in Modbus protocol
+    reg_area.address = (void*)&holding_reg_params.holding_data0; // Set pointer to storage instance
+    reg_area.size = sizeof(float) << 2; // Set the size of register storage instance
+    err = mbc_slave_set_descriptor(reg_area);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                    "mbc_slave_set_descriptor fail, returns(0x%x).",
+                                    (uint32_t)err);
+
+    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
+    err = mbc_slave_set_descriptor(reg_area);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                    "mbc_slave_set_descriptor fail, returns(0x%x).",
+                                    (uint32_t)err);
+
+    // Initialization of Input Registers area
+    reg_area.type = MB_PARAM_INPUT;
+    reg_area.start_offset = MB_REG_INPUT_START_AREA0;
+    reg_area.address = (void*)&input_reg_params.input_data0;
+    reg_area.size = sizeof(float) << 2;
+    err = mbc_slave_set_descriptor(reg_area);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                        "mbc_slave_set_descriptor fail, returns(0x%x).",
+                                        (uint32_t)err);
+    reg_area.type = MB_PARAM_INPUT;
+    reg_area.start_offset = MB_REG_INPUT_START_AREA1;
+    reg_area.address = (void*)&input_reg_params.input_data4;
+    reg_area.size = sizeof(float) << 2;
+    err = mbc_slave_set_descriptor(reg_area);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                        "mbc_slave_set_descriptor fail, returns(0x%x).",
+                                        (uint32_t)err);
+
+    // Initialization of Coils register area
+    reg_area.type = MB_PARAM_COIL;
+    reg_area.start_offset = MB_REG_COILS_START;
+    reg_area.address = (void*)&coil_reg_params;
+    reg_area.size = sizeof(coil_reg_params);
+    err = mbc_slave_set_descriptor(reg_area);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                    "mbc_slave_set_descriptor fail, returns(0x%x).",
+                                    (uint32_t)err);
+
+    // Initialization of Discrete Inputs register area
+    reg_area.type = MB_PARAM_DISCRETE;
+    reg_area.start_offset = MB_REG_DISCRETE_INPUT_START;
+    reg_area.address = (void*)&discrete_reg_params;
+    reg_area.size = sizeof(discrete_reg_params);
+    err = mbc_slave_set_descriptor(reg_area);
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                    "mbc_slave_set_descriptor fail, returns(0x%x).",
+                                    (uint32_t)err);
+
+    // Set values into known state
+    setup_reg_data();
+
+    // Starts of modbus controller and stack
+    err = mbc_slave_start();
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                        "mbc_slave_start fail, returns(0x%x).",
+                                        (uint32_t)err);
+    vTaskDelay(5);
+    return err;
+}
+
+static esp_err_t slave_destroy(void)
+{
+    esp_err_t err = mbc_slave_destroy();
+    SLAVE_CHECK((err == ESP_OK), ESP_ERR_INVALID_STATE,
+                                "mbc_slave_destroy fail, returns(0x%x).",
+                                (uint32_t)err);
+    return err;
+}
+
+// An example application of Modbus slave. It is based on freemodbus stack.
+// See deviceparams.h file for more information about assigned Modbus parameters.
+// These parameters can be accessed from main application and also can be changed
+// by external Modbus master host.
+void app_main(void)
+{
+    ESP_ERROR_CHECK(init_services());
+
+    // Set UART log level
+    esp_log_level_set(SLAVE_TAG, ESP_LOG_INFO);
+
+    mb_communication_info_t comm_info = { 0 };
+
+#if !CONFIG_EXAMPLE_CONNECT_IPV6
+    comm_info.ip_addr_type = MB_IPV4;
+#else
+    comm_info.ip_addr_type = MB_IPV6;
+#endif
+    comm_info.ip_mode = MB_MODE_TCP;
+
+    comm_info.ip_port = MB_TCP_PORT_NUMBER;
+    ESP_ERROR_CHECK(slave_init(&comm_info));
+
+    // The Modbus slave logic is located in this function (user handling of Modbus)
+    slave_operation_func(NULL);
+
+    ESP_ERROR_CHECK(slave_destroy());
+    ESP_ERROR_CHECK(destroy_services());
 }