diff --git a/modbusino.c b/modbusino.c
index 637f328..e0a4749 100644
--- a/modbusino.c
+++ b/modbusino.c
@@ -12,16 +12,30 @@
         defined(__THUMBEL__) || defined(__AARCH64EL__) || defined(_MIPSEL) || defined(__MIPSEL) || defined(__MIPSEL__)
 #define MBSN_LITTLE_ENDIAN
 #else
-#error "Failed to automatically detect system endianness. Please define either MBSN_BIG_ENDIAN or MBSN_LITTLE_ENDIAN"
+#error "Failed to automatically detect platform endianness. Please define either MBSN_BIG_ENDIAN or MBSN_LITTLE_ENDIAN"
 #endif
 #endif
 
+
+#define get_1(m)                                                                                                       \
+    (m)->msg_buf[(m)->msg_buf_idx];                                                                                    \
+    (m)->msg_buf_idx++
+#define put_1(m, b)                                                                                                    \
+    (m)->msg_buf[(m)->msg_buf_idx] = (b);                                                                              \
+    (m)->msg_buf_idx++
+
 #ifdef MBSN_BIG_ENDIAN
-#define HTONS(x) (x)
-#define NTOHS(x) (x)
+#define get_2(m) ((m)->msg_buf[(m)->msg_buf[(m)->msg_buf_idx]);                                                        \
+    (m)->msg_buf_idx += 2
+#define put_2(m, w) (m)->msg_buf(m)[(m)->msg_buf_idx]] = (w);                                                          \
+    (m)->msg_buf_idx += 2
 #else
-#define HTONS(x) (((x) >> 8) | ((x) << 8))
-#define NTOHS(x) (((x) >> 8) | ((x) << 8))
+#define get_2(m)                                                                                                       \
+    ((m)->msg_buf[(m)->msg_buf_idx] >> 8 | (m)->msg_buf[(m)->msg_buf_idx] << 8);                                       \
+    (m)->msg_buf_idx += 2
+#define put_2(m, w)                                                                                                    \
+    (m)->msg_buf[(m)->msg_buf_idx] = ((w) >> 8 | (w) << 8);                                                            \
+    (m)->msg_buf_idx += 2
 #endif
 
 
@@ -31,24 +45,22 @@ typedef struct msg_state {
     uint16_t transaction_id;
     bool broadcast;
     bool ignored;
-    uint16_t crc;
 } msg_state;
 
 
 static msg_state msg_state_create() {
     msg_state s;
     memset(&s, 0, sizeof(msg_state));
-    s.crc = 0xFFFF;
     return s;
 }
 
 
 static msg_state res_from_req(msg_state* req) {
     msg_state res = *req;
-    res.crc = 0xFFFF;
     return res;
 }
 
