Skip to content

Commit 60e98e7

Browse files
cpqcpq
authored
Merge pull request #3482 from cesanta/modbus
Add modbus-tcp support
2 parents 2740eed + d67b30a commit 60e98e7

File tree

7 files changed

+885
-3
lines changed

7 files changed

+885
-3
lines changed

‎mongoose.c‎

Lines changed: 233 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4596,6 +4596,239 @@ void mg_md5_final(mg_md5_ctx *ctx, unsigned char digest[16]) {
45964596
}
45974597
#endif
45984598

4599+
#ifdef MG_ENABLE_LINES
4600+
#line 1 "src/modbus.c"
4601+
#endif
4602+
4603+
4604+
4605+
4606+
4607+
#define MG_MODBUS_MIN_PAYLOAD_LEN 12
4608+
#define MG_MODBUS_HEADER_LEN 7
4609+
4610+
static void mg_modbus_send_mbap(struct mg_connection *c, uint16_t txid,
4611+
uint16_t pdu_len, uint8_t unit_id) {
4612+
uint8_t hdr[7];
4613+
memset(hdr, 0, sizeof(hdr));
4614+
MG_STORE_BE16(&hdr[0], txid & 0xFFFFU);
4615+
MG_STORE_BE16(&hdr[4], (pdu_len + 1) & 0xFFFFU);
4616+
hdr[6] = unit_id;
4617+
mg_send(c, hdr, sizeof(hdr));
4618+
}
4619+
4620+
static void mg_modbus_send_response(struct mg_connection *c,
4621+
struct mg_modbus_req *req,
4622+
uint16_t txid, uint8_t unit_id) {
4623+
uint8_t buf[5];
4624+
uint8_t out;
4625+
uint16_t reg;
4626+
uint16_t i, nbytes;
4627+
4628+
memset(buf, 0, sizeof(buf));
4629+
if (req == NULL) return;
4630+
if (req->error != MG_MODBUS_ERR_NONE) {
4631+
mg_modbus_send_mbap(c, txid, 2, unit_id);
4632+
buf[0] = (uint8_t) (req->func | 0x80);
4633+
buf[1] = req->error;
4634+
mg_send(c, buf, 2);
4635+
return;
4636+
}
4637+
switch (req->func) {
4638+
case MG_MODBUS_FUNC_READ_COILS:
4639+
case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
4640+
if (req->u.bits == NULL) return;
4641+
nbytes = (uint16_t) ((req->len + 7) / 8);
4642+
mg_modbus_send_mbap(c, txid, (uint16_t) (nbytes + 2), unit_id);
4643+
buf[0] = req->func;
4644+
buf[1] = (uint8_t) nbytes;
4645+
mg_send(c, buf, 2);
4646+
memset(buf, 0, sizeof(buf));
4647+
for (i = 0; i < req->len; i++) {
4648+
if (i % 8 == 0) out = 0;
4649+
if (req->u.bits[i]) out |= (uint8_t) (1 << (i % 8));
4650+
if ((i % 8) == 7 || i + 1 == req->len) {
4651+
mg_send(c, &out, 1);
4652+
}
4653+
}
4654+
break;
4655+
case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
4656+
case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
4657+
if (req->u.regs == NULL) return;
4658+
nbytes = (uint16_t) (req->len * 2);
4659+
mg_modbus_send_mbap(c, txid, (uint16_t) (nbytes + 2), unit_id);
4660+
buf[0] = req->func;
4661+
buf[1] = (uint8_t) nbytes;
4662+
mg_send(c, buf, 2);
4663+
for (i = 0; i < req->len; i++) {
4664+
MG_STORE_BE16(&reg, req->u.regs[i] & 0xFFFFU);
4665+
mg_send(c, &reg, 2);
4666+
}
4667+
break;
4668+
case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
4669+
if (req->len != 1 || req->u.bits == NULL) return;
4670+
mg_modbus_send_mbap(c, txid, 5, unit_id);
4671+
buf[0] = req->func;
4672+
MG_STORE_BE16(&buf[1], req->addr & 0xFFFFu);
4673+
MG_STORE_BE16(&buf[3], req->u.bits[0] ? 0xFF00U : 0x0000U);
4674+
mg_send(c, buf, 5);
4675+
break;
4676+
case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
4677+
if (req->len != 1 || req->u.regs == NULL) return;
4678+
mg_modbus_send_mbap(c, txid, 5, unit_id);
4679+
buf[0] = req->func;
4680+
MG_STORE_BE16(&buf[1], req->addr & 0xFFFFU);
4681+
MG_STORE_BE16(&buf[3], req->u.regs[0] & 0xFFFFU);
4682+
mg_send(c, buf, 5);
4683+
break;
4684+
case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS:
4685+
case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS:
4686+
mg_modbus_send_mbap(c, txid, 5, unit_id);
4687+
buf[0] = req->func;
4688+
MG_STORE_BE16(&buf[1], req->addr & 0xFFFFU);
4689+
MG_STORE_BE16(&buf[3], req->len & 0xFFFFU);
4690+
mg_send(c, buf, 5);
4691+
break;
4692+
default:
4693+
break;
4694+
}
4695+
}
4696+
4697+
static void handle_pdu(struct mg_connection *c, uint8_t *buf, size_t len) {
4698+
uint16_t max_len, val, txid, addr, quantity;
4699+
uint8_t func, unit_id, byte_count, packed;
4700+
int i;
4701+
size_t pdu_len = len - MG_MODBUS_HEADER_LEN, len_check;
4702+
struct mg_modbus_req mr;
4703+
4704+
if (len < MG_MODBUS_MIN_PAYLOAD_LEN) return;
4705+
memset(&mr, 0, sizeof(mr));
4706+
txid = MG_LOAD_BE16(&buf[0]);
4707+
unit_id = buf[6];
4708+
func = buf[7];
4709+
addr = MG_LOAD_BE16(&buf[8]);
4710+
mr.func = func;
4711+
mr.addr = addr;
4712+
mr.error = MG_MODBUS_ERR_NONE;
4713+
mr.len = 0;
4714+
mr.u.bits = NULL;
4715+
switch (func) {
4716+
case MG_MODBUS_FUNC_READ_COILS:
4717+
case MG_MODBUS_FUNC_READ_DISCRETE_INPUTS:
4718+
if (pdu_len != 5) goto modbus_illegal_value;
4719+
quantity = MG_LOAD_BE16(&buf[10]);
4720+
max_len = 0x07D0; // 2000 bits
4721+
if (quantity == 0 || quantity > max_len) goto modbus_illegal_value;
4722+
mr.len = quantity;
4723+
mr.u.bits = (bool *) mg_calloc((size_t) mr.len, sizeof(bool));
4724+
if (mr.u.bits == NULL) goto modbus_oom;
4725+
break;
4726+
case MG_MODBUS_FUNC_READ_HOLDING_REGISTERS:
4727+
case MG_MODBUS_FUNC_READ_INPUT_REGISTERS:
4728+
if (pdu_len != 5) goto modbus_illegal_value;
4729+
quantity = MG_LOAD_BE16(&buf[10]);
4730+
max_len = 0x007D; // 125 registers
4731+
if (quantity == 0 || quantity > max_len) goto modbus_illegal_value;
4732+
mr.len = quantity;
4733+
mr.u.regs = (uint16_t *) mg_calloc((size_t) mr.len, sizeof(uint16_t));
4734+
if (mr.u.regs == NULL) goto modbus_oom;
4735+
break;
4736+
case MG_MODBUS_FUNC_WRITE_SINGLE_COIL:
4737+
if (pdu_len != 5) goto modbus_illegal_value;
4738+
mr.len = 1;
4739+
mr.u.bits = (bool *) mg_calloc(1, sizeof(bool));
4740+
if (mr.u.bits == NULL) goto modbus_oom;
4741+
val = MG_LOAD_BE16(&buf[10]);
4742+
if (val != 0x0000 && val != 0xFF00) goto modbus_illegal_value;
4743+
mr.u.bits[0] = (val == 0xFF00);
4744+
break;
4745+
case MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER:
4746+
if (pdu_len != 5) goto modbus_illegal_value;
4747+
mr.len = 1;
4748+
mr.u.regs = (uint16_t *) mg_calloc(1, sizeof(uint16_t));
4749+
if (mr.u.regs == NULL) goto modbus_oom;
4750+
mr.u.regs[0] = MG_LOAD_BE16(&buf[10]);
4751+
break;
4752+
case MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS: {
4753+
quantity = MG_LOAD_BE16(&buf[10]);
4754+
if (quantity == 0 || quantity > 0x07B0) goto modbus_illegal_value;
4755+
if (pdu_len < 6) goto modbus_illegal_value;
4756+
byte_count = buf[12];
4757+
if (byte_count != (uint8_t) ((quantity + 7) / 8)) goto modbus_illegal_value;
4758+
len_check = (size_t) (6 + byte_count);
4759+
if (len_check != pdu_len) goto modbus_illegal_value;
4760+
mr.len = quantity;
4761+
mr.u.bits = (bool *) mg_calloc((size_t) mr.len, sizeof(bool));
4762+
if (mr.u.bits == NULL) goto modbus_oom;
4763+
for (i = 0; i < mr.len; i++) {
4764+
packed = buf[13 + (i / 8)];
4765+
mr.u.bits[i] = ((packed >> (i % 8)) & 1) != 0;
4766+
}
4767+
break;
4768+
}
4769+
case MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS: {
4770+
quantity = MG_LOAD_BE16(&buf[10]);
4771+
if (quantity == 0 || quantity > 0x007B) goto modbus_illegal_value;
4772+
if (pdu_len < 6) goto modbus_illegal_value;
4773+
byte_count = buf[12];
4774+
if (byte_count != (uint8_t) (quantity * 2)) goto modbus_illegal_value;
4775+
len_check = (size_t) (6 + quantity * 2);
4776+
if (len_check != pdu_len) goto modbus_illegal_value;
4777+
mr.len = quantity;
4778+
mr.u.regs = (uint16_t *) mg_calloc((size_t) mr.len, sizeof(uint16_t));
4779+
if (mr.u.regs == NULL) goto modbus_oom;
4780+
for (i = 0; i < mr.len; i++) {
4781+
mr.u.regs[i] = MG_LOAD_BE16(&buf[13 + i * 2]);
4782+
}
4783+
break;
4784+
}
4785+
default:
4786+
MG_ERROR(("Unsupported modbus function"));
4787+
mr.error = MG_MODBUS_ERR_ILLEGAL_FUNCTION;
4788+
mg_modbus_send_response(c, &mr, txid, unit_id);
4789+
return;
4790+
}
4791+
mg_call(c, MG_EV_MODBUS_REQ, &mr);
4792+
goto modbus_exit;
4793+
4794+
modbus_illegal_value:
4795+
MG_ERROR(("Invalid data"));
4796+
mr.error = MG_MODBUS_ERR_ILLEGAL_VALUE;
4797+
goto modbus_exit;
4798+
modbus_oom:
4799+
MG_ERROR(("OOM"));
4800+
mr.error = MG_MODBUS_ERR_DEVICE_FAILURE;
4801+
modbus_exit:
4802+
mg_modbus_send_response(c, &mr, txid, unit_id);
4803+
if (mr.u.bits != NULL) mg_free(mr.u.bits);
4804+
}
4805+
4806+
static void modbus_ev_handler(struct mg_connection *c, int ev, void *ev_data) {
4807+
if (ev == MG_EV_READ) {
4808+
while (c->recv.len >= MG_MODBUS_MIN_PAYLOAD_LEN) {
4809+
uint16_t len = MG_LOAD_BE16(&c->recv.buf[4]); // PDU length
4810+
uint16_t proto_id = MG_LOAD_BE16(&c->recv.buf[2]);
4811+
if (len < 6 || proto_id != 0) {
4812+
mg_error(c, "invalid pdu");
4813+
break;
4814+
} else if (c->recv.len < len + 6U) {
4815+
break; // Partial frame, buffer more
4816+
} else {
4817+
handle_pdu(c, c->recv.buf, len + 6); // Parse PDU and call user
4818+
mg_iobuf_del(&c->recv, 0, len + 6); // Delete received PDU
4819+
}
4820+
}
4821+
}
4822+
(void) ev_data;
4823+
}
4824+
4825+
struct mg_connection *mg_modbus_listen(struct mg_mgr *mgr, const char *url,
4826+
mg_event_handler_t fn, void *fn_data) {
4827+
struct mg_connection *c = mg_listen(mgr, url, fn, fn_data);
4828+
if (c != NULL) c->pfn = modbus_ev_handler;
4829+
return c;
4830+
}
4831+
45994832
#ifdef MG_ENABLE_LINES
46004833
#line 1 "src/mqtt.c"
46014834
#endif

