//------------------------------------------------------------------------------
tOplkError timesynck_init(void)
{
tOplkError ret;
OPLK_MEMSET(×ynckInstance_l, 0, sizeof(timesynckInstance_l));
timesynckInstance_l.syncEventCycle = 1; // Default every cycle
ret = timesynckcal_init();
if (ret != kErrorOk)
return ret;
#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)
timesynckInstance_l.pSharedMemory = timesynckcal_getSharedMemory();
if (timesynckInstance_l.pSharedMemory == NULL)
return kErrorNoResource;
// Initialize shared memory
OPLK_MEMSET(timesynckInstance_l.pSharedMemory, 0, sizeof(*timesynckInstance_l.pSharedMemory));
// Initialize triple buffer
timesynckInstance_l.pSharedMemory->kernelToUserSocTime.clean = 0;
timesynckInstance_l.pSharedMemory->kernelToUserSocTime.read = 1;
timesynckInstance_l.pSharedMemory->kernelToUserSocTime.write = 2;
OPLK_DCACHE_FLUSH(timesynckInstance_l.pSharedMemory, sizeof(*timesynckInstance_l.pSharedMemory));
#endif
return ret;
}
//------------------------------------------------------------------------------
tOplkError ctrlucal_init(void)
{
tHostifReturn hifRet;
tHostifConfig hifConfig;
OPLK_MEMSET(&instance_l, 0, sizeof(instance_l));
OPLK_MEMSET(&hifConfig, 0, sizeof(hifConfig));
hifConfig.instanceNum = 0;
hifConfig.pBase = (UINT8*)HOSTIF_BASE; //FIXME: Get it from somewhere else?
hifConfig.version.revision = HOSTIF_VERSION_REVISION;
hifConfig.version.minor = HOSTIF_VERSION_MINOR;
hifConfig.version.major = HOSTIF_VERSION_MAJOR;
hifRet = hostif_create(&hifConfig, &instance_l.hifInstance);
if (hifRet != kHostifSuccessful)
{
DEBUG_LVL_ERROR_TRACE("Could not initialize host interface (0x%X)\n", hifRet);
return kErrorNoResource;
}
//disable master IRQ
instance_l.fIrqMasterEnable = FALSE;
hifRet = hostif_irqMasterEnable(instance_l.hifInstance, instance_l.fIrqMasterEnable);
if (hifRet != kHostifSuccessful)
{
DEBUG_LVL_ERROR_TRACE("Could not disable master IRQ (0x%X)\n", hifRet);
return kErrorNoResource;
}
return kErrorOk;
}
//------------------------------------------------------------------------------
void dllkfilter_setupFilters(void)
{
OPLK_MEMSET(dllkInstance_g.aFilter, 0, sizeof(dllkInstance_g.aFilter));
setupAsndFilter(&dllkInstance_g.aFilter[DLLK_FILTER_ASND]);
setupSocFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOC]);
setupSoaFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOA]);
setupSoaIdentReqFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOA_IDREQ],
dllkInstance_g.dllConfigParam.nodeId,
&dllkInstance_g.pTxBuffer[DLLK_TXFRAME_IDENTRES]);
setupSoaStatusReqFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOA_STATREQ],
dllkInstance_g.dllConfigParam.nodeId,
&dllkInstance_g.pTxBuffer[DLLK_TXFRAME_STATUSRES]);
setupSoaNmtReqFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOA_NMTREQ],
dllkInstance_g.dllConfigParam.nodeId,
&dllkInstance_g.pTxBuffer[DLLK_TXFRAME_NMTREQ]);
#if (CONFIG_DLL_PRES_CHAINING_CN != FALSE)
setupSoaSyncReqFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOA_SYNCREQ],
dllkInstance_g.dllConfigParam.nodeId,
&dllkInstance_g.pTxBuffer[DLLK_TXFRAME_SYNCRES]);
#endif
setupSoaUnspecReqFilter(&dllkInstance_g.aFilter[DLLK_FILTER_SOA_NONPLK],
dllkInstance_g.dllConfigParam.nodeId,
&dllkInstance_g.pTxBuffer[DLLK_TXFRAME_NONPLK]);
#if defined(CONFIG_INCLUDE_VETH)
setupVethUnicast(&dllkInstance_g.aFilter[DLLK_FILTER_VETH_UNICAST],
edrv_getMacAddr(),
TRUE);
setupVethBroadcast(&dllkInstance_g.aFilter[DLLK_FILTER_VETH_BROADCAST], TRUE);
#endif
}
//------------------------------------------------------------------------------
tMemMapReturn memmap_init(void)
{
ULONG bytesReturned;
tMemStruc inMemStruc;
tMemStruc* pOutMemStruc = &memMapInstance_l.memStruc;
OPLK_MEMSET(&inMemStruc, 0, sizeof(inMemStruc));
memMapInstance_l.hFileHandle = ctrlucal_getFd();
if (memMapInstance_l.hFileHandle == NULL)
return kMemMapNoResource;
if (!DeviceIoControl(memMapInstance_l.hFileHandle,
PLK_CMD_MAP_MEM,
&inMemStruc,
sizeof(tMemStruc),
pOutMemStruc,
sizeof(tMemStruc),
&bytesReturned,
NULL))
{
return kMemMapNoResource;
}
if ((bytesReturned == 0) || (pOutMemStruc->pUserAddr == NULL))
return kMemMapNoResource;
return kMemMapOk;
}
//------------------------------------------------------------------------------
static BOOL getLinkStatus(const char* pIfName_p)
{
BOOL fRunning;
struct ifreq ethreq;
int fd;
fd = socket(AF_INET, SOCK_DGRAM, 0);
OPLK_MEMSET(ðreq, 0, sizeof(ethreq));
/* set the name of the interface we wish to check */
strncpy(ethreq.ifr_name, pIfName_p, IFNAMSIZ);
/* grab flags associated with this interface */
ioctl(fd, SIOCGIFFLAGS, ðreq);
if (ethreq.ifr_flags & IFF_RUNNING)
{
fRunning = TRUE;
}
else
{
fRunning = FALSE;
}
close(fd);
return fRunning;
}
//------------------------------------------------------------------------------
tOplkError ledu_init(tLeduStateChangeCallback pfnCbStateChange_p)
{
OPLK_MEMSET(&leduInstance_g, 0, sizeof(tLeduInstance));
leduInstance_g.pfnCbStateChange = pfnCbStateChange_p;
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError timesyncu_init(tSyncCb pfnSyncCb_p)
{
tOplkError ret;
OPLK_MEMSET(&instance_l, 0, sizeof(tTimesyncuInstance));
#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)
instance_l.pSharedMemory = NULL;
#if defined(CONFIG_INCLUDE_NMT_MN)
instance_l.fFirstSyncEventDone = FALSE;
#endif
#endif
// Store the pointer function locally for synchronization callback
instance_l.pfnSyncCb = pfnSyncCb_p;
ret = timesyncucal_init(syncCb);
if (ret != kErrorOk)
return ret;
#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)
instance_l.pSharedMemory = timesyncucal_getSharedMemory();
if (instance_l.pSharedMemory == NULL)
return kErrorNoResource;
#endif
return ret;
}
//------------------------------------------------------------------------------
tCircBufInstance* circbuf_createInstance(UINT8 id_p, BOOL fNew_p)
{
tCircBufInstance* pInstance;
tCircBufArchInstance* pArch;
char semName[16];
if ((pInstance = OPLK_MALLOC(sizeof(tCircBufInstance) +
sizeof(tCircBufArchInstance))) == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s() malloc failed!\n", __func__);
return NULL;
}
OPLK_MEMSET(pInstance, 0, sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance));
pInstance->pCircBufArchInstance = (BYTE*)pInstance + sizeof(tCircBufInstance);
pInstance->bufferId = id_p;
pArch = (tCircBufArchInstance*)pInstance->pCircBufArchInstance;
sprintf(semName, "/semCircbuf-%d", id_p);
if (fNew_p)
{
sem_unlink(semName);
}
if ((pArch->lockSem = sem_open(semName, O_CREAT, S_IRWXG, 1)) == SEM_FAILED)
{
DEBUG_LVL_ERROR_TRACE("%s() open sem failed!\n", __func__);
OPLK_FREE(pInstance);
return NULL;
}
return pInstance;
}
//------------------------------------------------------------------------------
tOplkError edrvcyclic_setMaxTxBufferListSize(UINT maxListSize_p)
{
tOplkError ret = kErrorOk;
if (edrvcyclicInstance_l.maxTxBufferCount != maxListSize_p)
{
edrvcyclicInstance_l.maxTxBufferCount = maxListSize_p;
if (edrvcyclicInstance_l.ppTxBufferList != NULL)
{
OPLK_FREE(edrvcyclicInstance_l.ppTxBufferList);
edrvcyclicInstance_l.ppTxBufferList = NULL;
}
edrvcyclicInstance_l.ppTxBufferList = OPLK_MALLOC(sizeof(*edrvcyclicInstance_l.ppTxBufferList) * maxListSize_p * 2);
if (edrvcyclicInstance_l.ppTxBufferList == NULL)
{
ret = kErrorEdrvNoFreeBufEntry;
}
edrvcyclicInstance_l.curTxBufferList = 0;
OPLK_MEMSET(edrvcyclicInstance_l.ppTxBufferList, 0, sizeof(*edrvcyclicInstance_l.ppTxBufferList) * maxListSize_p * 2);
}
return ret;
}
//------------------------------------------------------------------------------
tCircBufInstance* circbuf_createInstance(UINT8 id_p, BOOL fNew_p)
{
tCircBufInstance* pInstance;
tCircBufArchInstance* pArch;
TCHAR mutexName[MAX_PATH];
UNUSED_PARAMETER(fNew_p);
if ((pInstance = OPLK_MALLOC(sizeof(tCircBufInstance) +
sizeof(tCircBufArchInstance))) == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s() malloc failed!\n", __func__);
return NULL;
}
OPLK_MEMSET(pInstance, 0, sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance));
pInstance->pCircBufArchInstance = (BYTE*)pInstance + sizeof(tCircBufInstance);
pInstance->bufferId = id_p;
pArch = (tCircBufArchInstance*)pInstance->pCircBufArchInstance;
sprintf(mutexName, "Local\\circbufMutex%d", id_p);
if ((pArch->lockMutex = CreateMutex(NULL, FALSE, mutexName)) == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s() creating mutex failed!\n", __func__);
OPLK_FREE(pInstance);
return NULL;
}
return pInstance;
}
//------------------------------------------------------------------------------
tOplkError eventucal_init(void)
{
tOplkError ret = kErrorOk;
struct sched_param schedParam;
OPLK_MEMSET(&instance_l, 0, sizeof(tEventuCalInstance));
instance_l.fd = ctrlucal_getFd();
instance_l.fStopKernelThread = FALSE;
instance_l.fStopProcessThread = FALSE;
sem_unlink("/semUserEvent"); // Deinitialize any existing instance of the semaphore
if ((instance_l.semUserData = sem_open("/semUserEvent", O_CREAT | O_RDWR, S_IRWXG, 0)) == SEM_FAILED)
goto Exit;
if (eventucal_initQueueCircbuf(kEventQueueUInt) != kErrorOk)
goto Exit;
if (eventucal_setSignalingCircbuf(kEventQueueUInt, signalUIntEvent) != kErrorOk)
goto Exit;
// Create thread for fetching new user data from kernel
if (pthread_create(&instance_l.kernelEventThreadId, NULL, k2uEventFetchThread, NULL) != 0)
goto Exit;
schedParam.sched_priority = KERNEL_EVENT_FETCH_THREAD_PRIORITY;
if (pthread_setschedparam(instance_l.kernelEventThreadId, SCHED_FIFO, &schedParam) != 0)
{
DEBUG_LVL_ERROR_TRACE("%s(): couldn't set K2U thread scheduling parameters! %d\n",
__func__,
schedParam.sched_priority);
}
#if (defined(__GLIBC__) && (__GLIBC__ >= 2) && (__GLIBC_MINOR__ >= 12))
pthread_setname_np(instance_l.kernelEventThreadId, "oplk-eventufetch");
#endif
// Create thread for processing pending user data
if (pthread_create(&instance_l.processEventThreadId, NULL, eventProcessThread, NULL) != 0)
goto Exit;
schedParam.sched_priority = EVENT_PROCESS_THREAD_PRIORITY;
if (pthread_setschedparam(instance_l.processEventThreadId, SCHED_FIFO, &schedParam) != 0)
{
DEBUG_LVL_ERROR_TRACE("%s(): couldn't set event process thread scheduling parameters! %d\n",
__func__,
schedParam.sched_priority);
}
#if (defined(__GLIBC__) && (__GLIBC__ >= 2) && (__GLIBC_MINOR__ >= 12))
pthread_setname_np(instance_l.processEventThreadId, "oplk-eventuprocess");
#endif
instance_l.fInitialized = TRUE;
return kErrorOk;
Exit:
eventucal_exit();
return ret;
}
//------------------------------------------------------------------------------
static void getMacAdrs(UINT8* pMac_p)
{
struct ifreq ifr;
int sock;
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0)
{
DEBUG_LVL_VETH_TRACE("%s: Cannot open udp socket for MAC reading: %s\n",
__func__,
strerror(errno));
return;
}
OPLK_MEMSET(&ifr, 0, sizeof(struct ifreq));
strncpy(ifr.ifr_name, "plk", IFNAMSIZ);
if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0)
{
DEBUG_LVL_VETH_TRACE("Cannot get MAC address: '%s' (%d)\n", strerror(errno), errno);
}
DEBUG_LVL_VETH_TRACE("Get Mac addr %02x:%02x:%02x:%02x:%02x:%02x\n",
ifr.ifr_hwaddr.sa_data[0],
ifr.ifr_hwaddr.sa_data[1],
ifr.ifr_hwaddr.sa_data[2],
ifr.ifr_hwaddr.sa_data[3],
ifr.ifr_hwaddr.sa_data[4],
ifr.ifr_hwaddr.sa_data[5]);
close(sock);
OPLK_MEMCPY(pMac_p, &ifr.ifr_hwaddr.sa_data[0], ETH_ALEN);
}
//------------------------------------------------------------------------------
void ctrlk_exit(void)
{
ctrlkcal_exit();
// Reset the instance
OPLK_MEMSET(&instance_l, 0, sizeof(instance_l));
}
//------------------------------------------------------------------------------
tOplkError eventu_postError(tEventSource eventSource_p,
tOplkError error_p,
UINT argSize_p,
const void* pArg_p)
{
tOplkError ret;
tEventError eventError;
tEvent event;
// Check parameter validity
ASSERT((argSize_p == 0) ||
(pArg_p != NULL));
// create argument
eventError.eventSource = eventSource_p;
eventError.oplkError = error_p;
argSize_p = (UINT)min((size_t)argSize_p, sizeof(eventError.errorArg));
OPLK_MEMCPY(&eventError.errorArg, pArg_p, argSize_p);
// create event
event.eventType = kEventTypeError;
event.eventSink = kEventSinkApi;
OPLK_MEMSET(&event.netTime, 0x00, sizeof(event.netTime));
event.eventArgSize = offsetof(tEventError, errorArg) + argSize_p;
event.eventArg.pEventArg = &eventError;
ret = eventu_postEvent(&event);
return ret;
}
//------------------------------------------------------------------------------
tOplkError cfmu_init(tCfmCbEventCnProgress pfnCbEventCnProgress_p,
tCfmCbEventCnResult pfnCbEventCnResult_p)
{
tOplkError ret = kErrorOk;
UINT subindex;
tVarParam varParam;
OPLK_MEMSET(&cfmInstance_g, 0, sizeof(tCfmInstance));
cfmInstance_g.pfnCbEventCnProgress = pfnCbEventCnProgress_p;
cfmInstance_g.pfnCbEventCnResult = pfnCbEventCnResult_p;
// link domain with 4 zero-bytes to object 0x1F22 CFM_ConciseDcfList_ADOM
varParam.pData = &cfmInstance_g.leDomainSizeNull;
varParam.size = (tObdSize)sizeof(cfmInstance_g.leDomainSizeNull);
varParam.index = 0x1F22; // CFM_ConciseDcfList_ADOM
for (subindex = 1; subindex <= NMT_MAX_NODE_ID; subindex++)
{
varParam.subindex = subindex;
varParam.validFlag = kVarValidAll;
ret = obd_defineVar(&varParam);
if ((ret != kErrorOk) &&
(ret != kErrorObdIndexNotExist) &&
(ret != kErrorObdSubindexNotExist))
{
return ret;
}
}
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError eventucal_init(void)
{
tOplkError ret = kErrorOk;
struct sched_param schedParam;
OPLK_MEMSET(&instance_l, 0, sizeof(tEventuCalInstance));
instance_l.fd = ctrlucal_getFd();
instance_l.fStopThread = FALSE;
//create thread for signaling new data
if (pthread_create(&instance_l.threadId, NULL, eventThread, NULL) != 0)
{
goto Exit;
}
schedParam.sched_priority = USER_EVENT_THREAD_PRIORITY;
if (pthread_setschedparam(instance_l.threadId, SCHED_FIFO, &schedParam) != 0)
{
DEBUG_LVL_ERROR_TRACE("%s(): couldn't set thread scheduling parameters! %d\n",
__func__, schedParam.sched_priority);
}
#if (defined(__GLIBC__) && __GLIBC__ >= 2 && __GLIBC_MINOR__ >= 12)
pthread_setname_np(instance_l.threadId, "oplk-eventu");
#endif
Exit:
return ret;
}
//------------------------------------------------------------------------------
static tOplkError loadCdcFile(char* pCdcFilename_p)
{
tOplkError ret = kErrorOk;
tObdCdcInfo cdcInfo;
UINT32 error;
OPLK_MEMSET(&cdcInfo, 0, sizeof(tObdCdcInfo));
cdcInfo.type = kObdCdcTypeFile;
cdcInfo.handle.fdCdcFile = open(pCdcFilename_p, O_RDONLY | O_BINARY, 0666);
if (!IS_FD_VALID(cdcInfo.handle.fdCdcFile))
{ // error occurred
error = (UINT32)errno;
ret = eventu_postError(kEventSourceObdu, kErrorObdErrnoSet, sizeof(UINT32), &error);
return ret;
}
cdcInfo.cdcSize = lseek(cdcInfo.handle.fdCdcFile, 0, SEEK_END);
lseek(cdcInfo.handle.fdCdcFile, 0, SEEK_SET);
ret = processCdc(&cdcInfo);
if (cdcInfo.pCurBuffer != NULL)
{
OPLK_FREE(cdcInfo.pCurBuffer);
cdcInfo.pCurBuffer = NULL;
cdcInfo.bufferSize = 0;
}
close(cdcInfo.handle.fdCdcFile);
return ret;
}
//------------------------------------------------------------------------------
tOplkError timesyncucal_init(tSyncCb pfnSyncCb_p)
{
#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)
tOplkError ret;
#endif
UNUSED_PARAMETER(pfnSyncCb_p);
OPLK_MEMSET(&instance_l, 0, sizeof(tTimesyncucalInstance));
instance_l.syncSem = sem_open(TIMESYNC_SYNC_BSDSEM, O_CREAT, S_IRWXG, 1);
if (instance_l.syncSem == SEM_FAILED)
{
DEBUG_LVL_ERROR_TRACE("%s() creating sem failed!\n", __func__);
return kErrorNoResource;
}
#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)
ret = createTimestampShm();
if (ret != kErrorOk)
{
// Close the semaphore
sem_close(instance_l.syncSem);
// Unlink the semaphore
sem_unlink(TIMESYNC_SYNC_BSDSEM);
return ret;
}
#endif
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError timesyncucal_init(tSyncCb pfnSyncCb_p)
{
tHostifReturn hifRet;
OPLK_MEMSET(&instance_l, 0, sizeof(tTimesyncucalInstance));
instance_l.pHifInstance = hostif_getInstance(0);
if (instance_l.pHifInstance == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s: Could not find hostif instance!\n", __func__);
return kErrorNoResource;
}
hifRet = hostif_irqRegHdl(instance_l.pHifInstance, kHostifIrqSrcSync, hostifIrqSyncCb);
if (hifRet != kHostifSuccessful)
{
DEBUG_LVL_ERROR_TRACE("%s: Enable irq not possible!\n", __func__);
return kErrorNoResource;
}
instance_l.pfnSyncCb = pfnSyncCb_p;
#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)
instance_l.pSharedMemory = (tTimesyncSharedMemory*)getSharedMemory(instance_l.pHifInstance);
#endif
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError sdotestcom_init(sdoApiCbComTest sdoComCbApi_p)
{
tOplkError ret = kErrorOk;
tCircBufError cbret;
OPLK_MEMSET(&sdoTestComInst, 0, sizeof(sdoTestComInst));
sdoTestComInst.tCmdCon.tState = kOplkTestSdoComStateIdle;
sdoTestComInst.tApiCb = sdoComCbApi_p;
// Init sequence layer
ret = sdoseq_init(sdotestcom_receiveCb, sdotestcom_conCb);
if (kErrorOk != ret)
{
return kErrorInvalidOperation;
}
// Init circular buffer for module internal communication
cbret = circbuf_alloc(CIRCBUF_USER_INTERNAL_QUEUE, CONFIG_EVENT_SIZE_CIRCBUF_USER_INTERNAL, &sdoTestComInst.tCmdCon.pCbBufInst);
if (kCircBufOk != cbret)
{
(void)sdoseq_delInstance();
return kErrorInvalidOperation;
}
return ret;
}
//------------------------------------------------------------------------------
tOplkError edrvcyclic_startCycle(void)
{
tOplkError ret = kErrorOk;
if (edrvcyclicInstance_l.cycleTimeUs == 0)
{
ret = kErrorEdrvInvalidCycleLen;
goto Exit;
}
// clear Tx buffer list
edrvcyclicInstance_l.curTxBufferList = 0;
edrvcyclicInstance_l.curTxBufferEntry = 0;
OPLK_MEMSET(edrvcyclicInstance_l.ppTxBufferList, 0,
sizeof(*edrvcyclicInstance_l.ppTxBufferList) * edrvcyclicInstance_l.maxTxBufferCount * 2);
ret = hrestimer_modifyTimer(&edrvcyclicInstance_l.timerHdlCycle,
edrvcyclicInstance_l.cycleTimeUs * 1000ULL,
timerHdlCycleCb, 0L, TRUE);
#if (EDRV_USE_TTTX == TRUE)
edrvcyclicInstance_l.fNextCycleValid = FALSE;
#endif
#if CONFIG_EDRV_CYCLIC_USE_DIAGNOSTICS != FALSE
edrvcyclicInstance_l.lastSlotTimeStamp = 0;
#endif
Exit:
return ret;
}
//------------------------------------------------------------------------------
static void receiveFromSocket(const tSdoUdpSocketInstance* pInstance_p)
{
struct sockaddr_in remoteAddr;
int error;
UINT8 aBuffer[SDO_MAX_RX_FRAME_SIZE_UDP];
size_t size;
tSdoUdpCon sdoUdpCon;
OPLK_MEMSET(&remoteAddr, 0, sizeof(remoteAddr));
size = sizeof(struct sockaddr);
error = recvfrom(pInstance_p->udpSocket,
(char*)&aBuffer[0],
sizeof(aBuffer),
0,
(struct sockaddr*)&remoteAddr,
(socklen_t*)&size);
if (error > 0)
{
const tAsySdoSeq* pSdoSeqData;
size_t dataSize = (size_t)error - ASND_HEADER_SIZE;
pSdoSeqData = (const tAsySdoSeq*)&aBuffer[ASND_HEADER_SIZE];
sdoUdpCon.ipAddr = remoteAddr.sin_addr.s_addr;
sdoUdpCon.port = remoteAddr.sin_port;
sdoudp_receiveData(&sdoUdpCon, pSdoSeqData, dataSize);
}
else
{
DEBUG_LVL_SDO_TRACE("%s() error=%d\n", __func__, error);
}
}
//------------------------------------------------------------------------------
tOplkError ctrlucal_init(void)
{
tDualprocReturn dualRet;
tDualprocConfig dualProcConfig;
INT loopCount = 0;
OPLK_MEMSET(&instance_l, 0, sizeof(tCtrluCalInstance));
OPLK_MEMSET(&dualProcConfig, 0, sizeof(tDualprocConfig));
dualProcConfig.procInstance = kDualProcSecond;
dualProcConfig.procId = CTRL_PROC_ID;
dualRet = dualprocshm_create(&dualProcConfig, &instance_l.dualProcDrvInst);
if (dualRet != kDualprocSuccessful)
{
DEBUG_LVL_ERROR_TRACE("%s Could not create dual processor driver instance (0x%X)\n",
__func__, dualRet);
dualprocshm_delete(instance_l.dualProcDrvInst);
return kErrorNoResource;
}
for (loopCount = 0; loopCount <= DPSHM_ENABLE_TIMEOUT_SEC; loopCount++)
{
target_msleep(1000U);
dualRet = dualprocshm_checkShmIntfState(instance_l.dualProcDrvInst);
if (dualRet != kDualprocshmIntfDisabled)
break;
}
if (dualRet != kDualprocshmIntfEnabled)
{
DEBUG_LVL_ERROR_TRACE("%s dualprocshm interface is not enabled (0x%X)\n",
__func__, dualRet);
return kErrorNoResource;
}
dualRet = dualprocshm_initInterrupts(instance_l.dualProcDrvInst);
if (dualRet != kDualprocSuccessful)
{
DEBUG_LVL_ERROR_TRACE("%s Error Initializing interrupts %x\n ", __func__, dualRet);
return kErrorNoResource;
}
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError oplk_allocProcessImage(UINT sizeProcessImageIn_p, UINT sizeProcessImageOut_p)
{
tOplkError ret = kErrorOk;
TRACE("%s(): Alloc(%u, %u)\n", __func__, sizeProcessImageIn_p,
sizeProcessImageOut_p);
if (!ctrlu_stackIsInitialized())
return kErrorApiNotInitialized;
if ((instance_l.inputImage.pImage != NULL) || (instance_l.outputImage.pImage != NULL))
{
return kErrorApiPIAlreadyAllocated;
}
if (sizeProcessImageIn_p != 0)
{
instance_l.inputImage.pImage = OPLK_MALLOC(sizeProcessImageIn_p);
if (instance_l.inputImage.pImage == NULL)
{
return kErrorApiPIOutOfMemory;
}
instance_l.inputImage.imageSize = sizeProcessImageIn_p;
OPLK_MEMSET(instance_l.inputImage.pImage, 0x00, sizeProcessImageIn_p);
}
if (sizeProcessImageOut_p != 0)
{
instance_l.outputImage.pImage = OPLK_MALLOC(sizeProcessImageOut_p);
if (instance_l.outputImage.pImage == NULL)
{
// Output image allocation failed, therefore we free input image to be consistent
oplk_freeProcessImage();
return kErrorApiPIOutOfMemory;
}
instance_l.outputImage.imageSize = sizeProcessImageOut_p;
OPLK_MEMSET(instance_l.outputImage.pImage, 0x00, sizeProcessImageOut_p);
}
TRACE("%s: Alloc(%p, %u, %p, %u)\n", __func__,
instance_l.inputImage.pImage, instance_l.inputImage.imageSize,
instance_l.outputImage.pImage, instance_l.outputImage.imageSize);
return ret;
}
//------------------------------------------------------------------------------
tOplkError ledu_exit(void)
{
tOplkError ret = kErrorOk;
ret = timeru_deleteTimer(&leduInstance_g.timerHdlLedBlink);
OPLK_MEMSET(&leduInstance_g, 0, sizeof(tLeduInstance));
return ret;
}
//------------------------------------------------------------------------------
static tOplkError initPowerlink(tInstance* pInstance_p)
{
tOplkError ret = kErrorOk;
static tOplkApiInitParam initParam;
PRINTF("Initializing openPOWERLINK stack...\n");
OPLK_MEMSET(&initParam, 0, sizeof(initParam));
initParam.sizeOfInitParam = sizeof(initParam);
initParam.nodeId = pInstance_p->nodeId;
initParam.ipAddress = (0xFFFFFF00 & IP_ADDR) | initParam.nodeId;
OPLK_MEMCPY(initParam.aMacAddress, pInstance_p->aMacAddr, sizeof(initParam.aMacAddress));
initParam.fAsyncOnly = FALSE;
initParam.featureFlags = -1;
initParam.cycleLen = pInstance_p->cycleLen; // required for error detection
initParam.isochrTxMaxPayload = 36; // const
initParam.isochrRxMaxPayload = 36; // const
initParam.presMaxLatency = 2000; // const; only required for IdentRes
initParam.asndMaxLatency = 2000; // const; only required for IdentRes
initParam.preqActPayloadLimit = 36; // required for initialization (+28 bytes)
initParam.presActPayloadLimit = 36; // required for initialization of Pres frame (+28 bytes)
initParam.multiplCylceCnt = 0; // required for error detection
initParam.asyncMtu = 300; // required to set up max frame size
initParam.prescaler = 2; // required for sync
initParam.lossOfFrameTolerance = 100000;
initParam.asyncSlotTimeout = 3000000;
initParam.waitSocPreq = 0;
initParam.deviceType = -1; // NMT_DeviceType_U32
initParam.vendorId = -1; // NMT_IdentityObject_REC.VendorId_U32
initParam.productCode = -1; // NMT_IdentityObject_REC.ProductCode_U32
initParam.revisionNumber = -1; // NMT_IdentityObject_REC.RevisionNo_U32
initParam.serialNumber = -1; // NMT_IdentityObject_REC.SerialNo_U32
initParam.applicationSwDate = 0;
initParam.applicationSwTime = 0;
initParam.subnetMask = SUBNET_MASK;
initParam.defaultGateway = DEFAULT_GATEWAY;
sprintf((char*)initParam.sHostname, "%02x-%08x", initParam.nodeId, initParam.vendorId);
initParam.syncNodeId = C_ADR_SYNC_ON_SOC;
initParam.fSyncOnPrcNode = FALSE;
// set callback functions
initParam.pfnCbEvent = processEvents;
initParam.pfnCbSync = processSync;
// initialize POWERLINK stack
ret = oplk_init(&initParam);
if (ret != kErrorOk)
{
PRINTF("oplk_init() failed (Error:0x%x!\n", ret);
return ret;
}
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError sdoudp_initSocket(void)
{
OPLK_MEMSET(&instance_l, 0x00, sizeof(instance_l));
instance_l.threadHandle = 0;
instance_l.udpSocket = INVALID_SOCKET;
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_init(void)
{
int result = 0;
UINT index;
tHresTimerInfo* pTimerInfo;
UINT8 reg;
unsigned int irqNo;
OPLK_MEMSET(&hresTimerInstance_l, 0, sizeof(tHresTimerInstance));
hresTimerInstance_l.pIoAddr = ioremap(XTTC_BASE, SIZE);
if (hresTimerInstance_l.pIoAddr == NULL)
{
return kErrorNoResource;
}
PRINTF("%s: IoAddr=%p\n", __func__, hresTimerInstance_l.pIoAddr);
pTtcBaseAddr_l[XTTC1] = (void*)(hresTimerInstance_l.pIoAddr + XTTC1_TIMERBASE);
pTtcBaseAddr_l[XTTC2] = (void*)(hresTimerInstance_l.pIoAddr + XTTC2_TIMERBASE);
pTimerInfo = &hresTimerInstance_l.aTimerInfo[0];
for (index = 0; index < TIMER_COUNT; index++, pTimerInfo++)
{
pTimerInfo->index = index;
irqNo = XTTC_IRQ + index;
result = request_irq(irqNo, timerCounterIsr, IRQF_DISABLED, "Zynq", pTimerInfo);
PRINTF("%s: interrupt%d registered (return=%d)\n", __func__, pTimerInfo->index, result);
if (result != 0)
{
iounmap(hresTimerInstance_l.pIoAddr);
return kErrorNoResource;
}
reg = XTTCPSS_CNT_CNTRL_DISABLE;
XTTCPSS_WRITE_REG(index, XTTCPSS_CNT_CNTRL_OFFSET, reg);
reg = CLK_CNTRL_PRESCALE;
XTTCPSS_WRITE_REG(index, XTTCPSS_CLK_CNTRL_OFFSET, reg);
// set the necessary counter control param , for now we keep the timer disabled
reg = 0;
reg = XTTCPSS_READ_REG(index, XTTCPSS_CNT_CNTRL_OFFSET);
reg |= (XTTCPSS_CNT_CNTRL_EN_WAVE);
XTTCPSS_WRITE_REG(index, XTTCPSS_CNT_CNTRL_OFFSET, reg);
// clear all interrupts
reg = 0;
XTTCPSS_WRITE_REG(index, XTTCPSS_IER_OFFSET, reg);
}
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError nmtcnu_sendNmtRequestEx(UINT nodeId_p, tNmtCommand nmtCommand_p,
void* pNmtCommandData_p, UINT dataSize_p)
{
tOplkError ret;
tFrameInfo nmtRequestFrameInfo;
tPlkFrame nmtRequestFrame;
ret = kErrorOk;
// build frame
OPLK_MEMSET(&nmtRequestFrame.aDstMac[0], 0x00, sizeof(nmtRequestFrame.aDstMac)); // set by DLL
OPLK_MEMSET(&nmtRequestFrame.aSrcMac[0], 0x00, sizeof(nmtRequestFrame.aSrcMac)); // set by DLL
ami_setUint16Be(&nmtRequestFrame.etherType, C_DLL_ETHERTYPE_EPL);
ami_setUint8Le(&nmtRequestFrame.dstNodeId, (BYTE)C_ADR_MN_DEF_NODE_ID); // node id of the MN
ami_setUint8Le(&nmtRequestFrame.messageType, (BYTE)kMsgTypeAsnd);
ami_setUint8Le(&nmtRequestFrame.data.asnd.serviceId, (BYTE)kDllAsndNmtRequest);
ami_setUint8Le(&nmtRequestFrame.data.asnd.payload.nmtRequestService.nmtCommandId,
(BYTE)nmtCommand_p);
ami_setUint8Le(&nmtRequestFrame.data.asnd.payload.nmtRequestService.targetNodeId,
(BYTE)nodeId_p); // target for the nmt command
OPLK_MEMSET(&nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData[0], 0x00,
sizeof(nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData));
if (pNmtCommandData_p && (dataSize_p != 0))
{
OPLK_MEMCPY(&nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData[0],
pNmtCommandData_p,
min(dataSize_p,
sizeof(nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData)));
}
// build info-structure
nmtRequestFrameInfo.pFrame = &nmtRequestFrame;
nmtRequestFrameInfo.frameSize = C_DLL_MINSIZE_NMTREQ; // sizeof(nmtRequestFrame);
// send NMT request
ret = dllucal_sendAsyncFrame(&nmtRequestFrameInfo, kDllAsyncReqPrioNmt);
return ret;
}
//------------------------------------------------------------------------------
tOplkError eventucal_init(void)
{
tOplkError ret = kErrorOk;
OPLK_MEMSET(&instance_l, 0, sizeof(tEventuCalInstance));
if ((instance_l.semUserData = CreateSemaphore(NULL, 0, 100, "Local\\semUserEvent")) == NULL)
goto Exit;
if ((instance_l.semKernelData = CreateSemaphore(NULL, 0, 100, "Local\\semKernelEvent")) == NULL)
goto Exit;
if (eventucal_initQueueCircbuf(kEventQueueK2U) != kErrorOk)
goto Exit;
if (eventucal_initQueueCircbuf(kEventQueueU2K) != kErrorOk)
goto Exit;
eventucal_setSignalingCircbuf(kEventQueueU2K, signalKernelEvent);
if (eventucal_initQueueCircbuf(kEventQueueUInt) != kErrorOk)
goto Exit;
eventucal_setSignalingCircbuf(kEventQueueUInt, signalUserEvent);
instance_l.fStopThread = FALSE;
if ((instance_l.threadHandle = CreateThread(NULL, 0, eventThread, (LPVOID)&instance_l,
0, NULL)) == NULL)
{
TRACE("%s() CreateThread fails! Error:%ld\n", __func__, GetLastError());
goto Exit;
}
// jba set thread priority!!!
instance_l.fInitialized = TRUE;
return kErrorOk;
Exit:
if (instance_l.semUserData != NULL)
CloseHandle(instance_l.semUserData);
if (instance_l.semKernelData != NULL)
CloseHandle(instance_l.semKernelData);
eventucal_exitQueueCircbuf(kEventQueueK2U);
eventucal_exitQueueCircbuf(kEventQueueU2K);
eventucal_exitQueueCircbuf(kEventQueueUInt);
return kErrorNoResource;
}
