//------------------------------------------------------------------------------
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;
}
//------------------------------------------------------------------------------
static tOplkError downloadCycleLength(tCfmNodeInfo* pNodeInfo_p)
{
tOplkError ret = kErrorOk;
#if (CONFIG_CFM_CONFIGURE_CYCLE_LENGTH != FALSE)
pNodeInfo_p->eventCnProgress.objectIndex = 0x1006;
pNodeInfo_p->eventCnProgress.objectSubIndex = 0x00;
ret = sdoWriteObject(pNodeInfo_p, &cfmInstance_g.leCycleLength, sizeof(UINT32));
if (ret == kErrorOk)
{ // SDO transfer started
ret = kErrorReject;
}
else
{
DEBUG_LVL_CFM_TRACE("CN%x Writing 0x1006 returns 0x%X\n", pNodeInfo_p->eventCnProgress.nodeId, ret);
}
#endif
return ret;
}
//------------------------------------------------------------------------------
static tOplkError finishConfig(tCfmNodeInfo* pNodeInfo_p,
tNmtNodeCommand nmtNodeCommand_p)
{
tOplkError ret = kErrorOk;
if (pNodeInfo_p->sdoComConHdl != UINT_MAX)
{
ret = sdocom_undefineConnection(pNodeInfo_p->sdoComConHdl);
pNodeInfo_p->sdoComConHdl = UINT_MAX;
if (ret != kErrorOk)
{
DEBUG_LVL_CFM_TRACE("SDO Free Error!\n");
return ret;
}
}
pNodeInfo_p->cfmState = kCfmStateIdle;
if (cfmInstance_g.pfnCbEventCnResult != NULL)
ret = cfmInstance_g.pfnCbEventCnResult(pNodeInfo_p->eventCnProgress.nodeId, nmtNodeCommand_p);
return ret;
}
//------------------------------------------------------------------------------
static tOplkError cbSdoCon(tSdoComFinished* pSdoComFinished_p)
{
tOplkError ret = kErrorOk;
tCfmNodeInfo* pNodeInfo = (tCfmNodeInfo*)pSdoComFinished_p->pUserArg;
tNmtNodeCommand nmtNodeCommand;
if (pNodeInfo == NULL)
return kErrorInvalidNodeId;
pNodeInfo->eventCnProgress.sdoAbortCode = pSdoComFinished_p->abortCode;
pNodeInfo->eventCnProgress.bytesDownloaded += pSdoComFinished_p->transferredBytes;
if ((ret = callCbProgress(pNodeInfo)) != kErrorOk)
return ret;
switch (pNodeInfo->cfmState)
{
case kCfmStateIdle:
ret = finishConfig(pNodeInfo, kNmtNodeCommandConfErr);
break;
case kCfmStateUpToDate:
if (pSdoComFinished_p->sdoComConState == kSdoComTransferFinished)
nmtNodeCommand = kNmtNodeCommandConfReset; // continue boot-up of CN with NMT command Reset Configuration
else
nmtNodeCommand = kNmtNodeCommandConfErr; // indicate configuration error CN
ret = finishConfig(pNodeInfo, nmtNodeCommand);
break;
case kCfmStateDownload:
if (pSdoComFinished_p->sdoComConState == kSdoComTransferFinished)
ret = downloadObject(pNodeInfo);
else
ret = finishConfig(pNodeInfo, kNmtNodeCommandConfErr); // configuration was not successful
break;
case kCfmStateDownloadNetConf:
#if defined(CONFIG_INCLUDE_NMT_RMN)
if (pSdoComFinished_p->sdoComConState == kSdoComTransferFinished)
ret = downloadNetConf(pNodeInfo);
else
ret = finishConfig(pNodeInfo, kNmtNodeCommandConfErr);
#else
// Normally we don't get into this case if redundancy MN is
// not configured. If we get here, we issue kNmtNodeCommandConfErr.
ret = finishConfig(pNodeInfo, kNmtNodeCommandConfErr);
#endif
break;
case kCfmStateWaitRestore:
if (pSdoComFinished_p->sdoComConState == kSdoComTransferFinished)
{ // configuration successfully restored
DEBUG_LVL_CFM_TRACE("\nCN%x - Restore Complete. Resetting Node...\n", pNodeInfo->eventCnProgress.nodeId);
// send NMT command reset node to activate the original configuration
ret = finishConfig(pNodeInfo, kNmtNodeCommandConfRestored);
}
else
{ // restore configuration not available
// start downloading the ConciseDCF
pNodeInfo->cfmState = kCfmStateDownload;
ret = downloadObject(pNodeInfo);
}
break;
case kCfmStateWaitStore:
if ((ret = downloadCycleLength(pNodeInfo)) == kErrorReject)
{
pNodeInfo->cfmState = kCfmStateUpToDate;
ret = kErrorOk;
}
else
{
ret = finishConfig(pNodeInfo, kNmtNodeCommandConfReset);
}
break;
case kCfmStateInternalAbort:
// configuration was aborted
break;
}
return ret;
}
//------------------------------------------------------------------------------
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;
}
//------------------------------------------------------------------------------
static tOplkError sdoWriteObject(tCfmNodeInfo* pNodeInfo_p, void* pLeSrcData_p, UINT size_p)
{
tOplkError ret = kErrorOk;
tSdoComTransParamByIndex transParamByIndex;
if ((pLeSrcData_p == NULL) || (size_p == 0))
return kErrorApiInvalidParam;
if (pNodeInfo_p->sdoComConHdl == UINT_MAX)
{
// init command layer connection
ret = sdocom_defineConnection(&pNodeInfo_p->sdoComConHdl,
pNodeInfo_p->eventCnProgress.nodeId,
kSdoTypeAsnd);
if ((ret != kErrorOk) && (ret != kErrorSdoComHandleExists))
return ret;
}
transParamByIndex.pData = pLeSrcData_p;
transParamByIndex.sdoAccessType = kSdoAccessTypeWrite;
transParamByIndex.sdoComConHdl = pNodeInfo_p->sdoComConHdl;
transParamByIndex.dataSize = size_p;
transParamByIndex.index = pNodeInfo_p->eventCnProgress.objectIndex;
transParamByIndex.subindex = pNodeInfo_p->eventCnProgress.objectSubIndex;
transParamByIndex.pfnSdoFinishedCb = cbSdoCon;
transParamByIndex.pUserArg = pNodeInfo_p;
ret = sdocom_initTransferByIndex(&transParamByIndex);
if (ret == kErrorSdoComHandleBusy)
{
ret = sdocom_abortTransfer(pNodeInfo_p->sdoComConHdl, SDO_AC_DATA_NOT_TRANSF_DUE_LOCAL_CONTROL);
if (ret == kErrorOk)
{
ret = sdocom_initTransferByIndex(&transParamByIndex);
}
}
else if (ret == kErrorSdoSeqConnectionBusy)
{
// close connection
ret = sdocom_undefineConnection(pNodeInfo_p->sdoComConHdl);
pNodeInfo_p->sdoComConHdl = UINT_MAX;
if (ret != kErrorOk)
{
DEBUG_LVL_CFM_TRACE("SDO Free Error!\n");
return ret;
}
// reinit command layer connection
ret = sdocom_defineConnection(&pNodeInfo_p->sdoComConHdl,
pNodeInfo_p->eventCnProgress.nodeId,
kSdoTypeAsnd);
if ((ret != kErrorOk) && (ret != kErrorSdoComHandleExists))
return ret;
// retry transfer
transParamByIndex.sdoComConHdl = pNodeInfo_p->sdoComConHdl;
ret = sdocom_initTransferByIndex(&transParamByIndex);
}
return ret;
}
