一个好用的Modbus解析函数

一个好用的Modbus解析函数

包含03/10/04/06

#include <stdio.h>
#include <stdint.h>
#include <string.h>

#define MODBUS_TCP  1
#define MODBUS_RTU  2
#define MODBUS_MODE MODBUS_RTU

// modbusTCP格式
// MBAP头：事务ID(0x1234) + 协议ID(0x0000) + PDU长度(0x0006) + 单元ID(0x01)
// PDU：从机地址(0x01) + 功能码(0x03) + 起始地址(0x5000) + 寄存器数量(0x0002)

uint8_t addr = 1;

typedef struct {
    uint32_t testdata_0;
    uint32_t testdata_1;
    uint32_t testdata_2;
} modbus_testdata_t;

modbus_testdata_t testdata = {
        .testdata_0 = 0x99999999,
        .testdata_1 = 0x88888888,
        .testdata_2 = 0x77777777,
};


typedef enum {
    Com_AnswerErr0 = 0,    // 成功
    Com_AnswerErr1 = 1,    // 处理失败
    Com_AnswerErr2 = 2,    // 地址/功能码无效
    Com_AnswerErr3 = 3,    // 数量超限
    Com_NoAnswer = 0xFF    // 无响应
} modbus_err_t;

typedef struct {
    uint16_t Addr;         // 寄存器起始地址
    uint8_t len;          // 寄存器数量
    uint16_t pra;          // 参数（保留）
    uint8_t FunIndex;     // 对应处理函数在ModFunList中的索引
} modbus_coretab_t;

typedef modbus_err_t (*modbus_function_t)(uint8_t *buff, uint16_t pra);

typedef struct {
    uint8_t data[256];    // 数据缓冲区
    uint16_t len;        // 有效数据长度
} modbus_data_t;

void modbus_send(int pra, uint8_t *buff, uint16_t len) {
    // 对接发送函数
    //tcp_send(sock,buff, len);
    for (int i = 0; i < len; i++) {
        printf("%02X ", buff[i]);
    }
    printf("\n");
}

