/*----------------------------------------------------------------------------*\
Read method for the Modbus RTU server module
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardRead (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sServerModuleLocal *local;
io_sServerLocal *local_server;
pwr_sClass_Modbus_RTU_ServerModule *op;
pwr_sClass_Modbus_RTU_Server *server;
op = (pwr_sClass_Modbus_RTU_ServerModule *) cp->op;
local = (io_sServerModuleLocal *) cp->Local;
server = (pwr_sClass_Modbus_RTU_Server *) rp->op;
local_server = (io_sServerLocal *) rp->Local;
if ( server->DisableServer || !local)
return IO__SUCCESS;
if (server->Status == MB__NORMAL) {
thread_MutexLock( &local_server->mutex);
io_bus_card_read(ctx, rp, cp, local->input_area, NULL, pwr_eByteOrderingEnum_BigEndian, pwr_eFloatRepEnum_FloatIntel);
thread_MutexUnlock( &local_server->mutex);
}
// printf("Method Modbus_Module-IoCardRead\n");
return IO__SUCCESS;
}
void
que_Put (
pwr_tStatus *status,
que_sQue *qp,
lst_sEntry *lp,
void *item
)
{
pwr_dStatus(sts, status, QUE__SUCCESS);
thread_MutexLock(&qp->mutex);
lst_InsertPred(NULL, &qp->lh, lp, item);
thread_CondSignal(&qp->cond);
thread_MutexUnlock(&qp->mutex);
}
void *
que_Get (
pwr_tStatus *status,
que_sQue *qp,
pwr_tDeltaTime *tp,
void *tmo_item
)
{
void *p = NULL;
pwr_dStatus(sts, status, QUE__SUCCESS);
thread_MutexLock(&qp->mutex);
while((p = lst_RemoveSucc(NULL, &qp->lh, NULL)) == NULL && ODD(*sts) && *sts != THREAD__TIMEDOUT)
*sts = thread_CondTimedWait(&qp->cond, &qp->mutex, tp);
thread_MutexUnlock(&qp->mutex);
return p == NULL ? tmo_item : p;
}
/* C_DigestKey continues a multi-part message-digesting
* operation, by digesting the value of a secret key as part of
* the data already digested. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestKey)
(
CK_SESSION_HANDLE hSession, /* the session's handle */
CK_OBJECT_HANDLE hKey /* secret key to digest */
)
{
CK_RV rv = CKR_OK;
P11_LOG_START("C_DigestKey");
thread_MutexLock(st.async_lock);
rv = CKR_FUNCTION_NOT_SUPPORTED;
log_Log(LOG_MED, "Function not supported");
thread_MutexUnlock(st.async_lock);
P11_LOG_END("C_DigestKey");
return rv;
}
/* C_DigestInit initializes a message-digesting operation. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestInit)
(
CK_SESSION_HANDLE hSession, /* the session's handle */
CK_MECHANISM_PTR pMechanism /* the digesting mechanism */
)
{
CK_RV rv = CKR_OK;
P11_LOG_START("C_DigestInit");
thread_MutexLock(st.async_lock);
rv = CKR_FUNCTION_NOT_SUPPORTED;
log_Log(LOG_MED, "Function not supported");
thread_MutexUnlock(st.async_lock);
P11_LOG_END("C_DigestInit");
return rv;
}
/* C_DigestUpdate continues a multiple-part message-digesting
* operation. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestUpdate)
(
CK_SESSION_HANDLE hSession, /* the session's handle */
CK_BYTE_PTR pPart, /* data to be digested */
CK_ULONG ulPartLen /* bytes of data to be digested */
)
{
CK_RV rv = CKR_OK;
P11_LOG_START("C_DigestUpdate");
thread_MutexLock(st.async_lock);
rv = CKR_FUNCTION_NOT_SUPPORTED;
log_Log(LOG_MED, "Function not supported");
thread_MutexUnlock(st.async_lock);
P11_LOG_END("C_DigestUpdate");
return rv;
}
/* C_DigestFinal finishes a multiple-part message-digesting
* operation. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestFinal)
(
CK_SESSION_HANDLE hSession, /* the session's handle */
CK_BYTE_PTR pDigest, /* gets the message digest */
CK_ULONG_PTR pulDigestLen /* gets byte count of digest */
)
{
CK_RV rv = CKR_OK;
P11_LOG_START("C_DigestFinal");
thread_MutexLock(st.async_lock);
rv = CKR_FUNCTION_NOT_SUPPORTED;
log_Log(LOG_MED, "Function not supported");
thread_MutexUnlock(st.async_lock);
P11_LOG_END("C_DigestFinal");
return rv;
}
static void *mb_receive( void *data)
{
io_sRack *rp = ((mb_sCondata *)data)->rp;
int l_idx = ((mb_sCondata *)data)->idx;
io_sServerLocal* local = rp->Local;
int c_socket = local->connections[l_idx].c_socket;
pwr_sClass_Modbus_TCP_Server *op = (pwr_sClass_Modbus_TCP_Server *) rp->op;
ssize_t data_size;
rec_buf *rb;
unsigned char fc;
unsigned char exception_code;
ssize_t ssts;
int size_of_msg;
free( data);
op->Connections++;
while ( 1) {
size_of_msg = 0;
data_size = recv(c_socket, rcv_buffer, sizeof(rec_buf), 0);
if ( data_size < 0) {
op->ErrorCount++;
continue;
}
if ( data_size == 0) {
/* Disconnected */
op->Connections--;
close( c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
if ( op->DisableServer)
continue;
while ( data_size > 0) {
if (data_size < sizeof(mbap_header))
break;
op->RX_packets++;
rb = (rec_buf *) &rcv_buffer[size_of_msg];
if ( rb->head.length == 0)
break;
size_of_msg += ntohs(rb->head.length) + 6;
data_size -= ntohs(rb->head.length) + 6;
fc = (unsigned char) *rb->buf;
time_GetTime( &local->connections[l_idx].last_req_time);
exception_code = 0;
switch ( fc) {
case pwr_eModbus_FCEnum_ReadHoldingRegisters: {
io_sCard *cardp;
io_sServerModuleLocal *local_card;
pwr_sClass_Modbus_TCP_ServerModule *mp;
read_req *rmsg = (read_req *)rb;
rsp_read msg;
int found;
short addr = ntohs( rmsg->addr);
short quant = ntohs( rmsg->quant);
unsigned char unit_id = rmsg->head.unit_id;
if ( quant < 1 || quant >= 0x07d0) {
exception_code = 3;
break;
}
/* Check the address */
found = 0;
for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
if ( mp->UnitId == unit_id) {
local_card = cardp->Local;
found = 1;
break;
}
}
if ( !found) {
exception_code = 2;
break;
}
addr -= mp->ReadAddress;
if ( addr < 0 ||
addr + quant * 2 > local_card->output_size) {
exception_code = 2;
break;
}
msg.fc = fc;
msg.bc = quant * 2;
msg.head.trans_id = rmsg->head.trans_id;
// msg.head.length = htons( sizeof(msg) - 6);
msg.head.length = htons( sizeof(msg) - sizeof(msg.buf) + quant * 2 - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
thread_MutexLock( &local->mutex);
memcpy( msg.buf, (char *)local_card->output_area + addr, quant * 2);
thread_MutexUnlock( &local->mutex);
ssts = send( c_socket, &msg, ntohs(msg.head.length) + 6, MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_ReadCoils:
case pwr_eModbus_FCEnum_ReadDiscreteInputs: {
io_sCard *cardp;
io_sServerModuleLocal *local_card;
pwr_sClass_Modbus_TCP_ServerModule *mp;
read_req *rmsg = (read_req *)rb;
rsp_read msg;
int found;
unsigned char mask;
unsigned int bytes;
int i;
int offs;
short addr = ntohs( rmsg->addr);
short quant = ntohs( rmsg->quant);
unsigned char unit_id = rmsg->head.unit_id;
if ( quant < 1 || quant >= 0x07d0) {
exception_code = 3;
break;
}
/* Check the address */
found = 0;
for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
if ( mp->UnitId == unit_id) {
local_card = cardp->Local;
found = 1;
break;
}
}
if ( !found) {
exception_code = 2;
break;
}
offs = addr / 8;
bytes = (addr + quant) / 8 + (((addr + quant) % 8 == 0) ? 0 : 1) - offs;
if ( addr < 0 ||
offs + bytes + local_card->do_offset > local_card->output_size ||
offs + bytes > local_card->do_size) {
exception_code = 2;
break;
}
memset( &msg, 0, sizeof(msg));
msg.fc = fc;
msg.bc = bytes;
msg.head.trans_id = rmsg->head.trans_id;
// msg.head.length = htons( sizeof(msg) - 6);
msg.head.length = htons( sizeof(msg) - sizeof(msg.buf) + bytes - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
thread_MutexLock( &local->mutex);
if ( addr % 8 == 0) {
memcpy( msg.buf, (char *)local_card->output_area + local_card->do_offset + addr/8, bytes);
mask = 0;
for ( i = 0; i < quant % 8; i++)
mask |= 1 << i;
if ( quant % 8 != 0) {
unsigned char *b = (unsigned char *) msg.buf;
b[bytes - 1] &= mask;
}
}
else {
mb_shift_read( (unsigned char *)local_card->output_area + local_card->do_offset + addr / 8,
(unsigned char *)msg.buf,
addr % 8, quant);
}
thread_MutexUnlock( &local->mutex);
ssts = send( c_socket, &msg, ntohs(msg.head.length) + 6, MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_WriteSingleRegister: {
io_sCard *cardp;
io_sServerModuleLocal *local_card;
pwr_sClass_Modbus_TCP_ServerModule *mp;
write_single_req *rmsg = (write_single_req *)rb;
rsp_single_write msg;
int found;
short addr = ntohs( rmsg->addr);
unsigned char unit_id = rmsg->head.unit_id;
/* Check the address */
found = 0;
for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
if ( mp->UnitId == unit_id) {
local_card = cardp->Local;
found = 1;
break;
}
}
if ( !found) {
exception_code = 2;
break;
}
addr -= mp->WriteAddress;
if ( addr < 0 ||
addr + 2 > local_card->input_size) {
exception_code = 2;
break;
}
thread_MutexLock( &local->mutex);
memcpy( (char *)local_card->input_area + addr, &rmsg->value, 2);
thread_MutexUnlock( &local->mutex);
msg.fc = fc;
msg.addr = rmsg->addr;
msg.value = rmsg->value;
msg.head.trans_id = rmsg->head.trans_id;
msg.head.length = htons( sizeof(msg) - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_WriteMultipleRegisters: {
io_sCard *cardp;
io_sServerModuleLocal *local_card;
pwr_sClass_Modbus_TCP_ServerModule *mp;
write_reg_req *rmsg = (write_reg_req *)rb;
rsp_write msg;
int found;
short addr = ntohs( rmsg->addr);
short quant = ntohs( rmsg->quant);
unsigned char unit_id = rmsg->head.unit_id;
if ( quant < 1 || quant >= 0x07d0) {
exception_code = 3;
break;
}
/* Check the address */
found = 0;
for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
if ( mp->UnitId == unit_id) {
local_card = cardp->Local;
found = 1;
break;
}
}
if ( !found) {
exception_code = 2;
break;
}
addr -= mp->WriteAddress;
if ( addr < 0 ||
addr + quant * 2 > local_card->input_size) {
exception_code = 2;
break;
}
thread_MutexLock( &local->mutex);
memcpy( (char *)local_card->input_area + addr, rmsg->reg, quant * 2);
thread_MutexUnlock( &local->mutex);
msg.fc = fc;
msg.addr = rmsg->addr;
msg.quant = rmsg->quant;
msg.head.trans_id = rmsg->head.trans_id;
msg.head.length = htons( sizeof(msg) - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_WriteSingleCoil: {
io_sCard *cardp;
io_sServerModuleLocal *local_card;
pwr_sClass_Modbus_TCP_ServerModule *mp;
write_single_req *rmsg = (write_single_req *)rb;
rsp_single_write msg;
int found;
unsigned char mask;
int offs;
short addr = ntohs( rmsg->addr);
unsigned short value = ntohs( rmsg->value);
unsigned char unit_id = rmsg->head.unit_id;
/* Check the address */
found = 0;
for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
if ( mp->UnitId == unit_id) {
local_card = cardp->Local;
found = 1;
break;
}
}
if ( !found) {
exception_code = 2;
break;
}
offs = addr / 8;
if ( addr < 0 ||
offs + local_card->di_offset >= local_card->input_size ||
offs >= local_card->di_size) {
exception_code = 2;
break;
}
mask = 1 << (addr % 8);
if ( value == 0xFF00 || value == 0) {
thread_MutexLock( &local->mutex);
if ( value == 0xFF00)
*((char *)local_card->input_area + local_card->di_offset + offs) |= mask;
else
*((char *)local_card->input_area + local_card->di_offset + offs) &= ~mask;
thread_MutexUnlock( &local->mutex);
}
msg.fc = fc;
msg.addr = rmsg->addr;
msg.value = rmsg->value;
msg.head.trans_id = rmsg->head.trans_id;
msg.head.length = htons( sizeof(msg) - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_WriteMultipleCoils: {
io_sCard *cardp;
io_sServerModuleLocal *local_card;
pwr_sClass_Modbus_TCP_ServerModule *mp;
write_reg_req *rmsg = (write_reg_req *)rb;
rsp_write msg;
int found;
unsigned char mask;
unsigned int bytes;
int i;
int offs;
short addr = ntohs( rmsg->addr);
short quant = ntohs( rmsg->quant);
unsigned char unit_id = rmsg->head.unit_id;
if ( quant < 1 || quant >= 0x07d0) {
exception_code = 3;
break;
}
/* Check the address */
found = 0;
for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
if ( mp->UnitId == unit_id) {
local_card = cardp->Local;
found = 1;
break;
}
}
if ( !found) {
exception_code = 2;
break;
}
offs = addr / 8;
bytes = (addr + quant) / 8 + (((addr + quant) % 8 == 0) ? 0 : 1) - offs;
if ( addr < 0 ||
offs + bytes + local_card->di_offset > local_card->input_size ||
offs + bytes > local_card->di_size) {
exception_code = 2;
break;
}
thread_MutexLock( &local->mutex);
if ( addr % 8 == 0) {
if ( quant % 8 != 0) {
mask = 0;
for ( i = 0; i < quant % 8; i++)
mask |= 1 << i;
memcpy( (char *)local_card->input_area + local_card->di_offset + addr / 8,
rmsg->reg, bytes - 1);
*((char *)local_card->input_area + local_card->di_offset + addr / 8 + bytes - 1) &= ~mask;
*((char *)local_card->input_area + local_card->di_offset + addr / 8 + bytes - 1) |= *((char *)rmsg->reg + bytes - 1) & mask;
}
else
memcpy( (char *)local_card->input_area + local_card->di_offset + addr / 8,
rmsg->reg, bytes);
}
else {
mb_shift_write( (unsigned char *)rmsg->reg,
(unsigned char *)local_card->input_area + local_card->di_offset + addr / 8,
addr % 8, quant);
}
thread_MutexUnlock( &local->mutex);
msg.fc = fc;
msg.addr = rmsg->addr;
msg.quant = rmsg->quant;
msg.head.trans_id = rmsg->head.trans_id;
msg.head.length = htons( sizeof(msg) - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
case 43: {
/* Encapsulated Interface Transport, Read Device Identification */
read_dev_id_req *rmsg = (read_dev_id_req *)rb;
rsp_dev_id msg;
int i;
int len;
if ( rmsg->mei_type != 0x2b) {
exception_code = 1;
break;
}
if ( rmsg->id_code != 1) {
exception_code = 1;
break;
}
if ( rmsg->object_id != 0) {
exception_code = 1;
break;
}
msg.fc = rmsg->fc;
msg.mei_type = rmsg->mei_type;
msg.id_code = rmsg->id_code;
msg.conformity_level = 1;
msg.more_follows = 0;
msg.next_object_id = 0;
msg.number_of_objects = 3;
i = 0;
/* Vendor name */
msg.list[i++] = 0;
len = strlen("Proview");
msg.list[i++] = len;
strncpy( (char *)&msg.list[i], "Proview", len);
i += len;
/* Product code */
msg.list[i++] = 0;
len = strlen("-");
msg.list[i++] = len;
strncpy( (char *)&msg.list[i], "-", len);
i += len;
/* Major Minor Revision */
msg.list[i++] = 0;
len = strlen(pwrv_cPwrVersionStr);
msg.list[i++] = len;
strncpy( (char *)&msg.list[i], pwrv_cPwrVersionStr, len);
i += len;
msg.head.trans_id = rmsg->head.trans_id;
msg.head.length = htons( sizeof(msg) - sizeof(msg.list) + i - 6);
msg.head.unit_id = rmsg->head.unit_id;
msg.head.proto_id = rmsg->head.proto_id;
ssts = send( c_socket, &msg, ntohs(msg.head.length) + 6, MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
break;
}
default:
exception_code = 1;
}
if ( exception_code) {
rsp_fault rsp_f;
rsp_f.fc = fc + 0x80;
rsp_f.ec = exception_code;
rsp_f.head.trans_id = rb->head.trans_id;
rsp_f.head.length = htons( sizeof(rsp_f) - 6);
rsp_f.head.unit_id = rb->head.unit_id;
ssts = send( c_socket, &rsp_f, sizeof(rsp_f), MSG_DONTWAIT);
if (ssts < 0) {
op->Connections--;
close(c_socket);
local->connections[l_idx].occupied = 0;
errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
return 0;
}
op->TX_packets++;
}
}
}
return 0;
}
