diff --git a/README.md b/README.md
index 82bee1a..c41efe4 100644
--- a/README.md
+++ b/README.md
@@ -119,8 +119,8 @@ values will be treated as transport errors.
 
 ### Platform functions argument
 
-Platform functions can access arbitrary user data through their `void* arg` argument. The argument is useful, for
-example, to pass the connection a function should operate on.    
+Platform functions and server callbacks can access arbitrary user data through their `void* arg` argument. The argument
+is useful, for example, to pass the connection a function should operate on.    
 Its initial value can be set inside the `nmbs_platform_conf` struct when creating the `nmbs_t` instance, and changed at
 any time via the `nmbs_set_platform_arg` API method.
 
diff --git a/nanomodbus.c b/nanomodbus.c
index 0571eb5..7d17045 100644
--- a/nanomodbus.c
+++ b/nanomodbus.c
@@ -255,8 +255,7 @@ static nmbs_error recv_msg_header(nmbs_t* nmbs, bool* first_byte_received) {
 
     msg_state_reset(nmbs);
 
-    if (first_byte_received)
-        *first_byte_received = false;
+    *first_byte_received = false;
 
     if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
         nmbs_error err = recv(nmbs, 1);
@@ -266,8 +265,7 @@ static nmbs_error recv_msg_header(nmbs_t* nmbs, bool* first_byte_received) {
         if (err != NMBS_ERROR_NONE)
             return err;
 
-        if (first_byte_received)
-            *first_byte_received = true;
+        *first_byte_received = true;
 
         nmbs->msg.unit_id = get_1(nmbs);
 
@@ -285,8 +283,7 @@ static nmbs_error recv_msg_header(nmbs_t* nmbs, bool* first_byte_received) {
         if (err != NMBS_ERROR_NONE)
             return err;
 
-        if (first_byte_received)
-            *first_byte_received = true;
+        *first_byte_received = true;
 
         // Advance buf_idx
         discard_1(nmbs);
@@ -341,7 +338,8 @@ static nmbs_error recv_res_header(nmbs_t* nmbs) {
     uint8_t req_unit_id = nmbs->msg.unit_id;
     uint8_t req_fc = nmbs->msg.fc;
 
-    nmbs_error err = recv_msg_header(nmbs, NULL);
+    bool first_byte_received;
+    nmbs_error err = recv_msg_header(nmbs, &first_byte_received);
     if (err != NMBS_ERROR_NONE)
         return err;
 
@@ -503,7 +501,7 @@ static nmbs_error handle_read_discrete(nmbs_t* nmbs, nmbs_error (*callback)(uint
 #endif
 
 
-#if !defined(NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED) || !defined(NMBS_SERVER_READ_INPUTS_REGISTERS_DISABLED)
+#if !defined(NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED) || !defined(NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED)
 static nmbs_error handle_read_registers(nmbs_t* nmbs, nmbs_error (*callback)(uint16_t, uint16_t, uint16_t*, void*)) {
     nmbs_error err = recv(nmbs, 4);
     if (err != NMBS_ERROR_NONE)
diff --git a/nanomodbus.h b/nanomodbus.h
index dcfb6b1..bad0f09 100644
--- a/nanomodbus.h
+++ b/nanomodbus.h
@@ -53,15 +53,14 @@ extern "C" {
  */
 
 #ifdef NMBS_SERVER_DISABLED
-    #define NMBS_SERVER_READ_COILS_DISABLED 1
-    #define NMBS_SERVER_READ_COILS_DISABLED 1
-    #define NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED 1
-    #define NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED 1
-    #define NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED 1
-    #define NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED 1
-    #define NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED 1
-    #define NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED 1
-    #define NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED 1
+#define NMBS_SERVER_READ_COILS_DISABLED
+#define NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED
+#define NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED
+#define NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED
+#define NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED
+#define NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED
+#define NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED
+#define NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED
 #endif
 
 /**
@@ -161,37 +160,38 @@ typedef struct nmbs_platform_conf {
  */
 typedef struct nmbs_callbacks {
 #ifndef NMBS_SERVER_READ_COILS_DISABLED
-    nmbs_error (*read_coils)(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, void *arg);
+    nmbs_error (*read_coils)(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED
-    nmbs_error (*read_discrete_inputs)(uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out, void *arg);
+    nmbs_error (*read_discrete_inputs)(uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED
-    nmbs_error (*read_holding_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, void *arg);
+    nmbs_error (*read_holding_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED
-    nmbs_error (*read_input_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, void *arg);
+    nmbs_error (*read_input_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED
-    nmbs_error (*write_single_coil)(uint16_t address, bool value, void *arg);
+    nmbs_error (*write_single_coil)(uint16_t address, bool value, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED
-    nmbs_error (*write_single_register)(uint16_t address, uint16_t value, void *arg);
+    nmbs_error (*write_single_register)(uint16_t address, uint16_t value, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED
-    nmbs_error (*write_multiple_coils)(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, void *arg);
+    nmbs_error (*write_multiple_coils)(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, void* arg);
 #endif
 
 #ifndef NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED
-    nmbs_error (*write_multiple_registers)(uint16_t address, uint16_t quantity, const uint16_t* registers, void *arg);
+    nmbs_error (*write_multiple_registers)(uint16_t address, uint16_t quantity, const uint16_t* registers, void* arg);
 #endif
 
+    char _nonempty;    // Struct may become empty, which is undefined behavior
 } nmbs_callbacks;