uint16_t modbus_crc16(const uint8_t *buf, uint16_t len) {
    uint16_t crc = 0xFFFF;
    for (uint16_t i = 0; i < len; i++) {
        crc ^= buf[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

/**********************************业务处理函数START**********************************/

typedef union {
    uint8_t bytes[4];
    float f;
    uint32_t u32;
} float_union_t;

//modbus功能函数
modbus_err_t ReadWriteData(uint8_t *buff, uint16_t pra) {
    float_union_t data;

    if ((pra & 0x8000) == 0x8000) {
        //可写入
        switch (pra & 0x0F) {
            case 0:
                printf("WriteElecData0\n");
                testdata.testdata_0 = (buff[0] << 24) | (buff[1] << 16) | (buff[2] << 8) | buff[3];
                break;
            case 1:
                printf("WriteElecData1\n");
                testdata.testdata_1 = (buff[0] << 24) | (buff[1] << 16) | (buff[2] << 8) | buff[3];
                break;
            default:
                printf("WriteElecData2\n");
                testdata.testdata_2 = (buff[0] << 24) | (buff[1] << 16) | (buff[2] << 8) | buff[3];
                break;
        }
    } else {
        switch (pra & 0x0F) {
            case 0:
                data.u32 = testdata.testdata_0;
                printf("ReadElecData0: %08X\n", data.u32);
                break;
            case 1:
                data.u32 = testdata.testdata_1;
                printf("ReadElecData1: %08X\n", data.u32);
                break;
            default:
                data.u32 = testdata.testdata_2;
                printf("ReadElecData2: %08X\n", data.u32);
                break;
        }
    }

    *buff++ = data.bytes[3];
    *buff++ = data.bytes[2];
    *buff++ = data.bytes[1];
    *buff++ = data.bytes[0];

    return Com_AnswerErr0;
}

const modbus_coretab_t modbus_coretab_03[] = {
        //    地址        数量   参数(高四位写入低四位索引)  索引
        {0x0000, 2, 0x11, 0},
        {0x0004, 2, 0x11, 0},
};
const modbus_coretab_t modbus_coretab_04[] = {


};

// 处理函数列表（与寄存器表FunIndex对应）
modbus_function_t modbus_function[] = {
        ReadWriteData,
};
/**********************************业务处理函数END**********************************/


const uint8_t modbus_coretab_size_03 = sizeof(modbus_coretab_03) / sizeof(modbus_coretab_t);
const uint8_t modbus_coretab_size_04 = sizeof(modbus_coretab_04) / sizeof(modbus_coretab_t);


// 核心：Modbus连续读取处理函数
modbus_err_t RWModBusReg(modbus_data_t *modbus_data, uint8_t type) {
    uint8_t uc_RegCnt = modbus_data->data[5];    // 请求的寄存器数量（PDU第5字节）
    uint16_t ui_RegAddr = (modbus_data->data[2] << 8) | modbus_data->data[3];  // 起始地址（PDU第2-3字节）
    uint8_t read_cnt = 0;                   // 已读取的寄存器数量

    // 1. 数量校验（Modbus标准最大125个）
    if (uc_RegCnt == 0 || uc_RegCnt > 125) {
        return Com_AnswerErr3;  // 数量无效
    }

    const modbus_coretab_t *modbus_coretab = NULL;
    uint8_t modbus_coretab_size = 0;
    if (type == 0x03) {
        modbus_coretab = modbus_coretab_03;
        modbus_coretab_size = modbus_coretab_size_03;
    } else if (type == 0x04) {
        modbus_coretab = modbus_coretab_04;
        modbus_coretab_size = modbus_coretab_size_04;
    }

    // 2. 初始化响应头（从机地址+功能码）
    modbus_data->data[0] = addr;  // 复用请求中的从机地址
    modbus_data->data[1] = type;             // 功能码（0x03）
    modbus_data->len = 3;                   // 初始长度：头3字节（地址+功能码+字节计数）

    // 3. 遍历寄存器表处理连续读取
    while (read_cnt < uc_RegCnt) {
        uint16_t current_addr = ui_RegAddr + read_cnt;  // 当前需要读取的地址（动态更新）
        uint8_t found = 0;                              // 是否找到匹配的表项

        for (uint8_t i = 0; i < modbus_coretab_size; i++) {  // 遍历所有表项找匹配
            const modbus_coretab_t *tab = &modbus_coretab[i];
            // 计算当前地址在表项中的偏移（相对于表项起始地址）
            uint16_t offset = current_addr - tab->Addr;
            if (offset < 0 || offset >= tab->len) continue;  // 地址不在当前表项范围内

            // 计算当前表项可读取的最大寄存器数量（避免越界）
            uint8_t max_read = tab->len - offset;
            uint8_t actual_read = (uc_RegCnt - read_cnt) < max_read ? (uc_RegCnt - read_cnt) : max_read;

            // 调用处理函数读取数据（填充到响应缓冲区，每个寄存器2字节）
            modbus_err_t err = modbus_function[tab->FunIndex](modbus_data->data + modbus_data->len, tab->pra);
            if (err != Com_AnswerErr0) return err;

            // 更新已读取数量和数据长度
            read_cnt += actual_read;
            modbus_data->len += actual_read * 2;
            found = 1;
            break;  // 找到匹配表项并处理后，跳出for循环继续下一个地址
        }

        if (!found) {  // 关键修改：未找到表项时填充0值
            // 填充0x0000（两个字节表示一个寄存器的0值）
            modbus_data->data[modbus_data->len] = 0x00;
            modbus_data->data[modbus_data->len + 1] = 0x00;
            modbus_data->len += 2;  // 数据长度增加2字节（一个寄存器）
            read_cnt += 1;      // 已读取数量+1
        }
    }

    // 4. 填充字节计数（总数据字节数=已读数量×2）
    modbus_data->data[2] = read_cnt * 2;

    return Com_AnswerErr0;  // 已完全读取
}

// 核心：Modbus写寄存器处理函数（支持0x06写单个、0x10写多个）
modbus_err_t WriteModBusReg(modbus_data_t *modbus_data) {
    uint8_t func_code = modbus_data->data[1];  // 功能码（0x06/0x10）
    uint16_t reg_addr = (modbus_data->data[2] << 8) | modbus_data->data[3];  // 起始地址
    uint16_t reg_cnt, byte_cnt;
    uint8_t data_offset;

    // 1. 参数解析与校验
    if (func_code == 0x06) {  // 写单个寄存器
        reg_cnt = 1;
        byte_cnt = 2;
        data_offset = 4;  // 数据从PDU第4字节开始
    } else if (func_code == 0x10) {  // 写多个寄存器
        reg_cnt = (modbus_data->data[4] << 8) | modbus_data->data[5];  // 寄存器数量
        byte_cnt = modbus_data->data[6];                                // 字节计数
        data_offset = 7;  // 数据从PDU第7字节开始
        if (byte_cnt != reg_cnt * 2 || reg_cnt > 123) {  // Modbus标准限制
            return Com_AnswerErr3;  // 数量超限或数据长度错误
        }
    } else {
        return Com_AnswerErr2;  // 不支持的功能码
    }

    // 2. 遍历寄存器表验证地址并执行写入
    const modbus_coretab_t *coretab = modbus_coretab_03;  // 使用0x03功能码的寄存器表
    uint8_t coretab_size = modbus_coretab_size_03;
    uint16_t remaining = reg_cnt;

    while (remaining > 0) {
        uint8_t found = 0;
        for (uint8_t i = 0; i < coretab_size; i++) {
            const modbus_coretab_t *tab = &coretab[i];
            if (reg_addr < tab->Addr || reg_addr >= tab->Addr + tab->len) continue;  // 地址不匹配
            if (!(tab->pra & 0x10)) return Com_AnswerErr2;

            // 计算当前表项可处理的寄存器数量
            uint8_t process_cnt = (tab->Addr + tab->len - reg_addr) < remaining
                                  ? (tab->Addr + tab->len - reg_addr)
                                  : remaining;

            // 调用写处理函数（pra | 0x8000表示写操作）
            modbus_err_t err = modbus_function[tab->FunIndex](
                    modbus_data->data + data_offset,  // 指向写入数据
                    tab->pra | 0x8000);  // 写操作标志
            if (err != Com_AnswerErr0) return err;

            // 更新状态
            remaining -= process_cnt;
            reg_addr += process_cnt;
            data_offset += process_cnt * 2;  // 数据偏移后移（每个寄存器2字节）
            found = 1;
            break;
        }
        if (!found) return Com_AnswerErr2;  // 地址无效
    }

    // 3. 构造响应（Modbus标准格式）
    modbus_data->len = 6;  // 响应固定6字节（地址+功能码+起始地址+数量）
    modbus_data->data[2] = (reg_addr - reg_cnt) >> 8;  // 起始地址高字节
    modbus_data->data[3] = (reg_addr - reg_cnt) & 0xFF; // 起始地址低字节
    modbus_data->data[4] = reg_cnt >> 8;                // 数量高字节
    modbus_data->data[5] = reg_cnt & 0xFF;              // 数量低字节

    return Com_AnswerErr0;
}

void modbus_parse(int pra, uint8_t *data, uint16_t len) {
    modbus_data_t modbus_data;
    uint16_t pdu_len;

#if MODBUS_MODE == MODBUS_TCP
    // --- TCP 模式 ---
    memcpy(modbus_data.data, data + 6, len - 6);  // 去掉 MBAP
    pdu_len = len - 6;

#elif MODBUS_MODE == MODBUS_RTU
    // --- RTU 模式 ---
    if (len < 4) return;  // 最少 地址+功能码+CRC
    uint16_t crc_calc = modbus_crc16(data, len - 2);
    uint16_t crc_recv = (data[len - 2]) | (data[len - 1] << 8);
    if (crc_calc != crc_recv) {
        printf("CRC ERROR %04X %04X\n", crc_calc, crc_recv);
        return;
    } else{
        printf("CRC OK %04X %04X\n", crc_calc, crc_recv);
    }
    memcpy(modbus_data.data, data, len - 2);  // 去掉 CRC
    pdu_len = len - 2;
#endif

    // 调用已有逻辑
    modbus_data.len = pdu_len;
    modbus_err_t err;
    switch (modbus_data.data[1]) {
        case 0x03:
        case 0x04:
            err = RWModBusReg(&modbus_data, modbus_data.data[1]);
            break;
        case 0x06:
        case 0x10:
            err = WriteModBusReg(&modbus_data);
            break;
        default:
            err = Com_AnswerErr2;
            break;
    }

    // 构造响应
    if (err == Com_AnswerErr0) {
#if MODBUS_MODE == MODBUS_TCP
        uint8_t senddata[300];
        senddata[0] = data[0]; senddata[1] = data[1]; // 事务ID
        senddata[2] = 0x00; senddata[3] = 0x00;      // 协议ID
        senddata[4] = 0x00; senddata[5] = modbus_data.len;
        memcpy(senddata + 6, modbus_data.data, modbus_data.len);
        modbus_send(pra, senddata, modbus_data.len + 6);

#elif MODBUS_MODE == MODBUS_RTU
        uint8_t senddata[300];
        memcpy(senddata, modbus_data.data, modbus_data.len);
        uint16_t crc = modbus_crc16(senddata, modbus_data.len);
        senddata[modbus_data.len] = crc & 0xFF;
        senddata[modbus_data.len + 1] = crc >> 8;
        modbustcp_send(pra, senddata, modbus_data.len + 2);
#endif
    }
}


#define TEST 0x06

int main() {
#if (TEST == 0x01)
    // 03 功能码连读测试 modbusTCP
    uint8_t request_data[] = {
            0x12, 0x34, 0x00, 0x00, 0x00, 0x06,  // MBAP头（7字节）
            0x00, 0x03, 0x00, 0x00, 0x00, 0x02         // PDU（6字节）
    };
    modbus_parse(0, request_data, sizeof(request_data));  // 调用解析函数

#elif (TEST == 0x02)
    // 03 功能码连读测试 modbusRTU
    uint8_t request_data[] = {
            //01 03 00 00 00 06 C5 C8
            0x01, 0x03, 0x00, 0x00, 0x00, 0x06, 0xC5, 0xC8
    };
    modbus_parse(0, request_data, sizeof(request_data));  // 调用解析函数

#elif (TEST == 0x03)
    // 10 功能码连写测试 modbusTCP
    uint8_t request_data[] = {
            0x12, 0x34, 0x00, 0x00, 0x00, 0x06,  // MBAP头（7字节）
            0x00, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, 0x11, 0x11, 0x11, 0x11
    };
    modbus_parse(0, request_data, sizeof(request_data));  // 调用解析函数
    printf("10 功能码连写测试 modbusTCP %08X\n", testdata.testdata_1);
#elif (TEST == 0x04)
    // 10 功能码连写测试 modbusRTU
        uint8_t request_data[] = {
                // 00 10 00 00 00 02 04 01 00 01 00 F7 3F
                0x00, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, 0x01, 0x00, 0x01, 0x00, 0xF7, 0x3F
    };
    modbus_parse(0, request_data, sizeof(request_data));  // 调用解析函数
    printf("10 功能码连写测试 modbusRTU %08X\n", testdata.testdata_1);


#elif (TEST == 0x05)
    // 06 功能码写测试 modbusTCP
    uint8_t request_data[] = {
            0x12, 0x34, 0x00, 0x00, 0x00, 0x06,  // MBAP头（7字节）
            0x00, 0x06, 0x00, 0x00, 0x00, 0x02         // PDU（6字节）
    };
    modbus_parse(0, request_data, sizeof(request_data));  // 调用解析函数
    printf("06 功能码写测试 modbusTCP %08X\n", testdata.testdata_1);
#elif (TEST == 0x06)
    // 06 功能码写测试 modbusRTU
    uint8_t request_data[] = {
            //00 06 00 00 00 22 08 02
            0x00, 0x06, 0x00, 0x00, 0x00, 0x22, 0x08, 0x02
    };
    modbus_parse(0, request_data, sizeof(request_data));  // 调用解析函数
    printf("06 功能码写测试 modbusRTU %08X\n", testdata.testdata_1);

#endif
    return 0;
}