Stuff
This commit is contained in:
Valerio De Benedetto
parent
95b11722da
commit
2241a563f6
596
modbusino.c
596
modbusino.c
@ -1,9 +1,9 @@
|
||||
#include "modbusino.h"
|
||||
#include "modbusino_platform.h"
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
|
||||
#if !defined(MBSN_BIG_ENDIAN) && !defined(MBSN_LITTLE_ENDIAN)
|
||||
#if defined(__BYTE_ORDER) && __BYTE_ORDER == __BIG_ENDIAN || defined(__BIG_ENDIAN__) || defined(__ARMEB__) || \
|
||||
defined(__THUMBEB__) || defined(__AARCH64EB__) || defined(_MIBSEB) || defined(__MIBSEB) || defined(__MIBSEB__)
|
||||
@ -18,49 +18,63 @@
|
||||
#endif
|
||||
|
||||
#ifdef MBSN_BIG_ENDIAN
|
||||
#define HTONS(x) void(0)
|
||||
#define NTOHS(x) void(0)
|
||||
#define HTONS(x) (x)
|
||||
#define NTOHS(x) (x)
|
||||
#else
|
||||
#define HTONS(x) (((x) >> 8) | ((x) << 8));
|
||||
#define NTOHS(x) (((x) >> 8) | ((x) << 8));
|
||||
#define HTONS(x) (((x) >> 8) | ((x) << 8))
|
||||
#define NTOHS(x) (((x) >> 8) | ((x) << 8))
|
||||
#endif
|
||||
|
||||
|
||||
#define CLOCKS_PER_MS ((uint64_t) (CLOCKS_PER_SEC / 1000));
|
||||
|
||||
|
||||
typedef struct req_state {
|
||||
uint8_t client_id;
|
||||
uint8_t fc;
|
||||
uint16_t transaction_id;
|
||||
bool broadcast;
|
||||
bool ignored;
|
||||
uint16_t crc;
|
||||
} req_state;
|
||||
|
||||
|
||||
uint64_t clock_ms() {
|
||||
return (uint64_t) clock() / CLOCKS_PER_MS;
|
||||
}
|
||||
|
||||
int mbsn_create(mbsn_t* mbsn, mbsn_transport transport) {
|
||||
|
||||
int mbsn_create(mbsn_t* mbsn, mbsn_transport_conf transport_conf) {
|
||||
memset(mbsn, 0, sizeof(mbsn_t));
|
||||
|
||||
mbsn->byte_timeout_ms = -1;
|
||||
mbsn->response_timeout_ms = -1;
|
||||
|
||||
mbsn->transport = transport;
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
mbsn->transport_read_byte = mbsn_rtu_read_byte;
|
||||
mbsn->transport_write_byte = mbsn_rtu_write_byte;
|
||||
}
|
||||
else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
|
||||
mbsn->transport_read_byte = mbsn_tcp_read_byte;
|
||||
mbsn->transport_write_byte = mbsn_tcp_write_byte;
|
||||
}
|
||||
else {
|
||||
mbsn->transport = transport_conf.transport;
|
||||
if (mbsn->transport != MBSN_TRANSPORT_RTU && mbsn->transport != MBSN_TRANSPORT_TCP)
|
||||
return MBSN_ERROR_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
if (!transport_conf.read_byte || !transport_conf.write_byte)
|
||||
return MBSN_ERROR_INVALID_ARGUMENT;
|
||||
|
||||
mbsn->transport_read_byte = transport_conf.read_byte;
|
||||
mbsn->transport_write_byte = transport_conf.write_byte;
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error mbsn_client_create(mbsn_t* mbsn, mbsn_transport transport) {
|
||||
return mbsn_create(mbsn, transport);
|
||||
mbsn_error mbsn_client_create(mbsn_t* mbsn, mbsn_transport_conf transport_conf) {
|
||||
return mbsn_create(mbsn, transport_conf);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error mbsn_server_create(mbsn_t* mbsn, mbsn_transport transport, uint8_t address, mbsn_callbacks callbacks) {
|
||||
mbsn_error ret = mbsn_create(mbsn, transport);
|
||||
mbsn_error mbsn_server_create(mbsn_t* mbsn, uint8_t address, mbsn_transport_conf transport_conf,
|
||||
mbsn_callbacks callbacks) {
|
||||
if (address == 0)
|
||||
return MBSN_ERROR_INVALID_ARGUMENT;
|
||||
|
||||
mbsn_error ret = mbsn_create(mbsn, transport_conf);
|
||||
mbsn->address_rtu = address;
|
||||
mbsn->callbacks = callbacks;
|
||||
return ret;
|
||||
@ -82,6 +96,33 @@ void mbsn_client_set_server_address_rtu(mbsn_t* mbsn, uint8_t address) {
|
||||
}
|
||||
|
||||
|
||||
uint16_t crc_add_n(uint16_t crc, const uint8_t* data, unsigned int length) {
|
||||
for (int i = 0; i < length; i++) {
|
||||
crc ^= (uint16_t) data[i];
|
||||
for (int j = 8; j != 0; j--) {
|
||||
if ((crc & 0x0001) != 0) {
|
||||
crc >>= 1;
|
||||
crc ^= 0xA001;
|
||||
}
|
||||
else
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
uint16_t crc_add_1(uint16_t crc, const uint8_t b) {
|
||||
return crc_add_n(crc, &b, 1);
|
||||
}
|
||||
|
||||
|
||||
uint16_t crc_add_2(uint16_t crc, const uint8_t w) {
|
||||
return crc_add_n(crc, &w, 2);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
mbsn_error read(uint8_t* buf, uint64_t len, uint64_t byte_timeout_ms, mbsn_error (*transport_read_byte)(uint8_t*)) {
|
||||
uint64_t now = clock_ms();
|
||||
@ -104,7 +145,8 @@ mbsn_error read(uint8_t* buf, uint64_t len, uint64_t byte_timeout_ms, mbsn_error
|
||||
}
|
||||
*/
|
||||
|
||||
mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count) {
|
||||
|
||||
mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* crc) {
|
||||
//uint64_t start = clock_ms();
|
||||
int r = 0;
|
||||
while (r != count) {
|
||||
@ -120,6 +162,7 @@ mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count) {
|
||||
else if (ret != 1)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
*crc = crc_add_1(*crc, buf[r]);
|
||||
r++;
|
||||
//start = clock_ms();
|
||||
}
|
||||
@ -128,13 +171,13 @@ mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count) {
|
||||
}
|
||||
|
||||
|
||||
mbsn_error recv_1(mbsn_t* mbsn, uint8_t* b) {
|
||||
return recv_n(mbsn, b, 1);
|
||||
mbsn_error recv_1(mbsn_t* mbsn, uint8_t* b, uint16_t* crc) {
|
||||
return recv_n(mbsn, b, 1, crc);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error recv_2(mbsn_t* mbsn, uint16_t* w) {
|
||||
mbsn_error err = recv_n(mbsn, (uint8_t*) w, 2);
|
||||
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;
|
||||
|
||||
@ -188,7 +231,7 @@ mbsn_error send(mbsn_t* mbsn) {
|
||||
*/
|
||||
|
||||
|
||||
mbsn_error send_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count) {
|
||||
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]);
|
||||
@ -200,63 +243,143 @@ mbsn_error send_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count) {
|
||||
w++;
|
||||
}
|
||||
|
||||
if (crc)
|
||||
*crc = crc_add_n(*crc, buf, count);
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error send_1(mbsn_t* mbsn, uint8_t b) {
|
||||
return send_n(mbsn, &b, 1);
|
||||
mbsn_error send_1(mbsn_t* mbsn, uint8_t b, uint16_t* crc) {
|
||||
return send_n(mbsn, &b, 1, crc);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error send_2(mbsn_t* mbsn, uint16_t w) {
|
||||
mbsn_error send_2(mbsn_t* mbsn, uint16_t w, uint16_t* crc) {
|
||||
w = HTONS(w);
|
||||
return send_n(mbsn, (uint8_t*) &w, 2);
|
||||
return send_n(mbsn, (uint8_t*) &w, 2, crc);
|
||||
}
|
||||
|
||||
|
||||
void handle_exception(mbsn_t* mbsn, uint16_t fc, uint8_t exception) {
|
||||
send_1(mbsn, fc + 0x80);
|
||||
send_1(mbsn, exception);
|
||||
mbsn_error send_mbap(mbsn_t* mbsn, req_state* s, uint16_t data_length) {
|
||||
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->client_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_discrete(mbsn_t* mbsn, uint16_t fc, bool ignored, bool broadcast,
|
||||
mbsn_error (*callback)(uint16_t, uint16_t, mbsn_bitfield)) {
|
||||
mbsn_error send_req_header(mbsn_t* mbsn, req_state* s, uint8_t data_length, uint16_t* crc) {
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
mbsn_error err = send_1(mbsn, s->client_id, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, s->fc, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
|
||||
return send_mbap(mbsn, s, data_length);
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error send_req_footer(mbsn_t* mbsn, uint16_t crc) {
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU)
|
||||
return send_2(mbsn, crc, NULL);
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_exception(mbsn_t* mbsn, req_state* s, uint8_t exception) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
mbsn_error err = send_req_header(mbsn, s, 1, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, s->fc + 0x80, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, exception, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
return send_req_footer(mbsn, crc);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_discrete(mbsn_t* mbsn, req_state* s, mbsn_error (*callback)(uint16_t, uint16_t, mbsn_bitfield)) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr);
|
||||
mbsn_error err = recv_2(mbsn, &addr, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity);
|
||||
err = recv_2(mbsn, &quantity, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t recv_crc;
|
||||
err = recv_2(mbsn, &recv_crc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
if (quantity < 1 || quantity > 2000)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
if ((uint32_t) addr + (uint32_t) quantity > 65535)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_ADDRESS;
|
||||
|
||||
if (!ignored) {
|
||||
if (!s->ignored) {
|
||||
if (callback) {
|
||||
mbsn_bitfield bf;
|
||||
err = callback(addr, quantity, bf);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!broadcast) {
|
||||
err = send_1(mbsn, fc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (err < 0)
|
||||
return MBSN_EXCEPTION_SERVER_DEVICE_FAILURE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
if (!s->broadcast) {
|
||||
uint8_t discrete_bytes = (quantity / 8) + 1;
|
||||
err = send_1(mbsn, discrete_bytes);
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
err = send_req_header(mbsn, s, discrete_bytes, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_n(mbsn, bf, discrete_bytes);
|
||||
err = send_1(mbsn, discrete_bytes, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_n(mbsn, bf, discrete_bytes, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_req_footer(mbsn, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
@ -265,44 +388,68 @@ mbsn_error handle_read_discrete(mbsn_t* mbsn, uint16_t fc, bool ignored, bool br
|
||||
return MBSN_EXCEPTION_ILLEGAL_FUNCTION;
|
||||
}
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_registers(mbsn_t* mbsn, uint16_t fc, bool ignored, bool broadcast,
|
||||
mbsn_error (*callback)(uint16_t, uint16_t, uint16_t*)) {
|
||||
mbsn_error handle_read_registers(mbsn_t* mbsn, req_state* s, mbsn_error (*callback)(uint16_t, uint16_t, uint16_t*)) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr);
|
||||
mbsn_error err = recv_2(mbsn, &addr, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity);
|
||||
err = recv_2(mbsn, &quantity, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t recv_crc;
|
||||
err = recv_2(mbsn, &recv_crc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
if (quantity < 1 || quantity > 125)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
if ((uint32_t) addr + (uint32_t) quantity > 65535)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_ADDRESS;
|
||||
|
||||
if (!ignored) {
|
||||
if (!s->ignored) {
|
||||
if (callback) {
|
||||
uint16_t regs[125];
|
||||
|
||||
err = callback(addr, quantity, regs);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (err < 0)
|
||||
return MBSN_EXCEPTION_SERVER_DEVICE_FAILURE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
for (int i = 0; i < quantity; i++)
|
||||
regs[i] = HTONS(regs[i]);
|
||||
|
||||
if (!broadcast) {
|
||||
send_1(mbsn, fc);
|
||||
|
||||
if (!s->broadcast) {
|
||||
uint8_t regs_bytes = quantity * 2;
|
||||
send_1(mbsn, regs_bytes);
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
err = send_n(mbsn, (uint8_t*) regs, regs_bytes);
|
||||
err = send_req_header(mbsn, s, regs_bytes, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, regs_bytes, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_n(mbsn, (uint8_t*) regs, regs_bytes, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_req_footer(mbsn, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
@ -316,121 +463,166 @@ mbsn_error handle_read_registers(mbsn_t* mbsn, uint16_t fc, bool ignored, bool b
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_coils(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
return handle_read_discrete(mbsn, 1, ignored, broadcast, mbsn->callbacks.read_coils);
|
||||
mbsn_error handle_read_coils(mbsn_t* mbsn, req_state* s) {
|
||||
return handle_read_discrete(mbsn, s, mbsn->callbacks.read_coils);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_discrete_inputs(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
return handle_read_discrete(mbsn, 2, ignored, broadcast, mbsn->callbacks.read_discrete_inputs);
|
||||
mbsn_error handle_read_discrete_inputs(mbsn_t* mbsn, req_state* s) {
|
||||
return handle_read_discrete(mbsn, s, mbsn->callbacks.read_discrete_inputs);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_holding_registers(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
return handle_read_registers(mbsn, 3, ignored, broadcast, mbsn->callbacks.read_holding_registers);
|
||||
mbsn_error handle_read_holding_registers(mbsn_t* mbsn, req_state* s) {
|
||||
return handle_read_registers(mbsn, s, mbsn->callbacks.read_holding_registers);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_read_input_registers(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
return handle_read_registers(mbsn, 4, ignored, broadcast, mbsn->callbacks.read_input_registers);
|
||||
mbsn_error handle_read_input_registers(mbsn_t* mbsn, req_state* s) {
|
||||
return handle_read_registers(mbsn, s, mbsn->callbacks.read_input_registers);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_write_single_coil(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
mbsn_error handle_write_single_coil(mbsn_t* mbsn, req_state* s) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr);
|
||||
mbsn_error err = recv_2(mbsn, &addr, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t value;
|
||||
err = recv_2(mbsn, &value);
|
||||
err = recv_2(mbsn, &value, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t recv_crc;
|
||||
err = recv_2(mbsn, &recv_crc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
if (value != 0 && value != 0xFF00)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
if (!ignored) {
|
||||
if (!s->ignored) {
|
||||
if (mbsn->callbacks.write_single_coil) {
|
||||
err = mbsn->callbacks.write_single_coil(addr, value == 0 ? false : true);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (err < 0)
|
||||
return MBSN_EXCEPTION_SERVER_DEVICE_FAILURE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
if (!s->broadcast) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
err = send_req_header(mbsn, s, 2, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_req_footer(mbsn, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return MBSN_EXCEPTION_ILLEGAL_FUNCTION;
|
||||
}
|
||||
}
|
||||
|
||||
if (!broadcast) {
|
||||
err = send_1(mbsn, 5);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_write_single_register(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
mbsn_error handle_write_single_register(mbsn_t* mbsn, req_state* s) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr);
|
||||
mbsn_error err = recv_2(mbsn, &addr, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t value;
|
||||
err = recv_2(mbsn, &value);
|
||||
err = recv_2(mbsn, &value, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!ignored) {
|
||||
uint16_t recv_crc;
|
||||
err = recv_2(mbsn, &recv_crc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
if (!s->ignored) {
|
||||
if (mbsn->callbacks.write_single_register) {
|
||||
err = mbsn->callbacks.write_single_register(addr, value);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (err < 0)
|
||||
return MBSN_EXCEPTION_SERVER_DEVICE_FAILURE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
if (!s->broadcast) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
err = send_req_header(mbsn, s, 1, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, value, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_req_footer(mbsn, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return MBSN_EXCEPTION_ILLEGAL_FUNCTION;
|
||||
}
|
||||
}
|
||||
|
||||
if (!broadcast) {
|
||||
err = send_1(mbsn, 6);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, value);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, req_state* s) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr);
|
||||
mbsn_error err = recv_2(mbsn, &addr, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity);
|
||||
err = recv_2(mbsn, &quantity, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t coils_bytes;
|
||||
err = recv_1(mbsn, &coils_bytes);
|
||||
err = recv_1(mbsn, &coils_bytes, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
mbsn_bitfield coils;
|
||||
err = recv_n(mbsn, coils, coils_bytes, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t recv_crc;
|
||||
err = recv_2(mbsn, &recv_crc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
if (quantity < 1 || quantity > 0x07B0) // 0x07B0 == 1968
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
@ -440,56 +632,74 @@ mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, bool ignored, bool broadcas
|
||||
if ((quantity / 8) + 1 != coils_bytes)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
mbsn_bitfield coils;
|
||||
err = recv_n(mbsn, coils, coils_bytes);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!ignored) {
|
||||
if (!s->ignored) {
|
||||
if (mbsn->callbacks.write_multiple_coils) {
|
||||
err = mbsn->callbacks.write_multiple_coils(addr, quantity, coils);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (err < 0)
|
||||
return MBSN_EXCEPTION_SERVER_DEVICE_FAILURE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
if (!s->broadcast) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
err = send_req_header(mbsn, s, 4, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_req_footer(mbsn, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return MBSN_EXCEPTION_ILLEGAL_FUNCTION;
|
||||
}
|
||||
}
|
||||
|
||||
if (!broadcast) {
|
||||
err = send_1(mbsn, 15);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, bool ignored, bool broadcast) {
|
||||
mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, req_state* s) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr);
|
||||
mbsn_error err = recv_2(mbsn, &addr, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity);
|
||||
err = recv_2(mbsn, &quantity, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t registers_bytes;
|
||||
err = recv_1(mbsn, ®isters_bytes);
|
||||
err = recv_1(mbsn, ®isters_bytes, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t registers[0x007B];
|
||||
err = recv_n(mbsn, (uint8_t*) registers, registers_bytes, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t recv_crc;
|
||||
err = recv_2(mbsn, &recv_crc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
if (quantity < 1 || quantity > 0x007B) // 0x007B == 123
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
@ -499,103 +709,134 @@ mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, bool ignored, bool broa
|
||||
if (registers_bytes != quantity * 2)
|
||||
return MBSN_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
|
||||
uint16_t registers[0x007B];
|
||||
err = recv_n(mbsn, (uint8_t*) registers, registers_bytes);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
for (int i = 0; i < quantity; i++)
|
||||
registers[i] = NTOHS(registers[i]);
|
||||
|
||||
if (!ignored) {
|
||||
if (!s->ignored) {
|
||||
if (mbsn->callbacks.write_multiple_registers) {
|
||||
err = mbsn->callbacks.write_multiple_registers(addr, quantity, registers);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (err < 0)
|
||||
return MBSN_EXCEPTION_SERVER_DEVICE_FAILURE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
if (!s->broadcast) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
err = send_req_header(mbsn, s, 4, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity, &crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_req_footer(mbsn, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return MBSN_EXCEPTION_ILLEGAL_FUNCTION;
|
||||
}
|
||||
}
|
||||
|
||||
if (!broadcast) {
|
||||
err = send_1(mbsn, 16);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
int mbsn_server_receive(mbsn_t* mbsn) {
|
||||
mbsn_error err = MBSN_ERROR_NONE;
|
||||
uint8_t fc = 0;
|
||||
bool broadcast = false;
|
||||
bool ignored = false;
|
||||
req_state s = {0};
|
||||
s.crc = 0xFFFF;
|
||||
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
uint8_t id;
|
||||
err = recv_1(mbsn, &id);
|
||||
mbsn_error err = recv_1(mbsn, &id, &s.crc);
|
||||
if (err != 0)
|
||||
return err;
|
||||
|
||||
// Check if request is for us
|
||||
if (id == 0)
|
||||
broadcast = true;
|
||||
s.broadcast = true;
|
||||
else if (id != mbsn->address_rtu)
|
||||
ignored = true;
|
||||
s.ignored = true;
|
||||
else
|
||||
ignored = false;
|
||||
s.ignored = false;
|
||||
|
||||
err = recv_1(mbsn, &fc);
|
||||
err = recv_1(mbsn, &s.fc, &s.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
|
||||
err = recv_1(mbsn, &fc);
|
||||
mbsn_error err = recv_2(mbsn, &s.transaction_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
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 (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv_1(mbsn, &s.client_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv_1(mbsn, &s.fc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
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)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
}
|
||||
|
||||
switch (fc) {
|
||||
mbsn_error err;
|
||||
|
||||
switch (s.fc) {
|
||||
case 1:
|
||||
err = handle_read_coils(mbsn, ignored, broadcast);
|
||||
err = handle_read_coils(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
err = handle_read_discrete_inputs(mbsn, ignored, broadcast);
|
||||
err = handle_read_discrete_inputs(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
err = handle_read_holding_registers(mbsn, ignored, broadcast);
|
||||
err = handle_read_holding_registers(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 4:
|
||||
err = handle_read_input_registers(mbsn, ignored, broadcast);
|
||||
err = handle_read_input_registers(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 5:
|
||||
err = handle_write_single_coil(mbsn, ignored, broadcast);
|
||||
err = handle_write_single_coil(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 6:
|
||||
err = handle_write_single_register(mbsn, ignored, broadcast);
|
||||
err = handle_write_single_register(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 15:
|
||||
err = handle_write_multiple_coils(mbsn, ignored, broadcast);
|
||||
err = handle_write_multiple_coils(mbsn, &s);
|
||||
break;
|
||||
|
||||
case 16:
|
||||
err = handle_write_multiple_registers(mbsn, ignored, broadcast);
|
||||
err = handle_write_multiple_registers(mbsn, &s);
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -603,8 +844,11 @@ int mbsn_server_receive(mbsn_t* mbsn) {
|
||||
}
|
||||
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (!broadcast && !ignored && mbsn_error_is_exception(err))
|
||||
handle_exception(mbsn, fc, err);
|
||||
if (!s.broadcast && !s.ignored && mbsn_error_is_exception(err)) {
|
||||
err = handle_exception(mbsn, &s, err);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
}
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
13
modbusino.h
13
modbusino.h
@ -35,6 +35,12 @@ typedef enum mbsn_transport {
|
||||
MBSN_TRANSPORT_TCP = 2,
|
||||
} mbsn_transport;
|
||||
|
||||
typedef struct mbsn_transport_conf {
|
||||
mbsn_transport transport;
|
||||
int (*read_byte)(uint8_t* b);
|
||||
int (*write_byte)(uint8_t b);
|
||||
} mbsn_transport_conf;
|
||||
|
||||
|
||||
typedef struct {
|
||||
char* vendor_name;
|
||||
@ -48,7 +54,7 @@ typedef struct {
|
||||
} mbsn_device_identification_data_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
typedef struct mbsn_callbacks {
|
||||
mbsn_error (*read_coils)(uint16_t address, uint16_t quantity, mbsn_bitfield coils_out);
|
||||
mbsn_error (*read_discrete_inputs)(uint16_t address, uint16_t quantity, mbsn_bitfield inputs_out);
|
||||
mbsn_error (*read_holding_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out);
|
||||
@ -76,9 +82,10 @@ typedef struct mbsn_t {
|
||||
} mbsn_t;
|
||||
|
||||
|
||||
mbsn_error mbsn_client_create(mbsn_t* mbsn, mbsn_transport transport);
|
||||
mbsn_error mbsn_client_create(mbsn_t* mbsn, mbsn_transport_conf transport_conf);
|
||||
|
||||
mbsn_error mbsn_server_create(mbsn_t* mbsn, mbsn_transport transport, uint8_t address, mbsn_callbacks callbacks);
|
||||
mbsn_error mbsn_server_create(mbsn_t* mbsn, uint8_t address, mbsn_transport_conf transport_conf,
|
||||
mbsn_callbacks callbacks);
|
||||
|
||||
void mbsn_set_response_timeout(mbsn_t* mbsn, int64_t timeout_ms);
|
||||
|
||||
|
||||
@ -3,9 +3,16 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
mbsn_error read_byte(uint8_t* b) {}
|
||||
|
||||
mbsn_error write_byte(uint8_t b) {}
|
||||
|
||||
int main() {
|
||||
mbsn_t mbsn = {0};
|
||||
mbsn_error err = mbsn_client_create(&mbsn, MBSN_TRANSPORT_TCP);
|
||||
mbsn_error err = mbsn_server_create(
|
||||
&mbsn,
|
||||
(mbsn_transport_conf){.transport = MBSN_TRANSPORT_TCP, .read_byte = read_byte, .write_byte = write_byte});
|
||||
|
||||
assert(err == MBSN_ERROR_NONE);
|
||||
|
||||
mbsn_bitfield bf;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user