diff --git a/src/modbus.c b/src/modbus.c
index fef189d..ef20165 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -40,15 +40,44 @@ ModbusError_t mb_handleRequest(
         size_t *resp_size
 )
 {
-    uint16_t txn_id, msglen, protocol_id, ref, count, ref2, count2, value, and_mask, or_mask;
+    uint16_t txn_id, protocol_id, ref;
+
+#ifdef MB_SUPPORT_FC22
+    uint16_t and_mask, or_mask;
+#endif
+
+#ifdef MB_SUPPORT_FC23
+    uint16_t ref2, count2;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02)
     bool bvalue;
+#endif
+
+#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC04) \
+    || defined(MB_SUPPORT_FC16) || defined(MB_SUPPORT_FC22) || defined(MB_SUPPORT_FC23) \
+    || defined(MB_SUPPORT_FC05) || defined(MB_SUPPORT_FC06)
+    uint16_t value;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02) || defined(MB_SUPPORT_FC15)
+    uint8_t scratch, bytecount, bitcnt;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02) || defined(MB_SUPPORT_FC03) \
+    || defined(MB_SUPPORT_FC04) || defined(MB_SUPPORT_FC15) || defined(MB_SUPPORT_FC16) \
+    || defined(MB_SUPPORT_FC23)
+    uint16_t count;
+#endif
+
     ModbusException_t exc = MB_EXCEPTION_OK;
     size_t numbytes;
-    uint8_t fcx, bytecount, bitcnt, scratch;
+    uint8_t fcx;
+
     const size_t TCP_RESP_OVERHEAD = 8;
     const size_t RTU_RESP_OVERHEAD = 4;
     const size_t overhead = ms->proto == MB_PROTO_TCP ? TCP_RESP_OVERHEAD : RTU_RESP_OVERHEAD;
-    pb_mark_t resp_fc_mark, resp_len_mark, resp_pld_start_mark;
+    pb_mark_t resp_fc_mark = NULL, resp_len_mark = NULL, resp_pld_start_mark = NULL;
 
     const bool tcp = ms->proto == MB_PROTO_TCP;
     PayloadParser pp = pp_start_be(req, req_size, NULL);
