diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0f536bc..71da2ff 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,9 +43,9 @@ set(srcs
     "mb_transports/tcp/tcp_slave.c"
 )
 
-set(include_dirs mb_transports mb_controller/common/include mb_objects/include mb_ports/common mb_ports/serial mb_ports/tcp)
+set(include_dirs mb_transports mb_controller/common/include mb_objects/common mb_ports/common mb_ports/serial mb_ports/tcp)
 
-set(priv_include_dirs mb_controller/serial mb_controller/tcp mb_controller/common mb_transports/rtu mb_transports/ascii mb_transports/tcp)
+set(priv_include_dirs mb_controller/serial mb_controller/tcp mb_controller/common mb_objects/include mb_transports/rtu mb_transports/ascii mb_transports/tcp)
 
 if(CONFIG_FMB_EXT_TYPE_SUPPORT)
      list(APPEND srcs "mb_controller/common/mb_endianness_utils.c")
diff --git a/Kconfig b/Kconfig
index e6fbdcc..6d4adce 100644
--- a/Kconfig
+++ b/Kconfig
@@ -224,4 +224,21 @@ menu "Modbus configuration"
                 otherwise the only legacy types are supported. The extended types include
                 integer, float, double types with different endianness and size.
 
+    config FMB_CONTROLLER_SLAVE_ID_MAX_SIZE
+        int "Modbus Slave ID maximum buffer size (bytes)"
+        range 4 255
+        default 32
+        depends on FMB_CONTROLLER_SLAVE_ID_SUPPORT
+        help
+                Modbus slave ID buffer size used to store vendor specific ID information
+                for the <Report Slave ID> command.
+
+    config FMB_FUNC_HANDLERS_MAX
+        int "Maximum number of Modbus function handlers"
+        range 16 64
+        default 16
+        help
+                This option defines the maximum number of Modbus command handlers for Modbus master and slave.
+                The option can be useful to register additional commands and its handlers.
+
 endmenu
diff --git a/modbus/mb_controller/common/esp_modbus_master.c b/modbus/mb_controller/common/esp_modbus_master.c
index 7b0e00f..f385a69 100644
--- a/modbus/mb_controller/common/esp_modbus_master.c
+++ b/modbus/mb_controller/common/esp_modbus_master.c
@@ -732,3 +732,33 @@ esp_err_t mbc_master_set_param_data(void* dest, void* src, mb_descr_type_t param
     }
     return err;
 }
+
+/**
+ * Register or override command handler for the command in master command handler table
+ */
+esp_err_t mbc_master_set_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp phandler)
+{
+    MB_RETURN_ON_FALSE(ctx, ESP_ERR_INVALID_STATE, TAG,
+                            "Master interface is not correctly initialized.");
+    mbm_controller_iface_t *mbm_controller = MB_MASTER_GET_IFACE(ctx);
+    mb_base_t *pmb_obj = (mb_base_t *)mbm_controller->mb_base;
+    MB_RETURN_ON_FALSE((pmb_obj && pmb_obj->descr.is_master), ESP_ERR_INVALID_STATE, TAG,
+                            "Master interface is not correctly initialized.");
+    mb_err_enum_t ret = mbm_set_handler(func_code, phandler);
+    return  MB_ERR_TO_ESP_ERR(ret);
+}
+
+/**
+ * Get command master command handler from the slave command handler table
+ */
+esp_err_t mbc_master_get_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp *phandler)
+{
+    MB_RETURN_ON_FALSE(ctx, ESP_ERR_INVALID_STATE, TAG,
+                            "Master interface is not correctly initialized.");
+    mbm_controller_iface_t *mbm_controller = MB_MASTER_GET_IFACE(ctx);
+    mb_base_t *pmb_obj = (mb_base_t *)mbm_controller->mb_base;
+    MB_RETURN_ON_FALSE((pmb_obj && pmb_obj->descr.is_master), ESP_ERR_INVALID_STATE, TAG,
+                            "Master interface is not correctly initialized.");
+    mb_err_enum_t ret = mbm_get_handler(func_code, phandler);
+    return  MB_ERR_TO_ESP_ERR(ret);
+}
\ No newline at end of file
diff --git a/modbus/mb_controller/common/esp_modbus_slave.c b/modbus/mb_controller/common/esp_modbus_slave.c
index 8538ff2..4aee45a 100644
--- a/modbus/mb_controller/common/esp_modbus_slave.c
+++ b/modbus/mb_controller/common/esp_modbus_slave.c
@@ -547,3 +547,33 @@ mb_err_enum_t mbc_reg_discrete_slave_cb(mb_base_t *inst, uint8_t *reg_buffer, ui
     }
     return status;
 }
+
+/**
+ * Register or override command handler for the command in slave command handler table
+ */
+esp_err_t mbc_slave_set_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp phandler)
+{
+    MB_RETURN_ON_FALSE(ctx, ESP_ERR_INVALID_STATE, TAG,
+                            "Slave interface is not correctly initialized.");
+    mbs_controller_iface_t *mbs_controller = MB_SLAVE_GET_IFACE(ctx);
+    mb_base_t *pmb_obj = (mb_base_t *)mbs_controller->mb_base;
+    MB_RETURN_ON_FALSE((pmb_obj && !pmb_obj->descr.is_master), ESP_ERR_INVALID_STATE, TAG,
+                            "Slave interface is not correctly initialized.");
+    mb_err_enum_t ret = mbs_set_handler(func_code, phandler);
+    return  MB_ERR_TO_ESP_ERR(ret);
+}
+
+/**
+ * Get slave command handler from the slave command handler table
+ */
+esp_err_t mbc_slave_get_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp *phandler)
+{
+    MB_RETURN_ON_FALSE(ctx, ESP_ERR_INVALID_STATE, TAG,
+                            "Slave interface is not correctly initialized.");
+    mbs_controller_iface_t *mbs_controller = MB_SLAVE_GET_IFACE(ctx);
+    mb_base_t *pmb_obj = (mb_base_t *)mbs_controller->mb_base;
+    MB_RETURN_ON_FALSE((pmb_obj && !pmb_obj->descr.is_master), ESP_ERR_INVALID_STATE, TAG,
+                            "Slave interface is not correctly initialized.");
+    mb_err_enum_t ret = mbs_get_handler(func_code, phandler);
+    return  MB_ERR_TO_ESP_ERR(ret);
+}
\ No newline at end of file
diff --git a/modbus/mb_controller/common/include/esp_modbus_common.h b/modbus/mb_controller/common/include/esp_modbus_common.h
index 9d2a5e8..52b138a 100644
--- a/modbus/mb_controller/common/include/esp_modbus_common.h
+++ b/modbus/mb_controller/common/include/esp_modbus_common.h
@@ -201,6 +201,27 @@ typedef union
 typedef esp_err_t (*iface_create_fp)(mb_communication_info_t*, void **);    /*!< Interface method create */
 typedef esp_err_t (*iface_method_default_fp)(void *ctx);                   /*!< Interface method default prototype */
 
