//------------------------------------------------------------------------------
UINT pdoklut_getChannel(tPdoklutEntry* pLut_p, UINT8 index_p, UINT8 nodeId_p)
{
if (index_p >= PDOKLUT_MAX_CHANNELS_PER_NODE)
{
DEBUG_LVL_PDO_TRACE("%s() INVALID CHANNEL: index:%d nodeId:%d\n", __func__, index_p, nodeId_p);
return PDOKLUT_INVALID_CHANNEL;
}
DEBUG_LVL_PDO_TRACE ("%s() channel:%d index:%d nodeId:%d\n",
__func__, pLut_p[nodeId_p].channelId[index_p], index_p, nodeId_p);
return pLut_p[nodeId_p].channelId[index_p];
}
//------------------------------------------------------------------------------
tOplkError pdokcal_freeMem(BYTE* pMem_p, size_t memSize_p)
{
UNUSED_PARAMETER(memSize_p);
DEBUG_LVL_PDO_TRACE("%s()\n", __func__);
OPLK_FREE(pMem_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 pdoucal_initPdoMem(const tPdoChannelSetup* pPdoChannels_p,
size_t rxPdoMemSize_p,
size_t txPdoMemSize_p)
{
void* pMem = NULL;
size_t pdoMemSize;
UNUSED_PARAMETER(pPdoChannels_p);
pdoMemSize = rxPdoMemSize_p + txPdoMemSize_p;
if (pPdoMem_l != NULL)
pdoucal_cleanupPdoMem();
memSize_l = (pdoMemSize * 3) + sizeof(tPdoMemRegion);
if (memSize_l != 0)
{
if (pdoucal_allocateMem(memSize_l, &pMem) != kErrorOk)
{
DEBUG_LVL_ERROR_TRACE("%s() Allocating PDO memory failed!\n", __func__);
return kErrorNoResource;
}
}
pPdoMem_l = (tPdoMemRegion*)pMem;
pTripleBuf_l[0] = (UINT8*)pPdoMem_l + sizeof(tPdoMemRegion);
pTripleBuf_l[1] = (UINT8*)pTripleBuf_l[0] + pdoMemSize;
pTripleBuf_l[2] = (UINT8*)pTripleBuf_l[1] + pdoMemSize;
DEBUG_LVL_PDO_TRACE("%s() Mapped shared memory for PDO mem region at %p size %d\n",
__func__,
pPdoMem_l,
memSize_l);
DEBUG_LVL_PDO_TRACE("%s() Triple buffers at: %p/%p/%p\n",
__func__,
pTripleBuf_l[0],
pTripleBuf_l[1],
pTripleBuf_l[2]);
OPLK_ATOMIC_INIT(pPdoMem_l);
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError pdokcal_freeMem(UINT8* pMem_p, size_t memSize_p)
{
UNUSED_PARAMETER(pMem_p);
UNUSED_PARAMETER(memSize_p);
DEBUG_LVL_PDO_TRACE("%s() try to free address %p (%p)\n",
__func__,
pMem_p,
limPdo_l.pBase);
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError pdoucal_setTxPdo(UINT8 channelId_p,
void* pPdo_p,
size_t pdoSize_p)
{
OPLK_ATOMIC_T temp;
UNUSED_PARAMETER(pPdo_p); // Used to avoid compiler warning if OPLK_DCACHE_FLUSH is not set
UNUSED_PARAMETER(pdoSize_p); // Used to avoid compiler warning if OPLK_DCACHE_FLUSH is not set
OPLK_DCACHE_FLUSH(pPdo_p, pdoSize_p);
DEBUG_LVL_PDO_TRACE("%s() chan:%d wi:%d\n",
__func__,
channelId_p,
pPdoMem_l->txChannelInfo[channelId_p].writeBuf);
// Invalidate data cache already done in pdoucal_getTxPdoAdrs()
OPLK_DCACHE_INVALIDATE(&pPdoMem_l->txChannelInfo[channelId_p],
sizeof(tPdoBufferInfo));
temp = pPdoMem_l->txChannelInfo[channelId_p].writeBuf;
OPLK_ATOMIC_EXCHANGE(&pPdoMem_l->txChannelInfo[channelId_p].cleanBuf,
temp,
pPdoMem_l->txChannelInfo[channelId_p].writeBuf);
pPdoMem_l->txChannelInfo[channelId_p].newData = 1;
// Flush data cache for variables changed in this function
OPLK_DCACHE_FLUSH(&pPdoMem_l->txChannelInfo[channelId_p],
sizeof(tPdoBufferInfo));
DEBUG_LVL_PDO_TRACE("%s() chan:%d new wi:%d\n",
__func__,
channelId_p,
pPdoMem_l->txChannelInfo[channelId_p].writeBuf);
return kErrorOk;
}
//------------------------------------------------------------------------------
void* pdoucal_getTxPdoAdrs(UINT8 channelId_p)
{
OPLK_ATOMIC_T wi;
void* pPdo;
// Invalidate data cache for addressed txChannelInfo
OPLK_DCACHE_INVALIDATE(&(pPdoMem_l->txChannelInfo[channelId_p]), sizeof(tPdoBufferInfo));
wi = pPdoMem_l->txChannelInfo[channelId_p].writeBuf;
DEBUG_LVL_PDO_TRACE("%s() channelId:%d wi:%d\n",
__func__,
channelId_p,
wi);
pPdo = (UINT8*)pTripleBuf_l[wi] + pPdoMem_l->txChannelInfo[channelId_p].channelOffset;
return pPdo;
}
//------------------------------------------------------------------------------
tOplkError pdok_deAllocChannelMem(void)
{
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)
{
DEBUG_LVL_PDO_TRACE("%s() dllk_deleteNode failed (%s)\n",
__func__, debugstr_getRetValStr(Ret));
return Ret;
}
#endif // NMT_MAX_NODE_ID > 0
// de-allocate mem for RX PDO channels
if (pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount != 0)
{
pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount = 0;
if (pdokInstance_g.pdoChannels.pRxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pRxPdoChannel);
pdokInstance_g.pdoChannels.pRxPdoChannel = NULL;
}
}
// de-allocate mem for TX PDO channels
if (pdokInstance_g.pdoChannels.allocation.txPdoChannelCount != 0)
{
pdokInstance_g.pdoChannels.allocation.txPdoChannelCount = 0;
if (pdokInstance_g.pdoChannels.pTxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pTxPdoChannel);
pdokInstance_g.pdoChannels.pTxPdoChannel = NULL;
}
}
return Ret;
}
//------------------------------------------------------------------------------
tOplkError pdokcal_freeMem(UINT8* pMem_p, size_t memSize_p)
{
tDualprocReturn dualRet;
UNUSED_PARAMETER(pMem_p);
UNUSED_PARAMETER(memSize_p);
DEBUG_LVL_PDO_TRACE("%s() try to free address %p\n", __func__, pMem_p);
dualRet = dualprocshm_freeMemory(memPdo_l.pDrvInstance, DUALPROCSHM_BUFF_ID_PDO, TRUE);
if (dualRet != kDualprocSuccessful)
{
DEBUG_LVL_ERROR_TRACE("%s() couldn't free PDO buffer (%d)\n",
__func__,
dualRet);
return kErrorNoResource;
}
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError pdoklut_addChannel(tPdoklutEntry* pLut_p, tPdoChannel* pPdoChannel_p, UINT channelId_p)
{
tOplkError ret = kErrorIllegalInstance;
int i;
UINT8 nodeId;
nodeId = pPdoChannel_p->nodeId;
if (nodeId == 255)
return ret;
for (i = 0; i < PDOKLUT_MAX_CHANNELS_PER_NODE; ++i)
{
if (pLut_p[nodeId].channelId[i] == PDOKLUT_INVALID_CHANNEL)
{
DEBUG_LVL_PDO_TRACE ("Adding PDO Lut channel:%d node:%d index:%d\n", channelId_p, nodeId, i);
pLut_p[nodeId].channelId[i] = channelId_p;
ret = kErrorOk;
break;
}
}
return ret;
}