@@ -58,12 +87,11 @@ ModbusError_t mb_handleRequest(
         /* Parse header */
         txn_id = pp_u16(&pp);
         protocol_id = pp_u16(&pp);
-        msglen = pp_u16(&pp);
+        pp_skip(&pp, 2); // msglen
         if (protocol_id != 0) {
             return MB_ERR_BADPROTO;
         }
     } else {
-        msglen = req_size;
         /* check  CRC */
         uint16_t crc = crc16_init();
         for (int pos = 0; pos < req_size /* size of CRC */; pos++) {
@@ -108,6 +136,7 @@ ModbusError_t mb_handleRequest(
     }
 
     switch (fcx) {
+#ifdef MB_SUPPORT_FC05
         case FC05_WRITE_SINGLE_COIL:
             if (!ms->writeCoil) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -133,17 +162,24 @@ ModbusError_t mb_handleRequest(
             pb_u16(&pb, ref);
             pb_u16(&pb, value);
             break;
+#endif
 
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02)
         case FC01_READ_COILS:
         case FC02_READ_DISCRETES:
             /* check we have the needed function */
+#ifdef MB_SUPPORT_FC01
             if (fcx == FC01_READ_COILS && !ms->readCoil) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
                 goto exception;
-            } else if (!ms->readDiscrete) {
+            }
+#endif
+#ifdef MB_SUPPORT_FC02
+            if (fcx == FC02_READ_DISCRETES && !ms->readDiscrete) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
                 goto exception;
             }
+#endif
             ref = pp_u16(&pp);
             count = pp_u16(&pp);
             if (!pp_ok(&pp)) {
@@ -158,11 +194,16 @@ ModbusError_t mb_handleRequest(
             bitcnt = 0;
             scratch = 0;
             while (count-- > 0) {
+#ifdef MB_SUPPORT_FC01
                 if (fcx == FC01_READ_COILS) {
                     exc = ms->readCoil(ms, ref++, &bvalue);
-                } else {
+                }
+#endif
+#ifdef MB_SUPPORT_FC02
+                if (fcx == FC02_READ_DISCRETES) {
                     exc = ms->readDiscrete(ms, ref++, &bvalue);
                 }
+#endif
                 if (exc != 0) {
                     goto exception;
                 }
@@ -178,18 +219,24 @@ ModbusError_t mb_handleRequest(
                 pb_u8(&pb, scratch);
             }
             break;
+#endif
 
+#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC04)
         case FC03_READ_HOLDING_REGISTERS:
         case FC04_READ_INPUT_REGISTERS:
+#ifdef MB_SUPPORT_FC03
             /* check we have the needed function */
             if (fcx == FC03_READ_HOLDING_REGISTERS && !ms->readHolding) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
                 goto exception;
-            } else if (!ms->readInput) {
+            }
+#endif
+#ifdef MB_SUPPORT_FC04
+            if (fcx == FC04_READ_INPUT_REGISTERS && !ms->readInput) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
                 goto exception;
             }
-
+#endif
             ref = pp_u16(&pp);
             count = pp_u16(&pp);
             if (!pp_ok(&pp)) {
@@ -201,18 +248,25 @@ ModbusError_t mb_handleRequest(
             }
             pb_u8(&pb, count*2);
             while (count-- > 0) {
+#ifdef MB_SUPPORT_FC03
                 if (fcx == FC03_READ_HOLDING_REGISTERS) {
                     exc = ms->readHolding(ms, ref++, &value);
-                } else {
+                }
+#endif
+#ifdef MB_SUPPORT_FC04
+                if (fcx == FC04_READ_INPUT_REGISTERS) {
                     exc = ms->readInput(ms, ref++, &value);
                 }
+#endif
                 if (exc != 0) {
                     goto exception;
                 }
                 pb_u16(&pb, value);
             }
             break;
+#endif
 
+#ifdef MB_SUPPORT_FC06
         case FC06_WRITE_SINGLE_REGISTER:
             if (!ms->writeHolding) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -234,7 +288,9 @@ ModbusError_t mb_handleRequest(
             pb_u16(&pb, ref);
             pb_u16(&pb, value);
             break;
+#endif
 
+#ifdef MB_SUPPORT_FC16
         case FC16_WRITE_MULTIPLE_REGISTERS:
             if (!ms->writeHolding) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -263,7 +319,9 @@ ModbusError_t mb_handleRequest(
                 }
             }
             break;
+#endif
 
+#ifdef MB_SUPPORT_FC15
         case FC15_WRITE_MULTIPLE_COILS:
             if (!ms->writeCoil) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -298,7 +356,9 @@ ModbusError_t mb_handleRequest(
                 }
             }
             break;
+#endif
 
+#ifdef MB_SUPPORT_FC22
         case FC22_MASK_WRITE_REGISTER:
             if (!ms->writeHolding || !ms->readHolding) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -329,7 +389,9 @@ ModbusError_t mb_handleRequest(
             pb_u16(&pb, and_mask);
             pb_u16(&pb, or_mask);
             break;
+#endif
 
+#ifdef MB_SUPPORT_FC23
         case FC23_READ_WRITE_MULTIPLE_REGISTERS:
             if (!ms->writeHolding || !ms->readHolding) {
                 exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -370,6 +432,7 @@ ModbusError_t mb_handleRequest(
                 pb_u16(&pb, value);
             }
             break;
+#endif
 
         default:
             exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -386,7 +449,6 @@ ModbusError_t mb_handleRequest(
     checksum:
     numbytes = pb_length(&pb);
     if (tcp) {
-        pb_mark_t end = pb_save(&pb);
         pb_restore(&pb, resp_len_mark);
         pb_u16(&pb, numbytes - 6);
         *resp_size = numbytes;
diff --git a/src/modbus.h b/src/modbus.h
index 4549176..c416e0c 100644
--- a/src/modbus.h
+++ b/src/modbus.h
@@ -1,5 +1,5 @@
 /**
- * TODO file description
+ * Modbus slave
  */
 
 #ifndef MODBUS_H
@@ -9,6 +9,18 @@
 #include <stdbool.h>
 #include <sys/types.h>
 
+// Config - TODO do this via cmake
+#define MB_SUPPORT_FC01  // READ_COILS
+#define MB_SUPPORT_FC02  // READ_DISCRETES
+#define MB_SUPPORT_FC03  // READ_HOLDING_REGISTERS
+#define MB_SUPPORT_FC04  // READ_INPUT_REGISTERS
+#define MB_SUPPORT_FC05  // WRITE_SINGLE_COIL
+#define MB_SUPPORT_FC06  // WRITE_SINGLE_REGISTER
+#define MB_SUPPORT_FC15  // WRITE_MULTIPLE_COILS
+#define MB_SUPPORT_FC16  // WRITE_MULTIPLE_REGISTERS
+#define MB_SUPPORT_FC22  // MASK_WRITE_REGISTER
+#define MB_SUPPORT_FC23  // READ_WRITE_MULTIPLE_REGISTERS
+
 typedef enum ModbusException {
   MB_EXCEPTION_OK = 0,
   MB_EXCEPTION_ILLEGAL_FUNCTION = 1,
@@ -53,12 +65,30 @@ struct ModbusSlave {
   void *userData;
   ModbusException_t (*startOfAccess)(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id);
   void (*endOfAccess)(ModbusSlave_t *ms);
+
+#ifdef MB_SUPPORT_FC01
   ModbusException_t (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value);
+#endif
+
+#ifdef MB_SUPPORT_FC02
   ModbusException_t (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value);
+#endif
+
+#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC22) || defined(MB_SUPPORT_FC23)
   ModbusException_t (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
+#endif
+
+#ifdef MB_SUPPORT_FC04
   ModbusException_t (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
+#endif
+
+#if defined(MB_SUPPORT_FC15) || defined(MB_SUPPORT_FC05)
   ModbusException_t (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value);
+#endif
+
+#if defined(MB_SUPPORT_FC06) || defined(MB_SUPPORT_FC16) || defined(MB_SUPPORT_FC22) || defined(MB_SUPPORT_FC23)
   ModbusException_t (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value);
+#endif
 };
 
 ModbusError_t mb_handleRequest(
diff --git a/src/pp/payload_builder.c b/src/pp/payload_builder.c
index 4ccdf54..14afc9e 100644
--- a/src/pp/payload_builder.c
+++ b/src/pp/payload_builder.c
@@ -1,5 +1,5 @@
 #include <string.h>
-#include "payload_builder.h"
+#include "pp/payload_builder.h"
 
 #define pb_check_capacity(pb, needed)                                                   \
     if ((pb)->current + (needed) > (pb)->end) {                                         \
diff --git a/src/pp/payload_builder.h b/src/pp/payload_builder.h
index 2612a4a..fd4297b 100644
--- a/src/pp/payload_builder.h
+++ b/src/pp/payload_builder.h
@@ -4,7 +4,7 @@
 /**
  * PayloadBuilder, part of the TinyFrame utilities collection
  *
- * (c) Ondřej Hruška, 2014-2017. MIT license.
+ * (c) Ondřej Hruška, 2014-2022. MIT license.
  *
  * The builder supports big and little endian which is selected when
  * initializing it or by accessing the bigendian struct field.
diff --git a/src/pp/payload_parser.c b/src/pp/payload_parser.c
index 6b3ad86..68be4f9 100644
--- a/src/pp/payload_parser.c
+++ b/src/pp/payload_parser.c
@@ -1,4 +1,4 @@
-#include "payload_parser.h"
+#include "pp/payload_parser.h"
 
 #define pp_check_capacity(pp, needed)                                               \
     if ((pp)->current + (needed) > (pp)->end) {                                     \
diff --git a/src/pp/payload_parser.h b/src/pp/payload_parser.h
index afb17ef..ebd8908 100644
--- a/src/pp/payload_parser.h
+++ b/src/pp/payload_parser.h
@@ -4,7 +4,7 @@
 /**
  * PayloadParser, part of the TinyFrame utilities collection
  *
- * (c) Ondřej Hruška, 2016-2017. MIT license.
+ * (c) Ondřej Hruška, 2016-2022. MIT license.
  *
  * This module helps you with parsing payloads (not only from TinyFrame).
  *