/*----------------------------------------------------------------------------*\
Init method for the Pb module
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardInit(
io_tCtx ctx, io_sAgent* ap, io_sRack* rp, io_sCard* cp)
{
io_sFDLCardLocal* local;
pwr_sClass_Pb_FDL_DataTransfer* op = (pwr_sClass_Pb_FDL_DataTransfer*)cp->op;
unsigned int input_area_offset = 0;
unsigned int input_area_chansize = 0;
unsigned int output_area_offset = 0;
unsigned int output_area_chansize = 0;
local = (io_sFDLCardLocal*)calloc(1, sizeof(io_sFDLCardLocal));
cp->Local = local;
/* Initialize remote address structure */
local->byte_ordering = ((pwr_sClass_Pb_FDL_SAP*)rp->op)->ByteOrdering;
io_bus_card_init(ctx, cp, &input_area_offset, &input_area_chansize,
&output_area_offset, &output_area_chansize, local->byte_ordering,
io_eAlignment_Packed);
local->input_area_size = input_area_offset + input_area_chansize;
local->output_area_size = output_area_offset + output_area_chansize;
if (local->input_area_size > 0)
local->input_area = calloc(1, local->input_area_size);
if (local->output_area_size > 0)
local->output_area = calloc(1, local->output_area_size);
errh_Info("Init of Profibus FDL Data transfer '%s'", cp->Name);
op->Status = PB__NORMAL;
return IO__SUCCESS;
}
static pwr_tStatus mb_init_channels( io_tCtx ctx, io_sAgent *ap, io_sRack *rp)
{
io_sServerModuleLocal *local_card;
io_sCard *cardp;
io_sServerLocal *local;
pwr_sClass_Modbus_TCP_Server *op;
char name[196];
pwr_tStatus sts;
io_sChannel *chanp;
int i;
sts = gdh_ObjidToName(rp->Objid, (char *) &name, sizeof(name), cdh_mNName);
op = (pwr_sClass_Modbus_TCP_Server *) rp->op;
local = rp->Local;
/* Create socket, store in local struct */
/* 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));
cardp->Local = local_card;
local_card->input_area = (void *) &(op->Inputs) + input_area_offset +
input_area_chansize;
local_card->output_area = (void *) &(op->Outputs) + output_area_offset +
output_area_chansize;
io_bus_card_init( ctx, cardp, &input_area_offset, &input_area_chansize,
&output_area_offset, &output_area_chansize,
pwr_eByteOrderingEnum_BigEndian);
for (i = 0; i < cardp->ChanListSize; i++) {
chanp = &cardp->chanlist[i];
switch (chanp->ChanClass) {
case pwr_cClass_ChanDi: {
pwr_sClass_ChanDi *chan_di = (pwr_sClass_ChanDi *) chanp->cop;
if (local_card->di_size == 0)
local_card->di_offset = chanp->offset;
if (chan_di->Number == 0 || local_card->di_size == 0)
local_card->di_size += GetChanSize(chan_di->Representation);
break;
}
case pwr_cClass_ChanDo: {
pwr_sClass_ChanDo *chan_do = (pwr_sClass_ChanDo *) chanp->cop;
if (local_card->do_size == 0)
local_card->do_offset = chanp->offset;
if (chan_do->Number == 0 || local_card->do_size == 0)
local_card->do_size += GetChanSize(chan_do->Representation);
break;
}
case pwr_cClass_ChanD: {
pwr_sClass_ChanD *chan_d = (pwr_sClass_ChanD *) chanp->cop;
if ( chan_d->Type == pwr_eDChanTypeEnum_Di) {
if (local_card->di_size == 0)
local_card->di_offset = chanp->offset;
if (chan_d->Number == 0 || local_card->di_size == 0)
local_card->di_size += GetChanSize(chan_d->Representation);
}
else {
if (local_card->do_size == 0)
local_card->do_offset = chanp->offset;
if (chan_d->Number == 0 || local_card->do_size == 0)
local_card->do_size += GetChanSize(chan_d->Representation);
}
break;
}
}
}
local_card->input_size = input_area_offset + input_area_chansize -
prev_input_area_offset;
local_card->output_size = output_area_offset + output_area_chansize -
prev_output_area_offset;
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;
}
/*----------------------------------------------------------------------------*\
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;
}
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;
}
static pwr_tStatus IoCardInit( io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp)
{
io_sLocalSPI_Slave *local;
pwr_sClass_SPI_Slave *op = (pwr_sClass_SPI_Slave *)cp->op;
unsigned int input_area_offset = 0;
unsigned int input_area_chansize = 0;
unsigned int output_area_offset = 0;
unsigned int output_area_chansize = 0;
int sts;
unsigned char mode;
unsigned char lsb;
unsigned char bits;
__u32 speed;
local = (io_sLocalSPI_Slave *) calloc( 1, sizeof(io_sLocalSPI_Slave));
cp->Local = local;
op->Status = IOM__UDP_INIT;
local->fd = open( op->Device, O_RDWR);
if ( local->fd < 0) {
errh_Error( "SPI Slave, unable to open device %s, '%s'", op->Device, cp->Name);
op->Status = IOM__SPI_DEVICE;
return IO__INITFAIL;
}
/* Set mode */
switch ( op->Mode) {
case pwr_eSPI_ModeEnum_Mode0:
mode = SPI_MODE_0;
break;
case pwr_eSPI_ModeEnum_Mode1:
mode = SPI_MODE_1;
break;
case pwr_eSPI_ModeEnum_Mode2:
mode = SPI_MODE_2;
break;
case pwr_eSPI_ModeEnum_Mode3:
mode = SPI_MODE_3;
break;
default:
errh_Error( "SPI Slave, invalid mode, '%s'", errno, cp->Name);
op->Status = IOM__SPI_INIT;
return IO__INITFAIL;
}
sts = ioctl( local->fd, SPI_IOC_WR_MODE, &mode);
if ( sts < 0) {
errh_Error( "SPI Slave, unable to set mode, init error errno %d, '%s'", errno, cp->Name);
op->Status = IOM__SPI_INIT;
return IO__INITFAIL;
}
/* Set LSB first encoding */
if ( op->LSB_First)
lsb = 1;
else
lsb = 0;
sts = ioctl( local->fd, SPI_IOC_WR_LSB_FIRST, &lsb);
if ( sts < 0) {
errh_Error( "SPI Slave, unable to set LSB first, init error errno %d, '%s'", errno, cp->Name);
op->Status = IOM__SPI_INIT;
return IO__INITFAIL;
}
/* Set bits per word */
bits = op->BitsPerWord;
sts = ioctl( local->fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
if ( sts < 0) {
errh_Error( "SPI Slave, unable to set Bits per word, init error errno %d, '%s'", errno, cp->Name);
op->Status = IOM__SPI_INIT;
return IO__INITFAIL;
}
/* Set Max speed */
speed = op->MaxSpeed;
if ( speed != 0) {
sts = ioctl( local->fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
if ( sts < 0) {
errh_Error( "SPI Slave, unable to set Max speed, init error errno %d, '%s'", errno, cp->Name);
op->Status = IOM__SPI_INIT;
return IO__INITFAIL;
}
}
local->byte_ordering = op->ByteOrdering;
io_bus_card_init( ctx, cp, &input_area_offset, &input_area_chansize,
&output_area_offset, &output_area_chansize, local->byte_ordering,
io_eAlignment_Packed);
local->input_area_size = input_area_offset + input_area_chansize;
local->output_area_size = output_area_offset + output_area_chansize;
op->InputAreaSize = local->input_area_size;
op->OutputAreaSize = local->output_area_size;
if ( local->input_area_size > 0)
local->input_area = calloc( 1, local->input_area_size);
if ( local->output_area_size > 0)
local->output_area = calloc( 1, local->output_area_size);
errh_Info( "Init of SPI Slave '%s'", cp->Name);
op->Status = IOM__SPI_NORMAL;
return IO__SUCCESS;
}