+/**
+ * @brief The function registers the new function handler for specified command and 
+ *        allows to override the existing one for the communication object.
+ *        If the phandler == 0, the function allows to clear the actual function handler for the `func_code` parameter.
+ * 
+ * @param[in] ctx context pointer to the communication object
+ * @param[in] func_code the function code for the handler
+ * @param[in] phandler the pointer to function handler being used for command
+ *
+ * @return
+ *     - esp_err_t ESP_OK - the function handler is correctly set
+ *     - esp_err_t ESP_ERR_INVALID_ARG - invalid argument of function or parameter descriptor
+ *     - esp_err_t ESP_ERR_INVALID_RESPONSE - an invalid response from slave during processing of parameter
+ *     - esp_err_t ESP_ERR_INVALID_STATE - invalid state during data processing or allocation failure
+ *     - esp_err_t ESP_ERR_NOT_FOUND - the requested slave is not found (not connected or not configured)
+ *     - esp_err_t ESP_ERR_TIMEOUT - operation timed out and no response from slave
+ *     - esp_err_t ESP_ERR_NOT_SUPPORTED - the request command is not supported by slave
+ *     - esp_err_t ESP_FAIL - slave returned an exception or other failure
+*/
+esp_err_t mbc_register_cb(void *ctx, uint8_t func_code, mb_fn_handler_fp phandler);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/modbus/mb_controller/common/include/esp_modbus_master.h b/modbus/mb_controller/common/include/esp_modbus_master.h
index cfe86f0..4734846 100644
--- a/modbus/mb_controller/common/include/esp_modbus_master.h
+++ b/modbus/mb_controller/common/include/esp_modbus_master.h
@@ -466,6 +466,38 @@ mb_err_enum_t mbc_reg_coils_master_cb(mb_base_t *inst, uint8_t *reg_buffer, uint
 */
 esp_err_t mbc_master_set_param_data(void* dest, void* src, mb_descr_type_t param_type, size_t param_size);
 
+/**
+ * @brief The function registers the new function handler for specified command 
+ *        and allows to override the existing one for the master object.
+ *        If the phandler == NULL, the function allows to reset the actual function handler
+ *        for the `func_code` parameter.
+ * 
+ * @param[in] ctx context pointer to the master controller object
+ * @param[in] func_code the function code for the handler
+ * @param[in] phandler the pointer to function handler being used for command
+ *
+ * @return
+ *     - esp_err_t ESP_OK - the function handler is correctly set
+ *     - esp_err_t ESP_ERR_INVALID_ARG - invalid argument of function or parameter descriptor
+ *     - esp_err_t ESP_ERR_INVALID_RESPONSE - an invalid response from slave during processing of parameter
+ *     - esp_err_t ESP_ERR_INVALID_STATE - invalid state during data processing or allocation failure
+ *     - esp_err_t ESP_ERR_NOT_FOUND - the requested slave is not found (not connected or not configured)
+*/
+esp_err_t mbc_master_set_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp phandler);
+
+/**
+ * @brief The function gets function handler for specified command from master command handler table.
+ * 
+ * @param[in] ctx context pointer to the master controller object
+ * @param[in] func_code the function code for the handler
+ * @param[out] phandler the pointer to function handler being returned
+ *
+ * @return
+ *     - esp_err_t ESP_OK - the function handler is returned in the 
+ *     - esp_err_t ESP_ERR_INVALID_ARG - invalid argument of function or parameter descriptor
+*/
+esp_err_t mbc_master_get_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp *phandler);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/modbus/mb_controller/common/include/esp_modbus_slave.h b/modbus/mb_controller/common/include/esp_modbus_slave.h
index fad35ee..d56ba56 100644
--- a/modbus/mb_controller/common/include/esp_modbus_slave.h
+++ b/modbus/mb_controller/common/include/esp_modbus_slave.h
@@ -292,6 +292,37 @@ mb_err_enum_t mbc_reg_discrete_slave_cb(mb_base_t *inst, uint8_t *reg_buffer, ui
  */
 mb_err_enum_t mbc_reg_coils_slave_cb(mb_base_t *inst, uint8_t *reg_buffer, uint16_t address, uint16_t n_coils, mb_reg_mode_enum_t mode) __attribute__ ((weak));
 
+/**
+ * @brief The function registers the new function handler for specified command 
+ *        and allows to override the existing one for the slave object.
+ *        If the phandler == NULL, the function allows to reset the actual function handler
+ *        for the `func_code` parameter.
+ * 
+ * @param[in] ctx context pointer to the slave controller object
+ * @param[in] func_code the function code for the handler
+ * @param[in] phandler the pointer to function handler being used for command
+ *
+ * @return
+ *     - esp_err_t ESP_OK - the function handler is correctly set
+ *     - esp_err_t ESP_ERR_INVALID_ARG - invalid argument of function or parameter descriptor
+ *     - esp_err_t ESP_ERR_INVALID_RESPONSE - an invalid response from slave during processing of parameter
+ *     - esp_err_t ESP_ERR_INVALID_STATE - invalid state during data processing or allocation failure
+*/
+esp_err_t mbc_slave_set_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp phandler);
+
+/**
+ * @brief The function gets function handler for specified command from slave command handler table.
+ * 
+ * @param[in] ctx context pointer to the slave controller object
+ * @param[in] func_code the function code for the handler
+ * @param[out] phandler the pointer to function handler being returned
+ *
+ * @return
+ *     - esp_err_t ESP_OK - the function handler is returned in the 
+ *     - esp_err_t ESP_ERR_INVALID_ARG - invalid argument of function or parameter descriptor
+*/
+esp_err_t mbc_slave_get_handler(void *ctx, uint8_t func_code, mb_fn_handler_fp *phandler);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/modbus/mb_controller/common/mbc_slave.h b/modbus/mb_controller/common/mbc_slave.h
index 3e6a0bb..f585e5e 100644
--- a/modbus/mb_controller/common/mbc_slave.h
+++ b/modbus/mb_controller/common/mbc_slave.h
@@ -18,6 +18,7 @@ extern "C" {
 
 #include "mb_common.h"              // for mb_base_t
 #include "esp_modbus_slave.h"       // for public type defines
+#include "mb_slave.h"
 
 /* ----------------------- Defines ------------------------------------------*/
 #define MB_INST_MIN_SIZE                    (1) // The minimal size of Modbus registers area in bytes
diff --git a/modbus/mb_controller/serial/mbc_serial_master.c b/modbus/mb_controller/serial/mbc_serial_master.c
index fe24903..9986226 100644
--- a/modbus/mb_controller/serial/mbc_serial_master.c
+++ b/modbus/mb_controller/serial/mbc_serial_master.c
@@ -256,9 +256,19 @@ static esp_err_t mbc_serial_master_send_request(void *ctx, mb_param_request_t *r
             break;
 #endif
         default:
-            ESP_LOGE(TAG, "%s: Incorrect or unsupported function in request (%u) ",
-                     __FUNCTION__, mb_command);
-            mb_error = MB_ENOREG;
+            mb_fn_handler_fp phandler = NULL;
+            // check registered function handler
+            mb_error = mbm_get_handler(mb_command, &phandler);
+            if (mb_error == MB_ENOERR) {
+                // send the request for custom command
+                mb_error = mbm_rq_custom(mbm_controller_iface->mb_base, mb_slave_addr, mb_command,
+                                            data_ptr, (uint16_t)(mb_size << 1),
+                                            pdMS_TO_TICKS(MB_MAX_RESP_DELAY_MS));
+                ESP_LOGD(TAG, "%s: Send custom request (%u), error = (0x%d) ", __FUNCTION__, mb_command, (int)mb_error);
+            } else {
+                ESP_LOGE(TAG, "%s: Incorrect or unsupported function in request (%u) ", __FUNCTION__, mb_command);
+                mb_error = MB_ENOREG;
+            }
             break;
         }
     } else {
diff --git a/modbus/mb_controller/tcp/mbc_tcp_master.c b/modbus/mb_controller/tcp/mbc_tcp_master.c
index 46104b9..a803816 100644
--- a/modbus/mb_controller/tcp/mbc_tcp_master.c
+++ b/modbus/mb_controller/tcp/mbc_tcp_master.c
@@ -255,9 +255,19 @@ static esp_err_t mbc_tcp_master_send_request(void *ctx, mb_param_request_t *requ
                 break;
 #endif
             default:
-                ESP_LOGE(TAG, "%s: Incorrect or unsupported function in request (%u) ",
-                                                        __FUNCTION__, (unsigned)mb_command);
-                mb_error = MB_ENOREG;
+                mb_fn_handler_fp phandler = NULL;
+                // check registered function handler
+                mb_error = mbm_get_handler(mb_command, &phandler);
+                if (mb_error == MB_ENOERR) {
+                    // send the request for custom command
+                    mb_error = mbm_rq_custom(mbm_controller_iface->mb_base, mb_slave_addr, mb_command,
+                                                data_ptr, (uint16_t)(mb_size << 1),
+                                                pdMS_TO_TICKS(MB_MAX_RESP_DELAY_MS));
+                    ESP_LOGD(TAG, "%s: Send custom request (%u), error = (0x%d) ", __FUNCTION__, mb_command, (int)mb_error);
+                } else {
+                    ESP_LOGE(TAG, "%s: Incorrect or unsupported function in request (%u) ", __FUNCTION__, mb_command);
+                    mb_error = MB_ENOREG;
+                }
                 break;
         }
     } else {
diff --git a/modbus/mb_objects/include/mb_port_types.h b/modbus/mb_objects/common/mb_port_types.h
similarity index 99%
rename from modbus/mb_objects/include/mb_port_types.h
rename to modbus/mb_objects/common/mb_port_types.h
index 08fcd73..6765501 100644
--- a/modbus/mb_objects/include/mb_port_types.h
+++ b/modbus/mb_objects/common/mb_port_types.h
@@ -5,8 +5,8 @@
  */
 #pragma once
 
-#include "mb_config.h"
 #include "mb_types.h"
+#include "sdkconfig.h"
 
 #if __has_include("esp_idf_version.h")
 #include "esp_idf_version.h"
diff --git a/modbus/mb_objects/include/mb_types.h b/modbus/mb_objects/common/mb_types.h
similarity index 91%
rename from modbus/mb_objects/include/mb_types.h
rename to modbus/mb_objects/common/mb_types.h
index 11ae9c4..256dd37 100644
--- a/modbus/mb_objects/include/mb_types.h
+++ b/modbus/mb_objects/common/mb_types.h
@@ -85,6 +85,26 @@ typedef enum _mb_event_enum {
     EV_MASTER_PROCESS_SUCCESS = 0x0400          /*!< Master error process. */
 } mb_event_enum_t;
 
+/*! \ingroup modbus
+ * \brief Modbus exception types used in the stack.
+ */
+typedef enum _mb_exception_enum
+{
+    MB_EX_NONE = 0x00,
+    MB_EX_ILLEGAL_FUNCTION = 0x01,
+    MB_EX_ILLEGAL_DATA_ADDRESS = 0x02,
+    MB_EX_ILLEGAL_DATA_VALUE = 0x03,
+    MB_EX_SLAVE_DEVICE_FAILURE = 0x04,
+    MB_EX_ACKNOWLEDGE = 0x05,
+    MB_EX_SLAVE_BUSY = 0x06,
+    MB_EX_MEMORY_PARITY_ERROR = 0x08,
+    MB_EX_GATEWAY_PATH_FAILED = 0x0A,
+    MB_EX_GATEWAY_TGT_FAILED = 0x0B,
+    MB_EX_CRITICAL = 0xFF
+} mb_exception_t;
+
+typedef mb_exception_t (*mb_fn_handler_fp)(void *, uint8_t *frame_ptr, uint16_t *len_buf);
+
 /*! \ingroup modbus
  * \brief Error event type
  */
diff --git a/modbus/mb_objects/functions/mbfuncother.c b/modbus/mb_objects/functions/mbfuncother.c
index 7ffb687..e284133 100644
--- a/modbus/mb_objects/functions/mbfuncother.c
+++ b/modbus/mb_objects/functions/mbfuncother.c
@@ -44,6 +44,39 @@
 /* ----------------------- Start implementation -----------------------------*/
 mb_exception_t mb_error_to_exception(mb_err_enum_t error_code);
 
+/**
+ * This function will request read coil.
+ *
+ * @param uid slave address
+ * @param fc custom function code
+ * @param buf additional data to send
+ * @param buf_size size of data to send
+ * @param timeout timeout (-1 will waiting forever)
+ *
+ * @return error code (mb_err_enum_t)
+ */
+mb_err_enum_t mbm_rq_custom(mb_base_t *inst, uint8_t uid, uint8_t fc, uint8_t *buf, uint16_t buf_size, uint32_t tout)
+{
+    uint8_t *mb_frame_ptr;
+    if (!buf || (uid > MB_ADDRESS_MAX) || (buf_size >= (MB_BUFFER_SIZE - 2))) {
+        return MB_EINVAL;
+    }
+    if (!mb_port_event_res_take(inst->port_obj, tout)) {
+        return MB_EBUSY;
+    }
+    inst->get_send_buf(inst, &mb_frame_ptr);
+    inst->set_dest_addr(inst, uid);
+
+    mb_frame_ptr[MB_PDU_FUNC_OFF] = fc;
+
+    memcpy(&mb_frame_ptr[MB_PDU_DATA_OFF], buf, buf_size);
+
+    inst->set_send_len(inst, MB_PDU_SIZE_MIN + buf_size);
+
+    (void)mb_port_event_post(inst->port_obj, EVENT(EV_FRAME_TRANSMIT | EV_TRANS_START));
+    return mb_port_event_wait_req_finish(inst->port_obj);
+}
+
 mb_err_enum_t mbm_rq_report_slave_id(mb_base_t *inst, uint8_t slave_addr, uint32_t timeout)
 {
     uint8_t *mb_frame_ptr = NULL;
diff --git a/modbus/mb_objects/functions/mbutils.c b/modbus/mb_objects/functions/mbutils.c
index 2ee87f3..fe3141e 100644
--- a/modbus/mb_objects/functions/mbutils.c
+++ b/modbus/mb_objects/functions/mbutils.c
@@ -126,4 +126,52 @@ mb_exception_t mb_error_to_exception(mb_err_enum_t error_code)
     return status;
 }
 
+mb_err_enum_t mb_set_handler(mb_fn_handler_t *pfh_table, uint8_t func_code, mb_fn_handler_fp phandler)
+{
+    int index;
+    mb_err_enum_t status = MB_EILLSTATE;
+    if ((0 < func_code) && (func_code <= MB_FUNC_CODE_MAX)) {
+        if (phandler != NULL) {
+            for(index = 0; index < MB_FUNC_HANDLERS_MAX; index++) {
+                if ((pfh_table[index].handler == NULL) ||
+                    (pfh_table[index].handler == phandler)) {
+                    pfh_table[index].func_code = func_code;
+                    pfh_table[index].handler = phandler;
+                    break;
+                }
+            }
+            status = (index != MB_FUNC_HANDLERS_MAX) ? MB_ENOERR : MB_ENORES;
+        } else {
+            for (index = 0; index < MB_FUNC_HANDLERS_MAX; index++) {
+                if (pfh_table[index].func_code == func_code) {
+                    pfh_table[index].func_code = 0;
+                    pfh_table[index].handler = NULL;
+                    break;
+                }
+            }
+            status = (index < MB_FUNC_HANDLERS_MAX) ? MB_ENOERR : MB_ENORES;
+        }
+    } else {
+        status = MB_EINVAL;
+    }
+    return status;
+}
+
+mb_err_enum_t mb_get_handler(mb_fn_handler_t *pfh_table, uint8_t func_code, mb_fn_handler_fp *phandler)
+{
+    int index;
+    mb_err_enum_t status = MB_EILLSTATE;
+    if (pfh_table && phandler && (0 < func_code) && (func_code <= MB_FUNC_CODE_MAX)) {
+        for(index = 0; index < MB_FUNC_HANDLERS_MAX; index++) {
+            if (func_code == pfh_table[index].func_code) {
+                *phandler = pfh_table[index].handler;
+                break;
+            }
+        }
+        status = (index < MB_FUNC_HANDLERS_MAX) ? MB_ENOERR : MB_ENORES;
+    } else {
+        status = MB_EINVAL;
+    }
+    return status;
+}
 
diff --git a/modbus/mb_objects/include/mb_common.h b/modbus/mb_objects/include/mb_common.h
index b923859..b072746 100644
--- a/modbus/mb_objects/include/mb_common.h
+++ b/modbus/mb_objects/include/mb_common.h
@@ -152,7 +152,8 @@ struct mb_base_t
     mb_rw_callbacks_t rw_cbs;
 };
 
