From 77e42b309ab529df6f3aeefe1a610930fc1833b7 Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Tue, 26 Apr 2022 13:08:05 +0200
Subject: [PATCH] Extracted custom patches on idf modbus implementation to run
 taskless

---
 CMakeLists.txt                                |  2 +-
 freemodbus/common/esp_modbus_master_serial.c  |  4 +-
 freemodbus/common/esp_modbus_master_tcp.c     |  4 +-
 freemodbus/common/esp_modbus_slave.c          | 16 +++---
 freemodbus/common/esp_modbus_slave_serial.c   |  6 +--
 freemodbus/common/esp_modbus_slave_tcp.c      |  6 +--
 .../{ => include}/esp_modbus_callbacks.h      |  0
 freemodbus/common/include/esp_modbus_common.h | 11 ++--
 freemodbus/common/include/esp_modbus_master.h |  5 +-
 freemodbus/common/include/esp_modbus_slave.h  | 39 +++++---------
 freemodbus/{modbus => common}/include/mb.h    |  2 +-
 freemodbus/{modbus => common}/include/mb_m.h  |  0
 freemodbus/common/include/mb_temp.h           | 41 ++++++++++++++
 freemodbus/common/{ => include}/mbc_master.h  |  0
 freemodbus/common/{ => include}/mbc_slave.h   |  4 +-
 .../{modbus => common}/include/mbconfig.h     | 19 ++-----
 .../{modbus => common}/include/mbframe.h      |  0
 .../{modbus => common}/include/mbfunc.h       |  0
 .../{modbus => common}/include/mbport.h       | 11 +---
 .../{modbus => common}/include/mbproto.h      |  0
 .../{modbus => common}/include/mbutils.h      |  0
 freemodbus/{port => common/include}/port.h    | 12 -----
 freemodbus/modbus/ascii/mbascii.c             | 29 +++-------
 freemodbus/modbus/ascii/mbascii_m.c           | 49 +++++++----------
 freemodbus/modbus/mb.c                        |  4 +-
 freemodbus/modbus/mb_m.c                      | 14 ++---
 freemodbus/modbus/rtu/mbcrc.c                 |  2 +-
 freemodbus/modbus/rtu/mbrtu.c                 | 32 ++++-------
 freemodbus/modbus/rtu/mbrtu_m.c               | 47 +++++-----------
 freemodbus/port/port.c                        |  8 ---
 freemodbus/port/portevent.c                   |  4 +-
 freemodbus/port/portserial.c                  | 24 ++-------
 freemodbus/port/portserial_m.c                | 24 +--------
 freemodbus/port/porttimer.c                   |  2 +-
 freemodbus/port/porttimer_m.c                 |  2 +-
 freemodbus/sdkconfig.rename                   | 18 +++++++
 .../modbus_controller/mbc_serial_master.c     | 39 ++++++++------
 .../modbus_controller/mbc_serial_master.h     |  2 +-
 .../modbus_controller/mbc_serial_slave.c      | 52 ++++++++++--------
 .../modbus_controller/mbc_serial_slave.h      |  2 +-
 .../modbus_controller/mbc_tcp_master.c        | 48 ++++++++++-------
 .../modbus_controller/mbc_tcp_master.h        |  2 +-
 .../modbus_controller/mbc_tcp_slave.c         | 53 +++++++++++--------
 .../modbus_controller/mbc_tcp_slave.h         |  2 +-
 freemodbus/tcp_slave/port/port_tcp_slave.c    |  6 ---
 45 files changed, 299 insertions(+), 348 deletions(-)
 rename freemodbus/common/{ => include}/esp_modbus_callbacks.h (100%)
 rename freemodbus/{modbus => common}/include/mb.h (99%)
 rename freemodbus/{modbus => common}/include/mb_m.h (100%)
 create mode 100644 freemodbus/common/include/mb_temp.h
 rename freemodbus/common/{ => include}/mbc_master.h (100%)
 rename freemodbus/common/{ => include}/mbc_slave.h (97%)
 rename freemodbus/{modbus => common}/include/mbconfig.h (96%)
 rename freemodbus/{modbus => common}/include/mbframe.h (100%)
 rename freemodbus/{modbus => common}/include/mbfunc.h (100%)
 rename freemodbus/{modbus => common}/include/mbport.h (95%)
 rename freemodbus/{modbus => common}/include/mbproto.h (100%)
 rename freemodbus/{modbus => common}/include/mbutils.h (100%)
 rename freemodbus/{port => common/include}/port.h (95%)
 create mode 100644 freemodbus/sdkconfig.rename

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d2a64a..2cc98c7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,7 +46,7 @@ set(srcs
 set(include_dirs common/include)
 
 set(priv_include_dirs common port modbus modbus/ascii modbus/functions
-                      modbus/rtu modbus/tcp modbus/include)
+                      modbus/rtu modbus/tcp)
 
 list(APPEND priv_include_dirs serial_slave/port serial_slave/modbus_controller
      serial_master/port serial_master/modbus_controller
diff --git a/freemodbus/common/esp_modbus_master_serial.c b/freemodbus/common/esp_modbus_master_serial.c
index 9ea355b..48c0c7f 100644
--- a/freemodbus/common/esp_modbus_master_serial.c
+++ b/freemodbus/common/esp_modbus_master_serial.c
@@ -12,14 +12,14 @@
 /**
  * Initialization of Modbus master serial
  */
-esp_err_t mbc_master_init(mb_port_type_t port_type, void** handler)
+esp_err_t mbc_master_init(mb_port_type_t port_type, void** handler, bool start_controller_task)
 {
     void* port_handler = NULL;
     esp_err_t error = ESP_ERR_NOT_SUPPORTED;
     switch(port_type)
     {
     case MB_PORT_SERIAL_MASTER:
-        error = mbc_serial_master_create(&port_handler);
+        error = mbc_serial_master_create(&port_handler, start_controller_task);
         break;
     default:
         return ESP_ERR_NOT_SUPPORTED;
diff --git a/freemodbus/common/esp_modbus_master_tcp.c b/freemodbus/common/esp_modbus_master_tcp.c
index 8350f73..087dc87 100644
--- a/freemodbus/common/esp_modbus_master_tcp.c
+++ b/freemodbus/common/esp_modbus_master_tcp.c
@@ -11,10 +11,10 @@
 /**
  * Initialization of Modbus TCP Master controller interface
  */
-esp_err_t mbc_master_init_tcp(void** handler)
+esp_err_t mbc_master_init_tcp(void** handler, bool start_controller_task)
 {
     void* port_handler = NULL;
-    esp_err_t error = mbc_tcp_master_create(&port_handler);
+    esp_err_t error = mbc_tcp_master_create(&port_handler, start_controller_task);
 
     if ((port_handler != NULL) && (error == ESP_OK)) {
         mbc_master_init_iface(port_handler);
diff --git a/freemodbus/common/esp_modbus_slave.c b/freemodbus/common/esp_modbus_slave.c
index ba5c29b..68a31ff 100644
--- a/freemodbus/common/esp_modbus_slave.c
+++ b/freemodbus/common/esp_modbus_slave.c
@@ -76,9 +76,9 @@ static void mbc_slave_free_descriptors(void) {
     }
 }
 
-void mbc_slave_init_iface(void* handler)
+void mbc_slave_init_iface(mb_slave_interface_t* handler)
 {
-    slave_interface_ptr = (mb_slave_interface_t*) handler;
+    slave_interface_ptr = handler;
     mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts;
     // Initialize list head for register areas
     LIST_INIT(&mbs_opts->mbs_area_descriptors[MB_PARAM_INPUT]);
@@ -476,7 +476,8 @@ eMBErrorCode eMBRegDiscreteCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usN
     if (slave_interface_ptr->slave_reg_cb_discrete) {
         error = slave_interface_ptr->slave_reg_cb_discrete(pucRegBuffer, usAddress, usNDiscrete);
     } else {
-        error = mbc_reg_discrete_slave_cb(pucRegBuffer, usAddress, usNDiscrete);
+        error = MB_EIO;
+        ESP_LOGE(TAG, "slave_reg_cb_discrete invalid");
     }
 
     return error;
@@ -493,7 +494,8 @@ eMBErrorCode eMBRegCoilsCB(UCHAR* pucRegBuffer, USHORT usAddress,
     if (slave_interface_ptr->slave_reg_cb_coils) {
         error = slave_interface_ptr->slave_reg_cb_coils(pucRegBuffer, usAddress, usNCoils, eMode);
     } else {
-        error = mbc_reg_coils_slave_cb(pucRegBuffer, usAddress, usNCoils, eMode);
+        error = MB_EIO;
+        ESP_LOGE(TAG, "slave_reg_cb_coils invalid");
     }
     return error;
 }
@@ -509,7 +511,8 @@ eMBErrorCode eMBRegHoldingCB(UCHAR * pucRegBuffer, USHORT usAddress,
     if (slave_interface_ptr->slave_reg_cb_holding) {
         error = slave_interface_ptr->slave_reg_cb_holding(pucRegBuffer, usAddress, usNRegs, eMode);
     } else {
-        error = mbc_reg_holding_slave_cb(pucRegBuffer, usAddress, usNRegs, eMode);
+        error = MB_EIO;
+        ESP_LOGE(TAG, "slave_reg_cb_holding invalid");
     }
     return error;
 }
@@ -524,7 +527,8 @@ eMBErrorCode eMBRegInputCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNReg
     if (slave_interface_ptr->slave_reg_cb_input) {
         error = slave_interface_ptr->slave_reg_cb_input(pucRegBuffer, usAddress, usNRegs);
     } else {
-        error = mbc_reg_input_slave_cb(pucRegBuffer, usAddress, usNRegs);
+        error = MB_EIO;
+        ESP_LOGE(TAG, "slave_reg_cb_input invalid");
     }
     return error;
 }
diff --git a/freemodbus/common/esp_modbus_slave_serial.c b/freemodbus/common/esp_modbus_slave_serial.c
index eb08dfd..63e4056 100644
--- a/freemodbus/common/esp_modbus_slave_serial.c
+++ b/freemodbus/common/esp_modbus_slave_serial.c
@@ -13,7 +13,7 @@
 /**
  * Initialization of Modbus Serial slave controller
  */
-esp_err_t mbc_slave_init(mb_port_type_t port_type, void** handler)
+esp_err_t mbc_slave_init(mb_port_type_t port_type, mb_slave_interface_t **handler, bool start_controller_task)
 {
     void* port_handler = NULL;
     esp_err_t error = ESP_ERR_NOT_SUPPORTED;
@@ -21,13 +21,13 @@ esp_err_t mbc_slave_init(mb_port_type_t port_type, void** handler)
     {
         case MB_PORT_SERIAL_SLAVE:
             // Call constructor function of actual port implementation
-            error = mbc_serial_slave_create(&port_handler);
+            error = mbc_serial_slave_create(&port_handler, start_controller_task);
             break;
         default:
             return ESP_ERR_NOT_SUPPORTED;
     }
     if ((port_handler != NULL) && (error == ESP_OK)) {
-        mbc_slave_init_iface(port_handler);
+        mbc_slave_init_iface((mb_slave_interface_t*)port_handler);
         *handler = port_handler;
     }
     return error;
diff --git a/freemodbus/common/esp_modbus_slave_tcp.c b/freemodbus/common/esp_modbus_slave_tcp.c
index c3b3ee6..29f132b 100644
--- a/freemodbus/common/esp_modbus_slave_tcp.c
+++ b/freemodbus/common/esp_modbus_slave_tcp.c
@@ -11,13 +11,13 @@
 /**
  * Initialization of Modbus TCP Slave controller
  */
-esp_err_t mbc_slave_init_tcp(void** handler)
+esp_err_t mbc_slave_init_tcp(mb_slave_interface_t** handler, bool start_controller_task)
 {
     void* port_handler = NULL;
-    esp_err_t error = mbc_tcp_slave_create(&port_handler);
+    esp_err_t error = mbc_tcp_slave_create(&port_handler, start_controller_task);
 
     if ((port_handler != NULL) && (error == ESP_OK)) {
-        mbc_slave_init_iface(port_handler);
+        mbc_slave_init_iface((mb_slave_interface_t*)port_handler);
         *handler = port_handler;
     }
     return  error;
diff --git a/freemodbus/common/esp_modbus_callbacks.h b/freemodbus/common/include/esp_modbus_callbacks.h
similarity index 100%
rename from freemodbus/common/esp_modbus_callbacks.h
rename to freemodbus/common/include/esp_modbus_callbacks.h
diff --git a/freemodbus/common/include/esp_modbus_common.h b/freemodbus/common/include/esp_modbus_common.h
index f443286..aa4a06f 100644
--- a/freemodbus/common/include/esp_modbus_common.h
+++ b/freemodbus/common/include/esp_modbus_common.h
@@ -15,7 +15,6 @@ extern "C" {
 
 #if __has_include("esp_check.h")
 #include "esp_check.h"
-#include "esp_log.h"
 
 #define MB_RETURN_ON_FALSE(a, err_code, tag, format, ...) ESP_RETURN_ON_FALSE(a, err_code, tag, format __VA_OPT__(,) __VA_ARGS__)
 
@@ -25,7 +24,7 @@ extern "C" {
 
 #define MB_RETURN_ON_FALSE(a, err_code, tag, format, ...) do {                                         \
         if (!(a)) {                                                                              \
-            ESP_LOGE(tag, "%s(%d): " format, __FUNCTION__, __LINE__ __VA_OPT__(,) __VA_ARGS__);        \
+            MB_LOGE(tag, "%s(%d): " format, __FUNCTION__, __LINE__ __VA_OPT__(,) __VA_ARGS__);        \
             return err_code;                                                                               \
         }                                                                                                  \
 } while(0)
@@ -36,10 +35,10 @@ extern "C" {
 #define MB_CONTROLLER_PRIORITY              (CONFIG_FMB_PORT_TASK_PRIO - 1)    // priority of MB controller task
 
 // Default port defines
-#define MB_DEVICE_ADDRESS   (1)                 // Default slave device address in Modbus
-#define MB_DEVICE_SPEED     (115200)            // Default Modbus speed for now hard defined
+#define MB_DEVICE_ADDRESS   (1)             // Default slave device address in Modbus
+#define MB_DEVICE_SPEED     (115200)        // Default Modbus speed for now hard defined
 #define MB_UART_PORT        (UART_NUM_MAX - 1)  // Default UART port number
-#define MB_PAR_INFO_TOUT    (10)                // Timeout for get parameter info
+#define MB_PAR_INFO_TOUT    (10)            // Timeout for get parameter info
 #define MB_PARITY_NONE      (UART_PARITY_DISABLE)
 
 // The Macros below handle the endianness while transfer N byte data into buffer
@@ -153,7 +152,7 @@ typedef union {
 /**
  * common interface method types
  */
-typedef esp_err_t (*iface_init)(void**);        /*!< Interface method init */
+typedef esp_err_t (*iface_init)(void**,bool);   /*!< Interface method init */
 typedef esp_err_t (*iface_destroy)(void);       /*!< Interface method destroy */
 typedef esp_err_t (*iface_setup)(void*);        /*!< Interface method setup */
 typedef esp_err_t (*iface_start)(void);         /*!< Interface method start */
diff --git a/freemodbus/common/include/esp_modbus_master.h b/freemodbus/common/include/esp_modbus_master.h
index d11ade7..12a1cff 100644
--- a/freemodbus/common/include/esp_modbus_master.h
+++ b/freemodbus/common/include/esp_modbus_master.h
@@ -9,7 +9,6 @@
 
 #include <stdint.h>                 // for standard int types definition
 #include <stddef.h>                 // for NULL and std defines
-#include "soc/soc.h"                // for BITN definitions
 #include "esp_modbus_common.h"      // for common types
 
 #ifdef __cplusplus
@@ -114,7 +113,7 @@ typedef struct {
  *     - ESP_ERR_NOT_SUPPORTED  Port type not supported
  *     - ESP_ERR_INVALID_STATE  Initialization failure
  */
-esp_err_t mbc_master_init_tcp(void** handler);
+esp_err_t mbc_master_init_tcp(void** handler, bool start_controller_task);
 
 /**
  * @brief Initialize Modbus Master controller and stack for Serial port
@@ -127,7 +126,7 @@ esp_err_t mbc_master_init_tcp(void** handler);
  *     - ESP_ERR_NOT_SUPPORTED  Port type not supported
  *     - ESP_ERR_INVALID_STATE  Initialization failure
  */
-esp_err_t mbc_master_init(mb_port_type_t port_type, void** handler);
+esp_err_t mbc_master_init(mb_port_type_t port_type, void** handler, bool start_controller_task);
 
 /**
  * @brief Initialize Modbus Master controller interface handle
diff --git a/freemodbus/common/include/esp_modbus_slave.h b/freemodbus/common/include/esp_modbus_slave.h
index 7d79b51..923ef02 100644
--- a/freemodbus/common/include/esp_modbus_slave.h
+++ b/freemodbus/common/include/esp_modbus_slave.h
@@ -10,10 +10,11 @@
 // Public interface header for slave
 #include <stdint.h>                 // for standard int types definition
 #include <stddef.h>                 // for NULL and std defines
-#include "soc/soc.h"                // for BITN definitions
 #include "freertos/FreeRTOS.h"      // for task creation and queues access
 #include "freertos/event_groups.h"  // for event groups
 #include "esp_modbus_common.h"      // for common types
+#include "mb.h"
+#include "mbc_slave.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -25,27 +26,6 @@ extern "C" {
         if (!(con)) { ESP_LOGE(TAG, "assert errno:%d, errno_str: !(%s)", errno, strerror(errno)); assert(0 && #con); } \
     } while (0)
 
-/**
- * @brief Parameter access event information type
- */
-typedef struct {
-    uint32_t time_stamp;                    /*!< Timestamp of Modbus Event (uS)*/
-    uint16_t mb_offset;                     /*!< Modbus register offset */
-    mb_event_group_t type;                  /*!< Modbus event type */
-    uint8_t* address;                       /*!< Modbus data storage address */
-    size_t size;                            /*!< Modbus event register size (number of registers)*/
-} mb_param_info_t;
-
-/**
- * @brief Parameter storage area descriptor
- */
-typedef struct {
-    uint16_t start_offset;                  /*!< Modbus start address for area descriptor */
-    mb_param_type_t type;                   /*!< Type of storage area descriptor */
-    void* address;                          /*!< Instance address for storage area descriptor */
-    size_t size;                            /*!< Instance size for area descriptor (bytes) */
-} mb_register_area_descriptor_t;
-
 /**
  * @brief Initialize Modbus Slave controller and stack for TCP port
  *
@@ -56,7 +36,7 @@ typedef struct {
  *     - ESP_ERR_NOT_SUPPORTED  Port type not supported
  *     - ESP_ERR_INVALID_STATE  Initialization failure
  */
-esp_err_t mbc_slave_init_tcp(void** handler);
+esp_err_t mbc_slave_init_tcp(mb_slave_interface_t** handler, bool start_controller_task);
 
 /**
  * @brief Initialize Modbus Slave controller and stack for Serial port
@@ -69,14 +49,14 @@ esp_err_t mbc_slave_init_tcp(void** handler);
  *     - ESP_ERR_NOT_SUPPORTED  Port type not supported
  *     - ESP_ERR_INVALID_STATE  Initialization failure
  */
-esp_err_t mbc_slave_init(mb_port_type_t port_type, void** handler);
+esp_err_t mbc_slave_init(mb_port_type_t port_type, mb_slave_interface_t **handler, bool start_controller_task);
 
 /**
  * @brief Initialize Modbus Slave controller interface handle
  *
  * @param[in] handler - pointer to slave interface data structure
  */
-void mbc_slave_init_iface(void* handler);
+void mbc_slave_init_iface(mb_slave_interface_t* handler);
 
 /**
  * @brief Destroy Modbus controller and stack
@@ -141,6 +121,15 @@ esp_err_t mbc_slave_get_param_info(mb_param_info_t* reg_info, uint32_t timeout);
  */
 esp_err_t mbc_slave_set_descriptor(mb_register_area_descriptor_t descr_data);
 
+// Callback function for reading of MB Discrete Input Registers
+eMBErrorCode mbc_reg_discrete_slave_cb(UCHAR* reg_buffer, USHORT address, USHORT n_discrete);
+// Callback function for reading of MB Coils Registers
+eMBErrorCode mbc_reg_coils_slave_cb(UCHAR* reg_buffer, USHORT address, USHORT n_coils, eMBRegisterMode mode);
+// Callback function for reading of MB Holding Registers
+// Executed by stack when request to read/write holding registers is received
+eMBErrorCode mbc_reg_holding_slave_cb(UCHAR * reg_buffer, USHORT address, USHORT n_regs, eMBRegisterMode mode);
+// Callback function for reading of MB Input Registers
+eMBErrorCode mbc_reg_input_slave_cb(UCHAR * reg_buffer, USHORT address, USHORT n_regs);
 #ifdef __cplusplus
 }
 #endif
diff --git a/freemodbus/modbus/include/mb.h b/freemodbus/common/include/mb.h
similarity index 99%
rename from freemodbus/modbus/include/mb.h
rename to freemodbus/common/include/mb.h
index 964e3e1..bb38834 100644
--- a/freemodbus/modbus/include/mb.h
+++ b/freemodbus/common/include/mb.h
@@ -225,7 +225,7 @@ eMBErrorCode    eMBDisable( void );
  *   returns eMBErrorCode::MB_EILLSTATE. Otherwise it returns
  *   eMBErrorCode::MB_ENOERR.
  */
-eMBErrorCode    eMBPoll( void );
+eMBErrorCode    eMBPoll( TickType_t xTicksToWait );
 
 /*! \ingroup modbus
  * \brief Configure the slave id of the device.
diff --git a/freemodbus/modbus/include/mb_m.h b/freemodbus/common/include/mb_m.h
similarity index 100%
rename from freemodbus/modbus/include/mb_m.h
rename to freemodbus/common/include/mb_m.h
diff --git a/freemodbus/common/include/mb_temp.h b/freemodbus/common/include/mb_temp.h
new file mode 100644
index 0000000..a478e87
--- /dev/null
+++ b/freemodbus/common/include/mb_temp.h
@@ -0,0 +1,41 @@
+/* Copyright 2018 Espressif Systems (Shanghai) PTE LTD
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef _MB_TEMP_H
+#define _MB_TEMP_H
+
+#include "esp_modbus_common.h"
+
+/**
+ * @brief Parameter access event information type
+ */
+typedef struct {
+    uint32_t time_stamp;                    /*!< Timestamp of Modbus Event (uS)*/
+    uint16_t mb_offset;                     /*!< Modbus register offset */
+    mb_event_group_t type;                  /*!< Modbus event type */
+    uint8_t* address;                       /*!< Modbus data storage address */
+    size_t size;                            /*!< Modbus event register size (number of registers)*/
+} mb_param_info_t;
+
+/**
+ * @brief Parameter storage area descriptor
+ */
+typedef struct {
+    uint16_t start_offset;                  /*!< Modbus start address for area descriptor */
+    mb_param_type_t type;                   /*!< Type of storage area descriptor */
+    void* address;                          /*!< Instance address for storage area descriptor */
+    size_t size;                            /*!< Instance size for area descriptor (bytes) */
+} mb_register_area_descriptor_t;
+
+#endif
diff --git a/freemodbus/common/mbc_master.h b/freemodbus/common/include/mbc_master.h
similarity index 100%
rename from freemodbus/common/mbc_master.h
rename to freemodbus/common/include/mbc_master.h
diff --git a/freemodbus/common/mbc_slave.h b/freemodbus/common/include/mbc_slave.h
similarity index 97%
rename from freemodbus/common/mbc_slave.h
rename to freemodbus/common/include/mbc_slave.h
index 5b54038..7bffcb7 100644
--- a/freemodbus/common/mbc_slave.h
+++ b/freemodbus/common/include/mbc_slave.h
@@ -12,9 +12,11 @@
 #include "sys/queue.h"      // for list
 #include "esp_log.h"        // for log write
 #include "string.h"         // for strerror()
+#include "freertos/event_groups.h"
 
-#include "esp_modbus_slave.h"    // for public type defines
+//#include "esp_modbus_slave.h"    // for public type defines
 #include "esp_modbus_callbacks.h"   // for callback functions
+#include "mb_temp.h"
 
 /* ----------------------- Defines ------------------------------------------*/
 #define MB_INST_MIN_SIZE                    (2) // The minimal size of Modbus registers area in bytes
diff --git a/freemodbus/modbus/include/mbconfig.h b/freemodbus/common/include/mbconfig.h
similarity index 96%
rename from freemodbus/modbus/include/mbconfig.h
rename to freemodbus/common/include/mbconfig.h
index 268b748..c98acbd 100644
--- a/freemodbus/modbus/include/mbconfig.h
+++ b/freemodbus/common/include/mbconfig.h
@@ -41,12 +41,11 @@
 
 #include "sdkconfig.h" // for KConfig options
 
-#if __has_include("esp_idf_version.h")
-#include "esp_idf_version.h"
-#endif
+#include "port.h"
 
 #ifdef __cplusplus
-PR_BEGIN_EXTERN_C
+//PR_BEGIN_EXTERN_C
+extern "C" {
 #endif
 /* ----------------------- Defines ------------------------------------------*/
 /*! \defgroup modbus_cfg Modbus Configuration
@@ -77,15 +76,6 @@ PR_BEGIN_EXTERN_C
 #error "None of Modbus communication mode is enabled. Please enable one of (ASCII, RTU, TCP) mode in Kconfig."
 #endif
 
-#ifdef ESP_IDF_VERSION
-
-#if (ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(4, 4, 0))
-// Features supported from 4.4
-#define MB_TIMER_SUPPORTS_ISR_DISPATCH_METHOD 1
-#endif
-
-#endif
-
 /*! \brief This option defines the number of data bits per ASCII character.
  *
  * A parity bit is added before the stop bit which keeps the actual byte size at 10 bits.
@@ -171,7 +161,8 @@ PR_BEGIN_EXTERN_C
 
 /*! @} */
 #ifdef __cplusplus
-    PR_END_EXTERN_C
+//    PR_END_EXTERN_C
+    }
 #endif
 
 #if MB_MASTER_RTU_ENABLED || MB_MASTER_ASCII_ENABLED || MB_MASTER_TCP_ENABLED
diff --git a/freemodbus/modbus/include/mbframe.h b/freemodbus/common/include/mbframe.h
similarity index 100%
rename from freemodbus/modbus/include/mbframe.h
rename to freemodbus/common/include/mbframe.h
diff --git a/freemodbus/modbus/include/mbfunc.h b/freemodbus/common/include/mbfunc.h
similarity index 100%
rename from freemodbus/modbus/include/mbfunc.h
rename to freemodbus/common/include/mbfunc.h
diff --git a/freemodbus/modbus/include/mbport.h b/freemodbus/common/include/mbport.h
similarity index 95%
rename from freemodbus/modbus/include/mbport.h
rename to freemodbus/common/include/mbport.h
index acaf908..6475e81 100644
--- a/freemodbus/modbus/include/mbport.h
+++ b/freemodbus/common/include/mbport.h
@@ -112,7 +112,7 @@ BOOL            xMBPortEventInit( void );
 
 BOOL            xMBPortEventPost( eMBEventType eEvent );
 
-BOOL            xMBPortEventGet(  /*@out@ */ eMBEventType * eEvent );
+BOOL            xMBPortEventGet(  /*@out@ */ eMBEventType * eEvent, TickType_t xTicksToWait );
 
 #if MB_MASTER_RTU_ENABLED || MB_MASTER_ASCII_ENABLED || MB_MASTER_TCP_ENABLED
 BOOL            xMBMasterPortEventInit( void );
@@ -145,10 +145,6 @@ BOOL            xMBPortSerialGetByte( CHAR * pucByte );
 
 BOOL            xMBPortSerialPutByte( CHAR ucByte );
 
-BOOL            xMBSerialPortGetRequest( UCHAR **ppucMBSerialFrame, USHORT * pusSerialLength ) __attribute__ ((weak));
-
-BOOL            xMBSerialPortSendResponse( UCHAR *pucMBSerialFrame, USHORT usSerialLength ) __attribute__ ((weak));
-
 #if MB_MASTER_RTU_ENABLED || MB_MASTER_ASCII_ENABLED
 BOOL            xMBMasterPortSerialInit( UCHAR ucPort, ULONG ulBaudRate,
                                    UCHAR ucDataBits, eMBParity eParity );
@@ -162,11 +158,6 @@ void            vMBMasterPortSerialEnable( BOOL xRxEnable, BOOL xTxEnable );
 BOOL            xMBMasterPortSerialGetByte( CHAR * pucByte );
 
 BOOL            xMBMasterPortSerialPutByte( CHAR ucByte );
-
-BOOL            xMBMasterSerialPortGetResponse( UCHAR **ppucMBSerialFrame, USHORT * usSerialLength );
-
-BOOL            xMBMasterSerialPortSendRequest( UCHAR *pucMBSerialFrame, USHORT usSerialLength );
-
 #endif
 
 /* ----------------------- Timers functions ---------------------------------*/
diff --git a/freemodbus/modbus/include/mbproto.h b/freemodbus/common/include/mbproto.h
similarity index 100%
rename from freemodbus/modbus/include/mbproto.h
rename to freemodbus/common/include/mbproto.h
diff --git a/freemodbus/modbus/include/mbutils.h b/freemodbus/common/include/mbutils.h
similarity index 100%
rename from freemodbus/modbus/include/mbutils.h
rename to freemodbus/common/include/mbutils.h
diff --git a/freemodbus/port/port.h b/freemodbus/common/include/port.h
similarity index 95%
rename from freemodbus/port/port.h
rename to freemodbus/common/include/port.h
index b5d8858..42ae780 100644
--- a/freemodbus/port/port.h
+++ b/freemodbus/common/include/port.h
@@ -38,18 +38,8 @@
 #define PORT_COMMON_H_
 
 #include "freertos/FreeRTOS.h"
-#include "freertos/queue.h"         // for queue
-
 #include "esp_log.h"                // for ESP_LOGE macro
 #include "esp_timer.h"
-#include "driver/uart.h"            // for uart_event_t
-
-#if __has_include("driver/gptimer.h")
-#include "driver/gptimer.h"
-#else
-#include "driver/timer.h"
-#endif
-
 #include "mbconfig.h"
 
 #define INLINE                      inline
@@ -184,8 +174,6 @@ void prvvMBTCPLogFrame( const CHAR * pucMsg, UCHAR * pucFrame, USHORT usFrameLen
 void vMBPortSetMode( UCHAR ucMode );
 UCHAR ucMBPortGetMode( void );
 
-BOOL xMBPortSerialWaitEvent(QueueHandle_t xMbUartQueue, uart_event_t* pxEvent, ULONG xTimeout);
-
 #ifdef __cplusplus
 PR_END_EXTERN_C
 #endif /* __cplusplus */
diff --git a/freemodbus/modbus/ascii/mbascii.c b/freemodbus/modbus/ascii/mbascii.c
index 4400c25..462a641 100644
--- a/freemodbus/modbus/ascii/mbascii.c
+++ b/freemodbus/modbus/ascii/mbascii.c
@@ -151,33 +151,26 @@ eMBErrorCode
 eMBASCIIReceive( UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * pusLength )
 {
     eMBErrorCode    eStatus = MB_ENOERR;
-    UCHAR          *pucMBASCIIFrame = ( UCHAR* ) ucASCIIBuf;
-    USHORT          usFrameLength = usRcvBufferPos;
 
-    if( xMBSerialPortGetRequest( &pucMBASCIIFrame, &usFrameLength ) == FALSE )
-    {
-        return MB_EIO;
-    }
-    
     ENTER_CRITICAL_SECTION(  );
-    assert( usFrameLength < MB_SER_PDU_SIZE_MAX );
+    assert( usRcvBufferPos < MB_SER_PDU_SIZE_MAX );
 
     /* Length and CRC check */
-   if( ( usFrameLength >= MB_ASCII_SER_PDU_SIZE_MIN )
-        && ( prvucMBLRC( ( UCHAR * ) pucMBASCIIFrame, usFrameLength ) == 0 ) )
+    if( ( usRcvBufferPos >= MB_ASCII_SER_PDU_SIZE_MIN )
+        && ( prvucMBLRC( ( UCHAR * ) ucASCIIBuf, usRcvBufferPos ) == 0 ) )
     {
         /* Save the address field. All frames are passed to the upper layed
          * and the decision if a frame is used is done there.
          */
-        *pucRcvAddress = pucMBASCIIFrame[MB_SER_PDU_ADDR_OFF];
+        *pucRcvAddress = ucASCIIBuf[MB_SER_PDU_ADDR_OFF];
 
         /* Total length of Modbus-PDU is Modbus-Serial-Line-PDU minus
          * size of address field and CRC checksum.
          */
-        *pusLength = ( USHORT )( usFrameLength - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_LRC );
+        *pusLength = ( USHORT )( usRcvBufferPos - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_LRC );
 
         /* Return the start of the Modbus PDU to the caller. */
-        *pucFrame = ( UCHAR * ) & pucMBASCIIFrame[MB_SER_PDU_PDU_OFF];
+        *pucFrame = ( UCHAR * ) & ucASCIIBuf[MB_SER_PDU_PDU_OFF];
     }
     else
     {
@@ -193,14 +186,13 @@ eMBASCIISend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength )
     eMBErrorCode    eStatus = MB_ENOERR;
     UCHAR           usLRC;
 
-   
+    ENTER_CRITICAL_SECTION(  );
     /* Check if the receiver is still in idle state. If not we where too
      * slow with processing the received frame and the master sent another
      * frame on the network. We have to abort sending the frame.
      */
     if( eRcvState == STATE_RX_IDLE )
     {
-         ENTER_CRITICAL_SECTION(  );
         /* First byte before the Modbus-PDU is the slave address. */
         pucSndBufferCur = ( UCHAR * ) pucFrame - 1;
         usSndBufferCount = 1;
@@ -215,13 +207,6 @@ eMBASCIISend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength )
 
         /* Activate the transmitter. */
         eSndState = STATE_TX_START;
-        EXIT_CRITICAL_SECTION(  );
-
-        if ( xMBSerialPortSendResponse( ( UCHAR * ) pucSndBufferCur, usSndBufferCount ) == FALSE )
-        {
-            eStatus = MB_EIO;
-        }
-
         vMBPortSerialEnable( FALSE, TRUE );
     }
     else
diff --git a/freemodbus/modbus/ascii/mbascii_m.c b/freemodbus/modbus/ascii/mbascii_m.c
index 849ab65..8106598 100644
--- a/freemodbus/modbus/ascii/mbascii_m.c
+++ b/freemodbus/modbus/ascii/mbascii_m.c
@@ -154,34 +154,29 @@ eMBErrorCode
 eMBMasterASCIIReceive( UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * pusLength )
 {
     eMBErrorCode    eStatus = MB_ENOERR;
-    UCHAR          *pucMBASCIIFrame = ( UCHAR* ) ucMasterASCIIRcvBuf;
-    USHORT          usFrameLength = usMasterRcvBufferPos;
 
-    if( xMBMasterSerialPortGetResponse( &pucMBASCIIFrame, &usFrameLength ) == FALSE )
-    {
-        return MB_EIO;
-    }
     ENTER_CRITICAL_SECTION(  );
-    assert( usFrameLength < MB_SER_PDU_SIZE_MAX );
+    assert( usMasterRcvBufferPos < MB_SER_PDU_SIZE_MAX );
 
-    assert( pucMBASCIIFrame );
     /* Length and CRC check */
-    if( ( usFrameLength >= MB_ASCII_SER_PDU_SIZE_MIN )
-            && ( prvucMBLRC( ( UCHAR * ) pucMBASCIIFrame, usFrameLength ) == 0 ) )
+    if( ( usMasterRcvBufferPos >= MB_ASCII_SER_PDU_SIZE_MIN )
+        && ( prvucMBLRC( ( UCHAR * ) ucMasterASCIIRcvBuf, usMasterRcvBufferPos ) == 0 ) )
     {
         /* Save the address field. All frames are passed to the upper layed
-        * and the decision if a frame is used is done there.
-        */
-       *pucRcvAddress = pucMBASCIIFrame[MB_SER_PDU_ADDR_OFF];
+         * and the decision if a frame is used is done there.
+         */
+        *pucRcvAddress = ucMasterASCIIRcvBuf[MB_SER_PDU_ADDR_OFF];
 
-       /* Total length of Modbus-PDU is Modbus-Serial-Line-PDU minus
-        * size of address field and CRC checksum.
-        */
-       *pusLength = ( USHORT )( usFrameLength - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_LRC );
+        /* Total length of Modbus-PDU is Modbus-Serial-Line-PDU minus
+         * size of address field and CRC checksum.
+         */
+        *pusLength = ( USHORT )( usMasterRcvBufferPos - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_LRC );
 
-       /* Return the start of the Modbus PDU to the caller. */
-       *pucFrame = ( UCHAR * ) & pucMBASCIIFrame[MB_SER_PDU_PDU_OFF];
-   } else {
+        /* Return the start of the Modbus PDU to the caller. */
+        *pucFrame = ( UCHAR * ) & ucMasterASCIIRcvBuf[MB_SER_PDU_PDU_OFF];
+    }
+    else
+    {
         eStatus = MB_EIO;
     }
     EXIT_CRITICAL_SECTION(  );
@@ -196,13 +191,13 @@ eMBMasterASCIISend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLengt
 
     if ( ucSlaveAddress > MB_MASTER_TOTAL_SLAVE_NUM ) return MB_EINVAL;
 
+    ENTER_CRITICAL_SECTION(  );
     /* Check if the receiver is still in idle state. If not we where too
      * slow with processing the received frame and the master sent another
      * frame on the network. We have to abort sending the frame.
      */
     if(eRcvState == STATE_M_RX_IDLE)
     {
-        ENTER_CRITICAL_SECTION(  );
         /* First byte before the Modbus-PDU is the slave address. */
         pucMasterSndBufferCur = ( UCHAR * ) pucFrame - 1;
         usMasterSndBufferCount = 1;
@@ -213,22 +208,17 @@ eMBMasterASCIISend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLengt
 
         /* Calculate LRC checksum for Modbus-Serial-Line-PDU. */
         usLRC = prvucMBLRC( ( UCHAR * ) pucMasterSndBufferCur, usMasterSndBufferCount );
-        pucMasterSndBufferCur[usMasterSndBufferCount++] = usLRC;
+        ucMasterASCIISndBuf[usMasterSndBufferCount++] = usLRC;
 
         /* Activate the transmitter. */
         eSndState = STATE_M_TX_START;
-        EXIT_CRITICAL_SECTION(  );
-
-        if ( xMBMasterSerialPortSendRequest( ( UCHAR * ) pucMasterSndBufferCur, usMasterSndBufferCount ) == FALSE )
-        {
-            eStatus = MB_EIO;
-        }
         vMBMasterPortSerialEnable( FALSE, TRUE );
     }
     else
     {
         eStatus = MB_EIO;
     }
+    EXIT_CRITICAL_SECTION(  );
     return eStatus;
 }
 
@@ -457,7 +447,7 @@ xMBMasterASCIITransmitFSM( void )
     return xNeedPoll;
 }
 
-BOOL MB_PORT_ISR_ATTR
+BOOL
 xMBMasterASCIITimerT1SExpired( void )
 {
     BOOL xNeedPoll = FALSE;
@@ -467,6 +457,7 @@ xMBMasterASCIITimerT1SExpired( void )
         /* Timer t35 expired. Startup phase is finished. */
     case STATE_M_RX_INIT:
         xNeedPoll = xMBMasterPortEventPost(EV_MASTER_READY);
+        ESP_EARLY_LOGI("xMBMasterASCIITimerT1SExpired", "RX_INIT_EXPIRED");
         break;
 
         /* Start of message is not received during respond timeout.
diff --git a/freemodbus/modbus/mb.c b/freemodbus/modbus/mb.c
index 60ba538..4be0df4 100644
--- a/freemodbus/modbus/mb.c
+++ b/freemodbus/modbus/mb.c
@@ -337,7 +337,7 @@ eMBDisable( void )
 }
 
 eMBErrorCode
-eMBPoll( void )
+eMBPoll( TickType_t xTicksToWait )
 {
     static UCHAR    *ucMBFrame = NULL;
     static UCHAR    ucRcvAddress;
@@ -357,7 +357,7 @@ eMBPoll( void )
 
     /* Check if there is a event available. If not return control to caller.
      * Otherwise we will handle the event. */
-    if( xMBPortEventGet( &eEvent ) == TRUE )
+    if( xMBPortEventGet( &eEvent, xTicksToWait ) == TRUE )
     {
         switch ( eEvent )
         {
diff --git a/freemodbus/modbus/mb_m.c b/freemodbus/modbus/mb_m.c
index 1f13b9a..aa5922a 100644
--- a/freemodbus/modbus/mb_m.c
+++ b/freemodbus/modbus/mb_m.c
@@ -70,18 +70,18 @@
 
 /* ----------------------- Static variables ---------------------------------*/
 
-static UCHAR ucMBMasterDestAddress;
-static BOOL xMBRunInMasterMode = FALSE;
+static UCHAR    ucMBMasterDestAddress;
+static BOOL     xMBRunInMasterMode = FALSE;
 static volatile eMBMasterErrorEventType eMBMasterCurErrorType;
-static volatile USHORT usMasterSendPDULength;
+static volatile USHORT  usMasterSendPDULength;
 static volatile eMBMode eMBMasterCurrentMode;
 
 /*------------------------ Shared variables ---------------------------------*/
 
-volatile UCHAR ucMasterSndBuf[MB_SERIAL_BUF_SIZE];
-volatile UCHAR ucMasterRcvBuf[MB_SERIAL_BUF_SIZE];
+volatile UCHAR  ucMasterSndBuf[MB_SERIAL_BUF_SIZE];
+volatile UCHAR  ucMasterRcvBuf[MB_SERIAL_BUF_SIZE];
 volatile eMBMasterTimerMode eMasterCurTimerMode;
-volatile BOOL xFrameIsBroadcast = FALSE;
+volatile BOOL   xFrameIsBroadcast = FALSE;
 
 static enum
 {
@@ -152,7 +152,7 @@ static xMBFunctionHandler xMasterFuncHandlers[MB_FUNC_HANDLERS_MAX] = {
 };
 
 /* ----------------------- Start implementation -----------------------------*/
-#if MB_MASTER_TCP_ENABLED
+#if MB_MASTER_TCP_ENABLED > 0
 eMBErrorCode
 eMBMasterTCPInit( USHORT ucTCPPort )
 {
diff --git a/freemodbus/modbus/rtu/mbcrc.c b/freemodbus/modbus/rtu/mbcrc.c
index f413453..c46e7d1 100644
--- a/freemodbus/modbus/rtu/mbcrc.c
+++ b/freemodbus/modbus/rtu/mbcrc.c
@@ -39,7 +39,7 @@
 #include "port.h"
 #include "mbconfig.h"
 
-#if (MB_MASTER_RTU_ENABLED || MB_SLAVE_RTU_ENABLED || CONFIG_MB_UTEST)
+#if MB_MASTER_RTU_ENABLED || MB_SLAVE_RTU_ENABLED
 
 static const UCHAR aucCRCHi[] = {
     0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
diff --git a/freemodbus/modbus/rtu/mbrtu.c b/freemodbus/modbus/rtu/mbrtu.c
index 144e208..180bde2 100644
--- a/freemodbus/modbus/rtu/mbrtu.c
+++ b/freemodbus/modbus/rtu/mbrtu.c
@@ -155,33 +155,26 @@ eMBErrorCode
 eMBRTUReceive( UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * pusLength )
 {
     eMBErrorCode    eStatus = MB_ENOERR;
-    UCHAR          *pucMBRTUFrame = ( UCHAR* ) ucRTUBuf;
-    USHORT          usFrameLength = usRcvBufferPos;
-
-    if( xMBSerialPortGetRequest( &pucMBRTUFrame, &usFrameLength ) == FALSE )
-    {
-        return MB_EIO;
-    }
 
     ENTER_CRITICAL_SECTION(  );
-    assert( usFrameLength < MB_SER_PDU_SIZE_MAX );
+    assert( usRcvBufferPos < MB_SER_PDU_SIZE_MAX );
 
     /* Length and CRC check */
-    if( ( usFrameLength >= MB_SER_PDU_SIZE_MIN )
-        && ( usMBCRC16( ( UCHAR * ) pucMBRTUFrame, usFrameLength ) == 0 ) )
+    if( ( usRcvBufferPos >= MB_SER_PDU_SIZE_MIN )
+        && ( usMBCRC16( ( UCHAR * ) ucRTUBuf, usRcvBufferPos ) == 0 ) )
     {
         /* Save the address field. All frames are passed to the upper layed
          * and the decision if a frame is used is done there.
          */
-        *pucRcvAddress = pucMBRTUFrame[MB_SER_PDU_ADDR_OFF];
+        *pucRcvAddress = ucRTUBuf[MB_SER_PDU_ADDR_OFF];
 
         /* Total length of Modbus-PDU is Modbus-Serial-Line-PDU minus
          * size of address field and CRC checksum.
          */
-        *pusLength = ( USHORT )( usFrameLength - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_CRC );
+        *pusLength = ( USHORT )( usRcvBufferPos - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_CRC );
 
         /* Return the start of the Modbus PDU to the caller. */
-        *pucFrame = ( UCHAR * ) & pucMBRTUFrame[MB_SER_PDU_PDU_OFF];
+        *pucFrame = ( UCHAR * ) & ucRTUBuf[MB_SER_PDU_PDU_OFF];
     }
     else
     {
@@ -198,13 +191,14 @@ eMBRTUSend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength )
     eMBErrorCode    eStatus = MB_ENOERR;
     USHORT          usCRC16;
 
+    ENTER_CRITICAL_SECTION(  );
+
     /* Check if the receiver is still in idle state. If not we where to
      * slow with processing the received frame and the master sent another
      * frame on the network. We have to abort sending the frame.
      */
     if( eRcvState == STATE_RX_IDLE )
     {
-        ENTER_CRITICAL_SECTION(  );
         /* First byte before the Modbus-PDU is the slave address. */
         pucSndBufferCur = ( UCHAR * ) pucFrame - 1;
         usSndBufferCount = 1;
@@ -220,19 +214,13 @@ eMBRTUSend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength )
 
         /* Activate the transmitter. */
         eSndState = STATE_TX_XMIT;
-        EXIT_CRITICAL_SECTION(  );
-
-        if( xMBSerialPortSendResponse( ( UCHAR * ) pucSndBufferCur, usSndBufferCount ) == FALSE )
-        {
-            eStatus = MB_EIO;
-        }
-
         vMBPortSerialEnable( FALSE, TRUE );
     }
     else
     {
         eStatus = MB_EIO;
     }
+    EXIT_CRITICAL_SECTION(  );
     return eStatus;
 }
 
@@ -284,7 +272,7 @@ xMBRTUReceiveFSM( void )
     case STATE_RX_RCV:
         if( usRcvBufferPos < MB_SER_PDU_SIZE_MAX )
         {
-            if( xStatus ) {
+            if ( xStatus ) {
                 ucRTUBuf[usRcvBufferPos++] = ucByte;
             }
         }
diff --git a/freemodbus/modbus/rtu/mbrtu_m.c b/freemodbus/modbus/rtu/mbrtu_m.c
index 65ac4b7..47a5513 100644
--- a/freemodbus/modbus/rtu/mbrtu_m.c
+++ b/freemodbus/modbus/rtu/mbrtu_m.c
@@ -161,34 +161,26 @@ eMBErrorCode
 eMBMasterRTUReceive( UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * pusLength )
 {
     eMBErrorCode    eStatus = MB_ENOERR;
-    UCHAR          *pucMBRTUFrame = ( UCHAR* ) ucMasterRTURcvBuf;
-    USHORT          usFrameLength = usMasterRcvBufferPos;
-
-    if( xMBMasterSerialPortGetResponse( &pucMBRTUFrame, &usFrameLength ) == FALSE )
-    {
-        return MB_EIO;
-    }
 
     ENTER_CRITICAL_SECTION(  );
-    assert( usFrameLength < MB_SER_PDU_SIZE_MAX );
-    assert( pucMBRTUFrame );
+    assert( usMasterRcvBufferPos < MB_SER_PDU_SIZE_MAX );
 
     /* Length and CRC check */
-    if( ( usFrameLength >= MB_RTU_SER_PDU_SIZE_MIN )
-        && ( usMBCRC16( ( UCHAR * ) pucMBRTUFrame, usFrameLength ) == 0 ) )
+    if( ( usMasterRcvBufferPos >= MB_RTU_SER_PDU_SIZE_MIN )
+        && ( usMBCRC16( ( UCHAR * ) ucMasterRTURcvBuf, usMasterRcvBufferPos ) == 0 ) )
     {
-        /* Save the address field. All frames are passed to the upper layer
+        /* Save the address field. All frames are passed to the upper layed
          * and the decision if a frame is used is done there.
          */
-        *pucRcvAddress = pucMBRTUFrame[MB_SER_PDU_ADDR_OFF];
+        *pucRcvAddress = ucMasterRTURcvBuf[MB_SER_PDU_ADDR_OFF];
 
         /* Total length of Modbus-PDU is Modbus-Serial-Line-PDU minus
          * size of address field and CRC checksum.
          */
-        *pusLength = ( USHORT )( usFrameLength - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_CRC );
+        *pusLength = ( USHORT )( usMasterRcvBufferPos - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_CRC );
 
         /* Return the start of the Modbus PDU to the caller. */
-        *pucFrame = ( UCHAR * ) & pucMBRTUFrame[MB_SER_PDU_PDU_OFF];
+        *pucFrame = ( UCHAR * ) & ucMasterRTURcvBuf[MB_SER_PDU_PDU_OFF];
     }
     else
     {
@@ -207,13 +199,14 @@ eMBMasterRTUSend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength
 
     if ( ucSlaveAddress > MB_MASTER_TOTAL_SLAVE_NUM ) return MB_EINVAL;
 
+    ENTER_CRITICAL_SECTION(  );
+
     /* Check if the receiver is still in idle state. If not we where to
      * slow with processing the received frame and the master sent another
      * frame on the network. We have to abort sending the frame.
      */
     if( eRcvState == STATE_M_RX_IDLE )
     {
-        ENTER_CRITICAL_SECTION(  );
         /* First byte before the Modbus-PDU is the slave address. */
         pucMasterSndBufferCur = ( UCHAR * ) pucFrame - 1;
         usMasterSndBufferCount = 1;
@@ -224,18 +217,11 @@ eMBMasterRTUSend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength
 
         /* Calculate CRC16 checksum for Modbus-Serial-Line-PDU. */
         usCRC16 = usMBCRC16( ( UCHAR * ) pucMasterSndBufferCur, usMasterSndBufferCount );
-        pucMasterSndBufferCur[usMasterSndBufferCount++] = ( UCHAR )( usCRC16 & 0xFF );
-        pucMasterSndBufferCur[usMasterSndBufferCount++] = ( UCHAR )( usCRC16 >> 8 );
-        EXIT_CRITICAL_SECTION(  );
+        ucMasterRTUSndBuf[usMasterSndBufferCount++] = ( UCHAR )( usCRC16 & 0xFF );
+        ucMasterRTUSndBuf[usMasterSndBufferCount++] = ( UCHAR )( usCRC16 >> 8 );
 
         /* Activate the transmitter. */
         eSndState = STATE_M_TX_XMIT;
-        
-        if ( xMBMasterSerialPortSendRequest( ( UCHAR * ) pucMasterSndBufferCur, usMasterSndBufferCount ) == FALSE )
-        {
-            eStatus = MB_EIO;
-        }
-        
         // The place to enable RS485 driver
         vMBMasterPortSerialEnable( FALSE, TRUE );
     }
@@ -243,6 +229,7 @@ eMBMasterRTUSend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength
     {
         eStatus = MB_EIO;
     }
+    EXIT_CRITICAL_SECTION(  );
     return eStatus;
 }
 
@@ -286,15 +273,11 @@ xMBMasterRTUReceiveFSM( void )
         eSndState = STATE_M_TX_IDLE;
 
         usMasterRcvBufferPos = 0;
-        if( xStatus && ucByte ) {
-            ucMasterRTURcvBuf[usMasterRcvBufferPos++] = ucByte;
-            eRcvState = STATE_M_RX_RCV;
-        }
+        ucMasterRTURcvBuf[usMasterRcvBufferPos++] = ucByte;
+        eRcvState = STATE_M_RX_RCV;
 
         /* Enable t3.5 timers. */
-#if CONFIG_FMB_TIMER_PORT_ENABLED
         vMBMasterPortTimersT35Enable( );
-#endif
         break;
 
         /* We are currently receiving a frame. Reset the timer after
@@ -313,9 +296,7 @@ xMBMasterRTUReceiveFSM( void )
         {
             eRcvState = STATE_M_RX_ERROR;
         }
-#if CONFIG_FMB_TIMER_PORT_ENABLED
         vMBMasterPortTimersT35Enable( );
-#endif
         break;
     }
     return xStatus;
diff --git a/freemodbus/port/port.c b/freemodbus/port/port.c
index 4f672a9..1959104 100644
--- a/freemodbus/port/port.c
+++ b/freemodbus/port/port.c
@@ -38,7 +38,6 @@
 
 /* ----------------------- Modbus includes ----------------------------------*/
 #include "freertos/FreeRTOS.h"
-#include "freertos/queue.h"
 #include "sys/lock.h"
 #include "port.h"
 
@@ -73,13 +72,6 @@ vMBPortSetMode( UCHAR ucMode )
     EXIT_CRITICAL_SECTION();
 }
 
-BOOL xMBPortSerialWaitEvent(QueueHandle_t xMbUartQueue, uart_event_t* pxEvent, ULONG xTimeout)
-{
-    BOOL xResult = (BaseType_t)xQueueReceive(xMbUartQueue, (void*)pxEvent, (TickType_t) xTimeout);
-    ESP_LOGD(__func__, "UART event: %d ", pxEvent->type);
-    return xResult;
-}
-
 #if MB_TCP_DEBUG
 
 // This function is kept to realize legacy freemodbus frame logging functionality
diff --git a/freemodbus/port/portevent.c b/freemodbus/port/portevent.c
index 229635b..6230625 100644
--- a/freemodbus/port/portevent.c
+++ b/freemodbus/port/portevent.c
@@ -102,12 +102,12 @@ xMBPortEventPost( eMBEventType eEvent )
 }
 
 BOOL
-xMBPortEventGet(eMBEventType * peEvent)
+xMBPortEventGet(eMBEventType * peEvent, TickType_t xTicksToWait)
 {
     assert(xQueueHdl != NULL);
     BOOL xEventHappened = FALSE;
 
-    if (xQueueReceive(xQueueHdl, peEvent, portMAX_DELAY) == pdTRUE) {
+    if (xQueueReceive(xQueueHdl, peEvent, xTicksToWait) == pdTRUE) {
         xEventHappened = TRUE;
     }
     return xEventHappened;
diff --git a/freemodbus/port/portserial.c b/freemodbus/port/portserial.c
index aae45d9..a57a6c6 100644
--- a/freemodbus/port/portserial.c
+++ b/freemodbus/port/portserial.c
@@ -33,8 +33,6 @@
  *
  * File: $Id: portother.c,v 1.1 2010/06/06 13:07:20 wolti Exp $
  */
-
-#include "driver/uart.h"
 #include "port.h"
 #include "driver/uart.h"
 #include "freertos/queue.h" // for queue support
@@ -87,14 +85,15 @@ static USHORT usMBPortSerialRxPoll(size_t xEventSize)
 
     if (bRxStateEnabled) {
         // Get received packet into Rx buffer
-        while(xReadStatus && (usCnt++ <= xEventSize)) {
+        while(xReadStatus && (usCnt++ <= MB_SERIAL_BUF_SIZE)) {
             // Call the Modbus stack callback function and let it fill the buffers.
             xReadStatus = pxMBFrameCBByteReceived(); // callback to execute receive FSM
         }
         uart_flush_input(ucUartNumber);
         // Send event EV_FRAME_RECEIVED to allow stack process packet
 #if !CONFIG_FMB_TIMER_PORT_ENABLED
-        pxMBPortCBTimerExpired();
+        // Let the stack know that T3.5 time is expired and data is received
+        (void)pxMBPortCBTimerExpired(); // calls callback xMBRTUTimerT35Expired();
 #endif
         ESP_LOGD(TAG, "RX: %d bytes\n", usCnt);
     }
@@ -127,7 +126,7 @@ static void vUartTask(void *pvParameters)
     uart_event_t xEvent;
     USHORT usResult = 0;
     for(;;) {
-        if (xMBPortSerialWaitEvent(xMbUartQueue, (void*)&xEvent, portMAX_DELAY)) {
+        if (xQueueReceive(xMbUartQueue, (void*)&xEvent, portMAX_DELAY) == pdTRUE) {
             ESP_LOGD(TAG, "MB_uart[%d] event:", ucUartNumber);
             switch(xEvent.type) {
                 //Event of UART receving data
@@ -136,8 +135,6 @@ static void vUartTask(void *pvParameters)
                     // This flag set in the event means that no more
                     // data received during configured timeout and UART TOUT feature is triggered
                     if (xEvent.timeout_flag) {
-                        // Get buffered data length
-                        ESP_ERROR_CHECK(uart_get_buffered_data_len(ucUartNumber, &xEvent.size));
                         // Read received data and send it to modbus stack
                         usResult = usMBPortSerialRxPoll(xEvent.size);
                         ESP_LOGD(TAG,"Timeout occured, processed: %d bytes", usResult);
@@ -222,7 +219,7 @@ BOOL xMBPortSerialInit(UCHAR ucPORT, ULONG ulBaudRate,
         .stop_bits = UART_STOP_BITS_1,
         .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
         .rx_flow_ctrl_thresh = 2,
-        .source_clk = UART_SCLK_APB
+        .source_clk = UART_SCLK_APB,
     };
     // Set UART config
     xErr = uart_param_config(ucUartNumber, &xUartConfig);
@@ -260,17 +257,6 @@ BOOL xMBPortSerialInit(UCHAR ucPORT, ULONG ulBaudRate,
     return TRUE;
 }
 
-BOOL xMBSerialPortGetRequest( UCHAR **ppucMBSerialFrame, USHORT * usSerialLength )
-{
-    BOOL eStatus = TRUE;
-    return eStatus;
-}
-
-BOOL xMBSerialPortSendResponse( UCHAR *pucMBSerialFrame, USHORT usSerialLength )
-{
-    return TRUE;
-}
-
 void vMBPortSerialClose(void)
 {
     (void)vTaskSuspend(xMbTaskHandle);
diff --git a/freemodbus/port/portserial_m.c b/freemodbus/port/portserial_m.c
index 777eee8..b0324e3 100644
--- a/freemodbus/port/portserial_m.c
+++ b/freemodbus/port/portserial_m.c
@@ -89,7 +89,7 @@ static USHORT usMBMasterPortSerialRxPoll(size_t xEventSize)
     USHORT usCnt = 0;
 
     if (bRxStateEnabled) {
-        while(xReadStatus && (usCnt++ <= xEventSize)) {
+        while(xReadStatus && (usCnt++ <= MB_SERIAL_BUF_SIZE)) {
             // Call the Modbus stack callback function and let it fill the stack buffers.
             xReadStatus = pxMBMasterFrameCBByteReceived(); // callback to receive FSM
         }
@@ -134,7 +134,7 @@ static void vUartTask(void* pvParameters)
     uart_event_t xEvent;
     USHORT usResult = 0;
     for(;;) {
-        if (xMBPortSerialWaitEvent(xMbUartQueue, (void*)&xEvent, portMAX_DELAY)) {
+        if (xQueueReceive(xMbUartQueue, (void*)&xEvent, portMAX_DELAY) == pdTRUE) {
             ESP_LOGD(TAG, "MB_uart[%d] event:", ucUartNumber);
             switch(xEvent.type) {
                 //Event of UART receiving data
@@ -143,8 +143,6 @@ static void vUartTask(void* pvParameters)
                     // This flag set in the event means that no more
                     // data received during configured timeout and UART TOUT feature is triggered
                     if (xEvent.timeout_flag) {
-                        // Get buffered data length
-                        ESP_ERROR_CHECK(uart_get_buffered_data_len(ucUartNumber, &xEvent.size));
                         // Read received data and send it to modbus stack
                         usResult = usMBMasterPortSerialRxPoll(xEvent.size);
                         ESP_LOGD(TAG,"Timeout occured, processed: %d bytes", usResult);
@@ -268,24 +266,6 @@ BOOL xMBMasterPortSerialInit( UCHAR ucPORT, ULONG ulBaudRate, UCHAR ucDataBits,
     return TRUE;
 }
 
-/*
- * The function is called from ASCII/RTU module to get processed data buffer. Sets the
- * received buffer and its length using parameters.
- */
-BOOL xMBMasterSerialPortGetResponse( UCHAR **ppucMBSerialFrame, USHORT * usSerialLength )
-{
-    return TRUE;
-}
-
-/*
- * The function is called from ASCII/RTU module to set processed data buffer
- * to be sent in transmitter state machine.
- */
-BOOL xMBMasterSerialPortSendRequest( UCHAR *pucMBSerialFrame, USHORT usSerialLength )
-{
-    return TRUE;
-}
-
 void vMBMasterPortSerialClose(void)
 {
     (void)vTaskDelete(xMbTaskHandle);
diff --git a/freemodbus/port/porttimer.c b/freemodbus/port/porttimer.c
index 4fbfbfd..9e2673f 100644
--- a/freemodbus/port/porttimer.c
+++ b/freemodbus/port/porttimer.c
@@ -74,7 +74,7 @@ BOOL xMBPortTimersInit(USHORT usTimeOut50us)
     esp_timer_create_args_t xTimerConf = {
         .callback = vTimerAlarmCBHandler,
         .arg = NULL,
-#if (MB_TIMER_SUPPORTS_ISR_DISPATCH_METHOD && CONFIG_FMB_TIMER_USE_ISR_DISPATCH_METHOD)
+#if CONFIG_FMB_TIMER_USE_ISR_DISPATCH_METHOD
         .dispatch_method = ESP_TIMER_ISR,
 #else
         .dispatch_method = ESP_TIMER_TASK,
diff --git a/freemodbus/port/porttimer_m.c b/freemodbus/port/porttimer_m.c
index f1595a6..3982f27 100644
--- a/freemodbus/port/porttimer_m.c
+++ b/freemodbus/port/porttimer_m.c
@@ -71,7 +71,7 @@ BOOL xMBMasterPortTimersInit(USHORT usTimeOut50us)
     esp_timer_create_args_t xTimerConf = {
         .callback = vTimerAlarmCBHandler,
         .arg = NULL,
-#if (MB_TIMER_SUPPORTS_ISR_DISPATCH_METHOD && CONFIG_FMB_TIMER_USE_ISR_DISPATCH_METHOD)
+#if CONFIG_FMB_TIMER_USE_ISR_DISPATCH_METHOD
         .dispatch_method = ESP_TIMER_ISR,
 #else
         .dispatch_method = ESP_TIMER_TASK,
diff --git a/freemodbus/sdkconfig.rename b/freemodbus/sdkconfig.rename
new file mode 100644
index 0000000..89c1c23
--- /dev/null
+++ b/freemodbus/sdkconfig.rename
@@ -0,0 +1,18 @@
+# sdkconfig replacement configurations for deprecated options formatted as
+# CONFIG_DEPRECATED_OPTION CONFIG_NEW_OPTION
+
+CONFIG_MB_MASTER_TIMEOUT_MS_RESPOND     CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND
+CONFIG_MB_MASTER_DELAY_MS_CONVERT       CONFIG_FMB_MASTER_DELAY_MS_CONVERT
+CONFIG_MB_QUEUE_LENGTH                  CONFIG_FMB_QUEUE_LENGTH
+CONFIG_MB_SERIAL_TASK_STACK_SIZE        CONFIG_FMB_PORT_TASK_STACK_SIZE
+CONFIG_MB_SERIAL_BUF_SIZE               CONFIG_FMB_SERIAL_BUF_SIZE
+CONFIG_MB_SERIAL_TASK_PRIO              CONFIG_FMB_PORT_TASK_PRIO
+CONFIG_MB_CONTROLLER_SLAVE_ID_SUPPORT   CONFIG_FMB_CONTROLLER_SLAVE_ID_SUPPORT
+CONFIG_MB_CONTROLLER_SLAVE_ID           CONFIG_FMB_CONTROLLER_SLAVE_ID
+CONFIG_MB_CONTROLLER_NOTIFY_TIMEOUT     CONFIG_FMB_CONTROLLER_NOTIFY_TIMEOUT
+CONFIG_MB_CONTROLLER_NOTIFY_QUEUE_SIZE  CONFIG_FMB_CONTROLLER_NOTIFY_QUEUE_SIZE
+CONFIG_MB_CONTROLLER_STACK_SIZE         CONFIG_FMB_CONTROLLER_STACK_SIZE
+CONFIG_MB_EVENT_QUEUE_TIMEOUT           CONFIG_FMB_EVENT_QUEUE_TIMEOUT
+CONFIG_MB_TIMER_PORT_ENABLED            CONFIG_FMB_TIMER_PORT_ENABLED
+CONFIG_MB_TIMER_GROUP                   CONFIG_FMB_TIMER_GROUP
+CONFIG_MB_TIMER_INDEX                   CONFIG_FMB_TIMER_INDEX
diff --git a/freemodbus/serial_master/modbus_controller/mbc_serial_master.c b/freemodbus/serial_master/modbus_controller/mbc_serial_master.c
index 94bec3f..378bd54 100644
--- a/freemodbus/serial_master/modbus_controller/mbc_serial_master.c
+++ b/freemodbus/serial_master/modbus_controller/mbc_serial_master.c
@@ -30,7 +30,7 @@ extern BOOL xMBMasterPortSerialTxPoll(void);
 #define MB_RESPONSE_TICS pdMS_TO_TICKS(CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND + 10)
 
 
-static mb_master_interface_t* mbm_interface_ptr = NULL;
+static mb_master_interface_t* mbm_interface_ptr = NULL; //&default_interface_inst;
 static const char *TAG = "MB_CONTROLLER_MASTER";
 
 // Modbus event processing task
@@ -101,7 +101,7 @@ static esp_err_t mbc_serial_master_start(void)
             "mb stack initialization failure, eMBInit() returns (0x%x).", status);
     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);
+            "mb stack set slave ID failure, eMBEnable() returned (0x%x).", (uint32_t)status);
     // Set the mbcontroller start flag
     EventBits_t flag = xEventGroupSetBits(mbm_opts->mbm_event_group,
                                             (EventBits_t)MB_EVENT_STACK_STARTED);
@@ -639,7 +639,7 @@ eMBErrorCode eMBRegDiscreteCBSerialMaster(UCHAR * pucRegBuffer, USHORT usAddress
 }
 
 // Initialization of resources for Modbus serial master controller
-esp_err_t mbc_serial_master_create(void** handler)
+esp_err_t mbc_serial_master_create(void** handler, bool start_controller_task)
 {
     // Allocate space for master interface structure
     if (mbm_interface_ptr == NULL) {
@@ -664,21 +664,26 @@ esp_err_t mbc_serial_master_create(void** handler)
     mbm_opts->mbm_event_group = xEventGroupCreate();
     MB_MASTER_CHECK((mbm_opts->mbm_event_group != NULL),
                         ESP_ERR_NO_MEM, "mb event group error.");
-    // Create modbus controller task
-    status = xTaskCreatePinnedToCore((void*)&modbus_master_task,
-                            "modbus_matask",
-                            MB_CONTROLLER_STACK_SIZE,
-                            NULL,                       // No parameters
-                            MB_CONTROLLER_PRIORITY,
-                            &mbm_opts->mbm_task_handle,
-                            MB_PORT_TASK_AFFINITY);
-    if (status != pdPASS) {
-        vTaskDelete(mbm_opts->mbm_task_handle);
-        MB_MASTER_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
-                "mb controller task creation error, xTaskCreate() returns (0x%x).",
-                (uint32_t)status);
+    if (start_controller_task)
+    {
+        // Create modbus controller task
+        status = xTaskCreatePinnedToCore((void*)&modbus_master_task,
+                                "modbus_matask",
+                                MB_CONTROLLER_STACK_SIZE,
+                                NULL,                       // No parameters
+                                MB_CONTROLLER_PRIORITY,
+                                &mbm_opts->mbm_task_handle,
+                                MB_PORT_TASK_AFFINITY);
+        if (status != pdPASS) {
+            vTaskDelete(mbm_opts->mbm_task_handle);
+            MB_MASTER_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
+                    "mb controller task creation error, xTaskCreate() returns (0x%x).",
+                    (uint32_t)status);
+            MB_MASTER_ASSERT(mbm_opts->mbm_task_handle != NULL); // The task is created but handle is incorrect
+        }
     }
-    MB_MASTER_ASSERT(mbm_opts->mbm_task_handle != NULL); // The task is created but handle is incorrect
+    else
+        mbm_opts->mbm_task_handle = NULL;
 
     // Initialize public interface methods of the interface
     mbm_interface_ptr->init = mbc_serial_master_create;
diff --git a/freemodbus/serial_master/modbus_controller/mbc_serial_master.h b/freemodbus/serial_master/modbus_controller/mbc_serial_master.h
index 28aec53..1236288 100644
--- a/freemodbus/serial_master/modbus_controller/mbc_serial_master.h
+++ b/freemodbus/serial_master/modbus_controller/mbc_serial_master.h
@@ -23,6 +23,6 @@
  *     - ESP_OK   Success
  *     - ESP_ERR_NO_MEM Parameter error
  */
-esp_err_t mbc_serial_master_create(void** handler);
+esp_err_t mbc_serial_master_create(void** handler, bool start_controller_task);
 
 #endif // _MODBUS_SERIAL_CONTROLLER_MASTER
diff --git a/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c b/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c
index cae1361..e6ccdb8 100644
--- a/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c
+++ b/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.c
@@ -38,7 +38,7 @@ static void modbus_slave_task(void *pvParameters)
                                                 portMAX_DELAY);
         // Check if stack started then poll for data
         if (status & MB_EVENT_STACK_STARTED) {
-            (void)eMBPoll(); // allow stack to process data
+            (void)eMBPoll(portMAX_DELAY); // allow stack to process data
             // Send response buffer
             BOOL xSentState = xMBPortSerialTxPoll();
             if (xSentState) {
@@ -158,7 +158,8 @@ static esp_err_t mbc_serial_slave_destroy(void)
     // Disable and then destroy the Modbus stack
     mb_error = eMBDisable();
     MB_SLAVE_CHECK((mb_error == MB_ENOERR), ESP_ERR_INVALID_STATE, "mb stack disable failure.");
-    (void)vTaskDelete(mbs_opts->mbs_task_handle);
+    if (mbs_opts->mbs_task_handle)
+        (void)vTaskDelete(mbs_opts->mbs_task_handle);
     (void)vQueueDelete(mbs_opts->mbs_notification_queue_handle);
     (void)vEventGroupDelete(mbs_opts->mbs_event_group);
     mb_error = eMBClose();
@@ -170,7 +171,7 @@ static esp_err_t mbc_serial_slave_destroy(void)
 }
 
 // Initialization of Modbus controller
-esp_err_t mbc_serial_slave_create(void** handler)
+esp_err_t mbc_serial_slave_create(void** handler, bool start_controller_task)
 {
     // Allocate space for options
     if (mbs_interface_ptr == NULL) {
@@ -202,21 +203,28 @@ esp_err_t mbc_serial_slave_create(void** handler)
                                                 sizeof(mb_param_info_t));
     MB_SLAVE_CHECK((mbs_opts->mbs_notification_queue_handle != NULL),
             ESP_ERR_NO_MEM, "mb notify queue creation error.");
-    // Create Modbus controller task
-    status = xTaskCreatePinnedToCore((void*)&modbus_slave_task,
-                            "modbus_slave_task",
-                            MB_CONTROLLER_STACK_SIZE,
-                            NULL,
-                            MB_CONTROLLER_PRIORITY,
-                            &mbs_opts->mbs_task_handle,
-                            MB_PORT_TASK_AFFINITY);
-    if (status != pdPASS) {
-        vTaskDelete(mbs_opts->mbs_task_handle);
-        MB_SLAVE_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
-                "mb controller task creation error, xTaskCreate() returns (0x%x).",
-                (uint32_t)status);
+    if (start_controller_task)
+    {
+        // Create Modbus controller task
+        status = xTaskCreatePinnedToCore((void*)&modbus_slave_task,
+                                "modbus_slave_task",
+                                MB_CONTROLLER_STACK_SIZE,
+                                NULL,
+                                MB_CONTROLLER_PRIORITY,
+                                &mbs_opts->mbs_task_handle,
+                                MB_PORT_TASK_AFFINITY);
+        if (status != pdPASS) {
+            vTaskDelete(mbs_opts->mbs_task_handle);
+            MB_SLAVE_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
+                    "mb controller task creation error, xTaskCreate() returns (0x%x).",
+                    (uint32_t)status);
+        }
+
+        // The task is created but handle is incorrect
+        MB_SLAVE_ASSERT(mbs_opts->mbs_task_handle != NULL);
     }
-    MB_SLAVE_ASSERT(mbs_opts->mbs_task_handle != NULL); // The task is created but handle is incorrect
+    else
+        mbs_opts->mbs_task_handle = false;
 
     // Initialize interface function pointers
     mbs_interface_ptr->check_event = mbc_serial_slave_check_event;
@@ -228,12 +236,12 @@ esp_err_t mbc_serial_slave_create(void** handler)
     mbs_interface_ptr->start = mbc_serial_slave_start;
 
     // Initialize stack callback function pointers
-    mbs_interface_ptr->slave_reg_cb_discrete = NULL; // implemented in common layer
-    mbs_interface_ptr->slave_reg_cb_input = NULL;
-    mbs_interface_ptr->slave_reg_cb_holding = NULL;
-    mbs_interface_ptr->slave_reg_cb_coils = NULL;
+    mbs_interface_ptr->slave_reg_cb_discrete = mbc_reg_discrete_slave_cb;
+    mbs_interface_ptr->slave_reg_cb_input = mbc_reg_input_slave_cb;
+    mbs_interface_ptr->slave_reg_cb_holding = mbc_reg_holding_slave_cb;
+    mbs_interface_ptr->slave_reg_cb_coils = mbc_reg_coils_slave_cb;
 
-    *handler = (void*)mbs_interface_ptr;
+    *handler = mbs_interface_ptr;
 
     return ESP_OK;
 }
diff --git a/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.h b/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.h
index 9a515ba..1ed66c3 100644
--- a/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.h
+++ b/freemodbus/serial_slave/modbus_controller/mbc_serial_slave.h
@@ -25,6 +25,6 @@
  *     - ESP_OK   Success
  *     - ESP_ERR_NO_MEM Parameter error
  */
-esp_err_t mbc_serial_slave_create(void** handler);
+esp_err_t mbc_serial_slave_create(void **handler, bool start_controller_task);
 
 #endif // _MODBUS_SERIAL_CONTROLLER_SLAVE
diff --git a/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.c b/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.c
index f1f3916..def4eff 100644
--- a/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.c
+++ b/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.c
@@ -159,7 +159,7 @@ static esp_err_t mbc_tcp_master_start(void)
 
     status = eMBMasterEnable();
     MB_MASTER_CHECK((status == MB_ENOERR), ESP_ERR_INVALID_STATE,
-                    "mb stack enable failure, eMBMasterEnable() returned (0x%x).", (uint32_t)status);
+                    "mb stack set slave ID failure, eMBMasterEnable() returned (0x%x).", (uint32_t)status);
 
     // Add slave IP address for each slave to initialize connection
     mb_slave_addr_entry_t *p_slave_info;
@@ -172,12 +172,13 @@ static esp_err_t mbc_tcp_master_start(void)
     // Add end of list condition
     (void)xMBTCPPortMasterAddSlaveIp(0xFF, NULL, 0xFF);
 
+
     // Wait for connection done event
     bool start = (bool)xMBTCPPortMasterWaitEvent(mbm_opts->mbm_event_group,
-                                                    (EventBits_t)MB_EVENT_STACK_STARTED, MB_TCP_CONNECTION_TOUT);
+                                                 (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);
+                    "mb stack could not connect to slaves for %d seconds.",
+                    CONFIG_FMB_TCP_CONNECTION_TOUT_SEC);
     return ESP_OK;
 }
 
@@ -192,6 +193,11 @@ static esp_err_t mbc_tcp_master_destroy(void)
     // 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.");
+
+    if (mbm_opts->mbm_task_handle)
+        (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);
@@ -750,7 +756,7 @@ eMBErrorCode eMBRegDiscreteCBTcpMaster(UCHAR * pucRegBuffer, USHORT usAddress,
 }
 
 // Initialization of resources for Modbus TCP master controller
-esp_err_t mbc_tcp_master_create(void** handler)
+esp_err_t mbc_tcp_master_create(void** handler, bool start_controller_task)
 {
     // Allocate space for master interface structure
     if (mbm_interface_ptr == NULL) {
@@ -772,20 +778,26 @@ esp_err_t mbc_tcp_master_create(void** handler)
     // Parameter change notification queue
     mbm_opts->mbm_event_group = xEventGroupCreate();
     MB_MASTER_CHECK((mbm_opts->mbm_event_group != NULL), ESP_ERR_NO_MEM, "mb event group error.");
-    // Create modbus controller task
-    status = xTaskCreate((void*)&modbus_tcp_master_task,
-                            "modbus_tcp_master_task",
-                            MB_CONTROLLER_STACK_SIZE,
-                            NULL, // No parameters
-                            MB_CONTROLLER_PRIORITY,
-                            &mbm_opts->mbm_task_handle);
-    if (status != pdPASS) {
-        vTaskDelete(mbm_opts->mbm_task_handle);
-        MB_MASTER_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
-                        "mb controller task creation error, xTaskCreate() returns (0x%x).",
-                        (uint32_t)status);
+
+
+    if (start_controller_task)
+    {
+        // Create modbus controller task
+        status = xTaskCreate((void*)&modbus_tcp_master_task,
+                                "modbus_tcp_master_task",
+                                MB_CONTROLLER_STACK_SIZE,
+                                NULL, // No parameters
+                                MB_CONTROLLER_PRIORITY,
+                                &mbm_opts->mbm_task_handle);
+        if (status != pdPASS) {
+            vTaskDelete(mbm_opts->mbm_task_handle);
+            MB_MASTER_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
+                            "mb controller task creation error, xTaskCreate() returns (0x%x).",
+                            (uint32_t)status);
+        }
+        else
+            mbm_opts->mbm_task_handle = NULL;
     }
-    MB_MASTER_ASSERT(mbm_opts->mbm_task_handle != NULL); // The task is created but handle is incorrect
 
     LIST_INIT(&mbm_opts->mbm_slave_list); // Init slave address list
     mbm_opts->mbm_slave_list_count = 0;
diff --git a/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.h b/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.h
index b0731df..0ec0d63 100644
--- a/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.h
+++ b/freemodbus/tcp_master/modbus_controller/mbc_tcp_master.h
@@ -28,6 +28,6 @@
  *     - ESP_OK   Success
  *     - ESP_ERR_NO_MEM Parameter error
  */
-esp_err_t mbc_tcp_master_create(void** handler);
+esp_err_t mbc_tcp_master_create(void** handler, bool start_controller_task);
 
 #endif // _MODBUS_TCP_CONTROLLER_SLAVE
diff --git a/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.c b/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.c
index 45078b4..9e469bb 100644
--- a/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.c
+++ b/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.c
@@ -40,7 +40,7 @@ static void modbus_tcp_slave_task(void *pvParameters)
                                                 portMAX_DELAY);
         // Check if stack started then poll for data
         if (status & MB_EVENT_STACK_STARTED) {
-            (void)eMBPoll(); // allow stack to process data
+            (void)eMBPoll(portMAX_DELAY); // allow stack to process data
         }
     }
 }
@@ -105,7 +105,8 @@ static esp_err_t mbc_tcp_slave_destroy(void)
     // Disable and then destroy the Modbus stack
     mb_error = eMBDisable();
     MB_SLAVE_CHECK((mb_error == MB_ENOERR), ESP_ERR_INVALID_STATE, "mb stack disable failure.");
-    (void)vTaskDelete(mbs_opts->mbs_task_handle);
+    if (mbs_opts->mbs_task_handle)
+        (void)vTaskDelete(mbs_opts->mbs_task_handle);
     (void)vQueueDelete(mbs_opts->mbs_notification_queue_handle);
     (void)vEventGroupDelete(mbs_opts->mbs_event_group);
     (void)vMBTCPPortClose();
@@ -149,7 +150,7 @@ static esp_err_t mbc_tcp_slave_get_param_info(mb_param_info_t* reg_info, uint32_
 #pragma GCC diagnostic ignored "-Wtype-limits"
 
 // Initialization of Modbus controller
-esp_err_t mbc_tcp_slave_create(void** handler)
+esp_err_t mbc_tcp_slave_create(void** handler, bool start_controller_task)
 {
     // Allocate space for options
     if (mbs_interface_ptr == NULL) {
@@ -177,23 +178,29 @@ esp_err_t mbc_tcp_slave_create(void** handler)
                                                 sizeof(mb_param_info_t));
     MB_SLAVE_CHECK((mbs_opts->mbs_notification_queue_handle != NULL),
             ESP_ERR_NO_MEM, "mb notify queue creation error.");
-    // Create Modbus controller task
-    status = xTaskCreatePinnedToCore((void*)&modbus_tcp_slave_task,
-                            "modbus_tcp_slave_task",
-                            MB_CONTROLLER_STACK_SIZE,
-                            NULL,
-                            MB_CONTROLLER_PRIORITY,
-                            &mbs_opts->mbs_task_handle,
-                            MB_PORT_TASK_AFFINITY);
-    if (status != pdPASS) {
-        vTaskDelete(mbs_opts->mbs_task_handle);
-        MB_SLAVE_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
-                "mb controller task creation error, xTaskCreate() returns (0x%x).",
-                (uint32_t)status);
-    }
 
-    // The task is created but handle is incorrect
-    MB_SLAVE_ASSERT(mbs_opts->mbs_task_handle != NULL);
+    if (start_controller_task)
+    {
+        // Create Modbus controller task
+        status = xTaskCreatePinnedToCore((void*)&modbus_tcp_slave_task,
+                                "modbus_tcp_slave_task",
+                                MB_CONTROLLER_STACK_SIZE,
+                                NULL,
+                                MB_CONTROLLER_PRIORITY,
+                                &mbs_opts->mbs_task_handle,
+                                MB_PORT_TASK_AFFINITY);
+        if (status != pdPASS) {
+            vTaskDelete(mbs_opts->mbs_task_handle);
+            MB_SLAVE_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
+                    "mb controller task creation error, xTaskCreate() returns (0x%x).",
+                    (uint32_t)status);
+        }
+
+        // The task is created but handle is incorrect
+        MB_SLAVE_ASSERT(mbs_opts->mbs_task_handle != NULL);
+    }
+    else
+        mbs_opts->mbs_task_handle = NULL;
 
     // Initialization of interface pointers
     mbs_interface_ptr->init = mbc_tcp_slave_create;
@@ -205,10 +212,10 @@ esp_err_t mbc_tcp_slave_create(void** handler)
     mbs_interface_ptr->set_descriptor = NULL; // Use common descriptor setter
 
     // Initialize stack callback function pointers
-    mbs_interface_ptr->slave_reg_cb_discrete = NULL; // implemented in common layer
-    mbs_interface_ptr->slave_reg_cb_input = NULL;
-    mbs_interface_ptr->slave_reg_cb_holding = NULL;
-    mbs_interface_ptr->slave_reg_cb_coils = NULL;
+    mbs_interface_ptr->slave_reg_cb_discrete = mbc_reg_discrete_slave_cb;
+    mbs_interface_ptr->slave_reg_cb_input = mbc_reg_input_slave_cb;
+    mbs_interface_ptr->slave_reg_cb_holding = mbc_reg_holding_slave_cb;
+    mbs_interface_ptr->slave_reg_cb_coils = mbc_reg_coils_slave_cb;
 
     *handler = (void*)mbs_interface_ptr;
 
diff --git a/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.h b/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.h
index 56ece0d..4fee05b 100644
--- a/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.h
+++ b/freemodbus/tcp_slave/modbus_controller/mbc_tcp_slave.h
@@ -26,6 +26,6 @@
  *     - ESP_OK   Success
  *     - ESP_ERR_NO_MEM Parameter error
  */
-esp_err_t mbc_tcp_slave_create(void** handler);
+esp_err_t mbc_tcp_slave_create(void **handler, bool start_controller_task);
 
 #endif // _MODBUS_TCP_CONTROLLER_SLAVE
diff --git a/freemodbus/tcp_slave/port/port_tcp_slave.c b/freemodbus/tcp_slave/port/port_tcp_slave.c
index c5eee09..e7046d6 100644
--- a/freemodbus/tcp_slave/port/port_tcp_slave.c
+++ b/freemodbus/tcp_slave/port/port_tcp_slave.c
@@ -560,12 +560,6 @@ static void vMBTCPPortServerTask(void *pvParameters)
                                                                             pxClientInfo->xSockId, pxClientInfo->pcIpAddr, xErr);
                                         break;
                                 }
-                                
-                                if (xShutdownSemaphore) {
-                                    xSemaphoreGive(xShutdownSemaphore);
-                                    vTaskDelete(NULL);
-                                }
-
                                 // Close client connection
                                 xMBTCPPortCloseConnection(pxClientInfo);