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 @@ ...@@ -65,7 +65,7 @@
#include "rt_io_mb_locals.h" #include "rt_io_mb_locals.h"
char rcv_buffer[65536]; //char rcv_buffer[65536];
/* Check if channel should be fetched from diagnostic area, /* Check if channel should be fetched from diagnostic area,
i.e. channel name starts with "Diag_" */ i.e. channel name starts with "Diag_" */
...@@ -110,6 +110,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp) ...@@ -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); errh_Error( "Address error for IO modbus tcp slave %s %s", rp->Name, op->Address);
return sts; return sts;
} }
local->rem_addr.sin_addr.s_addr = ntohl(local->rem_addr.sin_addr.s_addr);
/* Connect to remote address */ /* Connect to remote address */
...@@ -131,7 +132,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp) ...@@ -131,7 +132,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp)
tv.tv_usec = 200000; tv.tv_usec = 200000;
} }
sts = select(32, &fdr, &fdw, NULL, &tv); sts = select((int)local->s+1, &fdr, &fdw, NULL, &tv);
if (sts <= 0) { if (sts <= 0) {
close(local->s); close(local->s);
...@@ -154,12 +155,11 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local, ...@@ -154,12 +155,11 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
pwr_sClass_Modbus_ModuleMsg *mp; pwr_sClass_Modbus_ModuleMsg *mp;
pwr_tStatus sts; pwr_tStatus sts;
fd_set fdr; /* For select call */ fd_set fdr; /* For select call */
fd_set fde; /* For select call */
fd_set fdw; /* For select call */
struct timeval tv; struct timeval tv;
pwr_tBoolean found; pwr_tBoolean found;
int data_size; int data_size;
rec_buf *rb; rec_buf *rb;
char rcv_buffer[260];
pwr_tCid cid; pwr_tCid cid;
unsigned char fc; unsigned char fc;
short int trans_id; short int trans_id;
...@@ -167,203 +167,200 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local, ...@@ -167,203 +167,200 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
int modules; int modules;
int i; int i;
/* Receive answer */ short int remaining_data; // Data we have to get from the socket.
short int received_data; // Data that has been received.
sts = 1;
sts = 1;
while (sts > 0) { rb = (rec_buf *) rcv_buffer;
FD_ZERO(&fdr);
FD_ZERO(&fdw); while (sts > 0) { /* Receive answer */
FD_ZERO(&fde);
FD_SET(local->s, &fdr); size_of_msg = 0;
FD_SET(local->s, &fdw); remaining_data = sizeof(mbap_header);
FD_SET(local->s, &fde); received_data = 0;
size_of_msg = 0; /*
tv.tv_sec = 0; First read at least the MBAP header, and no more.
tv.tv_usec = 0; 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
sts = select(32, &fdr, &fdw, &fde, &tv); only the first bytes of a packet.
*/
if (sts < 0) {
sp->Status = MB__CONNLOST; while ( (remaining_data > 0) && (sts > 0) ) { // if there is data to read and everything is ok, receive.
close(local->s);
errh_Error( "Connection lost to modbus slave, %s", rp->Name); FD_ZERO(&fdr);
return IO__SUCCESS; FD_SET(local->s, &fdr);
}
if (local->expected_msgs > 0) {
if (!(FD_ISSET(local->s, &fdw))) { tv.tv_sec = 0;
sp->Status = MB__CONNDOWN; tv.tv_usec = sp->ResponseTime * 1000;
close(local->s); } else {
errh_Error( "Connection down to modbus slave, %s", rp->Name); tv.tv_sec = 0;
return IO__SUCCESS; tv.tv_usec = 0;
} }
if (local->expected_msgs > 0) { sts = select((int)local->s+1, &fdr, NULL, NULL, &tv);
tv.tv_sec = 0;
tv.tv_usec = sp->ResponseTime * 1000; if (sts<=0) { // Timeout or error.
} else { if ((sts == 0) && (local->expected_msgs > 0)) { // Timeout but there are messages pending
tv.tv_sec = 0; local->msgs_lost++;
tv.tv_usec = 0; if (local->msgs_lost > MAX_MSGS_LOST) {
} sp->Status = MB__CONNDOWN;
close(local->s);
FD_ZERO(&fdr); errh_Error( "Data expected but timeout. Connection down to modbus slave, %s", rp->Name);
FD_ZERO(&fde); }
FD_SET(local->s, &fdr); return IO__SUCCESS;
FD_SET(local->s, &fde); }
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; 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