/*----------------------------------------------------------------------------*\
Read method for the Modbus Master agent
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoAgentRead(io_tCtx ctx, io_sAgent* ap)
{
io_sAgentLocal* local;
io_sRackLocal* local_rack;
pwr_tUInt16 sts;
io_sRack* rp;
pwr_tCid cid;
pwr_sClass_Modbus_TCP_Slave* sp;
local = (io_sAgentLocal*)ap->Local;
/* Do configuration check and initialize modules. */
rp = ap->racklist;
while (rp) {
cid = rp->Class;
while (ODD(gdh_GetSuperClass(cid, &cid, rp->Objid)))
;
switch (cid) {
case pwr_cClass_Modbus_TCP_Slave:
sp = (pwr_sClass_Modbus_TCP_Slave*)rp->op;
local_rack = rp->Local;
/* Start receving old data so the input buffer is flushed */
if (sp->Status == MB__NORMAL) {
sts = mb_recv_data(local_rack, rp, sp);
}
/* Request new data */
if (sp->Status == MB__NORMAL && sp->DisableSlave != 1) {
sts = mb_send_data(local_rack, rp, sp, mb_mSendMask_ReadReq);
}
break;
} /* End - switch ... */
rp = rp->next;
}
return IO__SUCCESS;
}
static pwr_tStatus IoRackInit (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp
)
{
io_sCardLocal *local_card;
io_sCard *cardp;
io_sRackLocal *local;
int no_di;
int no_do;
pwr_sClass_Modbus_RTU_Slave *op;
char name[196];
pwr_tStatus sts;
pwr_tCid cid;
io_sChannel *chanp;
int i;
sts = gdh_ObjidToName(rp->Objid, (char *) &name, sizeof(name), cdh_mNName);
errh_Info( "Init of Modbus TCP Slave and Modules %s", name);
op = (pwr_sClass_Modbus_RTU_Slave *) rp->op;
rp->Local = calloc(1, sizeof(io_sRackLocal));
local = rp->Local;
op->Status = MB__NORMAL;
/* Do configuration check and initialize modules. */
cardp = rp->cardlist;
unsigned int prev_input_area_offset = 0;
unsigned int prev_output_area_offset = 0;
unsigned int input_area_offset = 0;
unsigned int output_area_offset = 0;
unsigned int input_area_chansize = 0;
unsigned int output_area_chansize = 0;
while(cardp) {
local_card = calloc(1, sizeof(*local_card));
cid = cardp->Class;
/* Find the super class */
while ( ODD( gdh_GetSuperClass( cid, &cid, cardp->Objid))) ;
switch (cid) {
case pwr_cClass_Modbus_RTU_Module: {
pwr_sClass_Modbus_RTU_Module *modulep;
cardp->Local = local_card;
no_di = 0;
no_do = 0;
local_card->msg[0].input_area = (void *) &(op->Inputs) + input_area_offset +
input_area_chansize;
local_card->msg[0].output_area = (void *) &(op->Outputs) + output_area_offset +
output_area_chansize;
modulep = (pwr_sClass_Modbus_RTU_Module *) cardp->op;
modulep->Status = pwr_eModbusModule_StatusEnum_StatusUnknown;
io_bus_card_init( ctx, cardp, &input_area_offset, &input_area_chansize,
&output_area_offset, &output_area_chansize,
pwr_eByteOrderingEnum_BigEndian, io_eAlignment_Packed);
/* Count number of di and do */
for (i = 0; i < cardp->ChanListSize; i++) {
chanp = &cardp->chanlist[i];
switch (chanp->ChanClass) {
case pwr_cClass_ChanDi:
no_di++;
break;
case pwr_cClass_ChanDo:
no_do++;
break;
}
}
local_card->msg[0].input_size = input_area_offset + input_area_chansize -
prev_input_area_offset;
local_card->msg[0].output_size = output_area_offset + output_area_chansize -
prev_output_area_offset;
local_card->msg[0].no_di = no_di;
local_card->msg[0].no_do = no_do;
break;
}
} /* End - switch ... */
prev_input_area_offset = input_area_offset + input_area_chansize;
prev_output_area_offset = output_area_offset + output_area_chansize;
cardp = cardp->next;
}
local->input_size = input_area_offset + input_area_chansize;
local->output_size = output_area_offset + output_area_chansize;
return IO__SUCCESS;
}
pwr_tStatus mb_send_data(io_sRackLocal *local,
io_sRack *rp,
pwr_sClass_Modbus_TCP_Slave *sp,
mb_tSendMask mask)
{
io_sCardLocalMsg *local_card;
io_sCard *cardp;
pwr_sClass_Modbus_ModuleMsg *mp;
pwr_tStatus sts;
pwr_tCid cid;
int modules;
int i;
/* Send messages to slave */
cardp = rp->cardlist;
while(cardp) {
/* From v4.1.3 we can have subclasses, find the super class */
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;
break;
case pwr_cClass_Modbus_ModuleReadWrite:
mp = &((pwr_sClass_Modbus_ModuleReadWrite *) cardp->op)->Read;
modules = 2;
break;
default:
modules = 0;
}
if ( !modules) {
cardp = cardp->next;
continue;
}
for ( i = 0; i < modules; i++) {
sts = 1;
if (!mp->Continous && !mp->SendOp) {
break;
}
local_card = &((io_sCardLocal *)cardp->Local)->msg[i];
if ( mp->ScanInterval > 1 && local_card->interval_cnt != 0) {
cardp = cardp->next;
continue;
}
if (mask & mb_mSendMask_ReadReq) {
switch (mp->FunctionCode) {
case pwr_eModbus_FCEnum_ReadCoils:
case pwr_eModbus_FCEnum_ReadDiscreteInputs: {
read_req rr;
mp->SendOp = FALSE;
local->trans_id++;
local_card->trans_id = local->trans_id;
rr.head.trans_id = htons(local->trans_id);
rr.head.proto_id = 0;
rr.head.length = htons(sizeof(read_req) - 6);
rr.head.unit_id = mp->UnitId;
rr.fc = mp->FunctionCode;
rr.addr = htons(mp->Address);
rr.quant = htons(local_card->no_di);
// rr.quant = ntohs(local_card->input_size * 8);
sts = send(local->s, &rr, sizeof(read_req), MSG_DONTWAIT);
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
local->expected_msgs++;
sp->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_ReadHoldingRegisters:
case pwr_eModbus_FCEnum_ReadInputRegisters: {
read_req rr;
mp->SendOp = FALSE;
local->trans_id++;
local_card->trans_id = local->trans_id;
rr.head.trans_id = htons(local->trans_id);
rr.head.proto_id = 0;
rr.head.length = htons(sizeof(read_req) - 6);
rr.head.unit_id = mp->UnitId;
rr.fc = mp->FunctionCode;
rr.addr = htons(mp->Address);
rr.quant = ntohs((local_card->input_size + 1) / 2);
sts = send(local->s, &rr, sizeof(read_req), MSG_DONTWAIT);
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
sp->TX_packets++;
local->expected_msgs++;
break;
}
} /* End - switch FC ... */
}
if (mask & mb_mSendMask_WriteReq) {
switch (mp->FunctionCode) {
case pwr_eModbus_FCEnum_WriteSingleCoil: {
write_single_req wsr;
mp->SendOp = FALSE;
local->trans_id++;
local_card->trans_id = local->trans_id;
wsr.head.trans_id = htons(local->trans_id);
wsr.head.proto_id = 0;
wsr.head.length = htons(sizeof(wsr) - 6);
wsr.head.unit_id = mp->UnitId;
wsr.fc = mp->FunctionCode;
wsr.addr = htons(mp->Address);
if (local_card->output_size == 4) {
if (*(int *)local_card->output_area)
wsr.value = ntohs(0xFF00);
else wsr.value = 0;
} else if (local_card->output_size == 2) {
if (*(short int *)local_card->output_area)
wsr.value = ntohs(0xFF00);
else wsr.value = 0;
} else if (local_card->output_size == 1) {
if (*(char *)local_card->output_area)
wsr.value = ntohs(0xFF00);
else wsr.value = 0;
} else wsr.value = 0;
sts = send(local->s, &wsr, ntohs(wsr.head.length) + 6, MSG_DONTWAIT);
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
local->expected_msgs++;
sp->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_WriteMultipleCoils: {
write_coils_req wcr;
mp->SendOp = FALSE;
local->trans_id++;
local_card->trans_id = local->trans_id;
wcr.head.trans_id = htons(local->trans_id);
wcr.head.proto_id = 0;
wcr.head.length = htons(sizeof(wcr) - 6 -
sizeof(wcr.reg) + local_card->output_size);
wcr.head.unit_id = mp->UnitId;
wcr.fc = mp->FunctionCode;
wcr.addr = htons(mp->Address);
wcr.quant = htons(local_card->no_do);
// wcr.quant = ntohs((local_card->output_size) * 8);
wcr.bc = local_card->output_size;
memcpy(wcr.reg, local_card->output_area, local_card->output_size);
sts = send(local->s, &wcr, ntohs(wcr.head.length) + 6, MSG_DONTWAIT);
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
local->expected_msgs++;
sp->TX_packets++;
break;
}
case pwr_eModbus_FCEnum_WriteMultipleRegisters: {
write_reg_req wrr;
mp->SendOp = FALSE;
local->trans_id++;
local_card->trans_id = local->trans_id;
wrr.head.trans_id = htons(local->trans_id);
wrr.head.proto_id = 0;
wrr.head.length = htons(sizeof(wrr) - 6 -
sizeof(wrr.reg) + local_card->output_size);
wrr.head.unit_id = mp->UnitId;
wrr.fc = mp->FunctionCode;
wrr.addr = htons(mp->Address);
wrr.quant = ntohs((local_card->output_size) / 2);
wrr.bc = local_card->output_size;
memcpy(wrr.reg, local_card->output_area, local_card->output_size);
sts = send(local->s, &wrr, ntohs(wrr.head.length) + 6, MSG_DONTWAIT);
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
sp->TX_packets++;
local->expected_msgs++;
break;
}
case pwr_eModbus_FCEnum_WriteSingleRegister: {
write_single_req wrr;
mp->SendOp = FALSE;
local->trans_id++;
local_card->trans_id = local->trans_id;
wrr.head.trans_id = htons(local->trans_id);
wrr.head.proto_id = 0;
wrr.head.length = htons(sizeof(wrr) - 6);
wrr.head.unit_id = mp->UnitId;
wrr.fc = mp->FunctionCode;
wrr.addr = htons(mp->Address);
memcpy(&wrr.value, local_card->output_area, sizeof(wrr.value));
sts = send(local->s, &wrr, ntohs(wrr.head.length) + 6, MSG_DONTWAIT);
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
sp->TX_packets++;
local->expected_msgs++;
break;
}
} /* End - switch FC ... */
}
if ( sts < 0)
break;
mp++;
}
if (sts < 0) {
sp->Status = MB__CONNDOWN;
close(local->s);
errh_Error( "Connection down to modbus slave, %s", rp->Name);
return IO__SUCCESS;
}
if (sp->SingleOp)
sts = mb_recv_data(local, rp, sp);
if (sp->Status != MB__NORMAL) return IO__SUCCESS;
cardp = cardp->next;
} /* End - while cardp ... */
return IO__SUCCESS;
}
pwr_tStatus mb_recv_data(io_sRackLocal *local,
io_sRack *rp,
pwr_sClass_Modbus_TCP_Slave *sp)
{
io_sCardLocalMsg *local_card;
io_sCard *cardp;
pwr_sClass_Modbus_ModuleMsg *mp;
pwr_tStatus sts;
fd_set fdr; /* 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;
short int size_of_msg;
int modules;
int i;
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
return IO__SUCCESS;
}
static pwr_tStatus IoRackInit (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp
)
{
io_sCardLocal *local_card;
io_sCard *cardp;
short input_counter;
short output_counter;
pwr_sClass_Pb_DP_Slave *op;
pwr_sClass_Pb_Di *dip;
pwr_sClass_Pb_Do *dop;
pwr_sClass_Pb_Ai *aip;
pwr_sClass_Pb_Ao *aop;
pwr_sClass_Pb_Ii *iip;
pwr_sClass_Pb_Io *iop;
pwr_sClass_Pb_Module *mp;
char name[196];
pwr_tStatus sts;
pwr_tCid cid;
io_sChannel *chanp;
int i, latent_input_count, latent_output_count;
pwr_tInt32 chan_size;
pwr_sClass_ChanDi *chan_di;
pwr_sClass_ChanDo *chan_do;
pwr_sClass_ChanAi *chan_ai;
pwr_sClass_ChanAit *chan_ait;
pwr_sClass_ChanIi *chan_ii;
pwr_sClass_ChanAo *chan_ao;
pwr_sClass_ChanIo *chan_io;
sts = gdh_ObjidToName(rp->Objid, (char *) &name, sizeof(name), cdh_mNName);
errh_Info( "Init of Profibus DP Slave and Modules %s", name);
op = (pwr_sClass_Pb_DP_Slave *) rp->op;
// Do configuration check and initialize modules.
cardp = rp->cardlist;
input_counter = 0;
output_counter = 0;
op->NumberModules = 0;
latent_input_count = 0;
latent_output_count = 0;
while(cardp) {
local_card = calloc(1, sizeof(*local_card));
cardp->Local = local_card;
local_card->input_area = (void *) &(op->Inputs);
local_card->output_area = (void *) &(op->Outputs);
/* From v4.1.3 we can have subclasses, find the super class */
cid = cardp->Class;
while ( ODD( gdh_GetSuperClass( cid, &cid, cardp->Objid))) ;
switch (cid) {
/* Old style configuring with Pb_xx objects. Still here for combatibility reasons.
New systems (from v4.1.3) should be build with Pb_Module objects or subclasses */
case pwr_cClass_Pb_Di:
dip = (pwr_sClass_Pb_Di *) cardp->op;
dip->OffsetInputs = input_counter;
dip->BytesOfInput = dip->NumberOfChannels / 8;
input_counter += dip->BytesOfInput;
dip->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Do:
dop = (pwr_sClass_Pb_Do *) cardp->op;
dop->OffsetOutputs = output_counter;
dop->BytesOfOutput = dop->NumberOfChannels / 8;
output_counter += dop->BytesOfOutput;
dop->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Ai:
aip = (pwr_sClass_Pb_Ai *) cardp->op;
aip->OffsetInputs = input_counter;
aip->BytesOfInput = aip->NumberOfChannels * aip->BytesPerChannel;
input_counter += aip->BytesOfInput;
aip->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Ao:
aop = (pwr_sClass_Pb_Ao *) cardp->op;
aop->OffsetOutputs = output_counter;
aop->BytesOfOutput = aop->NumberOfChannels * aop->BytesPerChannel;
output_counter += aop->BytesOfOutput;
aop->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Ii:
iip = (pwr_sClass_Pb_Ii *) cardp->op;
iip->OffsetInputs = input_counter;
iip->BytesOfInput = iip->NumberOfChannels * iip->BytesPerChannel;
input_counter += iip->BytesOfInput;
iip->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Io:
iop = (pwr_sClass_Pb_Io *) cardp->op;
iop->OffsetOutputs = output_counter;
iop->BytesOfOutput = iop->NumberOfChannels * iop->BytesPerChannel;
output_counter += iop->BytesOfOutput;
iop->Status = PB_MODULE_STATE_OPERATE;
break;
/* New style configuring (from v4.1.3) with Pb_Module objects or subclass. Loop all channels
in the module and set channel size and offset. */
case pwr_cClass_Pb_Module:
mp = (pwr_sClass_Pb_Module *) cardp->op;
mp->Status = PB__INITFAIL;
cardp->offset = 0;
for (i=0; i<cardp->ChanListSize; i++) {
chanp = &cardp->chanlist[i];
if ( is_diag( &chanp->ChanAref)) {
chanp->udata |= PB_UDATA_DIAG;
switch (chanp->ChanClass) {
case pwr_cClass_ChanIi:
chanp->offset = ((pwr_sClass_ChanIi *)chanp->cop)->Number;
chanp->size = GetChanSize( ((pwr_sClass_ChanIi *)chanp->cop)->Representation);
break;
default:
errh_Error( "Diagnostic channel class, card %s", cardp->Name);
}
continue;
}
if (chanp->ChanClass != pwr_cClass_ChanDi) {
input_counter += latent_input_count;
latent_input_count = 0;
}
if (chanp->ChanClass != pwr_cClass_ChanDo) {
output_counter += latent_output_count;
latent_output_count = 0;
}
switch (chanp->ChanClass) {
case pwr_cClass_ChanDi:
chan_di = (pwr_sClass_ChanDi *) chanp->cop;
if (chan_di->Number == 0) {
input_counter += latent_input_count;
latent_input_count = 0;
}
chanp->offset = input_counter;
chanp->mask = 1<<chan_di->Number;
if (chan_di->Representation == pwr_eDataRepEnum_Bit16 && op->ByteOrdering == pwr_eByteOrderingEnum_BigEndian)
chanp->mask = swap16(chanp->mask);
if (chan_di->Representation == pwr_eDataRepEnum_Bit32 && op->ByteOrdering == pwr_eByteOrderingEnum_BigEndian)
chanp->mask = swap32((unsigned short) chanp->mask);
if (chan_di->Number == 0) latent_input_count = GetChanSize(chan_di->Representation);
// printf("Di channel found in %s, Number %d, Offset %d\n", cardp->Name, chan_di->Number, chanp->offset);
break;
case pwr_cClass_ChanAi:
chan_ai = (pwr_sClass_ChanAi *) chanp->cop;
chanp->offset = input_counter;
chan_size = GetChanSize(chan_ai->Representation);
chanp->size = chan_size;
chanp->mask = 0;
input_counter += chan_size;
io_AiRangeToCoef(chanp);
// printf("Ai channel found in %s, Number %d, Offset %d\n", cardp->Name, chan_ai->Number, chanp->offset);
break;
case pwr_cClass_ChanAit:
chan_ait = (pwr_sClass_ChanAit *) chanp->cop;
chanp->offset = input_counter;
chan_size = GetChanSize(chan_ait->Representation);
chanp->size = chan_size;
chanp->mask = 0;
input_counter += chan_size;
io_AiRangeToCoef(chanp);
break;
case pwr_cClass_ChanIi:
chan_ii = (pwr_sClass_ChanIi *) chanp->cop;
chanp->offset = input_counter;
chan_size = GetChanSize(chan_ii->Representation);
chanp->size = chan_size;
chanp->mask = 0;
input_counter += chan_size;
// printf("Ii channel found in %s, Number %d, Offset %d\n", cardp->Name, chan_ii->Number, chanp->offset);
break;
case pwr_cClass_ChanDo:
chan_do = (pwr_sClass_ChanDo *) chanp->cop;
if (chan_do->Number == 0) {
output_counter += latent_output_count;
latent_output_count = 0;
}
chanp->offset = output_counter;
chan_size = GetChanSize(chan_do->Representation);
chanp->mask = 1<<chan_do->Number;
if (chan_do->Representation == pwr_eDataRepEnum_Bit16 && op->ByteOrdering == pwr_eByteOrderingEnum_BigEndian)
chanp->mask = swap16(chanp->mask);
if (chan_do->Representation == pwr_eDataRepEnum_Bit32 && op->ByteOrdering == pwr_eByteOrderingEnum_BigEndian)
chanp->mask = swap32((unsigned short) chanp->mask);
if (chan_do->Number == 0) latent_output_count = GetChanSize(chan_do->Representation);
// printf("Do channel found in %s, Number %d, Offset %d\n", cardp->Name, chan_do->Number, chanp->offset);
break;
case pwr_cClass_ChanAo:
chan_ao = (pwr_sClass_ChanAo *) chanp->cop;
chanp->offset = output_counter;
chan_size = GetChanSize(chan_ao->Representation);
chanp->size = chan_size;
chanp->mask = 0;
output_counter += chan_size;
io_AoRangeToCoef(chanp);
// printf("Ao channel found in %s, Number %d, Offset %d\n", cardp->Name, chan_ao->Number, chanp->offset);
break;
case pwr_cClass_ChanIo:
chan_io = (pwr_sClass_ChanIo *) chanp->cop;
chanp->offset = output_counter;
chan_size = GetChanSize(chan_io->Representation);
chanp->size = chan_size;
chanp->mask = 0;
output_counter += chan_size;
// printf("Io channel found in %s, Number %d, Offset %d\n", cardp->Name, chan_io->Number, chanp->offset);
break;
}
}
mp->Status = PB__NOCONN;
break;
}
op->NumberModules++;
cardp = cardp->next;
}
return IO__SUCCESS;
}
int XNav::GetObjectMenu( xmenu_sMenuCall *ip,
pwr_tCid classid,
xmenu_sMenuItem **Item,
pwr_tUInt32 Level,
int *nItems,
int AddSeparator,
pwr_sAttrRef *CurrentObject)
{
int sts;
pwr_tObjid child;
pwr_tObjid menu_objid;
char menu[80];
pwr_tOName classname;
pwr_sAttrRef currentar = pwr_cNAttrRef;
pwr_tCid supercid;
if ( cdh_ObjidIsNotNull( CurrentObject->Objid))
currentar = *CurrentObject;
if ( ip->ItemType == xmenu_eItemType_Object) {
// Popup-menu for an object
if ( !cdh_tidIsCid( classid))
return XNAV__SUCCESS;
// Get the RtXtt common menu-objects
strcpy( menu, "pwrs:Class-$Object-RtXtt");
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, Level, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
// Get the RtXtt menu-objects for superclasses
// TODO shadow overlayed methods...
sts = gdh_GetSuperClass( classid, &supercid, CurrentObject->Objid);
while ( ODD(sts)) {
sts = gdh_ObjidToName( cdh_ClassIdToObjid( supercid), classname,
sizeof(classname), cdh_mName_volumeStrict);
if ( EVEN(sts)) return sts;
sprintf( menu, "%s-RtXtt", classname);
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, 0, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
sts = gdh_GetSuperClass( supercid, &supercid, CurrentObject->Objid);
}
// Get the RtXtt menu-objects for this class, or for superclasses
sts = gdh_ObjidToName( cdh_ClassIdToObjid( classid), classname,
sizeof(classname), cdh_mName_volumeStrict);
if ( EVEN(sts)) return sts;
sprintf( menu, "%s-RtXtt", classname);
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, 0, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
}
else if ( ip->ItemType == xmenu_eItemType_AttrObject) {
// Find attribute object methods...
if ( !cdh_tidIsCid( classid))
return XNAV__SUCCESS;
// Get the RtXtt common menu-objects
strcpy( menu, "pwrs:Class-$Object-RtXttAttrObject");
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, 0, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
// Get the RtXtt menu-objects for this class
sts = gdh_ObjidToName( cdh_ClassIdToObjid( classid), classname,
sizeof(classname), cdh_mName_volumeStrict);
if ( EVEN(sts)) return sts;
sprintf( menu, "%s-RtXtt", classname);
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, 0, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
}
else if ( ip->ItemType == xmenu_eItemType_Attribute) {
// Find attribute methods...
// Get the RtXttAttribute common menu-objects
strcpy( menu, "pwrs:Class-$Object-RtXttAttribute");
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, 0, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
}
else if ( ip->ItemType == xmenu_eItemType_Crossref) {
// Find attribute methods...
// Get the RtXttCrossref common menu-objects
strcpy( menu, "pwrs:Class-$Object-RtXttCrossref");
sts = gdh_NameToObjid( menu, &menu_objid);
if ( ODD(sts)) {
sts = gdh_GetChild( menu_objid, &child);
while( ODD(sts)) {
sts = getAllMenuItems( ip, Item, child, 0, nItems, 0, ¤tar);
if ( EVEN(sts)) return sts;
sts = gdh_GetNextSibling( child, &child);
}
}
}
return XNAV__SUCCESS;
}
/*----------------------------------------------------------------------------*\
Init method for the Powerlink module
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoAgentInit (io_tCtx ctx, io_sAgent *ap) {
io_sLocalEpl_MN *local;
int sts;
pwr_sClass_Epl_MN *op = (pwr_sClass_Epl_MN *)ap->op;
local = (io_sLocalEpl_MN *) calloc( 1, sizeof(io_sLocalEpl_MN));
ap->Local = local;
local->inputResetEnabled = 0;
op->NumberOfSlaves = 0;
static tEplApiInitParam EplApiInitParam;
tEplKernel EplRet = kEplSuccessful;
pwr_tFileName cdc_file;
char* sHostname = malloc(1023);
if ( strchr(op->CDCfile, '/') != 0)
strcpy( cdc_file, op->CDCfile);
else {
strcpy( cdc_file, "$pwrp_load/");
strcat( cdc_file, op->CDCfile);
}
dcli_translate_filename( cdc_file, cdc_file);
gethostname(sHostname, 1023);
if( op->StallAction == pwr_eStallActionEnum_ResetInputs)
local->inputResetEnabled = 1;
// Init the I/O area
unsigned int input_area_offset = 0;
unsigned int input_area_chansize = 0;
unsigned int output_area_offset = 0;
unsigned int output_area_chansize = 0;
io_sRack *rp;
io_sCard *cp;
pwr_tCid cid;
for ( rp = ap->racklist; rp; rp = rp->next) {
rp->Local = calloc( 1, sizeof(io_sLocalEpl_CN));
rp->MethodDisabled = 1;
op->NumberOfSlaves++;
if( ((pwr_sClass_Epl_CN *)rp->op)->StallAction == pwr_eStallActionEnum_ResetInputs)
local->inputResetEnabled = 1;
// Show device offset and size
if ( rp->Class == pwr_cClass_Epl_CN && rp->op) {
((pwr_sClass_Epl_CN *)rp->op)->InputAreaOffset = input_area_offset + input_area_chansize;
((pwr_sClass_Epl_CN *)rp->op)->OutputAreaOffset = output_area_offset + output_area_chansize;
}
// Get byte ordering
pwr_tAName name;
pwr_tEnum byte_ordering;
strcpy( name, rp->Name);
strcat( name, ".ByteOrdering");
sts = gdh_GetObjectInfo( name, &byte_ordering, sizeof(byte_ordering));
if ( ODD(sts))
((io_sLocalEpl_CN *)rp->Local)->byte_ordering = byte_ordering;
else
((io_sLocalEpl_CN *)rp->Local)->byte_ordering =
pwr_eByteOrderingEnum_LittleEndian;
for ( cp = rp->cardlist; cp; cp = cp->next) {
cid = cp->Class;
while ( ODD( gdh_GetSuperClass( cid, &cid, cp->Objid))) ;
cp->MethodDisabled = 1;
// Show module offset and size
if ( cid == pwr_cClass_Epl_Module && cp->op) {
((pwr_sClass_Epl_Module *)cp->op)->InputAreaOffset =
input_area_offset + input_area_chansize;
((pwr_sClass_Epl_Module *)cp->op)->OutputAreaOffset =
output_area_offset + output_area_chansize;
}
io_bus_card_init( ctx, cp, &input_area_offset, &input_area_chansize,
&output_area_offset, &output_area_chansize, byte_ordering,
io_eAlignment_Powerlink);
// Show module offset and size
if ( cid == pwr_cClass_Epl_Module && cp->op) {
((pwr_sClass_Epl_Module *)cp->op)->InputAreaSize =
input_area_offset + input_area_chansize - ((pwr_sClass_Epl_Module *)cp->op)->InputAreaOffset;
((pwr_sClass_Epl_Module *)cp->op)->OutputAreaSize =
output_area_offset + output_area_chansize - ((pwr_sClass_Epl_Module *)cp->op)->OutputAreaOffset;
}
if(rp->next == NULL) {
if(cp->next == NULL) {
((pwr_sClass_Epl_Module *)cp->op)->InputAreaSize +=
pwr_Align(input_area_offset + input_area_chansize, 4) -
(input_area_offset + input_area_chansize);
((pwr_sClass_Epl_Module *)cp->op)->OutputAreaSize +=
pwr_Align(output_area_offset + output_area_chansize, 4) -
(output_area_offset + output_area_chansize);
}
}
}
// Show slave offset and size
if ( rp->Class == pwr_cClass_Epl_CN && rp->op) {
((pwr_sClass_Epl_CN *)rp->op)->InputAreaSize = input_area_offset +
input_area_chansize - ((pwr_sClass_Epl_CN *)rp->op)->InputAreaOffset;
((pwr_sClass_Epl_CN *)rp->op)->OutputAreaSize = output_area_offset +
output_area_chansize - ((pwr_sClass_Epl_CN *)rp->op)->OutputAreaOffset;
if(rp->next == NULL) {
((pwr_sClass_Epl_CN *)rp->op)->InputAreaSize +=
pwr_Align(input_area_offset + input_area_chansize, 4) - (input_area_offset + input_area_chansize);
((pwr_sClass_Epl_CN *)rp->op)->OutputAreaSize +=
pwr_Align(output_area_offset + output_area_chansize, 4) - (output_area_offset + output_area_chansize);
}
}
}
// This is the calculated in- and outputarea size
local->input_area_size = pwr_Align(input_area_offset + input_area_chansize, 4);
local->output_area_size = pwr_Align(output_area_offset + output_area_chansize, 4);
// Show agent in- and output area size
op->InputAreaSize = local->input_area_size;
op->OutputAreaSize = local->output_area_size;
struct sched_param schedParam;
// adjust process priority
// push nice level in case we have no RTPreempt
if (nice (-20) == -1) {
errh_Error("%s() couldn't set nice value! (%s)", __func__, strerror(errno));
}
//schedParam.sched_priority = MIN(sched_get_priority_max(SCHED_FIFO),
// sched_get_priority_min(SCHED_FIFO) + op->Priority);
schedParam.__sched_priority = op->Priority;
if (pthread_setschedparam(pthread_self(), SCHED_RR, &schedParam) != 0) {
errh_Error("%s() couldn't set thread scheduling parameters! %d", __func__, schedParam.__sched_priority);
}
// binds all openPOWERLINK threads to the second CPU core
cpu_set_t affinity;
CPU_ZERO(&affinity);
CPU_SET(1, &affinity);
sched_setaffinity(0, sizeof(cpu_set_t), &affinity);
// Initialize target specific stuff
EplTgtInit();
EPL_MEMSET(&EplApiInitParam, 0, sizeof (EplApiInitParam));
EplApiInitParam.m_uiSizeOfStruct = sizeof (EplApiInitParam);
EplApiInitParam.m_pEventUserArg = ap;
// Get devicename from attribute in agent
EplApiInitParam.m_HwParam.m_pszDevName = op->Device;
// Get nodeid from attribute in agent
EplApiInitParam.m_uiNodeId = op->NodeId;
EplApiInitParam.m_dwIpAddress = ntohl( inet_addr( op->IpAddress));
// write 00:00:00:00:00:00 to MAC address, so that the driver uses the real hardware address
EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr, sizeof (EplApiInitParam.m_abMacAddress));
EplApiInitParam.m_fAsyncOnly = FALSE;
EplApiInitParam.m_dwFeatureFlags = -1;
// required for error detection
EplApiInitParam.m_dwCycleLen = uiCycleLen_g;
// const
EplApiInitParam.m_uiIsochrTxMaxPayload = 256;
// const
EplApiInitParam.m_uiIsochrRxMaxPayload = 256;
// const; only required for IdentRes
EplApiInitParam.m_dwPresMaxLatency = 50000;
// required for initialisation (+28 bytes)
EplApiInitParam.m_uiPreqActPayloadLimit = 36;
// required for initialisation of Pres frame (+28 bytes)
EplApiInitParam.m_uiPresActPayloadLimit = 36;
// const; only required for IdentRes
EplApiInitParam.m_dwAsndMaxLatency = 150000;
// required for error detection
EplApiInitParam.m_uiMultiplCycleCnt = 0;
// required to set up max frame size
EplApiInitParam.m_uiAsyncMtu = 1500;
// required for sync
EplApiInitParam.m_uiPrescaler = 2;
EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
EplApiInitParam.m_dwWaitSocPreq = 150000;
// NMT_DeviceType_U32
EplApiInitParam.m_dwDeviceType = -1;
// NMT_IdentityObject_REC.VendorId_U32
EplApiInitParam.m_dwVendorId = -1;
// NMT_IdentityObject_REC.ProductCode_U32
EplApiInitParam.m_dwProductCode = -1;
// NMT_IdentityObject_REC.RevisionNo_U32
EplApiInitParam.m_dwRevisionNumber = -1;
// NMT_IdentityObject_REC.SerialNo_U32
EplApiInitParam.m_dwSerialNumber = -1;
EplApiInitParam.m_dwSubnetMask = ntohl( inet_addr( op->IpNetmask));
EplApiInitParam.m_dwDefaultGateway = 0;
EPL_MEMCPY(EplApiInitParam.m_sHostname, sHostname, sizeof(EplApiInitParam.m_sHostname));
EplApiInitParam.m_uiSyncNodeId = EPL_C_ADR_SYNC_ON_SOA;
EplApiInitParam.m_fSyncOnPrcNode = FALSE;
// set callback functions
EplApiInitParam.m_pfnCbEvent = (tEplApiCbEvent)AppCbEvent;
EplApiInitParam.m_pfnObdInitRam = EplObdInitRam;
EplApiInitParam.m_pfnCbSync = AppCbSync;
// initialize POWERLINK stack
EplRet = EplApiInitialize(&EplApiInitParam);
if(EplRet != kEplSuccessful) {
errh_Error("EplApiInitialize() failed (Error:0x%x!", EplRet);
goto Exit;
}
EplRet = EplApiSetCdcFilename(cdc_file);
if(EplRet != kEplSuccessful) {
goto Exit;
}
// Allocate memory for the in- and outputareas
if( local->output_area_size > 0)
AppProcessImageIn_g = malloc(local->output_area_size);
if( local->input_area_size > 0) {
AppProcessImageOut_g = malloc(local->input_area_size);
}
// Save pointer to in- and outputareas in THIS agent object
local->input_area = AppProcessImageOut_g;
local->output_area = AppProcessImageIn_g;
if( local->inputResetEnabled && local->input_area_size > 0)
local->tmp_area = malloc(local->input_area_size);
else
local->tmp_area = local->input_area;
AppProcessImageCopyJob_g.m_fNonBlocking = FALSE;
AppProcessImageCopyJob_g.m_uiPriority = 0;
AppProcessImageCopyJob_g.m_In.m_pPart = AppProcessImageIn_g;
AppProcessImageCopyJob_g.m_In.m_uiOffset = 0;
AppProcessImageCopyJob_g.m_In.m_uiSize = local->output_area_size;
AppProcessImageCopyJob_g.m_Out.m_pPart = AppProcessImageOut_g;
AppProcessImageCopyJob_g.m_Out.m_uiOffset = 0;
AppProcessImageCopyJob_g.m_Out.m_uiSize = local->input_area_size;
EplRet = EplApiProcessImageAlloc(local->output_area_size, local->input_area_size, 2, 2);
if (EplRet != kEplSuccessful) {
goto Exit;
}
EplRet = EplApiProcessImageSetup();
if (EplRet != kEplSuccessful) {
goto Exit;
}
// start processing
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
if (EplRet != kEplSuccessful) {
IoAgentClose(NULL, NULL);
goto Exit;
}
errh_Success ("Powerlink init successfull");
return IO__SUCCESS;
Exit:
errh_Error("IoCardInit: returns 0x%X", EplRet);
return IO__SUCCESS;
}
