Skip to content

Instantly share code, notes, and snippets.

@wsxq2

wsxq2/modbus.c

Last active February 23, 2025 02:57
Show Gist options
  • Save wsxq2/538ef7b851e279fc575019bf16b56478 to your computer and use it in GitHub Desktop.
Save wsxq2/538ef7b851e279fc575019bf16b56478 to your computer and use it in GitHub Desktop.
Download ZIP
modbus rtu master driver for stm32. 依赖 queue.c 和 common.c，两种方式计算CRC16，一个时间复杂度低但空间复杂度高，另一个相反。使用 queue 时，利用指针强转实现泛型。大端实现目前用的是编译器宏，可能需要考虑更好的方式。响应报文的解析尚未完成，调用者如何使用也是个问题，可考虑再添加个状态机。用软件模拟slave，进行联合测试尚未完成，该测试需要使用socat工具创建两个回环的虚拟串口，一个串口和slave软件连接，另一个和该程序连接，slave软件尝试过modbusdriver的diagslave，但没有调试信息，不知道收到的报文和发出的报文，后续选择使用pymodbus中的simulator，它可以通过json文件配置其寄存…
Raw
modbus.c
#include "modbus.h"
#include <string.h>
#include "common.h"
#include <stdio.h>
#ifndef __weak
#define __weak __attribute__((weak))
#endif /* __weak */
#define ADU_FIXED_LEN (1+2) //slave_addr + crc
#define PDU_MIN_LEN (1+2+2) //func + start_address + count
#define ADU_MIN_LEN (ADU_FIXED_LEN + PDU_MIN_LEN)
#pragma pack(push,1)
typedef struct {
uint8_t func;
uint16_t start_address;
uint16_t count;
}mb_pdu_3_field; //used for all read and write single reg request
#pragma pack(pop)
static bool mb_check_data(const mb_data_unit * data)
{
if(data->type <= MB_REG_TYPE_BEGIN || data->type >= MB_REG_TYPE_END) return false;
if(data->value_count > MB_DU_VALUE_MAX || data->value_count < 1) return false;
return true;
}
bool mb_init(mb_handle *h, const mb_callback * callback)
{
memset(h, 0, sizeof(mb_handle));
h->state = MB_STATE_IDLE;
bool ok = q_create(&h->queue, Q_MSG_MAX_SIZE);
if(!ok) return false;
h->rx_done = false;
h->callback = *callback;
return true;
}
static bool mb_enqueue_task(q_queue * q, const mb_rw_task *task)
{
if(sizeof(mb_rw_task) > Q_MSG_BUF_LEN) return false;
q_message msg;
msg.len = sizeof(mb_rw_task);
mb_rw_task *pt=(mb_rw_task*)&msg.buf;
*pt = *task;
return q_enqueue(q, &msg);
}
static bool mb_dequeue_task(q_queue *q, mb_rw_task *task)
{
q_message msg;
bool ok = q_dequeue(q, &msg);
if(!ok) return false;
if(msg.len != sizeof(mb_rw_task)) return false;
mb_rw_task *pt=(mb_rw_task*)&msg.buf;
*task = *pt;
return true;
}
bool mb_write(mb_handle *h, uint8_t slave_addr, const mb_data_unit * data)
{
if(!mb_check_data(data)) {
return false;
}
mb_rw_task t;
t.slave_addr = slave_addr;
t.is_write = true;
t.data = *data;
mb_enqueue_task(&h->queue, &t);
return true;
}
bool mb_read(mb_handle *h, uint8_t slave_addr, const mb_data_unit * data)
{
if(!mb_check_data(data)) {
return false;
}
mb_rw_task t;
t.slave_addr = slave_addr;
t.is_write = false;
t.data = *data;
mb_enqueue_task(&h->queue, &t);
return true;
}
#if 1
static uint16_t mb_crc16_modbus_rtu(const uint8_t *buf, uint16_t buflen)
{
static const uint16_t crc_table[] = {
0X0000, 0XC0C1, 0XC181, 0X0140, 0XC301, 0X03C0, 0X0280, 0XC241,
0XC601, 0X06C0, 0X0780, 0XC741, 0X0500, 0XC5C1, 0XC481, 0X0440,
0XCC01, 0X0CC0, 0X0D80, 0XCD41, 0X0F00, 0XCFC1, 0XCE81, 0X0E40,
0X0A00, 0XCAC1, 0XCB81, 0X0B40, 0XC901, 0X09C0, 0X0880, 0XC841,
0XD801, 0X18C0, 0X1980, 0XD941, 0X1B00, 0XDBC1, 0XDA81, 0X1A40,
0X1E00, 0XDEC1, 0XDF81, 0X1F40, 0XDD01, 0X1DC0, 0X1C80, 0XDC41,
0X1400, 0XD4C1, 0XD581, 0X1540, 0XD701, 0X17C0, 0X1680, 0XD641,
0XD201, 0X12C0, 0X1380, 0XD341, 0X1100, 0XD1C1, 0XD081, 0X1040,
0XF001, 0X30C0, 0X3180, 0XF141, 0X3300, 0XF3C1, 0XF281, 0X3240,
0X3600, 0XF6C1, 0XF781, 0X3740, 0XF501, 0X35C0, 0X3480, 0XF441,
0X3C00, 0XFCC1, 0XFD81, 0X3D40, 0XFF01, 0X3FC0, 0X3E80, 0XFE41,
0XFA01, 0X3AC0, 0X3B80, 0XFB41, 0X3900, 0XF9C1, 0XF881, 0X3840,
0X2800, 0XE8C1, 0XE981, 0X2940, 0XEB01, 0X2BC0, 0X2A80, 0XEA41,
0XEE01, 0X2EC0, 0X2F80, 0XEF41, 0X2D00, 0XEDC1, 0XEC81, 0X2C40,
0XE401, 0X24C0, 0X2580, 0XE541, 0X2700, 0XE7C1, 0XE681, 0X2640,
0X2200, 0XE2C1, 0XE381, 0X2340, 0XE101, 0X21C0, 0X2080, 0XE041,
0XA001, 0X60C0, 0X6180, 0XA141, 0X6300, 0XA3C1, 0XA281, 0X6240,
0X6600, 0XA6C1, 0XA781, 0X6740, 0XA501, 0X65C0, 0X6480, 0XA441,
0X6C00, 0XACC1, 0XAD81, 0X6D40, 0XAF01, 0X6FC0, 0X6E80, 0XAE41,
0XAA01, 0X6AC0, 0X6B80, 0XAB41, 0X6900, 0XA9C1, 0XA881, 0X6840,
0X7800, 0XB8C1, 0XB981, 0X7940, 0XBB01, 0X7BC0, 0X7A80, 0XBA41,
0XBE01, 0X7EC0, 0X7F80, 0XBF41, 0X7D00, 0XBDC1, 0XBC81, 0X7C40,
0XB401, 0X74C0, 0X7580, 0XB541, 0X7700, 0XB7C1, 0XB681, 0X7640,
0X7200, 0XB2C1, 0XB381, 0X7340, 0XB101, 0X71C0, 0X7080, 0XB041,
0X5000, 0X90C1, 0X9181, 0X5140, 0X9301, 0X53C0, 0X5280, 0X9241,
0X9601, 0X56C0, 0X5780, 0X9741, 0X5500, 0X95C1, 0X9481, 0X5440,
0X9C01, 0X5CC0, 0X5D80, 0X9D41, 0X5F00, 0X9FC1, 0X9E81, 0X5E40,
0X5A00, 0X9AC1, 0X9B81, 0X5B40, 0X9901, 0X59C0, 0X5880, 0X9841,
0X8801, 0X48C0, 0X4980, 0X8941, 0X4B00, 0X8BC1, 0X8A81, 0X4A40,
0X4E00, 0X8EC1, 0X8F81, 0X4F40, 0X8D01, 0X4DC0, 0X4C80, 0X8C41,
0X4400, 0X84C1, 0X8581, 0X4540, 0X8701, 0X47C0, 0X4680, 0X8641,
0X8201, 0X42C0, 0X4380, 0X8341, 0X4100, 0X81C1, 0X8081, 0X4040 };
uint8_t tmp;
uint16_t crc = 0xFFFF;
while (buflen--)
{
tmp = *buf++ ^ crc;
crc >>= 8;
crc ^= crc_table[tmp];
}
return crc;
}
#else
static uint16_t mb_crc16_modbus_rtu(const uint8_t *buf, uint16_t buflen)
{
uint16_t crc = 0xFFFF;
for (int pos = 0; pos < buflen; pos++)
{
crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc
for (int i = 8; i != 0; i--) { // Loop over each bit
if ((crc & 0x0001) != 0) { // If the LSB is set
crc >>= 1; // Shift right and XOR 0xA001
crc ^= 0xA001;
}
else // Else LSB is not set
crc >>= 1; // Just shift right
}
}
// Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
return crc;
}
#endif
static bool mb_frame_is_valid(const uint8_t *buf, uint16_t buflen)
{
uint16_t crc = mb_crc16_modbus_rtu(buf, buflen -2);
uint16_t actual_crc = buf[buflen - 2] | (buf[buflen -1]<<8);
return crc == actual_crc;
}
static bool mb_pack_frame(const mb_rw_task * t, uint8_t * buf, uint16_t bufsize, uint16_t * buflen)
{
if(bufsize < ADU_MIN_LEN) return false;
if(!mb_check_data(&t->data)) return false;
buf[0]=t->slave_addr;
const mb_data_unit *pd = &t->data;
uint16_t buflen_ = 0;
mb_pdu_3_field * pdu = (mb_pdu_3_field *) &buf[1];
pdu->start_address = pd->start_address;
if(!t->is_write) {
buflen_ = sizeof(mb_pdu_3_field) + ADU_FIXED_LEN;
if(bufsize < buflen_ ) return false;
switch (pd->type) {
case MB_REG_TYPE_COILS:
pdu->func = MB_FUNC_READ_COILS;
break;
case MB_REG_TYPE_DISCRETE_INPUTS:
pdu->func = MB_FUNC_READ_DISCRETE_INPUTS;
break;
case MB_REG_TYPE_INPUT_REGISTERS:
pdu->func = MB_FUNC_READ_INPUT_REGS;
break;
case MB_REG_TYPE_HOLDING_REGISTERS:
pdu->func = MB_FUNC_READ_HOLDING_REGS;
break;
default:
return false;
break;
}
pdu->count = pd->value_count;
} else {
if(pd->value_count == 1) {
buflen_ = sizeof(mb_pdu_3_field) + ADU_FIXED_LEN;
if(bufsize < buflen_) return false;
switch (pd->type) {
case MB_REG_TYPE_COILS:
pdu->func = MB_FUNC_WRITE_SINGLE_COIL;
break;
case MB_REG_TYPE_HOLDING_REGISTERS:
pdu->func = MB_FUNC_WRITE_SINGLE_REG;
break;
default:
return false;
break;
}
pdu->count = pd->values[0];
} else {
uint16_t bytes = 0;
if(pd->type == MB_REG_TYPE_HOLDING_REGISTERS) {
bytes = pd->value_count * 2;
pdu->func = MB_FUNC_WRITE_MULTI_REGS;
} else if(pd->type == MB_REG_TYPE_COILS) {
uint16_t bytes = pd->value_count / 8;
bytes = (pd->value_count % 8 == 0? bytes: bytes + 1);
pdu->func = MB_FUNC_WRITE_MULTI_COILS;
} else {
return false;
}
buflen_ = sizeof(mb_pdu_3_field) + 1 + bytes + ADU_FIXED_LEN; //pdu + adu
if(bufsize < buflen_) return false;
pdu->count = pd->value_count;
uint8_t *p = buf + 1 + sizeof(mb_pdu_3_field);
*(p++) = bytes;
memcpy(p, pd->values, bytes);
}
}
if(buflen_ == ADU_FIXED_LEN) return false;
uint16_t crc = mb_crc16_modbus_rtu(buf, buflen_ - 2);
buf[buflen_ - 2] = (crc >> 0) & 0xff;
buf[buflen_ - 1] = (crc >> 8) & 0xff;
*buflen = buflen_;
return true;
}
void mb_set_rx_done(mb_handle *h, uint16_t rxlen)
{
h->rx_done = true;
h->rxbuff_len = rxlen;
}
void mb_fsm_process(mb_handle *h)
{
static mb_rw_task t;
switch (h->state) {
case MB_STATE_IDLE:
if(!q_empty(&h->queue)) {
bool ok = mb_dequeue_task(&h->queue, &t);
if(!ok) break;
h->state = MB_STATE_RW_ING;
}
break;
case MB_STATE_RW_ING:
{
uint8_t buf[MB_FRAME_MAX_LEN];
uint16_t buflen;
bool ok = mb_pack_frame(&t, buf, MB_FRAME_MAX_LEN, &buflen);
if(!ok) {
h->state = MB_STATE_IDLE;
break;
}
h->rx_done = false;
ok = h->callback.start_rx(h->rxbuff, MB_FRAME_MAX_LEN);
if(!ok) {
h->state = MB_STATE_IDLE;
break;
}
ok = h->callback.tx(buf, buflen);
if(!ok) {
h->state = MB_STATE_IDLE;
break;
}
h->state = MB_STATE_RW_WAITING;
break;
}
case MB_STATE_RW_WAITING:
{
if(h->rx_done) {
cm_print_buf("rx done: ", h->rxbuff, h->rxbuff_len);
if(!mb_frame_is_valid(h->rxbuff, h->rxbuff_len)) {
printf("frame is not valid");
}
h->state = MB_STATE_IDLE;
}
break;
}
default:
break;
}
}
#define MB_UNIT_TEST
#ifdef MB_UNIT_TEST
//#define SIMULATOR_TEST
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
#include <assert.h>
int g_serial_port;
bool g_start_rx = false;
uint8_t * g_p_rxbuf;
uint16_t g_rx_bufsize;
bool mb_callback_tx(const uint8_t *buf, uint16_t buflen)
{
cm_print_buf("tx: ", buf, buflen);
#ifdef SIMULATOR_TEST
ssize_t bytes_written = write(g_serial_port, buf, buflen);
if (bytes_written < 0) {
perror("Error from write");
return false;
} else {
printf("written success!\n");
return true;
}
#endif
return true;
}
bool mb_callback_start_rx(uint8_t *buf, uint16_t bufsize)
{
if(g_start_rx) return false;
g_start_rx = true;
g_p_rxbuf = buf;
g_rx_bufsize = bufsize;
return true;
}
int main(int argc, char *argv[])
{
(void) argc;
(void) argv;
// test mb_crc16_modbus_rtu
assert(mb_crc16_modbus_rtu((uint8_t[]){0x01,0x10,0x00,0x01,0x00,0x02,0x04,0x12,0x34,0x56,0x78}, 11) == 0x5749);
assert(mb_crc16_modbus_rtu((uint8_t[]){0x1,0x10,0x0,0x1,0x0,0x2}, 6) == 0x0810);
// test mb_enqueue_task and mb_dequeue_task
q_queue q={0};
mb_rw_task t={0},t2={0};
t.is_write =true;
t.slave_addr = 1;
t.data.start_address = 1;
t.data.value_count = 2;
t.data.type = MB_REG_TYPE_HOLDING_REGISTERS;
t.data.values[0] = 0x1234;
t.data.values[1] = 0x5678;
q_create(&q, Q_MSG_MAX_SIZE);
assert(mb_enqueue_task(&q, &t) == true);
assert(mb_dequeue_task(&q, &t2) == true);
assert(memcmp(&t, &t2, sizeof(mb_rw_task)) == 0);
// test mb_pack_frame
uint8_t buf[MB_FRAME_MAX_LEN];
uint16_t buflen=0;
mb_pack_frame(&t, buf, MB_FRAME_MAX_LEN, &buflen);
assert(buflen == 13);
assert(memcmp(buf, (uint8_t[]){0x01,0x10,0x00,0x01,0x00,0x02,0x04,0x12,0x34,0x56,0x78, 0x49, 0x57}, buflen)==0);
// integration test
mb_handle h;
mb_callback callback;
callback.tx = mb_callback_tx;
callback.start_rx = mb_callback_start_rx;
mb_init(&h, &callback);
time_t last_time = time(NULL);
mb_data_unit data;
data.type = MB_REG_TYPE_HOLDING_REGISTERS;
data.start_address = 1;
data.value_count = 2;
data.values[0] = 0x1234;
data.values[1] = 0x5678;
mb_data_unit data_r;
data_r.type = MB_REG_TYPE_HOLDING_REGISTERS;
data_r.start_address = 1;
data_r.value_count = 2;
mb_write(&h, 1, &data);
for (int i = 0; i<16; i++) {
mb_fsm_process(&h);
}
memcpy(g_p_rxbuf, (uint8_t []){0x1,0x10,0x0,0x1,0x0,0x2,0x10,0x8}, 8);
mb_set_rx_done(&h, 8);
for (int i = 0; i<16; i++) {
mb_fsm_process(&h);
}
#ifdef SIMULATOR_TEST
const char* portname = "./ttyS0"; // Replace with your serial port
g_serial_port = open(portname, O_RDWR);
if (g_serial_port < 0) {
perror("Error opening serial port");
return 1;
}
struct termios tty;
if (tcgetattr(g_serial_port, &tty) != 0) {
perror("Error from tcgetattr");
close(g_serial_port);
return 1;
}
int baudrate = B115200;
cfsetospeed(&tty, baudrate);
cfsetispeed(&tty, baudrate);
tty.c_cflag &= ~PARENB; // Disable parity bit
tty.c_cflag &= ~CSTOPB; // Use one stop bit
tty.c_cflag &= ~CSIZE; // Clear data size bits
tty.c_cflag |= CS8; // 8 data bits
tty.c_cflag &= ~CRTSCTS; // Disable hardware flow control
tty.c_cflag |= CREAD | CLOCAL; // Enable read and ignore control lines
tty.c_lflag &= ~ICANON; // Disable canonical mode
tty.c_lflag &= ~ECHO; // Disable echo
tty.c_lflag &= ~ECHOE; // Disable erasure
tty.c_lflag &= ~ECHONL; // Disable new-line echo
tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Disable software flow control
tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL);
tty.c_oflag &= ~OPOST; // Disable output processing
tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
tty.c_cc[VMIN] = 0; // Read returns immediately if no data available
tty.c_cc[VTIME] = 0; // Wait for up to 1s (10 deciseconds), returning as soon as any data is received.
if (tcsetattr(g_serial_port, TCSANOW, &tty) != 0) {
perror("Error from tcsetattr");
close(g_serial_port);
return 1;
}
//mb_callback_tx("abcdefg", 8);
while(true) {
mb_fsm_process(&h);
if(time(NULL) - last_time > 3) {
last_time = time(NULL);
mb_write(&h, 1, &data);
//mb_read(&h, 1, &data_r);
}
if(g_start_rx) {
ssize_t bytes_read = read(g_serial_port, g_p_rxbuf, g_rx_bufsize);
if (bytes_read < 0) {
perror("Error from read");
} else if (bytes_read > 0) {
g_start_rx = false;
cm_print_buf("rx: ", g_p_rxbuf, bytes_read);
mb_set_rx_done(&h, bytes_read);
} else {
//printf("No data received within the timeout period.\n");
}
}
}
#endif
return EXIT_SUCCESS;
}
#endif
Raw
modbus.h
#ifndef MODBUS_QRLAJD_H
#define MODBUS_QRLAJD_H
#include <stdint.h>
#include <stdbool.h>
#include "queue.h"
#define MB_DU_VALUE_MAX 3
#define MB_FRAME_MAX_LEN 256
typedef enum {
MB_REG_TYPE_BEGIN = 0,
MB_REG_TYPE_DISCRETE_INPUTS,
MB_REG_TYPE_COILS,
MB_REG_TYPE_INPUT_REGISTERS,
MB_REG_TYPE_HOLDING_REGISTERS,
MB_REG_TYPE_END,
} mb_reg_type_e;
#pragma scalar_storage_order big-endian
typedef struct {
mb_reg_type_e type;
uint16_t start_address;
uint16_t values[MB_DU_VALUE_MAX];
uint16_t value_count;
} mb_data_unit;
typedef enum {
MB_STATE_IDLE,
MB_STATE_GET_TASK,
MB_STATE_RW_ING,
MB_STATE_RW_WAITING,
} mb_state_e;
typedef enum {
MB_FUNC_READ_COILS = 0x01,
MB_FUNC_READ_DISCRETE_INPUTS = 0x02,
MB_FUNC_READ_HOLDING_REGS = 0x03,
MB_FUNC_READ_INPUT_REGS = 0x04,
MB_FUNC_WRITE_SINGLE_COIL = 0x05,
MB_FUNC_WRITE_SINGLE_REG = 0x06,
MB_FUNC_WRITE_MULTI_COILS = 0x0f,
MB_FUNC_WRITE_MULTI_REGS = 0x10,
} mb_func_e;
typedef struct {
uint8_t slave_addr;
bool is_write;
mb_data_unit data;
}mb_rw_task;
typedef bool (*mb_tx_callback)(const uint8_t *buf, uint16_t buflen);
typedef bool (*mb_start_rx_callback)(uint8_t *buf, uint16_t bufsize);
typedef struct {
mb_tx_callback tx;
mb_start_rx_callback start_rx;
}mb_callback;
typedef struct {
mb_state_e state;
q_queue queue;
bool rx_done;
uint8_t rxbuff[MB_FRAME_MAX_LEN];
uint16_t rxbuff_len;
mb_rw_task t;
mb_callback callback;
}mb_handle;
bool mb_init(mb_handle *h, const mb_callback * callback);
bool mb_write(mb_handle *h, uint8_t slave_addr, const mb_data_unit * data);
bool mb_read(mb_handle *h, uint8_t slave_addr, const mb_data_unit * data);
void mb_fsm_process(mb_handle *h);
void mb_set_rx_done(mb_handle *h, uint16_t rxlen);
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment