diff --git a/mongoose.c b/mongoose.c
index 8e991fe7..5a8ea0e9 100644
--- a/mongoose.c
+++ b/mongoose.c
@@ -3970,6 +3970,239 @@ void mg_md5_final(mg_md5_ctx *ctx, unsigned char digest[16]) {
 }
 #endif
 
+#ifdef MG_ENABLE_LINES
+#line 1 "src/modbus.c"
+#endif
+
+
+
+
+
+#define MG_MODBUS_MIN_PAYLOAD_LEN 12
+#define MG_MODBUS_HEADER_LEN 7
+
+static void mg_modbus_send_mbap(struct mg_connection *c, uint16_t txid,
+                                uint16_t pdu_len, uint8_t unit_id) {
+  uint8_t hdr[7];
+  memset(hdr, 0, sizeof(hdr));
+  MG_STORE_BE16(&hdr[0], txid & 0xFFFFU);
+  MG_STORE_BE16(&hdr[4], (pdu_len + 1) & 0xFFFFU);
+  hdr[6] = unit_id;
+  mg_send(c, hdr, sizeof(hdr));
+}
+
+static void mg_modbus_send_response(struct mg_connection *c,
+                                    struct mg_modbus_req *req,
+                                    uint16_t txid, uint8_t unit_id) {
+  uint8_t buf[5];
+  uint8_t out;
+  uint16_t reg;
+  uint16_t i, nbytes;
+
+  memset(buf, 0, sizeof(buf));
+  if (req == NULL) return;
+  if (req->error != MG_MODBUS_ERR_NONE) {
+    mg_modbus_send_mbap(c, txid, 2, unit_id);
+    buf[0] = (uint8_t) (req->func | 0x80);
+    buf[1] = req->error;
+    mg_send(c, buf, 2);
+    return;
+  }
+  switch (req->func) {
+    case MG_MODBUS_FUNC_READ_COILS:
+    case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
+      if (req->u.bits == NULL) return;
+      nbytes = (uint16_t) ((req->len + 7) / 8);
+      mg_modbus_send_mbap(c, txid, (uint16_t) (nbytes + 2), unit_id);
+      buf[0] = req->func;
+      buf[1] = (uint8_t) nbytes;
+      mg_send(c, buf, 2);
+      memset(buf, 0, sizeof(buf));
+      for (i = 0; i < req->len; i++) {
+        if (i % 8 == 0) out = 0;
+        if (req->u.bits[i]) out |= (uint8_t) (1 << (i % 8));
+        if ((i % 8) == 7 || i + 1 == req->len) {
+          mg_send(c, &out, 1);
+        }
+      }
+      break;
+    case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
+    case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
+      if (req->u.regs == NULL) return;
+      nbytes = (uint16_t) (req->len * 2);
+      mg_modbus_send_mbap(c, txid, (uint16_t) (nbytes + 2), unit_id);
+      buf[0] = req->func;
+      buf[1] = (uint8_t) nbytes;
+      mg_send(c, buf, 2);
+      for (i = 0; i < req->len; i++) {
+        MG_STORE_BE16(&reg, req->u.regs[i] & 0xFFFFU);
+        mg_send(c, &reg, 2);
+      }
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
+      if (req->len != 1 || req->u.bits == NULL) return;
+      mg_modbus_send_mbap(c, txid, 5, unit_id);
+      buf[0] = req->func;
+      MG_STORE_BE16(&buf[1], req->addr & 0xFFFFu);
+      MG_STORE_BE16(&buf[3], req->u.bits[0] ? 0xFF00U : 0x0000U);
+      mg_send(c, buf, 5);
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
+      if (req->len != 1 || req->u.regs == NULL) return;
+      mg_modbus_send_mbap(c, txid, 5, unit_id);
+      buf[0] = req->func;
+      MG_STORE_BE16(&buf[1], req->addr  & 0xFFFFU);
+      MG_STORE_BE16(&buf[3], req->u.regs[0]  & 0xFFFFU);
+      mg_send(c, buf, 5);
+      break;
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS:
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS:
+      mg_modbus_send_mbap(c, txid, 5, unit_id);
+      buf[0] = req->func;
+      MG_STORE_BE16(&buf[1], req->addr & 0xFFFFU);
+      MG_STORE_BE16(&buf[3], req->len & 0xFFFFU);
+      mg_send(c, buf, 5);
+      break;
+    default:
+      break;
+  }
+}
+
+static void handle_pdu(struct mg_connection *c, uint8_t *buf, size_t len) {
+  uint16_t max_len, val, txid, addr, quantity;
+  uint8_t func, unit_id, byte_count, packed;
+  int i;
+  size_t pdu_len = len - MG_MODBUS_HEADER_LEN, len_check;
+  struct mg_modbus_req mr;
+
+  if (len < MG_MODBUS_MIN_PAYLOAD_LEN) return;
+  memset(&mr, 0, sizeof(mr));
+  txid = MG_LOAD_BE16(&buf[0]);
+  unit_id = buf[6];
+  func = buf[7];
+  addr = MG_LOAD_BE16(&buf[8]);
+  mr.func = func;
+  mr.addr = addr;
+  mr.error = MG_MODBUS_ERR_NONE;
+  mr.len = 0;
+  mr.u.bits = NULL;
+  switch (func) {
+    case MG_MODBUS_FUNC_READ_COILS:
+    case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      quantity = MG_LOAD_BE16(&buf[10]);
+      max_len = 0x07D0;  // 2000 bits
+      if (quantity == 0 || quantity > max_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.bits = (bool *) mg_calloc((size_t) mr.len, sizeof(bool));
+      if (mr.u.bits == NULL) goto modbus_oom;
+      break;
+    case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
+    case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      quantity = MG_LOAD_BE16(&buf[10]);
+      max_len = 0x007D;  // 125 registers
+      if (quantity == 0 || quantity > max_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.regs = (uint16_t *) mg_calloc((size_t) mr.len, sizeof(uint16_t));
+      if (mr.u.regs == NULL) goto modbus_oom;
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      mr.len = 1;
+      mr.u.bits = (bool *) mg_calloc(1, sizeof(bool));
+      if (mr.u.bits == NULL) goto modbus_oom;
+      val = MG_LOAD_BE16(&buf[10]);
+      if (val != 0x0000 && val != 0xFF00) goto modbus_illegal_value;
+      mr.u.bits[0] = (val == 0xFF00);
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      mr.len = 1;
+      mr.u.regs = (uint16_t *) mg_calloc(1, sizeof(uint16_t));
+      if (mr.u.regs == NULL) goto modbus_oom;
+      mr.u.regs[0] = MG_LOAD_BE16(&buf[10]);
+      break;
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS: {
+      quantity = MG_LOAD_BE16(&buf[10]);
+      if (quantity == 0 || quantity > 0x07B0) goto modbus_illegal_value;
+      if (pdu_len < 6) goto modbus_illegal_value;
+      byte_count = buf[12];
+      if (byte_count != (uint8_t) ((quantity + 7) / 8)) goto modbus_illegal_value;
+      len_check = (size_t) (6 + byte_count);
+      if (len_check != pdu_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.bits = (bool *) mg_calloc((size_t) mr.len, sizeof(bool));
+      if (mr.u.bits == NULL) goto modbus_oom;
+      for (i = 0; i < mr.len; i++) {
+        packed = buf[13 + (i / 8)];
+        mr.u.bits[i] = ((packed >> (i % 8)) & 1) != 0;
+      }
+      break;
+    }
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS: {
+      quantity = MG_LOAD_BE16(&buf[10]);
+      if (quantity == 0 || quantity > 0x007B) goto modbus_illegal_value;
+      if (pdu_len < 6) goto modbus_illegal_value;
+      byte_count = buf[12];
+      if (byte_count != (uint8_t) (quantity * 2)) goto modbus_illegal_value;
+      len_check = (size_t) (6 + quantity * 2);
+      if (len_check != pdu_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.regs = (uint16_t *) mg_calloc((size_t) mr.len, sizeof(uint16_t));
+      if (mr.u.regs == NULL) goto modbus_oom;
+      for (i = 0; i < mr.len; i++) {
+        mr.u.regs[i] = MG_LOAD_BE16(&buf[13 + i * 2]);
+      }
+      break;
+    }
+    default:
+      MG_ERROR(("Unsupported modbus function"));
+      mr.error = MG_MODBUS_ERR_ILLEGAL_FUNCTION;
+      mg_modbus_send_response(c, &mr, txid, unit_id);
+      return;
+  }
+  mg_call(c, MG_EV_MODBUS_REQ, &mr);
+  goto modbus_exit;
+
+modbus_illegal_value:
+  MG_ERROR(("Invalid data"));
+  mr.error = MG_MODBUS_ERR_ILLEGAL_VALUE;
+  goto modbus_exit;
+modbus_oom:
+  MG_ERROR(("OOM"));
+  mr.error = MG_MODBUS_ERR_DEVICE_FAILURE;
+modbus_exit:
+  mg_modbus_send_response(c, &mr, txid, unit_id);
+  if (mr.u.bits != NULL) mg_free(mr.u.bits);
+}
+
+static void modbus_ev_handler(struct mg_connection *c, int ev, void *ev_data) {
+  if (ev == MG_EV_READ) {
+    while (c->recv.len >= MG_MODBUS_MIN_PAYLOAD_LEN) {
+      uint16_t len = MG_LOAD_BE16(&c->recv.buf[4]);  // PDU length
+      uint16_t proto_id = MG_LOAD_BE16(&c->recv.buf[2]);
+      if (len < 6 || proto_id != 0) {
+        mg_error(c, "invalid pdu");
+        break;
+      } else if (c->recv.len < len + 6U) {
+        break;  // Partial frame, buffer more
+      } else {
+        handle_pdu(c, c->recv.buf, len + 6);  // Parse PDU and call user
+        mg_iobuf_del(&c->recv, 0, len + 6);   // Delete received PDU
+      }
+    }
+  }
+  (void) ev_data;
+}
+
+struct mg_connection *mg_modbus_listen(struct mg_mgr *mgr, const char *url,
+                                       mg_event_handler_t fn, void *fn_data) {
+  struct mg_connection *c = mg_listen(mgr, url, fn, fn_data);
+  if (c != NULL) c->pfn = modbus_ev_handler;
+  return c;
+}
+
 #ifdef MG_ENABLE_LINES
 #line 1 "src/mqtt.c"
 #endif
diff --git a/mongoose.h b/mongoose.h
index ecadb01e..9cf46442 100644
--- a/mongoose.h
+++ b/mongoose.h
@@ -1660,6 +1660,7 @@ enum {
   MG_EV_WAKEUP,     // mg_wakeup() data received    struct mg_str *data
   MG_EV_MDNS_REQ,   // mDNS request                 struct mg_mdns_req *
   MG_EV_MDNS_RESP,  // mDNS response                struct mg_mdns_resp *
+  MG_EV_MODBUS_REQ, // ModBus request               struct mg_modbus_cmd *
   MG_EV_USER        // Starting ID for user events
 };
 
@@ -3024,6 +3025,40 @@ bool mg_mdns_query(struct mg_connection *, const char *, unsigned int);
 
 
 
+// Functions
+#define MG_MODBUS_FUNC_READ_COILS 1
+#define MG_MODBUS_FUNC_READ_DISCRETE_INPUTS 2
+#define MG_MODBUS_FUNC_READ_HOLDING_REGISTERS 3
+#define MG_MODBUS_FUNC_READ_INPUT_REGISTERS 4
+#define MG_MODBUS_FUNC_WRITE_SINGLE_COIL 5
+#define MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER 6
+#define MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS 15
+#define MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS 16
+
+// Error codes
+#define MG_MODBUS_ERR_NONE 0
+#define MG_MODBUS_ERR_ILLEGAL_FUNCTION 1
+#define MG_MODBUS_ERR_ILLEGAL_ADDRESS 2
+#define MG_MODBUS_ERR_ILLEGAL_VALUE 3
+#define MG_MODBUS_ERR_DEVICE_FAILURE 4
+
+struct mg_modbus_req {
+  uint8_t func;      // Function code, one of MG_MODBUS_FUNC_*
+  uint8_t error;     // Error code user handler can set
+  uint16_t addr;     // Address
+  union {
+    bool *bits;
+    uint16_t *regs;
+  } u;
+  uint16_t len;      // Number of registers or bits
+};
+
+struct mg_connection *mg_modbus_listen(struct mg_mgr *mgr, const char *url,
+                                       mg_event_handler_t fn, void *fn_data);
+
+
+
+
 
 #ifndef MG_JSON_MAX_DEPTH
 #define MG_JSON_MAX_DEPTH 30
diff --git a/src/event.h b/src/event.h
index 8f194400..bf44f1ee 100644
--- a/src/event.h
+++ b/src/event.h
@@ -29,5 +29,6 @@ enum {
   MG_EV_WAKEUP,     // mg_wakeup() data received    struct mg_str *data
   MG_EV_MDNS_REQ,   // mDNS request                 struct mg_mdns_req *
   MG_EV_MDNS_RESP,  // mDNS response                struct mg_mdns_resp *
+  MG_EV_MODBUS_REQ, // ModBus request               struct mg_modbus_cmd *
   MG_EV_USER        // Starting ID for user events
 };
diff --git a/src/modbus.c b/src/modbus.c
new file mode 100644
index 00000000..e4e02c51
--- /dev/null
+++ b/src/modbus.c
@@ -0,0 +1,229 @@
+#include "modbus.h"
+#include "event.h"
+#include "util.h"
+#include "log.h"
+
+#define MG_MODBUS_MIN_PAYLOAD_LEN 12
+#define MG_MODBUS_HEADER_LEN 7
+
+static void mg_modbus_send_mbap(struct mg_connection *c, uint16_t txid,
+                                uint16_t pdu_len, uint8_t unit_id) {
+  uint8_t hdr[7];
+  memset(hdr, 0, sizeof(hdr));
+  MG_STORE_BE16(&hdr[0], txid & 0xFFFFU);
+  MG_STORE_BE16(&hdr[4], (pdu_len + 1) & 0xFFFFU);
+  hdr[6] = unit_id;
+  mg_send(c, hdr, sizeof(hdr));
+}
+
+static void mg_modbus_send_response(struct mg_connection *c,
+                                    struct mg_modbus_req *req,
+                                    uint16_t txid, uint8_t unit_id) {
+  uint8_t buf[5];
+  uint8_t out;
+  uint16_t reg;
+  uint16_t i, nbytes;
+
+  memset(buf, 0, sizeof(buf));
+  if (req == NULL) return;
+  if (req->error != MG_MODBUS_ERR_NONE) {
+    mg_modbus_send_mbap(c, txid, 2, unit_id);
+    buf[0] = (uint8_t) (req->func | 0x80);
+    buf[1] = req->error;
+    mg_send(c, buf, 2);
+    return;
+  }
+  switch (req->func) {
+    case MG_MODBUS_FUNC_READ_COILS:
+    case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
+      if (req->u.bits == NULL) return;
+      nbytes = (uint16_t) ((req->len + 7) / 8);
+      mg_modbus_send_mbap(c, txid, (uint16_t) (nbytes + 2), unit_id);
+      buf[0] = req->func;
+      buf[1] = (uint8_t) nbytes;
+      mg_send(c, buf, 2);
+      memset(buf, 0, sizeof(buf));
+      for (i = 0; i < req->len; i++) {
+        if (i % 8 == 0) out = 0;
+        if (req->u.bits[i]) out |= (uint8_t) (1 << (i % 8));
+        if ((i % 8) == 7 || i + 1 == req->len) {
+          mg_send(c, &out, 1);
+        }
+      }
+      break;
+    case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
+    case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
+      if (req->u.regs == NULL) return;
+      nbytes = (uint16_t) (req->len * 2);
+      mg_modbus_send_mbap(c, txid, (uint16_t) (nbytes + 2), unit_id);
+      buf[0] = req->func;
+      buf[1] = (uint8_t) nbytes;
+      mg_send(c, buf, 2);
+      for (i = 0; i < req->len; i++) {
+        MG_STORE_BE16(&reg, req->u.regs[i] & 0xFFFFU);
+        mg_send(c, &reg, 2);
+      }
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
+      if (req->len != 1 || req->u.bits == NULL) return;
+      mg_modbus_send_mbap(c, txid, 5, unit_id);
+      buf[0] = req->func;
+      MG_STORE_BE16(&buf[1], req->addr & 0xFFFFu);
+      MG_STORE_BE16(&buf[3], req->u.bits[0] ? 0xFF00U : 0x0000U);
+      mg_send(c, buf, 5);
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
+      if (req->len != 1 || req->u.regs == NULL) return;
+      mg_modbus_send_mbap(c, txid, 5, unit_id);
+      buf[0] = req->func;
+      MG_STORE_BE16(&buf[1], req->addr  & 0xFFFFU);
+      MG_STORE_BE16(&buf[3], req->u.regs[0]  & 0xFFFFU);
+      mg_send(c, buf, 5);
+      break;
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS:
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS:
+      mg_modbus_send_mbap(c, txid, 5, unit_id);
+      buf[0] = req->func;
+      MG_STORE_BE16(&buf[1], req->addr & 0xFFFFU);
+      MG_STORE_BE16(&buf[3], req->len & 0xFFFFU);
+      mg_send(c, buf, 5);
+      break;
+    default:
+      break;
+  }
+}
+
+static void handle_pdu(struct mg_connection *c, uint8_t *buf, size_t len) {
+  uint16_t max_len, val, txid, addr, quantity;
+  uint8_t func, unit_id, byte_count, packed;
+  int i;
+  size_t pdu_len = len - MG_MODBUS_HEADER_LEN, len_check;
+  struct mg_modbus_req mr;
+
+  if (len < MG_MODBUS_MIN_PAYLOAD_LEN) return;
+  memset(&mr, 0, sizeof(mr));
+  txid = MG_LOAD_BE16(&buf[0]);
+  unit_id = buf[6];
+  func = buf[7];
+  addr = MG_LOAD_BE16(&buf[8]);
+  mr.func = func;
+  mr.addr = addr;
+  mr.error = MG_MODBUS_ERR_NONE;
+  mr.len = 0;
+  mr.u.bits = NULL;
+  switch (func) {
+    case MG_MODBUS_FUNC_READ_COILS:
+    case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      quantity = MG_LOAD_BE16(&buf[10]);
+      max_len = 0x07D0;  // 2000 bits
+      if (quantity == 0 || quantity > max_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.bits = (bool *) mg_calloc((size_t) mr.len, sizeof(bool));
+      if (mr.u.bits == NULL) goto modbus_oom;
+      break;
+    case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
+    case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      quantity = MG_LOAD_BE16(&buf[10]);
+      max_len = 0x007D;  // 125 registers
+      if (quantity == 0 || quantity > max_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.regs = (uint16_t *) mg_calloc((size_t) mr.len, sizeof(uint16_t));
+      if (mr.u.regs == NULL) goto modbus_oom;
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      mr.len = 1;
+      mr.u.bits = (bool *) mg_calloc(1, sizeof(bool));
+      if (mr.u.bits == NULL) goto modbus_oom;
+      val = MG_LOAD_BE16(&buf[10]);
+      if (val != 0x0000 && val != 0xFF00) goto modbus_illegal_value;
+      mr.u.bits[0] = (val == 0xFF00);
+      break;
+    case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
+      if (pdu_len != 5) goto modbus_illegal_value;
+      mr.len = 1;
+      mr.u.regs = (uint16_t *) mg_calloc(1, sizeof(uint16_t));
+      if (mr.u.regs == NULL) goto modbus_oom;
+      mr.u.regs[0] = MG_LOAD_BE16(&buf[10]);
+      break;
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS: {
+      quantity = MG_LOAD_BE16(&buf[10]);
+      if (quantity == 0 || quantity > 0x07B0) goto modbus_illegal_value;
+      if (pdu_len < 6) goto modbus_illegal_value;
+      byte_count = buf[12];
+      if (byte_count != (uint8_t) ((quantity + 7) / 8)) goto modbus_illegal_value;
+      len_check = (size_t) (6 + byte_count);
+      if (len_check != pdu_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.bits = (bool *) mg_calloc((size_t) mr.len, sizeof(bool));
+      if (mr.u.bits == NULL) goto modbus_oom;
+      for (i = 0; i < mr.len; i++) {
+        packed = buf[13 + (i / 8)];
+        mr.u.bits[i] = ((packed >> (i % 8)) & 1) != 0;
+      }
+      break;
+    }
+    case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS: {
+      quantity = MG_LOAD_BE16(&buf[10]);
+      if (quantity == 0 || quantity > 0x007B) goto modbus_illegal_value;
+      if (pdu_len < 6) goto modbus_illegal_value;
+      byte_count = buf[12];
+      if (byte_count != (uint8_t) (quantity * 2)) goto modbus_illegal_value;
+      len_check = (size_t) (6 + quantity * 2);
+      if (len_check != pdu_len) goto modbus_illegal_value;
+      mr.len = quantity;
+      mr.u.regs = (uint16_t *) mg_calloc((size_t) mr.len, sizeof(uint16_t));
+      if (mr.u.regs == NULL) goto modbus_oom;
+      for (i = 0; i < mr.len; i++) {
+        mr.u.regs[i] = MG_LOAD_BE16(&buf[13 + i * 2]);
+      }
+      break;
+    }
+    default:
+      MG_ERROR(("Unsupported modbus function"));
+      mr.error = MG_MODBUS_ERR_ILLEGAL_FUNCTION;
+      mg_modbus_send_response(c, &mr, txid, unit_id);
+      return;
+  }
+  mg_call(c, MG_EV_MODBUS_REQ, &mr);
+  goto modbus_exit;
+
+modbus_illegal_value:
+  MG_ERROR(("Invalid data"));
+  mr.error = MG_MODBUS_ERR_ILLEGAL_VALUE;
+  goto modbus_exit;
+modbus_oom:
+  MG_ERROR(("OOM"));
+  mr.error = MG_MODBUS_ERR_DEVICE_FAILURE;
+modbus_exit:
+  mg_modbus_send_response(c, &mr, txid, unit_id);
+  if (mr.u.bits != NULL) mg_free(mr.u.bits);
+}
+
+static void modbus_ev_handler(struct mg_connection *c, int ev, void *ev_data) {
+  if (ev == MG_EV_READ) {
+    while (c->recv.len >= MG_MODBUS_MIN_PAYLOAD_LEN) {
+      uint16_t len = MG_LOAD_BE16(&c->recv.buf[4]);  // PDU length
+      uint16_t proto_id = MG_LOAD_BE16(&c->recv.buf[2]);
+      if (len < 6 || proto_id != 0) {
+        mg_error(c, "invalid pdu");
+        break;
+      } else if (c->recv.len < len + 6U) {
+        break;  // Partial frame, buffer more
+      } else {
+        handle_pdu(c, c->recv.buf, len + 6);  // Parse PDU and call user
+        mg_iobuf_del(&c->recv, 0, len + 6);   // Delete received PDU
+      }
+    }
+  }
+  (void) ev_data;
+}
+
+struct mg_connection *mg_modbus_listen(struct mg_mgr *mgr, const char *url,
+                                       mg_event_handler_t fn, void *fn_data) {
+  struct mg_connection *c = mg_listen(mgr, url, fn, fn_data);
+  if (c != NULL) c->pfn = modbus_ev_handler;
+  return c;
+}
diff --git a/src/modbus.h b/src/modbus.h
new file mode 100644
index 00000000..1d01583e
--- /dev/null
+++ b/src/modbus.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "net.h"
+
+// Functions
+#define MG_MODBUS_FUNC_READ_COILS 1
+#define MG_MODBUS_FUNC_READ_DISCRETE_INPUTS 2
+#define MG_MODBUS_FUNC_READ_HOLDING_REGISTERS 3
+#define MG_MODBUS_FUNC_READ_INPUT_REGISTERS 4
+#define MG_MODBUS_FUNC_WRITE_SINGLE_COIL 5
+#define MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER 6
+#define MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS 15
+#define MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS 16
+
+// Error codes
+#define MG_MODBUS_ERR_NONE 0
+#define MG_MODBUS_ERR_ILLEGAL_FUNCTION 1
+#define MG_MODBUS_ERR_ILLEGAL_ADDRESS 2
+#define MG_MODBUS_ERR_ILLEGAL_VALUE 3
+#define MG_MODBUS_ERR_DEVICE_FAILURE 4
+
+struct mg_modbus_req {
+  uint8_t func;      // Function code, one of MG_MODBUS_FUNC_*
+  uint8_t error;     // Error code user handler can set
+  uint16_t addr;     // Address
+  union {
+    bool *bits;
+    uint16_t *regs;
+  } u;
+  uint16_t len;      // Number of registers or bits
+};
+
+struct mg_connection *mg_modbus_listen(struct mg_mgr *mgr, const char *url,
+                                       mg_event_handler_t fn, void *fn_data);
diff --git a/test/Makefile b/test/Makefile
index 578e5a51..58558cf3 100644
--- a/test/Makefile
+++ b/test/Makefile
@@ -251,7 +251,7 @@ mongoose.c: Makefile $(wildcard ../src/*.c) $(wildcard ../src/drivers/*.c)
 	cd .. && (export LC_ALL=C ; cat src/license.h; echo; echo '#include "mongoose.h"' ; (for F in src/*.c src/drivers/*.c ; do echo; echo '#ifdef MG_ENABLE_LINES'; echo "#line 1 \"$$F\""; echo '#endif'; cat $$F | sed -e 's,#include ".*,,'; done))> $@
 
 mongoose.h: $(HDRS) Makefile
-	cd .. && (cat src/license.h; echo; echo '#ifndef MONGOOSE_H'; echo '#define MONGOOSE_H'; echo; cat src/version.h ; echo; echo '#ifdef __cplusplus'; echo 'extern "C" {'; echo '#endif'; cat src/arch.h src/arch_*.h src/net_ft.h src/net_lwip.h src/net_rl.h src/config.h src/profile.h src/str.h src/queue.h src/fmt.h src/printf.h src/log.h src/timer.h src/fs.h src/util.h src/url.h src/iobuf.h src/base64.h src/md5.h src/sha1.h src/sha256.h src/event.h src/net.h src/http.h src/ssi.h src/tls.h src/tls_x25519.h src/tls_aes128.h src/tls_uecc.h src/tls_chacha20.h src/tls_rsa.h src/tls_mbed.h src/tls_openssl.h src/ws.h src/sntp.h src/mqtt.h src/dns.h src/json.h src/rpc.h src/ota.h src/flash.h src/wifi.h src/l2.h src/net_builtin.h src/drivers/*.h | sed -e '/keep/! s,#include ".*,,' -e 's,^#pragma once,,'; echo; echo '#ifdef __cplusplus'; echo '}'; echo '#endif'; echo '#endif  // MONGOOSE_H')> $@
+	cd .. && (cat src/license.h; echo; echo '#ifndef MONGOOSE_H'; echo '#define MONGOOSE_H'; echo; cat src/version.h ; echo; echo '#ifdef __cplusplus'; echo 'extern "C" {'; echo '#endif'; cat src/arch.h src/arch_*.h src/net_ft.h src/net_lwip.h src/net_rl.h src/config.h src/profile.h src/str.h src/queue.h src/fmt.h src/printf.h src/log.h src/timer.h src/fs.h src/util.h src/url.h src/iobuf.h src/base64.h src/md5.h src/sha1.h src/sha256.h src/event.h src/net.h src/http.h src/ssi.h src/tls.h src/tls_x25519.h src/tls_aes128.h src/tls_uecc.h src/tls_chacha20.h src/tls_rsa.h src/tls_mbed.h src/tls_openssl.h src/ws.h src/sntp.h src/mqtt.h src/dns.h src/modbus.h src/json.h src/rpc.h src/ota.h src/flash.h src/wifi.h src/l2.h src/net_builtin.h src/drivers/*.h | sed -e '/keep/! s,#include ".*,,' -e 's,^#pragma once,,'; echo; echo '#ifdef __cplusplus'; echo '}'; echo '#endif'; echo '#endif  // MONGOOSE_H')> $@
 
 
 clean: clean_tutorials clean_tutorials_embedded
@@ -285,4 +285,3 @@ tutorials_embedded:
 	@for X in $(TUTORIALS_EMBEDDED); do test -f $$X/Makefile || continue; $(MAKE) -C $$X build || exit 1; done
 clean_tutorials_embedded:
 	for X in $(TUTORIALS_EMBEDDED); do test -f $$X/Makefile || continue; $(MAKE) -C $$X clean || exit 1; done
-
diff --git a/test/unit_test.c b/test/unit_test.c
index 5a7028e8..189a8161 100644
--- a/test/unit_test.c
+++ b/test/unit_test.c
@@ -768,7 +768,8 @@ static void test_mqtt_ver(uint8_t mqtt_version) {
   memset(mbuf + 1, 0, sizeof(mbuf) - 1);
   test_data.flags = 0;
 
-  opts.props = 0; opts.num_props = 0;
+  opts.props = 0;
+  opts.num_props = 0;
   mg_mqtt_unsub(c, &opts);
   for (i = 0; i < 500 && test_data.flags == 0; i++) mg_mgr_poll(&mgr, 10);
   ASSERT(test_data.flags == flags_unsubscribed);
@@ -1466,7 +1467,8 @@ static void test_tls(void) {
                "%s",
                bd.len, bd.buf) == 200);
   ASSERT(cmpbody(buf, bd.buf) == 0);
-#if MG_TLS == MG_TLS_BUILTIN && defined(__linux__) && MG_ENABLE_CHACHA20  // skip for non-CHACHA tests
+#if MG_TLS == MG_TLS_BUILTIN && defined(__linux__) && \
+    MG_ENABLE_CHACHA20  // skip for non-CHACHA tests
   // fire patched server, test multiple TLS records per TCP segment handling
   // skip other TLS stacks to avoid "bad client hello", we are 1.3 only
   if (access("tls_multirec/server", X_OK) == 0) {
@@ -4041,11 +4043,359 @@ static void test_crypto(void) {
 #define DASHBOARD(x) \
   printf("HEALTH_DASHBOARD\t\"%s\": %s,\n", x, s_error ? "false" : "true");
 
+static uint8_t coils_database[25] = {
+  0x4D, 0x9C, 0xAA, 0x55, 0xCC,
+  0xA3, 0x66, 0xAF, 0x60, 0xBC,
+  0xCC, 0x6C, 0x53, 0xFF, 0x00,
+  0x55, 0x3C, 0x0F, 0xF0, 0x8F,
+  0x54, 0x99, 0xF8, 0x0D, 0x2A,
+};
+
+static bool modbus_read_coil(uint16_t i) {
+  uint16_t start_byte = i / 8;
+  int bit_pos = i % 8;
+  uint8_t data = ((coils_database[start_byte] >> bit_pos) & 0x01);
+  return data;
+}
+
+static void modbus_srv(struct mg_connection *c, int ev, void *ev_data) {
+  struct mg_modbus_req *mr = (struct mg_modbus_req *) ev_data;
+  uint16_t i, r;
+  size_t tc = *(size_t *) c->fn_data + 1;
+  static bool coil_1[] = {1};
+  static bool coil_7[] = {1, 0, 1, 0, 1, 0, 1};
+  static uint16_t reg_1[] = {0x0001};
+  static uint16_t reg_7[] = {0x0001, 0x0002, 0x0003, 0x0004,
+                             0x0005, 0x0006, 0x0007};
+
+  if (ev == MG_EV_READ) {
+    *(size_t *) c->fn_data += 1;
+  } else if (ev == MG_EV_MODBUS_REQ) {
+    if (mr->addr == 0xFFFF) {
+      mr->error = MG_MODBUS_ERR_ILLEGAL_ADDRESS;
+      return;
+    }
+    switch (mr->func) {
+      case MG_MODBUS_FUNC_READ_COILS:
+      case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
+        if (tc != 0x1D) {
+          for (i = 0; i < mr->len; i++) {
+            mr->u.bits[i] = (mr->len == 1 ? coil_1[i] : coil_7[i]);
+          }
+        } else {
+          for (i = 0; i < mr->len; i++) {
+            mr->u.bits[i] = modbus_read_coil(i);
+          }
+        }
+        break;
+      case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
+      case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
+        if (tc == 0x1c) {
+          mr->error = MG_MODBUS_ERR_DEVICE_FAILURE;
+          break;
+        }
+        if (mr->len == 1) {
+          mr->u.regs[0] = reg_1[0];
+        } else if (mr->len == 7) {
+          for (i = 0; i < 7; i++) mr->u.regs[i] = reg_7[i];
+        } else {
+          mr->error = MG_MODBUS_ERR_ILLEGAL_ADDRESS;
+        }
+        break;
+
+      case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
+        switch (tc) {
+          case 0x05:
+            ASSERT(mr->len == 1);
+            ASSERT(mr->addr == 0x14);
+            ASSERT(mr->u.bits != NULL);
+            ASSERT(mr->u.bits[0] == true);
+            break;
+          default:
+            ASSERT(false);
+            return;
+        }
+        break;
+      case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
+        switch (tc) {
+          case 0x06:
+            ASSERT(mr->len == 1);
+            ASSERT(mr->addr == 0x15);
+            ASSERT(mr->u.regs != NULL);
+            ASSERT(mr->u.regs[0] == 1);
+            break;
+          default:
+            ASSERT(false);
+            return;
+        }
+        break;
+      case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS:
+        switch (tc) {
+          case 0x14:
+            ASSERT(mr->addr == 0x23);
+            ASSERT(mr->len == 1);
+            ASSERT(mr->u.bits != NULL);
+            ASSERT(mr->u.bits[0] == true);
+            break;
+          case 0x15:
+            ASSERT(mr->addr == 0x24);
+            ASSERT(mr->len == 7);
+            ASSERT(mr->u.bits != NULL);
+            for (i = 0; i < 7; i++) {
+              ASSERT(mr->u.bits[i] == coil_7[i]);
+            }
+            break;
+          default:
+            ASSERT(false);
+            return;
+        }
+        break;
+      case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS:
+        switch (tc) {
+          case 0x17:
+            r = 0;
+            ASSERT(mr->addr == 0x26);
+            ASSERT(mr->len == 1);
+            ASSERT(mr->u.regs != NULL);
+            r = mr->u.regs[0];
+            ASSERT(r == 1);
+            break;
+          case 0x18:
+            ASSERT(mr->addr == 0x27);
+            ASSERT(mr->len == 7);
+            ASSERT(mr->u.regs != NULL);
+            for (i = 0; i < 7; i++) {
+              r = mr->u.regs[i];
+              ASSERT(r == reg_7[i]);
+            }
+            break;
+          default:
+            ASSERT(false);
+            return;
+        }
+        break;
+      default:
+        mr->error = MG_MODBUS_ERR_ILLEGAL_FUNCTION;
+        break;
+    }
+  }
+  (void) c, (void) ev_data;
+}
+
+static void test_modbus(void) {
+  struct modbus_test {
+    const char *name;
+    uint8_t req[256];
+    size_t req_len;
+    uint8_t resp[256];
+    size_t resp_len;
+  } tests[] = {
+      {"read coils - 1 @ 0x0010",
+       {0x00, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01, 0x01, 0x00, 0x10, 0x00, 0x01},
+       12,
+       {0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01, 0x01, 0x01, 0x01},
+       10},
+      {"read discrete inputs - 1 @ 0x0011",
+       {0x00, 0x02, 0x00, 0x00, 0x00, 0x06, 0x01, 0x02, 0x00, 0x11, 0x00, 0x01},
+       12,
+       {0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x01, 0x02, 0x01, 0x01},
+       10},
+      {"read holding registers - 1 @ 0x0012",
+       {0x00, 0x03, 0x00, 0x00, 0x00, 0x06, 0x01, 0x03, 0x00, 0x12, 0x00, 0x01},
+       12,
+       {0x00, 0x03, 0x00, 0x00, 0x00, 0x05, 0x01, 0x03, 0x02, 0x00, 0x01},
+       11},
+      {"read input registers - 1 @ 0x0013",
+       {0x00, 0x04, 0x00, 0x00, 0x00, 0x06, 0x01, 0x04, 0x00, 0x13, 0x00, 0x01},
+       12,
+       {0x00, 0x04, 0x00, 0x00, 0x00, 0x05, 0x01, 0x04, 0x02, 0x00, 0x01},
+       11},
+      {"write single coil @ 0x0014 = 1",
+       {0x00, 0x05, 0x00, 0x00, 0x00, 0x06, 0x01, 0x05, 0x00, 0x14, 0xFF, 0x00},
+       12,
+       {0x00, 0x05, 0x00, 0x00, 0x00, 0x06, 0x01, 0x05, 0x00, 0x14, 0xFF, 0x00},
+       12},
+      {"write single register @ 0x0015 = 1",
+       {0x00, 0x06, 0x00, 0x00, 0x00, 0x06, 0x01, 0x06, 0x00, 0x15, 0x00, 0x01},
+       12,
+       {0x00, 0x06, 0x00, 0x00, 0x00, 0x06, 0x01, 0x06, 0x00, 0x15, 0x00, 0x01},
+       12},
+
+      /* --- Multi-value reads: 0, 1, 7 --- */
+
+      {"read coils - 0 @ 0x0016",
+       {0x00, 0x07, 0x00, 0x00, 0x00, 0x06, 0x01, 0x01, 0x00, 0x16, 0x00, 0x00},
+       12,
+       {0x00, 0x07, 0x00, 0x00, 0x00, 0x03, 0x01, 0x81, 0x03},
+       9},
+      {"read coils - 1 @ 0x0017",
+       {0x00, 0x08, 0x00, 0x00, 0x00, 0x06, 0x01, 0x01, 0x00, 0x17, 0x00, 0x01},
+       12,
+       {0x00, 0x08, 0x00, 0x00, 0x00, 0x04, 0x01, 0x01, 0x01, 0x01},
+       10},
+      {"read coils - 7 @ 0x0018",
+       {0x00, 0x09, 0x00, 0x00, 0x00, 0x06, 0x01, 0x01, 0x00, 0x18, 0x00, 0x07},
+       12,
+       {0x00, 0x09, 0x00, 0x00, 0x00, 0x04, 0x01, 0x01, 0x01, 0x55},
+       10},
+
+      {"read discrete inputs - 0 @ 0x0019",
+       {0x00, 0x0A, 0x00, 0x00, 0x00, 0x06, 0x01, 0x02, 0x00, 0x19, 0x00, 0x00},
+       12,
+       {0x00, 0x0A, 0x00, 0x00, 0x00, 0x03, 0x01, 0x82, 0x03},
+       9},
+      {"read discrete inputs - 1 @ 0x001A",
+       {0x00, 0x0B, 0x00, 0x00, 0x00, 0x06, 0x01, 0x02, 0x00, 0x1A, 0x00, 0x01},
+       12,
+       {0x00, 0x0B, 0x00, 0x00, 0x00, 0x04, 0x01, 0x02, 0x01, 0x01},
+       10},
+      {"read discrete inputs - 7 @ 0x001B",
+       {0x00, 0x0C, 0x00, 0x00, 0x00, 0x06, 0x01, 0x02, 0x00, 0x1B, 0x00, 0x07},
+       12,
+       {0x00, 0x0C, 0x00, 0x00, 0x00, 0x04, 0x01, 0x02, 0x01, 0x55},
+       10},
+
+      {"read holding registers - 0 @ 0x001C",
+       {0x00, 0x0D, 0x00, 0x00, 0x00, 0x06, 0x01, 0x03, 0x00, 0x1C, 0x00, 0x00},
+       12,
+       {0x00, 0x0D, 0x00, 0x00, 0x00, 0x03, 0x01, 0x83, 0x03},
+       9},
+      {"read holding registers - 1 @ 0x001D",
+       {0x00, 0x0E, 0x00, 0x00, 0x00, 0x06, 0x01, 0x03, 0x00, 0x1D, 0x00, 0x01},
+       12,
+       {0x00, 0x0E, 0x00, 0x00, 0x00, 0x05, 0x01, 0x03, 0x02, 0x00, 0x01},
+       11},
+      {"read holding registers - 7 @ 0x001E",
+       {0x00, 0x0F, 0x00, 0x00, 0x00, 0x06, 0x01, 0x03, 0x00, 0x1E, 0x00, 0x07},
+       12,
+       {0x00, 0x0F, 0x00, 0x00, 0x00, 0x11, 0x01, 0x03, 0x0E, 0x00, 0x01, 0x00,
+        0x02, 0x00, 0x03, 0x00, 0x04, 0x00, 0x05, 0x00, 0x06, 0x00, 0x07},
+       23},
+
+      {"read input registers - 0 @ 0x001F",
+       {0x00, 0x10, 0x00, 0x00, 0x00, 0x06, 0x01, 0x04, 0x00, 0x1F, 0x00, 0x00},
+       12,
+       {0x00, 0x10, 0x00, 0x00, 0x00, 0x03, 0x01, 0x84, 0x03},
+       9},
+      {"read input registers - 1 @ 0x0020",
+       {0x00, 0x11, 0x00, 0x00, 0x00, 0x06, 0x01, 0x04, 0x00, 0x20, 0x00, 0x01},
+       12,
+       {0x00, 0x11, 0x00, 0x00, 0x00, 0x05, 0x01, 0x04, 0x02, 0x00, 0x01},
+       11},
+      {"read input registers - 7 @ 0x0021",
+       {0x00, 0x12, 0x00, 0x00, 0x00, 0x06, 0x01, 0x04, 0x00, 0x21, 0x00, 0x07},
+       12,
+       {0x00, 0x12, 0x00, 0x00, 0x00, 0x11, 0x01, 0x04, 0x0E, 0x00, 0x01, 0x00,
+        0x02, 0x00, 0x03, 0x00, 0x04, 0x00, 0x05, 0x00, 0x06, 0x00, 0x07},
+       23},
+      {"write multiple coils - 0 @ 0x0022",
+       {0x00, 0x13, 0x00, 0x00, 0x00, 0x07, 0x01, 0x0F, 0x00, 0x22, 0x00, 0x00,
+        0x00},
+       13,
+       {0x00, 0x13, 0x00, 0x00, 0x00, 0x03, 0x01, 0x8F, 0x03},
+       9},
+      {"write multiple coils - 1 @ 0x0023",
+       {0x00, 0x14, 0x00, 0x00, 0x00, 0x08, 0x01, 0x0F, 0x00, 0x23, 0x00, 0x01,
+        0x01, 0x01},
+       14,
+       {0x00, 0x14, 0x00, 0x00, 0x00, 0x06, 0x01, 0x0F, 0x00, 0x23, 0x00, 0x01},
+       12},
+      {"write multiple coils - 7 @ 0x0024",
+       {0x00, 0x15, 0x00, 0x00, 0x00, 0x08, 0x01, 0x0F, 0x00, 0x24, 0x00, 0x07,
+        0x01, 0x55},
+       14,
+       {0x00, 0x15, 0x00, 0x00, 0x00, 0x06, 0x01, 0x0F, 0x00, 0x24, 0x00, 0x07},
+       12},
+
+      {"write multiple registers - 0 @ 0x0025",
+       {0x00, 0x16, 0x00, 0x00, 0x00, 0x07, 0x01, 0x10, 0x00, 0x25, 0x00, 0x00,
+        0x00},
+       13,
+       {0x00, 0x16, 0x00, 0x00, 0x00, 0x03, 0x01, 0x90, 0x03},
+       9},
+      {"write multiple registers - 1 @ 0x0026",
+       {0x00, 0x17, 0x00, 0x00, 0x00, 0x09, 0x01, 0x10, 0x00, 0x26, 0x00, 0x01,
+        0x02, 0x00, 0x01},
+       15,
+       {0x00, 0x17, 0x00, 0x00, 0x00, 0x06, 0x01, 0x10, 0x00, 0x26, 0x00, 0x01},
+       12},
+      {"write multiple registers - 7 @ 0x0027",
+       {0x00, 0x18, 0x00, 0x00, 0x00, 0x15, 0x01, 0x10, 0x00,
+        0x27, 0x00, 0x07, 0x0E, 0x00, 0x01, 0x00, 0x02, 0x00,
+        0x03, 0x00, 0x04, 0x00, 0x05, 0x00, 0x06, 0x00, 0x07},
+       27,
+       {0x00, 0x18, 0x00, 0x00, 0x00, 0x06, 0x01, 0x10, 0x00, 0x27, 0x00, 0x07},
+       12},
+
+      /* --- Standard Modbus exception cases --- */
+
+      {"illegal function",
+       {0x00, 0x19, 0x00, 0x00, 0x00, 0x06, 0x01, 0x11, 0x00, 0x28, 0x00, 0x01},
+       12,
+       {0x00, 0x19, 0x00, 0x00, 0x00, 0x03, 0x01, 0x91, 0x01},
+       9},
+      {"illegal data address",
+       {0x00, 0x1A, 0x00, 0x00, 0x00, 0x06, 0x01, 0x03, 0xFF, 0xFF, 0x00, 0x01},
+       12,
+       {0x00, 0x1A, 0x00, 0x00, 0x00, 0x03, 0x01, 0x83, 0x02},
+       9},
+      {"illegal data value - bad coil value",
+       {0x00, 0x1B, 0x00, 0x00, 0x00, 0x06, 0x01, 0x05, 0x00, 0x29, 0x12, 0x34},
+       12,
+       {0x00, 0x1B, 0x00, 0x00, 0x00, 0x03, 0x01, 0x85, 0x03},
+       9},
+      {"server device failure",
+       {0x00, 0x1C, 0x00, 0x00, 0x00, 0x06, 0x01, 0x04, 0x00, 0x2A, 0x00, 0x01},
+       12,
+       {0x00, 0x1C, 0x00, 0x00, 0x00, 0x03, 0x01, 0x84, 0x04},
+       9},
+
+       /* --- Stress Test ---*/
+       {"read coils - 200 @ 0x0000",
+        {0x00, 0x1D, 0x00, 0x00, 0x00, 0x06, 0x01, 0x01, 0x00, 0x00, 0x00, 0xC8},
+        12,
+        {0x00, 0x1D, 0x00, 0x00, 0x00, 0x1C, 0x01, 0x01, 0x19,
+          0x4D, 0x9C, 0xAA, 0x55, 0xCC,
+          0xA3, 0x66, 0xAF, 0x60, 0xBC,
+          0xCC, 0x6C, 0x53, 0xFF, 0x00,
+          0x55, 0x3C, 0x0F, 0xF0, 0x8F,
+          0x54, 0x99, 0xF8, 0x0D, 0x2A},
+        34},
+  };
+
+  struct mg_mgr mgr;
+  struct mg_connection *c;
+  const char *url = "tcp://localhost:32625";
+  size_t i, j, total = 0, num_tests = sizeof(tests) / sizeof(tests[0]);
+
+  mg_mgr_init(&mgr);
+  mg_modbus_listen(&mgr, url, modbus_srv, &total);
+  c = mg_connect(&mgr, url, NULL, NULL);
+
+  for (i = 0; i < num_tests; i++) {
+    MG_INFO(("TEST %s", tests[i].name));
+    mg_send(c, tests[i].req, tests[i].req_len);
+    for (j = 0; j < 20; j++) mg_mgr_poll(&mgr, 1);
+    mg_hexdump(tests[i].req, tests[i].req_len);
+    mg_hexdump(tests[i].resp, tests[i].resp_len);
+    mg_hexdump(c->recv.buf, c->recv.len);
+    ASSERT(c->recv.len == tests[i].resp_len);
+    ASSERT(memcmp(c->recv.buf, tests[i].resp, c->recv.len) == 0);
+    c->recv.len = 0;
+  }
+  ASSERT(total == num_tests);
+  mg_mgr_free(&mgr);
+}
+
 int main(void) {
   const char *debug_level = getenv("V");
   if (debug_level == NULL) debug_level = "3";
   mg_log_set(atoi(debug_level));
 
+  s_error = false;
+  test_modbus();
+  DASHBOARD("modbus");
+
   s_error = false;
   test_crypto();
   DASHBOARD("crypto");