Skip to content

P
proview
Commit bc2d7bd3 authored by Claes Sjofors's avatar Claes Sjofors
Browse files
Options

Modbus TCP receive bugfix

parent 08880117
Hide whitespace changes
Inline Side-by-side
Showing with 197 additions and 200 deletions
otherio/lib/rt/src/os_linux/rt_io_m_mb_tcp_slave.c
View file @ bc2d7bd3
......@@ -65,7 +65,7 @@
#include "rt_io_mb_locals.h"
char rcv_buffer[65536];
//char rcv_buffer[65536];
/* Check if channel should be fetched from diagnostic area,
i.e. channel name starts with "Diag_" */
......@@ -110,6 +110,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp)
errh_Error( "Address error for IO modbus tcp slave %s %s", rp->Name, op->Address);
return sts;
}
local->rem_addr.sin_addr.s_addr = ntohl(local->rem_addr.sin_addr.s_addr);
/* Connect to remote address */
......@@ -131,7 +132,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp)
tv.tv_usec = 200000;
}
sts = select(32, &fdr, &fdw, NULL, &tv);
sts = select((int)local->s+1, &fdr, &fdw, NULL, &tv);
if (sts <= 0) {
close(local->s);
......@@ -154,12 +155,11 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
pwr_sClass_Modbus_ModuleMsg *mp;
pwr_tStatus sts;
fd_set fdr; /* For select call */
fd_set fde; /* For select call */
fd_set fdw; /* For select call */
struct timeval tv;
pwr_tBoolean found;
int data_size;
rec_buf *rb;
char rcv_buffer[260];
pwr_tCid cid;
unsigned char fc;
short int trans_id;
......@@ -167,203 +167,200 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
int modules;
int i;
/* Receive answer */
sts = 1;
while (sts > 0) {
FD_ZERO(&fdr);
FD_ZERO(&fdw);
FD_ZERO(&fde);
FD_SET(local->s, &fdr);
FD_SET(local->s, &fdw);
FD_SET(local->s, &fde);
size_of_msg = 0;
tv.tv_sec = 0;
tv.tv_usec = 0;
sts = select(32, &fdr, &fdw, &fde, &tv);
if (sts < 0) {
sp->Status = MB__CONNLOST;
close(local->s);
errh_Error( "Connection lost to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
if (!(FD_ISSET(local->s, &fdw))) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
if (local->expected_msgs > 0) {
tv.tv_sec = 0;
tv.tv_usec = sp->ResponseTime * 1000;
} else {
tv.tv_sec = 0;
tv.tv_usec = 0;
}
FD_ZERO(&fdr);
FD_ZERO(&fde);
FD_SET(local->s, &fdr);
FD_SET(local->s, &fde);
short int remaining_data; // Data we have to get from the socket.
short int received_data; // Data that has been received.
sts = 1;
rb = (rec_buf *) rcv_buffer;
while (sts > 0) { /* Receive answer */
size_of_msg = 0;
remaining_data = sizeof(mbap_header);
received_data = 0;
/*
First read at least the MBAP header, and no more.
Then, read the remaining bytes indicaterd in the header, but no more.
We control the amount of data because could be more than one message in the socket buffer or
only the first bytes of a packet.
*/
while ( (remaining_data > 0) && (sts > 0) ) { // if there is data to read and everything is ok, receive.
FD_ZERO(&fdr);
FD_SET(local->s, &fdr);
if (local->expected_msgs > 0) {
tv.tv_sec = 0;
tv.tv_usec = sp->ResponseTime * 1000;
} else {
tv.tv_sec = 0;
tv.tv_usec = 0;
}
sts = select((int)local->s+1, &fdr, NULL, NULL, &tv);
if (sts<=0) { // Timeout or error.
if ((sts == 0) && (local->expected_msgs > 0)) { // Timeout but there are messages pending
local->msgs_lost++;
if (local->msgs_lost > MAX_MSGS_LOST) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Data expected but timeout. Connection down to modbus slave, %s", rp->Name);
}
return IO__SUCCESS;
}
if (sts < 0) { // Error in the socket
sp->Status = MB__CONNLOST;
close(local->s);
errh_Error( "Socket Error. Connection lost to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
} else { // There are something to read (no timeout and no error). Could be a closed socket too, so we have to check later anyway.
data_size = recv(local->s, &rcv_buffer[received_data], remaining_data, 0);
if (data_size < 0) {
sp->Status = MB__CONNLOST;
close(local->s);
errh_Error( "Error reading data. Connection lost to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
if (data_size == 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Error reading data. Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
remaining_data = remaining_data - data_size;
received_data = received_data + data_size;
if ( (received_data >= sizeof(mbap_header)) && (size_of_msg == 0 )) {
// Compute the complete header
trans_id = ntohs(rb->head.trans_id);
size_of_msg = ntohs(rb->head.length) + 6;
// Check header data
if ((ntohs(rb->head.proto_id)!=0) || (size_of_msg>260)) { // Invalid modbus packet
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Invalid Modbus packet. Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
// Update remaining data
remaining_data = size_of_msg - received_data;
}
}
} // while
if (sts > 0) { // processing packet...
local->msgs_lost = 0;
sp->RX_packets++;
local->expected_msgs--;
cardp = rp->cardlist;
while(cardp) {
/* From v4.1.3 we can have subclasses, find the super class */
found = FALSE;
cid = cardp->Class;
while ( ODD( gdh_GetSuperClass( cid, &cid, cardp->Objid))) ;
switch (cid) {
case pwr_cClass_Modbus_Module:
mp = (pwr_sClass_Modbus_ModuleMsg *) &((pwr_sClass_Modbus_Module *) cardp->op)->FunctionCode;
modules = 1;
local_card = ((io_sCardLocal *)cardp->Local)->msg;
break;
case pwr_cClass_Modbus_ModuleReadWrite:
mp = &((pwr_sClass_Modbus_ModuleReadWrite *) cardp->op)->Read;
modules = 2;
local_card = ((io_sCardLocal *)cardp->Local)->msg;
break;
default:
modules = 0;
}
if ( !modules) {
cardp = cardp->next;
continue;
}
for ( i = 0; i < modules; i++) {
if (local_card->trans_id == trans_id) {
fc = (unsigned char) *rb->buf;
if (fc > 0x80) {
res_fault *res_f;
res_f = (res_fault *) rb->buf;
mp->Status = res_f->ec;
mp++;
local_card++;
continue;
}
if (fc != mp->FunctionCode) {
mp->Status = pwr_eModbusModule_StatusEnum_StatusUnknown;
mp++;
local_card++;
continue;
}
mp->Status = pwr_eModbusModule_StatusEnum_OK;
switch (fc) {
case pwr_eModbus_FCEnum_ReadCoils: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_ReadDiscreteInputs: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_ReadHoldingRegisters: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_ReadInputRegisters: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_WriteMultipleCoils:
case pwr_eModbus_FCEnum_WriteMultipleRegisters:
case pwr_eModbus_FCEnum_WriteSingleRegister:
// Nothing good to do here
break;
}
found = TRUE;
}
mp++;
local_card++;
} // for ( i = 0; i < modules; i++)
if (found)
break;
cardp = cardp->next;
} /* End - while(cardp) ... */
} // if (sts > 0) processing packet...
} // while (sts > 0) Receive answer
sts = select(32, &fdr, NULL, &fde, &tv);
if (sts < 0) {
sp->Status = MB__CONNLOST;
close(local->s);
errh_Error( "Connection lost to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
if ((sts == 0) && (local->expected_msgs > 0)) {
local->msgs_lost++;
if (local->msgs_lost > MAX_MSGS_LOST) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
}
return IO__SUCCESS;
}
if (sts > 0 && FD_ISSET(local->s, &fdr)) {
data_size = recv(local->s, rcv_buffer, sizeof(rec_buf), 0);
if (data_size < 0) {
sp->Status = MB__CONNLOST;
close(local->s);
errh_Error( "Connection lost to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
if (data_size == 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
while (data_size > 0) {
local->msgs_lost = 0;
sp->RX_packets++;
local->expected_msgs--;
cardp = rp->cardlist;
if (data_size < sizeof(mbap_header))
break;
rb = (rec_buf *) &rcv_buffer[size_of_msg];
trans_id = ntohs(rb->head.trans_id);
size_of_msg += ntohs(rb->head.length) + 6;
data_size -= ntohs(rb->head.length) + 6;
while(cardp) {
/* From v4.1.3 we can have subclasses, find the super class */
found = FALSE;
cid = cardp->Class;
while ( ODD( gdh_GetSuperClass( cid, &cid, cardp->Objid))) ;
switch (cid) {
case pwr_cClass_Modbus_Module:
mp = (pwr_sClass_Modbus_ModuleMsg *) &((pwr_sClass_Modbus_Module *) cardp->op)->FunctionCode;
modules = 1;
local_card = ((io_sCardLocal *)cardp->Local)->msg;
break;
case pwr_cClass_Modbus_ModuleReadWrite:
mp = &((pwr_sClass_Modbus_ModuleReadWrite *) cardp->op)->Read;
modules = 2;
local_card = ((io_sCardLocal *)cardp->Local)->msg;
break;
default:
modules = 0;
}
if ( !modules) {
cardp = cardp->next;
continue;
}
for ( i = 0; i < modules; i++) {
if (local_card->trans_id == trans_id) {
fc = (unsigned char) *rb->buf;
if (fc > 0x80) {
res_fault *res_f;
res_f = (res_fault *) rb->buf;
mp->Status = res_f->ec;
mp++;
local_card++;
continue;
}
if (fc != mp->FunctionCode) {
mp->Status = pwr_eModbusModule_StatusEnum_StatusUnknown;
mp++;
local_card++;
continue;
}
mp->Status = pwr_eModbusModule_StatusEnum_OK;
switch (fc) {
case pwr_eModbus_FCEnum_ReadCoils: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_ReadDiscreteInputs: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_ReadHoldingRegisters: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_ReadInputRegisters: {
res_read *res_r;
res_r = (res_read *) rb->buf;
memcpy(local_card->input_area, res_r->buf, MIN(res_r->bc, local_card->input_size));
break;
}
case pwr_eModbus_FCEnum_WriteMultipleCoils:
case pwr_eModbus_FCEnum_WriteMultipleRegisters:
case pwr_eModbus_FCEnum_WriteSingleRegister:
// Nothing good to do here
break;
}
found = TRUE;
}
mp++;
local_card++;
}
if (found)
break;
cardp = cardp->next;
} /* End - while(cardp) ... */
} /* End - data_size > 0 ... */
} /* End - if received message ... */
} /* End - receive messages ... */
return IO__SUCCESS;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7