//------------------------------------------------------------------------------
static tOplkError handleRxAsndFrame(const tFrameInfo* pFrameInfo_p)
{
    tMsgType    msgType;
    UINT        asndServiceId;
    tOplkError  ret = kErrorOk;

    msgType = (tMsgType)ami_getUint8Le(&pFrameInfo_p->frame.pBuffer->messageType);
    if (msgType != kMsgTypeAsnd)
    {
        ret = kErrorInvalidOperation;
        goto Exit;
    }

    asndServiceId = (UINT)ami_getUint8Le(&pFrameInfo_p->frame.pBuffer->data.asnd.serviceId);
    if (asndServiceId < DLL_MAX_ASND_SERVICE_ID)
    {   // ASnd service ID is valid
        if (instance_l.apfnDlluCbAsnd[asndServiceId] != NULL)
        {   // handler was registered
            ret = instance_l.apfnDlluCbAsnd[asndServiceId](pFrameInfo_p);
        }
    }

Exit:
    return ret;
}
//------------------------------------------------------------------------------
static tOplkError handleNotRxAsndFrame(const tDllAsndNotRx* pAsndNotRx_p)
{
    tOplkError  ret = kErrorOk;
    UINT8       aBuffer[DLLUCAL_NOTRX_FRAME_SIZE];
    tPlkFrame*  pFrame = (tPlkFrame*)aBuffer;
    tFrameInfo  frameInfo;
    UINT        asndServiceId;

    ami_setUint8Le(&pFrame->srcNodeId, pAsndNotRx_p->nodeId);
    ami_setUint8Le(&pFrame->messageType, (UINT8)kMsgTypeAsnd);
    ami_setUint8Le(&pFrame->data.asnd.serviceId, pAsndNotRx_p->serviceId);

    frameInfo.frameSize = DLLUCAL_NOTRX_FRAME_SIZE;
    frameInfo.frame.pBuffer = pFrame;

    asndServiceId = (UINT)ami_getUint8Le(&pFrame->data.asnd.serviceId);
    if (asndServiceId < DLL_MAX_ASND_SERVICE_ID)
    {   // ASnd service ID is valid
        if (instance_l.apfnDlluCbAsnd[asndServiceId] != NULL)
        {   // handler was registered
            ret = instance_l.apfnDlluCbAsnd[asndServiceId](&frameInfo);
        }
    }

    return ret;
}
//------------------------------------------------------------------------------
static tOplkError syncResponseCb(const tFrameInfo* pFrameInfo_p)
{
    tOplkError          ret = kErrorOk;
    UINT                nodeId;
    UINT                index;
    tSyncuCbResponse    pfnCbResponse;

    nodeId = ami_getUint8Le(&pFrameInfo_p->frame.pBuffer->srcNodeId);
    index  = nodeId - 1;

    if (index < tabentries(syncuInstance_g.aSyncRespQueue))
    {
        // memorize pointer to callback function
        pfnCbResponse = readResponseQueue(nodeId);
        if (pfnCbResponse == NULL)
        {   // response was not requested
            return ret;
        }

        if (pFrameInfo_p->frameSize < C_DLL_MINSIZE_SYNCRES)
        {   // SyncResponse not received or it has invalid size
            ret = pfnCbResponse(nodeId, NULL);
        }
        else
        {   // SyncResponse received
            ret = pfnCbResponse(nodeId, &pFrameInfo_p->frame.pBuffer->data.asnd.payload.syncResponse);
        }
    }

    return ret;
}
Ejemplo n.º 4
0
//------------------------------------------------------------------------------
static tNmtCommand getNmtCommand(tFrameInfo* pFrameInfo_p)
{
    tNmtCommand          nmtCommand;
    tNmtCommandService*  pNmtCommandService;

    pNmtCommandService = &pFrameInfo_p->frame.pBuffer->data.asnd.payload.nmtCommandService;
    nmtCommand = (tNmtCommand)ami_getUint8Le(&pNmtCommandService->nmtCommandId);

    return nmtCommand;
}
Ejemplo n.º 5
0
//------------------------------------------------------------------------------
static tOplkError downloadObject(tCfmNodeInfo* pNodeInfo_p)
{
    tOplkError          ret = kErrorOk;

    // forward data pointer for last transfer
    pNodeInfo_p->pDataConciseDcf += pNodeInfo_p->curDataSize;
    pNodeInfo_p->bytesRemaining -= pNodeInfo_p->curDataSize;

    if (pNodeInfo_p->entriesRemaining > 0)
    {
        if (pNodeInfo_p->bytesRemaining < CDC_OFFSET_DATA)
        {
            // not enough bytes left in ConciseDCF
            pNodeInfo_p->eventCnProgress.error = kErrorCfmInvalidDcf;
            if ((ret = callCbProgress(pNodeInfo_p)) != kErrorOk)
                return ret;
            return finishConfig(pNodeInfo_p, kNmtNodeCommandConfErr);
        }

        // fetch next item from ConciseDCF
        pNodeInfo_p->eventCnProgress.objectIndex = ami_getUint16Le(&pNodeInfo_p->pDataConciseDcf[CDC_OFFSET_INDEX]);
        pNodeInfo_p->eventCnProgress.objectSubIndex = ami_getUint8Le(&pNodeInfo_p->pDataConciseDcf[CDC_OFFSET_SUBINDEX]);
        pNodeInfo_p->curDataSize = (UINT)ami_getUint32Le(&pNodeInfo_p->pDataConciseDcf[CDC_OFFSET_SIZE]);
        pNodeInfo_p->pDataConciseDcf += CDC_OFFSET_DATA;
        pNodeInfo_p->bytesRemaining -= CDC_OFFSET_DATA;
        pNodeInfo_p->eventCnProgress.bytesDownloaded += CDC_OFFSET_DATA;

        if ((pNodeInfo_p->bytesRemaining < pNodeInfo_p->curDataSize) ||
            (pNodeInfo_p->curDataSize == 0))
        {
            // not enough bytes left in ConciseDCF
            pNodeInfo_p->eventCnProgress.error = kErrorCfmInvalidDcf;
            if ((ret = callCbProgress(pNodeInfo_p)) != kErrorOk)
                return ret;
            return finishConfig(pNodeInfo_p, kNmtNodeCommandConfErr);
        }

        pNodeInfo_p->entriesRemaining--;
        ret = sdoWriteObject(pNodeInfo_p, pNodeInfo_p->pDataConciseDcf, pNodeInfo_p->curDataSize);
        if (ret != kErrorOk)
            return ret;
    }
    else
    {   // download finished
#if defined(CONFIG_INCLUDE_NMT_RMN)
        ret = downloadNetConf(pNodeInfo_p);
#else
        ret = finishDownload(pNodeInfo_p);
#endif
        if (ret != kErrorOk)
            return ret;
    }

    return ret;
}
//------------------------------------------------------------------------------
static tOplkError processCdc(tObdCdcInfo* pCdcInfo_p)
{
    tOplkError      ret = kErrorOk;
    UINT32          entriesRemaining;
    UINT            objectIndex;
    UINT            objectSubIndex;
    size_t          curDataSize;

    if ((ret = loadNextBuffer(pCdcInfo_p, sizeof(UINT32))) != kErrorOk)
        return ret;

    entriesRemaining = ami_getUint32Le(pCdcInfo_p->pCurBuffer);

    if (entriesRemaining == 0)
    {
        ret = eventu_postError(kEventSourceObdu, kErrorObdNoConfigData, 0, NULL);
        return ret;
    }

    for (; entriesRemaining != 0; entriesRemaining--)
    {
        if ((ret = loadNextBuffer(pCdcInfo_p, CDC_OFFSET_DATA))  != kErrorOk)
            return ret;

        objectIndex = ami_getUint16Le(&pCdcInfo_p->pCurBuffer[CDC_OFFSET_INDEX]);
        objectSubIndex = ami_getUint8Le(&pCdcInfo_p->pCurBuffer[CDC_OFFSET_SUBINDEX]);
        curDataSize = (size_t)ami_getUint32Le(&pCdcInfo_p->pCurBuffer[CDC_OFFSET_SIZE]);

        DEBUG_LVL_OBD_TRACE("%s: Reading object 0x%04X/%u with size %u from CDC\n",
                             __func__, objectIndex, objectSubIndex, curDataSize);
        if ((ret = loadNextBuffer(pCdcInfo_p, curDataSize)) != kErrorOk)
        {
            DEBUG_LVL_OBD_TRACE("%s: Reading the corresponding data from CDC failed with 0x%02X\n", __func__, ret);
            return ret;
        }

        ret = obd_writeEntryFromLe(objectIndex, objectSubIndex, pCdcInfo_p->pCurBuffer,
                                   (tObdSize)curDataSize);
        if (ret != kErrorOk)
        {
            tEventObdError          obdError;

            obdError.index = objectIndex;
            obdError.subIndex = objectSubIndex;

            DEBUG_LVL_OBD_TRACE("%s: Writing object 0x%04X/%u to local OBD failed with 0x%02X\n",
                                 __func__, objectIndex, objectSubIndex, ret);
            ret = eventu_postError(kEventSourceObdu, ret, sizeof(tEventObdError), &obdError);
            if (ret != kErrorOk)
                return ret;
        }
    }

    return ret;
}
//------------------------------------------------------------------------------
static tOplkError sdoAsndCb(const tFrameInfo* pFrameInfo_p)
{
    tOplkError      ret = kErrorOk;
    UINT            count;
    UINT*           pConnection;
    UINT            nodeId;
    UINT            freeEntry = 0xFFFF;
    tSdoConHdl      sdoConHdl;
    tPlkFrame*      pFrame;

    pFrame = pFrameInfo_p->frame.pBuffer;
    nodeId = ami_getUint8Le(&pFrame->srcNodeId);

    // search corresponding entry in control structure
    count = 0;
    pConnection = &sdoAsndInstance_l.aSdoAsndConnection[0];
    while (count < CONFIG_SDO_MAX_CONNECTION_ASND)
    {
        if (nodeId == *pConnection)
            break;
        else if ((*pConnection == 0) && (freeEntry == 0xFFFF))
        {   // free entry
            freeEntry = count;
        }

        count++;
        pConnection++;
    }

    if (count == CONFIG_SDO_MAX_CONNECTION_ASND)
    {
        if (freeEntry != 0xFFFF)
        {
            pConnection = &sdoAsndInstance_l.aSdoAsndConnection[freeEntry];
            *pConnection = nodeId;
            count = freeEntry;
        }
        else
        {
            DEBUG_LVL_SDO_TRACE("%s(): no free handle\n", __func__);
            return ret;
        }
    }

    sdoConHdl = (tSdoConHdl)(count | SDO_ASND_HANDLE);
    sdoAsndInstance_l.pfnSdoAsySeqCb(sdoConHdl,
                                     &pFrame->data.asnd.payload.sdoSequenceFrame,
                                     (pFrameInfo_p->frameSize - 18));

    return ret;
}
Ejemplo n.º 8
0
//------------------------------------------------------------------------------
static BOOL checkNodeIdList(UINT8* pbNmtCommandDate_p)
{
    BOOL            fNodeIdInList;
    UINT            byteOffset = nmtCnuInstance_g.extNmtCmdByteOffset;
    UINT8           bitMask = nmtCnuInstance_g.extNmtCmdBitMask;
    UINT8           nodeListByte;

    nodeListByte = ami_getUint8Le(&pbNmtCommandDate_p[byteOffset]);
    if ((nodeListByte & bitMask) == 0)
        fNodeIdInList = FALSE;
    else
        fNodeIdInList = TRUE;

    return fNodeIdInList;
}
//------------------------------------------------------------------------------
tOplkError dllk_deleteNodeFilter(tDllkNodeInfo* pIntNodeInfo_p, tDllNodeOpType nodeOpType_p,
                                 BOOL fUpdateEdrv_p)
{
    tOplkError      ret = kErrorOk;
    BYTE            bPresFilterFlags = 0;

    switch (nodeOpType_p)
    {
        case kDllNodeOpTypeFilterPdo:
            bPresFilterFlags = DLLK_FILTER_FLAG_PDO;
            break;

        case kDllNodeOpTypeFilterHeartbeat:
            bPresFilterFlags = DLLK_FILTER_FLAG_HB;
            break;

        default:
            ret = kErrorDllInvalidParam;
            goto Exit;
    }

    pIntNodeInfo_p->presFilterFlags &= ~bPresFilterFlags;

    if (fUpdateEdrv_p != FALSE)
    {
        if ((pIntNodeInfo_p->presFilterFlags & (DLLK_FILTER_FLAG_PDO | DLLK_FILTER_FLAG_HB)) == 0)
        {
#if CONFIG_DLL_PRES_FILTER_COUNT < 0
            if (dllkInstance_g.usedPresFilterCount > 0)
                dllkInstance_g.usedPresFilterCount--;

            if (dllkInstance_g.usedPresFilterCount == 0)
            {
                // disable PRes Rx filter
                dllkInstance_g.aFilter[DLLK_FILTER_PRES].fEnable = FALSE;
                ret = edrv_changeRxFilter(dllkInstance_g.aFilter, DLLK_FILTER_COUNT,
                                       DLLK_FILTER_PRES, EDRV_FILTER_CHANGE_STATE);
                if (ret != kErrorOk)
                    goto Exit;
            }

#else
            UINT handle;

            for (handle = DLLK_FILTER_PRES; handle < DLLK_FILTER_COUNT; handle++)
            {
                if (ami_getUint8Le(&dllkInstance_g.aFilter[handle].aFilterValue[16]) ==
                                                pIntNodeInfo_p->nodeId)
                {
                    ami_setUint8Be(&dllkInstance_g.aFilter[handle].aFilterValue[16], C_ADR_INVALID);
                    dllkInstance_g.aFilter[handle].fEnable = FALSE;

                    ret = edrv_changeRxFilter(dllkInstance_g.aFilter, DLLK_FILTER_COUNT,
                                           handle, EDRV_FILTER_CHANGE_STATE);
                    if (ret != kErrorOk)
                        goto Exit;
                    break;
                }
            }
#endif
        }
    }
Exit:
    return ret;
}
Ejemplo n.º 10
0
//---------------------------------------------------------------------------
static tOplkError copyTxPdo(tPlkFrame* pFrame_p, UINT frameSize_p, BOOL fReadyFlag_p)
{
    tOplkError          ret = kErrorOk;
    BYTE                flag1;
    UINT                nodeId;
    tMsgType            msgType;
    tPdoChannel*        pPdoChannel;
    UINT                channelId;
    UINT16              pdoSize;

    // set TPDO invalid, so that only fully processed TPDOs are sent as valid
    flag1 = ami_getUint8Le(&pFrame_p->data.pres.flag1);
    ami_setUint8Le(&pFrame_p->data.pres.flag1, (flag1 & ~PLK_FRAME_FLAG1_RD));

    // retrieve POWERLINK message type
    msgType = (tMsgType)ami_getUint8Le(&pFrame_p->messageType);
    if (msgType == kMsgTypePres)
    {   // TPDO is PRes frame
        nodeId = PDO_PRES_NODE_ID;  // 0x00
    }
    else
    {   // TPDO is PReq frame
        // retrieve node ID
        nodeId = ami_getUint8Le(&pFrame_p->dstNodeId);
    }

    if (pdokInstance_g.fRunning)
    {
        // Get PDO channel reference
        channelId = pdokInstance_g.aTpdoChannelIdLut[nodeId];
        pPdoChannel = &pdokInstance_g.pdoChannels.pTxPdoChannel[channelId];

        // valid TPDO found
        if ((pPdoChannel->nodeId == nodeId) &&
            ((unsigned int)(pPdoChannel->pdoSize + 24) <= frameSize_p))
        {
            /*
            TRACE("%s() Channel:%d Node:%d MapObjectCnt:%d PdoSize:%d\n",
                  __func__, channelId, nodeId, pPdoChannel->mappObjectCount,
                  pPdoChannel->pdoSize);
            */

            // set PDO version in frame
            ami_setUint8Le(&pFrame_p->data.pres.pdoVersion, pPdoChannel->mappingVersion);

            pdokcal_readTxPdo(channelId, &pFrame_p->data.pres.aPayload[0],
                              pPdoChannel->pdoSize);

            // set PDO size in frame
            pdoSize = pPdoChannel->pdoSize;
        }
        else
        {   // TPDO is too short or invalid
            // $$$ raise PDO error, set ret
            pdoSize = 0;
        }
    }
    else
    {
        // set PDO size in frame to zero, because no TPDO mapped
        pdoSize = 0;
    }

    // set PDO size in frame
    ami_setUint16Le(&pFrame_p->data.pres.sizeLe, pdoSize);

    if (fReadyFlag_p != FALSE)
    {
        // set TPDO valid
        ami_setUint8Le(&pFrame_p->data.pres.flag1, (flag1 | PLK_FRAME_FLAG1_RD));
    }

    return ret;
}
Ejemplo n.º 11
0
//------------------------------------------------------------------------------
tOplkError pdok_processRxPdo(tPlkFrame* pFrame_p, UINT frameSize_p)
{
    tOplkError          ret = kErrorOk;
    BYTE                frameData;
    UINT                nodeId;
    tMsgType            msgType;
    tPdoChannel*        pPdoChannel;
    UINT                channelId;

    // check if received RPDO is valid
    frameData = ami_getUint8Le(&pFrame_p->data.pres.flag1);
    if ((frameData & PLK_FRAME_FLAG1_RD) == 0)
    {   // RPDO invalid
        goto Exit;
    }

    // retrieve POWERLINK message type
    msgType = (tMsgType)ami_getUint8Le(&pFrame_p->messageType);
    if (msgType == kMsgTypePreq)
    {   // RPDO is PReq frame
        nodeId = PDO_PREQ_NODE_ID;  // 0x00
    }
    else
    {   // RPDO is PRes frame
        // retrieve node ID
        nodeId = ami_getUint8Le(&pFrame_p->srcNodeId);
    }

    if (pdokInstance_g.fRunning)
    {
        // Get PDO channel reference
        channelId = pdokInstance_g.aRpdoChannelIdLut[nodeId];
        pPdoChannel = &pdokInstance_g.pdoChannels.pRxPdoChannel[channelId];

        if (pPdoChannel->nodeId != nodeId)
        {   // we received a PDO which we aren't interested in
            // discard it
            goto Exit;
        }

        // retrieve PDO version from frame
        frameData = ami_getUint8Le(&pFrame_p->data.pres.pdoVersion);
        if ((pPdoChannel->mappingVersion & PLK_VERSION_MAIN) != (frameData & PLK_VERSION_MAIN))
        {   // PDO versions do not match
            // $$$ raise PDO error
            // terminate processing of this RPDO
            goto Exit;
        }

        // valid RPDO found

        if ((unsigned int)(pPdoChannel->pdoSize + PLK_FRAME_OFFSET_PDO_PAYLOAD) > frameSize_p)
        {   // RPDO is too short
            // $$$ raise PDO error, set Ret
            goto Exit;
        }

        /*
        TRACE("%s() Channel:%d Node:%d MapObjectCnt:%d PdoSize:%d\n",
              __func__, channelId, nodeId, pPdoChannel->mappObjectCount,
              pPdoChannel->pdoSize);
        */

        pdokcal_writeRxPdo(channelId,
                           &pFrame_p->data.pres.aPayload[0],
                           pPdoChannel->pdoSize);
    }

Exit:
#if CONFIG_DLL_DEFERRED_RXFRAME_RELEASE_SYNC != FALSE
    dllk_releaseRxFrame(pFrame_p, frameSize_p);
    // $$$ return value?
#endif

    return ret;
}