/**
********************************************************************************
\brief Constructor
Constructs a POWERLINK object.
\param pMainWindow_p pointer to main window
\param uiNodeId_p node ID of the POWERLINK node
\param devName_p device name of the network interface
*******************************************************************************/
EplApi::EplApi(MainWindow *pMainWindow_p, unsigned int uiNodeId_p, QString devName_p)
{
const char* sHostname = HOSTNAME;
tEplKernel EplRet;
EplState* pEplState;
EplOutput* pEplOutput;
EplInput* pEplInput;
EplCnState* pEplCnState;
char devName[256];
pEplState = pMainWindow_p->getEplStateWidget();
pEplOutput = pMainWindow_p->getEplOutputWidget();
pEplInput = pMainWindow_p->getEplInputWidget();
pEplCnState = pMainWindow_p->getEplCnStateWidget();
pEplProcessThread = new EplProcessThread;
QObject::connect(pEplProcessThread, SIGNAL(eplStatusChanged(int)),
pEplState, SLOT(setEplStatusLed(int)));
QObject::connect(pEplProcessThread, SIGNAL(nmtStateChanged(const QString&)),
pEplState, SLOT(setNmtStateText(const QString &)));
QObject::connect(pEplProcessThread, SIGNAL(nodeAppeared(int)),
pEplInput, SLOT(addNode(int)));
QObject::connect(pEplProcessThread, SIGNAL(allNodesRemoved()),
pEplInput, SLOT(removeAllNodes()));
QObject::connect(pEplProcessThread, SIGNAL(nodeDisappeared(int)),
pEplInput, SLOT(removeNode(int)));
QObject::connect(pEplProcessThread, SIGNAL(nodeAppeared(int)),
pEplOutput, SLOT(addNode(int)));
QObject::connect(pEplProcessThread, SIGNAL(nodeDisappeared(int)),
pEplOutput, SLOT(removeNode(int)));
QObject::connect(pEplProcessThread, SIGNAL(allNodesRemoved()),
pEplOutput, SLOT(removeAllNodes()));
QObject::connect(pEplProcessThread, SIGNAL(nodeAppeared(int)),
pEplCnState, SLOT(addNode(int)));
QObject::connect(pEplProcessThread, SIGNAL(nodeDisappeared(int)),
pEplCnState, SLOT(removeNode(int)));
QObject::connect(pEplProcessThread, SIGNAL(allNodesRemoved()),
pEplCnState, SLOT(removeAllNodes()));
QObject::connect(pEplProcessThread, SIGNAL(nodeStatusChanged(int, int)),
pEplCnState, SLOT(setState(int, int)));
pEplDataInOutThread = new EplDataInOutThread;
QObject::connect(pEplDataInOutThread, SIGNAL(processImageOutChanged(unsigned int, unsigned int)),
pEplOutput, SLOT(setValue(unsigned int, unsigned int)));
QObject::connect(pEplDataInOutThread, SIGNAL(processImageInChanged(unsigned int, unsigned int)),
pEplInput, SLOT(setLeds(unsigned int, unsigned int)));
EPL_MEMSET(&EplApiInitParam, 0, sizeof (EplApiInitParam));
EplApiInitParam.m_uiSizeOfStruct = sizeof (EplApiInitParam);
EplApiInitParam.m_uiNodeId = uiNodeId_p;
EplApiInitParam.m_dwIpAddress = (IP_ADDR & 0xFFFFFF00) | EplApiInitParam.m_uiNodeId;
EplApiInitParam.m_fAsyncOnly = FALSE;
EplApiInitParam.m_dwFeatureFlags = -1;
EplApiInitParam.m_dwCycleLen = CYCLE_LEN; // required for error detection
EplApiInitParam.m_uiIsochrTxMaxPayload = 256; // const
EplApiInitParam.m_uiIsochrRxMaxPayload = 256; // const
EplApiInitParam.m_dwPresMaxLatency = 50000; // const; only required for IdentRes
EplApiInitParam.m_uiPreqActPayloadLimit = 36; // required for initialisation (+28 bytes)
EplApiInitParam.m_uiPresActPayloadLimit = 36; // required for initialisation of Pres frame (+28 bytes)
EplApiInitParam.m_dwAsndMaxLatency = 150000; // const; only required for IdentRes
EplApiInitParam.m_uiMultiplCycleCnt = 0; // required for error detection
EplApiInitParam.m_uiAsyncMtu = 1500; // required to set up max frame size
EplApiInitParam.m_uiPrescaler = 2; // required for sync
EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
EplApiInitParam.m_dwWaitSocPreq = 150000;
EplApiInitParam.m_dwDeviceType = -1; // NMT_DeviceType_U32
EplApiInitParam.m_dwVendorId = -1; // NMT_IdentityObject_REC.VendorId_U32
EplApiInitParam.m_dwProductCode = -1; // NMT_IdentityObject_REC.ProductCode_U32
EplApiInitParam.m_dwRevisionNumber = -1; // NMT_IdentityObject_REC.RevisionNo_U32
EplApiInitParam.m_dwSerialNumber = -1; // NMT_IdentityObject_REC.SerialNo_U32
EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
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 = pEplProcessThread->getEventCbFunc();
/* 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));
#ifdef CONFIG_POWERLINK_USERSTACK
EplApiInitParam.m_HwParam.m_pszDevName = devName_p.toAscii().data();
EplApiInitParam.m_pfnObdInitRam = EplObdInitRam;
EplApiInitParam.m_pfnCbSync = pEplDataInOutThread->getSyncCbFunc();
#else
// Sync call back function not required for init from user space
EplApiInitParam.m_pfnCbSync = NULL;
#endif
// init EPL
EplRet = EplApiInitialize(&EplApiInitParam);
if(EplRet != kEplSuccessful)
{
printf("%s: EplApiInitialize() failed\n", __FUNCTION__);
QMessageBox::critical(0, "POWERLINK demo",
QString("Initialization of openPOWERLINK Stack failed.\n") +
"Error code: 0x"+ QString::number(EplRet, 16) +
"\nThe most common error source are an unsupported Ethernet controller or the kernel module is not loaded."
"\nFor further information please consult the manual.");
goto Exit;
}
#ifdef CONFIG_POWERLINK_USERSTACK
EplRet = EplApiSetCdcFilename(pszCdcFilename_g);
if(EplRet != kEplSuccessful)
{
goto Exit;
}
#endif
EplRet = pEplDataInOutThread->SetupProcessImage();
if (EplRet != kEplSuccessful)
{
printf("%s: pEplDataInOutThread->SetupProcessImage() failed\n", __FUNCTION__);
goto Exit;
}
printf("Setup Process Image Successfull\n");
// start the EPL stack
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
// start process thread
pEplProcessThread->start();
#ifndef CONFIG_POWERLINK_USERSTACK
// start data in out thread
pEplDataInOutThread->start();
#endif
Exit:
printf("%s: returns 0x%X\n", __FUNCTION__, EplRet);
}
static int EplLinIoctl(struct inode *pDeviceFile_p, // information about the device to open
struct file *pInstance_p, // information about driver instance
unsigned int uiIoctlCmd_p, // Ioctl command to execute
unsigned long ulArg_p) // Ioctl command specific argument/parameter
{
tEplKernel EplRet;
int iErr;
int iRet;
// TRACE1("EPL: + EplLinIoctl (uiIoctlCmd_p=%d)...\n", uiIoctlCmd_p);
iRet = -EINVAL;
switch (uiIoctlCmd_p) {
// ----------------------------------------------------------
case EPLLIN_CMD_INITIALIZE:
{
tEplApiInitParam EplApiInitParam;
iErr =
copy_from_user(&EplApiInitParam,
(const void *)ulArg_p,
sizeof(EplApiInitParam));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
EplApiInitParam.m_pfnCbEvent = EplLinCbEvent;
EplApiInitParam.m_pfnCbSync = EplLinCbSync;
EplRet = EplApiInitialize(&EplApiInitParam);
uiEplState_g = EPL_STATE_RUNNING;
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_SHUTDOWN:
{ // shutdown the threads
// pass control to sync kernel thread, but signal termination
atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
wake_up_interruptible(&WaitQueueCbSync_g);
wake_up_interruptible(&WaitQueuePI_In_g);
// pass control to event queue kernel thread
atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
wake_up_interruptible(&WaitQueueCbEvent_g);
if (uiEplState_g == EPL_STATE_RUNNING) { // post NmtEventSwitchOff
EplRet =
EplApiExecNmtCommand(kEplNmtEventSwitchOff);
}
iRet = 0;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_READ_LOCAL_OBJECT:
{
tEplLinLocalObject LocalObject;
void *pData;
iErr =
copy_from_user(&LocalObject, (const void *)ulArg_p,
sizeof(LocalObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if ((LocalObject.m_pData == NULL)
|| (LocalObject.m_uiSize == 0)) {
iRet = (int)kEplApiInvalidParam;
goto Exit;
}
pData = vmalloc(LocalObject.m_uiSize);
if (pData == NULL) { // no memory available
iRet = -ENOMEM;
goto Exit;
}
EplRet =
EplApiReadLocalObject(LocalObject.m_uiIndex,
LocalObject.m_uiSubindex,
pData, &LocalObject.m_uiSize);
if (EplRet == kEplSuccessful) {
iErr =
copy_to_user(LocalObject.m_pData, pData,
LocalObject.m_uiSize);
vfree(pData);
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
// return actual size (LocalObject.m_uiSize)
iErr = put_user(LocalObject.m_uiSize,
(unsigned int *)(ulArg_p +
(unsigned long)
&LocalObject.
m_uiSize -
(unsigned long)
&LocalObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
} else {
vfree(pData);
}
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_WRITE_LOCAL_OBJECT:
{
tEplLinLocalObject LocalObject;
void *pData;
iErr =
copy_from_user(&LocalObject, (const void *)ulArg_p,
sizeof(LocalObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if ((LocalObject.m_pData == NULL)
|| (LocalObject.m_uiSize == 0)) {
iRet = (int)kEplApiInvalidParam;
goto Exit;
}
pData = vmalloc(LocalObject.m_uiSize);
if (pData == NULL) { // no memory available
iRet = -ENOMEM;
goto Exit;
}
iErr =
copy_from_user(pData, LocalObject.m_pData,
LocalObject.m_uiSize);
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
EplRet =
EplApiWriteLocalObject(LocalObject.m_uiIndex,
LocalObject.m_uiSubindex,
pData, LocalObject.m_uiSize);
vfree(pData);
iRet = (int)EplRet;
break;
}
case EPLLIN_CMD_READ_OBJECT:
{
tEplLinSdoObject SdoObject;
void *pData;
tEplLinSdoBufHeader *pBufHeader;
tEplSdoComConHdl *pSdoComConHdl;
iErr =
copy_from_user(&SdoObject, (const void *)ulArg_p,
sizeof(SdoObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if ((SdoObject.m_le_pData == NULL)
|| (SdoObject.m_uiSize == 0)) {
iRet = (int)kEplApiInvalidParam;
goto Exit;
}
pBufHeader =
(tEplLinSdoBufHeader *)
vmalloc(sizeof(tEplLinSdoBufHeader) +
SdoObject.m_uiSize);
if (pBufHeader == NULL) { // no memory available
iRet = -ENOMEM;
goto Exit;
}
// initiate temporary buffer
pBufHeader->m_pUserArg = SdoObject.m_pUserArg; // original user argument pointer
pBufHeader->m_pData = SdoObject.m_le_pData; // original data pointer from app
pData = pBufHeader + sizeof(tEplLinSdoBufHeader);
if (SdoObject.m_fValidSdoComConHdl != FALSE) {
pSdoComConHdl = &SdoObject.m_SdoComConHdl;
} else {
pSdoComConHdl = NULL;
}
EplRet =
EplApiReadObject(pSdoComConHdl,
SdoObject.m_uiNodeId,
SdoObject.m_uiIndex,
SdoObject.m_uiSubindex, pData,
&SdoObject.m_uiSize,
SdoObject.m_SdoType, pBufHeader);
// return actual SDO handle (SdoObject.m_SdoComConHdl)
iErr = put_user(SdoObject.m_SdoComConHdl,
(unsigned int *)(ulArg_p +
(unsigned long)
&SdoObject.
m_SdoComConHdl -
(unsigned long)
&SdoObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if (EplRet == kEplSuccessful) {
iErr =
copy_to_user(SdoObject.m_le_pData, pData,
SdoObject.m_uiSize);
vfree(pBufHeader);
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
// return actual size (SdoObject.m_uiSize)
iErr = put_user(SdoObject.m_uiSize,
(unsigned int *)(ulArg_p +
(unsigned long)
&SdoObject.
m_uiSize -
(unsigned long)
&SdoObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
} else if (EplRet != kEplApiTaskDeferred) { // error ocurred
vfree(pBufHeader);
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
}
iRet = (int)EplRet;
break;
}
case EPLLIN_CMD_WRITE_OBJECT:
{
tEplLinSdoObject SdoObject;
void *pData;
tEplLinSdoBufHeader *pBufHeader;
tEplSdoComConHdl *pSdoComConHdl;
iErr =
copy_from_user(&SdoObject, (const void *)ulArg_p,
sizeof(SdoObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if ((SdoObject.m_le_pData == NULL)
|| (SdoObject.m_uiSize == 0)) {
iRet = (int)kEplApiInvalidParam;
goto Exit;
}
pBufHeader =
(tEplLinSdoBufHeader *)
vmalloc(sizeof(tEplLinSdoBufHeader) +
SdoObject.m_uiSize);
if (pBufHeader == NULL) { // no memory available
iRet = -ENOMEM;
goto Exit;
}
// initiate temporary buffer
pBufHeader->m_pUserArg = SdoObject.m_pUserArg; // original user argument pointer
pBufHeader->m_pData = SdoObject.m_le_pData; // original data pointer from app
pData = pBufHeader + sizeof(tEplLinSdoBufHeader);
iErr =
copy_from_user(pData, SdoObject.m_le_pData,
SdoObject.m_uiSize);
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if (SdoObject.m_fValidSdoComConHdl != FALSE) {
pSdoComConHdl = &SdoObject.m_SdoComConHdl;
} else {
pSdoComConHdl = NULL;
}
EplRet =
EplApiWriteObject(pSdoComConHdl,
SdoObject.m_uiNodeId,
SdoObject.m_uiIndex,
SdoObject.m_uiSubindex, pData,
SdoObject.m_uiSize,
SdoObject.m_SdoType, pBufHeader);
// return actual SDO handle (SdoObject.m_SdoComConHdl)
iErr = put_user(SdoObject.m_SdoComConHdl,
(unsigned int *)(ulArg_p +
(unsigned long)
&SdoObject.
m_SdoComConHdl -
(unsigned long)
&SdoObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if (EplRet != kEplApiTaskDeferred) { // succeeded or error ocurred, but task not deferred
vfree(pBufHeader);
}
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_FREE_SDO_CHANNEL:
{
// forward SDO handle to EPL stack
EplRet =
EplApiFreeSdoChannel((tEplSdoComConHdl) ulArg_p);
iRet = (int)EplRet;
break;
}
#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
// ----------------------------------------------------------
case EPLLIN_CMD_MN_TRIGGER_STATE_CHANGE:
{
tEplLinNodeCmdObject NodeCmdObject;
iErr =
copy_from_user(&NodeCmdObject,
(const void *)ulArg_p,
sizeof(NodeCmdObject));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
EplRet =
EplApiMnTriggerStateChange(NodeCmdObject.m_uiNodeId,
NodeCmdObject.
m_NodeCommand);
iRet = (int)EplRet;
break;
}
#endif
// ----------------------------------------------------------
case EPLLIN_CMD_GET_EVENT:
{
tEplLinEvent Event;
// save event structure
iErr =
copy_from_user(&Event, (const void *)ulArg_p,
sizeof(Event));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
// save return code from application's event callback function
RetCbEvent_g = Event.m_RetCbEvent;
if (RetCbEvent_g == kEplShutdown) {
// pass control to event queue kernel thread, but signal termination
atomic_set(&AtomicEventState_g,
EVENT_STATE_TERM);
wake_up_interruptible(&WaitQueueCbEvent_g);
// exit with error -> EplApiProcess() will leave the infinite loop
iRet = 1;
goto Exit;
}
// pass control to event queue kernel thread
atomic_set(&AtomicEventState_g, EVENT_STATE_IOCTL);
wake_up_interruptible(&WaitQueueCbEvent_g);
// fall asleep itself in own wait queue
iErr = wait_event_interruptible(WaitQueueProcess_g,
(atomic_read
(&AtomicEventState_g)
== EVENT_STATE_READY)
||
(atomic_read
(&AtomicEventState_g)
== EVENT_STATE_TERM));
if (iErr != 0) { // waiting was interrupted by signal
// pass control to event queue kernel thread, but signal termination
atomic_set(&AtomicEventState_g,
EVENT_STATE_TERM);
wake_up_interruptible(&WaitQueueCbEvent_g);
// exit with this error -> EplApiProcess() will leave the infinite loop
iRet = iErr;
goto Exit;
} else if (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM) { // termination in progress
// pass control to event queue kernel thread, but signal termination
wake_up_interruptible(&WaitQueueCbEvent_g);
// exit with this error -> EplApiProcess() will leave the infinite loop
iRet = 1;
goto Exit;
}
// copy event to user space
iErr =
copy_to_user(Event.m_pEventType, &EventType_g,
sizeof(EventType_g));
if (iErr != 0) { // not all data could be copied
iRet = -EIO;
goto Exit;
}
// $$$ d.k. perform SDO event processing
if (EventType_g == kEplApiEventSdo) {
void *pData;
tEplLinSdoBufHeader *pBufHeader;
pBufHeader =
(tEplLinSdoBufHeader *) pEventArg_g->m_Sdo.
m_pUserArg;
pData =
pBufHeader + sizeof(tEplLinSdoBufHeader);
if (pEventArg_g->m_Sdo.m_SdoAccessType ==
kEplSdoAccessTypeRead) {
// copy read data to user space
iErr =
copy_to_user(pBufHeader->m_pData,
pData,
pEventArg_g->m_Sdo.
m_uiTransferredByte);
if (iErr != 0) { // not all data could be copied
iRet = -EIO;
goto Exit;
}
}
pEventArg_g->m_Sdo.m_pUserArg =
pBufHeader->m_pUserArg;
vfree(pBufHeader);
}
iErr =
copy_to_user(Event.m_pEventArg, pEventArg_g,
min(sizeof(tEplApiEventArg),
Event.m_uiEventArgSize));
if (iErr != 0) { // not all data could be copied
iRet = -EIO;
goto Exit;
}
// return to EplApiProcess(), which will call the application's event callback function
iRet = 0;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_PI_SETUP:
{
EplRet = EplApiProcessImageSetup();
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_PI_IN:
{
tEplApiProcessImage ProcessImageIn;
// save process image structure
iErr =
copy_from_user(&ProcessImageIn,
(const void *)ulArg_p,
sizeof(ProcessImageIn));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
// pass control to event queue kernel thread
atomic_set(&AtomicSyncState_g, EVENT_STATE_IOCTL);
// fall asleep itself in own wait queue
iErr = wait_event_interruptible(WaitQueuePI_In_g,
(atomic_read
(&AtomicSyncState_g) ==
EVENT_STATE_READY)
||
(atomic_read
(&AtomicSyncState_g) ==
EVENT_STATE_TERM));
if (iErr != 0) { // waiting was interrupted by signal
// pass control to sync kernel thread, but signal termination
atomic_set(&AtomicSyncState_g,
EVENT_STATE_TERM);
wake_up_interruptible(&WaitQueueCbSync_g);
// exit with this error -> application will leave the infinite loop
iRet = iErr;
goto Exit;
} else if (atomic_read(&AtomicSyncState_g) == EVENT_STATE_TERM) { // termination in progress
// pass control to sync kernel thread, but signal termination
wake_up_interruptible(&WaitQueueCbSync_g);
// exit with this error -> application will leave the infinite loop
iRet = 1;
goto Exit;
}
// exchange process image
EplRet = EplApiProcessImageExchangeIn(&ProcessImageIn);
// return to EplApiProcessImageExchangeIn()
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_PI_OUT:
{
tEplApiProcessImage ProcessImageOut;
// save process image structure
iErr =
copy_from_user(&ProcessImageOut,
(const void *)ulArg_p,
sizeof(ProcessImageOut));
if (iErr != 0) {
iRet = -EIO;
goto Exit;
}
if (atomic_read(&AtomicSyncState_g) !=
EVENT_STATE_READY) {
iRet = (int)kEplInvalidOperation;
goto Exit;
}
// exchange process image
EplRet =
EplApiProcessImageExchangeOut(&ProcessImageOut);
// pass control to sync kernel thread
atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
wake_up_interruptible(&WaitQueueCbSync_g);
// return to EplApiProcessImageExchangeout()
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
case EPLLIN_CMD_NMT_COMMAND:
{
// forward NMT command to EPL stack
EplRet = EplApiExecNmtCommand((tEplNmtEvent) ulArg_p);
iRet = (int)EplRet;
break;
}
// ----------------------------------------------------------
default:
{
break;
}
}
Exit:
// TRACE1("EPL: - EplLinIoctl (iRet=%d)\n", iRet);
return (iRet);
}
//---------------------------------------------------------------------------
//
// Function: main
//
// Description: main function of demo application
//
// Parameters:
//
// Returns:
//---------------------------------------------------------------------------
int main (int argc, char **argv)
{
tEplKernel EplRet = kEplSuccessful;
static tEplApiInitParam EplApiInitParam;
char* sHostname = HOSTNAME;
char cKey = 0;
#ifdef CONFIG_POWERLINK_USERSTACK
// variables for Pcap
char sErr_Msg[ PCAP_ERRBUF_SIZE ];
char devName[128];
pcap_if_t * alldevs;
pcap_if_t * seldev;
int i = 0;
int inum;
#endif
int opt;
#if (TARGET_SYSTEM == _LINUX_)
/* get command line parameters */
while ((opt = getopt(argc, argv, "c:l:")) != -1)
{
switch (opt)
{
case 'c':
uiCycleLen_g = strtoul(optarg, NULL, 10);
break;
case 'l':
pLogFile_g = optarg;
break;
default: /* '?' */
fprintf (stderr, "Usage: %s [-c CYCLE_TIME] [-l LOGFILE]\n", argv[0]);
goto Exit;
}
}
#endif
#ifdef CONFIG_POWERLINK_USERSTACK
#if (TARGET_SYSTEM == _LINUX_)
struct sched_param schedParam;
/* adjust process priority */
if (nice (-20) == -1) // push nice level in case we have no RTPreempt
{
EPL_DBGLVL_ERROR_TRACE("%s() couldn't set nice value! (%s)\n", __func__, strerror(errno));
}
schedParam.__sched_priority = MAIN_THREAD_PRIORITY;
if (pthread_setschedparam(pthread_self(), SCHED_RR, &schedParam) != 0)
{
EPL_DBGLVL_ERROR_TRACE("%s() couldn't set thread scheduling parameters! %d\n",
__func__, schedParam.__sched_priority);
}
/* Initialize target specific stuff */
EplTgtInit();
#elif (TARGET_SYSTEM == _WIN32_)
// activate realtime priority class
SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);
// lower the priority of this thread
SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_IDLE);
#endif // (TARGET_SYSTEM == _WIN32_)
#endif // CONFIG_POWERLINK_USERSTACK
#if (TARGET_SYSTEM == _LINUX_)
#ifdef SET_CPU_AFFINITY
{
/* 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);
}
#endif
#endif
/* Enabling ftrace for debugging */
FTRACE_OPEN();
FTRACE_ENABLE(TRUE);
/*
EPL_DBGLVL_ALWAYS_TRACE("%s(): Main Thread Id:%ld\n", __func__,
syscall(SYS_gettid));
*/
printf("----------------------------------------------------\n");
printf("openPOWERLINK console MN DEMO application\n");
printf("----------------------------------------------------\n");
EPL_MEMSET(&EplApiInitParam, 0, sizeof (EplApiInitParam));
EplApiInitParam.m_uiSizeOfStruct = sizeof (EplApiInitParam);
#ifdef CONFIG_POWERLINK_USERSTACK
/* Retrieve the device list on the local machine */
if (pcap_findalldevs(&alldevs, sErr_Msg) == -1)
{
fprintf(stderr, "Error in pcap_findalldevs: %s\n", sErr_Msg);
EplRet = kEplNoResource;
goto Exit;
}
PRINTF("--------------------------------------------------\n");
PRINTF("List of Ethernet Cards Found in this System: \n");
PRINTF("--------------------------------------------------\n");
/* Print the list */
for (seldev = alldevs; seldev != NULL; seldev = seldev->next)
{
PRINTF("%d. ", ++i);
if (seldev->description)
{
PRINTF("%s\n %s\n", seldev->description, seldev->name);
}
else
{
PRINTF("%s\n", seldev->name);
}
}
if (i == 0)
{
PRINTF("\nNo interfaces found! Make sure pcap library is installed.\n");
EplRet = kEplNoResource;
goto Exit;
}
PRINTF("--------------------------------------------------\n");
PRINTF("Select the interface to be used for POWERLINK (1-%d):",i);
if (scanf("%d", &inum) == EOF)
{
pcap_freealldevs(alldevs);
EplRet = kEplNoResource;
goto Exit;
}
PRINTF("--------------------------------------------------\n");
if ((inum < 1) || (inum > i))
{
PRINTF("\nInterface number out of range.\n");
/* Free the device list */
pcap_freealldevs(alldevs);
EplRet = kEplNoResource;
goto Exit;
}
/* Jump to the selected adapter */
for (seldev = alldevs, i = 0;
i < (inum - 1);
seldev = seldev->next, i++)
{ // do nothing
}
strncpy(devName, seldev->name, 127);
// pass selected device name to Edrv
EplApiInitParam.m_HwParam.m_pszDevName = devName;
#endif
EplApiInitParam.m_uiNodeId = uiNodeId_g = NODEID;
EplApiInitParam.m_dwIpAddress = (0xFFFFFF00 & IP_ADDR) | EplApiInitParam.m_uiNodeId;
/* 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;
EplApiInitParam.m_dwCycleLen = uiCycleLen_g; // required for error detection
EplApiInitParam.m_uiIsochrTxMaxPayload = 256; // const
EplApiInitParam.m_uiIsochrRxMaxPayload = 256; // const
EplApiInitParam.m_dwPresMaxLatency = 50000; // const; only required for IdentRes
EplApiInitParam.m_uiPreqActPayloadLimit = 36; // required for initialisation (+28 bytes)
EplApiInitParam.m_uiPresActPayloadLimit = 36; // required for initialisation of Pres frame (+28 bytes)
EplApiInitParam.m_dwAsndMaxLatency = 150000; // const; only required for IdentRes
EplApiInitParam.m_uiMultiplCycleCnt = 0; // required for error detection
EplApiInitParam.m_uiAsyncMtu = 1500; // required to set up max frame size
EplApiInitParam.m_uiPrescaler = 2; // required for sync
EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
EplApiInitParam.m_dwWaitSocPreq = 150000;
EplApiInitParam.m_dwDeviceType = -1; // NMT_DeviceType_U32
EplApiInitParam.m_dwVendorId = -1; // NMT_IdentityObject_REC.VendorId_U32
EplApiInitParam.m_dwProductCode = -1; // NMT_IdentityObject_REC.ProductCode_U32
EplApiInitParam.m_dwRevisionNumber = -1; // NMT_IdentityObject_REC.RevisionNo_U32
EplApiInitParam.m_dwSerialNumber = -1; // NMT_IdentityObject_REC.SerialNo_U32
EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
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 = AppCbEvent;
#ifdef CONFIG_POWERLINK_USERSTACK
EplApiInitParam.m_pfnObdInitRam = EplObdInitRam;
EplApiInitParam.m_pfnCbSync = AppCbSync;
#else
EplApiInitParam.m_pfnCbSync = NULL;
#endif
printf("\n\nHello, I'm a Userspace POWERLINK node running as %s!\n (build: %s / %s)\n\n",
(uiNodeId_g == EPL_C_ADR_MN_DEF_NODE_ID ?
"Managing Node" : "Controlled Node"),
__DATE__, __TIME__);
// initialize POWERLINK stack
printf ("Initializing openPOWERLINK stack...\n");
EplRet = EplApiInitialize(&EplApiInitParam);
if(EplRet != kEplSuccessful)
{
printf("EplApiInitialize() failed (Error:0x%x!\n", EplRet);
goto Exit;
}
// initialize application
printf ("Initializing openPOWERLINK application...\n");
EplRet = AppInit();
if(EplRet != kEplSuccessful)
{
printf("ApiInit() failed!\n");
goto Exit;
}
#ifdef CONFIG_POWERLINK_USERSTACK
/* At this point, we don't need any more the device list. Free it */
pcap_freealldevs(alldevs);
EplRet = EplApiSetCdcFilename(pszCdcFilename_g);
if(EplRet != kEplSuccessful)
{
goto Exit;
}
#else
// create event thread
if (pthread_create(&eventThreadId, NULL,
&powerlinkEventThread, NULL) != 0)
{
goto Exit;
}
// create sync thread
if (pthread_create(&syncThreadId, NULL,
&powerlinkSyncThread, NULL) != 0)
{
goto Exit;
}
#endif
printf("Initializing process image...\n");
printf("Size of input process image: %ld\n", sizeof(AppProcessImageIn_g));
printf("Size of output process image: %ld\n", sizeof (AppProcessImageOut_g));
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 = sizeof (AppProcessImageIn_g);
AppProcessImageCopyJob_g.m_Out.m_pPart = &AppProcessImageOut_g;
AppProcessImageCopyJob_g.m_Out.m_uiOffset = 0;
AppProcessImageCopyJob_g.m_Out.m_uiSize = sizeof (AppProcessImageOut_g);
EplRet = EplApiProcessImageAlloc(sizeof (AppProcessImageIn_g), sizeof (AppProcessImageOut_g), 2, 2);
if (EplRet != kEplSuccessful)
{
goto Exit;
}
EplRet = EplApiProcessImageSetup();
if (EplRet != kEplSuccessful)
{
goto Exit;
}
// start processing
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
if (EplRet != kEplSuccessful)
{
goto ExitShutdown;
}
printf("\n-------------------------------\n");
printf("Press Esc to leave the program\n");
printf("Press r to reset the node\n");
printf("-------------------------------\n\n");
// wait for key hit
while (cKey != 0x1B)
{
if( EplTgtKbhit() )
{
cKey = (BYTE) EplTgtGetch();
switch (cKey)
{
case 'r':
{
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
if (EplRet != kEplSuccessful)
{
goto ExitShutdown;
}
break;
}
case 'c':
{
EplRet = EplApiExecNmtCommand(kEplNmtEventNmtCycleError);
if (EplRet != kEplSuccessful)
{
goto ExitShutdown;
}
break;
}
default:
{
break;
}
}
}
EplTgtMilliSleep( 1500 );
}
FTRACE_ENABLE(FALSE);
ExitShutdown:
// halt the NMT state machine
// so the processing of POWERLINK frames stops
EplRet = EplApiExecNmtCommand(kEplNmtEventSwitchOff);
// delete process image
EplRet = EplApiProcessImageFree();
// delete instance for all modules
EplRet = EplApiShutdown();
Exit:
PRINTF("main(): returns 0x%X\n", EplRet);
#if (TARGET_SYSTEM == _WIN32_)
PRINTF("Press Enter to quit!\n");
EplTgtGetch();
#endif
return EplRet;
}
//---------------------------------------------------------------------------
//
// Function: EplLinInit
//
// Description: Entry point of kernel module
////
//
// Parameters: N/A
//
//
// Returns: Return code
//
//---------------------------------------------------------------------------
static int __init EplLinInit (void)
{
tEplKernel EplRet;
static tEplApiInitParam EplApiInitParam = {0};
char* sHostname = HOSTNAME;
BOOL fApiInit = FALSE;
BOOL fLinProcInit =FALSE;
atomic_set(&AtomicShutdown_g, TRUE);
// open character device from debugfs to disable tracing when necessary
hAppFdTracingEnabled_g = open("/sys/kernel/debug/tracing/tracing_enabled", O_WRONLY, 0666);
// get node ID from insmod command line
EplApiInitParam.m_uiNodeId = uiNodeId_g;
if (EplApiInitParam.m_uiNodeId == EPL_C_ADR_INVALID)
{ // invalid node ID set
// set default node ID
EplApiInitParam.m_uiNodeId = NODEID;
}
uiNodeId_g = EplApiInitParam.m_uiNodeId;
// calculate IP address
EplApiInitParam.m_dwIpAddress = (0xFFFFFF00 & IP_ADDR) | EplApiInitParam.m_uiNodeId;
EplApiInitParam.m_fAsyncOnly = FALSE;
EplApiInitParam.m_uiSizeOfStruct = sizeof (EplApiInitParam);
EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr, sizeof (EplApiInitParam.m_abMacAddress));
EplApiInitParam.m_dwFeatureFlags = (DWORD) ~0UL;
EplApiInitParam.m_dwCycleLen = uiCycleLen_g; // required for error detection
EplApiInitParam.m_uiIsochrTxMaxPayload = 100; // const
EplApiInitParam.m_uiIsochrRxMaxPayload = 100; // const
EplApiInitParam.m_dwPresMaxLatency = 50000; // const; only required for IdentRes
EplApiInitParam.m_uiPreqActPayloadLimit = 36; // required for initialisation (+28 bytes)
EplApiInitParam.m_uiPresActPayloadLimit = 36; // required for initialisation of Pres frame (+28 bytes)
EplApiInitParam.m_dwAsndMaxLatency = 150000; // const; only required for IdentRes
EplApiInitParam.m_uiMultiplCycleCnt = 0; // required for error detection
EplApiInitParam.m_uiAsyncMtu = 1500; // required to set up max frame size
EplApiInitParam.m_uiPrescaler = 2; // required for sync
EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
EplApiInitParam.m_dwWaitSocPreq = 150000;
EplApiInitParam.m_dwDeviceType = (DWORD) ~0UL; // NMT_DeviceType_U32
EplApiInitParam.m_dwVendorId = (DWORD) ~0UL; // NMT_IdentityObject_REC.VendorId_U32
EplApiInitParam.m_dwProductCode = (DWORD) ~0UL; // NMT_IdentityObject_REC.ProductCode_U32
EplApiInitParam.m_dwRevisionNumber = (DWORD) ~0UL; // NMT_IdentityObject_REC.RevisionNo_U32
EplApiInitParam.m_dwSerialNumber = (DWORD) ~0UL; // NMT_IdentityObject_REC.SerialNo_U32
EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
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 = AppCbEvent;
EplApiInitParam.m_pfnCbSync = AppCbSync;
EplApiInitParam.m_pfnObdInitRam = EplObdInitRam;
printk("\n\n Hello, I'm a simple POWERLINK node running as %s!\n (build: %s / %s)\n\n",
(uiNodeId_g == EPL_C_ADR_MN_DEF_NODE_ID ?
"Managing Node" : "Controlled Node"),
__DATE__, __TIME__);
// initialize the Linux a wait queue for shutdown of this module
init_waitqueue_head(&WaitQueueShutdown_g);
// initialize the procfs device
EplRet = EplLinProcInit();
if (EplRet != kEplSuccessful)
{
PRINTF("EplLinProcInit failed!\n");
goto Exit;
}
fLinProcInit = TRUE;
// initialize POWERLINK stack
EplRet = EplApiInitialize(&EplApiInitParam);
if(EplRet != kEplSuccessful)
{
PRINTF("EplApiInitialize failed!\n");
goto Exit;
}
fApiInit = TRUE;
EplRet = EplApiSetCdcFilename(pszCdcFilename_g);
if(EplRet != kEplSuccessful)
{
goto Exit;
}
PRINTF("Initializing process image...\n");
PRINTF("Size of input process image: %ld\n", sizeof(AppProcessImageIn_g));
PRINTF("Size of output process image: %ld\n", sizeof (AppProcessImageOut_g));
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 = sizeof (AppProcessImageIn_g);
AppProcessImageCopyJob_g.m_Out.m_pPart = &AppProcessImageOut_g;
AppProcessImageCopyJob_g.m_Out.m_uiOffset = 0;
AppProcessImageCopyJob_g.m_Out.m_uiSize = sizeof (AppProcessImageOut_g);
EplRet = EplApiProcessImageAlloc(sizeof (AppProcessImageIn_g), sizeof (AppProcessImageOut_g), 2, 2);
if (EplRet != kEplSuccessful)
{
goto Exit;
}
EplRet = EplApiProcessImageSetup();
if (EplRet != kEplSuccessful)
{
goto Exit;
}
// start the NMT state machine
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
atomic_set(&AtomicShutdown_g, FALSE);
Exit:
PRINTF("EplLinInit(): returns 0x%X\n", EplRet);
if (EplRet != kEplSuccessful)
{
if (fApiInit != FALSE)
{
EplApiShutdown();
}
if (fLinProcInit != FALSE)
{
EplLinProcFree();
}
return -ENODEV;
}
else
{
return 0;
}
}
//---------------------------------------------------------------------------
//
// Function: openPowerlinkInit
//
// Description:
// Initialize and start the openPOWERLINK demo application
//
// Parameters:
// pszEthName = pointer to string with name of network interface to use
//
// Returns: int
//
//---------------------------------------------------------------------------
int openPowerlinkInit (char *pszEthName, unsigned int uiDevNumber)
{
tEplKernel EplRet;
static tEplApiInitParam EplApiInitParam = {0};
char* sHostname = HOSTNAME;
BOOL fApiInit = FALSE;
int tid;
// Adjust task priorities of system tasks
/* shell task is normally set to priority 1 which could disturb
* openPOWERLINK if long-running commands are entered. Therefore
* we lower the performance to 20
*/
tid = taskIdSelf();
printf ("Task ID: %d\n", tid);
taskPrioritySet(tid, 20);
/* The priority of the network stack task has to be increased (default:50)
* in order to get network packets in time!
*/
if ((tid = taskNameToId("tNet0")) != -1 )
{
taskPrioritySet(tid, 5);
}
// Initialize high-resolution timer library
hrtimer_init(EPL_TASK_PRIORITY_HRTIMER, EPL_TASK_STACK_SIZE);
// get node ID from insmod command line
EplApiInitParam.m_uiNodeId = uiNodeId_g;
if (EplApiInitParam.m_uiNodeId == EPL_C_ADR_INVALID)
{ // invalid node ID set
// set default node ID
EplApiInitParam.m_uiNodeId = NODEID;
}
uiNodeId_g = EplApiInitParam.m_uiNodeId;
// calculate IP address
EplApiInitParam.m_dwIpAddress = (0xFFFFFF00 & IP_ADDR) |
EplApiInitParam.m_uiNodeId;
EplApiInitParam.m_fAsyncOnly = FALSE;
// store the specified device name
EplApiInitParam.m_HwParam.m_pszDevName = pszEthName;
EplApiInitParam.m_HwParam.m_uiDevNumber = uiDevNumber;
EplApiInitParam.m_uiSizeOfStruct = sizeof (EplApiInitParam);
EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr,
sizeof (EplApiInitParam.m_abMacAddress));
EplApiInitParam.m_dwFeatureFlags = (DWORD) ~0UL;
EplApiInitParam.m_dwCycleLen = uiCycleLen_g; // required for error detection
EplApiInitParam.m_uiIsochrTxMaxPayload = 100; // const
EplApiInitParam.m_uiIsochrRxMaxPayload = 100; // const
EplApiInitParam.m_dwPresMaxLatency = 50000; // const; only required for IdentRes
EplApiInitParam.m_uiPreqActPayloadLimit = 36; // required for initialisation (+28 bytes)
EplApiInitParam.m_uiPresActPayloadLimit = 36; // required for initialisation of Pres frame (+28 bytes)
EplApiInitParam.m_dwAsndMaxLatency = 150000; // const; only required for IdentRes
EplApiInitParam.m_uiMultiplCycleCnt = 0; // required for error detection
EplApiInitParam.m_uiAsyncMtu = 1500; // required to set up max frame size
EplApiInitParam.m_uiPrescaler = 2; // required for sync
EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
EplApiInitParam.m_dwWaitSocPreq = 0;
EplApiInitParam.m_dwDeviceType = (DWORD) ~0UL; // NMT_DeviceType_U32
EplApiInitParam.m_dwVendorId = (DWORD) ~0UL; // NMT_IdentityObject_REC.VendorId_U32
EplApiInitParam.m_dwProductCode = (DWORD) ~0UL; // NMT_IdentityObject_REC.ProductCode_U32
EplApiInitParam.m_dwRevisionNumber = (DWORD) ~0UL; // NMT_IdentityObject_REC.RevisionNo_U32
EplApiInitParam.m_dwSerialNumber = (DWORD) ~0UL; // NMT_IdentityObject_REC.SerialNo_U32
EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
EplApiInitParam.m_dwDefaultGateway = 0;
EPL_MEMCPY(EplApiInitParam.m_sHostname, sHostname, sizeof(EplApiInitParam.m_sHostname));
EplApiInitParam.m_uiSyncNodeId = EPL_C_ADR_SYNC_ON_SOA; // for fSyncOnPrcNode==TRUE, this means last PRC node
EplApiInitParam.m_fSyncOnPrcNode = TRUE;
// set callback functions
EplApiInitParam.m_pfnCbEvent = AppCbEvent;
EplApiInitParam.m_pfnCbSync = AppCbSync;
EplApiInitParam.m_pfnObdInitRam = EplObdInitRam;
printf("\n\n Hello, I'm a VxWorks POWERLINK node running as %s!\n"
"(build: %s / %s)\n\n",
(uiNodeId_g == EPL_C_ADR_MN_DEF_NODE_ID ?
"Managing Node" : "Controlled Node"),
__DATE__, __TIME__);
// initialize POWERLINK stack
EplRet = EplApiInitialize(&EplApiInitParam);
if(EplRet != kEplSuccessful)
{
goto Exit;
}
fApiInit = TRUE;
// set CDC filename
EplRet = EplApiSetCdcFilename(pszCdcFilename_g);
if(EplRet != kEplSuccessful)
{
printf ("Error set cdc filename!\n");
goto Exit;
}
// initialize application
printf ("Initializing openPOWERLINK application...\n");
EplRet = AppInit();
if(EplRet != kEplSuccessful)
{
printf("ApiInit() failed!\n");
goto Exit;
}
// initialize process image
printf("Initializing process image...\n");
printf("Size of input process image: %ld\n", sizeof(AppProcessImageIn_g));
printf("Size of output process image: %ld\n", sizeof (AppProcessImageOut_g));
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 = sizeof (AppProcessImageIn_g);
AppProcessImageCopyJob_g.m_Out.m_pPart = &AppProcessImageOut_g;
AppProcessImageCopyJob_g.m_Out.m_uiOffset = 0;
AppProcessImageCopyJob_g.m_Out.m_uiSize = sizeof (AppProcessImageOut_g);
EplRet = EplApiProcessImageAlloc(sizeof (AppProcessImageIn_g),
sizeof (AppProcessImageOut_g), 2, 2);
if (EplRet != kEplSuccessful)
{
goto Exit;
}
EplRet = EplApiProcessImageSetup();
if (EplRet != kEplSuccessful)
{
goto Exit;
}
// start the NMT state machine
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
Exit:
PRINTF("%s(): returns 0x%X\n", __func__, EplRet);
if (EplRet != kEplSuccessful)
{
if (fApiInit != FALSE)
{
EplApiShutdown();
}
// Shutdown high-resolution timer library
hrtimer_shutdown();
fOpenPowerlinkIsRunning_g = FALSE;
return -ENODEV;
}
else
{
fOpenPowerlinkIsRunning_g = TRUE;
return 0;
}
}
int openPowerlink(void)
{
const BYTE abMacAddr[] = {MAC_ADDR};
static tEplApiInitParam EplApiInitParam = {0};
// needed for process var
tEplObdSize ObdSize;
tEplKernel EplRet;
unsigned int uiVarEntries;
fShutdown_l = FALSE;
////////////////////////
// setup th EPL Stack //
////////////////////////
// set EPL init parameters
EplApiInitParam.m_uiSizeOfStruct = sizeof (EplApiInitParam);
#ifdef NODESWITCH_SPI_BASE
// read node-ID from hex switch on baseboard, which is connected via SPI shift register
IOWR_ALTERA_AVALON_SPI_TXDATA(NODESWITCH_SPI_BASE, 0xFF); // generate pulse for latching inputs
while ((IORD_ALTERA_AVALON_SPI_STATUS(NODESWITCH_SPI_BASE) & ALTERA_AVALON_SPI_STATUS_RRDY_MSK) == 0)
{ // wait
}
EplApiInitParam.m_uiNodeId = IORD_ALTERA_AVALON_SPI_RXDATA(NODESWITCH_SPI_BASE);
#endif
#ifdef NODESWITCH_PIO_BASE
EplApiInitParam.m_uiNodeId = IORD_ALTERA_AVALON_PIO_DATA(NODESWITCH_PIO_BASE);
#endif
if (EplApiInitParam.m_uiNodeId == EPL_C_ADR_INVALID)
{
EplApiInitParam.m_uiNodeId = NODEID; // defined at the top of this file!
}
EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr, sizeof(EplApiInitParam.m_abMacAddress));
EplApiInitParam.m_abMacAddress[5] = (BYTE) EplApiInitParam.m_uiNodeId;
// calculate IP address
EplApiInitParam.m_dwIpAddress = (0xFFFFFF00 & IP_ADDR) | EplApiInitParam.m_uiNodeId;
EplApiInitParam.m_uiIsochrTxMaxPayload = 256;
EplApiInitParam.m_uiIsochrRxMaxPayload = 256;
EplApiInitParam.m_dwPresMaxLatency = 2000;
EplApiInitParam.m_dwAsndMaxLatency = 2000;
EplApiInitParam.m_fAsyncOnly = FALSE;
EplApiInitParam.m_dwFeatureFlags = -1;
EplApiInitParam.m_dwCycleLen = CYCLE_LEN;
EplApiInitParam.m_uiPreqActPayloadLimit = 36;
EplApiInitParam.m_uiPresActPayloadLimit = 36;
EplApiInitParam.m_uiMultiplCycleCnt = 0;
EplApiInitParam.m_uiAsyncMtu = 1500;
EplApiInitParam.m_uiPrescaler = 2;
EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
EplApiInitParam.m_dwWaitSocPreq = 0;
EplApiInitParam.m_dwDeviceType = -1;
EplApiInitParam.m_dwVendorId = -1;
EplApiInitParam.m_dwProductCode = -1;
EplApiInitParam.m_dwRevisionNumber = -1;
EplApiInitParam.m_dwSerialNumber = -1;
EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
EplApiInitParam.m_dwDefaultGateway = 0;
EplApiInitParam.m_dwApplicationSwDate = 1; // PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
EplApiInitParam.m_pfnCbEvent = AppCbEvent;
EplApiInitParam.m_pfnCbSync = AppCbSync;
EplApiInitParam.m_pfnObdInitRam = EplObdInitRam;
EplApiInitParam.m_dwSyncResLatency = EPL_C_DLL_T_IFG;
// initialize EPL stack
printf("init EPL Stack with node-ID 0x%02X:\n", EplApiInitParam.m_uiNodeId);
EplRet = EplApiInitialize(&EplApiInitParam);
if(EplRet != kEplSuccessful) {
printf("init EPL Stack... error %X\n\n", EplRet);
goto Exit;
}
printf("init EPL Stack...ok\n\n");
// link process variables used by CN to object dictionary
printf("linking process vars:\n");
ObdSize = sizeof(bButtonInputs_l);
uiVarEntries = 1;
EplRet = EplApiLinkObject(0x6000, &bButtonInputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
ObdSize = sizeof(abVirtualInputs_l[0]);
uiVarEntries = 17;
EplRet = EplApiLinkObject(0x2000, abVirtualInputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
ObdSize = sizeof(adwVirtualInputs_l[0]);
uiVarEntries = 8;
EplRet = EplApiLinkObject(0x2001, adwVirtualInputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
ObdSize = sizeof(wDigitalOutputs_l);
uiVarEntries = 1;
EplRet = EplApiLinkObject(0x6300, &wDigitalOutputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
ObdSize = sizeof(bLedOutputs_l);
uiVarEntries = 1;
EplRet = EplApiLinkObject(0x6200, &bLedOutputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
ObdSize = sizeof(abVirtualOutputs_l[0]);
uiVarEntries = 15;
EplRet = EplApiLinkObject(0x2200, abVirtualOutputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
ObdSize = sizeof(adwVirtualOutputs_l[0]);
uiVarEntries = 8;
EplRet = EplApiLinkObject(0x2201, adwVirtualOutputs_l, &uiVarEntries, &ObdSize, 0x01);
if (EplRet != kEplSuccessful)
{
printf("linking process vars... error\n\n");
goto ExitShutdown;
}
printf("linking process vars... ok\n\n");
// start the EPL stack
printf("start EPL Stack...\n");
EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
if (EplRet != kEplSuccessful) {
printf("start EPL Stack... error\n\n");
goto ExitShutdown;
}
printf("start EPL Stack... ok\n\n");
printf("NIOS II with openPowerlink is ready!\n\n");
#ifdef LED_PIO_BASE
IOWR_ALTERA_AVALON_PIO_DATA(LED_PIO_BASE, 0xFF);
#endif
while(1)
{
EplApiProcess();
if (fShutdown_l == TRUE)
break;
}
ExitShutdown:
printf("Shutdown EPL Stack\n");
EplApiShutdown(); //shutdown node
Exit:
return EplRet;
}
/*----------------------------------------------------------------------------*\
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;
}