+
 static msg_state req_create(uint8_t unit_id, uint8_t fc) {
     static uint16_t TID = 0;
 
@@ -63,6 +75,7 @@ static msg_state req_create(uint8_t unit_id, uint8_t fc) {
     return req;
 }
 
+
 int mbsn_create(mbsn_t* mbsn, mbsn_transport_conf transport_conf) {
     if (!mbsn)
         return MBSN_ERROR_INVALID_ARGUMENT;
@@ -149,16 +162,20 @@ static uint16_t crc_add_2(uint16_t crc, const uint8_t w) {
 }
 
 
-static mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* crc) {
+static void reset_msg_buf(mbsn_t* mbsn) {
+    mbsn->msg_buf_idx = 0;
+}
+
+
+static mbsn_error recv(mbsn_t* mbsn, uint32_t count) {
     int r = 0;
     while (r != count) {
-        int ret = mbsn->transport_read_byte(buf + r, mbsn->byte_timeout_ms);
+        int ret = mbsn->transport_read_byte(mbsn->msg_buf + mbsn->msg_buf_idx + r, mbsn->byte_timeout_ms);
         if (ret == 0)
             return MBSN_ERROR_TIMEOUT;
         else if (ret != 1)
             return MBSN_ERROR_TRANSPORT;
 
-        *crc = crc_add_1(*crc, buf[r]);
         r++;
     }
 
@@ -166,70 +183,41 @@ static mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* c
 }
 
 
-static mbsn_error recv_1(mbsn_t* mbsn, uint8_t* b, uint16_t* crc) {
-    return recv_n(mbsn, b, 1, crc);
-}
-
-
-static mbsn_error recv_2(mbsn_t* mbsn, uint16_t* w, uint16_t* crc) {
-    mbsn_error err = recv_n(mbsn, (uint8_t*) w, 2, crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    *w = NTOHS(*w);
-    return MBSN_ERROR_NONE;
-}
-
-
-static mbsn_error send_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* crc) {
-    int w = 0;
-    while (w != count) {
-        int ret = mbsn->transport_write_byte(buf[w], mbsn->read_timeout_ms);
+static mbsn_error send(mbsn_t* mbsn) {
+    for (int i = 0; i < mbsn->msg_buf_idx; i++) {
+        int ret = mbsn->transport_write_byte(mbsn->msg_buf[i], mbsn->read_timeout_ms);
         if (ret == 0)
             return MBSN_ERROR_TIMEOUT;
         else if (ret != 1)
             return MBSN_ERROR_TRANSPORT;
-
-        w++;
     }
 
-    if (crc)
-        *crc = crc_add_n(*crc, buf, count);
-
     return MBSN_ERROR_NONE;
 }
 
 
-static mbsn_error send_1(mbsn_t* mbsn, uint8_t b, uint16_t* crc) {
-    return send_n(mbsn, &b, 1, crc);
-}
-
-
-static mbsn_error send_2(mbsn_t* mbsn, uint16_t w, uint16_t* crc) {
-    w = HTONS(w);
-    return send_n(mbsn, (uint8_t*) &w, 2, crc);
-}
-
-
-static mbsn_error recv_msg_header(mbsn_t* mbsn, msg_state* s_out) {
+static mbsn_error recv_msg_header(mbsn_t* mbsn, msg_state* s_out, bool* first_byte_received) {
     // We wait for the read timeout here, just for the first message byte
     int32_t old_byte_timeout = mbsn->byte_timeout_ms;
     mbsn->byte_timeout_ms = mbsn->read_timeout_ms;
 
     *s_out = msg_state_create();
+    reset_msg_buf(mbsn);
+    if (first_byte_received)
+        *first_byte_received = false;
 
     if (mbsn->transport == MBSN_TRANSPORT_RTU) {
-        uint8_t unit_id;
-        mbsn_error err = recv_1(mbsn, &unit_id, &s_out->crc);
+        mbsn_error err = recv(mbsn, 1);
 
         mbsn->byte_timeout_ms = old_byte_timeout;
 
-        if (err != 0) {
-            if (err == MBSN_ERROR_TIMEOUT)
-                return MBSN_ERROR_NONE;
-            else
-                return err;
-        }
+        if (err != MBSN_ERROR_NONE)
+            return err;
+
+        if (first_byte_received)
+            *first_byte_received = true;
+
+        uint8_t unit_id = get_1(mbsn);
 
         // Check if message is for us
         if (unit_id == 0)
@@ -239,59 +227,55 @@ static mbsn_error recv_msg_header(mbsn_t* mbsn, msg_state* s_out) {
         else
             s_out->ignored = false;
 
-        err = recv_1(mbsn, &s_out->fc, &s_out->crc);
+        err = recv(mbsn, 1);
         if (err != MBSN_ERROR_NONE)
             return err;
+
+        s_out->fc = get_1(mbsn);
     }
     else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
-        mbsn_error err = recv_2(mbsn, &s_out->transaction_id, NULL);
+        mbsn_error err = recv(mbsn, 1);
 
         mbsn->byte_timeout_ms = old_byte_timeout;
 
-        if (err != 0) {
-            if (err == MBSN_ERROR_TIMEOUT)
-                return MBSN_ERROR_NONE;
-            else
-                return err;
-        }
-
-        uint16_t protocol_id = 0xFFFF;
-        err = recv_2(mbsn, &protocol_id, NULL);
         if (err != MBSN_ERROR_NONE)
             return err;
 
-        uint16_t length = 0xFFFF;
-        err = recv_2(mbsn, &length, NULL);
+        if (first_byte_received)
+            *first_byte_received = true;
+
+        err = recv(mbsn, 7);
         if (err != MBSN_ERROR_NONE)
             return err;
 
-        err = recv_1(mbsn, &s_out->unit_id, NULL);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+        reset_msg_buf(mbsn);
 
-        err = recv_1(mbsn, &s_out->fc, NULL);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+        uint16_t protocol_id = get_2(mbsn);
+        // TODO maybe we should actually check the length of the request against this value
+        uint16_t length = get_2(mbsn);
+        s_out->unit_id = get_1(mbsn);
+        s_out->fc = get_1(mbsn);
 
         if (protocol_id != 0)
             return MBSN_ERROR_TRANSPORT;
 
-        // TODO maybe we should actually check the length of the request against this value
-        if (length == 0xFFFF)
+        if (length > 255)
             return MBSN_ERROR_TRANSPORT;
     }
 
     return MBSN_ERROR_NONE;
 }
 
+
 static mbsn_error recv_msg_footer(mbsn_t* mbsn, msg_state* s) {
     if (mbsn->transport == MBSN_TRANSPORT_RTU) {
-        uint16_t recv_crc;
-        mbsn_error err = recv_2(mbsn, &recv_crc, NULL);
+        mbsn_error err = recv(mbsn, 2);
         if (err != MBSN_ERROR_NONE)
             return err;
 
-        if (recv_crc != s->crc)
+        uint16_t recv_crc = get_2(mbsn);
+        uint16_t crc = crc_add_n(0xFFFF, mbsn->msg_buf, mbsn->msg_buf_idx);
+        if (recv_crc != crc)
             return MBSN_ERROR_TRANSPORT;
     }
 
@@ -301,43 +285,29 @@ static mbsn_error recv_msg_footer(mbsn_t* mbsn, msg_state* s) {
 
 static mbsn_error send_msg_header(mbsn_t* mbsn, msg_state* s, uint8_t data_length) {
     if (mbsn->transport == MBSN_TRANSPORT_RTU) {
-        s->crc = 0xFFFF;
-        mbsn_error err = send_1(mbsn, s->unit_id, &s->crc);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+        put_1(mbsn, s->unit_id);
     }
     else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
-        mbsn_error err = send_2(mbsn, s->transaction_id, NULL);
-        if (err != MBSN_ERROR_NONE)
-            return err;
-
-        const uint16_t protocol_id = 0;
-        err = send_2(mbsn, protocol_id, NULL);
-        if (err != MBSN_ERROR_NONE)
-            return err;
-
-        err = send_2(mbsn, 1 + 1 + data_length, NULL);
-        if (err != MBSN_ERROR_NONE)
-            return err;
-
-        err = send_1(mbsn, s->unit_id, NULL);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+        put_2(mbsn, s->transaction_id);
+        put_2(mbsn, 0);
+        put_2(mbsn, 1 + 1 + data_length);
+        put_2(mbsn, s->unit_id);
     }
 
-    mbsn_error err = send_1(mbsn, s->fc, &s->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
+    put_1(mbsn, s->fc);
     return MBSN_ERROR_NONE;
 }
 
 
 static mbsn_error send_msg_footer(mbsn_t* mbsn, msg_state* s) {
-    if (mbsn->transport == MBSN_TRANSPORT_RTU)
-        return send_2(mbsn, s->crc, NULL);
+    if (mbsn->transport == MBSN_TRANSPORT_RTU) {
+        uint16_t crc = crc_add_n(0xFFFF, mbsn->msg_buf, mbsn->msg_buf_idx);
+        put_2(mbsn, crc);
+    }
 
-    return MBSN_ERROR_NONE;
+    mbsn_error err = send(mbsn);
+    reset_msg_buf(mbsn);
+    return err;
 }
 
 
@@ -349,9 +319,7 @@ static mbsn_error handle_exception(mbsn_t* mbsn, msg_state* req, uint8_t excepti
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_1(mbsn, exception, &res.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_1(mbsn, exception);
 
     return send_msg_footer(mbsn, &res);
 }
@@ -359,13 +327,7 @@ static mbsn_error handle_exception(mbsn_t* mbsn, msg_state* req, uint8_t excepti
 
 static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
                                        mbsn_error (*callback)(uint16_t, uint16_t, mbsn_bitfield)) {
-    uint16_t addr;
-    mbsn_error err = recv_2(mbsn, &addr, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    uint16_t quantity;
-    err = recv_2(mbsn, &quantity, &req->crc);
+    mbsn_error err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
@@ -374,6 +336,9 @@ static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
         return err;
 
     if (!req->ignored) {
+        uint16_t addr = get_2(mbsn);
+        uint16_t quantity = get_2(mbsn);
+
         if (quantity < 1 || quantity > 2000)
             return handle_exception(mbsn, req, MBSN_EXCEPTION_ILLEGAL_DATA_VALUE);
 
@@ -398,13 +363,11 @@ static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
-                err = send_1(mbsn, discrete_bytes, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
+                put_1(mbsn, discrete_bytes);
 
-                err = send_n(mbsn, bf, discrete_bytes, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
+                for (int i = 0; i < discrete_bytes; i++) {
+                    put_1(mbsn, bf[i]);
+                }
 
                 err = send_msg_footer(mbsn, &res);
                 if (err != MBSN_ERROR_NONE)
@@ -422,13 +385,7 @@ static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
 
 static mbsn_error handle_read_registers(mbsn_t* mbsn, msg_state* req,
                                         mbsn_error (*callback)(uint16_t, uint16_t, uint16_t*)) {
-    uint16_t addr;
-    mbsn_error err = recv_2(mbsn, &addr, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    uint16_t quantity;
-    err = recv_2(mbsn, &quantity, &req->crc);
+    mbsn_error err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
@@ -437,6 +394,9 @@ static mbsn_error handle_read_registers(mbsn_t* mbsn, msg_state* req,
         return err;
 
     if (!req->ignored) {
+        uint16_t addr = get_2(mbsn);
+        uint16_t quantity = get_2(mbsn);
+
         if (quantity < 1 || quantity > 125)
             return handle_exception(mbsn, req, MBSN_EXCEPTION_ILLEGAL_DATA_VALUE);
 
@@ -461,14 +421,10 @@ static mbsn_error handle_read_registers(mbsn_t* mbsn, msg_state* req,
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
-                err = send_1(mbsn, regs_bytes, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
+                put_1(mbsn, regs_bytes);
 
                 for (int i = 0; i < quantity; i++) {
-                    err = send_2(mbsn, regs[i], &res.crc);
-                    if (err != MBSN_ERROR_NONE)
-                        return err;
+                    put_2(mbsn, regs[i]);
                 }
 
                 err = send_msg_footer(mbsn, &res);
@@ -506,13 +462,7 @@ static mbsn_error handle_read_input_registers(mbsn_t* mbsn, msg_state* req) {
 
 
 static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
-    uint16_t addr;
-    mbsn_error err = recv_2(mbsn, &addr, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    uint16_t value;
-    err = recv_2(mbsn, &value, &req->crc);
+    mbsn_error err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
@@ -521,6 +471,9 @@ static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
         return err;
 
     if (!req->ignored) {
+        uint16_t addr = get_2(mbsn);
+        uint16_t value = get_2(mbsn);
+
         if (value != 0 && value != 0xFF00)
             return handle_exception(mbsn, req, MBSN_EXCEPTION_ILLEGAL_DATA_VALUE);
 
@@ -540,9 +493,7 @@ static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
-                err = send_2(mbsn, addr, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
+                put_2(mbsn, addr);
 
                 err = send_msg_footer(mbsn, &res);
                 if (err != MBSN_ERROR_NONE)
@@ -559,13 +510,7 @@ static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
 
 
 static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
-    uint16_t addr;
-    mbsn_error err = recv_2(mbsn, &addr, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    uint16_t value;
-    err = recv_2(mbsn, &value, &req->crc);
+    mbsn_error err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
@@ -574,6 +519,9 @@ static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
         return err;
 
     if (!req->ignored) {
+        uint16_t addr = get_2(mbsn);
+        uint16_t value = get_2(mbsn);
+
         if (mbsn->callbacks.write_single_register) {
             err = mbsn->callbacks.write_single_register(addr, value);
             if (err != MBSN_ERROR_NONE) {
@@ -590,7 +538,7 @@ static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
-                err = send_2(mbsn, value, &res.crc);
+                err = put_2(mbsn, value);
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
@@ -609,25 +557,21 @@ static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
 
 
 static mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, msg_state* req) {
-    uint16_t addr;
-    mbsn_error err = recv_2(mbsn, &addr, &req->crc);
+    mbsn_error err = recv(mbsn, 5);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t quantity;
-    err = recv_2(mbsn, &quantity, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    uint16_t addr = get_2(mbsn);
+    uint16_t quantity = get_2(mbsn);
+    uint8_t coils_bytes = get_1(mbsn);
 
-    uint8_t coils_bytes;
-    err = recv_1(mbsn, &coils_bytes, &req->crc);
+    err = recv(mbsn, coils_bytes);
     if (err != MBSN_ERROR_NONE)
         return err;
 
     mbsn_bitfield coils;
-    err = recv_n(mbsn, coils, coils_bytes, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    for (int i = 0; i < coils_bytes; i++)
+        coils[i] = get_1(mbsn);
 
     err = recv_msg_footer(mbsn, req);
     if (err != MBSN_ERROR_NONE)
@@ -659,13 +603,8 @@ static mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, msg_state* req) {
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
-                err = send_2(mbsn, addr, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
-
-                err = send_2(mbsn, quantity, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
+                put_2(mbsn, addr);
+                put_2(mbsn, quantity);
 
                 err = send_msg_footer(mbsn, &res);
                 if (err != MBSN_ERROR_NONE)
@@ -682,26 +621,21 @@ static mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, msg_state* req) {
 
 
 static mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, msg_state* req) {
-    uint16_t addr;
-    mbsn_error err = recv_2(mbsn, &addr, &req->crc);
+    mbsn_error err = recv(mbsn, 5);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t quantity;
-    err = recv_2(mbsn, &quantity, &req->crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    uint16_t addr = get_2(mbsn);
+    uint16_t quantity = get_2(mbsn);
+    uint8_t registers_bytes = get_1(mbsn);
 
-    uint8_t registers_bytes;
-    err = recv_1(mbsn, &registers_bytes, &req->crc);
+    err = recv(mbsn, registers_bytes);
     if (err != MBSN_ERROR_NONE)
         return err;
 
     uint16_t registers[0x007B];
     for (int i = 0; i < quantity; i++) {
-        err = recv_2(mbsn, registers + i, &req->crc);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+        registers[i] = get_2(mbsn);
     }
 
     err = recv_msg_footer(mbsn, req);
@@ -734,13 +668,8 @@ static mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, msg_state* req)
                 if (err != MBSN_ERROR_NONE)
                     return err;
 
-                err = send_2(mbsn, addr, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
-
-                err = send_2(mbsn, quantity, &res.crc);
-                if (err != MBSN_ERROR_NONE)
-                    return err;
+                put_2(mbsn, addr);
+                put_2(mbsn, quantity);
 
                 err = send_msg_footer(mbsn, &res);
                 if (err != MBSN_ERROR_NONE)
@@ -802,9 +731,14 @@ static mbsn_error handle_req_fc(mbsn_t* mbsn, msg_state* req) {
 mbsn_error mbsn_server_receive(mbsn_t* mbsn) {
     msg_state req = msg_state_create();
 
-    mbsn_error err = recv_msg_header(mbsn, &req);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    bool first_byte_received = false;
+    mbsn_error err = recv_msg_header(mbsn, &req, &first_byte_received);
+    if (err != MBSN_ERROR_NONE) {
+        if (!first_byte_received && err == MBSN_ERROR_TIMEOUT)
+            return MBSN_ERROR_NONE;
+        else
+            return err;
+    }
 
     /*
     // We should wait for the read timeout for the first message byte
@@ -879,7 +813,7 @@ mbsn_error mbsn_server_receive(mbsn_t* mbsn) {
 
 
 static mbsn_error recv_res_header(mbsn_t* mbsn, msg_state* req, msg_state* res_out) {
-    mbsn_error err = recv_msg_header(mbsn, res_out);
+    mbsn_error err = recv_msg_header(mbsn, res_out, NULL);
     if (err != MBSN_ERROR_NONE)
         return err;
 
@@ -896,7 +830,7 @@ static mbsn_error recv_res_header(mbsn_t* mbsn, msg_state* req, msg_state* res_o
 }
 
 
-static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t quantity, mbsn_bitfield* values) {
+static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t quantity, mbsn_bitfield values) {
     if (quantity < 1 || quantity > 2000)
         return MBSN_ERROR_INVALID_ARGUMENT;
 
@@ -905,13 +839,8 @@ static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_2(mbsn, address, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_2(mbsn, quantity, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_2(mbsn, address);
+    put_2(mbsn, quantity);
 
     err = send_msg_footer(mbsn, &req);
     if (err != MBSN_ERROR_NONE)
@@ -922,15 +851,20 @@ static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint8_t coils_bytes;
-    err = recv_1(mbsn, &coils_bytes, &res.crc);
+    err = recv(mbsn, 1);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = recv_n(mbsn, (uint8_t*) &values, coils_bytes, &res.crc);
+    uint8_t coils_bytes = get_1(mbsn);
+
+    err = recv(mbsn, coils_bytes);
     if (err != MBSN_ERROR_NONE)
         return err;
 
+    for (int i = 0; i < coils_bytes; i++) {
+        values[i] = get_1(mbsn);
+    }
+
     err = recv_msg_footer(mbsn, &res);
     if (err != MBSN_ERROR_NONE)
         return err;
@@ -939,12 +873,12 @@ static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint
 }
 
 
-mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* coils_out) {
+mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield coils_out) {
     return read_discrete(mbsn, 1, address, quantity, coils_out);
 }
 
 
-mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* inputs_out) {
+mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield inputs_out) {
     return read_discrete(mbsn, 1, address, quantity, inputs_out);
 }
 
@@ -958,13 +892,8 @@ mbsn_error read_registers(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t q
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_2(mbsn, address, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_2(mbsn, quantity, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_2(mbsn, address);
+    put_2(mbsn, quantity);
 
     err = send_msg_footer(mbsn, &req);
     if (err != MBSN_ERROR_NONE)
@@ -975,15 +904,18 @@ mbsn_error read_registers(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t q
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint8_t registers_bytes;
-    err = recv_1(mbsn, &registers_bytes, &res.crc);
+    err = recv(mbsn, 1);
+    if (err != MBSN_ERROR_NONE)
+        return err;
+
+    uint8_t registers_bytes = get_1(mbsn);
+
+    err = recv(mbsn, registers_bytes);
     if (err != MBSN_ERROR_NONE)
         return err;
 
     for (int i = 0; i < registers_bytes / 2; i++) {
-        err = recv_2(mbsn, registers + i, &res.crc);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+        registers[i] = get_2(mbsn);
     }
 
     err = recv_msg_footer(mbsn, &res);
@@ -1013,13 +945,8 @@ mbsn_error mbsn_write_single_coil(mbsn_t* mbsn, uint16_t address, bool value) {
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_2(mbsn, address, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_2(mbsn, value ? 0xFF00 : 0, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_2(mbsn, address);
+    put_2(mbsn, value ? 0xFF00 : 0);
 
     err = send_msg_footer(mbsn, &req);
     if (err != MBSN_ERROR_NONE)
@@ -1030,15 +957,12 @@ mbsn_error mbsn_write_single_coil(mbsn_t* mbsn, uint16_t address, bool value) {
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t address_res;
-    err = recv_2(mbsn, &address_res, &res.crc);
+    err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t value_res;
-    err = recv_2(mbsn, &value_res, &res.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    uint16_t address_res = get_2(mbsn);
+    uint16_t value_res = get_2(mbsn);
 
     err = recv_msg_footer(mbsn, &res);
     if (err != MBSN_ERROR_NONE)
@@ -1060,13 +984,8 @@ mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t v
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_2(mbsn, address, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_2(mbsn, value, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_2(mbsn, address);
+    put_2(mbsn, value);
 
     err = send_msg_footer(mbsn, &req);
     if (err != MBSN_ERROR_NONE)
@@ -1077,15 +996,12 @@ mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t v
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t address_res;
-    err = recv_2(mbsn, &address_res, &res.crc);
+    err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t value_res;
-    err = recv_2(mbsn, &value_res, &res.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    uint16_t address_res = get_2(mbsn);
+    uint16_t value_res = get_2(mbsn);
 
     err = recv_msg_footer(mbsn, &res);
     if (err != MBSN_ERROR_NONE)
@@ -1101,7 +1017,7 @@ mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t v
 }
 
 
-mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield* coils) {
+mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield coils) {
     if (quantity < 0 || quantity > 0x07B0)
         return MBSN_ERROR_INVALID_ARGUMENT;
 
@@ -1112,21 +1028,13 @@ mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t qu
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_2(mbsn, address, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_2(mbsn, address);
+    put_2(mbsn, quantity);
+    put_1(mbsn, coils_bytes);
 
-    err = send_2(mbsn, quantity, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_1(mbsn, coils_bytes, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_n(mbsn, (uint8_t*) coils, coils_bytes, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    for (int i = 0; i < coils_bytes; i++) {
+        put_1(mbsn, coils[i]);
+    }
 
     err = send_msg_footer(mbsn, &req);
     if (err != MBSN_ERROR_NONE)
@@ -1137,15 +1045,12 @@ mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t qu
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t address_res;
-    err = recv_2(mbsn, &address_res, &res.crc);
+    err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t quantity_res;
-    err = recv_2(mbsn, &quantity_res, &res.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    uint16_t address_res = get_2(mbsn);
+    uint16_t quantity_res = get_2(mbsn);
 
     err = recv_msg_footer(mbsn, &res);
     if (err != MBSN_ERROR_NONE)
@@ -1172,22 +1077,12 @@ mbsn_error mbsn_write_multiple_registers(mbsn_t* mbsn, uint16_t address, uint16_
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    err = send_2(mbsn, address, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    put_2(mbsn, address);
+    put_2(mbsn, quantity);
+    put_1(mbsn, registers_bytes);
 
-    err = send_2(mbsn, quantity, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    err = send_1(mbsn, registers_bytes, &req.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
-
-    for(int i = 0; i < quantity; i++) {
-        err = send_2(mbsn, registers[i], &req.crc);
-        if (err != MBSN_ERROR_NONE)
-            return err;
+    for (int i = 0; i < quantity; i++) {
+        put_2(mbsn, registers[i]);
     }
 
     err = send_msg_footer(mbsn, &req);
@@ -1199,15 +1094,12 @@ mbsn_error mbsn_write_multiple_registers(mbsn_t* mbsn, uint16_t address, uint16_
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t address_res;
-    err = recv_2(mbsn, &address_res, &res.crc);
+    err = recv(mbsn, 4);
     if (err != MBSN_ERROR_NONE)
         return err;
 
-    uint16_t quantity_res;
-    err = recv_2(mbsn, &quantity_res, &res.crc);
-    if (err != MBSN_ERROR_NONE)
-        return err;
+    uint16_t address_res = get_2(mbsn);
+    uint16_t quantity_res = get_2(mbsn);
 
     err = recv_msg_footer(mbsn, &res);
     if (err != MBSN_ERROR_NONE)
diff --git a/modbusino.h b/modbusino.h
index 7979389..d429338 100644
--- a/modbusino.h
+++ b/modbusino.h
@@ -38,8 +38,8 @@ typedef enum mbsn_transport {
 
 typedef struct mbsn_transport_conf {
     mbsn_transport transport;
-    int (*read_byte)(uint8_t* b, uint32_t);
-    int (*write_byte)(uint8_t b, uint32_t);
+    int (*read_byte)(uint8_t* b, int32_t);
+    int (*write_byte)(uint8_t b, int32_t);
 } mbsn_transport_conf;
 
 
@@ -69,14 +69,17 @@ typedef struct mbsn_callbacks {
 
 typedef struct mbsn_t {
     // Private fields
+    uint8_t msg_buf[260];
+    uint16_t msg_buf_idx;
+
     mbsn_callbacks callbacks;
 
     int32_t byte_timeout_ms;
     int32_t read_timeout_ms;
 
     mbsn_transport transport;
-    mbsn_error (*transport_read_byte)(uint8_t*, uint32_t);
-    mbsn_error (*transport_write_byte)(uint8_t, uint32_t);
+    mbsn_error (*transport_read_byte)(uint8_t*, int32_t);
+    mbsn_error (*transport_write_byte)(uint8_t, int32_t);
 
     uint8_t address_rtu;
     uint8_t server_dest_address_rtu;
@@ -96,9 +99,9 @@ void mbsn_client_set_server_address_rtu(mbsn_t* mbsn, uint8_t address);
 
 mbsn_error mbsn_server_receive(mbsn_t* mbsn);
 
-mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* coils_out);
+mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield coils_out);
 
-mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* inputs_out);
+mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield inputs_out);
 
 mbsn_error mbsn_read_holding_registers(mbsn_t* mbsn, uint16_t address, uint16_t quantity, uint16_t* registers_out);
 
@@ -108,7 +111,7 @@ mbsn_error mbsn_write_single_coil(mbsn_t* mbsn, uint16_t address, bool value);
 
 mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t value);
 
-mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield* coils);
+mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield coils);
 
 mbsn_error mbsn_write_multiple_registers(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const uint16_t* registers);
 
diff --git a/tests/modbusino_tests.c b/tests/modbusino_tests.c
index c364ddb..69679f6 100644
--- a/tests/modbusino_tests.c
+++ b/tests/modbusino_tests.c
@@ -74,9 +74,7 @@ void test_server_receive_base(mbsn_transport transport) {
 
         uint64_t diff = now_ms() - start;
 
-        if (diff < read_timeout_ms) {
-            fail();
-        }
+        assert(diff >= read_timeout_ms);
     }
 
     should("honor byte_timeout and return MBSN_ERROR_TIMEOUT");
@@ -90,7 +88,7 @@ void test_server_receive_base(mbsn_transport transport) {
     err = mbsn_server_create(&mbsn, TEST_SERVER_ADDR, transport_conf, (mbsn_callbacks){});
     check(err);
 
-    mbsn_set_read_timeout(&mbsn, -1);
+    mbsn_set_read_timeout(&mbsn, 1000);
     mbsn_set_byte_timeout(&mbsn, byte_timeout_ms);
 
     err = mbsn_server_receive(&mbsn);
diff --git a/tests/modbusino_tests.h b/tests/modbusino_tests.h
index e941f4a..2f66ee7 100644
--- a/tests/modbusino_tests.h
+++ b/tests/modbusino_tests.h
@@ -1,11 +1,11 @@
 #include "modbusino.h"
+#include <assert.h>
+#include <stdio.h>
 #include <time.h>
 #include <unistd.h>
 
 unsigned int nesting = 0;
 
-#define fail() (assert(false))
-
 #define should(s)                                                                                                      \
     {                                                                                                                  \
         for (int i = 0; i < nesting; i++) {                                                                            \
@@ -25,26 +25,31 @@ unsigned int nesting = 0;
 
 const uint8_t TEST_SERVER_ADDR = 1;
 
-static uint64_t now_ms() {
+
+uint64_t now_ms() {
     struct timespec ts = {0, 0};
     clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
     return (uint64_t) (ts.tv_sec) * 1000 + (uint64_t) (ts.tv_nsec) / 1000000;
 }
 
-int read_byte_empty(uint8_t* b, uint32_t timeout) {
+
+int read_byte_empty(uint8_t* b, int32_t timeout) {
     return 0;
 }
 
-int write_byte_empty(uint8_t b, uint32_t timeout) {
+
+int write_byte_empty(uint8_t b, int32_t timeout) {
     return 0;
 }
 
-int read_byte_timeout_1s(uint8_t* b, uint32_t timeout) {
+
+int read_byte_timeout_1s(uint8_t* b, int32_t timeout) {
     usleep(timeout * 1000);
     return 0;
 }
 
-int read_byte_timeout_third(uint8_t* b, uint32_t timeout) {
+
+int read_byte_timeout_third(uint8_t* b, int32_t timeout) {
     static int stage = 0;
     switch (stage) {
         case 0:
@@ -53,7 +58,8 @@ int read_byte_timeout_third(uint8_t* b, uint32_t timeout) {
             stage++;
             return 1;
         case 2:
-            usleep(timeout * 1000 + 100);
+            assert(timeout > 0);
+            usleep(timeout * 1000 + 100 * 1000);
             stage = 0;
             return 0;
         default: