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