//---------------------------------------------------------------------------
// Function: EplTimerHighReskAddInstance()
//
// Description: initializes the high resolution timer module.
//
// Parameters: void
//
// Return: tEplKernel = error code
//---------------------------------------------------------------------------
tEplKernel PUBLIC EplTimerHighReskAddInstance(void)
{
tEplKernel Ret;
UINT uiIndex;
struct sched_param schedParam;
tEplTimerHighReskTimerInfo* pTimerInfo;
Ret = kEplSuccessful;
EPL_MEMSET(&EplTimerHighReskInstance_l, 0, sizeof (EplTimerHighReskInstance_l));
/* Initialize timer threads for all usable timers. */
for (uiIndex = 0; uiIndex < TIMER_COUNT; uiIndex++)
{
pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
pTimerInfo->m_fTerminate = FALSE;
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
pTimerInfo->m_maxLatency = 0;
pTimerInfo->m_minLatency = 999999999;
#endif
if (sem_init(&pTimerInfo->m_syncSem, 0, 0) != 0)
{
EPL_DBGLVL_ERROR_TRACE("%s() Couldn't init semaphore!\n", __func__);
Ret = kEplNoResource;
goto Exit;
}
if (pthread_create(&pTimerInfo->m_timerThread, NULL, EplTimerHighReskProcessThread,
(void *)uiIndex) != 0)
{
Ret = kEplNoResource;
sem_destroy(&pTimerInfo->m_syncSem);
goto Exit;
}
schedParam.__sched_priority = EPL_THREAD_PRIORITY_HIGH;
if (pthread_setschedparam(pTimerInfo->m_timerThread, SCHED_FIFO, &schedParam) != 0)
{
EPL_DBGLVL_ERROR_TRACE("%s() Couldn't set thread scheduling parameters!\n", __func__);
Ret = kEplNoResource;
sem_destroy(&pTimerInfo->m_syncSem);
pthread_cancel(pTimerInfo->m_timerThread);
goto Exit;
}
}
Exit:
return Ret;
}
//------------------------------------------------------------------------------
// Function: ShbIpcSignalNewData
//
// Description: Signal new data (called from writing process)
//
// Parameters: pShbInstance_p pointer to shared buffer instance
//
// Return: tShbError = error code
//------------------------------------------------------------------------------
tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p)
{
tShbMemInst* pShbMemInst;
tShbMemHeader* pShbMemHeader;
tShbError ShbError;
//TRACEX("%s() pShbInstance_p=%p\n", __func__, pShbInstance_p);
if (pShbInstance_p == NULL)
{
//TRACEX("%s() Invalid instance!\n", __func__);
return (kShbInvalidArg);
}
pShbMemInst = ShbIpcGetShbMemInst (pShbInstance_p);
pShbMemHeader = ShbIpcGetShbMemHeader (pShbMemInst);
ShbError = kShbOk;
//set semaphore
if (semGive(pShbMemHeader->m_semNewData) < 0)
{
EPL_DBGLVL_ERROR_TRACE("%s() sem_post failed! (%s)\n", __func__,
strerror(errno));
}
/* if this shared buffer is connected to a master buffer, signal the
* data also to the master buffer.
*/
if (pShbMemHeader->m_pShbInstMaster != NULL)
{
return ShbIpcSignalNewData(pShbMemHeader->m_pShbInstMaster);
}
return ShbError;
}
//------------------------------------------------------------------------------
// Function: ShbIpcStartSignalingNewData
//
// Description: Start signaling of new data (called from reading process)
//
// Parameters: pShbInstance_p pointer to shared buffer instance
// pfnSignalHandlerNewData_p pointer to master buffer instance
//
// Return: tShbError = error code
//------------------------------------------------------------------------------
tShbError ShbIpcStartSignalingNewData(tShbInstance pShbInstance_p,
tSigHndlrNewData pfnSignalHandlerNewData_p,
tShbPriority ShbPriority_p)
{
tShbMemInst* pShbMemInst;
tShbMemHeader* pShbMemHeader;
tShbError ShbError;
INT iSchedPriority;
if ((pShbInstance_p == NULL) || (pfnSignalHandlerNewData_p == NULL))
{
return (kShbInvalidArg);
}
pShbMemInst = ShbIpcGetShbMemInst (pShbInstance_p);
pShbMemHeader = ShbIpcGetShbMemHeader (pShbInstance_p);
ShbError = kShbOk;
if ((pShbMemInst->m_fNewDataThreadStarted) ||
(pShbMemInst->m_pfnSigHndlrNewData != NULL))
{
ShbError = kShbAlreadySignaling;
EPL_DBGLVL_ERROR_TRACE("%s() Thread already started!\n", __func__);
goto Exit;
}
pShbMemInst->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
iSchedPriority = EPL_TASK_PRIORITY_SHB;
switch (ShbPriority_p)
{
case kShbPriorityLow:
iSchedPriority += 5;
break;
case kShbPriorityNormal:
iSchedPriority += 0;
break;
case kShbPriorityHigh:
iSchedPriority -= 5;
break;
}
if ((pShbMemInst->m_tThreadNewDataId =
taskSpawn ("tShbIpc", iSchedPriority,
0, EPL_TASK_STACK_SIZE, &ShbIpcThreadSignalNewData,
(int)pShbInstance_p, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR)
{
pShbMemInst->m_pfnSigHndlrNewData = NULL;
ShbError = kShbInvalidSigHndlr;
goto Exit;
}
Exit:
return (ShbError);
}
//------------------------------------------------------------------------------
// Function: ShbIpcStartSignalingJobReady
//
// Description: Start signaling for job ready (called from waiting process)
//
// Parameters:
// pShbInstance_p pointer to shared buffer instance
// ulTimeOut_p job ready timeout
// pfnSignalHandlerJobReady_p function pointer to callback function
//
// Return: tShbError = error code
//------------------------------------------------------------------------------
tShbError ShbIpcStartSignalingJobReady(tShbInstance pShbInstance_p,
unsigned long ulTimeOut_p,
tSigHndlrJobReady pfnSignalHandlerJobReady_p)
{
tShbMemInst* pShbMemInst;
tShbMemHeader* pShbMemHeader;
tShbError ShbError;
struct sched_param schedParam;
if ((pShbInstance_p == NULL) || (pfnSignalHandlerJobReady_p == NULL))
{
return (kShbInvalidArg);
}
pShbMemInst = ShbIpcGetShbMemInst (pShbInstance_p);
pShbMemHeader = ShbIpcGetShbMemHeader (pShbInstance_p);
ShbError = kShbOk;
if ((pShbMemInst->m_fJobReadyThreadStarted) ||
(pShbMemInst->m_pfnSigHndlrJobReady != NULL))
{
ShbError = kShbAlreadySignaling;
goto Exit;
}
pShbMemInst->m_ulTimeOutMsJobReady = ulTimeOut_p;
pShbMemInst->m_pfnSigHndlrJobReady = pfnSignalHandlerJobReady_p;
//create thread for job ready signaling
if (pthread_create(&pShbMemInst->m_tThreadJobReadyId, NULL,
&ShbIpcThreadSignalJobReady, pShbInstance_p) != 0)
{
pShbMemInst->m_pfnSigHndlrJobReady = NULL;
ShbError = kShbInvalidSigHndlr;
goto Exit;
}
schedParam.__sched_priority = EPL_THREAD_PRIORITY_LOW;
if (pthread_setschedparam(pShbMemInst->m_tThreadNewDataId, SCHED_FIFO,
&schedParam) != 0)
{
EPL_DBGLVL_ERROR_TRACE("%s(): couldn't set thread scheduling parameters! %d\n",
__func__, schedParam.__sched_priority);
}
Exit:
return ShbError;
}
//------------------------------------------------------------------------------
// Function: ShbIpcStopSignalingNewData
//
// Description: Stop signaling of new data (called from reading process)
//
// Parameters: pShbInstance_p pointer to shared buffer instance
//
// Return: tShbError = error code
//------------------------------------------------------------------------------
tShbError ShbIpcStopSignalingNewData(tShbInstance pShbInstance_p)
{
tShbMemInst *pShbMemInst;
tShbMemHeader *pShbMemHeader;
tShbError ShbError;
INT iRetVal = -1;
sem_t* pSemStopSignalingNewData;
sem_t* pSemNewData;
struct timespec curTime, timeout;
if (pShbInstance_p == NULL)
{
return (kShbInvalidArg);
}
pShbMemHeader = ShbIpcGetShbMemHeader (pShbInstance_p);
pShbMemInst = ShbIpcGetShbMemInst (pShbInstance_p);
pSemNewData = &pShbMemHeader->m_semNewData;
pSemStopSignalingNewData = &pShbMemHeader->m_semStopSignalingNewData;
ShbError = kShbOk;
if (!pShbMemInst->m_fNewDataThreadStarted)
{
ShbError = kShbBufferAlreadyCompleted;
goto Exit;
}
//set termination flag and signal new data to terminate thread
pShbMemInst->m_fThreadTermFlag = TRUE;
sem_post(pSemNewData);
// waiting for thread to terminate
clock_gettime(CLOCK_REALTIME, &curTime);
timeout.tv_sec = 1;
timeout.tv_nsec = TIMEOUT_WAITING_THREAD * 1000;
timespecadd(&timeout, &curTime);
iRetVal = sem_timedwait(pSemStopSignalingNewData, &timeout);
if (iRetVal != 0)
{
EPL_DBGLVL_ERROR_TRACE("%s() Stop Sem TIMEOUT %d (%s)\n", __func__, iRetVal, strerror(errno));
}
Exit:
return (ShbError);
}
//------------------------------------------------------------------------------
// Function: ShbIpcStopSignalingNewData
//
// Description: Stop signaling of new data (called from reading process)
//
// Parameters: pShbInstance_p pointer to shared buffer instance
//
// Return: tShbError = error code
//------------------------------------------------------------------------------
tShbError ShbIpcStopSignalingNewData(tShbInstance pShbInstance_p)
{
tShbMemInst *pShbMemInst;
tShbMemHeader *pShbMemHeader;
tShbError ShbError;
INT iRetVal = -1;
if (pShbInstance_p == NULL)
{
return (kShbInvalidArg);
}
pShbMemHeader = ShbIpcGetShbMemHeader (pShbInstance_p);
pShbMemInst = ShbIpcGetShbMemInst (pShbInstance_p);
ShbError = kShbOk;
if (!pShbMemInst->m_fNewDataThreadStarted)
{
ShbError = kShbBufferAlreadyCompleted;
goto Exit;
}
//set termination flag and signal new data to terminate thread
pShbMemInst->m_fThreadTermFlag = TRUE;
semGive(pShbMemHeader->m_semNewData);
iRetVal = semTake(pShbMemHeader->m_semStopSignalingNewData,
TIMEOUT_WAITING_THREAD);
if (iRetVal == ERROR)
{
EPL_DBGLVL_ERROR_TRACE("%s() Stop Sem TIMEOUT %d (%s)\n", __func__,
iRetVal, strerror(errno));
}
Exit:
return (ShbError);
}
//------------------------------------------------------------------------------
// Function: ShbIpcAllocBuffer
//
// Description: Allocates memory for the shared buffers
//
// Parameters:
// ulBufferSize_p size of the shared buffer to allocate
// pszBufferId_p string containing the shared buffer identifier
// ppShbInstance_p pointer to store the instance of this shared
// buffer
// pfShbNewCreated_p pointer to store the buffer creation flag
//
// Return: tShbError = error
//------------------------------------------------------------------------------
tShbError ShbIpcAllocBuffer (ULONG ulBufferSize_p, const char* pszBufferID_p,
tShbInstance* ppShbInstance_p,
UINT* pfShbNewCreated_p)
{
tShbError ShbError;
UINT uiBufferKey;
BOOL fShbNewCreated;
ULONG ulShMemSize;
tShbMemHeader* pShbMemHeader;
tShbMemInst* pShbMemInst = NULL;
tShbInstance pShbInstance;
ulShMemSize = ulBufferSize_p + sizeof(tShbMemHeader);
//create Buffer Key
uiBufferKey = ShbIpcCrc32GetCrc(pszBufferID_p);
EPL_DBGLVL_SHB_TRACE("%s() Allocate %lu Bytes, sBufferID:%s BufferKey:%08x\n",
__func__, ulShMemSize, pszBufferID_p, (key_t)uiBufferKey);
//---------------------------------------------------------------
// (1) open an existing or create a new shared memory
//---------------------------------------------------------------
// try to create shared memory
if ((pShbMemHeader = ShbIpcFindMem(uiBufferKey)) == NULL)
{
fShbNewCreated = TRUE;
if ((pShbMemHeader = ShbIpcAlloc(uiBufferKey, ulShMemSize)) == NULL)
{
//unable to create mem
EPL_DBGLVL_ERROR_TRACE("%s() Shared memory allocation error!\n",
__func__);
ShbError = kShbOutOfMem;
goto Exit;
}
else
{
EPL_DBGLVL_SHB_TRACE("%s() Shared memory allocated, Addr:%p Key:%08x size:%ld\n",
__func__, (void *)pShbMemHeader, uiBufferKey, ulShMemSize);
}
}
else
{
EPL_DBGLVL_SHB_TRACE("%s() Attached to shared memory, Addr:%p Key:%08x size:%ld\n",
__func__, (void *)pShbMemHeader, uiBufferKey, ulShMemSize);
fShbNewCreated = FALSE;
}
//---------------------------------------------------------------
// (2) setup or update header and management information
//---------------------------------------------------------------
// allocate a memory block from process specific mempool to save
// process local information to administrate/manage the shared buffer
if ((pShbMemInst = (tShbMemInst*)ShbIpcAllocPrivateMem(sizeof(tShbMemInst)))
== NULL)
{
EPL_DBGLVL_ERROR_TRACE("%s() Couldn't alloc private mem!\n", __func__);
ShbError = kShbOutOfMem;
goto Exit;
}
memset(pShbMemInst, 0, sizeof(tShbMemInst));
// reset complete header to default values
pShbMemInst->m_SbiMagicID = SBI_MAGIC_ID;
pShbMemInst->m_uiSharedMemId = uiBufferKey;
strncpy(pShbMemInst->m_sBufId, pszBufferID_p, MAX_LEN_BUFFER_ID - 1);
pShbMemInst->m_pfnSigHndlrNewData = NULL;
pShbMemInst->m_fNewDataThreadStarted = FALSE;
pShbMemInst->m_ulTimeOutMsJobReady = 0;
pShbMemInst->m_pfnSigHndlrJobReady = NULL;
pShbMemInst->m_pShbMemHeader = pShbMemHeader;
pShbMemInst->m_fThreadTermFlag = FALSE;
ShbError = kShbOk;
if (fShbNewCreated)
{
memset (pShbMemHeader, 0, sizeof(tShbMemHeader));
// this process was the first who wanted to use the shared memory,
// so a new shared memory was created
// -> setup new header information inside the shared memory region
// itself
pShbMemHeader->m_ulShMemSize = ulShMemSize;
pShbMemHeader->m_ulRefCount = 1;
pShbMemHeader->m_uiBufferKey = uiBufferKey;
//create semaphores for buffer access and signal new data
if ((pShbMemHeader->m_mutexBuffAccess =
semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE)) == NULL)
{
ShbError = kShbOutOfMem;
goto Exit;
}
if ((pShbMemHeader->m_semNewData = semCCreate(SEM_Q_PRIORITY, 0)) == NULL)
{
ShbError = kShbOutOfMem;
goto Exit;
}
if ((pShbMemHeader->m_semStopSignalingNewData =
semCCreate(SEM_Q_PRIORITY, 0)) == NULL)
{
ShbError = kShbOutOfMem;
goto Exit;
}
if ((pShbMemHeader->m_semJobReady = semCCreate(SEM_Q_PRIORITY, 0)) == NULL)
{
ShbError = kShbOutOfMem;
goto Exit;
}
}
else
{
// any other process has created the shared memory and this
// process has only attached to it
// -> check and update existing header information inside the
// shared memory region itself
if (pShbMemHeader->m_uiBufferKey != uiBufferKey)
{
EPL_DBGLVL_ERROR_TRACE("%s() Shared Mem mismatch buffer key %x:%x!\n",
__func__, uiBufferKey, pShbMemHeader->m_uiBufferKey);
ShbError = kShbOpenMismatch;
goto Exit;
}
//TRACEX("%s() Check mem size is:%ld should be:%ld \n", __func__,
// pShbMemHeader->m_ulShMemSize, ulShMemSize);
if (pShbMemHeader->m_ulShMemSize != ulShMemSize)
{
EPL_DBGLVL_ERROR_TRACE("%s() Shared Mem mismatch size! %ld:%ld\n",
__func__, ulShMemSize, pShbMemHeader->m_ulShMemSize);
ShbError = kShbOpenMismatch;
goto Exit;
}
pShbMemHeader->m_ulRefCount++;
}
Exit:
if (ShbError != kShbOk)
{
EPL_DBGLVL_ERROR_TRACE("%s() allocating shared buf failed!\n (%d)",
__func__, ShbError);
if (pShbMemInst != NULL)
{
ShbIpcReleasePrivateMem (pShbMemInst);
}
if ((pShbMemHeader != NULL) && (fShbNewCreated))
{
ShbIpcFree(uiBufferKey);
}
}
pShbInstance = (tShbInstance*)pShbMemInst;
*pfShbNewCreated_p = fShbNewCreated;
*ppShbInstance_p = pShbInstance;
return (ShbError);
}
//---------------------------------------------------------------------------
// Function: EplTimerHighReskProcessThread()
//
// Description: Main function of the high-resolution timer thread.
//
// EplTimerHighReskProcessThread() waits for a timer start
// signal. When it is received it reads the high-resolution
// timer information structure and calculates the timeout value.
// It sleeps by calling clock_nanosleep() until the timeout is
// reached. When the timeout is reached the callback function
// registered in the timer info structure is called. If the
// flag m_fContinuously is set the thread loops until the
// timer is deleted.
//
// Parameters: pArgument_p * = pointer to timer info structure
//
// Return: void * = return value is specified by the pthread
// interface but is not used!
//---------------------------------------------------------------------------
static void * EplTimerHighReskProcessThread(void *pArgument_p)
{
INT iRet;
tEplTimerHighReskTimerInfo *pTimerInfo;
struct timespec startTime, curTime, timeout;
ULONGLONG ullPeriod;
tEplTimerHdl timerHdl;
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
struct timespec debugtime;
#endif
EPL_DBGLVL_TIMERH_TRACE("%s(): ThreadId:%ld\n", __func__, syscall(SYS_gettid));
EPL_DBGLVL_TIMERH_TRACE("%s(): timer:%lx\n", __func__, (unsigned long)pArgument_p);
/* thread parameter contains the address of the timer information structure */
pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[(int)pArgument_p];
/* loop forever until thread will be canceled */
while (1)
{
/* wait for semaphore which signals a timer start */
sem_wait(&pTimerInfo->m_syncSem);
/* check if thread should terminate */
if (pTimerInfo->m_fTerminate)
{
EPL_DBGLVL_TIMERH_TRACE("%s() Exiting signal received!\n", __func__);
break;
}
else
{
/* save timer information into local variables */
startTime = pTimerInfo->m_startTime;
timerHdl = pTimerInfo->m_EventArg.m_TimerHdl;
ullPeriod = pTimerInfo->m_ullTime;
/* calculate the timeout value for the timer cycle */
timespec_add(&startTime, ullPeriod, &timeout);
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
clock_gettime(CLOCK_MONOTONIC, &curTime);
#endif
do
{
/* sleep until timeout */
iRet = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &timeout, NULL);
if (iRet < 0)
{
EPL_DBGLVL_ERROR_TRACE("%s(): Error in clock_nanosleep!\n",
__func__);
/* todo how to signal that timeout wasn't correct? */
}
FTRACE_MARKER("HighReskTimer(%d) expired (%d ns)",
(int)pArgument_p, ullPeriod);
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
clock_gettime(CLOCK_MONOTONIC, &curTime);
timespec_sub(&timeout, &curTime, &debugtime);
FTRACE_MARKER("%s latency=%ld:%ld", __func__, debugtime.tv_sec,
debugtime.tv_nsec);
if (debugtime.tv_nsec > pTimerInfo->m_maxLatency)
{
EPL_DBGLVL_TIMERH_TRACE("%s() Timer elapsed: max latency=%ld ns\n",
__func__, debugtime.tv_nsec);
pTimerInfo->m_maxLatency = debugtime.tv_nsec;
}
if (timeout.tv_nsec < pTimerInfo->m_minLatency)
{
EPL_DBGLVL_TIMERH_TRACE("%s() Timer elapsed: min latency=%ld ns\n",
__func__, debugtime.tv_nsec);
pTimerInfo->m_minLatency = debugtime.tv_nsec;
}
#endif
/* check if timer handle is valid */
if (timerHdl == pTimerInfo->m_EventArg.m_TimerHdl)
{
/* call callback function */
if (pTimerInfo->m_pfnCallback != NULL)
{
pTimerInfo->m_pfnCallback(&pTimerInfo->m_EventArg);
}
}
/* check if timer handle is still valid. Could be modified in callback! */
if (timerHdl == pTimerInfo->m_EventArg.m_TimerHdl)
{
if (pTimerInfo->m_fContinuously)
{
/* calculate timeout value for next timer cycle */
timespec_add(&timeout, ullPeriod, &timeout);
}
}
} while ((pTimerInfo->m_fContinuously) &&
(timerHdl == pTimerInfo->m_EventArg.m_TimerHdl));
}
}
return NULL;
}
//---------------------------------------------------------------------------
//
// 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: ShbIpcStartSignalingNewData
//
// Description: Start signaling of new data (called from reading process)
//
// Parameters: pShbInstance_p pointer to shared buffer instance
// pfnSignalHandlerNewData_p pointer to master buffer instance
//
// Return: tShbError = error code
//------------------------------------------------------------------------------
tShbError ShbIpcStartSignalingNewData(tShbInstance pShbInstance_p,
tSigHndlrNewData pfnSignalHandlerNewData_p,
tShbPriority ShbPriority_p)
{
tShbMemInst* pShbMemInst;
tShbMemHeader* pShbMemHeader;
tShbError ShbError;
struct sched_param schedParam;
INT iSchedPriority;
if ((pShbInstance_p == NULL) || (pfnSignalHandlerNewData_p == NULL))
{
return (kShbInvalidArg);
}
pShbMemInst = ShbIpcGetShbMemInst (pShbInstance_p);
pShbMemHeader = ShbIpcGetShbMemHeader (pShbInstance_p);
ShbError = kShbOk;
if ((pShbMemInst->m_fNewDataThreadStarted) ||
(pShbMemInst->m_pfnSigHndlrNewData != NULL))
{
ShbError = kShbAlreadySignaling;
EPL_DBGLVL_ERROR_TRACE("%s() Thread already started!\n", __func__);
goto Exit;
}
pShbMemInst->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
iSchedPriority = EPL_THREAD_PRIORITY_MEDIUM;
switch (ShbPriority_p)
{
case kShbPriorityLow:
iSchedPriority -= 5;
break;
case kShbPriorityNormal:
iSchedPriority += 0;
break;
case kShbPriorityHigh:
iSchedPriority += 5;
break;
}
//create thread for signaling new data
if (pthread_create(&pShbMemInst->m_tThreadNewDataId, NULL,
&ShbIpcThreadSignalNewData, pShbInstance_p) != 0)
{
pShbMemInst->m_pfnSigHndlrNewData = NULL;
ShbError = kShbInvalidSigHndlr;
goto Exit;
}
schedParam.__sched_priority = iSchedPriority;
if (pthread_setschedparam(pShbMemInst->m_tThreadNewDataId, SCHED_FIFO,
&schedParam) != 0)
{
EPL_DBGLVL_ERROR_TRACE("%s(): couldn't set thread scheduling parameters! %d\n",
__func__, schedParam.__sched_priority);
}
Exit:
return (ShbError);
}
