From f054501d0ef7aaeaee28f25bc5688e52a840ae98 Mon Sep 17 00:00:00 2001 From: Janez Date: Fri, 31 May 2024 01:47:28 +0200 Subject: [PATCH] SRDO updated to current CANopenNode --- 301/CO_Emergency.h | 4 +- 301/CO_config.h | 6 - 304/CO_GFC.c | 134 ++-- 304/CO_GFC.h | 46 +- 304/CO_SRDO.c | 1533 +++++++++++++++++++++----------------------- 304/CO_SRDO.h | 295 +++++---- CANopen.c | 177 +++-- CANopen.h | 44 +- README.md | 2 +- 9 files changed, 1142 insertions(+), 1099 deletions(-) diff --git a/301/CO_Emergency.h b/301/CO_Emergency.h index cfa4c7e..f4e3b24 100644 --- a/301/CO_Emergency.h +++ b/301/CO_Emergency.h @@ -304,8 +304,8 @@ typedef enum { /** 0x1C, communication, critical, Heartbeat consumer detected remote node * reset */ CO_EM_HB_CONSUMER_REMOTE_RESET = 0x1CU, - /** 0x1D, communication, critical, (unused) */ - CO_EM_1D_unused = 0x1DU, + /** 0x1D, communication, critical, Error in SRDO configuration parameters. */ + CO_EM_SRDO_CONFIGURATION = 0x1DU, /** 0x1E, communication, critical, (unused) */ CO_EM_1E_unused = 0x1EU, /** 0x1F, communication, critical, (unused) */ diff --git a/301/CO_config.h b/301/CO_config.h index c883643..a5e8f89 100644 --- a/301/CO_config.h +++ b/301/CO_config.h @@ -570,10 +570,6 @@ extern "C" { * Possible flags, can be ORed: * - CO_CONFIG_SRDO_ENABLE - Enable the SRDO object. * - CO_CONFIG_SRDO_CHECK_TX - Enable checking data before sending. - * - CO_CONFIG_RSRDO_CALLS_EXTENSION - Enable calling configured extension - * callbacks when received RSRDO CAN message modifies OD entries. - * - CO_CONFIG_TRSRDO_CALLS_EXTENSION - Enable calling configured extension - * callbacks before TSRDO CAN message is sent. * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing * received RSRDO CAN message. * Callback is configured by CO_SRDO_initCallbackPre(). @@ -585,8 +581,6 @@ extern "C" { #endif #define CO_CONFIG_SRDO_ENABLE 0x01 #define CO_CONFIG_SRDO_CHECK_TX 0x02 -#define CO_CONFIG_RSRDO_CALLS_EXTENSION 0x04 -#define CO_CONFIG_TSRDO_CALLS_EXTENSION 0x08 /** * SRDO Tx time delay diff --git a/304/CO_GFC.c b/304/CO_GFC.c index 4b315c5..191268e 100644 --- a/304/CO_GFC.c +++ b/304/CO_GFC.c @@ -4,7 +4,8 @@ * @file CO_GFC.c * @ingroup CO_GFC * @author Robert Grüning - * @copyright 2020 - 2020 Robert Grüning + * @copyright 2020 Robert Grüning + * @copyright 2024 Janez Paternoster * * This file is part of CANopenNode, an opensource CANopen Stack. * Project home page is . @@ -27,32 +28,49 @@ #if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER +/* + * Custom function for reading or writing OD object. + * + * For more information see file CO_ODinterface.h, OD_IO_t. + */ +static ODR_t +OD_write_1300(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; + } -static void CO_GFC_receive(void *object, void *msg) -{ - CO_GFC_t *GFC; + CO_GFC_t* GFC = stream->object; + + uint8_t value = CO_getUint8(buf); + if (value > 1) { + return ODR_INVALID_VALUE; + } + + GFC->valid = (value == 1); + + /* write value to the original location in the Object Dictionary */ + return OD_writeOriginal(stream, buf, count, countWritten); +} + +#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER +static void +CO_GFC_receive(void* object, void* msg) { + CO_GFC_t* GFC; uint8_t DLC = CO_CANrxMsg_readDLC(msg); - GFC = (CO_GFC_t *) - object; /* this is the correct pointer type of the first argument */ + GFC = (CO_GFC_t*)object; /* this is the correct pointer type of the first argument */ - if ((*GFC->valid == 0x01) && (DLC == 0)) { + if (GFC->valid && (DLC == 0)) { -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER - /* Optional signal to RTOS, which can resume task, which handles SRDO. - */ + /* Callback signals Global Failsafe Command */ if (GFC->pFunctSignalSafe != NULL) { GFC->pFunctSignalSafe(GFC->functSignalObjectSafe); } -#endif } } -void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC, - void *object, - void (*pFunctSignalSafe)(void *object)) -{ +void +CO_GFC_initCallbackEnterSafeState(CO_GFC_t* GFC, void* object, void (*pFunctSignalSafe)(void* object)) { if (GFC != NULL) { GFC->functSignalObjectSafe = object; GFC->pFunctSignalSafe = pFunctSignalSafe; @@ -60,70 +78,70 @@ void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC, } #endif -CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC, - uint8_t *valid, - CO_CANmodule_t *GFC_CANdevRx, - uint16_t GFC_rxIdx, - uint16_t CANidRxGFC, - CO_CANmodule_t *GFC_CANdevTx, - uint16_t GFC_txIdx, - uint16_t CANidTxGFC) -{ - if (GFC == NULL || valid == NULL || GFC_CANdevRx == NULL || - GFC_CANdevTx == NULL) { + +CO_ReturnError_t +CO_GFC_init(CO_GFC_t* GFC, OD_entry_t* OD_1300_gfcParameter, CO_CANmodule_t* GFC_CANdevRx, uint16_t GFC_rxIdx, uint16_t CANidRxGFC, + CO_CANmodule_t* GFC_CANdevTx, uint16_t GFC_txIdx, uint16_t CANidTxGFC) { + if (GFC == NULL || OD_1300_gfcParameter == NULL || GFC_CANdevRx == NULL || GFC_CANdevTx == NULL) { return CO_ERROR_ILLEGAL_ARGUMENT; } - GFC->valid = valid; -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER - GFC->CANdevTx = GFC_CANdevTx; -#endif -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER - GFC->functSignalObjectSafe = NULL; - GFC->pFunctSignalSafe = NULL; -#endif + + uint8_t valid = 0; + if (OD_get_u8(OD_1300_gfcParameter, 0, &valid, true) != ODR_OK) { + return CO_ERROR_OD_PARAMETERS; + } + GFC->valid = (valid == 1); + + /* Configure Object dictionary entry at index 0x1300+ */ + GFC->OD_gfcParam_ext.object = GFC; + GFC->OD_gfcParam_ext.read = OD_readOriginal; + GFC->OD_gfcParam_ext.write = OD_write_1300; + OD_extension_init(OD_1300_gfcParameter, &GFC->OD_gfcParam_ext); #if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER - GFC->CANtxBuff = CO_CANtxBufferInit( - GFC->CANdevTx, /* CAN device */ - GFC_txIdx, /* index of specific buffer inside CAN module */ - CANidTxGFC, /* CAN identifier */ - 0, /* rtr */ - 0, /* number of data bytes */ - 0); /* synchronous message flag bit */ + GFC->CANdevTx = GFC_CANdevTx; + GFC->CANtxBuff = CO_CANtxBufferInit(GFC->CANdevTx, /* CAN device */ + GFC_txIdx, /* index of specific buffer inside CAN module */ + CANidTxGFC, /* CAN identifier */ + 0, /* rtr */ + 0, /* number of data bytes */ + 0); /* synchronous message flag bit */ if (GFC->CANtxBuff == NULL) { return CO_ERROR_TX_UNCONFIGURED; } #else - (void)GFC_txIdx; /* unused */ - (void)CANidTxGFC; /* unused */ + (void)GFC_txIdx; /* unused */ + (void)CANidTxGFC; /* unused */ #endif + #if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER - const CO_ReturnError_t r = CO_CANrxBufferInit( - GFC_CANdevRx, /* CAN device */ - GFC_rxIdx, /* rx buffer index */ - CANidRxGFC, /* CAN identifier */ - 0x7FF, /* mask */ - 0, /* rtr */ - (void *)GFC, /* object passed to receive function */ - CO_GFC_receive); /* this function will process received message */ + GFC->functSignalObjectSafe = NULL; + GFC->pFunctSignalSafe = NULL; + const CO_ReturnError_t r = CO_CANrxBufferInit(GFC_CANdevRx, /* CAN device */ + GFC_rxIdx, /* rx buffer index */ + CANidRxGFC, /* CAN identifier */ + 0x7FF, /* mask */ + 0, /* rtr */ + (void*)GFC, /* object passed to receive function */ + CO_GFC_receive); /* this function will process received message */ if (r != CO_ERROR_NO) { return r; } #else - (void)GFC_rxIdx; /* unused */ - (void)CANidRxGFC; /* unused */ + (void)GFC_rxIdx; /* unused */ + (void)CANidRxGFC; /* unused */ #endif return CO_ERROR_NO; } #if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER - -CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC) -{ - if (*GFC->valid == 0x01) +CO_ReturnError_t +CO_GFCsend(CO_GFC_t* GFC) { + if (GFC->valid) { return CO_CANsend(GFC->CANdevTx, GFC->CANtxBuff); + } return CO_ERROR_NO; } #endif diff --git a/304/CO_GFC.h b/304/CO_GFC.h index 575b6bd..677f22e 100644 --- a/304/CO_GFC.h +++ b/304/CO_GFC.h @@ -4,7 +4,8 @@ * @file CO_GFC.h * @ingroup CO_GFC * @author Robert Grüning - * @copyright 2020 - 2020 Robert Grüning + * @copyright 2020 Robert Grüning + * @copyright 2024 Janez Paternoster * * This file is part of CANopenNode, an opensource CANopen Stack. * Project home page is . @@ -27,6 +28,7 @@ #define CO_GFC_H #include "301/CO_driver.h" +#include "301/CO_ODinterface.h" /* default configuration, see CO_config.h */ #ifndef CO_CONFIG_GFC @@ -47,26 +49,26 @@ extern "C" { * @{ * Very simple consumer/producer protocol. * A net can have multiple GFC producer and multiple GFC consumer. - * On a safety-relevant the producer can send a GFC message (ID 0, DLC 0). + * On a safety-relevant the producer can send a GFC message (ID 1, DLC 0). * The consumer can use this message to start the transition to a safe state. * The GFC is optional for the security protocol and is not monitored (timed). */ - /** * GFC object. */ typedef struct { - uint8_t *valid; /**< From CO_GFC_init() */ -#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN - CO_CANmodule_t *CANdevTx; /**< From CO_GFC_init() */ - CO_CANtx_t *CANtxBuff; /**< CAN transmit buffer inside CANdevTx */ + bool_t valid; /**< From OD parameter 1300 */ + OD_extension_t OD_gfcParam_ext; /**< Extension for OD object */ +#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN + CO_CANmodule_t* CANdevTx; /**< From CO_GFC_init() */ + CO_CANtx_t* CANtxBuff; /**< CAN transmit buffer inside CANdevTx */ #endif -#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN +#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN /** From CO_GFC_initCallbackEnterSafeState() or NULL */ - void (*pFunctSignalSafe)(void *object); + void (*pFunctSignalSafe)(void* object); /** From CO_GFC_initCallbackEnterSafeState() or NULL */ - void *functSignalObjectSafe; + void* functSignalObjectSafe; #endif } CO_GFC_t; @@ -76,7 +78,8 @@ typedef struct { * Function must be called in the communication reset section. * * @param GFC This object will be initialized. - * @param valid pointer to the valid flag in OD (0x1300) + * @param OD_1300_gfcParameter Pointer to _Global fail-safe command parameter_ + * variable from Object dictionary (index 0x1300). * @param GFC_CANdevRx CAN device used for SRDO reception. * @param GFC_rxIdx Index of receive buffer in the above CAN device. * @param CANidRxGFC GFC CAN ID for reception @@ -86,16 +89,11 @@ typedef struct { * * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT. */ -CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC, - uint8_t *valid, - CO_CANmodule_t *GFC_CANdevRx, - uint16_t GFC_rxIdx, - uint16_t CANidRxGFC, - CO_CANmodule_t *GFC_CANdevTx, - uint16_t GFC_txIdx, - uint16_t CANidTxGFC); +CO_ReturnError_t CO_GFC_init(CO_GFC_t* GFC, OD_entry_t* OD_1300_gfcParameter, + CO_CANmodule_t* GFC_CANdevRx, uint16_t GFC_rxIdx, uint16_t CANidRxGFC, + CO_CANmodule_t* GFC_CANdevTx, uint16_t GFC_txIdx, uint16_t CANidTxGFC); -#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN +#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN /** * Initialize GFC callback function. * @@ -107,12 +105,10 @@ CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC, * Can be NULL * @param pFunctSignalSafe Pointer to the callback function. Not called if NULL. */ -void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC, - void *object, - void (*pFunctSignalSafe)(void *object)); +void CO_GFC_initCallbackEnterSafeState(CO_GFC_t* GFC, void* object, void (*pFunctSignalSafe)(void* object)); #endif -#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN +#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN /** * Send GFC message. * @@ -123,7 +119,7 @@ void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC, * * @return Same as CO_CANsend(). */ -CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC); +CO_ReturnError_t CO_GFCsend(CO_GFC_t* GFC); #endif /** @} */ /* CO_GFC */ diff --git a/304/CO_SRDO.c b/304/CO_SRDO.c index 980d8ff..18bdeb3 100644 --- a/304/CO_SRDO.c +++ b/304/CO_SRDO.c @@ -4,7 +4,9 @@ * @file CO_SRDO.c * @ingroup CO_SRDO * @author Robert Grüning - * @copyright 2020 - 2020 Robert Grüning + * @copyright 2020 Robert Grüning + * @copyright 2024 temi54c1l8@github + * @copyright 2024 Janez Paternoster * * This file is part of CANopenNode, an opensource CANopen Stack. * Project home page is . @@ -31,987 +33,940 @@ /* verify configuration */ #if !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE) - #error CO_CONFIG_CRC16_ENABLE must be enabled. +#error CO_CONFIG_CRC16_ENABLE must be enabled. #endif -#define CO_SRDO_INVALID (0U) -#define CO_SRDO_TX (1U) -#define CO_SRDO_RX (2U) +/* 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 (0xA5) -#define CO_SRDO_VALID_MAGIC (0xA5) +/* macro for information about SRDO configuration error */ +#define ERR_INFO(index, subindex, info) (((uint32_t)(index) << 16) | ((uint32_t)(subindex) << 8) | ((uint32_t)(info))) - -#define CO_SRDO_BITCMD_OPERATIONAL (0x01U) -#define CO_SRDO_BITCMD_CALC_CRC (0x02U) - -static void CO_SRDO_receive_normal(void *object, void *msg){ - CO_SRDO_t *SRDO; +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); - uint8_t *data = CO_CANrxMsg_readData(msg); + uint8_t* data = CO_CANrxMsg_readData(msg); - SRDO = (CO_SRDO_t*)object; /* this is the correct pointer type of the first argument */ - - if( (SRDO->valid == CO_SRDO_RX) && - (DLC >= SRDO->dataLength) && !CO_FLAG_READ(SRDO->CANrxNew[1])){ + 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 */ memcpy(SRDO->CANrxData[0], data, sizeof(SRDO->CANrxData[0])); CO_FLAG_SET(SRDO->CANrxNew[0]); #if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE /* Optional signal to RTOS, which can resume task, which handles SRDO. */ - if(SRDO->pFunctSignalPre != NULL) { + 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; +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); - uint8_t *data = CO_CANrxMsg_readData(msg); + uint8_t* data = CO_CANrxMsg_readData(msg); - SRDO = (CO_SRDO_t*)object; /* this is the correct pointer type of the first argument */ - - if( (SRDO->valid == CO_SRDO_RX) && - (DLC >= SRDO->dataLength) && CO_FLAG_READ(SRDO->CANrxNew[0])){ + 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 */ memcpy(SRDO->CANrxData[1], data, sizeof(SRDO->CANrxData[1])); CO_FLAG_SET(SRDO->CANrxNew[1]); #if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE /* Optional signal to RTOS, which can resume task, which handles SRDO. */ - if(SRDO->pFunctSignalPre != NULL) { + 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_SRDOconfigCom(CO_SRDO_t* SRDO, uint32_t COB_IDnormal, uint32_t COB_IDinverted){ - uint16_t IDs[2][2] = {0}; - uint16_t* ID; - uint16_t successCount = 0; +/* 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; - int16_t i; + SRDOGuard->configurationValid = SRDOGuard->_configurationValid = false; - uint32_t COB_ID[2]; - COB_ID[0] = COB_IDnormal; - COB_ID[1] = COB_IDinverted; - - SRDO->valid = CO_SRDO_INVALID; - - /* is SRDO used? */ - if((SRDO->SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC) && ((SRDO->CommPar_informationDirection == CO_SRDO_TX) || (SRDO->CommPar_informationDirection == CO_SRDO_RX)) && - SRDO->dataLength){ - ID = &IDs[SRDO->CommPar_informationDirection - 1][0]; - /* is used default COB-ID? */ - for(i = 0; i < 2; i++){ - if(!(COB_ID[i] & 0xBFFFF800L)){ - ID[i] = (uint16_t)COB_ID[i] & 0x7FF; - - if((ID[i] == SRDO->defaultCOB_ID[i]) && (SRDO->nodeId <= 64U)){ - ID[i] += (uint16_t)SRDO->nodeId*2; - } - if((0x101U <= ID[i]) && (ID[i] <= 0x180U) && ((ID[i] & 1) != i )){ - successCount++; - } - } - } - } - /* all ids are ok*/ - if(successCount == 2){ - SRDO->valid = SRDO->CommPar_informationDirection; - - if (SRDO->valid == CO_SRDO_TX){ - SRDO->timer = (uint32_t)SRDO->nodeId * 500U; /* 0.5ms * node-ID delay*/ - } - else if (SRDO->valid == CO_SRDO_RX){ - SRDO->timer = (uint32_t)SRDO->CommPar_safetyCycleTime * 1000U; - } - else { /* MISRA C 2004 14.10 */ } - } - else{ - memset(IDs, 0, sizeof(IDs)); - CO_FLAG_CLEAR(SRDO->CANrxNew[0]); - CO_FLAG_CLEAR(SRDO->CANrxNew[1]); - SRDO->valid = CO_SRDO_INVALID; - } - - - for(i = 0; i < 2; i++){ - CO_ReturnError_t r; - SRDO->CANtxBuff[i] = CO_CANtxBufferInit( - SRDO->CANdevTx, /* CAN device */ - SRDO->CANdevTxIdx[i], /* index of specific buffer inside CAN module */ - IDs[0][i], /* CAN identifier */ - 0, /* rtr */ - SRDO->dataLength, /* number of data bytes */ - 0); /* synchronous message flag bit */ - - if(SRDO->CANtxBuff[i] == 0){ - SRDO->valid = CO_SRDO_INVALID; - } - - r = CO_CANrxBufferInit( - SRDO->CANdevRx, /* CAN device */ - SRDO->CANdevRxIdx[i], /* rx buffer index */ - IDs[1][i], /* CAN identifier */ - 0x7FF, /* mask */ - 0, /* rtr */ - (void*)SRDO, /* object passed to receive function */ - i ? CO_SRDO_receive_inverted : CO_SRDO_receive_normal); /* this function will process received message */ - if(r != CO_ERROR_NO){ - SRDO->valid = CO_SRDO_INVALID; - CO_FLAG_CLEAR(SRDO->CANrxNew[i]); - } + OD_IO->write(&OD_IO->stream, &val, sizeof(val), &dummy); } } -static CO_ReturnError_t SRDO_configMap(CO_SRDO_t* SRDO, - OD_t *OD, - OD_entry_t *OD_SRDOMapPar){ - ODR_t odRet; - size_t pdoDataLength[2] = { 0, 0 }; - uint8_t mappedObjectsCount = 0; - uint8_t *errInfo = NULL; - //uint32_t *erroneousMap = NULL; - - /* number of mapped application objects in SRDO */ - odRet = OD_get_u8(OD_SRDOMapPar, 0, &mappedObjectsCount, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_SRDOMapPar)) << 8) | 0U; - } - return CO_ERROR_OD_PARAMETERS; - } - if (mappedObjectsCount > CO_SRDO_MAX_SIZE) { - //*erroneousMap = 1; - return CO_ERROR_NO; - } - - /* iterate mapped OD variables */ - for(uint8_t i=0; iCommPar_informationDirection == CO_SRDO_RX; - uint32_t map = 0; - uint8_t plain_inverted = i%2; - - odRet = OD_get_u32(OD_SRDOMapPar, i + 1, &map, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_SRDOMapPar)) << 8) | i; - } - return CO_ERROR_OD_PARAMETERS; - } - - 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; - uint8_t pdoDataStart = pdoDataLength[plain_inverted]; - pdoDataLength[plain_inverted] += mappedLength; - - if (((mappedLengthBits & 0x07) != 0) || (pdoDataLength[plain_inverted] > CO_SRDO_MAX_SIZE)) { - //*erroneousMap = map; - return CO_ERROR_NO; - } - - /* is there a reference to the dummy entry */ - if ((index < 0x20) && (subIndex == 0U)) { - for (uint8_t j = pdoDataStart; j < pdoDataLength[plain_inverted]; j++) { - static uint8_t dummyTX = 0; - static uint8_t dummyRX; - SRDO->mapPointer[plain_inverted][j] = isRSRDO ? &dummyRX : &dummyTX; - } - continue; - } - - /* find entry in the Object Dictionary, original location */ - OD_IO_t OD_IO; - OD_entry_t *entry = OD_find(OD, index); - OD_attr_t testAttribute = isRSRDO ? ODA_RSRDO : ODA_TSRDO; - - odRet = OD_getSub(entry, subIndex, &OD_IO, true); - if (odRet != ODR_OK - || (OD_IO.stream.attribute & testAttribute) == 0 - || OD_IO.stream.dataLength < mappedLength - || OD_IO.stream.dataOrig == NULL - ) { - //*erroneousMap = map; - return CO_ERROR_NO; - } - - /* write locations to OD variable data bytes into PDO map pointers */ -#ifdef CO_BIG_ENDIAN - if((OD_IO.stream.attribute & ODA_MB) != 0) { - uint8_t *odDataPointer = OD_IO.stream.dataOrig - + OD_IO.stream.dataLength - 1; - for (uint8_t j = pdoDataStart; j < pdoDataLength; j++) { - SRDO->mapPointer[j] = odDataPointer--; - } - } - else -#endif - { - uint8_t *odDataPointer = OD_IO.stream.dataOrig; - for (uint8_t j = pdoDataStart; j < pdoDataLength[plain_inverted]; j++) { - SRDO->mapPointer[plain_inverted][j] = odDataPointer++; - } - } - } - - if(pdoDataLength[0] == pdoDataLength[1]){ - SRDO->dataLength = pdoDataLength[0]; - //SRDO->mappedObjectsCount = pdoDataLength[0]; - }else{ - SRDO->dataLength = 0; - CO_errorReport(SRDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, 0); - return CO_ERROR_OD_PARAMETERS; - } - - return CO_ERROR_NO; -} - -static uint16_t CO_SRDOcalcCrc(const CO_SRDO_t *SRDO){ - uint16_t i,tmp_u16; - uint16_t result = 0x0000; - uint8_t buffer[4]; - uint32_t cob, tmp_u32; - uint32_t map; - ODR_t odRet; - - result = crc16_ccitt(&SRDO->CommPar_informationDirection, 1, result); - tmp_u16 = CO_SWAP_16(SRDO->CommPar_safetyCycleTime); - memcpy(&buffer[0], &tmp_u16, sizeof(tmp_u16)); - result = crc16_ccitt(&buffer[0], 2, result); - result = crc16_ccitt(&SRDO->CommPar_safetyRelatedValidationTime, 1, result); - - /* adjust COB-ID if the default is used - Caution: if the node id changes and you are using the default COB-ID you have to recalculate the checksum - This behaviour is controversial and could be changed or made optional. - */ - cob = SRDO->CommPar_COB_ID1_normal; - if(((cob&0x7FFU) == SRDO->defaultCOB_ID[0]) && (SRDO->nodeId <= 64U)){ - cob += (uint32_t)SRDO->nodeId*2; - } - tmp_u32 = CO_SWAP_32(cob); - memcpy(&buffer[0], &tmp_u32, sizeof(tmp_u32)); - result = crc16_ccitt(&buffer[0], 4, result); - - cob = SRDO->CommPar_COB_ID2_inverted; - if(((cob&0x7FFU) == SRDO->defaultCOB_ID[1]) && (SRDO->nodeId <= 64U)){ - cob += (uint32_t)SRDO->nodeId*2; - } - tmp_u32 = CO_SWAP_32(cob); - memcpy(&buffer[0], &tmp_u32, sizeof(tmp_u32)); - result = crc16_ccitt(&buffer[0], 4, result); - - result = crc16_ccitt(&SRDO->mappedObjectsCount, 1, result); - for(i = 0; i < SRDO->mappedObjectsCount;){ - uint8_t subindex = i + 1U; - result = crc16_ccitt(&subindex, 1, result); - odRet = OD_get_u32(SRDO->SRDOMapPar, subindex, &map, true); - if (odRet != ODR_OK) { - map = 0; - } - tmp_u32 = CO_SWAP_32(map); - memcpy(&buffer[0], &tmp_u32, sizeof(tmp_u32)); - result = crc16_ccitt(&buffer[0], 4, result); - i = subindex; - } - return result; -} - - - -static ODR_t OD_read_SRDO_communicationParam(OD_stream_t *stream, void *buf, - OD_size_t count, OD_size_t *countRead) +/* + * 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) { - ODR_t returnCode = OD_readOriginal(stream, buf, count, countRead); - if ( returnCode != ODR_OK ){ - return returnCode; - } - - if ((stream == NULL) || (buf == NULL) || (countRead == NULL) - ) { - return ODR_DEV_INCOMPAT; - } - - CO_SRDO_t *SRDO = stream->object; - - /* Reading Object Dictionary variable */ - if ((stream->subIndex == 5U) || (stream->subIndex == 6U)){ - uint32_t value = CO_getUint32(buf); - uint16_t index = stream->subIndex - 5U; - - /* if default COB ID is used, write default value here */ - if(((value&0x7FFU) == SRDO->defaultCOB_ID[index]) && (SRDO->nodeId <= 64U)){ - value += (uint32_t)SRDO->nodeId*2; - } - - /* If SRDO is not valid, set bit 31. but I do not think this applies to SRDO ?! */ - /* if(!SRDO->valid){ value |= 0x80000000L; }*/ - - CO_setUint32(buf, value); - } + (void) stream; (void) buf; + if (countWritten != NULL) { *countWritten = count; } return ODR_OK; } - -static ODR_t OD_write_SRDO_communicationParam(OD_stream_t *stream, const void *buf, - OD_size_t count, OD_size_t *countWritten) +static ODR_t +OD_read_dummy(OD_stream_t *stream, void *buf, OD_size_t count, OD_size_t *countRead) { - if ((stream == NULL) || (stream->subIndex == 0U) || (buf == NULL) - || (countWritten == NULL) || (count > 4) - ) { + if (buf == NULL || stream == NULL || countRead == NULL) { return ODR_DEV_INCOMPAT; } - CO_SRDO_t *SRDO = stream->object; - CO_SRDOGuard_t *SRDOGuard = SRDO->SRDOGuard; + if (count > stream->dataLength) { + count = stream->dataLength; + } + + memset(buf, 0, count); + + *countRead = count; + return ODR_OK; +} + +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 == 4) { + CO_SRDO_t* SRDO = stream->object; + + uint32_t value = CO_getUint32(buf); + uint16_t defaultCOB_ID = SRDO->defaultCOB_ID + (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 * 2; + } + + 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 > 4)) { + return ODR_DEV_INCOMPAT; + } + + CO_SRDO_t* SRDO = stream->object; + CO_SRDOGuard_t* SRDOGuard = SRDO->SRDOGuard; uint8_t bufCopy[4]; memcpy(bufCopy, buf, count); /* Writing Object Dictionary variable */ - if(SRDOGuard->operatingState){ - return ODR_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */ + 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){ + if (stream->subIndex == 1U) { /* Information direction */ uint8_t value = CO_getUint8(buf); - if (value > 2U){ + if (value > 2U) { return ODR_INVALID_VALUE; } - SRDO->CommPar_informationDirection = value; - } - else if(stream->subIndex == 2U){ + SRDO->informationDirection = value; + } else if (stream->subIndex == 2) { /* SCT */ uint16_t value = CO_getUint16(buf); - if (value == 0U){ + if (value < ((CO_CONFIG_SRDO_MINIMUM_DELAY / 1000) + 1)) { return ODR_INVALID_VALUE; } - SRDO->CommPar_safetyCycleTime = value; - } - else if(stream->subIndex == 3U){ + } else if (stream->subIndex == 3) { /* SRVT */ uint8_t value = CO_getUint8(buf); - if (value == 0U){ + if (value == 0) { return ODR_INVALID_VALUE; } - SRDO->CommPar_safetyRelatedValidationTime = value; - } - else if(stream->subIndex == 4){ /* Transmission_type */ + } else if (stream->subIndex == 4) { /* Transmission_type */ uint8_t value = CO_getUint8(buf); - if(value != 254){ - return ODR_INVALID_VALUE; /* Invalid value for parameter (download only). */ + if (value != 254) { + return ODR_INVALID_VALUE; } - SRDO->CommPar_transmissionType = value; - } - else if(stream->subIndex == 5U || stream->subIndex == 6U){ /* COB_ID */ + } else if (stream->subIndex == 5U || stream->subIndex == 6U) { /* COB_ID */ uint32_t value = CO_getUint32(buf); uint16_t index = 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 < 0x101) || (value > 0x180U) || ((value & 1) == index)){ - return ODR_INVALID_VALUE; /* Invalid value for parameter (download only). */ + if ((value < 0x101) || (value > 0x180U) || ((value & 1) == 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 == (SRDO->defaultCOB_ID[index] + ((uint32_t)SRDO->nodeId*2)))){ - value = SRDO->defaultCOB_ID[index]; + if ((SRDO->nodeId <= 64U) && (value == (defaultCOB_ID + ((uint32_t)SRDO->nodeId * 2)))) { + value = defaultCOB_ID; CO_setUint32(bufCopy, value); } - if(index == 0U){ - SRDO->CommPar_COB_ID1_normal = value; - } - else{ - SRDO->CommPar_COB_ID2_inverted = value; - } + } else { /* MISRA C 2004 14.10 */ } - else { /* MISRA C 2004 14.10 */ } - SRDOGuard->configurationValid = CO_SRDO_INVALID; + /* 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) - ) { +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; } - - CO_SRDO_t *SRDO = stream->object; - CO_SRDOGuard_t *SRDOGuard = SRDO->SRDOGuard; + + CO_SRDO_t* SRDO = stream->object; + CO_SRDOGuard_t* SRDOGuard = SRDO->SRDOGuard; /* Writing Object Dictionary variable */ - if(SRDOGuard->operatingState){ - return ODR_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */ + if (SRDOGuard->NMTisOperational) { + /* Data cannot be transferred or stored to the application because of the present device state. */ + return ODR_DATA_DEV_STATE; } - if(SRDO->CommPar_informationDirection){ /* SRDO must be deleted */ - return ODR_UNSUPP_ACCESS; /* Unsupported access to an object. */ + /* SRDO must be disabled */ + if (SRDO->informationDirection != 0) { + return ODR_UNSUPP_ACCESS; /* Unsupported access to an object. */ } /* numberOfMappedObjects */ - if(stream->subIndex == 0U){ + if (stream->subIndex == 0U) { uint8_t value = CO_getUint8(buf); - if((value > 16U) || (value & 1)){ /*only odd numbers are allowed*/ - return ODR_MAP_LEN; /* Number and length of object to be mapped exceeds SRDO length. */ + /* only odd numbers are allowed */ + if ((value > CO_SRDO_MAX_MAPPED_ENTRIES) || (value & 1)) { + return ODR_MAP_LEN; /* Number and length of object to be mapped exceeds SRDO length. */ } SRDO->mappedObjectsCount = value; } - else{ - if (SRDO->mappedObjectsCount != 0U){ + /* mapping objects */ + else { + if (SRDO->mappedObjectsCount != 0U) { return ODR_UNSUPP_ACCESS; } - } - SRDOGuard->configurationValid = CO_SRDO_INVALID; - - - /* write value to the original location in the Object Dictionary */ - return OD_writeOriginal(stream, buf, count, countWritten); -} - -static ODR_t OD_read_13FE(OD_stream_t *stream, void *buf, - OD_size_t count, OD_size_t *countRead) -{ - ODR_t returnCode = OD_readOriginal(stream, buf, count, countRead); - if ( returnCode != ODR_OK ){ - return returnCode; - } - - if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL) || (countRead == NULL) - ) { - return ODR_DEV_INCOMPAT; - } - - CO_SRDOGuard_t *SRDOGuard = stream->object; - CO_setUint8(buf, SRDOGuard->configurationValid); - - return ODR_OK; -} - -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; + /* No other checking is implemented here. Values are validated in the configuration function. */ } - CO_SRDOGuard_t *SRDOGuard = stream->object; - uint8_t value = CO_getUint8(buf); - - if(SRDOGuard->operatingState){ - return ODR_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */ - } - SRDOGuard->checkCRC = ( value == CO_SRDO_VALID_MAGIC ); - - SRDOGuard->configurationValid = value; + /* 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_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) - ) { +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; - uint16_t value = CO_getUint16(buf); + CO_SRDOGuard_t* SRDOGuard = stream->object; - if(SRDOGuard->operatingState){ - return ODR_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */ + if (SRDOGuard->NMTisOperational) { + /* Data cannot be transferred or stored to the application because of the present device state. */ + return ODR_DATA_DEV_STATE; } - SRDOGuard->configurationValid = CO_SRDO_INVALID; /* write value to the original location in the Object Dictionary */ return OD_writeOriginal(stream, buf, count, countWritten); } -CO_ReturnError_t CO_SRDOGuard_init( - CO_SRDOGuard_t *SRDOGuard, - OD_entry_t *OD_13FE_SRDOValid, - OD_entry_t *OD_13FF_SRDOCRC, - uint32_t *errInfo) -{ - ODR_t odRet; - - /* verify arguments */ - if((SRDOGuard==NULL) || (OD_13FE_SRDOValid==NULL) || (OD_13FF_SRDOCRC==NULL)){ - return CO_ERROR_ILLEGAL_ARGUMENT; +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; } - /* clear object */ - memset(SRDOGuard, 0, sizeof(CO_SRDOGuard_t)); + CO_SRDOGuard_t* SRDOGuard = stream->object; - SRDOGuard->operatingState = false; - SRDOGuard->SRDO_CRC = OD_13FF_SRDOCRC; - - uint8_t value = 0; - odRet = OD_get_u8(OD_13FE_SRDOValid, 0, &value, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_13FE_SRDOValid)) << 8) | 1U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDOGuard->configurationValid = value; - - SRDOGuard->checkCRC = (SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC); - - /* Configure Object dictionary entry at index 0x13FE and 0x13FF */ - SRDOGuard->OD_13FE_extension.object = SRDOGuard; - SRDOGuard->OD_13FE_extension.read = OD_read_13FE; - SRDOGuard->OD_13FE_extension.write = OD_write_13FE; - OD_extension_init(OD_13FE_SRDOValid, &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; - OD_extension_init(OD_13FF_SRDOCRC, &SRDOGuard->OD_13FF_extension); - - return CO_ERROR_NO; -} - -uint8_t CO_SRDOGuard_process( - CO_SRDOGuard_t *SRDOGuard, - bool_t NMTisOperational) -{ - uint8_t result = 0; - if(NMTisOperational != SRDOGuard->operatingState){ - SRDOGuard->operatingState = NMTisOperational; - if (NMTisOperational){ - result |= CO_SRDO_BITCMD_OPERATIONAL; - } + if (SRDOGuard->NMTisOperational) { + /* Data cannot be transferred or stored to the application because of the present device state. */ + return ODR_DATA_DEV_STATE; } - if(SRDOGuard->checkCRC){ - result |= CO_SRDO_BITCMD_CALC_CRC; - SRDOGuard->checkCRC = 0; - } - return result; + /* 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 -/******************************************************************************/ -void CO_SRDO_initCallbackPre( - CO_SRDO_t *SRDO, - void *object, - void (*pFunctSignalPre)(void *object)) -{ - if(SRDO != NULL){ +void +CO_SRDO_initCallbackPre(CO_SRDO_t* SRDO, void* object, void (*pFunctSignalPre)(void* object)) { + if (SRDO != NULL) { SRDO->functSignalObjectPre = object; SRDO->pFunctSignalPre = pFunctSignalPre; } } #endif -/******************************************************************************/ -void CO_SRDO_initCallbackEnterSafeState( - CO_SRDO_t *SRDO, - void *object, - void (*pFunctSignalSafe)(void *object)) -{ - if(SRDO != NULL){ - SRDO->functSignalObjectSafe = object; - SRDO->pFunctSignalSafe = pFunctSignalSafe; - } -} - -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 *CANdevRx, - uint16_t CANdevRxIdxNormal, - uint16_t CANdevRxIdxInverted, - CO_CANmodule_t *CANdevTx, - uint16_t CANdevTxIdxNormal, - uint16_t CANdevTxIdxInverted, - uint32_t *errInfo) -{ +CO_ReturnError_t +CO_SRDO_init_start(CO_SRDOGuard_t* SRDOGuard, OD_entry_t* OD_13FE_configurationValid, + OD_entry_t* OD_13FF_safetyConfigurationSignature, uint32_t* errInfo) { ODR_t odRet; - /* verify arguments */ - if((SRDO==NULL) || (SRDOGuard==NULL) || (OD==NULL) || (em==NULL) || - (OD_130x_SRDOCommPar==NULL) || (OD_138x_SRDOMapPar==NULL) || (CANdevRx==NULL) || (CANdevTx==NULL)){ + /* verify arguments */ + if ((SRDOGuard == NULL) || (OD_13FE_configurationValid == NULL) || (OD_13FF_safetyConfigurationSignature == NULL)) { return CO_ERROR_ILLEGAL_ARGUMENT; } - + /* clear object */ - memset(SRDO, 0, sizeof(CO_SRDO_t)); + memset(SRDOGuard, 0, sizeof(CO_SRDOGuard_t)); - /* Configure object variables */ - SRDO->SRDO_Index = SRDO_Index; - SRDO->SRDOGuard = SRDOGuard; - SRDO->em = em; - SRDO->OD = OD; - SRDO->SRDOCommPar = OD_130x_SRDOCommPar; - SRDO->SRDOMapPar = OD_138x_SRDOMapPar; - SRDO->CANdevRx = CANdevRx; - SRDO->CANdevRxIdx[0] = CANdevRxIdxNormal; - SRDO->CANdevRxIdx[1] = CANdevRxIdxInverted; - SRDO->CANdevTx = CANdevTx; - SRDO->CANdevTxIdx[0] = CANdevTxIdxNormal; - SRDO->CANdevTxIdx[1] = CANdevTxIdxInverted; - SRDO->nodeId = nodeId; - SRDO->defaultCOB_ID[0] = defaultCOB_ID; - SRDO->defaultCOB_ID[1] = defaultCOB_ID + 1U; - SRDO->valid = CO_SRDO_INVALID; - SRDO->toogle = 0; - SRDO->timer = 0; - SRDO->pFunctSignalSafe = NULL; - SRDO->functSignalObjectSafe = NULL; -#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE - SRDO->pFunctSignalPre = NULL; - SRDO->functSignalObjectPre = NULL; -#endif + /* 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; + OD_extension_init(OD_13FE_configurationValid, &SRDOGuard->OD_13FE_extension); - uint8_t informationDirection = 0; - odRet = OD_get_u8(OD_130x_SRDOCommPar, 1, &informationDirection, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_130x_SRDOCommPar)) << 8) | 1U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDO->CommPar_informationDirection = informationDirection; + SRDOGuard->OD_13FF_extension.object = SRDOGuard; + SRDOGuard->OD_13FF_extension.read = OD_readOriginal; + SRDOGuard->OD_13FF_extension.write = OD_write_13FF; + OD_extension_init(OD_13FF_safetyConfigurationSignature, &SRDOGuard->OD_13FF_extension); - uint16_t safetyCycleTime = 0; - odRet = OD_get_u16(OD_130x_SRDOCommPar, 2, &safetyCycleTime, true); - if (odRet != ODR_OK) { + /* 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 != 1) { if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_130x_SRDOCommPar)) << 8) | 2U; + *errInfo = (((uint32_t)OD_getIndex(OD_13FE_configurationValid)) << 8) | 1U; } return CO_ERROR_OD_PARAMETERS; } - SRDO->CommPar_safetyCycleTime= safetyCycleTime; - - uint8_t safetyRelatedValidationTime = 0; - odRet = OD_get_u8(OD_130x_SRDOCommPar, 3, &safetyRelatedValidationTime, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_130x_SRDOCommPar)) << 8) | 3U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDO->CommPar_safetyRelatedValidationTime = safetyRelatedValidationTime; - - uint8_t transmissionType = 0; - odRet = OD_get_u8(OD_130x_SRDOCommPar, 4, &transmissionType, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_130x_SRDOCommPar)) << 8) | 4U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDO->CommPar_transmissionType = transmissionType; - - uint32_t COB_ID1_normal = 0; - odRet = OD_get_u32(OD_130x_SRDOCommPar, 5, &COB_ID1_normal, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_130x_SRDOCommPar)) << 8) | 5U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDO->CommPar_COB_ID1_normal = COB_ID1_normal; - - uint32_t COB_ID2_inverted = 0; - odRet = OD_get_u32(OD_130x_SRDOCommPar, 6, &COB_ID2_inverted, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_130x_SRDOCommPar)) << 8) | 6U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDO->CommPar_COB_ID2_inverted = COB_ID2_inverted; - /* Configure Object dictionary entry at index 0x1301+ and 0x1381+ */ - SRDO->OD_communicationParam_ext.object = SRDO; - SRDO->OD_communicationParam_ext.read = OD_read_SRDO_communicationParam; - SRDO->OD_communicationParam_ext.write = OD_write_SRDO_communicationParam; - OD_extension_init(OD_130x_SRDOCommPar, &SRDO->OD_communicationParam_ext); - - uint8_t numberOfMappedObjects = 0; - odRet = OD_get_u8(OD_138x_SRDOMapPar, 0, &numberOfMappedObjects, true); - if (odRet != ODR_OK) { - if (errInfo != NULL) { - *errInfo = (((uint32_t)OD_getIndex(OD_138x_SRDOMapPar)) << 8) | 0U; - } - return CO_ERROR_OD_PARAMETERS; - } - SRDO->mappedObjectsCount = numberOfMappedObjects; - - SRDO->OD_mappingParam_extension.object = SRDO; - SRDO->OD_mappingParam_extension.read = OD_readOriginal; - SRDO->OD_mappingParam_extension.write = OD_write_SRDO_mappingParam; - OD_extension_init(OD_138x_SRDOMapPar, &SRDO->OD_mappingParam_extension); + /* Private variable, erroneous SRDO initialization will clear this. */ + SRDOGuard->_configurationValid = true; return CO_ERROR_NO; } -CO_ReturnError_t CO_SRDO_requestSend( - CO_SRDO_t *SRDO) -{ - if (SRDO->SRDOGuard->operatingState == false){ - return CO_ERROR_WRONG_NMT_STATE; +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, + OD_entry_t* OD_13FE_configurationValid, OD_entry_t* OD_13FF_safetyConfigurationSignature, + 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; + uint32_t err = 0; + bool_t configurationInProgress = false; + + /* variables will be retrieved from Object Dictionary */ + uint8_t cp_highestSubindexSupported; + uint8_t informationDirection; + uint16_t safetyCycleTime; + uint8_t safetyRelatedValidationTime; + uint8_t transmissionType; + uint32_t COB_ID1_normal; + uint32_t COB_ID2_inverted; + uint8_t configurationValid; + uint16_t crcSignatureFromOD; + uint8_t mappedObjectsCount; + uint32_t mapping[CO_SRDO_MAX_MAPPED_ENTRIES]; + + /* verify arguments */ + if ((SRDO == NULL) || (SRDOGuard == NULL) || (OD == NULL) || (em == NULL) || (OD_130x_SRDOCommPar == NULL) + || (OD_138x_SRDOMapPar == NULL) || (OD_13FE_configurationValid == NULL) || (OD_13FF_safetyConfigurationSignature == NULL) + || (CANdevRxNormal == NULL) || (CANdevRxInverted == NULL) || (CANdevTxNormal == NULL) || (CANdevTxInverted == NULL)) { + ret = CO_ERROR_ILLEGAL_ARGUMENT; + err = 1; } - if (SRDO->valid != CO_SRDO_TX){ - return CO_ERROR_TX_UNCONFIGURED; + /* clear object and configure some object variables */ + if (err == 0) { + memset(SRDO, 0, sizeof(CO_SRDO_t)); + + SRDO->SRDOGuard = SRDOGuard; + SRDO->em = em; + SRDO->defaultCOB_ID = defaultCOB_ID; + SRDO->nodeId = nodeId; + SRDO->CANdevTx[0] = CANdevTxNormal; + SRDO->CANdevTx[1] = CANdevTxInverted; + + /* 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; + 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; + OD_extension_init(OD_138x_SRDOMapPar, &SRDO->OD_mappingParam_extension); } - if(SRDO->toogle != 0U){ - return CO_ERROR_TX_BUSY; + /* Get variables from object Dictionary and verify it's structure. */ + if (err == 0) { + if (OD_get_u8(OD_130x_SRDOCommPar, 0, &cp_highestSubindexSupported, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 0, 1); + } + else if (OD_get_u8(OD_130x_SRDOCommPar, 1, &informationDirection, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 1, 1); + } + else if (OD_get_u16(OD_130x_SRDOCommPar, 2, &safetyCycleTime, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 2, 1); + } + else if (OD_get_u8(OD_130x_SRDOCommPar, 3, &safetyRelatedValidationTime, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 3, 1); + } + else if (OD_get_u8(OD_130x_SRDOCommPar, 4, &transmissionType, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 4, 1); + } + else if (OD_get_u32(OD_130x_SRDOCommPar, 5, &COB_ID1_normal, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 5, 1); + } + else if (OD_get_u32(OD_130x_SRDOCommPar, 6, &COB_ID2_inverted, true) != ODR_OK) { + err = ERR_INFO(0x1301 + SRDO_Index, 6, 1); + } + else if (OD_get_u8(OD_13FE_configurationValid, 0, &configurationValid, true) != ODR_OK) { + err = ERR_INFO(0x13FE, 0, 1); + } + else if (OD_get_u16(OD_13FF_safetyConfigurationSignature, SRDO_Index + 1, &crcSignatureFromOD, true) != ODR_OK) { + err = ERR_INFO(0x13FF, SRDO_Index + 1, 1); + } + else if (OD_get_u8(OD_138x_SRDOMapPar, 0, &mappedObjectsCount, true) != ODR_OK) { + err = ERR_INFO(0x1381 + SRDO_Index, 0, 1); + } + else { + for (uint8_t i = 1; i <= CO_SRDO_MAX_MAPPED_ENTRIES; i++) { + if (OD_get_u32(OD_138x_SRDOMapPar, i, &mapping[i], true) != ODR_OK) { + err = ERR_INFO(0x1381 + SRDO_Index, i, 1); + break; + } + } + } + + /* if OD contains default COB_IDs, add node-id */ + if (COB_ID1_normal == defaultCOB_ID && COB_ID2_inverted == (defaultCOB_ID + 1) && nodeId <= 64U) { + uint32_t add = (uint32_t)SRDO->nodeId * 2; + 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 != 0) { + ret = CO_ERROR_OD_PARAMETERS; + } } - SRDO->timer = 0; - return CO_ERROR_NO; + /* If configurationValid is set and SRDO is valid, continue with further configuration */ + if (err == 0 && configurationValid == CO_SRDO_VALID_MAGIC && informationDirection != CO_SRDO_INVALID) { + configurationInProgress = true; + } + + /* Verify parameters from OD */ + if (err == 0 && configurationInProgress) { + if (cp_highestSubindexSupported != 6) { + err = ERR_INFO(0x1301 + SRDO_Index, 0, 2); + } + else if (informationDirection > 3) { + err = ERR_INFO(0x1301 + SRDO_Index, 1, 2); + } + else if (safetyCycleTime < ((CO_CONFIG_SRDO_MINIMUM_DELAY / 1000) + 1)) { + err = ERR_INFO(0x1301 + SRDO_Index, 2, 2); + } + else if (safetyRelatedValidationTime < 1) { + err = ERR_INFO(0x1301 + SRDO_Index, 3, 2); + } + else if (transmissionType != 254) { + err = ERR_INFO(0x1301 + SRDO_Index, 4, 2); + } + else if (COB_ID1_normal < 0x101 || (COB_ID1_normal & 1) == 0) { + err = ERR_INFO(0x1301 + SRDO_Index, 5, 2); + } + else if ((COB_ID1_normal + 1) != COB_ID2_inverted || COB_ID2_inverted > 0x180) { + err = ERR_INFO(0x1301 + SRDO_Index, 6, 2); + } + else if (mappedObjectsCount > CO_SRDO_MAX_MAPPED_ENTRIES || (mappedObjectsCount & 1) != 0) { + err = ERR_INFO(0x1381 + SRDO_Index, 0, 2); + } + else { + /* MISRA C 2004 14.10 */ + } + } + + /* Verify CRC */ + if (err == 0 && 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 subindex = i + 1U; + crcResult = crc16_ccitt(&subindex, 1, crcResult); + tmp_u32 = CO_SWAP_32(mapping[i]); + crcResult = crc16_ccitt((uint8_t *)&tmp_u32, 4, crcResult); + } + + if (crcResult != crcSignatureFromOD) { + err = ERR_INFO(0x13FF, SRDO_Index + 1, 3); + } + } + + /* Configure mappings */ + if (err == 0 && configurationInProgress) { + size_t srdoDataLength[2] = {0, 0}; + + for (uint8_t i = 0; i < mappedObjectsCount; i++) { + uint8_t plain_inverted = i % 2; + 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(0x1381 + SRDO_Index, i + 1, 4); + } + /* is there a reference to the dummy entry */ + else if (index < 0x20 && subIndex == 0) { + OD_stream_t *stream = &OD_IO->stream; + memset(stream, 0, sizeof(OD_stream_t)); + stream->dataLength = 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(OD, index); + ODR_t odRet = OD_getSub(entry, subIndex, &OD_IOcopy, false); + if (odRet != ODR_OK) { + err = ERR_INFO(0x1381 + SRDO_Index, i + 1, 5); + } + else { + /* verify access attributes, byte alignment and length */ + OD_attr_t testAttribute = (informationDirection == CO_SRDO_RX) ? ODA_RSRDO : ODA_TSRDO; + if ((OD_IOcopy.stream.attribute & testAttribute) == 0 + || (mappedLengthBits & 0x07) != 0 + || OD_IOcopy.stream.dataLength < mappedLength + ) { + err = ERR_INFO(0x1381 + SRDO_Index, i + 1, 6); + } + + /* Copy values and store mappedLength temporary. */ + *OD_IO = OD_IOcopy; + OD_IO->stream.dataOffset = mappedLength; + srdoDataLength[plain_inverted] += mappedLength; + } + } + if (err != 0) { + break; + } + } /* for (uint8_t i = 0; i < mappedObjectsCount; i++) */ + + if (err == 0) { + if (srdoDataLength[0] != srdoDataLength[1]) { + err = ERR_INFO(0x1381 + SRDO_Index, 0, 7); + } + else if (srdoDataLength[0] == 0 || srdoDataLength[0] > CO_SRDO_MAX_SIZE) { + err = ERR_INFO(0x1381 + SRDO_Index, 0, 8); + } + else { + SRDO->dataLength = srdoDataLength[0]; + SRDO->mappedObjectsCount = mappedObjectsCount; + } + } + } + + /* Configure CAN tx buffers */ + if (err == 0 && configurationInProgress && informationDirection == CO_SRDO_TX) { + + SRDO->CANtxBuff[0] = CO_CANtxBufferInit(CANdevTxNormal, /* CAN device */ + CANdevTxIdxNormal, /* index of specific buffer inside CAN module */ + COB_ID1_normal, /* CAN identifier */ + 0, /* rtr */ + SRDO->dataLength, /* number of data bytes */ + 0); /* synchronous message flag bit */ + + if (SRDO->CANtxBuff[0] == NULL) { + err = ERR_INFO(0x1301 + SRDO_Index, 5, 10); + } + + SRDO->CANtxBuff[1] = CO_CANtxBufferInit(CANdevTxInverted, /* CAN device */ + CANdevTxIdxInverted, /* index of specific buffer inside CAN module */ + COB_ID2_inverted, /* CAN identifier */ + 0, /* rtr */ + SRDO->dataLength, /* number of data bytes */ + 0); /* synchronous message flag bit */ + + if (SRDO->CANtxBuff[1] == NULL) { + err = ERR_INFO(0x1301 + SRDO_Index, 6, 10); + } + } + + /* Configure CAN rx buffers */ + if (err == 0 && configurationInProgress && informationDirection == CO_SRDO_RX) { + CO_ReturnError_t ret; + + ret = CO_CANrxBufferInit(CANdevRxNormal, /* CAN device */ + CANdevRxIdxNormal, /* rx buffer index */ + COB_ID1_normal, /* CAN identifier */ + 0x7FF, /* mask */ + 0, /* rtr */ + (void*)SRDO, /* object passed to the receive function */ + CO_SRDO_receive_normal); /* this function will process received message */ + + if (ret != CO_ERROR_NO) { + err = ERR_INFO(0x1301 + SRDO_Index, 5, 11); + } + + ret = CO_CANrxBufferInit(CANdevRxInverted, /* CAN device */ + CANdevRxIdxInverted, /* rx buffer index */ + COB_ID2_inverted, /* CAN identifier */ + 0x7FF, /* mask */ + 0, /* rtr */ + (void*)SRDO, /* object passed to the receive function */ + CO_SRDO_receive_inverted); /* this function will process received message */ + + if (ret != CO_ERROR_NO) { + err = ERR_INFO(0x1301 + SRDO_Index, 6, 11); + } + } + + /* Configure remaining variables */ + if (err == 0) { + 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(em, CO_EM_SRDO_CONFIGURATION, CO_EMC_DATA_SET, err); + configurationValidUnset(SRDO->SRDOGuard); + } + } + + if (errInfo != NULL) { + *errInfo = err; + } + + return ret; } -void CO_SRDO_process( - CO_SRDO_t *SRDO, - uint8_t commands, - uint32_t timeDifference_us, - uint32_t *timerNext_us) -{ - ODR_t odRet; +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(commands & CO_SRDO_BITCMD_CALC_CRC){ - uint16_t crcValue = CO_SRDOcalcCrc(SRDO); - uint16_t crcSRDO = 0; - odRet = OD_get_u16(SRDO->SRDOGuard->SRDO_CRC, SRDO->SRDO_Index+1, &crcSRDO, true); - if ((odRet != ODR_OK) || (crcSRDO != crcValue)) { - SRDO->SRDOGuard->configurationValid = 0; + 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->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((commands & CO_SRDO_BITCMD_OPERATIONAL) && (SRDO->SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC)){ - if(SRDO_configMap(SRDO,SRDO->OD,SRDO->SRDOMapPar) == CO_ERROR_NO){ - CO_SRDOconfigCom(SRDO, SRDO->CommPar_COB_ID1_normal, SRDO->CommPar_COB_ID2_inverted); + if (SRDO->internalState <= CO_SRDO_state_unknown) { + SRDO->internalState = CO_SRDO_state_error_internal; /* should not happen */ } - else{ - SRDO->valid = CO_SRDO_INVALID; - } - } + else if (SRDO->informationDirection == CO_SRDO_TX) { + if (SRDO->cycleTimer == 0U) { + if (SRDO->nextIsNormal) { + uint8_t *dataSRDO[2] = {&SRDO->CANtxBuff[0]->data[0], &SRDO->CANtxBuff[1]->data[0]}; + size_t verifyLength = 0; - if(SRDO->valid && SRDO->SRDOGuard->operatingState){ - SRDO->timer = (SRDO->timer > timeDifference_us) ? (SRDO->timer - timeDifference_us) : 0U; - if(SRDO->valid == CO_SRDO_TX){ - if(SRDO->timer == 0U){ - if(SRDO->toogle){ - CO_CANsend(SRDO->CANdevTx, SRDO->CANtxBuff[1]); - SRDO->timer = ((uint32_t)SRDO->CommPar_safetyCycleTime * 1000U) - CO_CONFIG_SRDO_MINIMUM_DELAY; - } - else{ - uint16_t i; - uint8_t* pSRDOdataByte_normal; - uint8_t* pSRDOdataByte_inverted; + /* copy mapped data from Object Dictionary into CAN buffers */ + for (uint8_t i = 0; i < SRDO->mappedObjectsCount; i++) { + uint8_t plain_inverted = i % 2; + OD_IO_t *OD_IO = &SRDO->OD_IO[i]; + OD_stream_t *stream = &OD_IO->stream; - uint8_t** ppODdataByte_normal; - uint8_t** ppODdataByte_inverted; + /* get mappedLength from temporary storage */ + uint8_t mappedLength = (uint8_t) stream->dataOffset; - bool_t data_ok = true; -#if (CO_CONFIG_SRDO) & CO_CONFIG_TSRDO_CALLS_EXTENSION - if(SRDO->SDO->ODExtensions){ - /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */ - const uint32_t* pMap = &SRDO->SRDOMapPar->mappedObjects[0]; - CO_SDOserver_t *pSDO = SRDO->SDO; + /* additional safety check */ + verifyLength += mappedLength; + if (verifyLength > CO_SRDO_MAX_SIZE) { + break; + } - for(i=SRDO->mappedObjectsCount; i>0; i--){ - uint32_t map = *(pMap++); - uint16_t index = (uint16_t)(map>>16); - uint8_t subIndex = (uint8_t)(map>>8); - uint16_t entryNo = CO_OD_find(pSDO, index); - if ( entryNo != 0xFFFF ) - { - CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo]; - if( ext->pODFunc != NULL) - { - CO_ODF_arg_t ODF_arg; - memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t)); - ODF_arg.reading = true; - ODF_arg.index = index; - ODF_arg.subIndex = subIndex; - ODF_arg.object = ext->object; - ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex); - ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex); - ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); /* https://github.com/CANopenNode/CANopenNode/issues/100 */ - ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex); - ext->pODFunc(&ODF_arg); - } + /* 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) { + 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) { + memcpy(dataSRDO[plain_inverted], buf, mappedLength); + } + + dataSRDO[plain_inverted] += mappedLength; + } + + if (verifyLength > CO_SRDO_MAX_SIZE || verifyLength != 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 + /* check data before sending (optional) */ + for (uint8_t i = 0; i < SRDO->dataLength; i++) { + if (~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 = CO_CONFIG_SRDO_MINIMUM_DELAY; + SRDO->internalState = CO_SRDO_state_communicationEstablished; + } + else { + SRDO->internalState = CO_SRDO_state_error_txFail; } } } -#endif - pSRDOdataByte_normal = &SRDO->CANtxBuff[0]->data[0]; - ppODdataByte_normal = &SRDO->mapPointer[0][0]; - - pSRDOdataByte_inverted = &SRDO->CANtxBuff[1]->data[0]; - ppODdataByte_inverted = &SRDO->mapPointer[1][0]; - -#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_CHECK_TX - /* check data before sending (optional) */ - for(i = 0; idataLength; i++){ - uint8_t invert = ~*(ppODdataByte_inverted[i]); - if (*(ppODdataByte_normal[i]) != invert) - { - data_ok = false; - break; - } + } else { + if (CO_CANsend(SRDO->CANdevTx[1], SRDO->CANtxBuff[1]) == CO_ERROR_NO) { + SRDO->cycleTimer = SRDO->cycleTime_us - CO_CONFIG_SRDO_MINIMUM_DELAY; /* cycleTime_us is verified, result can't be <0 */ } -#endif - if(data_ok){ - /* Copy data from Object dictionary. */ - for(i = 0; idataLength; i++){ - pSRDOdataByte_normal[i] = *(ppODdataByte_normal[i]); - pSRDOdataByte_inverted[i] = *(ppODdataByte_inverted[i]); - } - - CO_CANsend(SRDO->CANdevTx, SRDO->CANtxBuff[0]); - - SRDO->timer = CO_CONFIG_SRDO_MINIMUM_DELAY; - } - else{ - SRDO->toogle = 1; - /* save state */ - if(SRDO->pFunctSignalSafe != NULL){ - SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe); - } + else { + SRDO->internalState = CO_SRDO_state_error_txFail; } } - SRDO->toogle = !SRDO->toogle; + SRDO->nextIsNormal = !SRDO->nextIsNormal; } #if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_TIMERNEXT - if(timerNext_us != NULL){ - if(*timerNext_us > SRDO->timer){ - *timerNext_us = SRDO->timer; /* Schedule for next message timer */ + if (timerNext_us != NULL) { + if (*timerNext_us > SRDO->cycleTimer) { + *timerNext_us = SRDO->cycleTimer; /* Schedule for the next message timer */ } } #endif } - else if(SRDO->valid == CO_SRDO_RX){ - if(CO_FLAG_READ(SRDO->CANrxNew[SRDO->toogle])){ + 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->validationTimer = SRDO->cycleTime_us; + SRDO->nextIsNormal = true; - if(SRDO->toogle){ - int16_t i; - uint8_t* pSRDOdataByte_normal; - uint8_t* pSRDOdataByte_inverted; - - uint8_t** ppODdataByte_normal; - uint8_t** ppODdataByte_inverted; + uint8_t *dataSRDO[2] = {&SRDO->CANrxData[0][0], &SRDO->CANrxData[1][0]}; bool_t data_ok = true; - pSRDOdataByte_normal = &SRDO->CANrxData[0][0]; - pSRDOdataByte_inverted = &SRDO->CANrxData[1][0]; - for(i = 0; idataLength; i++){ - uint8_t invert = (uint8_t)(~pSRDOdataByte_inverted[i]); - if(pSRDOdataByte_normal[i] != invert){ + /* Verify, if normal and inverted data matches properly */ + for (uint8_t i = 0; i < SRDO->dataLength; i++) { + if (~dataSRDO[0][i] != dataSRDO[1][i]) { data_ok = false; + SRDO->internalState = CO_SRDO_state_error_rxNotInverted; break; } } - if(data_ok){ - ppODdataByte_normal = &SRDO->mapPointer[0][0]; - ppODdataByte_inverted = &SRDO->mapPointer[1][0]; - /* Copy data to Object dictionary. If between the copy operation CANrxNew - * is set to true by receive thread, then copy the latest data again. */ + /* copy data from CAN messages into mapped data from Object Dictionary */ + if (data_ok) { + size_t verifyLength = 0; + for (uint8_t i = 0; i < SRDO->mappedObjectsCount; i++) { + uint8_t plain_inverted = i % 2; + OD_IO_t *OD_IO = &SRDO->OD_IO[i]; + OD_stream_t *stream = &OD_IO->stream; - for(i = 0; idataLength; i++){ - *(ppODdataByte_normal[i]) = pSRDOdataByte_normal[i]; - *(ppODdataByte_inverted[i]) = pSRDOdataByte_inverted[i]; - } - CO_FLAG_CLEAR(SRDO->CANrxNew[0]); - CO_FLAG_CLEAR(SRDO->CANrxNew[1]); -#if (CO_CONFIG_SRDO) & CO_CONFIG_RSRDO_CALLS_EXTENSION - if(SRDO->SDO->ODExtensions){ - int16_t i; - /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */ - const uint32_t* pMap = &SRDO->SRDOMapPar->mappedObjects[0]; - CO_SDOserver_t *pSDO = SRDO->SDO; + /* get mappedLength from temporary storage */ + OD_size_t *dataOffset = &stream->dataOffset; + uint8_t mappedLength = (uint8_t) (*dataOffset); - for(i=SRDO->mappedObjectsCount; i>0; i--){ - uint32_t map = *(pMap++); - uint16_t index = (uint16_t)(map>>16); - uint8_t subIndex = (uint8_t)(map>>8); - uint16_t entryNo = CO_OD_find(pSDO, index); - if ( entryNo != 0xFFFF ) - { - CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo]; - if( ext->pODFunc != NULL) - { - CO_ODF_arg_t ODF_arg; - memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t)); - ODF_arg.reading = false; - ODF_arg.index = index; - ODF_arg.subIndex = subIndex; - ODF_arg.object = ext->object; - ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex); - ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex); - ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); /* https://github.com/CANopenNode/CANopenNode/issues/100 */ - ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex); - ext->pODFunc(&ODF_arg); - } + /* additional safety check */ + verifyLength += mappedLength; + if (verifyLength > 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) { + memset(buf, 0, sizeof(buf)); + 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 - } - else{ - CO_FLAG_CLEAR(SRDO->CANrxNew[0]); - CO_FLAG_CLEAR(SRDO->CANrxNew[1]); - /* save state */ - if(SRDO->pFunctSignalSafe != NULL){ - SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe); + + /* 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 > CO_SRDO_MAX_SIZE || verifyLength != SRDO->dataLength) { + SRDO->internalState = CO_SRDO_state_error_internal; } - } + else { + SRDO->internalState = CO_SRDO_state_communicationEstablished; + } + } /* if (data_ok) { */ - SRDO->timer = (uint32_t)SRDO->CommPar_safetyCycleTime * 1000U; - } - else{ - SRDO->timer = (uint32_t)SRDO->CommPar_safetyRelatedValidationTime * 1000U; - } - SRDO->toogle = (uint8_t)(!SRDO->toogle); + CO_FLAG_CLEAR(SRDO->CANrxNew[0]); + CO_FLAG_CLEAR(SRDO->CANrxNew[1]); + } /* inverted message received */ } - if(SRDO->timer == 0U){ - SRDO->toogle = 0; - SRDO->timer = (uint32_t)SRDO->CommPar_safetyRelatedValidationTime * 1000U; - CO_FLAG_CLEAR(SRDO->CANrxNew[0]); - CO_FLAG_CLEAR(SRDO->CANrxNew[1]); - /* save state */ - if(SRDO->pFunctSignalSafe != NULL){ - SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe); + /* 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 + 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 */ } } - } - else { /* MISRA C 2004 14.10 */ } - } - else{ - SRDO->valid = CO_SRDO_INVALID; +#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 = SRDO->SRDOGuard->NMTisOperational = NMTisOperational; + + return SRDO->internalState; } #endif /* (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE */ diff --git a/304/CO_SRDO.h b/304/CO_SRDO.h index 810ca88..e24bcd6 100644 --- a/304/CO_SRDO.h +++ b/304/CO_SRDO.h @@ -4,7 +4,9 @@ * @file CO_SRDO.h * @ingroup CO_SRDO * @author Robert Grüning - * @copyright 2020 - 2020 Robert Grüning + * @copyright 2020 Robert Grüning + * @copyright 2024 temi54c1l8@github + * @copyright 2024 Janez Paternoster * * This file is part of CANopenNode, an opensource CANopen Stack. * Project home page is . @@ -26,8 +28,8 @@ #ifndef CO_SRDO_H #define CO_SRDO_H -#include "301/CO_ODinterface.h" #include "301/CO_Emergency.h" +#include "301/CO_ODinterface.h" /* default configuration, see CO_config.h */ #ifndef CO_CONFIG_SRDO @@ -39,21 +41,42 @@ #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN +#warning SRDO is draft version and is not fully tested! + #ifdef __cplusplus extern "C" { #endif /** * @defgroup CO_SRDO SRDO - * CANopen Safety Related Data Object protocol. + * CANopen Safety Related Data Object protocol * * @ingroup CO_CANopen_304 * @{ - * The functionality is very similar to that of the PDOs. - * The main differences is every message is send and received twice. + * Safety Related Data Object protocol is specified by standard EN 50325-5:2010 (formerly CiA304). + * Its functionality is very similar to that of the PDOs. The main differences is every message is send and received twice. * The second message must be bitwise inverted. The delay between the two messages and between each message pair is monitored. * The distinction between sending and receiving SRDO is made at runtime (for PDO it is compile time). * If the security protocol is used, at least one SRDO is mandatory. + * + * If there is erroneous structure of OD entries for SRDO parameters, then @CO_SRDO_init() function + * returns error and CANopen device doesn't work. It is necessary to repair Object Dictionary and reprogram the device. + * + * If there are erroneous values inside SRDO parameters, then Emergency message CO_EM_SRDO_CONFIGURATION is sent. + * Info code (32bit) contains OD index, subindex and additional byte, which helps to determine erroneous OD object. + * + * SRDO configuration consists of one @CO_SRDO_init_start(), @CO_SRDO_init() for each SRDO and one @CO_SRDO_init_end(). + * These may be called in CANopen initialization section after all other CANopen objects are initialized. + * If SRDO OD parameters are edited (in NMT pre-operational state), NMT communication reset is necessary. + * Alternatively SRDO configuration may be executed just after transition to NMT operational state. + * + * @CO_SRDO_process() must be executed cyclically, similar as PDO processing. Function is fast, no time consuming tasks. + * Function returns @CO_SRDO_state_t value, which may be used to determine working-state or safe-state of safety related + * device. If return values from all SRDO objects are >= CO_SRDO_state_communicationEstablished, then working state is allowed. + * Otherwise SR device must be in safe state. + * + * Requirement for mapped objects: + * - @OD_attributes_t must have set bit ODA_RSRDO or ODA_RSRDO or ODA_TRSRDO (by CANopenEditor). */ /** Maximum size of SRDO message, 8 for standard CAN */ @@ -61,10 +84,10 @@ extern "C" { #define CO_SRDO_MAX_SIZE 8U #endif -/** Maximum number of entries, which can be mapped to PDO, 8 for standard CAN, - * may be less to preserve RAM usage */ +/** Maximum number of entries, which can be mapped to SRDO, 2*8 for standard + * CAN, may be less to preserve RAM usage. Must be multiple of 2. */ #ifndef CO_SRDO_MAX_MAPPED_ENTRIES -#define CO_SRDO_MAX_MAPPED_ENTRIES 8U +#define CO_SRDO_MAX_MAPPED_ENTRIES 16U #endif #ifndef CO_SRDO_OWN_TYPES @@ -72,108 +95,133 @@ extern "C" { typedef uint8_t CO_SRDO_size_t; #endif +/** + * SRDO internal state + */ +typedef enum { + CO_SRDO_state_error_internal = -10, /**< internal software error */ + CO_SRDO_state_error_configuration = -9, /**< error in parameters, emergency message was sent */ + CO_SRDO_state_error_txNotInverted = -6, /**< Transmitting SRDO messages was not inverted */ + CO_SRDO_state_error_txFail = -5, /**< SRDO CAN message transmission failed */ + CO_SRDO_state_error_rxTimeoutSRVT = -4, /**< SRDO message didn't receive inside SRVT time */ + CO_SRDO_state_error_rxTimeoutSCT = -3, /**< SRDO inverted message didn't receive inside SCT time */ + CO_SRDO_state_error_rxNotInverted = -2, /**< Received SRDO messages was not inverted */ + CO_SRDO_state_error_rxShort = -1, /**< Received SRDO message is too short */ + CO_SRDO_state_unknown = 0, /**< unknown state, set by @CO_SRDO_init */ + CO_SRDO_state_nmtNotOperational = 1, /**< Internal NMT operating state is not NMT operational */ + CO_SRDO_state_initializing = 2, /**< Just entered NMT operational state, SRDO message not yet received or transmitted */ + CO_SRDO_state_communicationEstablished = 3, /**< SRDO communication established, fully functional */ + CO_SRDO_state_deleted = 10 /**< informationDirection for this SRDO is set to 0 */ +} CO_SRDO_state_t; /** - * Gurad Object for SRDO - * monitors: + * Guard Object for SRDO. + * + * Guard object monitors all SRDO objects for: * - access to CRC objects * - access configuration valid flag * - change in operation state */ -typedef struct{ - bool operatingState; - uint8_t configurationValid; - OD_entry_t *SRDO_CRC; - uint8_t checkCRC; /**< specifies whether a CRC check should be performed */ +typedef struct { + /** True if NMT operating state is operational */ + bool_t NMTisOperational; + /** True if all SRDO objects are properly configured. Set after successful + * finish of all @CO_SRDO_init() functions. Cleared on configuration change. */ + bool_t configurationValid; + /** Private helper variable set on the start of SRDO configuration */ + bool_t _configurationValid; + /** Object for input / output on the OD variable 13FE:00. Configuration + * of any of the the SRDO parameters will write 0 to that variable. */ + OD_IO_t OD_IO_configurationValid; /** Extension for OD object */ OD_extension_t OD_13FE_extension; + /** Extension for OD object */ OD_extension_t OD_13FF_extension; -}CO_SRDOGuard_t; +} CO_SRDOGuard_t; /** * SRDO object. */ -typedef struct{ - CO_EM_t *em; /**< From CO_SRDO_init() */ - CO_SRDOGuard_t *SRDOGuard; /**< From CO_SRDO_init() */ - OD_t *OD; - uint8_t SRDO_Index; /**< From CO_SRDO_init() */ - /** Number of mapped objects in SRDO */ - uint8_t mappedObjectsCount; - /** Pointers to 2*8 data objects, where SRDO will be copied */ - uint8_t *mapPointer[2][CO_SRDO_MAX_SIZE]; +typedef struct { + CO_SRDOGuard_t* SRDOGuard; /**< From CO_SRDO_init() */ + CO_EM_t* em; /**< From CO_SRDO_init() */ + uint16_t defaultCOB_ID; /**< From CO_SRDO_init() */ + uint8_t nodeId; /**< From CO_SRDO_init() */ + CO_CANmodule_t* CANdevTx[2];/**< From CO_SRDO_init() */ + /** Internal state of this SRDO. */ + CO_SRDO_state_t internalState; + /** Copy of variable, internal usage. */ + bool_t NMTisOperationalPrevious; + /** 0 - SRDO is disabled; 1 - SRDO is producer (tx); 2 - SRDO is consumer (rx) */ + uint8_t informationDirection; + /** Safety Cycle Time from object dictionary translated to microseconds */ + uint32_t cycleTime_us; + /** Safety related validation time from object dictionary translated to microseconds */ + uint32_t validationTime_us; + /** cycle timer variable in microseconds */ + uint32_t cycleTimer; + /** validation timer variable in microseconds */ + uint32_t validationTimer; /** Data length of the received SRDO message. Calculated from mapping */ - CO_SRDO_size_t dataLength; - uint8_t nodeId; /**< From CO_SRDO_init() */ - uint16_t defaultCOB_ID[2]; /**< From CO_SRDO_init() */ - /** 0 - invalid, 1 - tx, 2 - rx */ - uint8_t valid; - OD_entry_t *SRDOCommPar; /**< From CO_SRDO_init() */ - OD_entry_t *SRDOMapPar; /**< From CO_SRDO_init() */ - CO_CANmodule_t *CANdevRx; /**< From CO_SRDO_init() */ - CO_CANmodule_t *CANdevTx; /**< From CO_SRDO_init() */ - CO_CANtx_t *CANtxBuff[2]; /**< CAN transmit buffer inside CANdevTx */ - uint16_t CANdevRxIdx[2]; /**< From CO_SRDO_init() */ - uint16_t CANdevTxIdx[2]; /**< From CO_SRDO_init() */ - uint8_t toogle; /**< defines the current state */ - uint32_t timer; /**< transmit timer and receive timeout */ + CO_SRDO_size_t dataLength; + /** Number of mapped objects in SRDO */ + uint8_t mappedObjectsCount; + /** Object dictionary interface for all mapped entries. OD_IO.dataOffset has + * special usage with SRDO. It stores information about mappedLength of + * the variable. mappedLength can be less or equal to the OD_IO.dataLength. + * mappedLength greater than OD_IO.dataLength indicates erroneous mapping. + * OD_IO.dataOffset is set to 0 before read/write function call and after + * the call OD_IO.dataOffset is set back to mappedLength. */ + OD_IO_t OD_IO[CO_SRDO_MAX_MAPPED_ENTRIES]; + /** CAN transmit buffers inside CANdevTx */ + CO_CANtx_t* CANtxBuff[2]; /** Variable indicates, if new SRDO message received from CAN bus. */ - volatile void *CANrxNew[2]; - /** 2*8 data bytes of the received message. */ - uint8_t CANrxData[2][8]; - /** From CO_SRDO_initCallbackEnterSafeState() or NULL */ - void (*pFunctSignalSafe)(void *object); - /** From CO_SRDO_initCallbackEnterSafeState() or NULL */ - void *functSignalObjectSafe; -#if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN - /** From CO_SRDO_initCallbackPre() or NULL */ - void (*pFunctSignalPre)(void *object); - /** From CO_SRDO_initCallbackPre() or NULL */ - void *functSignalObjectPre; -#endif + volatile void* CANrxNew[2]; + /** true, if received SRDO is too short */ + bool_t rxSrdoShort; + /** two buffers of data bytes for the received message. */ + uint8_t CANrxData[2][CO_SRDO_MAX_SIZE]; + /** If true, next processed SRDO message is normal (not inverted) */ + bool_t nextIsNormal; /** Extension for OD object */ OD_extension_t OD_communicationParam_ext; + /** Extension for OD object */ OD_extension_t OD_mappingParam_extension; - - uint8_t CommPar_informationDirection; - uint16_t CommPar_safetyCycleTime; - uint8_t CommPar_safetyRelatedValidationTime; - uint8_t CommPar_transmissionType; - uint32_t CommPar_COB_ID1_normal; - uint32_t CommPar_COB_ID2_inverted; -}CO_SRDO_t; +#if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN + /** From CO_SRDO_initCallbackPre() or NULL */ + void (*pFunctSignalPre)(void* object); + /** From CO_SRDO_initCallbackPre() or NULL */ + void* functSignalObjectPre; +#endif +} CO_SRDO_t; /** * Initialize SRDOGuard object. * - * Function must be called in the communication reset section. + * Function must be called in the communication reset section before @CO_SRDO_init functions. * * @param SRDOGuard This object will be initialized. - * @param SDO SDO object. - * @param operatingState Pointer to variable indicating CANopen device NMT internal state. - * @param configurationValid Pointer to variable with the SR valid flag - * @param idx_SRDOvalid Index in Object Dictionary - * @param idx_SRDOcrc Index in Object Dictionary + * @param OD_13FE_configurationValid Pointer to _Configuration valid_ variable from Object + * dictionary (index 0x13FE). + * @param OD_13FF_safetyConfigurationSignature Pointer to _Safety configuration signature_ variable + * from Object dictionary (index 0x13FF). + * @param [out] errInfo Additional information in case of error, may be NULL. * * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT. */ -CO_ReturnError_t CO_SRDOGuard_init( - CO_SRDOGuard_t *SRDOGuard, - OD_entry_t *OD_13FE_SRDOValid, - OD_entry_t *OD_13FF_SRDOCRC, - uint32_t *errInfo); +CO_ReturnError_t CO_SRDO_init_start(CO_SRDOGuard_t* SRDOGuard, OD_entry_t* OD_13FE_configurationValid, + OD_entry_t* OD_13FF_safetyConfigurationSignature, uint32_t* errInfo); /** - * Process operation and valid state changes. + * Finalize SRDOGuard object. * - * @param SRDOGuard This object. - * @return uint8_t command for CO_SRDO_process(). - * - bit 0 entered operational - * - bit 1 validate checksum + * Function must be called in the communication reset section after @CO_SRDO_init functions. + * + * @param SRDOGuard This object will be finalized. */ -uint8_t CO_SRDOGuard_process( - CO_SRDOGuard_t *SRDOGuard, - bool_t NMTisOperational); +static inline void CO_SRDO_init_end(CO_SRDOGuard_t* SRDOGuard) { + SRDOGuard->configurationValid = SRDOGuard->_configurationValid; +} /** * Initialize SRDO object. @@ -181,44 +229,37 @@ uint8_t CO_SRDOGuard_process( * Function must be called in the communication reset section. * * @param SRDO This object will be initialized. + * @param SRDO_Index OD index of this SRDO, 0 for the first. * @param SRDOGuard SRDOGuard object. + * @param OD CANopen Object Dictionary * @param em Emergency object. - * @param SDO SDO object. * @param nodeId CANopen Node ID of this device. If default COB_ID is used, value will be added. - * @param defaultCOB_ID Default COB ID for this SRDO (without NodeId). - * @param SRDOCommPar Pointer to _SRDO communication parameter_ record from Object + * @param defaultCOB_ID Default COB ID for this SRDO for plain data (without NodeId). + * @param OD_130x_SRDOCommPar Pointer to _SRDO communication parameter_ record from Object * dictionary (index 0x1301+). - * @param SRDOMapPar Pointer to _SRDO mapping parameter_ record from Object + * @param OD_138x_SRDOMapPar Pointer to _SRDO mapping parameter_ record from Object * dictionary (index 0x1381+). - * @param checksum - * @param idx_SRDOCommPar Index in Object Dictionary - * @param idx_SRDOMapPar Index in Object Dictionary + * @param OD_13FE_configurationValid Pointer to _Configuration valid_ variable from Object + * dictionary (index 0x13FE). + * @param OD_13FF_safetyConfigurationSignature Pointer to _Safety configuration signature_ variable + * from Object dictionary (index 0x13FF). * @param CANdevRx CAN device used for SRDO reception. * @param CANdevRxIdxNormal Index of receive buffer in the above CAN device. * @param CANdevRxIdxInverted Index of receive buffer in the above CAN device. * @param CANdevTx CAN device used for SRDO transmission. * @param CANdevTxIdxNormal Index of transmit buffer in the above CAN device. * @param CANdevTxIdxInverted Index of transmit buffer in the above CAN device. + * @param [out] errInfo Additional information in case of error, may be NULL. * - * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT. + * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OD_PARAMETERS. */ -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 *SRDOCommPar, - OD_entry_t *SRDOMapPar, - CO_CANmodule_t *CANdevRx, - uint16_t CANdevRxIdxNormal, - uint16_t CANdevRxIdxInverted, - CO_CANmodule_t *CANdevTx, - uint16_t CANdevTxIdxNormal, - uint16_t CANdevTxIdxInverted, - uint32_t *errInfo); +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, OD_entry_t* OD_13FE_configurationValid, + OD_entry_t* OD_13FF_safetyConfigurationSignature, 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); #if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN /** @@ -232,58 +273,32 @@ CO_ReturnError_t CO_SRDO_init( * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL * @param pFunctSignalPre Pointer to the callback function. Not called if NULL. */ -void CO_SRDO_initCallbackPre( - CO_SRDO_t *SRDO, - void *object, - void (*pFunctSignalPre)(void *object)); +void CO_SRDO_initCallbackPre(CO_SRDO_t* SRDO, void* object, void (*pFunctSignalPre)(void* object)); #endif -/** - * Initialize SRDO callback function. - * - * Function initializes optional callback function, that is called when SRDO enters a safe state. - * This happens when a timeout is reached or the data is inconsistent. The safe state itself is not further defined. - * One measure, for example, would be to go back to the pre-operational state - * Callback is called from CO_SRDO_process(). - * - * @param SRDO This object. - * @param object Pointer to object, which will be passed to pFunctSignalSafe(). Can be NULL - * @param pFunctSignalSafe Pointer to the callback function. Not called if NULL. - */ -void CO_SRDO_initCallbackEnterSafeState( - CO_SRDO_t *SRDO, - void *object, - void (*pFunctSignalSafe)(void *object)); - - /** * Send SRDO on event * - * Sends SRDO before the next refresh timer tiggers. The message itself is send in CO_SRDO_process(). + * Sends SRDO before the next refresh timer tiggers. The message itself is send + * in @CO_SRDO_process(). Note that RTOS have to trigger its processing quickly. * After the transmission the timer is reset to the full refresh time. * * @param SRDO This object. * @return CO_ReturnError_t CO_ERROR_NO if request is granted */ -CO_ReturnError_t CO_SRDO_requestSend( - CO_SRDO_t *SRDO); +CO_ReturnError_t CO_SRDO_requestSend(CO_SRDO_t* SRDO); /** - * Process transmitting/receiving SRDO messages. - * - * This function verifies the checksum on demand. - * This function also configures the SRDO on operation state change to operational + * Process transmitting/receiving individual SRDO message. * * @param SRDO This object. - * @param commands result from CO_SRDOGuard_process(). * @param timeDifference_us Time difference from previous function call in [microseconds]. - * @param [out] timerNext_us info to OS. + * @param [out] timerNext_us info to OS, may be null. + * @param NMTisOperational True if this node is in NMT_OPERATIONAL state. + * + * @return CO_SRDO_state_t internal state */ -void CO_SRDO_process( - CO_SRDO_t *SRDO, - uint8_t commands, - uint32_t timeDifference_us, - uint32_t *timerNext_us); +CO_SRDO_state_t CO_SRDO_process(CO_SRDO_t* SRDO, uint32_t timeDifference_us, uint32_t* timerNext_us, bool_t NMTisOperational); /** @} */ /* CO_SRDO */ diff --git a/CANopen.c b/CANopen.c index b295727..3cebb05 100644 --- a/CANopen.c +++ b/CANopen.c @@ -273,11 +273,11 @@ * number of them. Indexes are sorted in a way, that objects with highest * priority of the CAN identifier are listed first. */ #define CO_RX_IDX_NMT_SLV 0 -#define CO_RX_IDX_SYNC (CO_RX_IDX_NMT_SLV + CO_RX_CNT_NMT_SLV) +#define CO_RX_IDX_GFC (CO_RX_IDX_NMT_SLV + CO_RX_CNT_NMT_SLV) +#define CO_RX_IDX_SYNC (CO_RX_IDX_GFC + CO_RX_CNT_GFC) #define CO_RX_IDX_EM_CONS (CO_RX_IDX_SYNC + CO_RX_CNT_SYNC) #define CO_RX_IDX_TIME (CO_RX_IDX_EM_CONS + CO_RX_CNT_EM_CONS) -#define CO_RX_IDX_GFC (CO_RX_IDX_TIME + CO_RX_CNT_TIME) -#define CO_RX_IDX_SRDO (CO_RX_IDX_GFC + CO_RX_CNT_GFC) +#define CO_RX_IDX_SRDO (CO_RX_IDX_TIME + CO_RX_CNT_TIME) #define CO_RX_IDX_RPDO (CO_RX_IDX_SRDO + CO_RX_CNT_SRDO * 2) #define CO_RX_IDX_SDO_SRV (CO_RX_IDX_RPDO + CO_RX_CNT_RPDO) #define CO_RX_IDX_SDO_CLI (CO_RX_IDX_SDO_SRV + CO_RX_CNT_SDO_SRV) @@ -289,11 +289,11 @@ #define CO_CNT_ALL_RX_MSGS (CO_RX_IDX_LSS_MST + CO_RX_CNT_LSS_MST) #define CO_TX_IDX_NMT_MST 0 -#define CO_TX_IDX_SYNC (CO_TX_IDX_NMT_MST + CO_TX_CNT_NMT_MST) +#define CO_TX_IDX_GFC (CO_TX_IDX_NMT_MST + CO_TX_CNT_NMT_MST) +#define CO_TX_IDX_SYNC (CO_TX_IDX_GFC + CO_TX_CNT_GFC) #define CO_TX_IDX_EM_PROD (CO_TX_IDX_SYNC + CO_TX_CNT_SYNC) #define CO_TX_IDX_TIME (CO_TX_IDX_EM_PROD + CO_TX_CNT_EM_PROD) -#define CO_TX_IDX_GFC (CO_TX_IDX_TIME + CO_TX_CNT_TIME) -#define CO_TX_IDX_SRDO (CO_TX_IDX_GFC + CO_TX_CNT_GFC) +#define CO_TX_IDX_SRDO (CO_TX_IDX_TIME + CO_TX_CNT_TIME) #define CO_TX_IDX_TPDO (CO_TX_IDX_SRDO + CO_TX_CNT_SRDO * 2) #define CO_TX_IDX_SDO_SRV (CO_TX_IDX_TPDO + CO_TX_CNT_TPDO) #define CO_TX_IDX_SDO_CLI (CO_TX_IDX_SDO_SRV + CO_TX_CNT_SDO_SRV) @@ -555,6 +555,9 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { * highest priority of the CAN identifier are listed first. */ int16_t idxRx = 0; co->RX_IDX_NMT_SLV = idxRx; idxRx += RX_CNT_NMT_SLV; +#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE + co->RX_IDX_GFC = idxRx; idxRx += RX_CNT_GFC; +#endif #if (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE co->RX_IDX_SYNC = idxRx; idxRx += RX_CNT_SYNC; #endif @@ -562,9 +565,6 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { #if (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE co->RX_IDX_TIME = idxRx; idxRx += RX_CNT_TIME; #endif -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE - co->RX_IDX_GFC = idxRx; idxRx += RX_CNT_GFC; -#endif #if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE co->RX_IDX_SRDO = idxRx; idxRx += RX_CNT_SRDO * 2; #endif @@ -594,6 +594,9 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { int16_t idxTx = 0; co->TX_IDX_NMT_MST = idxTx; idxTx += TX_CNT_NMT_MST; +#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE + co->TX_IDX_GFC = idxTx; idxTx += TX_CNT_GFC; +#endif #if (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE co->TX_IDX_SYNC = idxTx; idxTx += TX_CNT_SYNC; #endif @@ -601,9 +604,6 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { #if (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE co->TX_IDX_TIME = idxTx; idxTx += TX_CNT_TIME; #endif -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE - co->TX_IDX_GFC = idxTx; idxTx += TX_CNT_GFC; -#endif #if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE co->TX_IDX_SRDO = idxTx; idxTx += TX_CNT_SRDO * 2; #endif @@ -1170,54 +1170,6 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co, } #endif -#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE - if (CO_GET_CNT(GFC) == 1) { - err = CO_GFC_init(co->GFC, - &OD_globalFailSafeCommandParameter, - co->CANmodule, - CO_GET_CO(RX_IDX_GFC), - CO_CAN_ID_GFC, - co->CANmodule, - CO_GET_CO(TX_IDX_GFC), - CO_CAN_ID_GFC); - if (err) { return err; } - } -#endif - -#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE - if (CO_GET_CNT(SRDO) > 0) { - err = CO_SRDOGuard_init(co->SRDOGuard, - OD_GET(H13FE, OD_H13FE_SRDO_VALID), - OD_GET(H13FF, OD_H13FF_SRDO_CHECKSUM), - errInfo); - if (err) { return err; } - - OD_entry_t *SRDOcomm = OD_GET(H1301, OD_H1301_SRDO_1_PARAM); - OD_entry_t *SRDOmap = OD_GET(H1381, OD_H1381_SRDO_1_MAPPING); - for (int16_t i = 0; i < CO_GET_CNT(SRDO); i++) { - uint16_t CANdevRxIdx = CO_GET_CO(RX_IDX_SRDO) + 2 * i; - uint16_t CANdevTxIdx = CO_GET_CO(TX_IDX_SRDO) + 2 * i; - - err = CO_SRDO_init(&co->SRDO[i], i, - co->SRDOGuard, - od, - em, - nodeId, - ((i == 0) ? CO_CAN_ID_SRDO_1 : 0), - SRDOcomm++, - SRDOmap++, - co->CANmodule, - CANdevRxIdx, - CANdevRxIdx + 1, - co->CANmodule, - CANdevTxIdx, - CANdevTxIdx + 1, - errInfo); - if (err) { return err; } - } - } -#endif - #if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER if (CO_GET_CNT(LSS_MST) == 1) { err = CO_LSSmaster_init(co->LSSmaster, @@ -1369,6 +1321,84 @@ CO_ReturnError_t CO_CANopenInitPDO(CO_t *co, } +/******************************************************************************/ +#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE) || ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) +CO_ReturnError_t CO_CANopenInitSRDO(CO_t *co, + CO_EM_t *em, + OD_t *od, + uint8_t nodeId, + uint32_t *errInfo) +{ + if (co == NULL) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + if (nodeId < 1 || nodeId > 127 || co->nodeIdUnconfigured) { + return (co->nodeIdUnconfigured) + ? CO_ERROR_NODE_ID_UNCONFIGURED_LSS : CO_ERROR_ILLEGAL_ARGUMENT; + } + + +#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE + if (CO_GET_CNT(GFC) == 1) { + CO_ReturnError_t err; + err = CO_GFC_init(co->GFC, + OD_GET(H1300, OD_H1300_GFC_PARAM), + co->CANmodule, + CO_GET_CO(RX_IDX_GFC), + CO_CAN_ID_GFC, + co->CANmodule, + CO_GET_CO(TX_IDX_GFC), + CO_CAN_ID_GFC); + if (err) { return err; } + } +#endif + +#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE + if (CO_GET_CNT(SRDO) > 0) { + CO_ReturnError_t err; + err = CO_SRDO_init_start(co->SRDOGuard, + OD_GET(H13FE, OD_H13FE_SRDO_VALID), + OD_GET(H13FF, OD_H13FF_SRDO_CHECKSUM), + errInfo); + if (err) { return err; } + + OD_entry_t *SRDOcomm = OD_GET(H1301, OD_H1301_SRDO_1_PARAM); + OD_entry_t *SRDOmap = OD_GET(H1381, OD_H1381_SRDO_1_MAPPING); + for (int16_t i = 0; i < CO_GET_CNT(SRDO); i++) { + uint16_t CANdevRxIdx = CO_GET_CO(RX_IDX_SRDO) + 2 * i; + uint16_t CANdevTxIdx = CO_GET_CO(TX_IDX_SRDO) + 2 * i; + + err = CO_SRDO_init(&co->SRDO[i], + i, + co->SRDOGuard, + od, + em, + nodeId, + ((i == 0) ? CO_CAN_ID_SRDO_1 : 0), + SRDOcomm++, + SRDOmap++, + OD_GET(H13FE, OD_H13FE_SRDO_VALID), + OD_GET(H13FF, OD_H13FF_SRDO_CHECKSUM), + co->CANmodule, + co->CANmodule, + CANdevRxIdx, + CANdevRxIdx + 1, + co->CANmodule, + co->CANmodule, + CANdevTxIdx, + CANdevTxIdx + 1, + errInfo); + if (err) { return err; } + } + + CO_SRDO_init_end(co->SRDOGuard); + } +#endif + + return CO_ERROR_NO; +} +#endif + /******************************************************************************/ CO_NMT_reset_cmd_t CO_process(CO_t *co, bool_t enableGateway, @@ -1593,25 +1623,30 @@ void CO_process_TPDO(CO_t *co, /******************************************************************************/ #if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE -void CO_process_SRDO(CO_t *co, - uint32_t timeDifference_us, - uint32_t *timerNext_us) +CO_SRDO_state_t CO_process_SRDO(CO_t *co, + uint32_t timeDifference_us, + uint32_t *timerNext_us) { if (co->nodeIdUnconfigured) { - return; + return CO_SRDO_state_unknown; } - + bool_t NMTisOperational = CO_NMT_getInternalState(co->NMT) == CO_NMT_OPERATIONAL; - uint8_t firstOperational = CO_SRDOGuard_process(co->SRDOGuard, - NMTisOperational); + + CO_SRDO_state_t lowestState = CO_SRDO_state_deleted; for (int16_t i = 0; i < CO_GET_CNT(SRDO); i++) { - CO_SRDO_process(&co->SRDO[i], - firstOperational, - timeDifference_us, - timerNext_us); + CO_SRDO_state_t state = CO_SRDO_process(&co->SRDO[i], + timeDifference_us, + timerNext_us, + NMTisOperational); + if (state < lowestState) { + state = lowestState; + } } + + return lowestState; } #endif diff --git a/CANopen.h b/CANopen.h index a1f8f14..5930b79 100644 --- a/CANopen.h +++ b/CANopen.h @@ -261,8 +261,8 @@ typedef struct { uint8_t CNT_SRDO; OD_entry_t *ENTRY_H1301; /**< OD entry for @ref CO_SRDO_init() */ OD_entry_t *ENTRY_H1381; /**< OD entry for @ref CO_SRDO_init() */ - OD_entry_t *ENTRY_H13FE; /**< OD entry for @ref CO_SRDOGuard_init() */ - OD_entry_t *ENTRY_H13FF; /**< OD entry for @ref CO_SRDOGuard_init() */ + OD_entry_t *ENTRY_H13FE; /**< OD entry for @ref CO_SRDO_init() */ + OD_entry_t *ENTRY_H13FF; /**< OD entry for @ref CO_SRDO_init() */ /** Number of LSSslave objects, 0 or 1 (CANrx + CANtx). */ uint8_t CNT_LSS_SLV; /** Number of LSSmaster objects, 0 or 1 (CANrx + CANtx). */ @@ -393,8 +393,8 @@ typedef struct { #endif #endif #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN - /** SRDO object, initialised by @ref CO_SRDOGuard_init(), single SRDOGuard - * object is included inside all SRDO objects */ + /** SRDO guard object, initialised by @ref CO_SRDO_init_start(), single + * SRDOGuard object is included inside all SRDO objects */ CO_SRDOGuard_t *SRDOGuard; /** SRDO objects, initialised by @ref CO_SRDO_init() */ CO_SRDO_t *SRDO; @@ -577,6 +577,34 @@ CO_ReturnError_t CO_CANopenInitPDO(CO_t *co, uint32_t *errInfo); + + +/** + * Initialize Safety related Data Objects. + * + * Function must be called in the end of communication reset section after all + * CANopen and application initialization, otherwise some OD variables wont be + * mapped into SRDO correctly. + * + * @param co CANopen object. + * @param em Emergency object, which is used inside PDO objects for error + * reporting. + * @param od CANopen Object dictionary + * @param nodeId CANopen Node ID (1 ... 127) or 0xFF(unconfigured). If + * unconfigured, then PDO will not be initialized nor processed. + * @param [out] errInfo Additional information in case of error, may be NULL. + * + * @return CO_ERROR_NO in case of success. + */ +#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE) || ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) +CO_ReturnError_t CO_CANopenInitSRDO(CO_t *co, + CO_EM_t *em, + OD_t *od, + uint8_t nodeId, + uint32_t *errInfo); +#endif + + /** * Process CANopen objects. * @@ -682,10 +710,12 @@ void CO_process_TPDO(CO_t *co, * @param timeDifference_us Time difference from previous function call in * microseconds. * @param [out] timerNext_us info to OS - see CO_process(). + * + * @return @CO_SRDO_state_t lowest state of the SRDO objects. */ -void CO_process_SRDO(CO_t *co, - uint32_t timeDifference_us, - uint32_t *timerNext_us); +CO_SRDO_state_t CO_process_SRDO(CO_t *co, + uint32_t timeDifference_us, + uint32_t *timerNext_us); #endif /** @} */ /* CO_CANopen */ diff --git a/README.md b/README.md index 157bb2e..44a47bd 100644 --- a/README.md +++ b/README.md @@ -137,7 +137,7 @@ File structure - **crc16-ccitt.h/.c** - Calculation of CRC 16 CCITT polynomial. - **303/** - CANopen Recommendation - **CO_LEDs.h/.c** - CANopen LED Indicators - - **304/** - CANopen Safety (Implemented only in v1.3, not updated for the latest version). + - **304/** - CANopen Safety Related Data Object, as specified by EN 50325-5:2010 - **CO_SRDO.h/.c** - CANopen Safety-relevant Data Object protocol. - **CO_GFC.h/.c** - CANopen Global Failsafe Command (producer and consumer). - **305/** - CANopen layer setting services (LSS) and protocols.