You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
475 lines
12 KiB
475 lines
12 KiB
/*
|
|
* modbus_slave.c
|
|
*
|
|
* Change Logs:
|
|
* Date Author Notes
|
|
* 2024-04-02 qiyongzhong first version
|
|
*/
|
|
|
|
#include <string.h>
|
|
#include "modbus.h"
|
|
#include "modbus_cvt.h"
|
|
#include "modbus_port.h"
|
|
|
|
#ifdef MB_USING_SLAVE
|
|
|
|
static void mb_slave_pdu_deal_read_coils(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->read_coil == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->rd_req.addr;
|
|
int nb = pdu->rd_req.nb;
|
|
memset(hinst->datas, 0, sizeof(hinst->datas));
|
|
for (int i=0; i<nb; i++)
|
|
{
|
|
u8 bit;
|
|
int rst = hinst->cb->read_coil(addr + i, &bit);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
mb_bitmap_set(hinst->datas, i, bit);
|
|
}
|
|
|
|
pdu->rd_rsp.dlen = (nb + 7) / 8;
|
|
pdu->rd_rsp.pdata = hinst->datas;
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_read_discs(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->read_disc == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->rd_req.addr;
|
|
int nb = pdu->rd_req.nb;
|
|
memset(hinst->datas, 0, sizeof(hinst->datas));
|
|
for (int i=0; i<nb; i++)
|
|
{
|
|
u8 bit;
|
|
int rst = hinst->cb->read_disc(addr + i, &bit);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
mb_bitmap_set(hinst->datas, i, bit);
|
|
}
|
|
|
|
pdu->rd_rsp.dlen = (nb + 7) / 8;
|
|
pdu->rd_rsp.pdata = hinst->datas;
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_read_holds(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->read_hold == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->rd_req.addr;
|
|
int nb = pdu->rd_req.nb;
|
|
u8 *p = hinst->datas;
|
|
for (int i=0; i<nb; i++)
|
|
{
|
|
u16 val;
|
|
int rst = hinst->cb->read_hold(addr + i, &val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
p += mb_cvt_u16_put(p, val);
|
|
}
|
|
|
|
pdu->rd_rsp.dlen = 2 * nb;
|
|
pdu->rd_rsp.pdata = hinst->datas;
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_read_inputs(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->read_input == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->rd_req.addr;
|
|
int nb = pdu->rd_req.nb;
|
|
u8 *p = hinst->datas;
|
|
for (int i=0; i<nb; i++)
|
|
{
|
|
u16 val;
|
|
int rst = hinst->cb->read_input(addr + i, &val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
p += mb_cvt_u16_put(p, val);
|
|
}
|
|
|
|
pdu->rd_rsp.dlen = 2 * nb;
|
|
pdu->rd_rsp.pdata = hinst->datas;
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_write_coil(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->write_coil == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->wr_single.addr;
|
|
u16 val = pdu->wr_single.val;
|
|
if ((val != 0xFF00) && (val != 0x0000))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_ILLEGAL_DATA_VALUE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
int rst = hinst->cb->write_coil(addr, (val ? 1 : 0));
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_write_reg(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->write_hold == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->wr_single.addr;
|
|
u16 val = pdu->wr_single.val;
|
|
int rst = hinst->cb->write_hold(addr, val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_write_coils(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->write_coil == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->wr_req.addr;
|
|
int nb = pdu->wr_req.nb;
|
|
u8 *pbits = pdu->wr_req.pdata;
|
|
for (int i=0; i<nb; i++)
|
|
{
|
|
u8 bit = mb_bitmap_get(pbits, i);
|
|
int rst = hinst->cb->write_coil(addr + i, bit);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_write_regs(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->write_hold == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->wr_req.addr;
|
|
int nb = pdu->wr_req.nb;
|
|
u8 *p = pdu->wr_req.pdata;
|
|
for (int i=0; i<nb; i++)
|
|
{
|
|
u16 val;
|
|
p += mb_cvt_u16_get(p, &val);
|
|
int rst = hinst->cb->write_hold(addr + i, val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_mask_write_reg(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->read_hold == NULL) || (hinst->cb->write_hold == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 addr = pdu->mask_wr.addr;
|
|
u16 val_and = pdu->mask_wr.val_and;
|
|
u16 val_or = pdu->mask_wr.val_or;
|
|
u16 val;
|
|
int rst = hinst->cb->read_hold(addr, &val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
val = ((val & val_and) | (val_or & ~val_and));
|
|
rst = hinst->cb->write_hold(addr, val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
}
|
|
|
|
static void mb_slave_pdu_deal_write_and_read_regs(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
if ((hinst->cb == NULL) || (hinst->cb->read_hold == NULL) || (hinst->cb->write_hold == NULL))
|
|
{
|
|
pdu->exc.ec = MODBUS_EC_SLAVE_OR_SERVER_FAILURE;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
|
|
u16 rd_addr = pdu->wr_rd_req.rd_addr;
|
|
int rd_nb = pdu->wr_rd_req.rd_nb;
|
|
u16 wr_addr = pdu->wr_rd_req.wr_addr;
|
|
int wr_nb = pdu->wr_rd_req.wr_nb;
|
|
u8 *p = pdu->wr_rd_req.pdata;
|
|
for (int i=0; i<wr_nb; i++)
|
|
{
|
|
u16 val;
|
|
p += mb_cvt_u16_get(p, &val);
|
|
int rst = hinst->cb->write_hold(wr_addr + i, val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
}
|
|
|
|
p = hinst->datas;
|
|
for (int i=0; i<rd_nb; i++)
|
|
{
|
|
u16 val;
|
|
int rst = hinst->cb->read_hold(rd_addr + i, &val);
|
|
if (rst < 0)
|
|
{
|
|
pdu->exc.ec = -rst;
|
|
pdu->exc.fc = MODBUS_FC_EXCEPT_MAKE(pdu->exc.fc);
|
|
return;
|
|
}
|
|
p += mb_cvt_u16_put(p, val);
|
|
}
|
|
|
|
pdu->rd_rsp.dlen = rd_nb * 2;
|
|
pdu->rd_rsp.pdata = hinst->datas;
|
|
}
|
|
|
|
static void mb_slave_pdu_deal(mb_inst_t *hinst, mb_pdu_t *pdu)
|
|
{
|
|
switch(pdu->fc)
|
|
{
|
|
case MODBUS_FC_READ_COILS :
|
|
mb_slave_pdu_deal_read_coils(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_READ_DISCRETE_INPUTS :
|
|
mb_slave_pdu_deal_read_discs(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_READ_HOLDING_REGISTERS :
|
|
mb_slave_pdu_deal_read_holds(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_READ_INPUT_REGISTERS :
|
|
mb_slave_pdu_deal_read_inputs(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_WRITE_SINGLE_COIL :
|
|
mb_slave_pdu_deal_write_coil(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_WRITE_SINGLE_REGISTER :
|
|
mb_slave_pdu_deal_write_reg(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_READ_EXCEPTION_STATUS :
|
|
break;
|
|
case MODBUS_FC_WRITE_MULTIPLE_COILS :
|
|
mb_slave_pdu_deal_write_coils(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_WRITE_MULTIPLE_REGISTERS :
|
|
mb_slave_pdu_deal_write_regs(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_REPORT_SLAVE_ID :
|
|
break;
|
|
case MODBUS_FC_MASK_WRITE_REGISTER :
|
|
mb_slave_pdu_deal_mask_write_reg(hinst, pdu);
|
|
break;
|
|
case MODBUS_FC_WRITE_AND_READ_REGISTERS :
|
|
mb_slave_pdu_deal_write_and_read_regs(hinst, pdu);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
#ifdef MB_USING_RTU_PROTOCOL
|
|
static void mb_slave_recv_deal_rtu(mb_inst_t *hinst, u8 *buf, int len)
|
|
{
|
|
mb_rtu_frm_t frm;
|
|
int pdu_len = mb_rtu_frm_parse(buf, len, &frm, MB_PDU_TYPE_REQ);
|
|
if (pdu_len == 0)//帧错误, 不处理
|
|
{
|
|
return;
|
|
}
|
|
|
|
#ifdef MB_USING_ADDR_CHK
|
|
if (frm.saddr != hinst->saddr)//地址不同, 不处理
|
|
{
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
if (pdu_len < 0)//功能码不支持, 响应异常帧
|
|
{
|
|
frm.pdu.exc.ec = MODBUS_EC_ILLEGAL_FUNCTION;
|
|
frm.pdu.exc.fc = MODBUS_FC_EXCEPT_MAKE(frm.pdu.exc.fc);
|
|
}
|
|
else
|
|
{
|
|
mb_slave_pdu_deal(hinst, &(frm.pdu));
|
|
}
|
|
|
|
int flen = mb_rtu_frm_make(hinst->buf, &frm, MB_PDU_TYPE_RSP);
|
|
mb_send(hinst, hinst->buf, flen);
|
|
}
|
|
#endif
|
|
|
|
#ifdef MB_USING_TCP_PROTOCOL
|
|
static void mb_slave_recv_deal_tcp(mb_inst_t *hinst, u8 *buf, int len)
|
|
{
|
|
mb_tcp_frm_t frm;
|
|
int pdu_len = mb_tcp_frm_parse(buf, len, &frm, MB_PDU_TYPE_REQ);
|
|
if (pdu_len == 0)//帧错误, 不处理
|
|
{
|
|
return;
|
|
}
|
|
|
|
#ifdef MB_USING_ADDR_CHK
|
|
if ((frm.mbap.did != 0xFF) && (frm.mbap.did != hinst->saddr))//使用地址且地址不同, 不处理
|
|
{
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
#ifdef MB_USING_MBAP_CHK
|
|
if (frm.mbap.pid != MB_TCP_MBAP_PID)//协议标识错误, 不处理
|
|
{
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
if (pdu_len < 0)//功能码不支持, 响应异常帧
|
|
{
|
|
frm.pdu.exc.ec = MODBUS_EC_ILLEGAL_FUNCTION;
|
|
frm.pdu.exc.fc = MODBUS_FC_EXCEPT_MAKE(frm.pdu.exc.fc);
|
|
}
|
|
else
|
|
{
|
|
mb_slave_pdu_deal(hinst, &(frm.pdu));
|
|
}
|
|
|
|
int flen = mb_tcp_frm_make(hinst->buf, &frm, MB_PDU_TYPE_RSP);
|
|
mb_send(hinst, hinst->buf, flen);
|
|
}
|
|
#endif
|
|
|
|
static void mb_slave_recv_deal(mb_inst_t *hinst, u8 *buf, int len)
|
|
{
|
|
switch(hinst->prot)
|
|
{
|
|
#ifdef MB_USING_RTU_PROTOCOL
|
|
case MB_PROT_RTU :
|
|
mb_slave_recv_deal_rtu(hinst, buf, len);
|
|
break;
|
|
#endif
|
|
#ifdef MB_USING_TCP_PROTOCOL
|
|
case MB_PROT_TCP :
|
|
mb_slave_recv_deal_tcp(hinst, buf, len);
|
|
break;
|
|
#endif
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
const mb_cb_table_t mb_cb_table = {
|
|
.read_disc = mb_port_read_disc, //读离散量输入
|
|
.read_coil = mb_port_read_coil, //读线圈
|
|
.write_coil = mb_port_write_coil, //写线圈
|
|
.read_input = mb_port_read_input, //读输入寄存器
|
|
.read_hold = mb_port_read_hold, //读保持寄存器
|
|
.write_hold = mb_port_write_hold, //写保持寄存器
|
|
};
|
|
|
|
//修改从机回调函数表, 默认使用modbus_port中接口函数做回调函数
|
|
void mb_set_cb_table(mb_inst_t *hinst, const mb_cb_table_t *cb)
|
|
{
|
|
MB_ASSERT(hinst != NULL);
|
|
MB_ASSERT(cb != NULL);
|
|
|
|
hinst->cb = (mb_cb_table_t *)cb;
|
|
}
|
|
|
|
//从机状态机处理, 在线程中循环调用即可
|
|
void mb_slave_fsm(mb_inst_t *hinst)
|
|
{
|
|
MB_ASSERT(hinst != NULL);
|
|
|
|
if (mb_connect(hinst) < 0)//连接失败, 延时返回
|
|
{
|
|
mb_port_delay_ms(1000);
|
|
return;
|
|
}
|
|
|
|
int rlen = mb_recv(hinst, hinst->buf, sizeof(hinst->buf));
|
|
if (rlen <= 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
mb_slave_recv_deal(hinst, hinst->buf, rlen);
|
|
}
|
|
|
|
#endif
|
|
|