nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

Updates and testing of OD interface, SDO server, SDO client, gateway, etc.

Browse source 
- SDO server: Change behavior for string data type + fixes + test all
- SDO client: fixes + test all
- OD interface: Special behavior for string data type
- CO_driver.h: added default debug configuration macros + fix spelling
- Gateway + fifo: added base64 support + updates
- CO_CANopenInit() - added arguments.
- update some documentation
- CO_epoll_interface fixes.
- CO_main_basic.c updates
This commit is contained in:
Janez 2020-12-24 14:15:10 +01:00
parent 09bf926856
commit 9e9fce74a9
22 changed files with 1434 additions and 739 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
301/CO_ODinterface.h
View file

@ -118,14 +118,16 @@ typedef enum {
ODA_RSRDO = 0x20, /**< Variable is mappable into receiving SRDO */
ODA_TRSRDO = 0x30, /**< Variable is mappable into tx or rx SRDO */
ODA_MB = 0x40, /**< Variable is multi-byte ((u)int16_t to (u)int64_t) */
ODA_RESERVED = 0x80, /**< Reserved for further use */
ODA_STR = 0x80 /**< Shorter value, than specified variable size, may be
written to the variable. SDO write will fill remaining memory with zeroes.
Attribute is used for VISIBLE_STRING and UNICODE_STRING. */
} OD_attributes_t;
/**
* Return codes from OD access functions.
*
* @ref OD_getSDOabCode() can be used to retrive corresponding SDO abort code.
* @ref OD_getSDOabCode() can be used to retrieve corresponding SDO abort code.
*/
typedef enum {
/* !!!! WARNING !!!!
@ -177,9 +179,9 @@ typedef enum {
ODR_GENERAL = 20,
/** SDO abort 0x08000020 - Data cannot be transferred or stored to app */
ODR_DATA_TRANSF = 21,
/** SDO abort 0x08000021 - Data can't be transf (local control) */
/** SDO abort 0x08000021 - Data can't be transferred (local control) */
ODR_DATA_LOC_CTRL = 22,
/** SDO abort 0x08000022 - Data can't be transf (present device state) */
/** SDO abort 0x08000022 - Data can't be transf. (present device state) */
ODR_DATA_DEV_STATE = 23,
/** SDO abort 0x08000023 - Object dictionary not present */
ODR_OD_MISSING = 23,

248
301/CO_SDOclient.c
View file

@ -61,20 +61,20 @@
* description of parameters see file CO_driver.h.
*/
static void CO_SDOclient_receive(void *object, void *msg) {
CO_SDOclient_t *SDO_C;
CO_SDOclient_t *SDO_C = (CO_SDOclient_t*)object;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
uint8_t *data = CO_CANrxMsg_readData(msg);
/* change void pointer to correct object type */
SDO_C = (CO_SDOclient_t*)object;
/* verify idle state, message length and message overflow (previous message
* was not processed yet) */
/* Ignore messages in idle state and messages with wrong length. Ignore
* message also if previous message was not processed yet and not abort */
if (SDO_C->state != CO_SDO_ST_IDLE && DLC == 8U
&& !CO_FLAG_READ(SDO_C->CANrxNew)
&& (!CO_FLAG_READ(SDO_C->CANrxNew) || data[0] == 0x80)
) {
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
if (SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
if (data[0] == 0x80 /* abort from server */
|| (SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ
&& SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP)
) {
#endif
/* copy data and set 'new message' flag */
memcpy((void *)&SDO_C->CANrxData[0], (const void *)&data[0], 8);
@ -89,21 +89,17 @@ static void CO_SDOclient_receive(void *object, void *msg) {
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
}
else {
else if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
/* block upload, copy data directly */
CO_SDO_state_t state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
uint8_t seqno;
seqno = data[0] & 0x7F;
uint8_t seqno = data[0] & 0x7F;
SDO_C->timeoutTimer = 0;
SDO_C->block_timeoutTimer = 0;
/* break sub-block if sequence number is too large */
if (seqno > SDO_C->block_blksize) {
state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
}
/* verify if sequence number is correct */
else if (seqno == (SDO_C->block_seqno + 1)) {
if (seqno <= SDO_C->block_blksize
&& seqno == (SDO_C->block_seqno + 1)
) {
SDO_C->block_seqno = seqno;
/* is this the last segment? */
@ -128,18 +124,33 @@ static void CO_SDOclient_receive(void *object, void *msg) {
}
}
}
else if (seqno == SDO_C->block_seqno || SDO_C->block_seqno == 0U) {
/* Ignore message, if it is duplicate or if sequence didn't
* start yet. */
}
else {
/* seqno is totally wrong, break sub-block. Data after last good
* seqno will be re-transmitted. */
/* If message is duplicate or sequence didn't start yet, ignore
* it. Otherwise seqno is wrong, so break sub-block. Data after
* last good seqno will be re-transmitted. */
else if (seqno != SDO_C->block_seqno && SDO_C->block_seqno != 0U) {
state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
#ifdef CO_DEBUG_SDO_CLIENT
char msg[80];
sprintf(msg,
"sub-block, rx WRONG: sequno=%02X, previous=%02X",
seqno, SDO_C->block_seqno);
CO_DEBUG_SDO_CLIENT(msg);
#endif
}
#ifdef CO_DEBUG_SDO_CLIENT
else {
char msg[80];
sprintf(msg,
"sub-block, rx ignored: sequno=%02X, expected=%02X",
seqno, SDO_C->block_seqno + 1);
CO_DEBUG_SDO_CLIENT(msg);
}
#endif
/* Is exit from sub-block receive state? */
if (state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
/* SDO_C->state has changed, processing will continue in another
* thread. Make memory barrier here with CO_FLAG_CLEAR() call.*/
/* Processing will continue in another thread, so make memory
* barrier here with CO_FLAG_CLEAR() call. */
CO_FLAG_CLEAR(SDO_C->CANrxNew);
SDO_C->state = state;
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE
@ -329,6 +340,19 @@ void CO_SDOclient_initCallbackPre(CO_SDOclient_t *SDOclient,
}
#endif
#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL) && defined CO_BIG_ENDIAN
static inline void reverseBytes(void *start, OD_size_t size) {
uint8_t *lo = (uint8_t *)start;
uint8_t *hi = (uint8_t *)start + size - 1;
uint8_t swap;
while (lo < hi) {
swap = *lo;
*lo++ = *hi;
*hi-- = swap;
}
}
#endif
/******************************************************************************/
CO_SDO_return_t CO_SDOclient_setup(CO_SDOclient_t *SDO_C,
@ -505,14 +529,9 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
else if (SDO_C->state == CO_SDO_ST_IDLE) {
ret = CO_SDO_RT_ok_communicationEnd;
}
else if (abort) {
abortCode =
(SDOabortCode != NULL) ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
SDO_C->state = CO_SDO_ST_ABORT;
}
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
/* Transfer data locally **************************************************/
else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER) {
else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER && !abort) {
/* search object dictionary in first pass */
if (SDO_C->OD_IO.write == NULL) {
ODR_t odRet;
@ -521,11 +540,17 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
odRet = OD_getSub(OD_find(SDO_C->OD, SDO_C->index), SDO_C->subIndex,
&subEntry, &SDO_C->OD_IO, false);
SDO_C->attribute = subEntry.attribute;
if (odRet != ODR_OK) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
ret = CO_SDO_RT_endedWithClientAbort;
}
else if ((subEntry.attribute & ODA_SDO_W) == 0) {
else if ((SDO_C->attribute & ODA_SDO_RW) == 0) {
abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
ret = CO_SDO_RT_endedWithClientAbort;
}
else if ((SDO_C->attribute & ODA_SDO_W) == 0) {
abortCode = CO_SDO_AB_READONLY;
ret = CO_SDO_RT_endedWithClientAbort;
}
@ -537,7 +562,7 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
/* write data, in several passes if necessary */
if (SDO_C->OD_IO.write != NULL) {
size_t count = CO_fifo_getOccupied(&SDO_C->bufFifo);
char buf[count];
char buf[count + 2];
CO_fifo_read(&SDO_C->bufFifo, buf, count, NULL);
SDO_C->sizeTran += count;
@ -553,9 +578,51 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
abortCode = CO_SDO_AB_DATA_LONG;
ret = CO_SDO_RT_endedWithClientAbort;
}
else {
ODR_t odRet;
/* Verify sizeTran is too small in last segment of data */
else if (!bufferPartial
&& SDO_C->sizeInd > 0 && SDO_C->sizeTran < SDO_C->sizeInd
) {
abortCode = CO_SDO_AB_DATA_SHORT;
ret = CO_SDO_RT_endedWithClientAbort;
}
/* is the last segment of data? */
else if (!bufferPartial) {
#ifdef CO_BIG_ENDIAN
/* swap int16_t .. uint64_t data if necessary */
if ((SDO_C->attribute & ODA_MB) != 0) {
reverseBytes(buf, count);
}
#endif
OD_size_t sizeInOd = SDO_C->OD_IO.stream.dataLength;
/* If dataType is string, then size of data downloaded may be
* shorter than size of OD data buffer. If so, add two zero
* bytes to terminate (unicode) string. Shorten also OD data
* size, (temporary, send info about EOF into OD_IO.write) */
if ((SDO_C->attribute & ODA_STR) != 0
&& (sizeInOd == 0 || SDO_C->sizeTran < sizeInOd)
) {
buf[count++] = 0;
SDO_C->sizeTran++;
if (sizeInOd == 0 || sizeInOd > SDO_C->sizeTran) {
buf[count++] = 0;
SDO_C->sizeTran++;
}
SDO_C->OD_IO.stream.dataLength = SDO_C->sizeTran;
}
/* Indicate OD data size, if necessary. Used for EOF check. */
else if (sizeInOd == 0) {
SDO_C->OD_IO.stream.dataLength = SDO_C->sizeTran;
}
/* Verify if size of data downloaded matches data size in OD. */
else if (SDO_C->sizeTran != sizeInOd) {
abortCode = (SDO_C->sizeTran > sizeInOd) ?
CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
ret = CO_SDO_RT_endedWithClientAbort;
}
}
if (abortCode == CO_SDO_AB_NONE) {
ODR_t odRet;
/* write data to Object Dictionary */
SDO_C->OD_IO.write(&SDO_C->OD_IO.stream, SDO_C->subIndex,
buf, count, &odRet);
@ -565,7 +632,7 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
ret = CO_SDO_RT_endedWithServerAbort;
}
/* error if OD variable was written completelly,
/* error if OD variable was written completely,
* but SDO download still has data */
else if (bufferPartial && odRet == ODR_OK) {
abortCode = CO_SDO_AB_DATA_LONG;
@ -579,13 +646,6 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
abortCode = CO_SDO_AB_DATA_SHORT;
ret = CO_SDO_RT_endedWithClientAbort;
}
/* Verify size transferred */
else if (SDO_C->sizeInd > 0
&& SDO_C->sizeTran < SDO_C->sizeInd
) {
abortCode = CO_SDO_AB_DATA_SHORT;
ret = CO_SDO_RT_endedWithClientAbort;
}
/* data transfer finished successfully */
else {
ret = CO_SDO_RT_ok_communicationEnd;
@ -600,8 +660,14 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
if (ret != CO_SDO_RT_waitingLocalTransfer) {
SDO_C->state = CO_SDO_ST_IDLE;
}
}
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
/* Inform OS to call this function again without delay. */
else if (timerNext_us != NULL) {
*timerNext_us = 0;
}
#endif
}
#endif /* CO_CONFIG_SDO_CLI_LOCAL */
/* CAN data received ******************************************************/
else if (CO_FLAG_READ(SDO_C->CANrxNew)) {
/* is SDO abort */
@ -612,6 +678,11 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
SDO_C->state = CO_SDO_ST_IDLE;
ret = CO_SDO_RT_endedWithServerAbort;
}
else if (abort) {
abortCode = (SDOabortCode != NULL)
? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
SDO_C->state = CO_SDO_ST_ABORT;
}
else switch (SDO_C->state) {
case CO_SDO_ST_DOWNLOAD_INITIATE_RSP: {
if (SDO_C->CANrxData[0] == 0x60) {
@ -771,6 +842,11 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
timeDifference_us = 0;
CO_FLAG_CLEAR(SDO_C->CANrxNew);
}
else if (abort) {
abortCode = (SDOabortCode != NULL)
? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
SDO_C->state = CO_SDO_ST_ABORT;
}
/* Timeout timers *********************************************************/
if (ret == CO_SDO_RT_waitingResponse) {
@ -918,9 +994,13 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
}
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ: {
if (CO_fifo_altGetOccupied(&SDO_C->bufFifo) < 7 && bufferPartial) {
/* wait until data are refilled */
break;
}
SDO_C->CANtxBuff->data[0] = ++SDO_C->block_seqno;
/* fill data bytes */
/* get up to 7 data bytes */
count = CO_fifo_altRead(&SDO_C->bufFifo,
(char *)&SDO_C->CANtxBuff->data[1], 7);
SDO_C->block_noData = 7 - count;
@ -935,7 +1015,7 @@ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
}
/* is end of transfer? Verify also sizeTran */
if (CO_fifo_altGetOccupied(&SDO_C->bufFifo) == 0 && bufferPartial) {
if (CO_fifo_altGetOccupied(&SDO_C->bufFifo) == 0 && !bufferPartial){
if (SDO_C->sizeInd > 0 && SDO_C->sizeTran < SDO_C->sizeInd) {
abortCode = CO_SDO_AB_DATA_SHORT;
SDO_C->state = CO_SDO_ST_ABORT;
@ -1038,7 +1118,7 @@ CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
SDO_C->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000;
SDO_C->timeoutTimer = 0;
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
SDO_C->block_SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 500;
SDO_C->block_SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 700;
#endif
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
@ -1071,6 +1151,7 @@ CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
/******************************************************************************/
CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
uint32_t timeDifference_us,
bool_t abort,
CO_SDO_abortCode_t *SDOabortCode,
size_t *sizeIndicated,
size_t *sizeTransferred,
@ -1090,7 +1171,7 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
}
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
/* Transfer data locally **************************************************/
else if (SDO_C->state == CO_SDO_ST_UPLOAD_LOCAL_TRANSFER) {
else if (SDO_C->state == CO_SDO_ST_UPLOAD_LOCAL_TRANSFER && !abort) {
/* search object dictionary in first pass */
if (SDO_C->OD_IO.read == NULL) {
ODR_t odRet;
@ -1099,11 +1180,17 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
odRet = OD_getSub(OD_find(SDO_C->OD, SDO_C->index), SDO_C->subIndex,
&subEntry, &SDO_C->OD_IO, false);
SDO_C->attribute = subEntry.attribute;
if (odRet != ODR_OK) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
ret = CO_SDO_RT_endedWithClientAbort;
}
else if ((subEntry.attribute & ODA_SDO_R) == 0) {
else if ((SDO_C->attribute & ODA_SDO_RW) == 0) {
abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
ret = CO_SDO_RT_endedWithClientAbort;
}
else if ((SDO_C->attribute & ODA_SDO_R) == 0) {
abortCode = CO_SDO_AB_WRITEONLY;
ret = CO_SDO_RT_endedWithClientAbort;
}
@ -1126,7 +1213,7 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
OD_size_t countData = SDO_C->OD_IO.stream.dataLength;
OD_size_t countBuf = (countData > 0 && countData <= countFifo)
? countData : countFifo;
char buf[countBuf];
char buf[countBuf + 1];
ODR_t odRet;
/* load data from OD variable into the buffer */
@ -1139,6 +1226,20 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
ret = CO_SDO_RT_endedWithServerAbort;
}
else {
/* if data is string, send only data up to null termination */
if (countRd > 0 && (SDO_C->attribute & ODA_STR) != 0) {
buf[countRd] = 0; /* (buf is one byte larger) */
OD_size_t countStr = strlen(buf);
if (countStr == 0) countStr = 1; /* ne zero length */
if (countStr < countRd) {
/* string terminator found, finish read, shorten data */
countRd = countStr;
odRet = ODR_OK;
SDO_C->OD_IO.stream.dataLength =
SDO_C->sizeTran + countRd;
}
}
CO_fifo_write(&SDO_C->bufFifo, buf, countRd, NULL);
SDO_C->sizeTran += countRd;
@ -1170,8 +1271,14 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
) {
SDO_C->state = CO_SDO_ST_IDLE;
}
}
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
/* Inform OS to call this function again without delay. */
else if (timerNext_us != NULL) {
*timerNext_us = 0;
}
#endif
}
#endif /* CO_CONFIG_SDO_CLI_LOCAL */
/* CAN data received ******************************************************/
else if (CO_FLAG_READ(SDO_C->CANrxNew)) {
/* is SDO abort */
@ -1182,6 +1289,11 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
SDO_C->state = CO_SDO_ST_IDLE;
ret = CO_SDO_RT_endedWithServerAbort;
}
else if (abort) {
abortCode = (SDOabortCode != NULL)
? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
SDO_C->state = CO_SDO_ST_ABORT;
}
else switch (SDO_C->state) {
case CO_SDO_ST_UPLOAD_INITIATE_RSP: {
if ((SDO_C->CANrxData[0] & 0xF0) == 0x40) {
@ -1431,6 +1543,11 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
timeDifference_us = 0;
CO_FLAG_CLEAR(SDO_C->CANrxNew);
}
else if (abort) {
abortCode = (SDOabortCode != NULL)
? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
SDO_C->state = CO_SDO_ST_ABORT;
}
/* Timeout timers *********************************************************/
if (ret == CO_SDO_RT_waitingResponse) {
@ -1573,6 +1690,10 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP: {
SDO_C->CANtxBuff->data[0] = 0xA2;
SDO_C->CANtxBuff->data[1] = SDO_C->block_seqno;
#ifdef CO_DEBUG_SDO_CLIENT
bool_t transferShort = SDO_C->block_seqno != SDO_C->block_blksize;
uint8_t seqnoStart = SDO_C->block_seqno;
#endif
/* Is last segment? */
if (SDO_C->finished) {
@ -1591,9 +1712,18 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
if (count >= 127) {
count = 127;
}
else if (CO_fifo_altGetOccupied(&SDO_C->bufFifo) > 0) {
else if (CO_fifo_getOccupied(&SDO_C->bufFifo) > 0) {
/* application must empty data buffer first */
ret = CO_SDO_RT_uploadDataBufferFull;
#ifdef CO_DEBUG_SDO_CLIENT
if (transferShort) {
char msg[80];
sprintf(msg,
"sub-block, uploadDataBufferFull: sequno=%02X",
seqnoStart);
CO_DEBUG_SDO_CLIENT(msg);
}
#endif
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
/* Inform OS to call this function again without delay. */
if (timerNext_us != NULL) {
@ -1612,10 +1742,18 @@ CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
SDO_C->CANtxBuff->data[2] = SDO_C->block_blksize;
/* reset timeout timer and send message */
SDO_C->timeoutTimer = 0;
/* reset block_timeoutTimer, but not SDO_C->timeoutTimer */
SDO_C->block_timeoutTimer = 0;
CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
#ifdef CO_DEBUG_SDO_CLIENT
if (transferShort && !SDO_C->finished) {
char msg[80];
sprintf(msg,
"sub-block restarted: sequnoPrev=%02X, blksize=%02X",
seqnoStart, SDO_C->block_blksize);
CO_DEBUG_SDO_CLIENT(msg);
}
#endif
break;
}

11
301/CO_SDOclient.h
View file

@ -37,7 +37,11 @@
#define CO_CONFIG_SDO_CLI (0)
#endif
#ifndef CO_CONFIG_SDO_CLI_BUFFER_SIZE
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 1000
#else
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
#endif
#endif
#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE) || defined CO_DOXYGEN
@ -68,6 +72,8 @@ typedef struct {
uint8_t nodeId;
/** Object dictionary interface for locally transferred object */
OD_IO_t OD_IO;
/** Attribute for locally transferred OD sub-object, see OD_attributes_t */
OD_attr_t attribute;
#endif
/** From CO_SDOclient_init() */
CO_CANmodule_t *CANdevRx;
@ -371,6 +377,8 @@ CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
* @param SDO_C This object.
* @param timeDifference_us Time difference from previous function call in
* [microseconds].
* @param abort If true, SDO client will send abort message from SDOabortCode
* and reception will be aborted.
* @param [out] SDOabortCode In case of error in communication, SDO abort code
* contains reason of error. Ignored if NULL.
* @param [out] sizeIndicated If larger than 0, then SDO server has indicated
@ -385,6 +393,7 @@ CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
*/
CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
uint32_t timeDifference_us,
bool_t abort,
CO_SDO_abortCode_t *SDOabortCode,
size_t *sizeIndicated,
size_t *sizeTransferred,

728
301/CO_SDOserver.c
View file

@ -30,8 +30,8 @@
/* verify configuration */
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED
#if CO_CONFIG_SDO_SRV_BUFFER_SIZE < 8
#error CO_CONFIG_SDO_SRV_BUFFER_SIZE must be greater or equal than 8.
#if CO_CONFIG_SDO_SRV_BUFFER_SIZE < 20
#error CO_CONFIG_SDO_SRV_BUFFER_SIZE must be greater or equal than 20.
#endif
#endif
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK
@ -41,8 +41,8 @@
#if !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE)
#error CO_CONFIG_CRC16_ENABLE must be enabled.
#endif
#if CO_CONFIG_SDO_SRV_BUFFER_SIZE < (127*7)
#error CO_CONFIG_SDO_SRV_BUFFER_SIZE must be greater or equal than (127*7).
#if CO_CONFIG_SDO_SRV_BUFFER_SIZE < 900
#error CO_CONFIG_SDO_SRV_BUFFER_SIZE must be greater or equal than 900.
#endif
#endif
@ -58,14 +58,15 @@ static void CO_SDO_receive(void *object, void *msg) {
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
uint8_t *data = CO_CANrxMsg_readData(msg);
/* verify message length and message queue overflow (if previous message
* was not processed yet) */
if (DLC == 8 && !CO_FLAG_READ(SDO->CANrxNew)) {
/* ignore messages with wrong length */
if (DLC == 8) {
if (data[0] == 0x80) {
/* abort from client, just make idle */
SDO->state = CO_SDO_ST_IDLE;
}
else if (CO_FLAG_READ(SDO->CANrxNew)) {
/* ignore message if previous message was not processed yet */
}
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK
else if (SDO->state == CO_SDO_ST_UPLOAD_BLK_END_CRSP && data[0]==0xA1) {
/* SDO block download successfully transferred, just make idle */
@ -73,21 +74,17 @@ static void CO_SDO_receive(void *object, void *msg) {
}
else if (SDO->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ) {
/* just in case, condition should always pass */
if (SDO->bufOffsetWr <= (CO_CONFIG_SDO_SRV_BUFFER_SIZE - 7)) {
if (SDO->bufOffsetWr <= (CO_CONFIG_SDO_SRV_BUFFER_SIZE - (7+2))) {
/* block download, copy data directly */
uint8_t seqno;
CO_SDO_state_t state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
seqno = data[0] & 0x7F;
uint8_t seqno = data[0] & 0x7F;
SDO->timeoutTimer = 0;
SDO->block_timeoutTimer = 0;
/* break sub-block if sequence number is too large */
if (seqno > SDO->block_blksize) {
state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
}
/* verify if sequence number is correct */
else if (seqno == (SDO->block_seqno + 1)) {
if (seqno <= SDO->block_blksize
&& seqno == (SDO->block_seqno + 1)
) {
SDO->block_seqno = seqno;
/* Copy data. There is always enough space in buffer,
@ -106,15 +103,28 @@ static void CO_SDO_receive(void *object, void *msg) {
state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
}
}
else if (seqno == SDO->block_seqno || SDO->block_seqno == 0U) {
/* Ignore message, if it is duplicate or if sequence didn't
* start yet. */
}
else {
/* seqno is totally wrong, break sub-block. Data after last
* good seqno will be re-transmitted. */
/* If message is duplicate or sequence didn't start yet, ignore
* it. Otherwise seqno is wrong, so break sub-block. Data after
* last good seqno will be re-transmitted. */
else if (seqno != SDO->block_seqno && SDO->block_seqno != 0U) {
state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
#ifdef CO_DEBUG_SDO_SERVER
char msg[80];
sprintf(msg,
"sub-block, rx WRONG: sequno=%02X, previous=%02X",
seqno, SDO->block_seqno);
CO_DEBUG_SDO_SERVER(msg);
#endif
}
#ifdef CO_DEBUG_SDO_SERVER
else {
char msg[80];
sprintf(msg,
"sub-block, rx ignored: sequno=%02X, expected=%02X",
seqno, SDO->block_seqno + 1);
CO_DEBUG_SDO_SERVER(msg);
}
#endif
if (state != CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ) {
/* SDO->state has changed, processing will continue in
@ -132,6 +142,9 @@ static void CO_SDO_receive(void *object, void *msg) {
}
}
}
else if (SDO->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP) {
/* ignore subsequent server messages, if response was requested */
}
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK */
else {
/* copy data and set 'new message' flag, data will be processed in
@ -360,6 +373,9 @@ CO_ReturnError_t CO_SDOserver_init(CO_SDOserver_t *SDO,
SDO->OD = OD;
SDO->nodeId = nodeId;
SDO->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000;
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK
SDO->block_SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 700;
#endif
SDO->state = CO_SDO_ST_IDLE;
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE
@ -473,10 +489,10 @@ void CO_SDOserver_initCallbackPre(CO_SDOserver_t *SDO,
#ifdef CO_BIG_ENDIAN
static inline void reverseBytes(void *start, int size) {
unsigned char *lo = start;
unsigned char *hi = start + size - 1;
unsigned char swap;
static inline void reverseBytes(void *start, OD_size_t size) {
uint8_t *lo = (uint8_t *)start;
uint8_t *hi = (uint8_t *)start + size - 1;
uint8_t swap;
while (lo < hi) {
swap = *lo;
*lo++ = *hi;
@ -486,6 +502,223 @@ static inline void reverseBytes(void *start, int size) {
#endif
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED
/** Helper function for writing data to Object dictionary. Function swaps data
* if necessary, calcualtes (and verifies CRC) writes data to OD and verifies
* data lengths.
*
* @param SDO SDO server
* @param [out] abortCode SDO abort code in case of error
* @param crcOperation 0=none, 1=calculate, 2=calculate and compare
* @parma crcClient crc checksum to campare with
*
* Returns true on success, otherwise write also abortCode and sets state to
* CO_SDO_ST_ABORT */
static bool_t validateAndWriteToOD(CO_SDOserver_t *SDO,
CO_SDO_abortCode_t *abortCode,
uint8_t crcOperation,
uint16_t crcClient)
{
OD_size_t bufOffsetWrOrig = SDO->bufOffsetWr;
if (SDO->finished) {
/* Verify if size of data downloaded matches size indicated. */
if (SDO->sizeInd > 0 && SDO->sizeTran != SDO->sizeInd) {
*abortCode = (SDO->sizeTran > SDO->sizeInd) ?
CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
#ifdef CO_BIG_ENDIAN
/* swap int16_t .. uint64_t data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
reverseBytes(SDO->buf, SDO->bufOffsetWr);
}
#endif
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
/* If dataType is string, then size of data downloaded may be
* shorter than size of OD data buffer. If so, add two zero bytes
* to terminate (unicode) string. Shorten also OD data size,
* (temporary, send information about EOF into OD_IO.write) */
if ((SDO->attribute & ODA_STR) != 0
&& (sizeInOd == 0 || SDO->sizeTran < sizeInOd)
&& (SDO->bufOffsetWr + 2) <= CO_CONFIG_SDO_SRV_BUFFER_SIZE
) {
SDO->buf[SDO->bufOffsetWr++] = 0;
SDO->sizeTran++;
if (sizeInOd == 0 || SDO->sizeTran < sizeInOd) {
SDO->buf[SDO->bufOffsetWr++] = 0;
SDO->sizeTran++;
}
SDO->OD_IO.stream.dataLength = SDO->sizeTran;
}
/* Indicate OD data size, if not indicated. Can be used for EOF check.*/
else if (sizeInOd == 0) {
SDO->OD_IO.stream.dataLength = SDO->sizeTran;
}
/* Verify if size of data downloaded matches data size in OD. */
else if (SDO->sizeTran != sizeInOd) {
*abortCode = (SDO->sizeTran > sizeInOd) ?
CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
}
else {
/* Verify if size of data downloaded is not too large. */
if (SDO->sizeInd > 0 && SDO->sizeTran > SDO->sizeInd) {
*abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
}
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK
/* calculate crc on current data */
if (SDO->block_crcEnabled && crcOperation > 0) {
SDO->block_crc = crc16_ccitt((unsigned char *)SDO->buf,
bufOffsetWrOrig,
SDO->block_crc);
if (crcOperation == 2 && crcClient != SDO->block_crc) {
*abortCode = CO_SDO_AB_CRC;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
}
#endif
/* may be unused */
(void) crcOperation; (void) crcClient; (void) bufOffsetWrOrig;
/* write data */
ODR_t odRet;
SDO->OD_IO.write(&SDO->OD_IO.stream, SDO->subIndex,
SDO->buf, SDO->bufOffsetWr, &odRet);
SDO->bufOffsetWr = 0;
/* verify write error value */
if (odRet != ODR_OK && odRet != ODR_PARTIAL) {
*abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
return false;
}
else if (SDO->finished && odRet == ODR_PARTIAL) {
/* OD variable was not written completely, but SDO download finished */
*abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
else if (!SDO->finished && odRet == ODR_OK) {
/* OD variable was written completely, but SDO download still has data*/
*abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
return true;
}
/** Helper function for reading data from Object dictionary. Function also swaps
* data if necessary and calcualtes CRC.
*
* @param SDO SDO server
* @param [out] abortCode SDO abort code in case of error
* @parma countMinimum if data size in buffer is less than countMinimum, then
* buffer is refilled from OD variable
* @param calculateCrc if true, crc is calculated
*
* Returns true on success, otherwise write also abortCode and sets state to
* CO_SDO_ST_ABORT */
static bool_t readFromOd(CO_SDOserver_t *SDO,
CO_SDO_abortCode_t *abortCode,
OD_size_t countMinimum,
bool_t calculateCrc)
{
OD_size_t countRemain = SDO->bufOffsetWr - SDO->bufOffsetRd;
if (!SDO->finished && countRemain < countMinimum) {
/* first move remaining data to the start of the buffer */
memmove(SDO->buf, SDO->buf + SDO->bufOffsetRd, countRemain);
SDO->bufOffsetRd = 0;
SDO->bufOffsetWr = countRemain;
/* Get size of free data buffer */
OD_size_t countRdRequest = CO_CONFIG_SDO_SRV_BUFFER_SIZE - countRemain;
/* load data from OD variable into the buffer */
ODR_t odRet;
char *bufShifted = SDO->buf + countRemain;
OD_size_t countRd = SDO->OD_IO.read(&SDO->OD_IO.stream, SDO->subIndex,
bufShifted,
countRdRequest,
&odRet);
if (odRet != ODR_OK && odRet != ODR_PARTIAL) {
*abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
return false;
}
/* if data is string, send only data up to null termination */
if (countRd > 0 && (SDO->attribute & ODA_STR) != 0) {
bufShifted[countRd] = 0; /* (SDO->buf is one byte larger) */
OD_size_t countStr = strlen(bufShifted);
if (countStr == 0) countStr = 1; /* zero length is not allowed */
if (countStr < countRd) {
/* string terminator found, read is finished, shorten data */
countRd = countStr;
odRet = ODR_OK;
SDO->OD_IO.stream.dataLength = SDO->sizeTran + countRd;
}
}
/* partial or finished read */
SDO->bufOffsetWr = countRemain + countRd;
if (SDO->bufOffsetWr == 0 || odRet == ODR_PARTIAL) {
SDO->finished = false;
if (SDO->bufOffsetWr < countMinimum) {
*abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
}
else {
SDO->finished = true;
}
#ifdef CO_BIG_ENDIAN
/* swap data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
if (SDO->finished) {
/* int16_t .. uint64_t */
reverseBytes(bufShifted, countRd);
}
else {
abortCode = CO_SDO_AB_PRAM_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
return false;
}
}
#endif
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK
/* update the crc */
if (calculateCrc && SDO->block_crcEnabled) {
SDO->block_crc = crc16_ccitt((uint8_t *)bufShifted,
countRd,
SDO->block_crc);
}
#endif
}
return true;
}
#endif
/******************************************************************************/
CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
bool_t NMTisPreOrOperational,
@ -557,7 +790,11 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->attribute = subEntry.attribute;
/* verify read/write attributes */
if (upload && (SDO->attribute & ODA_SDO_R) == 0) {
if ((SDO->attribute & ODA_SDO_RW) == 0) {
abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
SDO->state = CO_SDO_ST_ABORT;
}
else if (upload && (SDO->attribute & ODA_SDO_R) == 0) {
abortCode = CO_SDO_AB_WRITEONLY;
SDO->state = CO_SDO_ST_ABORT;
}
@ -571,53 +808,30 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED
/* load data from object dictionary, if upload and no error */
if (upload && abortCode == CO_SDO_AB_NONE) {
ODR_t odRet;
SDO->bufOffsetRd = SDO->bufOffsetWr = 0;
SDO->sizeTran = 0;
SDO->finished = false;
/* load data from OD variable into the buffer */
SDO->bufOffsetWr = SDO->OD_IO.read(&SDO->OD_IO.stream,
SDO->subIndex,
SDO->buf,
CO_CONFIG_SDO_SRV_BUFFER_SIZE,
&odRet);
SDO->bufOffsetRd = 0;
if (readFromOd(SDO, &abortCode, 7, false)) {
/* Size of variable in OD (may not be known yet) */
if (SDO->finished) {
/* OD variable was completely read, its size is known */
if (odRet != ODR_OK && odRet != ODR_PARTIAL) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
}
else if (odRet == ODR_PARTIAL && SDO->bufOffsetWr < 7) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
}
else {
SDO->finished = (odRet != ODR_PARTIAL);
#ifdef CO_BIG_ENDIAN
/* swap data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
if (!SDO->finished) {
SDO->sizeInd = SDO->OD_IO.stream.dataLength;
if (SDO->sizeInd == 0) {
SDO->sizeInd = SDO->bufOffsetWr;
}
else if (SDO->sizeInd != SDO->bufOffsetWr) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
}
else {
reverseBytes(SDO->buf, SDO->bufOffsetWr);
}
}
#endif
}
}
/* get and verify OD data size, if upload and still no error */
if (upload && abortCode == CO_SDO_AB_NONE) {
/* Size of variable in OD (may not be known yet) */
SDO->sizeInd = SDO->OD_IO.stream.dataLength;
SDO->sizeTran = 0;
if (SDO->finished) {
/* OD variable was completely read, so its size is known */
if (SDO->sizeInd == 0) {
SDO->sizeInd = SDO->bufOffsetWr;
}
else if (SDO->sizeInd != SDO->bufOffsetWr) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
else {
/* If data type is string, size is not known */
SDO->sizeInd = (SDO->attribute & ODA_STR) == 0
? SDO->OD_IO.stream.dataLength
: 0;
}
}
}
@ -629,40 +843,52 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
case CO_SDO_ST_DOWNLOAD_INITIATE_REQ: {
if (SDO->CANrxData[0] & 0x02) {
/* Expedited transfer, max 4 bytes of data */
OD_size_t sizeIndicated = 4;
/* Size of OD variable (>0 if indicated) */
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
ODR_t odRet;
/* is size indicated? */
if (SDO->CANrxData[0] & 0x01) {
sizeIndicated -= (SDO->CANrxData[0] >> 2) & 0x03;
if (sizeInOd > 0 && sizeIndicated != sizeInOd) {
abortCode = (sizeIndicated < sizeInOd) ?
CO_SDO_AB_DATA_SHORT :
CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
else {
if (sizeInOd > 4) {
abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
else {
sizeIndicated = sizeInOd;
}
}
/* Get SDO data size (indicated by SDO client or get from OD) */
OD_size_t dataSizeToWrite = 4;
if (SDO->CANrxData[0] & 0x01)
dataSizeToWrite -= (SDO->CANrxData[0] >> 2) & 0x03;
else if (sizeInOd > 0 && sizeInOd < 4)
dataSizeToWrite = sizeInOd;
/* swap data if necessary, then copy data */
/* copy data to the temp buffer, swap data if necessary */
uint8_t buf[6] = {0};
memcpy(buf, &SDO->CANrxData[4], dataSizeToWrite);
#ifdef CO_BIG_ENDIAN
if ((SDO->attribute & ODA_MB) != 0) {
reverseBytes(&SDO->CANrxData[4], sizeIndicated);
reverseBytes(buf, dataSizeToWrite);
}
#endif
/* If dataType is string, then size of data downloaded may be
* shorter as size of OD data buffer. If so, add two zero bytes
* to terminate (unicode) string. Shorten also OD data size,
* (temporary, send information about EOF into OD_IO.write) */
if ((SDO->attribute & ODA_STR) != 0
&& (sizeInOd == 0 || dataSizeToWrite < sizeInOd)
) {
OD_size_t delta = sizeInOd - dataSizeToWrite;
dataSizeToWrite += delta == 1 ? 1 : 2;
SDO->OD_IO.stream.dataLength = dataSizeToWrite;
}
else if (sizeInOd == 0) {
SDO->OD_IO.stream.dataLength = dataSizeToWrite;
}
/* Verify if size of data downloaded matches data size in OD. */
else if (dataSizeToWrite != sizeInOd) {
abortCode = (dataSizeToWrite > sizeInOd) ?
CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
/* Copy data */
ODR_t odRet;
SDO->OD_IO.write(&SDO->OD_IO.stream, SDO->subIndex,
&SDO->CANrxData[4], sizeIndicated, &odRet);
buf, dataSizeToWrite, &odRet);
if (odRet != ODR_OK) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
@ -686,13 +912,21 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->sizeInd = CO_SWAP_32(size);
/* Indicated size of SDO matches sizeof OD variable? */
if (sizeInOd > 0 && SDO->sizeInd != sizeInOd) {
abortCode = (SDO->sizeInd < sizeInOd) ?
CO_SDO_AB_DATA_SHORT :
CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
}
if (sizeInOd > 0) {
if (SDO->sizeInd > sizeInOd) {
abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
}
/* strings are allowed to be shorter */
else if (SDO->sizeInd < sizeInOd
&& (SDO->attribute & ODA_STR) == 0
) {
abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
}
else {
SDO->sizeInd = 0;
@ -710,9 +944,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED
case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ: {
if ((SDO->CANrxData[0] & 0xE0) == 0x00) {
OD_size_t count;
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
bool_t lastSegment = (SDO->CANrxData[0] & 0x01) != 0;
SDO->finished = (SDO->CANrxData[0] & 0x01) != 0;
/* verify and alternate toggle bit */
uint8_t toggle = SDO->CANrxData[0] & 0x10;
@ -721,17 +953,16 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->state = CO_SDO_ST_ABORT;
break;
}
SDO->toggle = (toggle == 0x00) ? 0x10 : 0x00;
/* get data size and write data to the buffer */
count = 7 - ((SDO->CANrxData[0] >> 1) & 0x07);
OD_size_t count = 7 - ((SDO->CANrxData[0] >> 1) & 0x07);
memcpy(SDO->buf + SDO->bufOffsetWr, &SDO->CANrxData[1], count);
SDO->bufOffsetWr += count;
SDO->sizeTran += count;
/* verify if size of data downloaded is too large */
if ((SDO->sizeInd > 0 && SDO->sizeTran > SDO->sizeInd)
|| (sizeInOd > 0 && SDO->sizeTran > sizeInOd)
/* if data size exceeds variable size, abort */
if (SDO->OD_IO.stream.dataLength > 0
&& SDO->sizeTran > SDO->OD_IO.stream.dataLength
) {
abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
@ -739,54 +970,13 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
}
/* if necessary, empty the buffer */
if (lastSegment
|| (CO_CONFIG_SDO_SRV_BUFFER_SIZE - SDO->bufOffsetWr) < 7
if (SDO->finished
|| (CO_CONFIG_SDO_SRV_BUFFER_SIZE - SDO->bufOffsetWr)<(7+2)
) {
ODR_t odRet;
#ifdef CO_BIG_ENDIAN
/* swap data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
reverseBytes(SDO->buf, SDO->bufOffsetWr);
}
#endif
/* write data */
SDO->OD_IO.write(&SDO->OD_IO.stream, SDO->subIndex,
SDO->buf, SDO->bufOffsetWr, &odRet);
SDO->bufOffsetWr = 0;
if (odRet != ODR_OK && odRet != ODR_PARTIAL) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
if (!validateAndWriteToOD(SDO, &abortCode, 0, 0))
break;
}
else if (lastSegment && odRet == ODR_PARTIAL) {
/* OD variable was not written completelly, but SDO
* download finished */
abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
else if (!lastSegment && odRet == ODR_OK) {
/* OD variable was written completelly, but SDO download
* still has data */
abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
/* If no more segments to be downloaded, finish */
if (lastSegment) {
/* verify size of data */
if (SDO->sizeInd > 0 && SDO->sizeTran < SDO->sizeInd) {
abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
else {
SDO->finished = true;
}
}
SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENT_RSP;
}
else {
@ -835,12 +1025,20 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->sizeInd = CO_SWAP_32(size);
/* Indicated size of SDO matches sizeof OD variable? */
if (sizeInOd > 0 && SDO->sizeInd != sizeInOd) {
abortCode = (SDO->sizeInd < sizeInOd) ?
CO_SDO_AB_DATA_SHORT :
CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
if (sizeInOd > 0) {
if (SDO->sizeInd > sizeInOd) {
abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
}
/* strings are allowed to be shorter */
else if (SDO->sizeInd < sizeInOd
&& (SDO->attribute & ODA_STR) == 0
) {
abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
}
else {
@ -858,8 +1056,6 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
case CO_SDO_ST_DOWNLOAD_BLK_END_REQ: {
if ((SDO->CANrxData[0] & 0xE3) == 0xC1) {
ODR_t odRet;
/* Get number of data bytes in last segment, that do not
* contain data. Then reduce buffer. */
uint8_t noData = ((SDO->CANrxData[0] >> 2) & 0x07);
@ -872,53 +1068,14 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->sizeTran -= noData;
SDO->bufOffsetWr -= noData;
/* verify length */
if (SDO->sizeInd > 0 && SDO->sizeTran != SDO->sizeInd) {
abortCode = (SDO->sizeTran > SDO->sizeInd) ?
CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
#ifdef CO_BIG_ENDIAN
/* swap data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
reverseBytes(SDO->buf, SDO->bufOffsetWr);
}
#endif
/* calculeate and verify CRC */
uint16_t crcClient = 0;
if (SDO->block_crcEnabled) {
uint16_t crcClient;
SDO->block_crc = crc16_ccitt((unsigned char *)SDO->buf,
SDO->bufOffsetWr,
SDO->block_crc);
crcClient = ((uint16_t) SDO->CANrxData[2]) << 8;
crcClient |= SDO->CANrxData[1];
if (crcClient != SDO->block_crc) {
abortCode = CO_SDO_AB_CRC;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
/* write the data */
SDO->OD_IO.write(&SDO->OD_IO.stream, SDO->subIndex,
SDO->buf, SDO->bufOffsetWr, &odRet);
SDO->bufOffsetWr = 0;
if (odRet == ODR_PARTIAL) {
/* OD variable was not written completelly, but SDO
* download finished */
abortCode = CO_SDO_AB_DATA_SHORT;
SDO->state = CO_SDO_ST_ABORT;
if (!validateAndWriteToOD(SDO, &abortCode, 2, crcClient))
break;
}
else if (odRet != ODR_OK) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
break;
}
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_END_RSP;
}
@ -980,8 +1137,6 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP: {
if (SDO->CANrxData[0] == 0xA2) {
OD_size_t countRemain, countMinimum;
SDO->block_blksize = SDO->CANrxData[2];
if (SDO->block_blksize < 1 || SDO->block_blksize > 127) {
SDO->block_blksize = 127;
@ -1004,52 +1159,11 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
}
/* refill data buffer if necessary */
countRemain = SDO->bufOffsetWr - SDO->bufOffsetRd;
countMinimum = SDO->block_blksize * 7;
if (!SDO->finished && countRemain < countMinimum) {
ODR_t odRet;
OD_size_t countRd;
if (!readFromOd(SDO, &abortCode, SDO->block_blksize * 7, true))
break;
/* first move remaining data to the start of the buffer */
memmove(SDO->buf, SDO->buf + SDO->bufOffsetRd, countRemain);
SDO->bufOffsetRd = 0;
SDO->bufOffsetWr = countRemain;
/* load data from OD variable into the buffer */
countRd = SDO->OD_IO.read(&SDO->OD_IO.stream, SDO->subIndex,
SDO->buf + SDO->bufOffsetWr,
CO_CONFIG_SDO_SRV_BUFFER_SIZE - SDO->bufOffsetWr,
&odRet);
countRemain += countRd;
if (odRet != ODR_OK && odRet != ODR_PARTIAL) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
break;
}
else if (odRet == ODR_PARTIAL) {
SDO->finished = false;
if (countRemain < countMinimum) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
else {
SDO->finished = true;
}
/* update the crc */
if (SDO->block_crcEnabled) {
SDO->block_crc = crc16_ccitt(
(unsigned char *)SDO->buf + SDO->bufOffsetWr,
countRd,
SDO->block_crc);
}
SDO->bufOffsetWr = countRemain;
}
if (countRemain == 0) {
if (SDO->bufOffsetWr == SDO->bufOffsetRd) {
SDO->state = CO_SDO_ST_UPLOAD_BLK_END_SREQ;
}
else {
@ -1164,6 +1278,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED
case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP: {
SDO->CANtxBuff->data[0] = 0x20 | SDO->toggle;
SDO->toggle = (SDO->toggle == 0x00) ? 0x10 : 0x00;
/* reset timeout timer and send message */
SDO->timeoutTimer = 0;
@ -1200,6 +1315,9 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
memcpy(&SDO->CANtxBuff->data[4],
&sizeIndSw, sizeof(sizeIndSw));
}
else {
SDO->CANtxBuff->data[0] = 0x40;
}
SDO->toggle = 0x00;
SDO->timeoutTimer = 0;
SDO->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
@ -1210,6 +1328,12 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
OD_size_t count = SDO->OD_IO.read(&SDO->OD_IO.stream, SDO->subIndex,
&SDO->CANtxBuff->data[4], 4,
&odRet);
/* strings are allowed to be shorter */
if (odRet == ODR_PARTIAL && (SDO->attribute & ODA_STR) != 0) {
odRet = ODR_OK;
}
if (odRet != ODR_OK || count == 0) {
abortCode = (odRet == ODR_OK) ? CO_SDO_AB_DEVICE_INCOMPAT :
(CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
@ -1220,7 +1344,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
#ifdef CO_BIG_ENDIAN
/* swap data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
reverseBytes(&SDO->CANtxBuff->data[4], count);
reverseBytes(buf, dataSizeToWrite);
}
#endif
SDO->CANtxBuff->data[0] = 0x43 | ((4 - count) << 2);
@ -1239,39 +1363,8 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED
case CO_SDO_ST_UPLOAD_SEGMENT_RSP: {
/* refill the data buffer if necessary */
if (!SDO->finished && (SDO->bufOffsetWr - SDO->bufOffsetRd) < 7) {
/* first move remaining data to the start of the buffer */
SDO->bufOffsetWr -= SDO->bufOffsetRd;
memmove(SDO->buf, SDO->buf + SDO->bufOffsetRd,
SDO->bufOffsetWr);
SDO->bufOffsetRd = 0;
/* load data from OD variable into the buffer */
ODR_t odRet;
OD_size_t countRd = SDO->OD_IO.read(&SDO->OD_IO.stream,
SDO->subIndex,
SDO->buf + SDO->bufOffsetWr,
CO_CONFIG_SDO_SRV_BUFFER_SIZE - SDO->bufOffsetWr,
&odRet);
SDO->bufOffsetWr += countRd;
if (odRet != ODR_OK && odRet != ODR_PARTIAL) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
break;
}
else if (odRet == ODR_PARTIAL) {
SDO->finished = false;
if (SDO->bufOffsetWr < 7) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
SDO->state = CO_SDO_ST_ABORT;
break;
}
}
else {
SDO->finished = true;
}
}
if (!readFromOd(SDO, &abortCode, 7, false))
break;
/* SDO command specifier with toggle bit */
SDO->CANtxBuff->data[0] = SDO->toggle;
@ -1279,7 +1372,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
OD_size_t count = SDO->bufOffsetWr - SDO->bufOffsetRd;
/* verify, if this is the last segment */
if (count <= 7) {
if (count < 7 || (SDO->finished && count == 7)) {
/* indicate last segment and nnn */
SDO->CANtxBuff->data[0] |= ((7 - count) << 1) | 0x01;
SDO->state = CO_SDO_ST_IDLE;
@ -1308,6 +1401,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
&& SDO->sizeTran < SDO->sizeInd
) {
abortCode = CO_SDO_AB_DATA_SHORT;
ret = CO_SDO_RT_waitingResponse;
SDO->state = CO_SDO_ST_ABORT;
break;
}
@ -1327,7 +1421,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->CANtxBuff->data[3] = SDO->subIndex;
/* calculate number of block segments from free buffer space */
OD_size_t count = CO_CONFIG_SDO_SRV_BUFFER_SIZE / 7;
OD_size_t count = (CO_CONFIG_SDO_SRV_BUFFER_SIZE-2) / 7;
if (count > 127) {
count = 127;
}
@ -1355,59 +1449,31 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP: {
SDO->CANtxBuff->data[0] = 0xA2;
SDO->CANtxBuff->data[1] = SDO->block_seqno;
#ifdef CO_DEBUG_SDO_SERVER
bool_t transferShort = SDO->block_seqno != SDO->block_blksize;
uint8_t seqnoStart = SDO->block_seqno;
#endif
/* Is last segment? */
if (SDO->finished) {
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_END_REQ;
}
else {
/* verify if size of data downloaded is too large */
if (SDO->sizeInd > 0 && SDO->sizeTran > SDO->sizeInd) {
abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
break;
}
/* calculate number of block segments from free buffer space */
OD_size_t count;
count = (CO_CONFIG_SDO_SRV_BUFFER_SIZE - SDO->bufOffsetWr) / 7;
count = (CO_CONFIG_SDO_SRV_BUFFER_SIZE-2-SDO->bufOffsetWr)/7;
if (count >= 127) {
count = 127;
}
else if (SDO->bufOffsetWr > 0) {
/* it is necessary to empty the buffer */
#ifdef CO_BIG_ENDIAN
/* swap data if necessary */
if ((SDO->attribute & ODA_MB) != 0) {
reverseBytes(SDO->buf, SDO->bufOffsetWr);
}
#endif
/* calculate crc on current data */
SDO->block_crc = crc16_ccitt((unsigned char *)SDO->buf,
SDO->bufOffsetWr,
SDO->block_crc);
/* write data */
ODR_t odRet;
SDO->OD_IO.write(&SDO->OD_IO.stream, SDO->subIndex,
SDO->buf, SDO->bufOffsetWr, &odRet);
SDO->bufOffsetWr = 0;
if (odRet == ODR_OK) {
/* OD variable was written completelly, but SDO download
* still has data */
abortCode = CO_SDO_AB_DATA_LONG;
SDO->state = CO_SDO_ST_ABORT;
if (!validateAndWriteToOD(SDO, &abortCode, 1, 0))
break;
}
else if (odRet != ODR_PARTIAL) {
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
SDO->state = CO_SDO_ST_ABORT;
break;
}
count = CO_CONFIG_SDO_SRV_BUFFER_SIZE / 7;
if (count >= 127) count = 127;
count =(CO_CONFIG_SDO_SRV_BUFFER_SIZE-2-SDO->bufOffsetWr)/7;
if (count >= 127) {
count = 127;
}
}
SDO->block_blksize = (uint8_t)count;
@ -1420,10 +1486,18 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->CANtxBuff->data[2] = SDO->block_blksize;
/* reset timeout timer and send message */
SDO->timeoutTimer = 0;
/* reset block_timeoutTimer, but not SDO->timeoutTimer */
SDO->block_timeoutTimer = 0;
CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
#ifdef CO_DEBUG_SDO_SERVER
if (transferShort && !SDO->finished) {
char msg[80];
sprintf(msg,
"sub-block restarted: sequnoPrev=%02X, blksize=%02X",
seqnoStart, SDO->block_blksize);
CO_DEBUG_SDO_SERVER(msg);
}
#endif
break;
}
@ -1461,7 +1535,7 @@ CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
SDO->CANtxBuff->data[0] = ++SDO->block_seqno;
OD_size_t count = SDO->bufOffsetWr - SDO->bufOffsetRd;
/* verify, if this is the last segment */
if (count <= 7) {
if (count < 7 || (SDO->finished && count == 7)) {
SDO->CANtxBuff->data[0] |= 0x80;
}
else {

4
301/CO_SDOserver.h
View file

@ -492,8 +492,8 @@ typedef struct {
uint32_t SDOtimeoutTime_us;
/** Timeout timer for SDO communication */
uint32_t timeoutTimer;
/** Interim data buffer for segmented or block transfer */
char buf[CO_CONFIG_SDO_SRV_BUFFER_SIZE];
/** Interim data buffer for segmented or block transfer + byte for '\0' */
char buf[CO_CONFIG_SDO_SRV_BUFFER_SIZE + 1];
/** Offset of next free data byte available for write in the buffer. */
OD_size_t bufOffsetWr;
/** Offset of first data available for read in the buffer */

33
301/CO_config.h
View file

@ -374,13 +374,13 @@ extern "C" {
/**
* Size of the internal data buffer for the SDO client.
*
* Circular buffer is used for SDO communication. it can be read or written
* Circular buffer is used for SDO communication. It can be read or written
* between successive SDO calls. So size of the buffer can be lower than size of
* the actual size of data transferred. If only segmented transfer is used, then
* buffer size can be as low as 7 bytes, if data are read/written each cycle. If
* block transfer is used, buffer size should be set to at least 889 bytes, so
* maximum blksize can be used. In that case data should be read/written proper
* time between cycles.
* block transfer is used, buffer size should be set to at least 1000 bytes, so
* maximum blksize can be used (blksize is calculated from free buffer space).
* Default value for block transfer is 1000, otherwise 32.
*/
#ifdef CO_DOXYGEN
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
@ -711,6 +711,31 @@ extern "C" {
#define CO_CONFIG_TRACE_OWN_INTTYPES 0x02
/** @} */ /* CO_STACK_CONFIG_TRACE */
/**
* @defgroup CO_STACK_CONFIG_DEBUG Debug messages
* Messages from different parts of the stack.
* @{
*/
/**
* Configuration of debug messages from different parts of the stack, which can
* be logged according to target specific function.
*
* Possible flags, can be ORed:
* - CO_CONFIG_DEBUG_COMMON - Define default CO_DEBUG_COMMON(msg) macro. This
* macro is target specific. This macro is then used as default macro in all
* other defined CO_DEBUG_XXX(msg) macros.
* - CO_CONFIG_DEBUG_SDO_CLIENT - Define default CO_DEBUG_SDO_CLIENT(msg) macro.
* - CO_CONFIG_DEBUG_SDO_SERVER - Define default CO_DEBUG_SDO_SERVER(msg) macro.
*/
#ifdef CO_DOXYGEN
#define CO_CONFIG_DEBUG (0)
#endif
#define CO_CONFIG_DEBUG_COMMON 0x01
#define CO_CONFIG_DEBUG_SDO_CLIENT 0x02
#define CO_CONFIG_DEBUG_SDO_SERVER 0x04
/** @} */ /* CO_STACK_CONFIG_DEBUG */
/** @} */ /* CO_STACK_CONFIG */
#ifdef __cplusplus

21
301/CO_driver.h
View file

@ -35,6 +35,15 @@
extern "C" {
#endif
#ifdef CO_DEBUG_COMMON
#if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_SDO_CLIENT
#define CO_DEBUG_SDO_CLIENT(msg) CO_DEBUG_COMMON(msg)
#endif
#if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_SDO_SERVER
#define CO_DEBUG_SDO_SERVER(msg) CO_DEBUG_COMMON(msg)
#endif
#endif
/**
* @defgroup CO_driver Driver
* @ingroup CO_CANopen_301
@ -199,7 +208,7 @@ void CANrx_callback(void *object, void *rxMsg);
*
* This is target specific function and is specific for specific
* microcontroller. It is best to implement it by using inline function or
* macro. `rxMsg` parameter should cast to a ponter to structure. For best
* macro. `rxMsg` parameter should cast to a pointer to structure. For best
* efficiency structure may have the same alignment as CAN registers inside CAN
* module.
*
@ -422,7 +431,7 @@ typedef struct {
*
* Default CANopen identifiers for CANopen communication objects. Same as
* 11-bit addresses of CAN messages. These are default identifiers and
* can be changed in CANopen. Especially PDO identifiers are confgured
* can be changed in CANopen. Especially PDO identifiers are configured
* in PDO linking phase of the CANopen network configuration.
*/
typedef enum {
@ -491,7 +500,7 @@ typedef enum {
CO_ERROR_TX_OVERFLOW = -9, /**< Previous message is still waiting,
buffer full */
CO_ERROR_TX_PDO_WINDOW = -10, /**< Synchronous TPDO is outside window */
CO_ERROR_TX_UNCONFIGURED = -11, /**< Transmit buffer was not confugured
CO_ERROR_TX_UNCONFIGURED = -11, /**< Transmit buffer was not configured
properly */
CO_ERROR_OD_PARAMETERS = -12, /**< Error in Object Dictionary parameters*/
CO_ERROR_DATA_CORRUPT = -13, /**< Stored data are corrupt */
@ -509,7 +518,7 @@ typedef enum {
/**
* Request CAN configuration (stopped) mode and *wait* untill it is set.
* Request CAN configuration (stopped) mode and *wait* until it is set.
*
* @param CANptr Pointer to CAN device
*/
@ -517,7 +526,7 @@ void CO_CANsetConfigurationMode(void *CANptr);
/**
* Request CAN normal (opearational) mode and *wait* untill it is set.
* Request CAN normal (operational) mode and *wait* until it is set.
*
* @param CANmodule CO_CANmodule_t object.
*/
@ -607,7 +616,7 @@ CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule,
* @param rtr If true, 'Remote Transmit Request' messages will be transmitted.
* @param noOfBytes Length of CAN message in bytes (0 to 8 bytes).
* @param syncFlag This flag bit is used for synchronous TPDO messages. If it is
* set, message will not be sent, if curent time is outside synchronous window.
* set, message will not be sent, if current time is outside synchronous window.
*
* @return Pointer to CAN transmit message buffer. 8 bytes data array inside
* buffer should be written, before CO_CANsend() function is called.

550
301/CO_fifo.c
View file

@ -308,27 +308,17 @@ bool_t CO_fifo_CommSearch(CO_fifo_t *fifo, bool_t clear) {
/******************************************************************************/
bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo) {
bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo, bool_t *insideComment) {
bool_t delimCommandFound = false;
if (fifo != NULL) {
if (fifo != NULL && insideComment != NULL) {
while (fifo->readPtr != fifo->writePtr) {
char c = fifo->buf[fifo->readPtr];
if (c == DELIM_COMMENT) {
/* comment found, clear all until next DELIM_COMMAND */
while (fifo->readPtr != fifo->writePtr) {
if (++fifo->readPtr == fifo->bufSize) {
fifo->readPtr = 0;
}
if (c == DELIM_COMMAND) {
delimCommandFound = true;
break;
}
}
break;
*insideComment = true;
}
else if (isgraph((int)c) != 0) {
else if (isgraph((int)c) != 0 && !(*insideComment)) {
break;
}
if (++fifo->readPtr == fifo->bufSize) {
@ -336,6 +326,7 @@ bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo) {
}
if (c == DELIM_COMMAND) {
delimCommandFound = true;
*insideComment = false;
break;
}
}
@ -471,8 +462,26 @@ size_t CO_fifo_readToken(CO_fifo_t *fifo,
#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES
/******************************************************************************/
/* Tables for mime-base64 encoding, as specified in RFC 2045, (without CR-LF,
* but one long string). Base64 is used for encoding binary data into easy
* transferable printable characters. In general, each three bytes of binary
* data are translated into four characters, where characters are selected from
* 64 characters long table. See https://en.wikipedia.org/wiki/Base64 */
static const char base64EncTable[] =
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
static const char base64DecTable[] = {
-1, -1, -1, -1, -1, -1, -1, -1, -1,103,101, -1, -1,102, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
103, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 62, -1, -1, -1, 63,
52, 53, 54, 55, 56, 57, 58, 59, 60, 61, -1, -1, -1,100, -1, -1,
-1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, -1, -1, -1, -1, -1,
-1, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, -1, -1, -1, -1, -1};
size_t CO_fifo_readU82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint8_t n;
uint8_t n=0;
if (fifo != NULL && count >= 6 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -484,7 +493,7 @@ size_t CO_fifo_readU82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readU162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint16_t n;
uint16_t n=0;
if (fifo != NULL && count >= 8 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -496,7 +505,7 @@ size_t CO_fifo_readU162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readU322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint32_t n;
uint32_t n=0;
if (fifo != NULL && count >= 12 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -508,7 +517,7 @@ size_t CO_fifo_readU322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readU642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint64_t n;
uint64_t n=0;
if (fifo != NULL && count >= 20 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -520,7 +529,7 @@ size_t CO_fifo_readU642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readX82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint8_t n;
uint8_t n=0;
if (fifo != NULL && count >= 6 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -532,7 +541,7 @@ size_t CO_fifo_readX82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readX162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint16_t n;
uint16_t n=0;
if (fifo != NULL && count >= 8 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -544,7 +553,7 @@ size_t CO_fifo_readX162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readX322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint32_t n;
uint32_t n=0;
if (fifo != NULL && count >= 12 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -556,7 +565,7 @@ size_t CO_fifo_readX322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readX642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
uint64_t n;
uint64_t n=0;
if (fifo != NULL && count >= 20 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -568,7 +577,7 @@ size_t CO_fifo_readX642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readI82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
int8_t n;
int8_t n=0;
if (fifo != NULL && count >= 6 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -580,7 +589,7 @@ size_t CO_fifo_readI82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readI162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
int16_t n;
int16_t n=0;
if (fifo != NULL && count >= 8 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -592,7 +601,7 @@ size_t CO_fifo_readI162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readI322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
int32_t n;
int32_t n=0;
if (fifo != NULL && count >= 13 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -604,7 +613,7 @@ size_t CO_fifo_readI322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readI642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
int64_t n;
int64_t n=0;
if (fifo != NULL && count >= 23 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -616,7 +625,7 @@ size_t CO_fifo_readI642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readR322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
float32_t n;
float32_t n=0;
if (fifo != NULL && count >= 20 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -628,7 +637,7 @@ size_t CO_fifo_readR322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
size_t CO_fifo_readR642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
float64_t n;
float64_t n=0;
if (fifo != NULL && count >= 30 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
@ -684,16 +693,85 @@ size_t CO_fifo_readVs2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
}
break;
}
if (c == '"') {
buf[len++] = '"';
else if (c != 0 && c != '\r') { /* skip null and CR inside string */
buf[len++] = c;
if (c == '"') {
buf[len++] = c;
}
}
buf[len++] = c;
}
}
return len;
}
size_t CO_fifo_readB642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
/* mime-base64 encoding, see description above base64EncTable */
size_t len = 0;
if (fifo != NULL && count >= 4) {
uint8_t step;
uint16_t word;
if (!fifo->started) {
fifo->started = true;
step = 0;
word = 0;
}
else {
/* get memorized variables from previous function calls */
step = (uint8_t)(fifo->aux >> 16);
word = (uint16_t)fifo->aux;
}
while ((len + 3) <= count) {
char c;
if(!CO_fifo_getc(fifo, &c)) {
/* buffer is empty, is also SDO communication finished? */
if (end) {
/* add padding if necessary */
switch (step) {
case 1:
buf[len++] = base64EncTable[(word >> 4) & 0x3F];
buf[len++] = '=';
buf[len++] = '=';
break;
case 2:
buf[len++] = base64EncTable[(word >> 6) & 0x3F];
buf[len++] = '=';
break;
}
}
break;
}
word |= (uint8_t)c;
switch (step++) {
case 0:
buf[len++] = base64EncTable[(word >> 2) & 0x3F];
break;
case 1:
buf[len++] = base64EncTable[(word >> 4) & 0x3F];
break;
default:
buf[len++] = base64EncTable[(word >> 6) & 0x3F];
buf[len++] = base64EncTable[word & 0x3F];
step = 0;
break;
}
word <<= 8;
}
/* memorize variables for next iteration */
fifo->aux = (uint32_t)step << 16 | word;
}
return len;
}
/******************************************************************************/
size_t CO_fifo_cpyTok2U8(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
@ -921,108 +999,8 @@ size_t CO_fifo_cpyTok2R64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
size_t CO_fifo_cpyTok2Hex(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
size_t destSpace, destSpaceStart;
bool_t finished = false;
CO_fifo_st st = 0;
if (dest == NULL || src == NULL) {
return 0;
}
/* get free space of the dest fifo */
destSpaceStart = destSpace = CO_fifo_getSpace(dest);
/* repeat until destination space available and no error and not finished
* and source characters available */
while (destSpace > 0 && (st & CO_fifo_st_errMask) == 0 && !finished) {
char c;
if (!CO_fifo_getc(src, &c)) {
break;
}
if (dest->started == false) {
/* search for first of two hex digits in token */
if (isxdigit((int)c) != 0) {
/* first hex digit is known */
dest->aux = c;
dest->started = true;
}
else if (c == DELIM_COMMAND) {
/* newline found, finish */
st |= CO_fifo_st_closed;
finished = true;
}
else if (c == DELIM_COMMENT) {
/* comment found, clear line and finish */
while (CO_fifo_getc(src, &c)) {
if (c == DELIM_COMMAND) {
st |= CO_fifo_st_closed;
break;
}
}
finished = true;
}
else if (isgraph((int)c) != 0) {
/* printable character, not hex digit, error */
st |= CO_fifo_st_errTok;
}
/* else just skip empty space */
}
else {
/* one hex digit is known, what is next */
if (isxdigit((int)c) != 0) {
/* two hex digits are known */
char s[3];
int32_t num;
s[0] = (char) dest->aux; s[1] = c, s[2] = 0;
num = strtol(s, NULL, 16);
/* copy the character */
CO_fifo_putc(dest, (char) num);
destSpace--;
}
else if (isgraph((int)c) != 0 && c != DELIM_COMMENT) {
/* printable character, not hex digit, error */
st |= CO_fifo_st_errTok;
}
else {
/* this is space or comment, single hex digit is known */
char s[2];
int32_t num;
s[0] = (char) dest->aux; s[1] = 0;
num = strtol(s, NULL, 16);
/* copy the character */
CO_fifo_putc(dest, (char) num);
destSpace--;
if (c == DELIM_COMMAND) {
/* newline found, finish */
st |= CO_fifo_st_closed;
finished = true;
}
else if (c == DELIM_COMMENT) {
/* comment found, clear line and finish */
while (CO_fifo_getc(src, &c)) {
if (c == DELIM_COMMAND) {
st |= CO_fifo_st_closed;
break;
}
}
finished = true;
}
}
dest->started = false;
}
}
if (!finished) {
st |= CO_fifo_st_partial;
}
if (status != NULL) *status = st;
return destSpaceStart - destSpace;
}
size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
size_t destSpace, destSpaceStart;
bool_t finished = false;
uint8_t step;
char firstChar;
CO_fifo_st st = 0;
if (dest == NULL || src == NULL) {
@ -1034,9 +1012,19 @@ size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
/* is this the first write into dest? */
if (!dest->started) {
/* aux variable is used as state here */
dest->aux = 0;
bool_t insideComment = false;
if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
/* command delimiter found without string, this is an error */
st |= CO_fifo_st_errTok;
}
dest->started = true;
step = 0;
firstChar = 0;
}
else {
/* get memorized variables from previous function calls */
step = (uint8_t)(dest->aux >> 8);
firstChar = (char)(dest->aux & 0xFF);
}
/* repeat until destination space available and no error and not finished
@ -1047,43 +1035,145 @@ size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
break;
}
switch (dest->aux) {
case 0: /* beginning of the string, first write into dest */
if (isgraph((int)c) == 0 || c == DELIM_COMMENT) {
if (CO_fifo_trimSpaces(src)) {
/* newline found without string, this is an error */
st |= CO_fifo_st_errTok;
}
if (step == 6) {
/* command is inside comment, waiting for command delimiter */
bool_t insideComment = true;
if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
st |= CO_fifo_st_closed;
finished = true;
}
else if (c == '"') {
continue;
}
if (isxdigit((int)c) != 0) {
/* first or second hex digit */
if (step == 0) {
firstChar = c;
step = 1;
}
else {
/* write the byte */
char s[3];
int32_t num;
s[0] = firstChar; s[1] = c; s[2] = 0;
num = strtol(s, NULL, 16);
CO_fifo_putc(dest, (char) num);
destSpace--;
step = 0;
}
}
else if (isgraph((int)c) != 0) {
/* printable character, not hex digit */
if (c == '#') /* comment start */
step = 6;
else /* syntax error */
st |= CO_fifo_st_errTok;
}
else {
/* this is space or delimiter */
if (step == 1) {
/* write the byte */
char s[2];
int32_t num;
s[0] = firstChar; s[1] = 0;
num = strtol(s, NULL, 16);
CO_fifo_putc(dest, (char) num);
destSpace--;
step = 0;
}
bool_t insideComment = false;
if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
/* newline found, finish */
st |= CO_fifo_st_closed;
finished = true;
}
else if (insideComment) {
step = 6;
}
}
} /* while ... */
if (!finished) {
st |= CO_fifo_st_partial;
/* memorize variables for next iteration */
dest->aux = (uint32_t)step << 8 | (uint8_t)firstChar;
}
if (status != NULL) *status = st;
return destSpaceStart - destSpace;
}
size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
size_t destSpace, destSpaceStart;
bool_t finished = false;
uint8_t step;
CO_fifo_st st = 0;
if (dest == NULL || src == NULL) {
return 0;
}
/* get free space of the dest fifo */
destSpaceStart = destSpace = CO_fifo_getSpace(dest);
/* is this the first write into dest? */
if (!dest->started) {
bool_t insideComment = false;
if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
/* command delimiter found without string, this is an error */
st |= CO_fifo_st_errTok;
}
dest->started = true;
step = 0;
}
else {
/* get memorized variables from previous function calls */
step = (uint8_t)dest->aux;
}
/* repeat until destination space available and no error and not finished
* and source characters available */
while (destSpace > 0 && (st & CO_fifo_st_errMask) == 0 && !finished) {
char c;
if (!CO_fifo_getc(src, &c)) {
break;
}
switch (step) {
case 0: /* beginning of the string, first write into dest */
if (c == '"') {
/* Indicated beginning of the string, skip this character. */
dest->aux = 1;
step = 1;
}
else {
/* this must be a single word string without '"' */
/* copy the character */
CO_fifo_putc(dest, c);
destSpace--;
dest->aux = 2;
step = 2;
}
break;
case 1: /* inside string, quoted string */
case 2: /* inside string, single word, no quotes */
if (c == '"') {
/* double qoute found, this may be end of the string or escaped
/* double quote found, this may be end of the string or escaped
* double quote (with two double quotes) */
dest->aux += 2;
step += 2;
}
else if (isgraph((int)c) == 0 && dest->aux == 2) {
else if (isgraph((int)c) == 0 && step == 2) {
/* end of single word string */
if (c == DELIM_COMMAND) {
bool_t insideComment = false;
if (c == DELIM_COMMAND
|| CO_fifo_trimSpaces(src, &insideComment)
) {
st |= CO_fifo_st_closed;
finished = true;
}
else if (CO_fifo_trimSpaces(src)) {
st |= CO_fifo_st_closed;
else {
step = insideComment ? 6 : 5;
}
finished = true;
}
else if (c == DELIM_COMMAND) {
/* no closing quote, error */
@ -1102,24 +1192,27 @@ size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
/* escaped double quote, copy the character and continue */
CO_fifo_putc(dest, c);
destSpace--;
dest->aux -= 2;
step -= 2;
}
else {
/* previous character was closing double quote */
if (dest->aux == 4) {
if (step == 4) {
/* no opening double quote, syntax error */
st |= CO_fifo_st_errTok;
}
else {
if (isgraph((int)c) == 0) {
/* string finished */
if (c == DELIM_COMMAND) {
/* end of quoted string */
bool_t insideComment = false;
if (c == DELIM_COMMAND
|| CO_fifo_trimSpaces(src, &insideComment)
) {
st |= CO_fifo_st_closed;
finished = true;
}
else if (CO_fifo_trimSpaces(src)) {
st |= CO_fifo_st_closed;
else {
step = insideComment ? 6 : 5;
}
finished = true;
}
else {
/* space must follow closing double quote, error */
@ -1129,6 +1222,31 @@ size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
}
break;
case 5: { /* String token is finished, waiting for command delimiter */
bool_t insideComment = false;
if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
st |= CO_fifo_st_closed;
finished = true;
}
else if (insideComment) {
step = 6;
}
else if (isgraph((int)c) != 0) {
if (c == '#') /* comment start */
step = 6;
else /* syntax error */
st |= CO_fifo_st_errTok;
}
break;
}
case 6: { /* String token is finished, waiting for command delimiter */
bool_t insideComment = true;
if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
st |= CO_fifo_st_closed;
finished = true;
}
break;
}
default: /* internal error */
st |= CO_fifo_st_errInt;
break;
@ -1137,6 +1255,120 @@ size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
if (!finished) {
st |= CO_fifo_st_partial;
/* memorize variables for next iteration */
dest->aux = step;
}
if (status != NULL) *status = st;
return destSpaceStart - destSpace;
}
size_t CO_fifo_cpyTok2B64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
/* mime-base64 decoding, see description above base64EncTable */
size_t destSpace, destSpaceStart;
bool_t finished = false;
uint8_t step;
uint32_t dword;
CO_fifo_st st = 0;
if (dest == NULL || src == NULL) {
return 0;
}
/* get free space of the dest fifo */
destSpaceStart = destSpace = CO_fifo_getSpace(dest);
/* is this the first write into dest? */
if (!dest->started) {
bool_t insideComment = false;
if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
/* command delimiter found without string, this is an error */
st |= CO_fifo_st_errTok;
}
dest->started = true;
step = 0;
dword = 0;
}
else {
/* get memorized variables from previous function calls */
step = (uint8_t)(dest->aux >> 24);
dword = dest->aux & 0xFFFFFF;
}
/* repeat until destination space available and no error and not finished
* and source characters available */
while (destSpace >= 3 && (st & CO_fifo_st_errMask) == 0 && !finished) {
char c;
if (!CO_fifo_getc(src, &c)) {
break;
}
if (step >= 5) {
/* String token is finished, waiting for command delimiter */
bool_t insideComment = step > 5;
if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
st |= CO_fifo_st_closed;
finished = true;
}
else if (insideComment) {
step = 6;
}
else if (isgraph((int)c) != 0 && c != '=') {
if (c == '#') /* comment start */
step = 6;
else /* syntax error */
st |= CO_fifo_st_errTok;
}
continue;
}
char code = base64DecTable[c & 0x7F];
if (c < 0 || code < 0) {
st |= CO_fifo_st_errTok;
}
else if (code >= 64 /* '=' (pad) or DELIM_COMMAND or space */) {
/* base64 string finished, write remaining bytes */
switch (step) {
case 2:
CO_fifo_putc(dest, (char)(dword >> 4));
destSpace --;
break;
case 3:
CO_fifo_putc(dest, (char)(dword >> 10));
CO_fifo_putc(dest, (char)(dword >> 2));
destSpace -= 2;
break;
}
bool_t insideComment = false;
if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
st |= CO_fifo_st_closed;
finished = true;
}
else {
step = insideComment ? 6 : 5;
}
}
else {
dword = (dword << 6) | code;
if (step++ == 3) {
CO_fifo_putc(dest, (char)((dword >> 16) & 0xFF));
CO_fifo_putc(dest, (char)((dword >> 8) & 0xFF));
CO_fifo_putc(dest, (char)(dword & 0xFF));
destSpace -= 3;
dword = 0;
step = 0;
}
}
} /* while ... */
if (!finished) {
st |= CO_fifo_st_partial;
/* memorize variables for next iteration */
dest->aux = (uint32_t)step << 24 | (dword & 0xFFFFFF);
}
if (status != NULL) *status = st;

40
301/CO_fifo.h
View file

@ -110,6 +110,24 @@ static inline void CO_fifo_reset(CO_fifo_t *fifo) {
}
/**
* Purge all data in fifo object, keep other properties
*
* @param fifo This object
*
* @return true, if data were purged or false, if fifo were already empty
*/
static inline bool_t CO_fifo_purge(CO_fifo_t *fifo) {
if (fifo != NULL && fifo->readPtr != fifo->writePtr) {
fifo->readPtr = 0;
fifo->writePtr = 0;
return true;
}
return false;
}
/**
* Get free buffer space in CO_fifo_t object
*
@ -342,10 +360,13 @@ bool_t CO_fifo_CommSearch(CO_fifo_t *fifo, bool_t clear);
* is also removed).
*
* @param fifo This object.
* @param [in, out] insideComment if set to true as input, it skips all
* characters and searches only for delimiter. As output it is set to true, if
* fifo is empty, is inside comment and command delimiter is not found.
*
* @return true if command delimiter was found.
*/
bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo);
bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo, bool_t *insideComment);
/**
@ -400,7 +421,7 @@ size_t CO_fifo_readToken(CO_fifo_t *fifo,
* @param fifo This object.
* @param buf Buffer into which ascii string will be copied.
* @param count Available count of bytes inside the buf.
* @param end If true, then fifo contains last bytes, which has to be read.
* @param end True indicates, that fifo contains last bytes of data.
*
* @return Number of ascii bytes written into buf.
*/
@ -437,8 +458,14 @@ size_t CO_fifo_readHex2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
/** Read data from fifo and output as visible string. A visible string is
* enclosed with double quotes. If a double quote is used within the string,
* the quotes are escaped by a second quotes, e.g. “Hello “”World””, CANopen
* is great”. See also CO_fifo_readU82a */
* is great”. UTF-8 characters and also line breaks works with this function.
* Function removes all '\0' and '\r' characters from output string.
* See also CO_fifo_readU82a */
size_t CO_fifo_readVs2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
/** Read data from fifo and output as mime-base64 encoded string. Encoding is as
* specified in RFC 2045, without CR-LF, but one long string. See also
* CO_fifo_readU82a */
size_t CO_fifo_readB642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
/** Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar */
@ -493,8 +520,13 @@ size_t CO_fifo_cpyTok2R64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
size_t CO_fifo_cpyTok2Hex(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
/** Copy visible string to data. A visible string must be enclosed with double
* quotes, if it contains space. If a double quote is used within the string,
* the quotes are escaped by a second quotes. See CO_fifo_cpyTok2U8 */
* the quotes are escaped by a second quotes. Input string can not contain
* newline characters. See CO_fifo_cpyTok2U8 */
size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
/** Read ascii mime-base64 encoded string from src fifo and copy as binary data
* to dest fifo. Encoding is as specified in RFC 2045, without CR-LF, but one
* long string in single line. See also CO_fifo_readU82a */
size_t CO_fifo_cpyTok2B64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES */

300
309/CO_gateway_ascii.c
View file

@ -51,8 +51,8 @@
CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
CO_SDOclient_t* SDO_C,
uint16_t SDOtimeoutTimeDefault,
bool_t SDOblockTransferEnableDefault,
uint16_t SDOclientTimeoutTime_ms,
bool_t SDOclientBlockTransfer,
#endif
#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
CO_NMT_t *NMT,
@ -69,7 +69,7 @@ CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
/* verify arguments */
if (gtwa == NULL
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
|| SDO_C == NULL || SDOtimeoutTimeDefault == 0
|| SDO_C == NULL || SDOclientTimeoutTime_ms == 0
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
|| NMT == NULL
@ -90,8 +90,8 @@ CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
/* initialize variables */
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
gtwa->SDO_C = SDO_C;
gtwa->SDOtimeoutTime = SDOtimeoutTimeDefault;
gtwa->SDOblockTransferEnable = SDOblockTransferEnableDefault;
gtwa->SDOtimeoutTime = SDOclientTimeoutTime_ms;
gtwa->SDOblockTransferEnable = SDOclientBlockTransfer;
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
gtwa->NMT = NMT;
@ -125,7 +125,8 @@ CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
void CO_GTWA_initRead(CO_GTWA_t* gtwa,
size_t (*readCallback)(void *object,
const char *buf,
size_t count),
size_t count,
uint8_t *connectionOK),
void *readCallbackObject)
{
if (gtwa != NULL) {
@ -153,74 +154,73 @@ void CO_GTWA_log_print(CO_GTWA_t* gtwa, const char *message) {
* HELPER FUNCTIONS
******************************************************************************/
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
/* help strings */
/* help strings ("\n" is used between string, "\r\n" closes the response.) */
static const char CO_GTWA_helpString[] =
"\r\nCommand strings start with '\"[\"<sequence>\"]\"' followed by:\r\n" \
"[[<net>] <node>] r[ead] <index> <subindex> [<datatype>] # SDO upload.\r\n" \
"[[<net>] <node>] w[rite] <index> <subindex> <datatype> <value> # SDO download.\r\n" \
"\r\n" \
"[[<net>] <node>] start # NMT Start node.\r\n" \
"[[<net>] <node>] stop # NMT Stop node.\r\n" \
"[[<net>] <node>] preop[erational] # NMT Set node to pre-operational.\r\n" \
"[[<net>] <node>] reset node # NMT Reset node.\r\n" \
"[[<net>] <node>] reset comm[unication] # NMT Reset communication.\r\n" \
"\r\n" \
"[<net>] set network <value> # Set default net.\r\n" \
"[<net>] set node <value> # Set default node.\r\n" \
"[<net>] set sdo_timeout <value> # Configure SDO time-out.\r\n" \
"[<net>] set sdo_block <value> # Enable/disable SDO block transfer.\r\n" \
"\r\n" \
"help [datatype|lss] # Print this or datatype or lss help.\r\n" \
"led # Print status LED diodes.\r\n" \
"log # Print message log.\r\n" \
"\r\n" \
"Response:\r\n" \
"\"[\"<sequence>\"]\" OK | <value> |\r\n" \
" ERROR:<SDO-abort-code> | ERROR:<internal-error-code>\r\n" \
"\r\n" \
"* Every command must be terminated with <CR><LF> ('\\r\\n'). characters. Same\r\n" \
" is response. String is not null terminated, <CR> is optional in command.\r\n" \
"* Comments started with '#' are ignored. They may be on the beginning of the\r\n" \
" line or after the command string.\r\n" \
"* 'sdo_timeout' is in milliseconds, 500 by default. Block transfer is\r\n" \
" disabled by default.\r\n" \
"* If '<net>' or '<node>' is not specified within commands, then value defined\r\n" \
"\nCommand strings start with '\"[\"<sequence>\"]\"' followed by:\n" \
"[[<net>] <node>] r[ead] <index> <subindex> [<datatype>] # SDO upload.\n" \
"[[<net>] <node>] w[rite] <index> <subindex> <datatype> <value> # SDO download.\n" \
"\n" \
"[[<net>] <node>] start # NMT Start node.\n" \
"[[<net>] <node>] stop # NMT Stop node.\n" \
"[[<net>] <node>] preop[erational] # NMT Set node to pre-operational.\n" \
"[[<net>] <node>] reset node # NMT Reset node.\n" \
"[[<net>] <node>] reset comm[unication] # NMT Reset communication.\n" \
"\n" \
"[<net>] set network <value> # Set default net.\n" \
"[<net>] set node <value> # Set default node.\n" \
"[<net>] set sdo_timeout <value> # Configure SDO client time-out in ms.\n" \
"[<net>] set sdo_block <0|1> # Enable/disable SDO block transfer.\n" \
"\n" \
"help [datatype|lss] # Print this or datatype or lss help.\n" \
"led # Print status LEDs of this device.\n" \
"log # Print message log.\n" \
"\n" \
"Response:\n" \
"\"[\"<sequence>\"]\" OK | <value> |\n" \
" ERROR:<SDO-abort-code> | ERROR:<internal-error-code>\n" \
"\n" \
"* Every command must be terminated with <CR><LF> ('\\r\\n'). characters. Same\n" \
" is response. String is not null terminated, <CR> is optional in command.\n" \
"* Comments started with '#' are ignored. They may be on the beginning of the\n" \
" line or after the command string.\n" \
"* 'sdo_timeout' is in milliseconds, 500 by default. Block transfer is\n" \
" disabled by default.\n" \
"* If '<net>' or '<node>' is not specified within commands, then value defined\n" \
" by 'set network' or 'set node' command is used.\r\n";
static const char CO_GTWA_helpStringDatatypes[] =
"\r\nDatatypes:\r\n" \
"b # Boolean.\r\n" \
"i8, i16, i32, i64 # Signed integers.\r\n" \
"u8, u16, u32, u64 # Unsigned integers.\r\n" \
"x8, x16, x32, x64 # Unsigned integers, displayed as hexadecimal, non-standard.\r\n" \
"r32, r64 # Real numbers.\r\n" \
"t, td # Time of day, time difference.\r\n" \
"vs # Visible string (between double quotes if multi-word).\r\n" \
"os, us # Octet, unicode string, (mime-base64 (RFC2045) based, line).\r\n" \
"d # domain (mime-base64 (RFC2045) based, one line).\r\n" \
"\nDatatypes:\n" \
"b # Boolean.\n" \
"i8, i16, i32, i64 # Signed integers.\n" \
"u8, u16, u32, u64 # Unsigned integers.\n" \
"x8, x16, x32, x64 # Unsigned integers, displayed as hexadecimal, non-standard.\n" \
"r32, r64 # Real numbers.\n" \
"vs # Visible string (between double quotes if multi-word).\n" \
"os, us # Octet, unicode string, (mime-base64 (RFC2045) based, line).\n" \
"d # domain (mime-base64 (RFC2045) based, one line).\n" \
"hex # Hexagonal data, optionally space separated, non-standard.\r\n";
static const char CO_GTWA_helpStringLss[] =
"\r\nLSS commands:\r\n" \
"lss_switch_glob <0|1> # Switch state global command.\r\n" \
"lss_switch_sel <vendorID> <product code> \\\r\n" \
" <revisionNo> <serialNo> #Switch state selective.\r\n" \
"lss_set_node <node> # Configure node-ID.\r\n" \
"lss_conf_bitrate <table_selector=0> \\\r\n" \
" <table_index> # Configure bit-rate.\r\n" \
"lss_activate_bitrate <switch_delay_ms> # Activate new bit-rate.\r\n" \
"lss_store # LSS store configuration.\r\n" \
"lss_inquire_addr [<LSSSUB=0..3>] # Inquire LSS address.\r\n" \
"lss_get_node # Inquire node-ID.\r\n" \
"_lss_fastscan [<timeout_ms>] # Identify fastscan, non-standard.\r\n" \
"lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\\r\n" \
" [<scanType0> <vendorId> <scanType1> <productCode>\\\r\n" \
" <scanType2> <revisionNo> <scanType3> <serialNo>]]]\r\n" \
" # Node-ID configuration of all nodes.\r\n" \
"\r\n" \
"* All LSS commands start with '\"[\"<sequence>\"]\" [<net>]'.\r\n" \
"* <table_index>: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,\r\n" \
" 4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto\r\n" \
"\nLSS commands:\n" \
"lss_switch_glob <0|1> # Switch state global command.\n" \
"lss_switch_sel <vendorID> <product code> \\\n" \
" <revisionNo> <serialNo> #Switch state selective.\n" \
"lss_set_node <node> # Configure node-ID.\n" \
"lss_conf_bitrate <table_selector=0> \\\n" \
" <table_index> # Configure bit-rate.\n" \
"lss_activate_bitrate <switch_delay_ms> # Activate new bit-rate.\n" \
"lss_store # LSS store configuration.\n" \
"lss_inquire_addr [<LSSSUB=0..3>] # Inquire LSS address.\n" \
"lss_get_node # Inquire node-ID.\n" \
"_lss_fastscan [<timeout_ms>] # Identify fastscan, non-standard.\n" \
"lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\\n" \
" [<scanType0> <vendorId> <scanType1> <productCode>\\\n" \
" <scanType2> <revisionNo> <scanType3> <serialNo>]]]\n" \
" # Node-ID configuration of all nodes.\n" \
"\n" \
"* All LSS commands start with '\"[\"<sequence>\"]\" [<net>]'.\n" \
"* <table_index>: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,\n" \
" 4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto\n" \
"* <scanType>: 0=fastscan, 1=ignore, 2=match value in next parameter\r\n";
#endif
@ -340,12 +340,10 @@ static const CO_GTWA_dataType_t dataTypes[] = {
{"x64", 8, CO_fifo_readX642a, CO_fifo_cpyTok2U64}, /* UNSIGNED64 */
{"r32", 4, CO_fifo_readR322a, CO_fifo_cpyTok2R32}, /* REAL32 */
{"r64", 8, CO_fifo_readR642a, CO_fifo_cpyTok2R64}, /* REAL64 */
// {"t", 0, CO_GWA_dtpHex, CO_DWA_dtsHex}, /* TIME_OF_DAY */
// {"td", 0, CO_GWA_dtpHex, CO_DWA_dtsHex}, /* TIME_DIFFERENCE */
{"vs", 0, CO_fifo_readVs2a, CO_fifo_cpyTok2Vs}//,
// {"os", 0, CO_GWA_dtpHex, CO_DWA_dtsHex}, /* ochar_t (OCTET_STRING) (mime-base64 (RFC2045) should be used here) */
// {"us", 0, CO_GWA_dtpHex, CO_DWA_dtsHex}, /* (UNICODE_STRING) (mime-base64 (RFC2045) should be used here) */
// {"d", 0, CO_GWA_dtpHex, CO_DWA_dtsHex} /* domain_t (DOMAIN) (mime-base64 (RFC2045) should be used here) */
{"vs", 0, CO_fifo_readVs2a, CO_fifo_cpyTok2Vs}, /* VISIBLE_STRING */
{"os", 0, CO_fifo_readB642a, CO_fifo_cpyTok2B64}, /* OCTET_STRING base64*/
{"us", 0, CO_fifo_readB642a, CO_fifo_cpyTok2B64},/* UNICODE_STRING base64*/
{"d", 0, CO_fifo_readB642a, CO_fifo_cpyTok2B64} /* DOMAIN - base64 */
};
@ -369,8 +367,11 @@ static const CO_GTWA_dataType_t *CO_GTWA_getDataType(char *token, bool_t *err) {
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
/* transfer response buffer and verify if all bytes was read */
static void respBufTransfer(CO_GTWA_t *gtwa) {
/* transfer response buffer and verify if all bytes was read. Return true on
* success, or false, if communication is broken. */
static bool_t respBufTransfer(CO_GTWA_t *gtwa) {
uint8_t connectionOK = 1;
if (gtwa->readCallback == NULL) {
/* no callback registered, just purge the response */
gtwa->respBufOffset = 0;
@ -382,7 +383,8 @@ static void respBufTransfer(CO_GTWA_t *gtwa) {
size_t countRead =
gtwa->readCallback(gtwa->readCallbackObject,
(const char *)&gtwa->respBuf[gtwa->respBufOffset],
gtwa->respBufCount);
gtwa->respBufCount,
&connectionOK);
if (countRead < gtwa->respBufCount) {
gtwa->respBufOffset += countRead;
@ -395,6 +397,7 @@ static void respBufTransfer(CO_GTWA_t *gtwa) {
gtwa->respHold = false;
}
}
return connectionOK != 0;
}
@ -495,10 +498,11 @@ static void responseWithError(CO_GTWA_t *gtwa,
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
static void responseWithErrorSDO(CO_GTWA_t *gtwa,
CO_SDO_abortCode_t abortCode)
CO_SDO_abortCode_t abortCode,
bool_t postponed)
{
int i;
int len = sizeof(errorDescs) / sizeof(errorDescs_t);
int len = sizeof(errorDescsSDO) / sizeof(errorDescs_t);
const char *desc = "-";
for (i = 0; i < len; i++) {
@ -508,9 +512,17 @@ static void responseWithErrorSDO(CO_GTWA_t *gtwa,
}
}
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%"PRId32"] ERROR:0x%08X #%s\r\n",
gtwa->sequence, abortCode, desc);
if (!postponed) {
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%"PRId32"] ERROR:0x%08X #%s\r\n",
gtwa->sequence, abortCode, desc);
}
else {
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"\n...ERROR:0x%08X #%s\r\n",
abortCode, desc);
}
respBufTransfer(gtwa);
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
@ -527,11 +539,20 @@ static inline void responseWithError(CO_GTWA_t *gtwa,
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
static inline void responseWithErrorSDO(CO_GTWA_t *gtwa,
CO_SDO_abortCode_t abortCode)
CO_SDO_abortCode_t abortCode,
bool_t postponed)
{
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%"PRId32"] ERROR:0x%08X\r\n",
gtwa->sequence, abortCode);
if (!postponed) {
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%"PRId32"] ERROR:0x%08X\r\n",
gtwa->sequence, abortCode);
}
else {
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"\n...ERROR:0x%08X\r\n",
abortCode);
}
respBufTransfer(gtwa);
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
@ -546,6 +567,13 @@ static inline void responseWithOK(CO_GTWA_t *gtwa) {
}
static inline void responseWithEmpty(CO_GTWA_t *gtwa) {
gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"\r\n");
respBufTransfer(gtwa);
}
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
static void responseLSS(CO_GTWA_t *gtwa, CO_LSSmaster_return_t lss_ret) {
if (lss_ret == CO_LSSmaster_OK) {
@ -649,6 +677,7 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
/* If empty line or just comment, continue with next command */
else if (n == 0 && closed != 0) {
responseWithEmpty(gtwa);
continue;
}
if (tok[0] != '[' || tok[strlen(tok)-1] != ']') {
@ -1511,26 +1540,28 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
gtwa->state = CO_GTWA_ST_IDLE;
}
else if (gtwa->state == CO_GTWA_ST_IDLE) {
return;
else switch (gtwa->state) {
case CO_GTWA_ST_IDLE: {
return; /* skip timerNext_us calculation */
}
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
/* SDO upload state */
else if (gtwa->state == CO_GTWA_ST_READ) {
case CO_GTWA_ST_READ: {
CO_SDO_abortCode_t abortCode;
size_t sizeTransferred;
CO_SDO_return_t ret;
ret = CO_SDOclientUpload(gtwa->SDO_C,
timeDifference_us,
false,
&abortCode,
NULL,
&sizeTransferred,
timerNext_us);
if (ret < 0) {
responseWithErrorSDO(gtwa, abortCode);
responseWithErrorSDO(gtwa, abortCode, gtwa->SDOdataCopyStatus);
gtwa->state = CO_GTWA_ST_IDLE;
}
/* Response data must be read, partially or whole */
@ -1568,21 +1599,34 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
/* transfer response to the application */
respBufTransfer(gtwa);
if (respBufTransfer(gtwa) == false) {
/* broken communication, send SDO abort and force finish. */
abortCode = CO_SDO_AB_DATA_TRANSF;
CO_SDOclientUpload(gtwa->SDO_C,
0,
true,
&abortCode,
NULL,
NULL,
NULL);
gtwa->state = CO_GTWA_ST_IDLE;
break;
}
} while (gtwa->respHold == false && fifoRemain > 0);
}
break;
}
/* SDO download state */
else if (gtwa->state == CO_GTWA_ST_WRITE) {
case CO_GTWA_ST_WRITE:
case CO_GTWA_ST_WRITE_ABORTED: {
CO_SDO_abortCode_t abortCode;
size_t sizeTransferred;
bool_t abort = false;
bool_t hold = false;
CO_SDO_return_t ret;
int loop = 0;
/* copy data to the SDO buffer if more data available */
/* copy data to the SDO buffer if previous dataTypeScan was partial */
if (gtwa->SDOdataCopyStatus) {
CO_fifo_st status;
gtwa->SDOdataType->dataTypeScan(&gtwa->SDO_C->bufFifo,
@ -1599,6 +1643,17 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
abort = true; /* abort SDO communication */
/* clear the rest of the command, if necessary */
if (closed != 1)
CO_fifo_CommSearch(&gtwa->commFifo, true);
}
if (gtwa->state == CO_GTWA_ST_WRITE_ABORTED) {
/* Stay in this state, until all data transferred via commFifo
* will be purged. */
if (!CO_fifo_purge(&gtwa->SDO_C->bufFifo) || closed == 1) {
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
}
/* If not all data were transferred, make sure, there is enough data in
@ -1617,8 +1672,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
hold = true;
}
}
if (!hold) {
if (!hold || abort) {
/* if OS has CANtx queue, speedup block transfer */
int loop = 0;
do {
ret = CO_SDOclientDownload(gtwa->SDO_C,
timeDifference_us,
@ -1634,19 +1690,23 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
/* send response in case of error or finish */
if (ret < 0) {
responseWithErrorSDO(gtwa, abortCode);
gtwa->state = CO_GTWA_ST_IDLE;
responseWithErrorSDO(gtwa, abortCode, false);
/* purge remaining data if necessary */
gtwa->state = gtwa->SDOdataCopyStatus
? CO_GTWA_ST_WRITE_ABORTED
: CO_GTWA_ST_IDLE;
}
else if (ret == CO_SDO_RT_ok_communicationEnd) {
responseWithOK(gtwa);
gtwa->state = CO_GTWA_ST_IDLE;
}
}
break;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
else if (gtwa->state == CO_GTWA_ST_LSS_SWITCH_GLOB) {
case CO_GTWA_ST_LSS_SWITCH_GLOB: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_switchStateSelect(gtwa->LSSmaster,
timeDifference_us,
@ -1655,8 +1715,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
responseLSS(gtwa, ret);
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_SWITCH_SEL) {
case CO_GTWA_ST_LSS_SWITCH_SEL: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_switchStateSelect(gtwa->LSSmaster,
timeDifference_us,
@ -1665,8 +1726,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
responseLSS(gtwa, ret);
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_SET_NODE) {
case CO_GTWA_ST_LSS_SET_NODE: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_configureNodeId(gtwa->LSSmaster,
timeDifference_us,
@ -1681,8 +1743,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_CONF_BITRATE) {
case CO_GTWA_ST_LSS_CONF_BITRATE: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_configureBitTiming(gtwa->LSSmaster,
timeDifference_us,
@ -1697,8 +1760,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_STORE) {
case CO_GTWA_ST_LSS_STORE: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_configureStore(gtwa->LSSmaster, timeDifference_us);
@ -1712,8 +1776,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_INQUIRE) {
case CO_GTWA_ST_LSS_INQUIRE: {
CO_LSSmaster_return_t ret;
uint32_t value;
@ -1739,8 +1804,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL) {
case CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_InquireLssAddress(gtwa->LSSmaster, timeDifference_us,
@ -1763,8 +1829,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST__LSS_FASTSCAN) {
case CO_GTWA_ST__LSS_FASTSCAN: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_IdentifyFastscan(gtwa->LSSmaster, timeDifference_us,
@ -1789,8 +1856,9 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
CO_LSSmaster_DEFAULT_TIMEOUT);
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
else if (gtwa->state == CO_GTWA_ST_LSS_ALLNODES) {
case CO_GTWA_ST_LSS_ALLNODES: {
CO_LSSmaster_return_t ret;
if (gtwa->lssSubState == 0) { /* _lss_fastscan */
ret = CO_LSSmaster_IdentifyFastscan(gtwa->LSSmaster,
@ -1804,7 +1872,7 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
/* no (more) nodes found, send report sum and finish */
gtwa->respBufCount =
snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"# Found %d nodes, search finished.\r\n" \
"# Found %d nodes, search finished.\n" \
"[%"PRId32"] OK\r\n",
gtwa->lssNodeCount,
gtwa->sequence);
@ -1877,7 +1945,7 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
else {
/* cycle finished successfully, send report */
uint8_t lssNidAssigned = gtwa->lssNID;
const char msg2Fmt[] = "# Not all nodes scanned!\r\n" \
const char msg2Fmt[] = "# Not all nodes scanned!\n" \
"[%"PRId32"] OK\r\n";
char msg2[sizeof(msg2Fmt)+10] = {0};
@ -1900,7 +1968,7 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
gtwa->respBufCount =
snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"# Node-ID %d assigned to: 0x%08"PRIX32" 0x%08" \
PRIX32" 0x%08"PRIX32" 0x%08"PRIX32"\r\n%s",
PRIX32" 0x%08"PRIX32" 0x%08"PRIX32"\n%s",
lssNidAssigned,
gtwa->lssFastscan.found.identity.vendorID,
gtwa->lssFastscan.found.identity.productCode,
@ -1910,12 +1978,13 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
respBufTransfer(gtwa);
}
}
} /* else if (gtwa->state == CO_GTWA_ST_LSS_ALLNODES) */
break;
} /* CO_GTWA_ST_LSS_ALLNODES */
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS */
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
/* print message log */
else if (gtwa->state == CO_GTWA_ST_LOG) {
case CO_GTWA_ST_LOG: {
do {
gtwa->respBufCount = CO_fifo_read(&gtwa->logFifo, gtwa->respBuf,
CO_GTWA_RESP_BUF_SIZE, NULL);
@ -1926,12 +1995,13 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
break;
}
} while (gtwa->respHold == false);
break;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
/* Print help string (in multiple segments if necessary) */
else if (gtwa->state == CO_GTWA_ST_HELP) {
case CO_GTWA_ST_HELP: {
size_t lenBuf = CO_GTWA_RESP_BUF_SIZE;
size_t lenHelp = strlen(gtwa->helpString);
@ -1952,12 +2022,13 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
break;
}
} while (gtwa->respHold == false);
break;
}
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
/* print CANopen status LED diodes */
else if (gtwa->state == CO_GTWA_ST_LED) {
case CO_GTWA_ST_LED: {
uint8_t i;
if (CO_fifo_CommSearch(&gtwa->commFifo, false)) {
@ -1977,15 +2048,18 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
respBufTransfer(gtwa);
gtwa->ledStringPreviousIndex = i;
}
break;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
/* illegal state */
else {
default: {
respErrorCode = CO_GTWA_respErrorInternalState;
responseWithError(gtwa, respErrorCode);
gtwa->state = CO_GTWA_ST_IDLE;
break;
}
} /* switch (gtwa->state) */
/* execute next CANopen processing immediately, if idle and more commands
* available */

25
309/CO_gateway_ascii.h
View file

@ -154,7 +154,7 @@ lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\
/** Timeout time in microseconds for some internal states. */
#ifndef CO_GTWA_STATE_TIMEOUT_TIME_US
#define CO_GTWA_STATE_TIMEOUT_TIME_US 1000000
#define CO_GTWA_STATE_TIMEOUT_TIME_US 1200000
#endif
@ -233,6 +233,8 @@ typedef enum {
CO_GTWA_ST_READ = 0x10U,
/** SDO 'write' (download) */
CO_GTWA_ST_WRITE = 0x11U,
/** SDO 'write' (download) - aborted, purging remaining data */
CO_GTWA_ST_WRITE_ABORTED = 0x12U,
/** LSS 'lss_switch_glob' */
CO_GTWA_ST_LSS_SWITCH_GLOB = 0x20U,
/** LSS 'lss_switch_sel' */
@ -298,10 +300,14 @@ typedef struct {
* @param object Void pointer to custom object
* @param buf Buffer from which data can be read
* @param count Count of bytes available inside buffer
* @param [out] connectionOK different than 0 indicates connection is OK.
*
* @return Count of bytes actually transferred.
*/
size_t (*readCallback)(void *object, const char *buf, size_t count);
size_t (*readCallback)(void *object,
const char *buf,
size_t count,
uint8_t *connectionOK);
/** Pointer to object, which will be used inside readCallback, from
* CO_GTWA_init() */
void *readCallbackObject;
@ -343,7 +349,9 @@ typedef struct {
uint16_t SDOtimeoutTime;
/** SDO block transfer enabled? */
bool_t SDOblockTransferEnable;
/** Indicate status of data copy from / to SDO buffer */
/** Indicate status of data copy from / to SDO buffer. If reading, true
* indicates, that response has started. If writing, true indicates, that
* SDO buffer contains only part of data and more data will follow. */
bool_t SDOdataCopyStatus;
/** Data type of variable in current SDO communication */
const CO_GTWA_dataType_t *SDOdataType;
@ -398,8 +406,8 @@ typedef struct {
*
* @param gtwa This object will be initialized
* @param SDO_C SDO client object
* @param SDOtimeoutTimeDefault in milliseconds, 500 typically
* @param SDOblockTransferEnableDefault true or false
* @param SDOclientTimeoutTime_ms Default timeout in milliseconds, 500 typically
* @param SDOclientBlockTransfer If true, block transfer will be set by default
* @param NMT NMT object
* @param LSSmaster LSS master object
* @param LEDs LEDs object
@ -410,8 +418,8 @@ typedef struct {
CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
CO_SDOclient_t* SDO_C,
uint16_t SDOtimeoutTimeDefault,
bool_t SDOblockTransferEnableDefault,
uint16_t SDOclientTimeoutTime_ms,
bool_t SDOclientBlockTransfer,
#endif
#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
CO_NMT_t *NMT,
@ -442,7 +450,8 @@ CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
void CO_GTWA_initRead(CO_GTWA_t* gtwa,
size_t (*readCallback)(void *object,
const char *buf,
size_t count),
size_t count,
uint8_t *connectionOK),
void *readCallbackObject);

10
CANopen.c
View file

@ -910,7 +910,9 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co,
const OD_entry_t *OD_statusBits,
CO_NMT_control_t NMTcontrol,
uint16_t firstHBTime_ms,
uint16_t SDOtimeoutTime_ms,
uint16_t SDOserverTimeoutTime_ms,
uint16_t SDOclientTimeoutTime_ms,
bool_t SDOclientBlockTransfer,
uint8_t nodeId)
{
CO_ReturnError_t err;
@ -1024,7 +1026,7 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co,
od,
SDOsrvPar++,
nodeId,
SDOtimeoutTime_ms,
SDOserverTimeoutTime_ms,
co->CANmodule,
CO_GET_CO(RX_IDX_SDO_SRV) + i,
co->CANmodule,
@ -1206,8 +1208,8 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co,
err = CO_GTWA_init(co->gtwa,
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
&co->SDOclient[0],
500,
false,
SDOclientTimeoutTime_ms,
SDOclientBlockTransfer,
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
co->NMT,

10
CANopen.h
View file

@ -519,7 +519,11 @@ CO_ReturnError_t CO_LSSinit(CO_t *co,
* @param OD_statusBits Argument passed to @ref CO_EM_init(). May be NULL.
* @param NMTcontrol Argument passed to @ref CO_NMT_init().
* @param firstHBTime_ms Argument passed to @ref CO_NMT_init().
* @param SDOtimeoutTime_ms Argument passed to @ref CO_SDOserver_init().
* @param SDOserverTimeoutTime_ms Argument passed to @ref CO_SDOserver_init().
* @param SDOclientTimeoutTime_ms Default timeout in milliseconds for SDO
* client, 500 typically. SDO client is configured from CO_GTWA_init().
* @param SDOclientBlockTransfer If true, block transfer will be set in SDO
* client by default. SDO client is configured from by CO_GTWA_init().
* @param nodeId CANopen Node ID (1 ... 127) or 0xFF(unconfigured). In the
* CANopen initialization it is the same as pendingBitRate from CO_LSSinit().
* If it is unconfigured, then some CANopen objects will not be initialized nor
@ -534,7 +538,9 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co,
const OD_entry_t *OD_statusBits,
CO_NMT_control_t NMTcontrol,
uint16_t firstHBTime_ms,
uint16_t SDOtimeoutTime_ms,
uint16_t SDOserverTimeoutTime_ms,
uint16_t SDOclientTimeoutTime_ms,
bool_t SDOclientBlockTransfer,
uint8_t nodeId);

1
Makefile
View file

@ -46,6 +46,7 @@ SOURCES = \
OBJS = $(SOURCES:%.c=%.o)
CC ?= gcc
OPT = -g -DCO_SINGLE_THREAD
#OPT = -g -DCO_SINGLE_THREAD -DCO_CONFIG_DEBUG=0xFFFF
#OPT = -g -pedantic -Wshadow -fanalyzer
#OPT = -g -DCO_USE_GLOBALS
#OPT = -g -DCO_MULTIPLE_OD

6
README.md
View file

@ -63,11 +63,13 @@ Code is documented in header files. Running [doxygen](http://www.doxygen.nl/)
in project base directory will produce complete html documentation.
Just open CANopenNode/doc/html/index.html in the browser. Alternatively browse
documentation [online](https://canopennode.github.io/CANopenSocket/).
Further documented examples are available in [CANopenSocket](https://github.com/CANopenNode/CANopenSocket) project.
Report issues on https://github.com/CANopenNode/CANopenNode/issues
Older and still active discussion group is on Sourceforge
http://sourceforge.net/p/canopennode/discussion/387151/
Join discussion on Slack: https://canopennode.slack.com/
Older discussion group is on Sourceforge: http://sourceforge.net/p/canopennode/discussion/387151/
Contributions are welcome. Best way to contribute your code is to fork
a project, modify it and then send a pull request. Some basic formatting

6
doc/gettingStarted.md
View file

@ -162,7 +162,7 @@ Please be careful when exposing your CANopen network to the outside world, it is
### Next steps
Assigning Node-ID or CAN bitrate, which support LSS configuration, is described in *LSSusage.md*.
Some further CANopenNode related Linux tools are available in [CANopenSocket](https://github.com/CANopenNode/CANopenSocket).
Further CANopenNode related tools and examples are available in [CANopenSocket](https://github.com/CANopenNode/CANopenSocket). Especially interesting is [basicDevice](https://github.com/CANopenNode/CANopenSocket/examples/basicDevice)
Custom CANopen device can be created based on own Object Dictionary, see README.md. There are also many very useful and high quality specifications for different [device profiles](http://www.can-cia.org/standardization/specifications/), some of them are public and free to download, for example CiA401.
@ -170,12 +170,16 @@ For own CANopen device with own microcontroller, see *deviceSupport.md*. There i
Another interesting tool is [CANopen for Python](https://github.com/christiansandberg/canopen).
Examples here worked in virtual CAN interface, for simplicity. Virtual CAN runs inside Linux kernel only, it does not have much practical usability. If one has real CAN network configuration, then above examples are suitable also for this network, if Linux machine is connected to it and CAN interface is properly configured. When connecting your devices to real CAN network, make sure, you have at least two devices communicating, connected with ground and pair of wires, terminated with two 120ohm resistors, correct baudrate, etc.
Accessing real CANopen devices is the same as described above for virtual CAN interface. Some tested USB to CAN interfaces, which are native in Linux kernel are:
- Simple serial [USBtin](http://www.fischl.de/usbtin/) - Start with: `sudo slcand -f -o -c -s8 /dev/ttyACM0 can0; sudo ip link set up can0`
- [EMS CPC-USB](https://www.ems-wuensche.com/?post_type=product&p=746) or [PCAN-USB FD](http://www.peak-system.com/PCAN-USB-FD.365.0.html?&L=1) - Start with: `sudo ip link set up can0 type can bitrate 250000`
- You can get the idea of other supported CAN interfaces in [Linux kernel source](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/net/can) (Kconfig files).
- Raspberry PI or similar has CAN capes available.
Examples here run in Linux, for simplicity. However, real usability of CANopen network is, when simple, microcontroller based devices are connected together with or without more advanced commander device. CANopenNode is basically written for simple microcontrollers and also has more advanced commander features, like above used CANopen gateway with ascii command interface.
Now you can enter the big world of [CANopen devices](http://can-newsletter.org/hardware).
Here we played with virtual CAN interface and result shown as pixels on screen. If you connect a real CAN interface to your computer, things may become dangerous. Keep control and safety on your machines!

10
doc/objectDictionary.md
View file

@ -449,9 +449,13 @@ Other elements listed in the above XML example are required by the standard. The
| - | (1) | TIME_OF_DAY | 0x0C |
| - | (1) | TIME_DIFFERENCE | 0x0D |
| app. specific | BITSTRING (4) | DOMAIN | 0x0F |
(1) Data is translated into OCTET_STRING.<br/>
(2) Additional property "CO_stringLengthMin" indicates the minimum length of the string in CANopenNode.<br/>
(3) Default value for BITSTRING (OCTET_STRING) is written as space separated, two hex digit bytes in little-endian format.<br/>
(1) Data is translated into OCTET_STRING.
(2) Additional property "CO_stringLengthMin" indicates the minimum length of the string in CANopenNode. Property is ignored, if strlen(defaultValue) is larger. Otherwise remaining data locations are filled with zeroes. Strings always have additional element on the end of the array with value=0, which is never written (string is always null terminated). All strings have additional attribute "ODA_STR" in object dictionary. Attribute enables SDO to transfer data of length, which corresponds to actual string length. (Actual size of data read or written by SDO may be smaller than data size of the OD variable.) VISIBLE_STRING may contain control characters and UTF-8 characters, which should work on most target systems (like printf("%s", ...)). So VISIBLE_STRING may be more usable for printing complete set of unicode characters than UNICODE_STRING.
(3) Default value for BITSTRING (OCTET_STRING) is written as space separated, two hex digit bytes in little-endian format.
(4) Default value for DOMAIN is stored as empty string.

12
example/CO_driver_target.h
View file

@ -23,8 +23,8 @@
*/
#ifndef CO_DRIVER_TARGET
#define CO_DRIVER_TARGET
#ifndef CO_DRIVER_TARGET_H
#define CO_DRIVER_TARGET_H
/* This file contains device and application specific definitions.
* It is included from CO_driver.h, which contains documentation
@ -79,7 +79,7 @@ extern "C" {
#endif
#ifndef CO_CONFIG_SDO_SRV_BUFFER_SIZE
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE (127*7)
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE 900
#endif
#ifndef CO_CONFIG_SDO_CLI
@ -92,10 +92,6 @@ extern "C" {
CO_CONFIG_FLAG_OD_DYNAMIC)
#endif
#ifndef CO_CONFIG_SDO_CLI_BUFFER_SIZE
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 1000
#endif
#ifndef CO_CONFIG_TIME
#define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE | \
CO_CONFIG_TIME_PRODUCER | \
@ -264,4 +260,4 @@ typedef struct {
}
#endif /* __cplusplus */
#endif /* CO_DRIVER_TARGET */
#endif /* CO_DRIVER_TARGET_H */

20
socketCAN/CO_driver_target.h
View file

@ -26,8 +26,8 @@
*/
#ifndef CO_DRIVER_TARGET
#define CO_DRIVER_TARGET
#ifndef CO_DRIVER_TARGET_H
#define CO_DRIVER_TARGET_H
/* This file contains device and application specific definitions.
* It is included from CO_driver.h, which contains documentation
@ -103,7 +103,7 @@ extern "C" {
#endif
#ifndef CO_CONFIG_SDO_SRV_BUFFER_SIZE
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE (127*7)
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE 900
#endif
#ifndef CO_CONFIG_SDO_CLI
@ -116,10 +116,6 @@ extern "C" {
CO_CONFIG_FLAG_OD_DYNAMIC)
#endif
#ifndef CO_CONFIG_SDO_CLI_BUFFER_SIZE
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 1000
#endif
#ifndef CO_CONFIG_TIME
#define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE | \
CO_CONFIG_TIME_PRODUCER | \
@ -186,6 +182,14 @@ extern "C" {
#endif
/* Print debug info from some internal parts of the stack */
#if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_COMMON
#include <stdio.h>
#include <syslog.h>
#define CO_DEBUG_COMMON(msg) log_printf(LOG_DEBUG, DBG_CO_DEBUG, msg);
#endif
/**
* @defgroup CO_socketCAN_driver_target CO_driver_target.h
* @ingroup CO_socketCAN
@ -495,4 +499,4 @@ bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule,
}
#endif /* __cplusplus */
#endif /* CO_DRIVER_TARGET */
#endif /* CO_DRIVER_TARGET_H */

32
socketCAN/CO_epoll_interface.c
View file

@ -43,6 +43,7 @@
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <signal.h>
#ifndef LISTEN_BACKLOG
#define LISTEN_BACKLOG 50
@ -342,7 +343,11 @@ void CO_epoll_processRT(CO_epoll_t *ep,
/* GATEWAY ********************************************************************/
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
/* write response string from gateway-ascii object */
static size_t gtwa_write_response(void *object, const char *buf, size_t count) {
static size_t gtwa_write_response(void *object,
const char *buf,
size_t count,
uint8_t *connectionOK)
{
int* fd = (int *)object;
/* nWritten = count -> in case of error (non-existing fd) data are purged */
size_t nWritten = count;
@ -353,9 +358,14 @@ static size_t gtwa_write_response(void *object, const char *buf, size_t count) {
nWritten = (size_t)n;
}
else {
/* probably EAGAIN - "Resource temporarily unavailable". Retry. */
log_printf(LOG_DEBUG, DBG_ERRNO, "write(gtwa_response)");
nWritten = 0;
}
}
else {
*connectionOK = 0;
}
return nWritten;
}
@ -424,6 +434,14 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw,
return CO_ERROR_SYSCALL;
}
/* Ignore the SIGPIPE signal, which may happen, if remote client broke
* the connection. Signal may be triggered by write call inside
* gtwa_write_response() function */
if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) {
log_printf(LOG_CRIT, DBG_ERRNO, "signal");
return CO_ERROR_SYSCALL;
}
log_printf(LOG_INFO, DBG_COMMAND_LOCAL_INFO, localSocketPath);
}
else if (commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN &&
@ -460,6 +478,14 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw,
return CO_ERROR_SYSCALL;
}
/* Ignore the SIGPIPE signal, which may happen, if remote client broke
* the connection. Signal may be triggered by write call inside
* gtwa_write_response() function */
if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) {
log_printf(LOG_CRIT, DBG_ERRNO, "signal");
return CO_ERROR_SYSCALL;
}
log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, commandInterface);
}
else {
@ -579,8 +605,8 @@ void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw,
ssize_t s = read(epGtw->gtwa_fd, buf, space);
if (co->nodeIdUnconfigured) {
/* purge data */
if (space == 0 || co->nodeIdUnconfigured) {
/* continue or purge data */
}
else if (s < 0 && errno != EAGAIN) {
log_printf(LOG_DEBUG, DBG_ERRNO, "read(gtwa_fd)");

1
socketCAN/CO_error_msgs.h
View file

@ -59,6 +59,7 @@ extern "C" {
*/
#define DBG_GENERAL "(%s) Error: %s%d", __func__
#define DBG_ERRNO "(%s) OS error \"%s\" in %s", __func__, strerror(errno)
#define DBG_CO_DEBUG "(%s) CO_DEBUG: %s", __func__
#define DBG_CAN_TX_FAILED "(%s) Transmitting CAN msg OID 0x%08x failed(%s)", __func__
#define DBG_CAN_RX_PARAM_FAILED "(%s) Setting CAN rx buffer failed (%s)", __func__
#define DBG_CAN_RX_FAILED "(%s) Receiving CAN msg failed (%s)", __func__

95
socketCAN/CO_main_basic.c
View file

@ -1,14 +1,13 @@
/*
* CANopen main program file for Linux SocketCAN.
* CANopen main program file for CANopenNode on Linux.
*
* @file main.c
* @file CO_main_basic.c
* @author Janez Paternoster
* @copyright 2015 - 2020 Janez Paternoster
* @copyright 2020 Janez Paternoster
*
* This file is part of CANopenSocket, a Linux implementation of CANopen
* stack with master functionality. Project home page is
* <https://github.com/CANopenNode/CANopenSocket>. CANopenSocket is based
* on CANopenNode: <https://github.com/CANopenNode/CANopenNode>.
* This file is part of CANopenNode, an opensource CANopen Stack.
* Project home page is <https://github.com/CANopenNode/CANopenNode>.
* For more information on CANopen see <http://www.can-cia.org/>.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
@ -30,6 +29,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <bits/getopt_core.h>
#include <string.h>
#include <sched.h>
#include <signal.h>
@ -51,9 +51,8 @@
#endif
/* Call external application functions. */
#if __has_include("CO_application.h")
#ifdef CO_USE_APPLICATION
#include "CO_application.h"
#define CO_USE_APPLICATION
#endif
/* Add trace functionality for recording variables over time */
@ -86,12 +85,21 @@
#ifndef FIRST_HB_TIME
#define FIRST_HB_TIME 500
#endif
#ifndef SDO_TIMEOUT_TIME
#define SDO_TIMEOUT_TIME 1000
#ifndef SDO_SRV_TIMEOUT_TIME
#define SDO_SRV_TIMEOUT_TIME 1000
#endif
#ifndef SDO_CLI_TIMEOUT_TIME
#define SDO_CLI_TIMEOUT_TIME 500
#endif
#ifndef SDO_CLI_BLOCK
#define SDO_CLI_BLOCK false
#endif
#ifndef GATEWAY_ENABLE
#define GATEWAY_ENABLE true
#endif
#ifndef OD_STATUS_BITS
#define OD_STATUS_BITS NULL
#endif
/* Other variables and objects */
#ifndef CO_SINGLE_THREAD
@ -153,6 +161,7 @@ void log_printf(int priority, const char *format, ...) {
#endif
}
#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
/* callback for emergency messages */
static void EmergencyRxCallback(const uint16_t ident,
const uint16_t errorCode,
@ -165,7 +174,10 @@ static void EmergencyRxCallback(const uint16_t ident,
log_printf(LOG_NOTICE, DBG_EMERGENCY_RX, nodeIdRx, errorCode,
errorRegister, errorBit, infoCode);
}
#endif
#if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE) \
|| ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE)
/* return string description of NMT state. */
static char *NmtState2Str(CO_NMT_internalState_t state)
{
@ -177,12 +189,15 @@ static char *NmtState2Str(CO_NMT_internalState_t state)
default: return "unknown";
}
}
#endif
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
/* callback for NMT change messages */
static void NmtChangedCallback(CO_NMT_internalState_t state)
{
log_printf(LOG_NOTICE, DBG_NMT_CHANGE, NmtState2Str(state), state);
}
#endif
#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE
/* callback for monitoring Heartbeat remote NMT state change */
@ -252,6 +267,7 @@ printf(
* Mainline thread
******************************************************************************/
int main (int argc, char *argv[]) {
int programExit = EXIT_SUCCESS;
CO_epoll_t epMain;
#ifndef CO_SINGLE_THREAD
pthread_t rt_thread_id;
@ -290,9 +306,6 @@ int main (int argc, char *argv[]) {
exit(EXIT_SUCCESS);
}
while((opt = getopt(argc, argv, "i:p:rc:T:s:a:")) != -1) {
const char comm_stdio[] = "stdio";
const char comm_local[] = "local-";
const char comm_tcp[] = "tcp-";
switch (opt) {
case 'i':
CO_pendingNodeId = (uint8_t)strtol(optarg, NULL, 0);
@ -305,7 +318,10 @@ int main (int argc, char *argv[]) {
case 'r': rebootEnable = true;
break;
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
case 'c':
case 'c': {
const char *comm_stdio = "stdio";
const char *comm_local = "local-";
const char *comm_tcp = "tcp-";
if (strcmp(optarg, comm_stdio) == 0) {
commandInterface = CO_COMMAND_IF_STDIO;
}
@ -328,6 +344,7 @@ int main (int argc, char *argv[]) {
exit(EXIT_FAILURE);
}
break;
}
case 'T':
socketTimeout_ms = strtoul(optarg, NULL, 0);
break;
@ -472,7 +489,9 @@ int main (int argc, char *argv[]) {
err = CO_CANinit(CO, (void *)&CANptr, 0 /* bit rate not used */);
if(err != CO_ERROR_NO) {
log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANinit()", err);
exit(EXIT_FAILURE);
programExit = EXIT_FAILURE;
CO_endProgram = 1;
continue;
}
CO_LSS_address_t lssAddress = {.identity = {
@ -485,7 +504,9 @@ int main (int argc, char *argv[]) {
&CO_pendingNodeId, &CO_pendingBitRate);
if(err != CO_ERROR_NO) {
log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_LSSinit()", err);
exit(EXIT_FAILURE);
programExit = EXIT_FAILURE;
CO_endProgram = 1;
continue;
}
CO_activeNodeId = CO_pendingNodeId;
@ -494,10 +515,12 @@ int main (int argc, char *argv[]) {
NULL, /* alternate NMT */
NULL, /* alternate em */
&OD, /* Object dictionary */
NULL, /* Optional OD_statusBits */
OD_STATUS_BITS, /* Optional OD_statusBits */
NMT_CONTROL, /* CO_NMT_control_t */
FIRST_HB_TIME, /* firstHBTime_ms */
SDO_TIMEOUT_TIME, /* SDOtimeoutTime_ms */
SDO_SRV_TIMEOUT_TIME, /* SDOserverTimeoutTime_ms */
SDO_CLI_TIMEOUT_TIME, /* SDOclientTimeoutTime_ms */
SDO_CLI_BLOCK, /* SDOclientBlockTransfer */
CO_activeNodeId);
if(err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) {
if (err == CO_ERROR_OD_PARAMETERS) {
@ -507,7 +530,9 @@ int main (int argc, char *argv[]) {
else {
log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInit()", err);
}
exit(EXIT_FAILURE);
programExit = EXIT_FAILURE;
CO_endProgram = 1;
continue;
}
/* initialize part of threadMain and callbacks */
@ -520,8 +545,12 @@ int main (int argc, char *argv[]) {
LSScfgStoreCallback);
#endif
if(!CO->nodeIdUnconfigured) {
#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
CO_EM_initCallbackRx(CO->em, EmergencyRxCallback);
#endif
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
CO_NMT_initCallbackChanged(CO->NMT, NmtChangedCallback);
#endif
#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE
CO_HBconsumer_initCallbackNmtChanged(CO->HBcons, NULL,
HeartbeatNmtChangedCallback);
@ -555,7 +584,9 @@ int main (int argc, char *argv[]) {
/* Create rt_thread and set priority */
if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "pthread_create(rt_thread)");
exit(EXIT_FAILURE);
programExit = EXIT_FAILURE;
CO_endProgram = 1;
continue;
}
if(rtPriority > 0) {
struct sched_param param;
@ -563,13 +594,27 @@ int main (int argc, char *argv[]) {
param.sched_priority = rtPriority;
if (pthread_setschedparam(rt_thread_id, SCHED_FIFO, &param) != 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "pthread_setschedparam()");
exit(EXIT_FAILURE);
programExit = EXIT_FAILURE;
CO_endProgram = 1;
continue;
}
}
#endif
#ifdef CO_USE_APPLICATION
/* Execute optional additional application code */
app_programStart(!CO->nodeIdUnconfigured);
uint16_t errinfo = 0;
err = app_programStart(!CO->nodeIdUnconfigured, &errinfo);
if(err != CO_ERROR_NO) {
if (err == CO_ERROR_OD_PARAMETERS) {
log_printf(LOG_CRIT, DBG_OD_ENTRY, errinfo);
}
else {
log_printf(LOG_CRIT, DBG_CAN_OPEN, "app_programStart()", err);
}
programExit = EXIT_FAILURE;
CO_endProgram = 1;
continue;
}
#endif
} /* if(firstRun) */
@ -654,7 +699,7 @@ int main (int argc, char *argv[]) {
}
}
exit(EXIT_SUCCESS);
exit(programExit);
}
#ifndef CO_SINGLE_THREAD
@ -680,7 +725,7 @@ static void* rt_thread(void* arg) {
#ifdef CO_USE_APPLICATION
/* Execute optional additional application code */
app_program1ms(!CO->nodeIdUnconfigured);
app_program1ms(!CO->nodeIdUnconfigured, epRT.timeDifference_us);
#endif
}