Skip to content

P
proview
Commit 04f1d849 authored by claes's avatar claes
Browse files
Options

Updates for v4.0

parent 22445455
Show whitespace changes
Inline Side-by-side
Showing with 326 additions and 273 deletions
src/lib/rt/src/os_linux/pbus.h
View file @ 04f1d849
......@@ -34,6 +34,7 @@
#define PB_IOCTL_WRITE_FIRST_SLAVE _IO(PB_IOCTL_MAGIC, 13)
#define PB_IOCTL_READVERSION _IO(PB_IOCTL_MAGIC, 14)
#define PB_IOCTL_READSERIAL _IO(PB_IOCTL_MAGIC, 15)
#define PB_IOCTL_SET_STALLTIME _IO(PB_IOCTL_MAGIC, 16)
#define ERROR_DESCR_LENGTH 32
......
src/lib/rt/src/os_linux/rt_io_m_pb_ai.c
View file @ 04f1d849
......@@ -186,7 +186,7 @@ static pwr_tStatus IoCardInit (
return 1;
}
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE) {
for (i=0; i<IO_MAXCHAN; i++) {
local->scancount[i] = 0;
......@@ -201,7 +201,7 @@ static pwr_tStatus IoCardInit (
else
errh_Info( "Error initializing Pb module Ai %s", cp->Name );
return 1;
return IO__SUCCESS;
}
......@@ -234,7 +234,7 @@ static pwr_tStatus IoCardRead (
op = (pwr_sClass_Pb_Ai *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE && slave->DisableSlave != 1) {
for (i=0; i<cp->ChanListSize; i++) {
......@@ -267,6 +267,25 @@ static pwr_tStatus IoCardRead (
break;
}
}
else if (op->BytesPerChannel == 3) {
udata32 = 0;
memcpy(&udata32, local->input_area + op->OffsetInputs + 3*i, 3);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata32 = swap32(udata32);
data32 = (pwr_tInt32) udata32;
sop->RawValue = 0;
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT)
sop->SigValue = udata32 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
else
sop->SigValue = data32 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
switch(chanp->ChanClass) {
case pwr_cClass_ChanAi:
ConvertAi(cop, 32, 0, 0, udata32, &actvalue, op->NumberRepresentation);
break;
case pwr_cClass_ChanAit:
ConvertAit((pwr_sClass_ChanAit *) cop, 32, 0, 0, udata32, &actvalue, op->NumberRepresentation);
break;
}
}
else if (op->BytesPerChannel == 2) {
memcpy(&udata16, local->input_area + op->OffsetInputs + 2*i, 2);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata16 = swap16(udata16);
......@@ -322,7 +341,7 @@ static pwr_tStatus IoCardRead (
} // for
} // if ...op->Status
return 1;
return IO__SUCCESS;
}
......@@ -337,11 +356,11 @@ static pwr_tStatus IoCardClose (
)
{
io_sCardLocal *local;
local = rp->Local;
local = cp->Local;
free ((char *) local);
return 1;
return IO__SUCCESS;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_ao.c
View file @ 04f1d849
......@@ -43,10 +43,10 @@ static pwr_tStatus IoCardInit (
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
return IO__SUCCESS;
}
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE) {
// Calculate polycoeff
for (i=0; i<cp->ChanListSize; i++) {
......@@ -58,7 +58,7 @@ static pwr_tStatus IoCardInit (
else
errh_Info( "Error initializing Pb module Ao %s", cp->Name );
return 1;
return IO__SUCCESS;
}
......@@ -79,6 +79,9 @@ static pwr_tStatus IoCardWrite (
pwr_tInt8 data8 = 0;
pwr_tInt16 data16 = 0;
pwr_tInt32 data32 = 0;
pwr_tUInt8 udata8 = 0;
pwr_tUInt16 udata16 = 0;
pwr_tUInt32 udata32 = 0;
pwr_sClass_ChanAo *cop;
pwr_sClass_Ao *sop;
io_sChannel *chanp;
......@@ -90,7 +93,7 @@ static pwr_tStatus IoCardWrite (
op = (pwr_sClass_Pb_Ao *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE && slave->DisableSlave != 1) {
fixout = ctx->Node->EmergBreakTrue && ctx->Node->EmergBreakSelect == FIXOUT;
......@@ -122,45 +125,74 @@ static pwr_tStatus IoCardWrite (
rawvalue = cop->OutPolyCoef1 * value + cop->OutPolyCoef0;
if (op->BytesPerChannel == 4) {
if ( rawvalue > 0)
rawvalue = rawvalue + 0.5;
else
rawvalue = rawvalue - 0.5;
// We dont use RawValue in Profibus I/O
sop->RawValue = 0;
// Calculate signal value
sop->SigValue = cop->SigValPolyCoef1 * value + cop->SigValPolyCoef0;
if (op->BytesPerChannel == 4) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
udata32 = (pwr_tUInt32) rawvalue;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata32 = swap32(udata32);
memcpy(local->output_area + op->OffsetOutputs + 4*i, &udata32, 4);
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
data32 = (pwr_tInt32) rawvalue;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
memcpy(local->output_area + op->OffsetOutputs + 4*i, &data32, 4);
}
}
else if (op->BytesPerChannel == 3) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
udata32 = (pwr_tUInt32) rawvalue;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata32 = swap32(udata32);
memcpy(local->output_area + op->OffsetOutputs + 3*i, &udata32, 3);
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
data32 = (pwr_tInt32) rawvalue;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
memcpy(local->output_area + op->OffsetOutputs + 3*i, &data32, 3);
}
}
else if (op->BytesPerChannel == 2) {
if ( rawvalue > 0)
sop->RawValue = rawvalue + 0.5;
else
sop->RawValue = rawvalue - 0.5;
data16 = (pwr_tInt16) sop->RawValue;
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
udata16 = (pwr_tUInt16) rawvalue;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata16 = swap16(udata16);
memcpy(local->output_area + op->OffsetOutputs + 2*i, &udata16, 2);
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
data16 = (pwr_tInt16) rawvalue;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data16 = swap16(data16);
memcpy(local->output_area + op->OffsetOutputs + 2*i, &data16, 2);
}
else if (op->BytesPerChannel == 1) {
if ( rawvalue > 0)
sop->RawValue = rawvalue + 0.5;
else
sop->RawValue = rawvalue - 0.5;
data8 = (pwr_tInt8) sop->RawValue;
}
else if (op->BytesPerChannel == 1) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
udata8 = (pwr_tUInt8) rawvalue;
memcpy(local->output_area + op->OffsetOutputs + i, &udata8, 1);
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
data8 = (pwr_tInt8) rawvalue;
memcpy(local->output_area + op->OffsetOutputs + i, &data8, 1);
}
}
}
}
return 1;
return IO__SUCCESS;
}
......@@ -175,11 +207,11 @@ static pwr_tStatus IoCardClose (
)
{
io_sCardLocal *local;
local = rp->Local;
local = cp->Local;
free ((char *) local);
return 1;
return IO__SUCCESS;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_di.c
View file @ 04f1d849
......@@ -42,14 +42,14 @@ static pwr_tStatus IoCardInit (
// Check configuration
if (op->NumberOfChannels != 8 && op->NumberOfChannels != 16 && op->NumberOfChannels != 32)
op->Status = 0;
op->Status = PB_MODULE_STATE_NOTINIT;
if (op->Orientation > op->NumberOfChannels)
op->Status = 0;
op->Status = PB_MODULE_STATE_NOTINIT;
if (op->Status < 1) errh_Info( "Error initializing Pb module Di %s", cp->Name );
if (op->Status < PB_MODULE_STATE_OPERATE) errh_Info( "Error initializing Pb module Di %s", cp->Name );
return 1;
return IO__SUCCESS;
}
......@@ -74,7 +74,7 @@ static pwr_tStatus IoCardRead (
op = (pwr_sClass_Pb_Di *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE && slave->DisableSlave != 1) {
memcpy(&data, local->input_area + op->OffsetInputs, op->BytesOfInput);
......@@ -100,7 +100,7 @@ static pwr_tStatus IoCardRead (
}
}
return 1;
return IO__SUCCESS;
}
......@@ -115,11 +115,11 @@ static pwr_tStatus IoCardClose (
)
{
io_sCardLocal *local;
local = rp->Local;
local = cp->Local;
free ((char *) local);
return 1;
return IO__SUCCESS;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_do.c
View file @ 04f1d849
......@@ -40,14 +40,14 @@ static pwr_tStatus IoCardInit (
local = (io_sCardLocal *) cp->Local;
if (op->NumberOfChannels != 8 && op->NumberOfChannels != 16 && op->NumberOfChannels != 32)
op->Status = 0;
op->Status = PB_MODULE_STATE_NOTINIT;
if (op->Orientation > op->NumberOfChannels)
op->Status = 0;
op->Status = PB_MODULE_STATE_NOTINIT;
if (op->Status < 1) errh_Info( "Error initializing Pb module Do %s", cp->Name );
if (op->Status < PB_MODULE_STATE_OPERATE) errh_Info( "Error initializing Pb module Do %s", cp->Name );
return 1;
return IO__SUCCESS;
}
......@@ -72,7 +72,7 @@ static pwr_tStatus IoCardWrite (
op = (pwr_sClass_Pb_Do *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE && slave->DisableSlave != 1) {
io_DoPackWord(cp, &data[0], 0);
if (op->NumberOfChannels > 16) io_DoPackWord(cp, &data[1], 1);
......@@ -90,7 +90,7 @@ static pwr_tStatus IoCardWrite (
memcpy(local->output_area + op->OffsetOutputs, &data, op->BytesOfOutput);
}
return 1;
return IO__SUCCESS;
}
......@@ -105,11 +105,11 @@ static pwr_tStatus IoCardClose (
)
{
io_sCardLocal *local;
local = rp->Local;
local = cp->Local;
free ((char *) local);
return 1;
return IO__SUCCESS;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_dp_slave.c
View file @ 04f1d849
......@@ -17,6 +17,7 @@
#include "rt_io_pb_locals.h"
#include "pwr.h"
#include "co_cdh.h"
#include "pwr_baseclasses.h"
#include "rt_gdh.h"
#include "rt_io_base.h"
......@@ -35,82 +36,25 @@ static pwr_tStatus IoRackInit (
io_sRack *rp
)
{
/*
io_sAgentLocal *local_agent;
io_sCardLocal *local_card;
pwr_sClass_Pb_DP_Slave *op;
int i;
pwr_tUInt16 sts;
int fp;
struct timespec rqtp, rmtp;
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;
char name[196];
pwr_tStatus sts;
local_agent = (io_sAgentLocal *) (ap->Local);
fp = local_agent->Pb_fp;
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;
rqtp.tv_sec = 1;
rqtp.tv_nsec = 0;
// Try to initialize slave.
if (op->Status < 1) {
// Three attempts
for(i=0; i<3; i++) {
if (op->AutoConfigure == 1) {
sts = pb_get_slave_cfg(fp,
op->SlaveAddress,
&op->ConfigDataLen,
&op->ConfigData);
}
else {
sts = PB_OK;
}
if (sts == PB_OK) {
sts = pb_download_all(fp,
op->SlaveAddress,
op->WdFact1,
op->WdFact2,
0,
op->PNOIdent,
op->GroupIdent,
op->PrmUserDataLen,
&op->PrmUserData,
op->ConfigDataLen,
&op->ConfigData);
}
if (sts == PB_OK) {
sts = pb_get_slave_info(fp,
op->SlaveAddress,
&op->OffsetInputs,
&op->OffsetOutputs,
&op->BytesOfInput,
&op->BytesOfOutput);
}
if (sts == PB_OK) break;
nanosleep(&rqtp, &rmtp);
}
if (sts != PB_OK) {
errh_Info( "ERROR Init DP slave %s", rp->Name );
return IO__ERRINIDEVICE;
}
}
// Do configuration check and initialize modules.
cardp = rp->cardlist;
......@@ -119,17 +63,10 @@ static pwr_tStatus IoRackInit (
op->NumberModules = 0;
while(cardp) {
if (!cardp->Local) {
local_card = calloc(1, sizeof(*local_card));
cardp->Local = local_card;
}
else
local_card = cardp->Local;
local_card->input_area = (void *) &(op->Inputs);
local_card->output_area = (void *) &(op->Outputs);
local_card->initialized = 0;
errh_Info("Init module %s", cardp->Name);
switch (cardp->Class) {
......@@ -138,8 +75,7 @@ static pwr_tStatus IoRackInit (
dip->OffsetInputs = input_counter;
dip->BytesOfInput = dip->NumberOfChannels / 8;
input_counter += dip->BytesOfInput;
dip->Status = 1;
local_card->initialized = 1;
dip->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Do:
......@@ -147,8 +83,7 @@ static pwr_tStatus IoRackInit (
dop->OffsetOutputs = output_counter;
dop->BytesOfOutput = dop->NumberOfChannels / 8;
output_counter += dop->BytesOfOutput;
dop->Status = 1;
local_card->initialized = 1;
dop->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Ai:
......@@ -156,8 +91,7 @@ static pwr_tStatus IoRackInit (
aip->OffsetInputs = input_counter;
aip->BytesOfInput = aip->NumberOfChannels * aip->BytesPerChannel;
input_counter += aip->BytesOfInput;
aip->Status = 1;
local_card->initialized = 1;
aip->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Ao:
......@@ -165,8 +99,7 @@ static pwr_tStatus IoRackInit (
aop->OffsetOutputs = output_counter;
aop->BytesOfOutput = aop->NumberOfChannels * aop->BytesPerChannel;
output_counter += aop->BytesOfOutput;
aop->Status = 1;
local_card->initialized = 1;
aop->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Ii:
......@@ -174,8 +107,7 @@ static pwr_tStatus IoRackInit (
iip->OffsetInputs = input_counter;
iip->BytesOfInput = iip->NumberOfChannels * iip->BytesPerChannel;
input_counter += iip->BytesOfInput;
iip->Status = 1;
local_card->initialized = 1;
iip->Status = PB_MODULE_STATE_OPERATE;
break;
case pwr_cClass_Pb_Io:
......@@ -183,8 +115,7 @@ static pwr_tStatus IoRackInit (
iop->OffsetOutputs = output_counter;
iop->BytesOfOutput = iop->NumberOfChannels * iop->BytesPerChannel;
output_counter += iop->BytesOfOutput;
iop->Status = 1;
local_card->initialized = 1;
iop->Status = PB_MODULE_STATE_OPERATE;
break;
}
......@@ -192,16 +123,6 @@ static pwr_tStatus IoRackInit (
cardp = cardp->next;
}
if (op->BytesOfInput != input_counter || op->BytesOfOutput != output_counter) {
errh_Info( "Configuration mismatch in DP slave %s", rp->Name);
// op->Status = 0;
// return IO__SUCCESS;
}
op->Status = PB_SLAVE_STATE_STOPPED;
*/
errh_Info( "Init DP slave %s", rp->Name );
return IO__SUCCESS;
}
......@@ -215,53 +136,50 @@ static pwr_tStatus IoRackRead (
io_sRack *rp
)
{
io_sAgentLocal *agent_local;
pwr_sClass_Pb_DP_Slave *op;
pwr_sClass_Pb_Profiboard *mp;
pwr_sClass_Pb_DP_Slave *sp;
int fp;
unsigned char diag;
pwr_tUInt16 sts;
agent_local = (io_sAgentLocal *) (ap->Local);
fp = agent_local->Pb_fp;
fp = ((io_sAgentLocal *) (ap->Local))->Pb_fp;
op = (pwr_sClass_Pb_DP_Slave *) rp->op;
sp = (pwr_sClass_Pb_DP_Slave *) rp->op;
mp = (pwr_sClass_Pb_Profiboard *) ap->op;
if (op->Status > PB_SLAVE_STATE_NOTINIT && op->DisableSlave == 0) {
if (sp->Status > PB_SLAVE_STATE_NOTINIT && mp->Status == PB_MASTER_STATE_OPERATE && sp->DisableSlave != 1 && mp->DisableBus != 1) {
sts = pb_cmi_get_data(fp, ID_DP_STATUS_IMAGE, op->SlaveAddress, 1, &diag);
sts = pb_cmi_get_data(fp, ID_DP_STATUS_IMAGE, sp->SlaveAddress, 1, &diag);
if ((sts != PB_OK) || (diag & 1)) {
op->Status = PB_SLAVE_STATE_STOPPED;
op->ErrorCount++;
if (op->ErrorCount > op->ErrorSoftLimit && op->StallAction >= PB_STALLACTION_RESET) {
memset(&op->Inputs, 0, op->BytesOfInput);
sp->Status = PB_SLAVE_STATE_STOPPED;
sp->ErrorCount++;
if (sp->ErrorCount > sp->ErrorSoftLimit && sp->StallAction >= PB_STALLACTION_RESET) {
memset(&sp->Inputs, 0, sp->BytesOfInput);
}
}
else {
op->Status = PB_SLAVE_STATE_OPERATE;
sp->Status = PB_SLAVE_STATE_OPERATE;
}
if ((op->Status > PB_SLAVE_STATE_STOPPED) && op->BytesOfInput > 0) {
if ((sp->Status > PB_SLAVE_STATE_STOPPED) && sp->BytesOfInput > 0) {
sts = pb_cmi_get_data(fp,
ID_DP_SLAVE_IO_IMAGE,
op->OffsetInputs,
op->BytesOfInput,
&op->Inputs);
sp->OffsetInputs,
sp->BytesOfInput,
&sp->Inputs);
if (sts != PB_OK)
op->ErrorCount++;
sp->ErrorCount++;
else
op->ErrorCount = 0;
sp->ErrorCount = 0;
}
if (op->ErrorCount > op->ErrorHardLimit && op->StallAction >= PB_STALLACTION_BREAK)
if (sp->ErrorCount > sp->ErrorHardLimit && sp->StallAction >= PB_STALLACTION_BREAK)
ctx->Node->EmergBreakTrue = 1;
}
/*
else if (op->Status == PB_SLAVE_STATE_NOTINIT)
IoRackInit(ctx, ap, rp);
*/
return IO__SUCCESS;
}
......@@ -275,28 +193,29 @@ static pwr_tStatus IoRackWrite (
io_sRack *rp
)
{
io_sAgentLocal *agent_local;
pwr_sClass_Pb_DP_Slave *op;
pwr_sClass_Pb_Profiboard *mp;
pwr_sClass_Pb_DP_Slave *sp;
int fp;
pwr_tUInt16 sts;
agent_local = (io_sAgentLocal *) (ap->Local);
fp = agent_local->Pb_fp;
op = (pwr_sClass_Pb_DP_Slave *) rp->op;
fp = ((io_sAgentLocal *) (ap->Local))->Pb_fp;
sp = (pwr_sClass_Pb_DP_Slave *) rp->op;
mp = (pwr_sClass_Pb_Profiboard *) ap->op;
// Write the whole I/O output area from local area
if (op->Status > PB_SLAVE_STATE_NOTINIT && op->DisableSlave == 0) {
if (sp->Status > PB_SLAVE_STATE_NOTINIT && mp->Status == PB_MASTER_STATE_OPERATE && sp->DisableSlave != 1 && mp->DisableBus != 1) {
if (op->BytesOfOutput > 0) {
if (sp->BytesOfOutput > 0) {
sts = pb_cmi_set_data(fp,
ID_DP_SLAVE_IO_IMAGE,
op->OffsetOutputs,
op->BytesOfOutput,
&op->Outputs);
sp->OffsetOutputs,
sp->BytesOfOutput,
&sp->Outputs);
if (sts != PB_OK) op->ErrorCount++;
if (sts != PB_OK) sp->ErrorCount++;
}
}
......@@ -313,10 +232,9 @@ static pwr_tStatus IoRackClose (
io_sRack *rp
)
{
return 1;
return IO__SUCCESS;
}
/*----------------------------------------------------------------------------*\
Every method to be exported to the workbench should be registred here.
......
src/lib/rt/src/os_linux/rt_io_m_pb_ii.c
View file @ 04f1d849
......@@ -43,12 +43,12 @@ static pwr_tStatus IoCardInit (
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
return IO__SUCCESS;
}
if (op->Status < 1) errh_Info( "Error initializing Pb module Ii %s", cp->Name );
if (op->Status < PB_MODULE_STATE_OPERATE) errh_Info( "Error initializing Pb module Ii %s", cp->Name );
return 1;
return IO__SUCCESS;
}
......@@ -81,7 +81,7 @@ static pwr_tStatus IoCardRead (
op = (pwr_sClass_Pb_Ii *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE && slave->DisableSlave != 1) {
for (i=0; i<cp->ChanListSize; i++) {
......@@ -105,6 +105,20 @@ static pwr_tStatus IoCardRead (
*(pwr_tInt32 *) chanp->vbp = data32;
}
}
else if (op->BytesPerChannel == 3) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
udata32 = 0;
memcpy(&udata32, local->input_area + op->OffsetInputs + 3*i, 3);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata32 = swap32(udata32);
*(pwr_tInt32 *) chanp->vbp = (pwr_tInt32) udata32;
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
data32 = 0;
memcpy(&data32, local->input_area + op->OffsetInputs + 3*i, 3);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
*(pwr_tInt32 *) chanp->vbp = data32;
}
}
else if (op->BytesPerChannel == 2) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
memcpy(&udata16, local->input_area + op->OffsetInputs + 2*i, 2);
......@@ -131,7 +145,7 @@ static pwr_tStatus IoCardRead (
}
}
return 1;
return IO__SUCCESS;
}
......@@ -146,11 +160,11 @@ static pwr_tStatus IoCardClose (
)
{
io_sCardLocal *local;
local = rp->Local;
local = cp->Local;
free ((char *) local);
return 1;
return IO__SUCCESS;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_io.c
View file @ 04f1d849
......@@ -41,14 +41,14 @@ static pwr_tStatus IoCardInit (
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
return IO__SUCCESS;
}
if (op->Status < 1) {
if (op->Status < PB_MODULE_STATE_OPERATE) {
errh_Info( "Error initializing Pb module Io %s", cp->Name );
}
return 1;
return IO__SUCCESS;
}
......@@ -78,7 +78,7 @@ static pwr_tStatus IoCardWrite (
op = (pwr_sClass_Pb_Io *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
if (op->Status >= PB_MODULE_STATE_OPERATE && slave->DisableSlave != 1) {
for (i=0; i<cp->ChanListSize; i++) {
......@@ -96,6 +96,10 @@ static pwr_tStatus IoCardWrite (
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
memcpy(local->output_area + op->OffsetOutputs + 4*i, &data32, 4);
}
else if (op->BytesPerChannel == 3) {
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
memcpy(local->output_area + op->OffsetOutputs + 3*i, &data32, 3);
}
else if (op->BytesPerChannel == 2) {
data16 = (pwr_tInt16) data32;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data16 = swap16(data16);
......@@ -107,7 +111,7 @@ static pwr_tStatus IoCardWrite (
}
}
}
return 1;
return IO__SUCCESS;
}
......@@ -122,11 +126,11 @@ static pwr_tStatus IoCardClose (
)
{
io_sCardLocal *local;
local = rp->Local;
local = cp->Local;
free ((char *) local);
return 1;
return IO__SUCCESS;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_profiboard.c
View file @ 04f1d849
......@@ -28,6 +28,8 @@
#include "rt_io_profiboard.h"
static int count;
static pwr_tStatus IoAgentInit (
io_tCtx ctx,
io_sAgent *ap
......@@ -265,7 +267,7 @@ static pwr_tStatus init_dp_slave (
op->Status = PB_SLAVE_STATE_NOTINIT;
errh_Info( "Config of Profibus DP slave %s", name );
errh_Info( "Config of Profibus DP Slave %s", name );
// Try to initialize slave, make three attempts before we give up
......@@ -328,13 +330,14 @@ static pwr_tStatus IoAgentInit (
pwr_tStatus status;
io_sAgentLocal *local;
unsigned char devname[25];
struct timespec rqtp, rmtp;
int i;
char ok;
pwr_tObjid slave_objid;
pwr_tClassId slave_class;
op = (pwr_sClass_Pb_Profiboard *) ap->op;
errh_Info( "Config of Profibus DP Master %s", ap->Name );
count=0;
/* Allocate area for local data structure */
ap->Local = calloc(1, sizeof(io_sAgentLocal));
......@@ -344,8 +347,7 @@ static pwr_tStatus IoAgentInit (
}
local = (io_sAgentLocal *) ap->Local;
if (op->DisableBus != 1) {
op = (pwr_sClass_Pb_Profiboard *) ap->op;
/* Open Pb driver */
sprintf(devname, "/dev/pbus%1d", op->BusNumber);
......@@ -359,6 +361,21 @@ static pwr_tStatus IoAgentInit (
return IO__ERRDEVICE;
}
/* If this is not the Profibus I/O process, return */
if ((op->Process & io_mProcess_Profibus) && (ctx->Process != io_mProcess_Profibus)) {
errh_Info( "Init template I/O agent for Profibus DP Master %s, %d", ap->Name, ctx->Process );
return IO__SUCCESS;
}
errh_Info( "Config of Profibus DP Master %s", ap->Name);
if (op->DisableBus != 1) {
ok = FALSE;
while (!ok) {
/* Initialize CMI */
sts = pb_cmi_init(local->Pb_fp);
......@@ -390,6 +407,14 @@ static pwr_tStatus IoAgentInit (
return IO__ERRINIDEVICE;
}
/* Set stalltime */
sts = pb_set_stalltime(local->Pb_fp, op->StallTime);
if (sts != PB_OK) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "set stalltime");
return IO__ERRINIDEVICE;
}
/* Move to STOP mode */
sts = act_param_loc(local->Pb_fp, op, DP_OP_MODE_STOP);
if (sts != PB_OK) {
......@@ -433,6 +458,22 @@ static pwr_tStatus IoAgentInit (
return IO__ERRINIDEVICE;
}
ok = TRUE;
for (i=0; i<4; i++) {
if (ok) {
sts = pb_cmi_poll(local->Pb_fp, NULL, NULL, NULL);
if (sts == PB_DEVICE_ERROR) {
errh_Info("Init problems, Reconfig - %d", i);
ok = FALSE;
}
rqtp.tv_sec = 0;
rqtp.tv_nsec = 200000000;
nanosleep(&rqtp, &rmtp);
}
}
}
}
return IO__SUCCESS;
......@@ -450,6 +491,7 @@ static pwr_tStatus IoAgentRead (
io_sAgentLocal *local;
pwr_sClass_Pb_Profiboard *op;
pwr_tUInt16 sts;
static int count;
local = (io_sAgentLocal *) ap->Local;
op = (pwr_sClass_Pb_Profiboard *) ap->op;
......@@ -458,6 +500,11 @@ static pwr_tStatus IoAgentRead (
Make a poll to see if there are diagnostics, the answer also tell us
if there are any hardware faults. In that case, make a reset and a new init. */
count++;
if ((op->Process & io_mProcess_Profibus) && (ctx->Process != io_mProcess_Profibus))
return IO__SUCCESS;
if (op->DisableBus != 1) {
switch (op->Status) {
......@@ -478,7 +525,7 @@ static pwr_tStatus IoAgentRead (
default:
op->Status = PB_MASTER_STATE_NOTINIT;
errh_Info( "Reconfig of Profibus DP Master %s", ap->Name );
errh_Info( "Reconfig of Profibus DP Master %s - %d", ap->Name, count );
IoAgentClose(ctx, ap);
IoAgentInit(ctx, ap);
break;
......
src/lib/rt/src/os_linux/rt_io_pb_locals.h
View file @ 04f1d849
#define IO_MAXCHAN 32
#define PB_MODULE_STATE_NOTINIT 0
#define PB_MODULE_STATE_OPERATE 1
#define PB_SLAVE_STATE_NOTINIT 0
#define PB_SLAVE_STATE_STOPPED 1
#define PB_SLAVE_STATE_OPERATE 2
......@@ -35,7 +38,6 @@ typedef struct {
} io_sRackLocal;
typedef struct {
int initialized;
void *input_area;
void *output_area;
int scancount[IO_MAXCHAN];
......
src/lib/rt/src/os_linux/rt_io_profiboard.c
View file @ 04f1d849
......@@ -70,6 +70,19 @@ pwr_tUInt16 pb_cmi_write(int fp,
return PB_DEVICE_ERROR;
}
pwr_tUInt16 pb_set_stalltime(int fp, unsigned short t)
{
int sts;
sts = ioctl(fp, PB_IOCTL_SET_STALLTIME, (char *) &t);
if (sts == 0)
return PB_OK;
else
return PB_DEVICE_ERROR;
}
pwr_tUInt16 pb_cmi_read(int fp,
T_PROFI_SERVICE_DESCR *sdb_ptr,
......
src/lib/rt/src/os_linux/rt_io_profiboard.h
View file @ 04f1d849
......@@ -23,6 +23,9 @@ pwr_tUInt16 pb_cmi_write(int fp,
void *data_ptr,
pwr_tUInt16 data_len);
extern
pwr_tUInt16 pb_set_stalltime(int fp, unsigned short t);
extern
pwr_tUInt16 pb_cmi_read(int fp,
T_PROFI_SERVICE_DESCR *sdb_ptr,
......
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