diff --git a/CMakeLists.txt b/CMakeLists.txt
index c88b605..e97758e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,10 +43,10 @@ set(srcs
     "common/esp_modbus_master_serial.c"
     "common/esp_modbus_slave_serial.c")
 
-set(include_dirs common/include)
+set(include_dirs common/include port modbus/include)
 
-set(priv_include_dirs common port modbus modbus/ascii modbus/functions
-                      modbus/rtu modbus/tcp modbus/include)
+set(priv_include_dirs common modbus modbus/ascii modbus/functions
+                      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/Kconfig b/Kconfig
index 134bad0..8edf820 100644
--- a/Kconfig
+++ b/Kconfig
@@ -211,5 +211,13 @@ menu "Modbus configuration"
                 to send timeout events from the callback function called from ISR.
                 This option has dependency with the UART_ISR_IN_IRAM option which places UART interrupt
                 handler into IRAM to prevent delays related to processing of UART events.
+    
+    config FMB_FUNC_HANDLERS_MAX
+        int "Number "
+        range 0 100
+        default 16
+        help
+                Modbus stack event queue timeout in milliseconds. This may help to optimize
+                Modbus stack event processing time.
 
 endmenu
diff --git a/freemodbus/modbus/include/mbconfig.h b/freemodbus/modbus/include/mbconfig.h
index 6dbb9ca..3db7f05 100644
--- a/freemodbus/modbus/include/mbconfig.h
+++ b/freemodbus/modbus/include/mbconfig.h
@@ -132,7 +132,7 @@ PR_BEGIN_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/freemodbus/modbus/include/mbport.h b/freemodbus/modbus/include/mbport.h
index c5e9ff2..a5408f1 100644
--- a/freemodbus/modbus/include/mbport.h
+++ b/freemodbus/modbus/include/mbport.h
@@ -149,6 +149,8 @@ void            vMBPortClose( void );
 
 void            xMBPortSerialClose( void );
 
+//eMBErrorCode    eMBRegisterCB( UCHAR ucFunctionCode, pxMBFunctionHandler pxHandler );
+
 void            vMBPortSerialEnable( BOOL xRxEnable, BOOL xTxEnable );
 
 BOOL            xMBPortSerialGetByte( CHAR * pucByte );
diff --git a/freemodbus/port/port.h b/freemodbus/port/port.h
index ffdb3b5..061b098 100644
--- a/freemodbus/port/port.h
+++ b/freemodbus/port/port.h
@@ -50,7 +50,8 @@
 #include "driver/timer.h"
 #endif
 
-#include "mbconfig.h"
+//#include "mbconfig.h"
+#include "sdkconfig.h"
 
 #define INLINE                      inline
 #define PR_BEGIN_EXTERN_C           extern "C" {
diff --git a/test/serial/mb_serial_master/main/master.c b/test/serial/mb_serial_master/main/master.c
index 79ad299..ca3f476 100644
--- a/test/serial/mb_serial_master/main/master.c
+++ b/test/serial/mb_serial_master/main/master.c
@@ -292,11 +292,23 @@ static esp_err_t master_init(void)
     return err;
 }
 
+//#include "port.h"
+#include "mb_m.h"
+
+eMBException functionHandler( UCHAR * pucFrame, USHORT * pusLength )
+{
+    return MB_EX_GATEWAY_PATH_FAILED;
+}
+
+extern eMBErrorCode eMBRegisterCB( UCHAR ucFunctionCode, pxMBFunctionHandler pxHandler );
+
 void app_main(void)
 {
     // Initialization of device peripheral and objects
     ESP_ERROR_CHECK(master_init());
     vTaskDelay(10);
 
+    eMBRegisterCB(10, &functionHandler);
+
     master_operation_func(NULL);
 }