//------------------------------------------------------------------------------ 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 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 downloadNetConf(tCfmNodeInfo* pNodeInfo_p) { tOplkError ret = kErrorOk; if (pNodeInfo_p->cfmState == kCfmStateDownload) { const tIdentResponse* pIdentResponse = NULL; identu_getIdentResponse(pNodeInfo_p->eventCnProgress.nodeId, &pIdentResponse); if (pIdentResponse == NULL) { DEBUG_LVL_CFM_TRACE("CN%x Ident Response is NULL\n", pNodeInfo_p->eventCnProgress.nodeId); return kErrorInvalidNodeId; } if (ami_getUint32Le(&pIdentResponse->featureFlagsLe) & NMT_FEATUREFLAGS_CFM) { pNodeInfo_p->cfmState = kCfmStateDownloadNetConf; pNodeInfo_p->entriesRemaining = NMT_MAX_NODE_ID; pNodeInfo_p->eventCnProgress.objectIndex = 0x1F22; } } for (; pNodeInfo_p->entriesRemaining > 0; pNodeInfo_p->entriesRemaining--) { UINT subindex = NMT_MAX_NODE_ID - pNodeInfo_p->entriesRemaining + 1; tObdSize obdSize; const UINT8* pData; obdSize = obdu_getDataSize(0x1F22, subindex); // Download only cDCFs with at least one entry if (obdSize <= 4) continue; // fetch pointer to ConciseDCF from object 0x1F22 // (this allows the application to link its own memory to this object) pData = (const UINT8*)obdu_getObjectDataPtr(0x1F22, subindex); if (pData == NULL) return kErrorCfmNoConfigData; pNodeInfo_p->entriesRemaining--; pNodeInfo_p->eventCnProgress.objectSubIndex = subindex; ret = sdoWriteObject(pNodeInfo_p, pData, (UINT)obdSize); return ret; } if (pNodeInfo_p->entriesRemaining == 0) { // download finished ret = finishDownload(pNodeInfo_p); if (ret != kErrorOk) return ret; } return ret; }
//------------------------------------------------------------------------------ void ami_getTimeOfDay (void* pAddr_p, tTimeOfDay* pTimeOfDay_p) { pTimeOfDay_p->msec = ami_getUint32Le(((UINT8*)pAddr_p)) & 0x0FFFFFFF; pTimeOfDay_p->days = ami_getUint16Le(((UINT8*)pAddr_p) + 4); }
//------------------------------------------------------------------------------ tOplkError cfmu_processNodeEvent(UINT nodeId_p, tNmtNodeEvent nodeEvent_p, tNmtState nmtState_p) { tOplkError ret = kErrorOk; static UINT32 leSignature; tCfmNodeInfo* pNodeInfo = NULL; tObdSize obdSize; UINT32 expConfTime = 0; UINT32 expConfDate = 0; tIdentResponse* pIdentResponse = NULL; BOOL fDoUpdate = FALSE; BOOL fDoNetConf = FALSE; if ((nodeEvent_p != kNmtNodeEventCheckConf) && (nodeEvent_p != kNmtNodeEventUpdateConf) && (nodeEvent_p != kNmtNodeEventFound) && ((nodeEvent_p != kNmtNodeEventNmtState) || (nmtState_p != kNmtCsNotActive))) return ret; if ((pNodeInfo = allocNodeInfo(nodeId_p)) == NULL) return kErrorInvalidNodeId; if (pNodeInfo->cfmState != kCfmStateIdle) { // Send abort if SDO command is not undefined if (pNodeInfo->sdoComConHdl != UINT_MAX) { // Set node CFM state to an intermediate state to catch the SDO callback pNodeInfo->cfmState = kCfmStateInternalAbort; ret = sdocom_abortTransfer(pNodeInfo->sdoComConHdl, SDO_AC_DATA_NOT_TRANSF_DUE_LOCAL_CONTROL); if (ret != kErrorOk) return ret; // close connection ret = sdocom_undefineConnection(pNodeInfo->sdoComConHdl); pNodeInfo->sdoComConHdl = UINT_MAX; if (ret != kErrorOk) { DEBUG_LVL_CFM_TRACE("SDO Free Error!\n"); return ret; } } // Set node CFM state to idle pNodeInfo->cfmState = kCfmStateIdle; } if ((nodeEvent_p == kNmtNodeEventFound) || ((nodeEvent_p == kNmtNodeEventNmtState) && (nmtState_p == kNmtCsNotActive))) { // just close SDO connection in case of IdentResponse or loss of connection return ret; } pNodeInfo->curDataSize = 0; // fetch pointer to ConciseDCF from object 0x1F22 // (this allows the application to link its own memory to this object) pNodeInfo->pDataConciseDcf = (UINT8*)obd_getObjectDataPtr(0x1F22, nodeId_p); if (pNodeInfo->pDataConciseDcf == NULL) return kErrorCfmNoConfigData; obdSize = obd_getDataSize(0x1F22, nodeId_p); pNodeInfo->bytesRemaining = (UINT32)obdSize; pNodeInfo->eventCnProgress.totalNumberOfBytes = pNodeInfo->bytesRemaining; #if (CONFIG_CFM_CONFIGURE_CYCLE_LENGTH != FALSE) pNodeInfo->eventCnProgress.totalNumberOfBytes += sizeof(UINT32); #endif pNodeInfo->eventCnProgress.bytesDownloaded = 0; pNodeInfo->eventCnProgress.error = kErrorOk; if (obdSize < sizeof(UINT32)) { pNodeInfo->eventCnProgress.error = kErrorCfmInvalidDcf; ret = callCbProgress(pNodeInfo); if (ret != kErrorOk) return ret; return pNodeInfo->eventCnProgress.error; } identu_getIdentResponse(nodeId_p, &pIdentResponse); if (pIdentResponse == NULL) { DEBUG_LVL_CFM_TRACE("CN%x Ident Response is NULL\n", nodeId_p); return kErrorInvalidNodeId; } #if defined(CONFIG_INCLUDE_NMT_RMN) if (ami_getUint32Le(&pIdentResponse->featureFlagsLe) & PLK_FEATURE_CFM) { UINT subindex; // add size of network configuration (domains in CFM_ConciseDcfList_ADOM) for (subindex = 1; subindex <= NMT_MAX_NODE_ID; subindex++) { obdSize = obd_getDataSize(0x1F22, subindex); // Download only cDCFs with at least one entry if (obdSize > 4) pNodeInfo->eventCnProgress.totalNumberOfBytes += (UINT32)obdSize; } fDoNetConf = TRUE; } #endif pNodeInfo->entriesRemaining = ami_getUint32Le(pNodeInfo->pDataConciseDcf); pNodeInfo->pDataConciseDcf += sizeof(UINT32); pNodeInfo->bytesRemaining -= sizeof(UINT32); pNodeInfo->eventCnProgress.bytesDownloaded += sizeof(UINT32); if (pNodeInfo->entriesRemaining == 0) { pNodeInfo->eventCnProgress.error = kErrorCfmNoConfigData; ret = callCbProgress(pNodeInfo); if (ret != kErrorOk) return ret; } else if (nodeEvent_p == kNmtNodeEventUpdateConf) fDoUpdate = TRUE; else { obdSize = sizeof(expConfDate); ret = obd_readEntry(0x1F26, nodeId_p, &expConfDate, &obdSize); if (ret != kErrorOk) { DEBUG_LVL_CFM_TRACE("CN%x Error Reading 0x1F26 returns 0x%X\n", nodeId_p, ret); } obdSize = sizeof(expConfTime); ret = obd_readEntry(0x1F27, nodeId_p, &expConfTime, &obdSize); if (ret != kErrorOk) { DEBUG_LVL_CFM_TRACE("CN%x Error Reading 0x1F27 returns 0x%X\n", nodeId_p, ret); } if ((expConfDate != 0) || (expConfTime != 0)) { // expected configuration date or time is set if ((ami_getUint32Le(&pIdentResponse->verifyConfigurationDateLe) != expConfDate) || (ami_getUint32Le(&pIdentResponse->verifyConfigurationTimeLe) != expConfTime)) { // update configuration because date or time differ from the expected value fDoUpdate = TRUE; } // store configuration in CN at the end of the download pNodeInfo->fDoStore = TRUE; pNodeInfo->eventCnProgress.totalNumberOfBytes += sizeof(UINT32); } else { // expected configuration date and time is not set fDoUpdate = TRUE; // do not store configuration in CN at the end of the download pNodeInfo->fDoStore = FALSE; } } #if (CONFIG_CFM_CONFIGURE_CYCLE_LENGTH != FALSE) obdSize = sizeof(cfmInstance_g.leCycleLength); ret = obd_readEntryToLe(0x1006, 0x00, &cfmInstance_g.leCycleLength, &obdSize); if (ret != kErrorOk) { // local OD access failed DEBUG_LVL_CFM_TRACE("Local OBD read failed %d\n", ret); return ret; } #endif if ((fDoUpdate == FALSE) && !(fDoNetConf && (pNodeInfo->entriesRemaining == 0))) { pNodeInfo->cfmState = kCfmStateIdle; // current version is already available on the CN, no need to write new values, we can continue DEBUG_LVL_CFM_TRACE("CN%x - Cfg Upto Date\n", nodeId_p); ret = downloadCycleLength(pNodeInfo); if (ret == kErrorReject) { pNodeInfo->cfmState = kCfmStateUpToDate; } } else if (nodeEvent_p == kNmtNodeEventUpdateConf) { pNodeInfo->cfmState = kCfmStateDownload; ret = downloadObject(pNodeInfo); if (ret == kErrorOk) { // SDO transfer started ret = kErrorReject; } } else { pNodeInfo->cfmState = kCfmStateWaitRestore; #if defined(CONFIG_INCLUDE_NMT_RMN) if (pNodeInfo->entriesRemaining == 0) { DEBUG_LVL_CFM_TRACE("CN%x - CFM Network-Cfg Update. Restoring Default...\n"); } else #endif { DEBUG_LVL_CFM_TRACE("CN%x - Cfg Mismatch | MN Expects: %lx-%lx ", nodeId_p, expConfDate, expConfTime); DEBUG_LVL_CFM_TRACE("CN Has: %lx-%lx. Restoring Default...\n", ami_getUint32Le(&pIdentResponse->verifyConfigurationDateLe), ami_getUint32Le(&pIdentResponse->verifyConfigurationTimeLe)); } //Restore Default Parameters pNodeInfo->eventCnProgress.totalNumberOfBytes += sizeof(leSignature); ami_setUint32Le(&leSignature, 0x64616F6C); pNodeInfo->eventCnProgress.objectIndex = 0x1011; pNodeInfo->eventCnProgress.objectSubIndex = 0x01; ret = sdoWriteObject(pNodeInfo, &leSignature, sizeof(leSignature)); if (ret == kErrorOk) { // SDO transfer started ret = kErrorReject; } else { // error occurred DEBUG_LVL_CFM_TRACE("CfmCbEvent(Node): sdoWriteObject() returned 0x%02X\n", ret); } } return ret; }