‎mongoose.h‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1678,6 +1678,7 @@ enum {
16781678
MG_EV_WAKEUP, // mg_wakeup() data received struct mg_str *data
16791679
MG_EV_MDNS_REQ, // mDNS request struct mg_mdns_req *
16801680
MG_EV_MDNS_RESP, // mDNS response struct mg_mdns_resp *
1681+
MG_EV_MODBUS_REQ, // ModBus request struct mg_modbus_cmd *
16811682
MG_EV_USER // Starting ID for user events
16821683
};
16831684

@@ -3045,6 +3046,40 @@ bool mg_mdns_query(struct mg_connection *, const char *, unsigned int);
30453046

30463047

30473048

3049+
// Functions
3050+
#define MG_MODBUS_FUNC_READ_COILS 1
3051+
#define MG_MODBUS_FUNC_READ_DISCRETE_INPUTS 2
3052+
#define MG_MODBUS_FUNC_READ_HOLDING_REGISTERS 3
3053+
#define MG_MODBUS_FUNC_READ_INPUT_REGISTERS 4
3054+
#define MG_MODBUS_FUNC_WRITE_SINGLE_COIL 5
3055+
#define MG_MODBUS_FUNC_WRITE_SINGLE_REGISTER 6
3056+
#define MG_MODBUS_FUNC_WRITE_MULTIPLE_COILS 15
3057+
#define MG_MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS 16
3058+
3059+
// Error codes
3060+
#define MG_MODBUS_ERR_NONE 0
3061+
#define MG_MODBUS_ERR_ILLEGAL_FUNCTION 1
3062+
#define MG_MODBUS_ERR_ILLEGAL_ADDRESS 2
3063+
#define MG_MODBUS_ERR_ILLEGAL_VALUE 3
3064+
#define MG_MODBUS_ERR_DEVICE_FAILURE 4
3065+
3066+
struct mg_modbus_req {
3067+
uint8_t func; // Function code, one of MG_MODBUS_FUNC_*
3068+
uint8_t error; // Error code user handler can set
3069+
uint16_t addr; // Address
3070+
union {
3071+
bool *bits;
3072+
uint16_t *regs;
3073+
} u;
3074+
uint16_t len; // Number of registers or bits
3075+
};
3076+
3077+
struct mg_connection *mg_modbus_listen(struct mg_mgr *mgr, const char *url,
3078+
mg_event_handler_t fn, void *fn_data);
3079+
3080+
3081+
3082+
30483083

30493084
#ifndef MG_JSON_MAX_DEPTH
30503085
#define MG_JSON_MAX_DEPTH 30

‎src/event.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,5 +29,6 @@ enum {
2929
MG_EV_WAKEUP, // mg_wakeup() data received struct mg_str *data
3030
MG_EV_MDNS_REQ, // mDNS request struct mg_mdns_req *
3131
MG_EV_MDNS_RESP, // mDNS response struct mg_mdns_resp *
32+
MG_EV_MODBUS_REQ, // ModBus request struct mg_modbus_cmd *
3233
MG_EV_USER // Starting ID for user events
3334
};

0 commit comments

Comments
 (0)