-typedef struct _port_tcp_opts mb_tcp_opts_t;
+mb_err_enum_t mb_set_handler(mb_fn_handler_t *pfh_table, uint8_t func_code, mb_fn_handler_fp phandler);
+mb_err_enum_t mb_get_handler(mb_fn_handler_t *pfh_table, uint8_t func_code, mb_fn_handler_fp *phandler);
 
 #if (CONFIG_FMB_COMM_MODE_ASCII_EN || CONFIG_FMB_COMM_MODE_RTU_EN)
 
@@ -163,8 +164,13 @@ mb_err_enum_t mbs_ascii_create(mb_serial_opts_t *ser_opts, void **in_out_obj);
 
 #endif
 
+#if (CONFIG_FMB_COMM_MODE_TCP_EN)
+
+typedef struct _port_tcp_opts mb_tcp_opts_t;
 mb_err_enum_t mbs_tcp_create(mb_tcp_opts_t *tcp_opts, void **in_out_obj);
 
+#endif
+
 mb_err_enum_t mbs_delete(mb_base_t *inst);
 mb_err_enum_t mbs_enable(mb_base_t *inst);
 mb_err_enum_t mbs_disable(mb_base_t *inst);
diff --git a/modbus/mb_objects/include/mb_config.h b/modbus/mb_objects/include/mb_config.h
index f2bdd42..4aad1b7 100644
--- a/modbus/mb_objects/include/mb_config.h
+++ b/modbus/mb_objects/include/mb_config.h
@@ -115,7 +115,7 @@ extern "C" {
  * the sum of all enabled functions in this file and custom function
  * handlers. If set to small adding more functions will fail.
  */
-#define MB_FUNC_HANDLERS_MAX                    (16)
+#define MB_FUNC_HANDLERS_MAX                    (CONFIG_FMB_FUNC_HANDLERS_MAX)
 
 /*! \brief Number of bytes which should be allocated for the <em>Report Slave ID
  *    </em>command.
diff --git a/modbus/mb_objects/include/mb_func.h b/modbus/mb_objects/include/mb_func.h
index 86579e6..56cb9f3 100644
--- a/modbus/mb_objects/include/mb_func.h
+++ b/modbus/mb_objects/include/mb_func.h
@@ -12,6 +12,8 @@
 extern "C" {
 #endif
 
+#define MB_FUNC_CODE_MAX 127
+
 typedef struct mb_base_t mb_base_t;
 
 #if MB_FUNC_OTHER_REP_SLAVEID_ENABLED
diff --git a/modbus/mb_objects/include/mb_master.h b/modbus/mb_objects/include/mb_master.h
index 6acc8cb..b02a6b6 100644
--- a/modbus/mb_objects/include/mb_master.h
+++ b/modbus/mb_objects/include/mb_master.h
@@ -10,7 +10,7 @@
 extern "C" {
 #endif
 
-typedef struct mb_base_t mb_base_t;  /*!< Type of moddus object */
+typedef struct mb_base_t mb_base_t;  /*!< Type of modbus object */
 
 mb_err_enum_t mbm_rq_read_inp_reg(mb_base_t *inst, uint8_t snd_addr, uint16_t reg_addr, uint16_t reg_num, uint32_t tout);
 mb_err_enum_t mbm_rq_write_holding_reg(mb_base_t *inst, uint8_t snd_addr, uint16_t reg_addr, uint16_t reg_data, uint32_t tout);
@@ -26,6 +26,7 @@ mb_err_enum_t mbm_rq_write_multi_coils(mb_base_t *inst, uint8_t snd_addr, uint16
 #if MB_FUNC_OTHER_REP_SLAVEID_ENABLED
 mb_err_enum_t mbm_rq_report_slave_id(mb_base_t *inst, uint8_t slave_addr, uint32_t timeout);
 mb_exception_t mbm_fn_report_slave_id(mb_base_t *inst, uint8_t * pframe, uint16_t *usLen);
+mb_err_enum_t mbm_rq_custom(mb_base_t *inst, uint8_t uid, uint8_t fc, uint8_t *buf, uint16_t buf_size, uint32_t tout);
 
 /*! \ingroup modbus_registers
  * \brief The common callback function used to transfer common data as bytes from command buffer in little endian format.
@@ -44,6 +45,12 @@ mb_exception_t mbm_fn_report_slave_id(mb_base_t *inst, uint8_t * pframe, uint16_
 mb_err_enum_t mbc_reg_common_cb(mb_base_t *inst, uint8_t *pdata, uint16_t address, uint16_t bytes);
 #endif
 
+// The function to register custom function handler for master
+mb_err_enum_t mbm_set_handler(uint8_t func_code, mb_fn_handler_fp phandler);
+
+// The helper function to get custom function handler for master
+mb_err_enum_t mbm_get_handler(uint8_t fc, mb_fn_handler_fp *phandler);
+
 #ifdef __cplusplus
 }
 #endif
\ No newline at end of file
diff --git a/modbus/mb_objects/include/mb_proto.h b/modbus/mb_objects/include/mb_proto.h
index 7f710f3..1a987b8 100644
--- a/modbus/mb_objects/include/mb_proto.h
+++ b/modbus/mb_objects/include/mb_proto.h
@@ -38,22 +38,6 @@ typedef enum _mb_commands_enum
 
 /* ----------------------- Type definitions ---------------------------------*/
 
-typedef enum
-{
-    MB_EX_NONE = 0x00,
-    MB_EX_ILLEGAL_FUNCTION = 0x01,
-    MB_EX_ILLEGAL_DATA_ADDRESS = 0x02,
-    MB_EX_ILLEGAL_DATA_VALUE = 0x03,
-    MB_EX_SLAVE_DEVICE_FAILURE = 0x04,
-    MB_EX_ACKNOWLEDGE = 0x05,
-    MB_EX_SLAVE_BUSY = 0x06,
-    MB_EX_MEMORY_PARITY_ERROR = 0x08,
-    MB_EX_GATEWAY_PATH_FAILED = 0x0A,
-    MB_EX_GATEWAY_TGT_FAILED = 0x0B
-} mb_exception_t;
-
-typedef mb_exception_t (*mb_fn_handler_fp)(void *, uint8_t *frame_ptr, uint16_t *len_buf);
-
 typedef struct
 {
     uint8_t func_code;
diff --git a/modbus/mb_objects/include/mb_slave.h b/modbus/mb_objects/include/mb_slave.h
index 1e59daf..bde4e0a 100644
--- a/modbus/mb_objects/include/mb_slave.h
+++ b/modbus/mb_objects/include/mb_slave.h
@@ -15,6 +15,10 @@ extern "C" {
 mb_exception_t mbs_fn_report_slave_id(mb_base_t *inst, uint8_t *frame_ptr, uint16_t *plen_buf);
 #endif
 
+// The helper function to register custom function handler for slave
+mb_err_enum_t mbs_set_handler(uint8_t func_code, mb_fn_handler_fp phandler);
+mb_err_enum_t mbs_get_handler(uint8_t func_code, mb_fn_handler_fp *phandler);
+
 #ifdef __cplusplus
 }
 #endif
\ No newline at end of file
diff --git a/modbus/mb_objects/mb_master.c b/modbus/mb_objects/mb_master.c
index b68ee35..8862646 100644
--- a/modbus/mb_objects/mb_master.c
+++ b/modbus/mb_objects/mb_master.c
@@ -16,9 +16,15 @@
 
 static const char *TAG = "mb_object.master";
 
+#if (MB_MASTER_ASCII_ENABLED || MB_MASTER_RTU_ENABLED) 
+
+typedef struct _port_serial_opts mb_serial_opts_t;
+
+#endif
+
 #if (MB_MASTER_ASCII_ENABLED || MB_MASTER_RTU_ENABLED || MB_MASTER_TCP_ENABLED)
 
-static const mb_fn_handler_t master_handlers[MB_FUNC_HANDLERS_MAX] =
+static mb_fn_handler_t master_handlers[MB_FUNC_HANDLERS_MAX] =
 {
 #if MB_FUNC_OTHER_REP_SLAVEID_ENABLED
         {MB_FUNC_OTHER_REPORT_SLAVEID, (void *)mbm_fn_report_slave_id},
@@ -57,7 +63,6 @@ typedef struct
     mb_base_t base;
     mb_comm_mode_t cur_mode;
     mb_state_enum_t cur_state;
-    const mb_fn_handler_t *func_handlers;
     uint8_t *rcv_frame;
     uint8_t *snd_frame;
     uint16_t pdu_snd_len;
@@ -82,7 +87,55 @@ static void mbm_set_dest_addr(mb_base_t *inst, uint8_t dest_addr);
 static uint8_t mbm_get_dest_addr(mb_base_t *inst);
 static void mbm_get_pdu_send_buf(mb_base_t *inst, uint8_t **pbuf);
 
-typedef struct _port_serial_opts mb_serial_opts_t;
+static _lock_t s_mbm_lock; // lock section for command handlers
+
+mb_err_enum_t mbm_set_handler(uint8_t func_code, mb_fn_handler_fp phandler)
+{
+    mb_err_enum_t status = MB_EILLSTATE;
+    CRITICAL_SECTION(s_mbm_lock) {
+        status = mb_set_handler(&master_handlers[0], func_code, phandler);
+    }
+    return status;
+}
+
+// The helper function to register custom function handler for master
+mb_err_enum_t mbm_get_handler(uint8_t fc, mb_fn_handler_fp *phandler)
+{
+    mb_err_enum_t status = MB_EINVAL;
+    if (phandler) {
+        CRITICAL_SECTION(s_mbm_lock) {
+            status = mb_get_handler(&master_handlers[0], fc, phandler);
+        }
+    }
+    return status;
+}
+
+static mb_err_enum_t mbm_check_invoke_handler(mb_base_t *inst, uint8_t fc, uint8_t *pbuf, uint16_t *plen)
+{
+    mb_exception_t exception = MB_EX_ILLEGAL_FUNCTION;
+    if (!fc) {
+        return MB_EX_ILLEGAL_FUNCTION;
+    }
+    if (fc & MB_FUNC_ERROR) {
+        exception = (mb_exception_t)pbuf[MB_PDU_DATA_OFF];
+        return exception;
+    }
+    CRITICAL_SECTION(s_mbm_lock) {
+        for (int i = 0; i < MB_FUNC_HANDLERS_MAX; i++) {
+            /* No more function handlers registered. Abort. */
+            if (master_handlers[i].func_code == 0) {
+                ESP_LOGE(TAG, MB_OBJ_FMT": function (0x%x), handler is missing.", MB_OBJ_PARENT(inst), (int)fc);
+                break;
+            }
+            if (master_handlers[i].func_code == fc) {
+                exception = master_handlers[i].handler(inst, pbuf, plen);
+                ESP_LOGD(__func__, MB_OBJ_FMT": function (0x%x), invoke handler.", MB_OBJ_PARENT(inst), (int)fc);
+                break;
+            }
+        }
+    }
+    return exception;
+}
 
 #if (MB_MASTER_RTU_ENABLED)
 
@@ -121,7 +174,6 @@ mb_err_enum_t mbm_rtu_create(mb_serial_opts_t *ser_opts, void **in_out_obj)
     ret = mbm_rtu_transp_create(ser_opts, (void **)&transp_obj);
     MB_GOTO_ON_FALSE((transp_obj && (ret == MB_ENOERR)), MB_EILLSTATE, error,
                      TAG, "transport creation, err: %d", (int)ret);
-    mbm_obj->func_handlers = master_handlers;
     mbm_obj->cur_mode = ser_opts->mode;
     mbm_obj->cur_state = STATE_DISABLED;
     transp_obj->get_tx_frm(transp_obj, (uint8_t **)&mbm_obj->snd_frame);
@@ -183,7 +235,6 @@ mb_err_enum_t mbm_ascii_create(mb_serial_opts_t *ser_opts, void **in_out_obj)
     ret = mbm_ascii_transp_create(ser_opts, (void **)&transp_obj);
     MB_GOTO_ON_FALSE((transp_obj && (ret == MB_ENOERR)), MB_EILLSTATE, error,
                      TAG, "transport creation, err: %d", (int)ret);
-    mbm_obj->func_handlers = master_handlers;
     mbm_obj->cur_mode = ser_opts->mode;
     mbm_obj->cur_state = STATE_DISABLED;
     transp_obj->get_tx_frm(transp_obj, (uint8_t **)&mbm_obj->snd_frame);
@@ -245,7 +296,6 @@ mb_err_enum_t mbm_tcp_create(mb_tcp_opts_t *tcp_opts, void **in_out_obj)
     ret = mbm_tcp_transp_create(tcp_opts, (void **)&transp_obj);
     MB_GOTO_ON_FALSE((transp_obj && (ret == MB_ENOERR)), MB_EILLSTATE, error,
                      TAG, "transport creation, err: %d", (int)ret);
-    mbm_obj->func_handlers = master_handlers;
     mbm_obj->cur_mode = tcp_opts->mode;
     mbm_obj->cur_state = STATE_DISABLED;
     transp_obj->get_tx_frm(transp_obj, (uint8_t **)&mbm_obj->snd_frame);
@@ -271,7 +321,7 @@ error:
 
 mb_err_enum_t mbm_delete(mb_base_t *inst)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     mb_err_enum_t status = MB_ENOERR;
     if (mbm_obj->cur_state == STATE_DISABLED) {
         if (MB_OBJ(mbm_obj->base.transp_obj)->frm_delete) {
@@ -303,7 +353,7 @@ mb_err_enum_t mbm_delete(mb_base_t *inst)
 
 mb_err_enum_t mbm_enable(mb_base_t *inst)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     mb_err_enum_t status = MB_ENOERR;
     CRITICAL_SECTION(inst->lock)
     {
@@ -322,7 +372,7 @@ mb_err_enum_t mbm_enable(mb_base_t *inst)
 mb_err_enum_t mbm_disable(mb_base_t *inst)
 {
     mb_err_enum_t status = MB_ENOERR;
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     CRITICAL_SECTION(inst->lock)
     {
         if (mbm_obj->cur_state == STATE_ENABLED) {
@@ -340,7 +390,7 @@ mb_err_enum_t mbm_disable(mb_base_t *inst)
 
 static void mbm_get_pdu_send_buf(mb_base_t *inst, uint8_t **pbuf)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     MB_OBJ(mbm_obj->base.transp_obj)->get_tx_frm(mbm_obj->base.transp_obj, pbuf);
 }
 
@@ -353,7 +403,7 @@ static void mbm_get_pdu_recv_buf(mb_base_t *inst, uint8_t **pbuf)
 
 static void mbm_set_pdu_send_length(mb_base_t *inst, uint16_t length)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     CRITICAL_SECTION(inst->lock)
     {
         mbm_obj->pdu_snd_len = length;
@@ -362,13 +412,13 @@ static void mbm_set_pdu_send_length(mb_base_t *inst, uint16_t length)
 
 static uint16_t mbm_get_pdu_send_length(mb_base_t *inst)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     return mbm_obj->pdu_snd_len;
 }
 
 static void mbm_set_dest_addr(mb_base_t *inst, uint8_t dest_addr)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     CRITICAL_SECTION(inst->lock)
     {
         mbm_obj->master_dst_addr = dest_addr;
@@ -377,7 +427,7 @@ static void mbm_set_dest_addr(mb_base_t *inst, uint8_t dest_addr)
 
 static uint8_t mbm_get_dest_addr(mb_base_t *inst)
 {
-    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);;
+    mbm_object_t *mbm_obj = MB_GET_OBJ_CTX(inst, mbm_object_t, base);
     return mbm_obj->master_dst_addr;
 }
 
@@ -496,31 +546,18 @@ mb_err_enum_t mbm_poll(mb_base_t *inst)
                     ESP_LOGD(TAG, MB_OBJ_FMT":EV_EXECUTE", MB_OBJ_PARENT(inst));
                     mbm_obj->func_code = mbm_obj->rcv_frame[MB_PDU_FUNC_OFF];
                     exception = MB_EX_ILLEGAL_FUNCTION;
-                    /* If receive frame has exception. The receive function code highest bit is 1.*/
-                    if (mbm_obj->func_code & MB_FUNC_ERROR) {
-                        exception = (mb_exception_t)mbm_obj->rcv_frame[MB_PDU_DATA_OFF];
-                    } else {
-                        for (int i = 0; i < MB_FUNC_HANDLERS_MAX; i++) {
-                            /* No more function handlers registered. Abort. */
-                            if (mbm_obj->func_handlers[i].func_code == 0) {
-                                break;
-                            }
-                            if (mbm_obj->func_handlers[i].func_code == mbm_obj->func_code) {
-                                /* If master request is broadcast,
-                                * the master need execute function for all slave.
-                                */
-                                if (MB_OBJ(inst->transp_obj)->frm_is_bcast(inst->transp_obj)) {
-                                    length = mbm_obj->pdu_snd_len;
-                                    for (int j = 1; j <= MB_MASTER_TOTAL_SLAVE_NUM; j++) {
-                                        mbm_set_dest_addr(inst, j);
-                                        exception = mbm_obj->func_handlers[i].handler(inst, mbm_obj->rcv_frame, &length);
-                                    }
-                                } else {
-                                    exception = mbm_obj->func_handlers[i].handler(inst, mbm_obj->rcv_frame, &mbm_obj->pdu_rcv_len);
-                                }
-                                break;
-                            }
+                    /* If master request is broadcast,
+                     * the master need to execute function for all slaves.
+                     */
+                    if (MB_OBJ(inst->transp_obj)->frm_is_bcast(inst->transp_obj)) {
+                        length = mbm_obj->pdu_snd_len;
+                        for (int j = 1; j <= MB_MASTER_TOTAL_SLAVE_NUM; j++) {
+                            mbm_set_dest_addr(inst, j);
+                            exception = mbm_check_invoke_handler(inst, mbm_obj->func_code, mbm_obj->rcv_frame, &length);
                         }
+                    } else {
+                        ESP_LOGD(__func__, MB_OBJ_FMT": function (0x%x), invoke handler.", MB_OBJ_PARENT(inst), (int)mbm_obj->func_code);
+                        exception = mbm_check_invoke_handler(inst, mbm_obj->func_code, mbm_obj->rcv_frame, &mbm_obj->pdu_rcv_len);
                     }
                     /* If master has exception, will send error process event. Otherwise the master is idle.*/
                     if (exception != MB_EX_NONE) {
diff --git a/modbus/mb_objects/mb_slave.c b/modbus/mb_objects/mb_slave.c
index 5c77439..864a107 100644
--- a/modbus/mb_objects/mb_slave.c
+++ b/modbus/mb_objects/mb_slave.c
@@ -30,7 +30,6 @@ typedef struct
     // here are slave object properties and methods
     uint8_t mb_address;
     mb_comm_mode_t cur_mode;
-    const mb_fn_handler_t *func_handlers;
     mb_state_enum_t cur_state;
     uint8_t *frame;
     uint16_t length;
@@ -74,11 +73,58 @@ static mb_fn_handler_t slave_handlers[MB_FUNC_HANDLERS_MAX] =
 #endif
 };
 
+static _lock_t s_mbs_lock;
+
 mb_err_enum_t mbs_delete(mb_base_t *inst);
 mb_err_enum_t mbs_enable(mb_base_t *inst);
 mb_err_enum_t mbs_disable(mb_base_t *inst);
 mb_err_enum_t mbs_poll(mb_base_t *inst);
 
+// The helper function to register custom function handler for slave
+mb_err_enum_t mbs_set_handler(uint8_t fc, mb_fn_handler_fp phandler)
+{
+    mb_err_enum_t status = MB_EINVAL;
+    CRITICAL_SECTION(s_mbs_lock) {
+        status = mb_set_handler(&slave_handlers[0], fc, phandler);
+    }
+    return status;
+}
+
+// The helper function to register custom function handler for slave
+mb_err_enum_t mbs_get_handler(uint8_t fc, mb_fn_handler_fp *phandler)
+{
+    mb_err_enum_t status = MB_EINVAL;
+    if (phandler) {
+        CRITICAL_SECTION(s_mbs_lock) {
+            status = mb_get_handler(&slave_handlers[0], fc, phandler);
+        }
+    }
+    return status;
+}
+
+static mb_err_enum_t mbs_check_invoke_handler(mb_base_t *inst, uint8_t fc, uint8_t *pbuf, uint16_t *plen)
+{
+    mb_exception_t exception = MB_EX_ILLEGAL_FUNCTION;
+    if (!fc || (fc & MB_FUNC_ERROR)) {
+        return MB_EX_ILLEGAL_FUNCTION;
+    }
+    CRITICAL_SECTION(s_mbs_lock) {
+        for (int i = 0; i < MB_FUNC_HANDLERS_MAX; i++) {
+            /* No more function handlers registered. Abort. */
+            if (slave_handlers[i].func_code == 0) {
+                ESP_LOGD(TAG, MB_OBJ_FMT": function (0x%x), handler is missing.", MB_OBJ_PARENT(inst), (int)fc);
+                break;
+            }
+            if (slave_handlers[i].func_code == fc) {
+                exception = slave_handlers[i].handler(inst, pbuf, plen);
+                ESP_LOGD(TAG, MB_OBJ_FMT": function (0x%x), invoke handler.", MB_OBJ_PARENT(inst), (int)fc);
+                break;
+            }
+        }
+    }
+    return exception;
+}
+
 #if (MB_SLAVE_RTU_ENABLED)
 
 mb_err_enum_t mbs_rtu_create(mb_serial_opts_t *ser_opts, void **in_out_obj)
@@ -111,7 +157,6 @@ mb_err_enum_t mbs_rtu_create(mb_serial_opts_t *ser_opts, void **in_out_obj)
     ret = mbs_rtu_transp_create(ser_opts, (void **)&transp_obj);
     MB_GOTO_ON_FALSE((transp_obj && (ret == MB_ENOERR)), MB_EILLSTATE, error, 
                                 TAG, "transport creation, err: %d", (int)ret);
-    mbs_obj->func_handlers = slave_handlers;
     mbs_obj->cur_mode = ser_opts->mode;
     mbs_obj->mb_address = ser_opts->uid;
     mbs_obj->cur_state = STATE_DISABLED;
@@ -166,7 +211,6 @@ mb_err_enum_t mbs_ascii_create(mb_serial_opts_t *ser_opts, void **in_out_obj)
     ret = mbs_ascii_transp_create(ser_opts, (void **)&transp_obj);
     MB_GOTO_ON_FALSE((transp_obj && (ret == MB_ENOERR)), MB_EILLSTATE, error, 
                         TAG, "transport creation, err: %d", (int)ret);
-    mbs_obj->func_handlers = slave_handlers;
     mbs_obj->cur_mode = ser_opts->mode;
     mbs_obj->mb_address = ser_opts->uid;
     mbs_obj->cur_state = STATE_DISABLED;
@@ -222,7 +266,6 @@ mb_err_enum_t mbs_tcp_create(mb_tcp_opts_t *tcp_opts, void **in_out_obj)
     ret = mbs_tcp_transp_create(tcp_opts, (void **)&transp_obj);
     MB_GOTO_ON_FALSE((transp_obj && (ret == MB_ENOERR)), MB_EILLSTATE, error, 
                                 TAG, "transport creation, err: %d", (int)ret);
-    mbs_obj->func_handlers = slave_handlers;
     mbs_obj->cur_mode = tcp_opts->mode;
     mbs_obj->mb_address = tcp_opts->uid;
     mbs_obj->cur_state = STATE_DISABLED;
@@ -358,20 +401,7 @@ mb_err_enum_t mbs_poll(mb_base_t *inst)
                 MB_RETURN_ON_FALSE(mbs_obj->frame, MB_EILLSTATE, TAG, "receive buffer fail.");
                 ESP_LOGD(TAG, MB_OBJ_FMT":EV_EXECUTE", MB_OBJ_PARENT(inst));
                 mbs_obj->func_code = mbs_obj->frame[MB_PDU_FUNC_OFF];
-                exception = MB_EX_ILLEGAL_FUNCTION;
-                // If receive frame has exception. The receive function code highest bit is 1.
-                for (int i = 0; (i < MB_FUNC_HANDLERS_MAX); i++) {
-                    // No more function handlers registered. Abort.
-                    if (mbs_obj->func_handlers[i].func_code == 0) {
-                        ESP_LOGD(TAG, MB_OBJ_FMT": function (0x%x), handler is not found.", MB_OBJ_PARENT(inst), (int)mbs_obj->func_code);
-                        break;
-                    }
-                    if ((mbs_obj->func_handlers[i].func_code) == mbs_obj->func_code) {
-                        ESP_LOGD(TAG, MB_OBJ_FMT": function (0x%x), start handler.", MB_OBJ_PARENT(inst), (int)mbs_obj->func_code);
-                        exception = mbs_obj->func_handlers[i].handler(inst, mbs_obj->frame, &mbs_obj->length);
-                        break;
-                    }
-                }
+                exception = mbs_check_invoke_handler(inst, mbs_obj->func_code, mbs_obj->frame, &mbs_obj->length);
                 // If the request was not sent to the broadcast address, return a reply.
                 if ((mbs_obj->rcv_addr != MB_ADDRESS_BROADCAST) || (mbs_obj->cur_mode == MB_TCP)) {
                     if (exception != MB_EX_NONE) {
diff --git a/modbus/mb_ports/common/port_common.h b/modbus/mb_ports/common/port_common.h
index d366b25..5dcf30a 100644
--- a/modbus/mb_ports/common/port_common.h
+++ b/modbus/mb_ports/common/port_common.h
@@ -29,6 +29,7 @@ extern "C"
 #define MB_EVENT_QUEUE_TIMEOUT_MAX_MS   (3000)
 #define MB_EVENT_QUEUE_TIMEOUT          (pdMS_TO_TICKS(CONFIG_FMB_EVENT_QUEUE_TIMEOUT))
 #define MB_EVENT_QUEUE_TIMEOUT_MAX      (pdMS_TO_TICKS(MB_EVENT_QUEUE_TIMEOUT_MAX_MS))
+#define MB_MS_TO_TICKS(time_ms)         ((TickType_t)(((time_ms)/portTICK_PERIOD_MS)))
 
 int lock_obj(_lock_t *plock);
 void unlock_obj(_lock_t *plock);