/*----------------------------------------------------------------------------*\ 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; }