//------------------------------------------------------------------------------ static void getMacAdrs(const char* pIfName_p, UINT8* pMacAddr_p) { ULONG outBufLen; PIP_ADAPTER_ADDRESSES pAdapterAddresses; PIP_ADAPTER_ADDRESSES pAdapter = NULL; UINT32 retVal = 0; // search for the corresponding MAC address via IPHLPAPI outBufLen = sizeof(IP_ADAPTER_ADDRESSES); pAdapterAddresses = (IP_ADAPTER_ADDRESSES*)OPLK_MALLOC(sizeof(IP_ADAPTER_ADDRESSES)); if (pAdapterAddresses == NULL) { DEBUG_LVL_ERROR_TRACE("Error allocating memory needed to call GetAdaptersAdresses\n"); goto Exit; } // Make an initial call to GetAdaptersAdresses to get // the necessary size into the outBufLen variable retVal = GetAdaptersAddresses(AF_UNSPEC, 0, NULL, pAdapterAddresses, &outBufLen); if (retVal == ERROR_BUFFER_OVERFLOW) { OPLK_FREE(pAdapterAddresses); pAdapterAddresses = (IP_ADAPTER_ADDRESSES*)OPLK_MALLOC(outBufLen); if (pAdapterAddresses == NULL) { DEBUG_LVL_ERROR_TRACE("Error allocating memory needed to call GetAdaptersAdresses\n"); goto Exit; } } // Get the real values retVal = GetAdaptersAddresses(AF_UNSPEC, 0, NULL, pAdapterAddresses, &outBufLen); if (retVal == NO_ERROR) { pAdapter = pAdapterAddresses; while (pAdapter) { if (pAdapter->IfType == IF_TYPE_ETHERNET_CSMACD) { if (strstr(pIfName_p, pAdapter->AdapterName) != NULL) { // corresponding adapter found OPLK_MEMCPY(pMacAddr_p, pAdapter->PhysicalAddress, min(pAdapter->PhysicalAddressLength, 6)); break; } } pAdapter = pAdapter->Next; } } else { DEBUG_LVL_ERROR_TRACE("GetAdaptersAddresses failed with error: %d\n", retVal); } if (pAdapterAddresses) OPLK_FREE(pAdapterAddresses); Exit: return; }
//------------------------------------------------------------------------------ tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t size_p) { if ((pInstance_p->pCircBufHeader = OPLK_MALLOC(sizeof(tCircBufHeader))) == NULL) { return kCircBufNoResource; } if ((pInstance_p->pCircBuf = OPLK_MALLOC(size_p)) == NULL) { OPLK_FREE(pInstance_p->pCircBufHeader); return kCircBufNoResource; } return kCircBufOk; }
//------------------------------------------------------------------------------ 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; }
//------------------------------------------------------------------------------ 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 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 timeru_setTimer(tTimerHdl* pTimerHdl_p, ULONG timeInMs_p, tTimerArg argument_p) { tTimeruData* pData; struct itimerspec relTime; tHrtimerSig sig; if (pTimerHdl_p == NULL) return kErrorTimerInvalidHandle; pData = (tTimeruData*)OPLK_MALLOC(sizeof(tTimeruData)); if (pData == NULL) { DEBUG_LVL_ERROR_TRACE("error allocating user timer memory!\n"); return kErrorNoResource; } OPLK_MEMCPY(&pData->timerArg, &argument_p, sizeof(tTimerArg)); addTimer(pData); sig.sigType = kHrtimerSigMsgQueue; sig.sigParam.m_signalMq.msgQueue = timeruInstance_l.msgQueue; sig.sigParam.m_signalMq.m_sigData = (ULONG)pData; if (hrtimer_create(CLOCK_MONOTONIC, &sig, &pData->timer) != 0) { DEBUG_LVL_ERROR_TRACE("%s() Error hrtimer_create!\n", __func__); return kErrorNoResource; } if (timeInMs_p >= 1000) { relTime.it_value.tv_sec = (timeInMs_p / 1000); relTime.it_value.tv_nsec = (timeInMs_p % 1000) * 1000000; } else { relTime.it_value.tv_sec = 0; relTime.it_value.tv_nsec = timeInMs_p * 1000000; } relTime.it_interval.tv_sec = 0; relTime.it_interval.tv_nsec = 0; DEBUG_LVL_TIMERU_TRACE("%s() Set timer:%08x timeInMs_p=%ld\n", __func__, *pData, timeInMs_p); if (hrtimer_settime(pData->timer, 0, &relTime, NULL) < 0) { DEBUG_LVL_ERROR_TRACE("%s() Error hrtimer_settime!\n", __func__); return kErrorTimerNoTimerCreated; } *pTimerHdl_p = (tTimerHdl)pData; return kErrorOk; }
//------------------------------------------------------------------------------ static tOplkError addInstance(tDllCalQueueInstance* ppDllCalQueue_p, tDllCalQueue dllCalQueue_p) { tOplkError ret = kErrorOk; tCircBufError error = kCircBufOk; tDllCalCircBufInstance* pDllCalCircBufInstance; pDllCalCircBufInstance = (tDllCalCircBufInstance*)OPLK_MALLOC(sizeof(tDllCalCircBufInstance)); if (pDllCalCircBufInstance == NULL) { DEBUG_LVL_ERROR_TRACE("%s() malloc error!\n", __func__); ret = kErrorNoResource; goto Exit; } //store parameters in instance pDllCalCircBufInstance->dllCalQueue = dllCalQueue_p; //initialize shared buffer switch (pDllCalCircBufInstance->dllCalQueue) { case kDllCalQueueTxGen: error = circbuf_alloc(CIRCBUF_DLLCAL_TXGEN, CONFIG_DLLCAL_BUFFER_SIZE_TX_GEN, &pDllCalCircBufInstance->pCircBufInstance); break; case kDllCalQueueTxNmt: error = circbuf_alloc(CIRCBUF_DLLCAL_TXNMT, CONFIG_DLLCAL_BUFFER_SIZE_TX_NMT, &pDllCalCircBufInstance->pCircBufInstance); break; case kDllCalQueueTxSync: error = circbuf_alloc(CIRCBUF_DLLCAL_TXSYNC, CONFIG_DLLCAL_BUFFER_SIZE_TX_SYNC, &pDllCalCircBufInstance->pCircBufInstance); break; case kDllCalQueueTxVeth: error = circbuf_alloc(CIRCBUF_DLLCAL_TXVETH, CONFIG_DLLCAL_BUFFER_SIZE_TX_VETH, &pDllCalCircBufInstance->pCircBufInstance); break; default: DEBUG_LVL_ERROR_TRACE("%s() Invalid Queue!\n", __func__); ret = kErrorInvalidInstanceParam; break; } if (error != kCircBufOk) { DEBUG_LVL_ERROR_TRACE("%s() circbuf_alloc error!\n", __func__); ret = kErrorNoResource; goto Exit; } *ppDllCalQueue_p = (tDllCalQueueInstance*)pDllCalCircBufInstance; Exit: return ret; }
//------------------------------------------------------------------------------ tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t* pSize_p) { // Check parameter validity ASSERT(pInstance_p != NULL); ASSERT(pSize_p != NULL); pInstance_p->pCircBufHeader = (tCircBufHeader*)OPLK_MALLOC(sizeof(tCircBufHeader)); if (pInstance_p->pCircBufHeader == NULL) return kCircBufNoResource; pInstance_p->pCircBuf = OPLK_MALLOC(*pSize_p); if (pInstance_p->pCircBuf == NULL) { OPLK_FREE(pInstance_p->pCircBufHeader); return kCircBufNoResource; } return kCircBufOk; }
//------------------------------------------------------------------------------ tOplkError timeru_setTimer(tTimerHdl* pTimerHdl_p, ULONG timeInMs_p, tTimerArg argument_p) { tTimeruData* pData; struct itimerspec relTime; struct itimerspec curTime; struct sigevent sev; if (pTimerHdl_p == NULL) return kErrorTimerInvalidHandle; pData = (tTimeruData*)OPLK_MALLOC(sizeof(tTimeruData)); if (pData == NULL) return kErrorNoResource; OPLK_MEMCPY(&pData->timerArgument, &argument_p, sizeof(tTimerArg)); addTimer(pData); sev.sigev_notify = SIGEV_SIGNAL; sev.sigev_signo = SIGRTMIN; sev.sigev_value.sival_ptr = pData; if (timer_create(CLOCK_MONOTONIC, &sev, &pData->timer) == -1) { DEBUG_LVL_ERROR_TRACE("%s() Error creating timer!\n", __func__); OPLK_FREE(pData); return kErrorNoResource; } if (timeInMs_p >= 1000) { relTime.it_value.tv_sec = (timeInMs_p / 1000); relTime.it_value.tv_nsec = (timeInMs_p % 1000) * 1000000; } else { relTime.it_value.tv_sec = 0; relTime.it_value.tv_nsec = timeInMs_p * 1000000; } /*DEBUG_LVL_TIMERU_TRACE("%s() Set timer: %p, timeInMs_p=%ld\n", __func__, (void *)pData, timeInMs_p); */ relTime.it_interval.tv_sec = 0; relTime.it_interval.tv_nsec = 0; if (timer_settime(pData->timer, 0, &relTime, &curTime) < 0) { DEBUG_LVL_ERROR_TRACE("%s() Error timer_settime!\n", __func__); return kErrorTimerNoTimerCreated; } *pTimerHdl_p = (tTimerHdl)pData; return kErrorOk; }
//------------------------------------------------------------------------------ static tOplkError addInstance(tDllCalQueueInstance* ppDllCalQueue_p, tDllCalQueue dllCalQueue_p) { tOplkError ret = kErrorOk; tCircBufError error = kCircBufOk; tDllCalCircBufInstance* pDllCalCircBufInstance; // Check parameter validity ASSERT(ppDllCalQueue_p != NULL); pDllCalCircBufInstance = (tDllCalCircBufInstance*)OPLK_MALLOC(sizeof(tDllCalCircBufInstance)); if (pDllCalCircBufInstance == NULL) { ret = kErrorNoResource; goto Exit; } //store parameters in instance pDllCalCircBufInstance->dllCalQueue = dllCalQueue_p; //initialize shared buffer switch (pDllCalCircBufInstance->dllCalQueue) { case kDllCalQueueTxGen: error = circbuf_connect(CIRCBUF_DLLCAL_TXGEN, &pDllCalCircBufInstance->pCircBufInstance); break; case kDllCalQueueTxNmt: error = circbuf_connect(CIRCBUF_DLLCAL_TXNMT, &pDllCalCircBufInstance->pCircBufInstance); break; case kDllCalQueueTxSync: error = circbuf_connect(CIRCBUF_DLLCAL_TXSYNC, &pDllCalCircBufInstance->pCircBufInstance); break; case kDllCalQueueTxVeth: error = circbuf_connect(CIRCBUF_DLLCAL_TXVETH, &pDllCalCircBufInstance->pCircBufInstance); break; default: ret = kErrorInvalidInstanceParam; break; } if (error != kCircBufOk) { ret = kErrorNoResource; goto Exit; } *ppDllCalQueue_p = (tDllCalQueueInstance*)pDllCalCircBufInstance; Exit: return ret; }
//------------------------------------------------------------------------------ tOplkError cfmu_cbObdAccess(tObdCbParam MEM* pParam_p) { tOplkError ret = kErrorOk; tObdVStringDomain* pMemVStringDomain; tCfmNodeInfo* pNodeInfo = NULL; UINT8* pBuffer; pParam_p->abortCode = 0; if ((pParam_p->index != 0x1F22) || (pParam_p->obdEvent != kObdEvWrStringDomain)) return ret; // abort any running SDO transfer pNodeInfo = CFM_GET_NODEINFO(pParam_p->subIndex); if ((pNodeInfo != NULL) && (pNodeInfo->sdoComConHdl != UINT_MAX)) { ret = sdocom_abortTransfer(pNodeInfo->sdoComConHdl, SDO_AC_DATA_NOT_TRANSF_DUE_DEVICE_STATE); } pMemVStringDomain = (tObdVStringDomain*)pParam_p->pArg; if ((pMemVStringDomain->objSize != pMemVStringDomain->downloadSize) || (pMemVStringDomain->pData == NULL)) { pNodeInfo = allocNodeInfo(pParam_p->subIndex); if (pNodeInfo == NULL) { pParam_p->abortCode = SDO_AC_OUT_OF_MEMORY; return kErrorNoResource; } pBuffer = pNodeInfo->pObdBufferConciseDcf; if (pBuffer != NULL) { OPLK_FREE(pBuffer); pNodeInfo->pObdBufferConciseDcf = NULL; } pBuffer = (UINT8*)OPLK_MALLOC(pMemVStringDomain->downloadSize); if (pBuffer == NULL) { pParam_p->abortCode = SDO_AC_OUT_OF_MEMORY; return kErrorNoResource; } pNodeInfo->pObdBufferConciseDcf = pBuffer; pMemVStringDomain->pData = pBuffer; pMemVStringDomain->objSize = pMemVStringDomain->downloadSize; } return ret; }
//------------------------------------------------------------------------------ tOplkError timeru_addInstance(void) { int nIdx; // reset instance structure OPLK_MEMSET(&timeruInstance_l, 0, sizeof(timeruInstance_l)); timeruInstance_l.pEntries = OPLK_MALLOC(sizeof(tTimerEntry) * TIMERU_MAX_ENTRIES); if (timeruInstance_l.pEntries == NULL) return kErrorNoResource; timeruInstance_l.pTimerListFirst = NULL; // fill free timer list for (nIdx = 0; nIdx < TIMERU_MAX_ENTRIES-1; nIdx++) { timeruInstance_l.pEntries[nIdx].pNext = &timeruInstance_l.pEntries[nIdx+1]; } timeruInstance_l.pEntries[TIMERU_MAX_ENTRIES-1].pNext = NULL; timeruInstance_l.pFreeListFirst = timeruInstance_l.pEntries; timeruInstance_l.freeEntries = TIMERU_MAX_ENTRIES; timeruInstance_l.minFreeEntries = TIMERU_MAX_ENTRIES; // set start time to a value which is in any case less or equal than getTickCount() // -> the only solution = 0 timeruInstance_l.startTimeInMs = 0; #if (TARGET_SYSTEM == _WIN32_ || TARGET_SYSTEM == _WINCE_ ) InitializeCriticalSection(&timeruInstance_l.aCriticalSections[TIMERU_TIMER_LIST]); InitializeCriticalSection(&timeruInstance_l.aCriticalSections[TIMERU_FREE_LIST]); timeruInstance_l.ahEvents[TIMERU_EVENT_WAKEUP] = CreateEvent(NULL, FALSE, FALSE, NULL); timeruInstance_l.ahEvents[TIMERU_EVENT_SHUTDOWN] = CreateEvent(NULL, FALSE, FALSE, NULL); if (timeruInstance_l.ahEvents[TIMERU_EVENT_WAKEUP] == NULL || timeruInstance_l.ahEvents[TIMERU_EVENT_SHUTDOWN] == NULL) { return kErrorTimerThreadError; } timeruInstance_l.hProcessThread = CreateThread(NULL, 0, processThread, NULL, 0, NULL); if (timeruInstance_l.hProcessThread == NULL) return kErrorTimerThreadError; #endif return kErrorOk; }
//------------------------------------------------------------------------------ tOplkError pdokcal_allocateMem(size_t memSize_p, void** ppPdoMem_p) { // Check parameter validity ASSERT(ppPdoMem_p != NULL); instance_l.pKernelVa = OPLK_MALLOC(memSize_p); if (instance_l.pKernelVa == NULL) { DEBUG_LVL_ERROR_TRACE("%s() Unable to allocate PDO memory !\n", __func__); return kErrorNoResource; } *ppPdoMem_p = instance_l.pKernelVa; instance_l.memSize = memSize_p; return kErrorOk; }
//------------------------------------------------------------------------------ tOplkError pdokcal_allocateMem(size_t memSize_p, BYTE** ppPdoMem_p) { DEBUG_LVL_PDO_TRACE("%s()\n", __func__); pdokcalmem_pPdo_g = (BYTE*)OPLK_MALLOC(memSize_p); if (pdokcalmem_pPdo_g == NULL) { DEBUG_LVL_ERROR_TRACE("%s() malloc failed!\n", __func__); *ppPdoMem_p = NULL; return kErrorNoResource; } *ppPdoMem_p = pdokcalmem_pPdo_g; DEBUG_LVL_PDO_TRACE("%s() Allocated memory for PDO at %p size:%d\n", __func__, *ppPdoMem_p, memSize_p); return kErrorOk; }
//------------------------------------------------------------------------------ tOplkError edrv_allocTxBuffer(tEdrvTxBuffer* pBuffer_p) { // Check parameter validity ASSERT(pBuffer_p != NULL); if (pBuffer_p->maxBufferSize > EDRV_MAX_FRAME_SIZE) return kErrorEdrvNoFreeBufEntry; // allocate buffer with malloc pBuffer_p->pBuffer = (UINT8*)OPLK_MALLOC(pBuffer_p->maxBufferSize); if (pBuffer_p->pBuffer == NULL) return kErrorEdrvNoFreeBufEntry; pBuffer_p->txBufferNumber.pArg = NULL; return kErrorOk; }
//------------------------------------------------------------------------------ tCircBufInstance* circbuf_createInstance(UINT8 id_p) { tCircBufInstance* pInstance; tCircBufArchInstance* pArch; if ((pInstance = OPLK_MALLOC(sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance))) == NULL) return NULL; OPLK_MEMSET(pInstance, 0, sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance)); pInstance->pCircBufArchInstance = (BYTE*)pInstance + sizeof(tCircBufInstance); pInstance->bufferId = id_p; pArch = (tCircBufArchInstance*)pInstance->pCircBufArchInstance; spin_lock_init(&pArch->spinlock); return pInstance; }
//------------------------------------------------------------------------------ static tCfmNodeInfo* allocNodeInfo(UINT nodeId_p) { tCfmNodeInfo* pNodeInfo = NULL; if ((nodeId_p == 0) || (nodeId_p > NMT_MAX_NODE_ID)) return NULL; pNodeInfo = CFM_GET_NODEINFO(nodeId_p); if (pNodeInfo != NULL) return pNodeInfo; pNodeInfo = (tCfmNodeInfo*)OPLK_MALLOC(sizeof(tCfmNodeInfo)); OPLK_MEMSET(pNodeInfo, 0, sizeof(tCfmNodeInfo)); pNodeInfo->eventCnProgress.nodeId = nodeId_p; pNodeInfo->sdoComConHdl = UINT_MAX; CFM_GET_NODEINFO(nodeId_p) = pNodeInfo; return pNodeInfo; }
//------------------------------------------------------------------------------ tCircBufInstance* circbuf_createInstance(UINT8 id_p, BOOL fNew_p) { tCircBufInstance* pInstance; tCircBufArchInstance* pArch; UNUSED_PARAMETER(fNew_p); pInstance = (tCircBufInstance*)OPLK_MALLOC(sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance)); if (pInstance == NULL) return NULL; OPLK_MEMSET(pInstance, 0, sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance)); pInstance->pCircBufArchInstance = (UINT8*)pInstance + sizeof(tCircBufInstance); pInstance->bufferId = id_p; pArch = (tCircBufArchInstance*)pInstance->pCircBufArchInstance; NdisAllocateSpinLock(&pArch->spinlock); return pInstance; }
//------------------------------------------------------------------------------ static tOplkError addInstance(tDllCalQueueInstance* ppDllCalQueue_p, tDllCalQueue dllCalQueue_p) { tOplkError ret = kErrorOk; tDllCalIoctlInstance* pInstance; pInstance = (tDllCalIoctlInstance*)OPLK_MALLOC(sizeof(tDllCalIoctlInstance)); if (pInstance == NULL) { ret = kErrorNoResource; goto Exit; } //store parameters in instance pInstance->dllCalQueue = dllCalQueue_p; pInstance->fd = ctrlucal_getFd(); *ppDllCalQueue_p = (tDllCalQueueInstance*)pInstance; Exit: return ret; }
//------------------------------------------------------------------------------ tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t* pSize_p) { size_t size; tCircBufArchInstance* pArch; pArch = (tCircBufArchInstance*)pInstance_p->pCircBufArchInstance; size = *pSize_p + sizeof(tCircBufHeader); pInstance_p->pCircBufHeader = OPLK_MALLOC(size); if (pInstance_p->pCircBufHeader == NULL) { DEBUG_LVL_ERROR_TRACE("%s() malloc failed!\n", __func__); return kCircBufNoResource; } pInstance_p->pCircBuf = ((BYTE*)pInstance_p->pCircBufHeader) + sizeof(tCircBufHeader); /* save for other threads - shared memory */ pHeader_l[pInstance_p->bufferId] = pInstance_p->pCircBufHeader; return kCircBufOk; }
//------------------------------------------------------------------------------ tOplkError timeru_setTimer(tTimerHdl* pTimerHdl_p, ULONG timeInMs_p, tTimerArg argument_p) { tOplkError ret = kErrorOk; tTimeruData* pData; // check pointer to handle if (pTimerHdl_p == NULL) return kErrorTimerInvalidHandle; pData = (tTimeruData*)OPLK_MALLOC(sizeof(tTimeruData)); if (pData == NULL) return kErrorNoResource; init_timer(&pData->timer); pData->timer.function = cbTimer; pData->timer.data = (unsigned long)pData; pData->timer.expires = jiffies + 1 + ((timeInMs_p * HZ) + 999) / 1000; OPLK_MEMCPY(&pData->timerArgument, &argument_p, sizeof(tTimerArg)); add_timer(&pData->timer); *pTimerHdl_p = (tTimerHdl)pData; return ret; }
//------------------------------------------------------------------------------ tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t* pSize_p) { size_t size = *pSize_p; if (pInstance_p->pCircBufArchInstance == NULL) { // Allocate requested size + header size += sizeof(tCircBufHeader); pInstance_p->pCircBufHeader = OPLK_MALLOC(size); if (pInstance_p->pCircBufHeader == NULL) { DEBUG_LVL_ERROR_TRACE("%s() malloc failed!\n", __func__); return kCircBufNoResource; } pInstance_p->pCircBuf = ((BYTE*)pInstance_p->pCircBufHeader) + sizeof(tCircBufHeader); // Return buffer size: pSize_p already holds the right value! } else { // Queue must use host interface tHostifReturn ret; tHostifInstance pHostif = (tHostifInstance*)pInstance_p->pCircBufArchInstance; UINT8* pBufBase; UINT bufSize; tCircBufHostiBuffer* pHostifBuffer; // Get buffer for queue ret = hostif_getBuf(pHostif, hostifInstance[pInstance_p->bufferId], &pBufBase, &bufSize); if (ret != kHostifSuccessful) { DEBUG_LVL_ERROR_TRACE("%s getting hostif buffer instance failed with 0x%X!\n", __func__, ret); return kCircBufNoResource; } // Check if there is enough memory available if (size > bufSize) { DEBUG_LVL_ERROR_TRACE("%s Hostif buffer (id=%d) only provides %d byte instead of %d byte!\n", __func__, pInstance_p->bufferId, bufSize, size); return kCircBufNoResource; } pHostifBuffer = (tCircBufHostiBuffer*)pBufBase; pInstance_p->pCircBufHeader = &(pHostifBuffer->circBufHeader); pInstance_p->pCircBuf = (UINT8*)pHostifBuffer + sizeof(tCircBufHostiBuffer); size -= sizeof(tCircBufHostiBuffer); // Return buffer size *pSize_p = size; HOSTIF_WR8(&(pHostifBuffer->lock), 0, CIRCBUF_HOSTIF_UNLOCK); } return kCircBufOk; }
//------------------------------------------------------------------------------ tOplkError sdotestseq_sendFrame(UINT nodeId_p, tSdoType sdoType_p, tAsySdoSeq* pSdoSeq_p, size_t sdoSize_p) { tOplkError ret = kErrorOk; tSdoTestSeqCon* pCon; size_t FrameSize; tPlkFrame* pFrame; tAsySdoSeq* pSequDst; // Check parameters FrameSize = sdoSize_p + PLK_FRAME_OFFSET_SDO_SEQU; if (FrameSize > C_DLL_MAX_ASYNC_MTU) { return kErrorInvalidOperation; } // Try to get a valid lower layer connection pCon = &sdoTestSeqInst.seqCon; if (pCon->state == kOplkTestSdoSequConIdle) { // We need a new connection switch (sdoType_p) { case kSdoTypeUdp: #if defined (CONFIG_INCLUDE_SDO_UDP) ret = sdoudp_initCon(&pCon->sdoConHandle, nodeId_p); #else ret = kErrorSdoSeqUnsupportedProt; #endif if (ret != kErrorOk) { return ret; } #if defined (CONFIG_INCLUDE_SDO_UDP) pCon->pFuncTable = &sdoTestSeqUdpFuncs; #endif break; case kSdoTypeAsnd: #if defined (CONFIG_INCLUDE_SDO_ASND) ret = sdoasnd_initCon(&pCon->sdoConHandle, nodeId_p); #else ret = kErrorSdoSeqUnsupportedProt; #endif if (ret != kErrorOk) { return ret; } #if defined (CONFIG_INCLUDE_SDO_ASND) pCon->pFuncTable = &sdoTestSeqAsndFuncs; #endif break; default: case kSdoTypeAuto: case kSdoTypePdo: // Current implementation only supports Asnd and UDP return kErrorSdoSeqUnsupportedProt; } // Save parameters pCon->state = kOplkTestSdoSequConActive; pCon->sdoType = sdoType_p; pCon->nodeId = nodeId_p; } else { // Connection exists, check parameters if ((nodeId_p != pCon->nodeId) || (sdoType_p != pCon->sdoType)) { return kErrorInvalidOperation; } } // Get frame buffer pFrame = (tPlkFrame*)OPLK_MALLOC(FrameSize); if (pFrame == NULL) { ret = kErrorNoResource; } else { // Set up frame OPLK_MEMSET(pFrame, 0, FrameSize); pSequDst = &pFrame->data.asnd.payload.sdoSequenceFrame; OPLK_MEMCPY(pSequDst, pSdoSeq_p, sdoSize_p); ami_setUint8Le(&pFrame->data.asnd.serviceId, (BYTE)kDllAsndSdo); // Send data ret = pCon->pFuncTable->pfnSendData(pCon->sdoConHandle, pFrame, sdoSize_p); OPLK_FREE(pFrame); } return ret; }
//------------------------------------------------------------------------------ static tOplkError setupLocalNodeMn(void) { tOplkError ret = kErrorOk; UINT handle; UINT index; UINT frameSize; UINT count = 0; tDllkNodeInfo* pIntNodeInfo; /*-------------------------------------------------------------------*/ /* register TxFrames in Edrv */ // SoC frameSize = C_DLL_MINSIZE_SOC; ret = dllkframe_createTxFrame(&handle, &frameSize, kMsgTypeSoc, kDllAsndNotDefined); if (ret != kErrorOk) { // error occurred while registering Tx frame return ret; } dllkInstance_g.pTxBuffer[handle].pfnTxHandler = dllkframe_processTransmittedSoc; handle++; dllkInstance_g.pTxBuffer[handle].pfnTxHandler = dllkframe_processTransmittedSoc; // SoA frameSize = C_DLL_MINSIZE_SOA; ret = dllkframe_createTxFrame(&handle, &frameSize, kMsgTypeSoa, kDllAsndNotDefined); if (ret != kErrorOk) { // error occurred while registering Tx frame return ret; } dllkInstance_g.pTxBuffer[handle].pfnTxHandler = dllkframe_processTransmittedSoa; handle++; dllkInstance_g.pTxBuffer[handle].pfnTxHandler = dllkframe_processTransmittedSoa; for (index = 0, pIntNodeInfo = &dllkInstance_g.aNodeInfo[0]; index < tabentries(dllkInstance_g.aNodeInfo); index++, pIntNodeInfo++) { if (pIntNodeInfo->preqPayloadLimit > 0) { // create PReq frame for this node count++; frameSize = pIntNodeInfo->preqPayloadLimit + PLK_FRAME_OFFSET_PDO_PAYLOAD; ret = dllkframe_createTxFrame(&handle, &frameSize, kMsgTypePreq, kDllAsndNotDefined); if (ret != kErrorOk) return ret; pIntNodeInfo->pPreqTxBuffer = &dllkInstance_g.pTxBuffer[handle]; } } // alloc TxBuffer pointer list count += 5; // SoC, PResMN, SoA, ASnd, NULL dllkInstance_g.ppTxBufferList = (tEdrvTxBuffer**)OPLK_MALLOC(sizeof(tEdrvTxBuffer*) * count); if (dllkInstance_g.ppTxBufferList == NULL) return kErrorDllOutOfMemory; ret = edrvcyclic_setMaxTxBufferListSize(count); if (ret != kErrorOk) return ret; ret = edrvcyclic_setCycleTime(dllkInstance_g.dllConfigParam.cycleLen); if (ret != kErrorOk) return ret; ret = edrvcyclic_regSyncHandler(cbMnSyncHandler); if (ret != kErrorOk) return ret; dllkfilter_setupPresFilter(&dllkInstance_g.aFilter[DLLK_FILTER_PRES], TRUE); return ret; }
//------------------------------------------------------------------------------ tOplkError sdotestcom_sendFrame(UINT nodeId_p, tSdoType sdoType_p, tAsySdoCom* pSdoCom_p, size_t sdoSize_p) { tOplkError ret; tSdoTestComCon* pCmdCon; tEvent Event; tCircBufError CbError; tPlkFrame* pFrame; tAsySdoCom* pSdoCom_Dst; size_t FrameSize; ret = kErrorOk; pCmdCon = &sdoTestComInst.tCmdCon; FrameSize = PLK_FRAME_OFFSET_SDO_COMU + sdoSize_p; // Check if parameters are valid if (FrameSize > C_DLL_MAX_ASYNC_MTU) { return kErrorInvalidOperation; } if (kOplkTestSdoComStateIdle != pCmdCon->tState) { // If the connection is already in use, node ID and SDO type have to match if ((pCmdCon->nodeId != nodeId_p) || (pCmdCon->tSdoType != sdoType_p)) { return kErrorInvalidOperation; } } // Get frame buffer pFrame = (tPlkFrame *)OPLK_MALLOC(FrameSize); if (pFrame == NULL) { ret = kErrorNoResource; } // Generate frame pSdoCom_Dst = &pFrame->data.asnd.payload.sdoSequenceFrame.sdoSeqPayload; OPLK_MEMSET(pFrame, 0, FrameSize); OPLK_MEMCPY(pSdoCom_Dst, pSdoCom_p, sdoSize_p); // Save frame in shared buffer CbError = circbuf_writeData(pCmdCon->pCbBufInst, pFrame, FrameSize); OPLK_FREE(pFrame); if (kCircBufOk != CbError) { ret = kErrorInvalidOperation; } else { // Sequence layer handling // Either create a new connection, or reuse existing one switch (pCmdCon->tState) { case kOplkTestSdoComStateIdle: // Get new sequence layer connection // When the connection is ready, the callback will trigger sending ret = sdoseq_initCon(&pCmdCon->tSeqHdl, nodeId_p, sdoType_p); pCmdCon->tState = kOplkTestSdoComStateWaitInit; pCmdCon->tSdoType = sdoType_p; pCmdCon->nodeId = nodeId_p; break; case kOplkTestSdoComStateWaitInit: // Connection setup is already in progress // Nothing left to do break; case kOplkTestSdoComStateConnected: // Connection is already up and running, // just trigger frame send event OPLK_MEMSET(&Event.netTime, 0x00, sizeof(Event.netTime)); Event.eventType = kEventTypeSdoAsySend; Event.pEventArg = pCmdCon; Event.eventArgSize = sizeof(*pCmdCon); Event.eventSink = kEventSinkSdoTest; ret = eventu_postEvent(&Event); break; default: // Reject unknown states ret = kErrorInvalidOperation; break; } } return ret; }
//------------------------------------------------------------------------------ tOplkError sdotestcom_cbEvent(tEvent* pOplkEvent_p) { tOplkError ret; tOplkError Sequret; tSdoTestComCon* pCmdCon; tPlkFrame* pFrame; tCircBufError Cbret; ULONG ulDataSize; size_t size_p; size_t FrameSize; ret = kErrorOk; pFrame = NULL; FrameSize = C_DLL_MAX_ASYNC_MTU; size_p = 0x00; ulDataSize = 0x00; switch (pOplkEvent_p->eventType) { case kEventTypeSdoAsySend: // Check parameter if (sizeof(*pCmdCon) == pOplkEvent_p->eventArgSize) { pCmdCon = (tSdoTestComCon*)pOplkEvent_p->pEventArg; } else { ret = kErrorSdoComInvalidParam; goto Exit; } pFrame = (tPlkFrame*)OPLK_MALLOC(FrameSize); if (pFrame == NULL) { ret = kErrorNoResource; goto Exit; } // Send all frames in that are currently in the shared buffer do { OPLK_MEMSET(pFrame, 0, FrameSize); Cbret = circbuf_readData(pCmdCon->pCbBufInst, pFrame, size_p, &FrameSize); if ((kCircBufOk == Cbret) && (ulDataSize > PLK_FRAME_OFFSET_SDO_COMU)) { Sequret = sdoseq_sendData(pCmdCon->tSeqHdl, ulDataSize - PLK_FRAME_OFFSET_SDO_COMU, pFrame); // Send all frames, but return error code if any frame fails if (Sequret != kErrorOk) { ret = Sequret; } } } while (Cbret == kCircBufOk); // kShbNoReadableData would be the only error free condition // for the preceding loop to end if (kCircBufNoReadableData != Cbret) { ret = kErrorInvalidOperation; } break; default: // Reject unknown events ret = kErrorInvalidOperation; break; } Exit: if (pFrame != NULL) { OPLK_FREE(pFrame); } return ret; }
//------------------------------------------------------------------------------ tOplkError pdok_allocChannelMem(tPdoAllocationParam* pAllocationParam_p) { tOplkError ret = kErrorOk; #if NMT_MAX_NODE_ID > 0 tDllNodeOpParam nodeOpParam; nodeOpParam.opNodeType = kDllNodeOpTypeFilterPdo; nodeOpParam.nodeId = C_ADR_BROADCAST; ret = dllk_deleteNode(&nodeOpParam); if (ret != kErrorOk) { goto Exit; } #endif // NMT_MAX_NODE_ID > 0 if (pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount != pAllocationParam_p->rxPdoChannelCount) { // allocation should be changed pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount = pAllocationParam_p->rxPdoChannelCount; if (pdokInstance_g.pdoChannels.pRxPdoChannel != NULL) { OPLK_FREE(pdokInstance_g.pdoChannels.pRxPdoChannel); pdokInstance_g.pdoChannels.pRxPdoChannel = NULL; } if (pAllocationParam_p->rxPdoChannelCount > 0) { pdokInstance_g.pdoChannels.pRxPdoChannel = (tPdoChannel*)OPLK_MALLOC(sizeof(*pdokInstance_g.pdoChannels.pRxPdoChannel) * pAllocationParam_p->rxPdoChannelCount); if (pdokInstance_g.pdoChannels.pRxPdoChannel == NULL) { ret = kErrorPdoInitError; goto Exit; } } } disablePdoChannels(pdokInstance_g.pdoChannels.pRxPdoChannel, pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount); if (pdokInstance_g.pdoChannels.allocation.txPdoChannelCount != pAllocationParam_p->txPdoChannelCount) { // allocation should be changed pdokInstance_g.pdoChannels.allocation.txPdoChannelCount = pAllocationParam_p->txPdoChannelCount; if (pdokInstance_g.pdoChannels.pTxPdoChannel != NULL) { OPLK_FREE(pdokInstance_g.pdoChannels.pTxPdoChannel); pdokInstance_g.pdoChannels.pTxPdoChannel = NULL; } if (pAllocationParam_p->txPdoChannelCount > 0) { pdokInstance_g.pdoChannels.pTxPdoChannel = (tPdoChannel*)OPLK_MALLOC(sizeof(*pdokInstance_g.pdoChannels.pTxPdoChannel) * pAllocationParam_p->txPdoChannelCount); if (pdokInstance_g.pdoChannels.pTxPdoChannel == NULL) { ret = kErrorPdoInitError; goto Exit; } } } disablePdoChannels(pdokInstance_g.pdoChannels.pTxPdoChannel, pdokInstance_g.pdoChannels.allocation.txPdoChannelCount); Exit: return ret; }
//------------------------------------------------------------------------------ static tOplkError loadNextBuffer(tObdCdcInfo* pCdcInfo_p, size_t bufferSize) { tOplkError ret = kErrorOk; int readSize; UINT8* pBuffer; switch (pCdcInfo_p->type) { case kObdCdcTypeFile: if (pCdcInfo_p->bufferSize < bufferSize) { if (pCdcInfo_p->pCurBuffer != NULL) { OPLK_FREE(pCdcInfo_p->pCurBuffer); pCdcInfo_p->pCurBuffer = NULL; pCdcInfo_p->bufferSize = 0; } pCdcInfo_p->pCurBuffer = OPLK_MALLOC(bufferSize); if (pCdcInfo_p->pCurBuffer == NULL) { ret = eventu_postError(kEventSourceObdu, kErrorObdOutOfMemory, 0, NULL); if (ret != kErrorOk) return ret; return kErrorReject; } pCdcInfo_p->bufferSize = bufferSize; } pBuffer = pCdcInfo_p->pCurBuffer; do { readSize = read(pCdcInfo_p->handle.fdCdcFile, pBuffer, bufferSize); if (readSize <= 0) { ret = eventu_postError(kEventSourceObdu, kErrorObdInvalidDcf, 0, NULL); if (ret != kErrorOk) return ret; return kErrorReject; } pBuffer += readSize; bufferSize -= readSize; pCdcInfo_p->cdcSize -= readSize; } while (bufferSize > 0); break; case kObdCdcTypeBuffer: if (pCdcInfo_p->bufferSize < bufferSize) { ret = eventu_postError(kEventSourceObdu, kErrorObdInvalidDcf, 0, NULL); if (ret != kErrorOk) return ret; return kErrorReject; } pCdcInfo_p->pCurBuffer = pCdcInfo_p->handle.pNextBuffer; pCdcInfo_p->handle.pNextBuffer += bufferSize; pCdcInfo_p->bufferSize -= bufferSize; break; } return ret; }