AtguiguCorp/linear-Slide
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
8955887f9860b7167b629f32a6d19e828691bc35
linear-Slide/Middleware/CANopenNode/304/CO_SRDO.c

1011 lines
42 KiB
C
Raw Normal View History

第一版代码，为了在EEPROM保存参数的时候走STM32的CRC，让Codex修改了一下，现在的效果是无法存储，codex表示原因是CRC方法不同，修改到一半今天的额度使用完了，有待后续解决CRC的bug
2026-02-28 17:36:05 +08:00
/*
* CANopen Safety Related Data Object protocol.
*
* @file CO_SRDO.c
* @ingroup CO_SRDO
* @author Robert Grüning
* @copyright 2020 Robert Grüning
* @copyright 2024 temi54c1l8(at)github
* @copyright 2024 Janez Paternoster
*
* This file is part of <https://github.com/CANopenNode/CANopenNode>, a CANopen Stack.
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not use this
* file except in compliance with the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software distributed under the License is
* distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and limitations under the License.
*/
#include "304/CO_SRDO.h"
#if ((CO_CONFIG_SRDO)&CO_CONFIG_SRDO_ENABLE) != 0
#include "301/crc16-ccitt.h"
/* verify configuration */
#if ((CO_CONFIG_CRC16)&CO_CONFIG_CRC16_ENABLE) == 0
#error CO_CONFIG_CRC16_ENABLE must be enabled.
#endif
#ifdef CO_CONFORMANCE_TEST_TOOL_ADAPTATION
#warning CO_CONFORMANCE_TEST_TOOL_ADAPTATION may be used only for conformance testing (because of CTT limitations)
#endif
/* values for informationDirection and configurationValid */
#define CO_SRDO_INVALID (0U)
#define CO_SRDO_TX (1U)
#define CO_SRDO_RX (2U)
#define CO_SRDO_VALID_MAGIC (0xA5U)
/* macro for information about SRDO configuration error */
#define ERR_INFO(index, subindex, info) (((uint32_t)(index) << 16) | ((uint32_t)(subindex) << 8) | ((uint32_t)(info)))
static void
CO_SRDO_receive_normal(void* object, void* msg) {
CO_SRDO_t* SRDO = (CO_SRDO_t*)object;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
const uint8_t* data = CO_CANrxMsg_readData(msg);
if ((SRDO->informationDirection == CO_SRDO_RX) && (DLC >= SRDO->dataLength) && !CO_FLAG_READ(SRDO->CANrxNew[1])) {
/* copy data into appropriate buffer and set 'new message' flag */
(void)memcpy(SRDO->CANrxData[0], data, sizeof(SRDO->CANrxData[0]));
CO_FLAG_SET(SRDO->CANrxNew[0]);
#if ((CO_CONFIG_SRDO)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
/* Optional signal to RTOS, which can resume task, which handles SRDO. */
if (SRDO->pFunctSignalPre != NULL) {
SRDO->pFunctSignalPre(SRDO->functSignalObjectPre);
}
#endif
} else if (DLC < SRDO->dataLength) {
SRDO->rxSrdoShort = true;
} else { /* MISRA C 2004 14.10 */
}
}
static void
CO_SRDO_receive_inverted(void* object, void* msg) {
CO_SRDO_t* SRDO = (CO_SRDO_t*)object;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
const uint8_t* data = CO_CANrxMsg_readData(msg);
if ((SRDO->informationDirection == CO_SRDO_RX) && (DLC >= SRDO->dataLength) && CO_FLAG_READ(SRDO->CANrxNew[0])) {
/* copy data into appropriate buffer and set 'new message' flag */
(void)memcpy(SRDO->CANrxData[1], data, sizeof(SRDO->CANrxData[1]));
CO_FLAG_SET(SRDO->CANrxNew[1]);
#if ((CO_CONFIG_SRDO)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
/* Optional signal to RTOS, which can resume task, which handles SRDO. */
if (SRDO->pFunctSignalPre != NULL) {
SRDO->pFunctSignalPre(SRDO->functSignalObjectPre);
}
#endif
} else if (DLC < SRDO->dataLength) {
SRDO->rxSrdoShort = true;
} else { /* MISRA C 2004 14.10 */
}
}
/* Set OD object 13FE:00 to CO_SRDO_INVALID and clear configurationValid flag. */
static void
configurationValidUnset(CO_SRDOGuard_t* SRDOGuard) {
if (SRDOGuard != NULL) {
OD_IO_t* OD_IO = &SRDOGuard->OD_IO_configurationValid;
uint8_t val = CO_SRDO_INVALID;
OD_size_t dummy;
SRDOGuard->configurationValid = false;
OD_IO->write(&OD_IO->stream, &val, sizeof(val), &dummy);
}
}
/*
* Custom functions for reading or writing OD object.
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_write_dummy(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
(void)stream;
(void)buf;
if (countWritten != NULL) {
*countWritten = count;
}
return ODR_OK;
}
static ODR_t
OD_read_dummy(OD_stream_t* stream, void* buf, OD_size_t count, OD_size_t* countRead) {
if ((buf == NULL) || (stream == NULL) || (countRead == NULL)) {
return ODR_DEV_INCOMPAT;
}
if (count > stream->dataLength) {
count = stream->dataLength;
}
(void)memset(buf, 0, count);
*countRead = count;
return ODR_OK;
}
#ifdef CO_CONFORMANCE_TEST_TOOL_ADAPTATION
static bool_t
OD_not_write_same_value(OD_stream_t* stream, const void* buf, OD_size_t count) {
/* The conformance test tool does not recognize CANopen Safety and on all object
* dictionaty tries to read and write the same value */
OD_size_t countRead = 0;
uint8_t bufRead[6] = {0};
if (count > 6U) {
return false;
}
ODR_t returnCode = OD_readOriginal(stream, bufRead, count, &countRead);
if (returnCode != ODR_OK) {
return false;
}
if (memcmp((const void*)(buf), (const void*)(bufRead), count) == 0) {
return true;
}
return false;
}
#endif
static ODR_t
OD_read_SRDO_communicationParam(OD_stream_t* stream, void* buf, OD_size_t count, OD_size_t* countRead) {
ODR_t returnCode = OD_readOriginal(stream, buf, count, countRead);
/* When reading COB_ID, add Node-Id to the read value, if necessary */
if ((returnCode == ODR_OK) && ((stream->subIndex == 5U) || (stream->subIndex == 6U)) && (*countRead == 4U)) {
CO_SRDO_t* SRDO = stream->object;
uint32_t value = CO_getUint32(buf);
uint16_t defaultCOB_ID = SRDO->defaultCOB_ID + ((uint16_t)(stream->subIndex) - 5U);
/* If default COB ID is used, then OD entry does not contain $NodeId. Add it here. */
if ((value == defaultCOB_ID) && (SRDO->nodeId <= 64U)) {
value += (uint32_t)SRDO->nodeId * 2U;
}
(void)CO_setUint32(buf, value);
}
return returnCode;
}
static ODR_t
OD_write_SRDO_communicationParam(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
if ((stream == NULL) || (buf == NULL) || (countWritten == NULL) || (count > 4U)) {
return ODR_DEV_INCOMPAT;
}
#ifdef CO_CONFORMANCE_TEST_TOOL_ADAPTATION
if (OD_not_write_same_value(stream, buf, count)) {
return ODR_OK;
}
#endif
CO_SRDO_t* SRDO = stream->object;
CO_SRDOGuard_t* SRDOGuard = SRDO->SRDOGuard;
uint8_t bufCopy[4];
(void)memcpy((void*)(bufCopy), (const void*)(buf), count);
/* Writing Object Dictionary variable */
if (SRDOGuard->NMTisOperational) {
/* Data cannot be transferred or stored to the application because of the present device state. */
return ODR_DATA_DEV_STATE;
}
if (stream->subIndex == 1U) { /* Information direction */
uint8_t value = CO_getUint8(buf);
if (value > 2U) {
return ODR_INVALID_VALUE;
}
SRDO->informationDirection = value;
} else if (stream->subIndex == 2U) { /* SCT */
uint16_t value = CO_getUint16(buf);
if (value < ((CO_CONFIG_SRDO_MINIMUM_DELAY / 1000U) + 1U)) {
return ODR_INVALID_VALUE;
}
} else if (stream->subIndex == 3U) { /* SRVT */
uint8_t value = CO_getUint8(buf);
if (value == 0U) {
return ODR_INVALID_VALUE;
}
} else if (stream->subIndex == 4U) { /* Transmission_type */
uint8_t value = CO_getUint8(buf);
if (value != 254U) {
return ODR_INVALID_VALUE;
}
} else if ((stream->subIndex == 5U) || (stream->subIndex == 6U)) { /* COB_ID */
uint32_t value = CO_getUint32(buf);
uint16_t index = (uint16_t)(stream->subIndex) - 5U;
uint16_t defaultCOB_ID = SRDO->defaultCOB_ID + index;
/* check value range, the spec does not specify if COB-ID flags are allowed */
if ((value < 0x101U) || (value > 0x180U) || ((value & 1U) == index)) {
return ODR_INVALID_VALUE; /* Invalid value for parameter (download only). */
}
/* if default COB-ID is being written, write defaultCOB_ID without nodeId */
if ((SRDO->nodeId <= 64U) && (value == (defaultCOB_ID + ((uint32_t)SRDO->nodeId * 2U)))) {
value = defaultCOB_ID;
(void)CO_setUint32(bufCopy, value);
}
} else { /* MISRA C 2004 14.10 */
}
/* set OD object 13FE:00 to CO_SRDO_INVALID */
configurationValidUnset(SRDOGuard);
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, bufCopy, count, countWritten);
}
static ODR_t
OD_write_SRDO_mappingParam(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
if ((stream == NULL) || (buf == NULL) || (countWritten == NULL)
|| (stream->subIndex > CO_SRDO_MAX_MAPPED_ENTRIES)) {
return ODR_DEV_INCOMPAT;
}
#ifdef CO_CONFORMANCE_TEST_TOOL_ADAPTATION
if (OD_not_write_same_value(stream, buf, count)) {
return ODR_OK;
}
#endif
CO_SRDO_t* SRDO = stream->object;
CO_SRDOGuard_t* SRDOGuard = SRDO->SRDOGuard;
/* Writing Object Dictionary variable */
if (SRDOGuard->NMTisOperational) {
/* Data cannot be transferred or stored to the application because of the present device state. */
return ODR_DATA_DEV_STATE;
}
/* SRDO must be disabled */
if (SRDO->informationDirection != 0U) {
return ODR_UNSUPP_ACCESS; /* Unsupported access to an object. */
}
/* numberOfMappedObjects */
if (stream->subIndex == 0U) {
uint8_t value = CO_getUint8(buf);
/* only odd numbers are allowed */
if ((value > CO_SRDO_MAX_MAPPED_ENTRIES) || ((value & 1U) != 0U)) {
return ODR_MAP_LEN; /* Number and length of object to be mapped exceeds SRDO length. */
}
SRDO->mappedObjectsCount = value;
}
/* mapping objects */
else {
if (SRDO->mappedObjectsCount != 0U) {
return ODR_UNSUPP_ACCESS;
}
/* No other checking is implemented here. Values are validated in the configuration function. */
}
/* set OD object 13FE:00 to CO_SRDO_INVALID */
configurationValidUnset(SRDOGuard);
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, buf, count, countWritten);
}
static ODR_t
OD_write_13FE(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL) || (countWritten == NULL)) {
return ODR_DEV_INCOMPAT;
}
CO_SRDOGuard_t* SRDOGuard = stream->object;
if (SRDOGuard->NMTisOperational) {
/* Data cannot be transferred or stored to the application because of the present device state. */
return ODR_DATA_DEV_STATE;
}
#ifdef CO_CONFORMANCE_TEST_TOOL_ADAPTATION
if (OD_not_write_same_value(stream, buf, count)) {
return ODR_OK;
}
#endif
uint8_t configurationValid = CO_getUint8(buf);
if (configurationValid == CO_SRDO_VALID_MAGIC) {
SRDOGuard->configurationValid = true;
} else {
SRDOGuard->configurationValid = false;
}
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, buf, count, countWritten);
}
static ODR_t
OD_write_13FF(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
if ((stream == NULL) || (stream->subIndex == 0U) || (buf == NULL) || (countWritten == NULL)) {
return ODR_DEV_INCOMPAT;
}
CO_SRDOGuard_t* SRDOGuard = stream->object;
if (SRDOGuard->NMTisOperational) {
/* Data cannot be transferred or stored to the application because of the present device state. */
return ODR_DATA_DEV_STATE;
}
#ifdef CO_CONFORMANCE_TEST_TOOL_ADAPTATION
if (OD_not_write_same_value(stream, buf, count)) {
return ODR_OK;
}
#endif
/* set OD object 13FE:00 to CO_SRDO_INVALID */
configurationValidUnset(SRDOGuard);
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, buf, count, countWritten);
}
#if ((CO_CONFIG_SRDO)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
void
CO_SRDO_initCallbackPre(CO_SRDO_t* SRDO, void* object, void (*pFunctSignalPre)(void* object)) {
if (SRDO != NULL) {
SRDO->functSignalObjectPre = object;
SRDO->pFunctSignalPre = pFunctSignalPre;
}
}
#endif
CO_ReturnError_t
CO_SRDOGuard_init(CO_SRDOGuard_t* SRDOGuard, OD_entry_t* OD_13FE_configurationValid,
OD_entry_t* OD_13FF_safetyConfigurationSignature, uint32_t* errInfo) {
ODR_t odRet;
uint8_t configurationValid;
/* verify arguments */
if ((SRDOGuard == NULL) || (OD_13FE_configurationValid == NULL) || (OD_13FF_safetyConfigurationSignature == NULL)) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear object */
(void)memset(SRDOGuard, 0, sizeof(CO_SRDOGuard_t));
SRDOGuard->OD_13FE_entry = OD_13FE_configurationValid;
SRDOGuard->OD_13FF_entry = OD_13FF_safetyConfigurationSignature;
/* Configure Object dictionary extensions */
SRDOGuard->OD_13FE_extension.object = SRDOGuard;
SRDOGuard->OD_13FE_extension.read = OD_readOriginal;
SRDOGuard->OD_13FE_extension.write = OD_write_13FE;
(void)OD_extension_init(OD_13FE_configurationValid, &SRDOGuard->OD_13FE_extension);
SRDOGuard->OD_13FF_extension.object = SRDOGuard;
SRDOGuard->OD_13FF_extension.read = OD_readOriginal;
SRDOGuard->OD_13FF_extension.write = OD_write_13FF;
(void)OD_extension_init(OD_13FF_safetyConfigurationSignature, &SRDOGuard->OD_13FF_extension);
/* Configure SRDOGuard->OD_IO_configurationValid variable. It will be used for writing 0 to OD variable 13FE,00 */
odRet = OD_getSub(OD_13FE_configurationValid, 0, &SRDOGuard->OD_IO_configurationValid, false);
if ((odRet != ODR_OK) || (SRDOGuard->OD_IO_configurationValid.stream.dataLength != 1U)) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_13FE_configurationValid)) << 8) | 1U;
}
return CO_ERROR_OD_PARAMETERS;
}
if (OD_get_u8(OD_13FE_configurationValid, 0, &configurationValid, true) != ODR_OK) {
*errInfo = (((uint32_t)OD_getIndex(OD_13FE_configurationValid)) << 8) | 1U;
return CO_ERROR_OD_PARAMETERS;
}
if (configurationValid == CO_SRDO_VALID_MAGIC) {
SRDOGuard->configurationValid = true;
} else {
SRDOGuard->configurationValid = false;
}
return CO_ERROR_NO;
}
CO_ReturnError_t
CO_SRDO_config(CO_SRDO_t* SRDO, uint8_t SRDO_Index, CO_SRDOGuard_t* SRDOGuard, uint32_t* errInfo) {
CO_ReturnError_t ret = CO_ERROR_NO;
uint32_t err = 0;
bool_t configurationInProgress = false;
/* variables will be retrieved from Object Dictionary */
uint8_t cp_highestSubindexSupported = 0;
uint8_t informationDirection = 0;
uint16_t safetyCycleTime = 0;
uint8_t safetyRelatedValidationTime = 0;
uint8_t transmissionType = 0;
uint32_t COB_ID1_normal = 0;
uint32_t COB_ID2_inverted = 0;
uint8_t configurationValid = 0;
uint16_t crcSignatureFromOD = 0;
uint8_t mappedObjectsCount = 0;
uint32_t mapping[CO_SRDO_MAX_MAPPED_ENTRIES];
/* Get variables from object Dictionary and verify it's structure. */
if (err == 0U) {
if (OD_get_u8(SRDOGuard->OD_13FE_entry, 0, &configurationValid, true) != ODR_OK) {
err = ERR_INFO(0x13FEUL, 0, 1);
} else if (OD_get_u16(SRDOGuard->OD_13FF_entry, SRDO_Index + 1U, &crcSignatureFromOD, true) != ODR_OK) {
err = ERR_INFO(0x13FFUL, SRDO_Index + 1UL, 1);
} else if (OD_get_u8(SRDO->OD_communicationParam_entry, 0, &cp_highestSubindexSupported, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 0, 1);
} else if (OD_get_u8(SRDO->OD_communicationParam_entry, 1, &informationDirection, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 1, 1);
} else if (OD_get_u16(SRDO->OD_communicationParam_entry, 2, &safetyCycleTime, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 2, 1);
} else if (OD_get_u8(SRDO->OD_communicationParam_entry, 3, &safetyRelatedValidationTime, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 3, 1);
} else if (OD_get_u8(SRDO->OD_communicationParam_entry, 4, &transmissionType, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 4, 1);
} else if (OD_get_u32(SRDO->OD_communicationParam_entry, 5, &COB_ID1_normal, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 5, 1);
} else if (OD_get_u32(SRDO->OD_communicationParam_entry, 6, &COB_ID2_inverted, true) != ODR_OK) {
err = ERR_INFO(0x1301UL + SRDO_Index, 6, 1);
} else if (OD_get_u8(SRDO->OD_mappingParam_entry, 0, &mappedObjectsCount, true) != ODR_OK) {
err = ERR_INFO(0x1381UL + SRDO_Index, 0, 1);
} else {
for (uint8_t i = 0; i < mappedObjectsCount; i++) {
if (OD_get_u32(SRDO->OD_mappingParam_entry, i + 1U, &mapping[i], true) != ODR_OK) {
err = ERR_INFO(0x1381UL + SRDO_Index, i + 1UL, 1);
break;
}
}
}
/* if OD contains default COB_IDs, add node-id */
if ((COB_ID1_normal == SRDO->defaultCOB_ID) && (COB_ID2_inverted == ((uint32_t)SRDO->defaultCOB_ID + 1UL))
&& (SRDO->nodeId <= 64U)) {
uint32_t add = (uint32_t)SRDO->nodeId * 2U;
COB_ID1_normal += add;
COB_ID2_inverted += add;
}
/* If this fails, something is wrong with the Object Dictionary. Device have to be reprogrammed. */
if (err != 0U) {
ret = CO_ERROR_OD_PARAMETERS;
}
}
/* If configurationValid is set and SRDO is valid, continue with further configuration */
if ((err == 0U) && (configurationValid == CO_SRDO_VALID_MAGIC) && (informationDirection != CO_SRDO_INVALID)) {
configurationInProgress = true;
}
/* Verify parameters from OD */
if ((err == 0U) && configurationInProgress) {
if (cp_highestSubindexSupported != 6U) {
err = ERR_INFO(0x1301UL + SRDO_Index, 0, 2);
} else if (informationDirection > 3U) {
err = ERR_INFO(0x1301UL + SRDO_Index, 1, 2);
} else if (safetyCycleTime < ((CO_CONFIG_SRDO_MINIMUM_DELAY / 1000U) + 1U)) {
err = ERR_INFO(0x1301UL + SRDO_Index, 2, 2);
} else if (safetyRelatedValidationTime < 1U) {
err = ERR_INFO(0x1301UL + SRDO_Index, 3, 2);
} else if (transmissionType != 254U) {
err = ERR_INFO(0x1301UL + SRDO_Index, 4, 2);
} else if ((COB_ID1_normal < 0x101U) || ((COB_ID1_normal & 1U) == 0U)) {
err = ERR_INFO(0x1301UL + SRDO_Index, 5, 2);
} else if (((COB_ID1_normal + 1U) != COB_ID2_inverted) || (COB_ID2_inverted > 0x180U)) {
err = ERR_INFO(0x1301UL + SRDO_Index, 6, 2);
} else if ((mappedObjectsCount > CO_SRDO_MAX_MAPPED_ENTRIES) || ((mappedObjectsCount & 1U) != 0U)) {
err = ERR_INFO(0x1381UL + SRDO_Index, 0, 2);
} else {
/* MISRA C 2004 14.10 */
}
}
/* Verify CRC */
if ((err == 0U) && configurationInProgress) {
uint16_t crcResult = 0x0000;
uint16_t tmp_u16;
uint32_t tmp_u32;
crcResult = crc16_ccitt(&informationDirection, 1, crcResult);
tmp_u16 = CO_SWAP_16(safetyCycleTime);
crcResult = crc16_ccitt((uint8_t*)&tmp_u16, 2, crcResult);
crcResult = crc16_ccitt(&safetyRelatedValidationTime, 1, crcResult);
tmp_u32 = CO_SWAP_32(COB_ID1_normal);
crcResult = crc16_ccitt((uint8_t*)&tmp_u32, 4, crcResult);
tmp_u32 = CO_SWAP_32(COB_ID2_inverted);
crcResult = crc16_ccitt((uint8_t*)&tmp_u32, 4, crcResult);
crcResult = crc16_ccitt(&mappedObjectsCount, 1, crcResult);
for (uint8_t i = 0; i < mappedObjectsCount; i++) {
uint8_t crcsubindex = i + 1U;
crcResult = crc16_ccitt(&crcsubindex, 1, crcResult);
tmp_u32 = CO_SWAP_32(mapping[i]);
crcResult = crc16_ccitt((uint8_t*)&tmp_u32, 4, crcResult);
}
if (crcResult != crcSignatureFromOD) {
err = ERR_INFO(0x13FFUL, SRDO_Index + 1UL, 3);
}
}
/* Configure mappings */
if ((err == 0U) && configurationInProgress) {
CO_SRDO_size_t srdoDataLength[2] = {0, 0};
for (uint8_t i = 0; i < mappedObjectsCount; i++) {
uint8_t plain_inverted = i % 2U;
uint32_t map = mapping[i];
uint16_t index = (uint16_t)(map >> 16);
uint8_t subIndex = (uint8_t)(map >> 8);
uint8_t mappedLengthBits = (uint8_t)map;
uint8_t mappedLength = mappedLengthBits >> 3;
OD_IO_t* OD_IO = &SRDO->OD_IO[i];
/* total SRDO length can not be more than CO_SRDO_MAX_SIZE bytes */
if (mappedLength > CO_SRDO_MAX_SIZE) {
err = ERR_INFO(0x1381UL + SRDO_Index, i + 1UL, 4);
}
/* is there a reference to the dummy entry */
else if ((index < 0x20U) && (subIndex == 0U)) {
OD_stream_t* stream = &OD_IO->stream;
(void)memset(stream, 0, sizeof(OD_stream_t));
stream->dataLength = mappedLength;
stream->dataOffset = mappedLength;
OD_IO->read = OD_read_dummy;
OD_IO->write = OD_write_dummy;
}
/* find entry in the Object Dictionary */
else {
OD_IO_t OD_IOcopy;
OD_entry_t* entry = OD_find(SRDO->OD, index);
ODR_t odRet = OD_getSub(entry, subIndex, &OD_IOcopy, false);
if (odRet != ODR_OK) {
err = ERR_INFO(0x1381UL + SRDO_Index, i + 1UL, 5);
} else {
/* verify access attributes, byte alignment and length */
OD_attr_t testAttribute = (informationDirection == CO_SRDO_RX) ? (OD_attr_t)(ODA_RSRDO)
: (OD_attr_t)(ODA_TSRDO);
if (((OD_IOcopy.stream.attribute & testAttribute) == 0U) || ((mappedLengthBits & 0x07U) != 0U)
|| (OD_IOcopy.stream.dataLength < mappedLength)) {
err = ERR_INFO(0x1381UL + SRDO_Index, i + 1UL, 6);
}
/* Copy values and store mappedLength temporary. */
*OD_IO = OD_IOcopy;
OD_IO->stream.dataOffset = mappedLength;
srdoDataLength[plain_inverted] += mappedLength;
}
}
if (err != 0U) {
break;
}
} /* for (uint8_t i = 0; i < mappedObjectsCount; i++) */
if (err == 0U) {
if (srdoDataLength[0] != srdoDataLength[1]) {
err = ERR_INFO(0x1381UL + SRDO_Index, 0, 7);
} else if ((srdoDataLength[0] == 0U) || (srdoDataLength[0] > CO_SRDO_MAX_SIZE)) {
err = ERR_INFO(0x1381UL + SRDO_Index, 0, 8);
} else {
SRDO->dataLength = srdoDataLength[0];
SRDO->mappedObjectsCount = mappedObjectsCount;
}
}
}
/* Configure CAN tx buffers */
if ((err == 0U) && configurationInProgress && (informationDirection == CO_SRDO_TX)) {
/* Normal Configuration */
SRDO->CANtxBuff[0] = CO_CANtxBufferInit(SRDO->CANdevTx[0], SRDO->CANdevTxIdx[0], (uint16_t)COB_ID1_normal,
false, SRDO->dataLength, false);
if (SRDO->CANtxBuff[0] == NULL) {
err = ERR_INFO(0x1301UL + SRDO_Index, 5, 10);
}
/* Inverted Configuration */
SRDO->CANtxBuff[1] = CO_CANtxBufferInit(SRDO->CANdevTx[1], SRDO->CANdevTxIdx[1], (uint16_t)COB_ID2_inverted,
false, SRDO->dataLength, false);
if (SRDO->CANtxBuff[1] == NULL) {
err = ERR_INFO(0x1301UL + SRDO_Index, 6, 10);
}
}
/* Configure CAN rx buffers */
if ((err == 0U) && configurationInProgress && (informationDirection == CO_SRDO_RX)) {
/* Normal Configuration */
ret = CO_CANrxBufferInit(SRDO->CANdevRx[0], SRDO->CANdevRxIdx[0], (uint16_t)COB_ID1_normal, 0x7FF, false,
(void*)SRDO, CO_SRDO_receive_normal);
if (ret != CO_ERROR_NO) {
err = ERR_INFO(0x1301UL + SRDO_Index, 5, 11);
}
/* Inverted Configuration */
ret = CO_CANrxBufferInit(SRDO->CANdevRx[1], SRDO->CANdevRxIdx[1], (uint16_t)COB_ID2_inverted, 0x7FF, false,
(void*)SRDO, CO_SRDO_receive_inverted);
if (ret != CO_ERROR_NO) {
err = ERR_INFO(0x1301UL + SRDO_Index, 6, 11);
}
}
/* Configure remaining variables */
if (err == 0U) {
SRDO->informationDirection = informationDirection;
SRDO->cycleTime_us = (uint32_t)safetyCycleTime * 1000U;
SRDO->validationTime_us = (uint32_t)safetyRelatedValidationTime * 1000U;
} else {
if (ret == CO_ERROR_NO) {
CO_errorReport(SRDO->em, CO_EM_SRDO_CONFIGURATION, CO_EMC_DATA_SET, err);
configurationValidUnset(SRDO->SRDOGuard);
}
}
if (errInfo != NULL) {
*errInfo = err;
}
return ret;
}
CO_ReturnError_t
CO_SRDO_init(CO_SRDO_t* SRDO, uint8_t SRDO_Index, CO_SRDOGuard_t* SRDOGuard, OD_t* OD, CO_EM_t* em, uint8_t nodeId,
uint16_t defaultCOB_ID, OD_entry_t* OD_130x_SRDOCommPar, OD_entry_t* OD_138x_SRDOMapPar,
CO_CANmodule_t* CANdevRxNormal, CO_CANmodule_t* CANdevRxInverted, uint16_t CANdevRxIdxNormal,
uint16_t CANdevRxIdxInverted, CO_CANmodule_t* CANdevTxNormal, CO_CANmodule_t* CANdevTxInverted,
uint16_t CANdevTxIdxNormal, uint16_t CANdevTxIdxInverted, uint32_t* errInfo) {
CO_ReturnError_t ret = CO_ERROR_NO;
/* verify arguments */
if ((SRDO == NULL) || (SRDOGuard == NULL) || (OD == NULL) || (em == NULL) || (OD_130x_SRDOCommPar == NULL)
|| (OD_138x_SRDOMapPar == NULL) || (CANdevRxNormal == NULL) || (CANdevRxInverted == NULL)
|| (CANdevTxNormal == NULL) || (CANdevTxInverted == NULL)) {
ret = CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear object and configure some object variables */
else {
(void)memset(SRDO, 0, sizeof(CO_SRDO_t));
SRDO->SRDOGuard = SRDOGuard;
SRDO->OD = OD;
SRDO->em = em;
SRDO->defaultCOB_ID = defaultCOB_ID;
SRDO->nodeId = nodeId;
SRDO->CANdevTx[0] = CANdevTxNormal;
SRDO->CANdevTx[1] = CANdevTxInverted;
SRDO->CANdevRx[0] = CANdevRxNormal;
SRDO->CANdevRx[1] = CANdevRxInverted;
SRDO->CANdevTxIdx[0] = CANdevTxIdxNormal;
SRDO->CANdevTxIdx[1] = CANdevTxIdxInverted;
SRDO->CANdevRxIdx[0] = CANdevRxIdxNormal;
SRDO->CANdevRxIdx[1] = CANdevRxIdxInverted;
SRDO->OD_communicationParam_entry = OD_130x_SRDOCommPar;
SRDO->OD_mappingParam_entry = OD_138x_SRDOMapPar;
/* Configure Object dictionary entry at index 0x1301+ */
SRDO->OD_communicationParam_ext.object = SRDO;
SRDO->OD_communicationParam_ext.read = OD_read_SRDO_communicationParam;
SRDO->OD_communicationParam_ext.write = OD_write_SRDO_communicationParam;
(void)OD_extension_init(OD_130x_SRDOCommPar, &SRDO->OD_communicationParam_ext);
/* Configure Object dictionary entry at index 0x1381+ */
SRDO->OD_mappingParam_extension.object = SRDO;
SRDO->OD_mappingParam_extension.read = OD_readOriginal;
SRDO->OD_mappingParam_extension.write = OD_write_SRDO_mappingParam;
(void)OD_extension_init(OD_138x_SRDOMapPar, &SRDO->OD_mappingParam_extension);
ret = CO_SRDO_config(SRDO, SRDO_Index, SRDOGuard, errInfo);
}
return ret;
}
CO_ReturnError_t
CO_SRDO_requestSend(CO_SRDO_t* SRDO) {
CO_ReturnError_t ret;
if (SRDO->SRDOGuard->NMTisOperational == false) {
ret = CO_ERROR_WRONG_NMT_STATE;
} else if (SRDO->SRDOGuard->configurationValid == false) {
ret = CO_ERROR_OD_PARAMETERS;
} else if (SRDO->informationDirection != CO_SRDO_TX) {
ret = CO_ERROR_TX_UNCONFIGURED;
} else if (SRDO->nextIsNormal == false) {
ret = CO_ERROR_TX_BUSY;
} else {
SRDO->cycleTimer = 0;
ret = CO_ERROR_NO;
}
return ret;
}
CO_SRDO_state_t
CO_SRDO_process(CO_SRDO_t* SRDO, uint32_t timeDifference_us, uint32_t* timerNext_us, bool_t NMTisOperational) {
(void)timerNext_us; /* may be unused */
if (NMTisOperational && (SRDO->informationDirection != CO_SRDO_INVALID) && SRDO->SRDOGuard->configurationValid
&& (SRDO->internalState >= CO_SRDO_state_unknown)) {
SRDO->cycleTimer = (SRDO->cycleTimer > timeDifference_us) ? (SRDO->cycleTimer - timeDifference_us) : 0U;
SRDO->invertedDelay = (SRDO->invertedDelay > timeDifference_us) ? (SRDO->invertedDelay - timeDifference_us)
: 0U;
SRDO->validationTimer = (SRDO->validationTimer > timeDifference_us)
? (SRDO->validationTimer - timeDifference_us)
: 0U;
/* Detect transition to NMT operational */
if (!SRDO->NMTisOperationalPrevious) {
SRDO->cycleTimer = (SRDO->informationDirection == CO_SRDO_TX)
? ((uint32_t)SRDO->nodeId * 500U) /* 0.5ms * node-ID delay */
: SRDO->cycleTime_us;
SRDO->validationTimer = SRDO->cycleTime_us;
SRDO->internalState = CO_SRDO_state_initializing;
SRDO->nextIsNormal = true;
}
if (SRDO->internalState <= CO_SRDO_state_unknown) {
SRDO->internalState = CO_SRDO_state_error_internal; /* should not happen */
} else if (SRDO->informationDirection == CO_SRDO_TX) {
if (SRDO->nextIsNormal) {
if (SRDO->cycleTimer == 0U) {
uint8_t* dataSRDO[2] = {&SRDO->CANtxBuff[0]->data[0], &SRDO->CANtxBuff[1]->data[0]};
size_t verifyLength[2] = {0, 0};
/* copy mapped data from Object Dictionary into CAN buffers */
for (uint8_t i = 0; i < SRDO->mappedObjectsCount; i++) {
uint8_t plain_inverted = i % 2U;
OD_IO_t* OD_IO = &SRDO->OD_IO[i];
OD_stream_t* stream = &OD_IO->stream;
/* get mappedLength from temporary storage */
uint8_t mappedLength = (uint8_t)stream->dataOffset;
/* additional safety check */
verifyLength[plain_inverted] += mappedLength;
if (verifyLength[plain_inverted] > CO_SRDO_MAX_SIZE) {
break;
}
/* length of OD variable may be larger than mappedLength */
OD_size_t ODdataLength = stream->dataLength;
if (ODdataLength > CO_SRDO_MAX_SIZE) {
ODdataLength = CO_SRDO_MAX_SIZE;
}
/* If mappedLength is smaller than ODdataLength, use auxiliary buffer */
uint8_t buf[CO_SRDO_MAX_SIZE];
uint8_t* dataSRDOCopy;
if (ODdataLength > mappedLength) {
(void)memset(buf, 0, sizeof(buf));
dataSRDOCopy = buf;
} else {
dataSRDOCopy = dataSRDO[plain_inverted];
}
/* Set stream.dataOffset to zero, perform OD_IO.read()
* and store mappedLength back to stream.dataOffset */
stream->dataOffset = 0;
OD_size_t countRd;
OD_IO->read(stream, dataSRDOCopy, ODdataLength, &countRd);
stream->dataOffset = mappedLength;
/* swap multibyte data if big-endian */
#ifdef CO_BIG_ENDIAN
if ((stream->attribute & ODA_MB) != 0) {
uint8_t* lo = dataSRDOCopy;
uint8_t* hi = dataSRDOCopy + ODdataLength - 1;
while (lo < hi) {
uint8_t swap = *lo;
*lo++ = *hi;
*hi-- = swap;
}
}
#endif
/* If auxiliary buffer, copy it to the SRDO */
if (ODdataLength > mappedLength) {
(void)memcpy(dataSRDO[plain_inverted], buf, mappedLength);
}
dataSRDO[plain_inverted] += mappedLength;
}
if ((verifyLength[0] != verifyLength[1]) || (verifyLength[0] > CO_SRDO_MAX_SIZE)
|| (verifyLength[0] != SRDO->dataLength)) {
SRDO->internalState = CO_SRDO_state_error_internal; /* should not happen */
} else {
bool_t data_ok = true;
#if ((CO_CONFIG_SRDO)&CO_CONFIG_SRDO_CHECK_TX) != 0
/* check data before sending (optional) */
for (uint8_t i = 0; i < SRDO->dataLength; i++) {
if ((uint8_t)(~SRDO->CANtxBuff[0]->data[i]) != SRDO->CANtxBuff[1]->data[i]) {
SRDO->internalState = CO_SRDO_state_error_txNotInverted;
data_ok = false;
break;
}
}
#endif
if (data_ok) {
if (CO_CANsend(SRDO->CANdevTx[0], SRDO->CANtxBuff[0]) == CO_ERROR_NO) {
SRDO->cycleTimer =
SRDO->cycleTime_us; /* cycleTime_us is verified, result can't be <0 */
SRDO->invertedDelay = CO_CONFIG_SRDO_MINIMUM_DELAY;
SRDO->nextIsNormal = false;
SRDO->internalState = CO_SRDO_state_communicationEstablished;
} else {
SRDO->internalState = CO_SRDO_state_error_txFail;
}
}
}
}
} else {
if (SRDO->invertedDelay == 0U) {
if (CO_CANsend(SRDO->CANdevTx[1], SRDO->CANtxBuff[1]) == CO_ERROR_NO) {
SRDO->nextIsNormal = true;
} else {
SRDO->internalState = CO_SRDO_state_error_txFail;
}
}
}
#if ((CO_CONFIG_SRDO)&CO_CONFIG_FLAG_TIMERNEXT) != 0
if (timerNext_us != NULL) {
if (*timerNext_us > SRDO->cycleTimer) {
*timerNext_us = SRDO->cycleTimer; /* Schedule for the next message timer */
}
}
#endif
} else { /* CO_SRDO_RX */
/* verify error from receive function */
if (SRDO->rxSrdoShort) {
CO_errorReport(SRDO->em, CO_EM_RPDO_WRONG_LENGTH, CO_EMC_PDO_LENGTH, 0);
SRDO->internalState = CO_SRDO_state_error_rxShort;
} else if (CO_FLAG_READ(SRDO->CANrxNew[SRDO->nextIsNormal ? 0 : 1])) {
/* normal message received ? */
if (SRDO->nextIsNormal) {
SRDO->validationTimer = SRDO->validationTime_us;
SRDO->nextIsNormal = false;
}
/* inverted message received */
else {
SRDO->cycleTimer = SRDO->cycleTime_us;
SRDO->validationTimer = SRDO->cycleTime_us;
SRDO->nextIsNormal = true;
uint8_t* dataSRDO[2] = {&SRDO->CANrxData[0][0], &SRDO->CANrxData[1][0]};
bool_t data_ok = true;
/* Verify, if normal and inverted data matches properly */
for (uint8_t i = 0; i < SRDO->dataLength; i++) {
if ((uint8_t)(~dataSRDO[0][i]) != dataSRDO[1][i]) {
data_ok = false;
SRDO->internalState = CO_SRDO_state_error_rxNotInverted;
break;
}
}
/* copy data from CAN messages into mapped data from Object Dictionary */
if (data_ok) {
size_t verifyLength[2] = {0, 0};
for (uint8_t i = 0; i < SRDO->mappedObjectsCount; i++) {
uint8_t plain_inverted = i % 2U;
OD_IO_t* OD_IO = &SRDO->OD_IO[i];
OD_stream_t* stream = &OD_IO->stream;
/* get mappedLength from temporary storage */
OD_size_t* dataOffset = &stream->dataOffset;
uint8_t mappedLength = (uint8_t)(*dataOffset);
/* additional safety check */
verifyLength[plain_inverted] += mappedLength;
if (verifyLength[plain_inverted] > CO_SRDO_MAX_SIZE) {
break;
}
/* length of OD variable may be larger than mappedLength */
OD_size_t ODdataLength = stream->dataLength;
if (ODdataLength > CO_SRDO_MAX_SIZE) {
ODdataLength = CO_SRDO_MAX_SIZE;
}
/* Prepare data for writing into OD variable. If mappedLength
* is smaller than ODdataLength, then use auxiliary buffer */
uint8_t buf[CO_SRDO_MAX_SIZE];
uint8_t* dataOD;
if (ODdataLength > mappedLength) {
(void)memset(buf, 0, sizeof(buf));
(void)memcpy(buf, dataSRDO[plain_inverted], mappedLength);
dataOD = buf;
} else {
dataOD = dataSRDO[plain_inverted];
}
/* swap multibyte data if big-endian */
#ifdef CO_BIG_ENDIAN
if ((stream->attribute & ODA_MB) != 0) {
uint8_t* lo = dataOD;
uint8_t* hi = dataOD + ODdataLength - 1;
while (lo < hi) {
uint8_t swap = *lo;
*lo++ = *hi;
*hi-- = swap;
}
}
#endif
/* Set stream.dataOffset to zero, perform OD_IO.write()
* and store mappedLength back to stream.dataOffset */
*dataOffset = 0;
OD_size_t countWritten;
OD_IO->write(&OD_IO->stream, dataOD, ODdataLength, &countWritten);
*dataOffset = mappedLength;
dataSRDO[plain_inverted] += mappedLength;
} /* for (uint8_t i = 0; i < SRDO->mappedObjectsCount; i++) */
/* safety check, this should not happen */
if ((verifyLength[0] != verifyLength[1]) || (verifyLength[0] > CO_SRDO_MAX_SIZE)
|| (verifyLength[0] != SRDO->dataLength)) {
SRDO->internalState = CO_SRDO_state_error_internal;
} else {
SRDO->internalState = CO_SRDO_state_communicationEstablished;
}
} /* if (data_ok) { */
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
} /* inverted message received */
} else { /* MISRA C 2004 14.10 */
}
/* verify timeouts */
if (SRDO->cycleTimer == 0U) {
SRDO->internalState = CO_SRDO_state_error_rxTimeoutSCT;
} else if (SRDO->validationTimer == 0U) {
SRDO->internalState = CO_SRDO_state_error_rxTimeoutSRVT;
} else {
/* MISRA C 2004 14.10 */
}
#if ((CO_CONFIG_SRDO)&CO_CONFIG_FLAG_TIMERNEXT) != 0
if (timerNext_us != NULL) {
if (*timerNext_us > SRDO->cycleTimer) {
*timerNext_us = SRDO->cycleTimer; /* Schedule for the next message timer */
}
if (*timerNext_us > SRDO->validationTimer) {
*timerNext_us = SRDO->validationTimer; /* Schedule for the next message timer */
}
}
#endif
} /* CO_SRDO_RX */
} /* if (NMTisOperational && ... */
else {
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
if (!SRDO->SRDOGuard->configurationValid) {
SRDO->internalState = CO_SRDO_state_error_configuration;
} else if (!NMTisOperational) {
SRDO->internalState = CO_SRDO_state_nmtNotOperational;
} else if (SRDO->informationDirection == CO_SRDO_INVALID) {
SRDO->internalState = CO_SRDO_state_deleted;
} else {
/* keep internalState unchanged */
/* MISRA C 2004 14.10 */
}
}
SRDO->NMTisOperationalPrevious = NMTisOperational;
SRDO->SRDOGuard->NMTisOperational = NMTisOperational;
return SRDO->internalState;
}
#endif /* (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE */
Reference in New Issue Copy Permalink