/* * CANopen Service Data Object - client. * * @file CO_SDOclient.c * @ingroup CO_SDOclient * @author Janez Paternoster * @author Matej Severkar * @copyright 2020 Janez Paternoster * * This file is part of CANopenNode, an opensource CANopen Stack. * Project home page is . * For more information on CANopen see . * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "301/CO_SDOclient.h" #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE /* verify configuration */ #if CO_CONFIG_SDO_CLI_BUFFER_SIZE < 7 #error CO_CONFIG_SDO_CLI_BUFFER_SIZE must be set to 7 or more. #endif #if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE) #error CO_CONFIG_FIFO_ENABLE must be enabled. #endif #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK #if !((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED) #error CO_CONFIG_SDO_CLI_SEGMENTED must be enabled. #endif #if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ) #error CO_CONFIG_FIFO_ALT_READ must be enabled. #endif #if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_CRC16_CCITT) #error CO_CONFIG_FIFO_CRC16_CCITT must be enabled. #endif #endif /* default 'protocol switch threshold' size for block transfer */ #ifndef CO_CONFIG_SDO_CLI_PST #define CO_CONFIG_SDO_CLI_PST 21 #endif /* * Read received message from CAN module. * * Function will be called (by CAN receive interrupt) every time, when CAN * message with correct identifier will be received. For more information and * 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*)object; uint8_t DLC = CO_CANrxMsg_readDLC(msg); uint8_t *data = CO_CANrxMsg_readData(msg); /* 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) || data[0] == 0x80) ) { #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK 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); CO_FLAG_SET(SDO_C->CANrxNew); #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE /* Optional signal to RTOS, which can resume task, which handles * SDO client processing. */ if (SDO_C->pFunctSignal != NULL) { SDO_C->pFunctSignal(SDO_C->functSignalObject); } #endif #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK } 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 = data[0] & 0x7F; SDO_C->timeoutTimer = 0; SDO_C->block_timeoutTimer = 0; /* verify if sequence number is correct */ if (seqno <= SDO_C->block_blksize && seqno == (SDO_C->block_seqno + 1) ) { SDO_C->block_seqno = seqno; /* is this the last segment? */ if ((data[0] & 0x80) != 0) { /* copy data to temporary buffer, because we don't know the * number of bytes not containing data */ memcpy((void *)&SDO_C->block_dataUploadLast[0], (const void *)&data[1], 7); SDO_C->finished = true; state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP; } else { /* Copy data. There is always enough space in fifo buffer, * because block_blksize was calculated before */ CO_fifo_write(&SDO_C->bufFifo, (const char *)&data[1], 7, &SDO_C->block_crc); SDO_C->sizeTran += 7; /* all segments in sub-block has been transferred */ if (seqno == SDO_C->block_blksize) { state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP; } } } /* 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) { /* 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 /* Optional signal to RTOS, which can resume task, which handles * SDO client processing. */ if (SDO_C->pFunctSignal != NULL) { SDO_C->pFunctSignal(SDO_C->functSignalObject); } #endif } } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */ } } /******************************************************************************/ CO_ReturnError_t CO_SDOclient_init(CO_SDOclient_t *SDO_C, void *SDO, CO_SDOclientPar_t *SDOClientPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx) { /* verify arguments */ if (SDO_C == NULL || SDOClientPar == NULL || SDOClientPar->maxSubIndex != 3 || CANdevRx==NULL || CANdevTx==NULL ) { return CO_ERROR_ILLEGAL_ARGUMENT; } /* Configure object variables */ #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL SDO_C->SDO = (CO_SDO_t *)SDO; #endif SDO_C->SDOClientPar = SDOClientPar; SDO_C->CANdevRx = CANdevRx; SDO_C->CANdevRxIdx = CANdevRxIdx; SDO_C->CANdevTx = CANdevTx; SDO_C->CANdevTxIdx = CANdevTxIdx; #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE SDO_C->pFunctSignal = NULL; SDO_C->functSignalObject = NULL; #endif /* prepare circular fifo buffer */ CO_fifo_init(&SDO_C->bufFifo, SDO_C->buf, CO_CONFIG_SDO_CLI_BUFFER_SIZE + 1); SDO_C->state = CO_SDO_ST_IDLE; CO_FLAG_CLEAR(SDO_C->CANrxNew); SDO_C->COB_IDClientToServerPrev = 0; SDO_C->COB_IDServerToClientPrev = 0; CO_SDOclient_setup(SDO_C, SDO_C->SDOClientPar->COB_IDClientToServer, SDO_C->SDOClientPar->COB_IDServerToClient, SDO_C->SDOClientPar->nodeIDOfTheSDOServer); return CO_ERROR_NO; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE /******************************************************************************/ void CO_SDOclient_initCallbackPre(CO_SDOclient_t *SDOclient, void *object, void (*pFunctSignal)(void *object)) { if (SDOclient != NULL) { SDOclient->functSignalObject = object; SDOclient->pFunctSignal = pFunctSignal; } } #endif /******************************************************************************/ CO_SDO_return_t CO_SDOclient_setup(CO_SDOclient_t *SDO_C, uint32_t COB_IDClientToServer, uint32_t COB_IDServerToClient, uint8_t nodeIDOfTheSDOServer) { uint32_t idCtoS, idStoC; uint8_t idNode; /* verify parameters */ if (SDO_C == NULL || (COB_IDClientToServer & 0x7FFFF800L) != 0 || (COB_IDServerToClient & 0x7FFFF800L) != 0 || nodeIDOfTheSDOServer > 127 ) { return CO_SDO_RT_wrongArguments; } /* Configure object variables */ SDO_C->state = CO_SDO_ST_IDLE; CO_FLAG_CLEAR(SDO_C->CANrxNew); /* setup Object Dictionary variables */ if ((COB_IDClientToServer & 0x80000000L) != 0 || (COB_IDServerToClient & 0x80000000L) != 0 || nodeIDOfTheSDOServer == 0 ) { /* SDO is NOT used */ idCtoS = 0x80000000L; idStoC = 0x80000000L; idNode = 0; } else { if (COB_IDClientToServer == 0 || COB_IDServerToClient == 0) { idCtoS = 0x600 + nodeIDOfTheSDOServer; idStoC = 0x580 + nodeIDOfTheSDOServer; } else { idCtoS = COB_IDClientToServer; idStoC = COB_IDServerToClient; } idNode = nodeIDOfTheSDOServer; } SDO_C->SDOClientPar->COB_IDClientToServer = idCtoS; SDO_C->SDOClientPar->COB_IDServerToClient = idStoC; SDO_C->SDOClientPar->nodeIDOfTheSDOServer = idNode; /* configure SDO client CAN reception, if differs. */ if (SDO_C->COB_IDClientToServerPrev != idCtoS || SDO_C->COB_IDServerToClientPrev != idStoC ) { CO_ReturnError_t ret = CO_CANrxBufferInit( SDO_C->CANdevRx, /* CAN device */ SDO_C->CANdevRxIdx, /* rx buffer index */ (uint16_t)idStoC, /* CAN identifier */ 0x7FF, /* mask */ 0, /* rtr */ (void*)SDO_C, /* object passed to receive function */ CO_SDOclient_receive); /* this function will process rx msg */ /* configure SDO client CAN transmission */ SDO_C->CANtxBuff = CO_CANtxBufferInit( SDO_C->CANdevTx, /* CAN device */ SDO_C->CANdevTxIdx, /* index of buffer inside CAN module */ (uint16_t)idCtoS, /* CAN identifier */ 0, /* rtr */ 8, /* number of data bytes */ 0); /* synchronous message flag bit */ SDO_C->COB_IDClientToServerPrev = idCtoS; SDO_C->COB_IDServerToClientPrev = idStoC; if (ret != CO_ERROR_NO || SDO_C->CANtxBuff == NULL) { return CO_SDO_RT_wrongArguments; } } return CO_SDO_RT_ok_communicationEnd; } /****************************************************************************** * DOWNLOAD * ******************************************************************************/ CO_SDO_return_t CO_SDOclientDownloadInitiate(CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, size_t sizeIndicated, uint16_t SDOtimeoutTime_ms, bool_t blockEnable) { /* verify parameters */ if (SDO_C == NULL) { return CO_SDO_RT_wrongArguments; } /* save parameters */ SDO_C->index = index; SDO_C->subIndex = subIndex; SDO_C->sizeInd = sizeIndicated; SDO_C->sizeTran = 0; SDO_C->finished = false; SDO_C->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000; SDO_C->timeoutTimer = 0; CO_fifo_reset(&SDO_C->bufFifo); #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL /* if node-ID of the SDO server is the same as node-ID of this node, then * transfer data within this node */ if (SDO_C->SDO != NULL && SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId ) { SDO_C->state = CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER; } else #endif #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK if (blockEnable && (sizeIndicated == 0 || sizeIndicated > CO_CONFIG_SDO_CLI_PST) ) { SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ; } else #endif { SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ; } CO_FLAG_CLEAR(SDO_C->CANrxNew); return CO_SDO_RT_ok_communicationEnd; } void CO_SDOclientDownloadInitiateSize(CO_SDOclient_t *SDO_C, size_t sizeIndicated) { if (SDO_C != NULL) { SDO_C->sizeInd = sizeIndicated; #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK if (SDO_C->state == CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ && sizeIndicated > 0 && sizeIndicated <= CO_CONFIG_SDO_CLI_PST ) { SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ; } #endif } } /******************************************************************************/ size_t CO_SDOclientDownloadBufWrite(CO_SDOclient_t *SDO_C, const char *buf, size_t count) { size_t ret = 0; if (SDO_C != NULL && buf != NULL) { ret = CO_fifo_write(&SDO_C->bufFifo, buf, count, NULL); } return ret; } /******************************************************************************/ CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, bool_t bufferPartial, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeTransferred, uint32_t *timerNext_us) { (void)timerNext_us; (void) bufferPartial; /* may be unused */ CO_SDO_return_t ret = CO_SDO_RT_waitingResponse; CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE; if (SDO_C == NULL) { abortCode = CO_SDO_AB_DEVICE_INCOMPAT; ret = CO_SDO_RT_wrongArguments; } else if (SDO_C->state == CO_SDO_ST_IDLE) { ret = CO_SDO_RT_ok_communicationEnd; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL /* Transfer data locally **************************************************/ else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER && !abort) { if (SDO_C->SDO->state != CO_SDO_ST_IDLE) { abortCode = CO_SDO_AB_DEVICE_INCOMPAT; ret = CO_SDO_RT_endedWithClientAbort; } else { /* Max data size is limited to fifo size (for now). */ size_t count = CO_fifo_getOccupied(&SDO_C->bufFifo); if (SDO_C->sizeInd > 0 && SDO_C->sizeInd != count) { abortCode = CO_SDO_AB_TYPE_MISMATCH; ret = CO_SDO_RT_endedWithClientAbort; } else { /* init ODF_arg */ abortCode = CO_SDO_initTransfer(SDO_C->SDO, SDO_C->index, SDO_C->subIndex); if (abortCode == CO_SDO_AB_NONE) { /* set buffer and write data to the Object dictionary */ SDO_C->SDO->ODF_arg.data = (uint8_t *)SDO_C->buf; abortCode = CO_SDO_writeOD(SDO_C->SDO, count); } if (abortCode == CO_SDO_AB_NONE) { SDO_C->sizeTran = count; ret = CO_SDO_RT_ok_communicationEnd; } else { ret = CO_SDO_RT_endedWithServerAbort; } } } SDO_C->state = CO_SDO_ST_IDLE; } #endif /* CO_CONFIG_SDO_CLI_LOCAL */ /* CAN data received ******************************************************/ else if (CO_FLAG_READ(SDO_C->CANrxNew)) { /* is SDO abort */ if (SDO_C->CANrxData[0] == 0x80) { uint32_t code; memcpy(&code, &SDO_C->CANrxData[4], sizeof(code)); abortCode = (CO_SDO_abortCode_t)CO_SWAP_32(code); 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) { /* verify index and subindex */ uint16_t index; uint8_t subindex; index = ((uint16_t) SDO_C->CANrxData[2]) << 8; index |= SDO_C->CANrxData[1]; subindex = SDO_C->CANrxData[3]; if (index != SDO_C->index || subindex != SDO_C->subIndex) { abortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = CO_SDO_ST_ABORT; break; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED if (SDO_C->finished) { /* expedited transfer */ SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; } else { /* segmented transfer - prepare the first segment */ SDO_C->toggle = 0x00; SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ; } #else /* expedited transfer */ SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; #endif } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP: { if ((SDO_C->CANrxData[0] & 0xEF) == 0x20) { /* verify and alternate toggle bit */ uint8_t toggle = SDO_C->CANrxData[0] & 0x10; if (toggle != SDO_C->toggle) { abortCode = CO_SDO_AB_TOGGLE_BIT; SDO_C->state = CO_SDO_ST_ABORT; break; } SDO_C->toggle = (toggle == 0x00) ? 0x10 : 0x00; /* is end of transfer? */ if (SDO_C->finished) { SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; } else { SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ; } } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */ #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP: { if ((SDO_C->CANrxData[0] & 0xFB) == 0xA0) { /* verify index and subindex */ uint16_t index; uint8_t subindex; index = ((uint16_t) SDO_C->CANrxData[2]) << 8; index |= SDO_C->CANrxData[1]; subindex = SDO_C->CANrxData[3]; if (index != SDO_C->index || subindex != SDO_C->subIndex) { abortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = CO_SDO_ST_ABORT; break; } SDO_C->block_crc = 0; SDO_C->block_blksize = SDO_C->CANrxData[4]; if (SDO_C->block_blksize < 1 || SDO_C->block_blksize > 127) SDO_C->block_blksize = 127; SDO_C->block_seqno = 0; CO_fifo_altBegin(&SDO_C->bufFifo, 0); SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ; } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ: case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP: { if (SDO_C->CANrxData[0] == 0xA2) { /* check number of segments */ if (SDO_C->CANrxData[1] < SDO_C->block_seqno) { /* NOT all segments transferred successfully. * Re-transmit data after erroneous segment. */ CO_fifo_altBegin(&SDO_C->bufFifo, (size_t)SDO_C->CANrxData[1] * 7); SDO_C->finished = false; } else if (SDO_C->CANrxData[1] > SDO_C->block_seqno) { /* something strange from server, break transmission */ abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; break; } /* confirm successfully transmitted data */ CO_fifo_altFinish(&SDO_C->bufFifo, &SDO_C->block_crc); if (SDO_C->finished) { SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_END_REQ; } else { SDO_C->block_blksize = SDO_C->CANrxData[2]; SDO_C->block_seqno = 0; CO_fifo_altBegin(&SDO_C->bufFifo, 0); SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ; } } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } case CO_SDO_ST_DOWNLOAD_BLK_END_RSP: { if (SDO_C->CANrxData[0] == 0xA1) { /* SDO block download successfully transferred */ SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */ default: { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; break; } } SDO_C->timeoutTimer = 0; 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) { if (SDO_C->timeoutTimer < SDO_C->SDOtimeoutTime_us) { SDO_C->timeoutTimer += timeDifference_us; } if (SDO_C->timeoutTimer >= SDO_C->SDOtimeoutTime_us) { abortCode = CO_SDO_AB_TIMEOUT; SDO_C->state = CO_SDO_ST_ABORT; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT else if (timerNext_us != NULL) { /* check again after timeout time elapsed */ uint32_t diff = SDO_C->SDOtimeoutTime_us - SDO_C->timeoutTimer; if (*timerNext_us > diff) { *timerNext_us = diff; } } #endif if (SDO_C->CANtxBuff->bufferFull) { ret = CO_SDO_RT_transmittBufferFull; } } /* Transmit CAN data ******************************************************/ if (ret == CO_SDO_RT_waitingResponse) { size_t count; memset((void *)&SDO_C->CANtxBuff->data[0], 0, 8); switch (SDO_C->state) { case CO_SDO_ST_DOWNLOAD_INITIATE_REQ: { SDO_C->CANtxBuff->data[0] = 0x20; SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index; SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8); SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; /* get count of data bytes to transfer */ count = CO_fifo_getOccupied(&SDO_C->bufFifo); /* is expedited transfer, <= 4bytes of data */ if ((SDO_C->sizeInd == 0 && count <= 4) || (SDO_C->sizeInd > 0 && SDO_C->sizeInd <= 4) ) { SDO_C->CANtxBuff->data[0] |= 0x02; /* verify length, indicate data size */ if (count == 0 || (SDO_C->sizeInd > 0 && SDO_C->sizeInd != count) ) { SDO_C->state = CO_SDO_ST_IDLE; abortCode = CO_SDO_AB_TYPE_MISMATCH; ret = CO_SDO_RT_endedWithClientAbort; break; } if (SDO_C->sizeInd > 0) { SDO_C->CANtxBuff->data[0] |= 0x01 | ((4 - count) << 2); } /* copy data */ CO_fifo_read(&SDO_C->bufFifo, (char *)&SDO_C->CANtxBuff->data[4], count, NULL); SDO_C->sizeTran = count; SDO_C->finished = true; } else { #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED /* segmented transfer, indicate data size */ if (SDO_C->sizeInd > 0) { uint32_t size = CO_SWAP_32(SDO_C->sizeInd); SDO_C->CANtxBuff->data[0] |= 0x01; memcpy(&SDO_C->CANtxBuff->data[4], &size, sizeof(size)); } #else SDO_C->state = CO_SDO_ST_IDLE; abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS; ret = CO_SDO_RT_endedWithClientAbort; break; #endif } /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_RSP; break; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ: { /* fill data bytes */ count = CO_fifo_read(&SDO_C->bufFifo, (char *)&SDO_C->CANtxBuff->data[1], 7, NULL); /* verify if sizeTran is too large */ SDO_C->sizeTran += count; if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd) { SDO_C->sizeTran -= count; abortCode = CO_SDO_AB_DATA_LONG; SDO_C->state = CO_SDO_ST_ABORT; break; } /* SDO command specifier */ SDO_C->CANtxBuff->data[0] = SDO_C->toggle | ((7 - count) << 1); /* is end of transfer? Verify also sizeTran */ if (CO_fifo_getOccupied(&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; break; } SDO_C->CANtxBuff->data[0] |= 0x01; SDO_C->finished = true; } /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_RSP; break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */ #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ: { SDO_C->CANtxBuff->data[0] = 0xC4; SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index; SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8); SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; /* indicate data size */ if (SDO_C->sizeInd > 0) { uint32_t size = CO_SWAP_32(SDO_C->sizeInd); SDO_C->CANtxBuff->data[0] |= 0x02; memcpy(&SDO_C->CANtxBuff->data[4], &size, sizeof(size)); } /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP; break; } 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; /* 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; /* verify if sizeTran is too large */ SDO_C->sizeTran += count; if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd) { SDO_C->sizeTran -= count; abortCode = CO_SDO_AB_DATA_LONG; SDO_C->state = CO_SDO_ST_ABORT; break; } /* is end of transfer? Verify also sizeTran */ 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; break; } SDO_C->CANtxBuff->data[0] |= 0x80; SDO_C->finished = true; SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP; } /* are all segments in current block transferred? */ else if (SDO_C->block_seqno >= SDO_C->block_blksize) { SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT else { /* Inform OS to call this function again without delay. */ if (timerNext_us != NULL) { *timerNext_us = 0; } } #endif /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } case CO_SDO_ST_DOWNLOAD_BLK_END_REQ: { SDO_C->CANtxBuff->data[0] = 0xC1 | (SDO_C->block_noData << 2); SDO_C->CANtxBuff->data[1] = (uint8_t) SDO_C->block_crc; SDO_C->CANtxBuff->data[2] = (uint8_t) (SDO_C->block_crc >> 8); /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_END_RSP; break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */ default: { break; } } } if (ret == CO_SDO_RT_waitingResponse) { if (SDO_C->state == CO_SDO_ST_ABORT) { uint32_t code = CO_SWAP_32((uint32_t)abortCode); /* Send SDO abort message */ SDO_C->CANtxBuff->data[0] = 0x80; SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index; SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8); SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; memcpy(&SDO_C->CANtxBuff->data[4], &code, sizeof(code)); CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_endedWithClientAbort; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ) { ret = CO_SDO_RT_blockDownldInProgress; } #endif } if (sizeTransferred != NULL) { *sizeTransferred = SDO_C->sizeTran; } if (SDOabortCode != NULL) { *SDOabortCode = abortCode; } return ret; } /****************************************************************************** * UPLOAD * ******************************************************************************/ CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, uint16_t SDOtimeoutTime_ms, bool_t blockEnable) { /* verify parameters */ if (SDO_C == NULL) { return CO_SDO_RT_wrongArguments; } /* save parameters */ SDO_C->index = index; SDO_C->subIndex = subIndex; SDO_C->sizeInd = 0; SDO_C->sizeTran = 0; SDO_C->finished = false; CO_fifo_reset(&SDO_C->bufFifo); 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 * 700; #endif #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL /* if node-ID of the SDO server is the same as node-ID of this node, then * transfer data within this node */ if (SDO_C->SDO != NULL && SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId ) { SDO_C->state = CO_SDO_ST_UPLOAD_LOCAL_TRANSFER; } else #endif #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK if (blockEnable) { SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ; } else #endif { SDO_C->state = CO_SDO_ST_UPLOAD_INITIATE_REQ; } CO_FLAG_CLEAR(SDO_C->CANrxNew); return CO_SDO_RT_ok_communicationEnd; } /******************************************************************************/ 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, uint32_t *timerNext_us) { (void)timerNext_us; /* may be unused */ CO_SDO_return_t ret = CO_SDO_RT_waitingResponse; CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE; if (SDO_C == NULL) { abortCode = CO_SDO_AB_DEVICE_INCOMPAT; ret = CO_SDO_RT_wrongArguments; } else if (SDO_C->state == CO_SDO_ST_IDLE) { ret = CO_SDO_RT_ok_communicationEnd; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL /* Transfer data locally **************************************************/ else if (SDO_C->state == CO_SDO_ST_UPLOAD_LOCAL_TRANSFER && !abort) { if (SDO_C->SDO->state != 0) { abortCode = CO_SDO_AB_DEVICE_INCOMPAT; ret = CO_SDO_RT_endedWithClientAbort; } else { /* Max data size is limited to fifo size (for now). */ size_t count = CO_fifo_getSpace(&SDO_C->bufFifo); /* init ODF_arg */ abortCode = CO_SDO_initTransfer(SDO_C->SDO, SDO_C->index, SDO_C->subIndex); if (abortCode == CO_SDO_AB_NONE) { /* set buffer and read data from the Object dictionary */ SDO_C->SDO->ODF_arg.data = (uint8_t *)SDO_C->buf; if (SDO_C->SDO->ODF_arg.ODdataStorage == 0) { /* set length if domain */ SDO_C->SDO->ODF_arg.dataLength = count; } abortCode = CO_SDO_readOD(SDO_C->SDO, count); } if (abortCode == CO_SDO_AB_NONE) { /* is SDO buffer too small */ if (SDO_C->SDO->ODF_arg.lastSegment == 0) { abortCode = CO_SDO_AB_OUT_OF_MEM; /* Out of memory */ ret = CO_SDO_RT_endedWithServerAbort; } else { SDO_C->sizeTran = (size_t)SDO_C->SDO->ODF_arg.dataLength; /* fifo was written directly, indicate data size manually */ SDO_C->bufFifo.writePtr = SDO_C->sizeTran; ret = CO_SDO_RT_ok_communicationEnd; } } else { ret = CO_SDO_RT_endedWithServerAbort; } } SDO_C->state = CO_SDO_ST_IDLE; } #endif /* CO_CONFIG_SDO_CLI_LOCAL */ /* CAN data received ******************************************************/ else if (CO_FLAG_READ(SDO_C->CANrxNew)) { /* is SDO abort */ if (SDO_C->CANrxData[0] == 0x80) { uint32_t code; memcpy(&code, &SDO_C->CANrxData[4], sizeof(code)); abortCode = (CO_SDO_abortCode_t)CO_SWAP_32(code); 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) { /* verify index and subindex */ uint16_t index; uint8_t subindex; index = ((uint16_t) SDO_C->CANrxData[2]) << 8; index |= SDO_C->CANrxData[1]; subindex = SDO_C->CANrxData[3]; if (index != SDO_C->index || subindex != SDO_C->subIndex) { abortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = CO_SDO_ST_ABORT; break; } if (SDO_C->CANrxData[0] & 0x02) { /* Expedited transfer */ size_t count = 4; /* is size indicated? */ if (SDO_C->CANrxData[0] & 0x01) { count -= (SDO_C->CANrxData[0] >> 2) & 0x03; } /* copy data, indicate size and finish */ CO_fifo_write(&SDO_C->bufFifo, (const char *)&SDO_C->CANrxData[4], count, NULL); SDO_C->sizeTran = count; SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; } else { #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED /* segmented transfer, is size indicated? */ if (SDO_C->CANrxData[0] & 0x01) { uint32_t size; memcpy(&size, &SDO_C->CANrxData[4], sizeof(size)); SDO_C->sizeInd = CO_SWAP_32(size); } SDO_C->toggle = 0x00; SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ; #else abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS; SDO_C->state = CO_SDO_ST_ABORT; #endif } } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED case CO_SDO_ST_UPLOAD_SEGMENT_RSP: { if ((SDO_C->CANrxData[0] & 0xE0) == 0x00) { size_t count, countWr; /* verify and alternate toggle bit */ uint8_t toggle = SDO_C->CANrxData[0] & 0x10; if (toggle != SDO_C->toggle) { abortCode = CO_SDO_AB_TOGGLE_BIT; SDO_C->state = CO_SDO_ST_ABORT; break; } SDO_C->toggle = (toggle == 0x00) ? 0x10 : 0x00; /* get data size and write data to the buffer */ count = 7 - ((SDO_C->CANrxData[0] >> 1) & 0x07); countWr = CO_fifo_write(&SDO_C->bufFifo, (const char *)&SDO_C->CANrxData[1], count, NULL); SDO_C->sizeTran += countWr; /* verify, if there was not enough space in fifo buffer */ if (countWr != count) { abortCode = CO_SDO_AB_OUT_OF_MEM; SDO_C->state = CO_SDO_ST_ABORT; break; } /* verify if size of data uploaded is too large */ if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd ) { abortCode = CO_SDO_AB_DATA_LONG; SDO_C->state = CO_SDO_ST_ABORT; break; } /* If no more segments to be upload, finish */ if (SDO_C->CANrxData[0] & 0x01) { /* verify size of data uploaded */ if (SDO_C->sizeInd > 0 && SDO_C->sizeTran < SDO_C->sizeInd ) { abortCode = CO_SDO_AB_DATA_SHORT; SDO_C->state = CO_SDO_ST_ABORT; } else { SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; } } else { SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ; } } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */ #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP: { if ((SDO_C->CANrxData[0] & 0xF9) == 0xC0) { uint16_t index; uint8_t subindex; /* get server CRC support info and data size */ if ((SDO_C->CANrxData[0] & 0x04) != 0) { SDO_C->block_crcEnabled = true; } else { SDO_C->block_crcEnabled = false; } if (SDO_C->CANrxData[0] & 0x02) { uint32_t size; memcpy(&size, &SDO_C->CANrxData[4], sizeof(size)); SDO_C->sizeInd = CO_SWAP_32(size); } /* verify index and subindex */ index = ((uint16_t) SDO_C->CANrxData[2]) << 8; index |= SDO_C->CANrxData[1]; subindex = SDO_C->CANrxData[3]; if (index != SDO_C->index || subindex != SDO_C->subIndex) { abortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = CO_SDO_ST_ABORT; } else { SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2; } } /* switch to regular transfer, CO_SDO_ST_UPLOAD_INITIATE_RSP */ else if ((SDO_C->CANrxData[0] & 0xF0) == 0x40) { /* verify index and subindex */ uint16_t index; uint8_t subindex; index = ((uint16_t) SDO_C->CANrxData[2]) << 8; index |= SDO_C->CANrxData[1]; subindex = SDO_C->CANrxData[3]; if (index != SDO_C->index || subindex != SDO_C->subIndex) { abortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = CO_SDO_ST_ABORT; break; } if (SDO_C->CANrxData[0] & 0x02) { /* Expedited transfer */ size_t count = 4; /* is size indicated? */ if (SDO_C->CANrxData[0] & 0x01) { count -= (SDO_C->CANrxData[0] >> 2) & 0x03; } /* copy data, indicate size and finish */ CO_fifo_write(&SDO_C->bufFifo, (const char *)&SDO_C->CANrxData[4], count, NULL); SDO_C->sizeTran = count; SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; } else { /* segmented transfer, is size indicated? */ if (SDO_C->CANrxData[0] & 0x01) { uint32_t size; memcpy(&size, &SDO_C->CANrxData[4], sizeof(size)); SDO_C->sizeInd = CO_SWAP_32(size); } SDO_C->toggle = 0x00; SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ; } } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ: { /* data are copied directly in the receive function */ break; } case CO_SDO_ST_UPLOAD_BLK_END_SREQ: { if ((SDO_C->CANrxData[0] & 0xE3) == 0xC1) { /* Get number of data bytes in last segment, that do not * contain data. Then copy remaining data into fifo */ uint8_t noData = ((SDO_C->CANrxData[0] >> 2) & 0x07); CO_fifo_write(&SDO_C->bufFifo, (const char *)&SDO_C->block_dataUploadLast[0], 7 - noData, &SDO_C->block_crc); SDO_C->sizeTran += 7 - noData; /* verify length */ if (SDO_C->sizeInd > 0 && SDO_C->sizeTran != SDO_C->sizeInd ) { abortCode = (SDO_C->sizeTran > SDO_C->sizeInd) ? CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT; SDO_C->state = CO_SDO_ST_ABORT; break; } /* verify CRC */ if (SDO_C->block_crcEnabled) { uint16_t crcServer; crcServer = ((uint16_t) SDO_C->CANrxData[2]) << 8; crcServer |= SDO_C->CANrxData[1]; if (crcServer != SDO_C->block_crc) { abortCode = CO_SDO_AB_CRC; SDO_C->state = CO_SDO_ST_ABORT; break; } } SDO_C->state = CO_SDO_ST_UPLOAD_BLK_END_CRSP; } else { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; } break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */ default: { abortCode = CO_SDO_AB_CMD; SDO_C->state = CO_SDO_ST_ABORT; break; } } SDO_C->timeoutTimer = 0; 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) { if (SDO_C->timeoutTimer < SDO_C->SDOtimeoutTime_us) { SDO_C->timeoutTimer += timeDifference_us; } if (SDO_C->timeoutTimer >= SDO_C->SDOtimeoutTime_us) { if (SDO_C->state == CO_SDO_ST_UPLOAD_SEGMENT_REQ || SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP ) { /* application didn't empty buffer */ abortCode = CO_SDO_AB_GENERAL; } else { abortCode = CO_SDO_AB_TIMEOUT; } SDO_C->state = CO_SDO_ST_ABORT; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT else if (timerNext_us != NULL) { /* check again after timeout time elapsed */ uint32_t diff = SDO_C->SDOtimeoutTime_us - SDO_C->timeoutTimer; if (*timerNext_us > diff) { *timerNext_us = diff; } } #endif #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK /* Timeout for sub-block reception */ if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) { if (SDO_C->block_timeoutTimer < SDO_C->block_SDOtimeoutTime_us) { SDO_C->block_timeoutTimer += timeDifference_us; } if (SDO_C->block_timeoutTimer >= SDO_C->block_SDOtimeoutTime_us) { /* SDO_C->state will change, processing will continue in this * thread. Make memory barrier here with CO_FLAG_CLEAR() call.*/ SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP; CO_FLAG_CLEAR(SDO_C->CANrxNew); } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT else if (timerNext_us != NULL) { /* check again after timeout time elapsed */ uint32_t diff = SDO_C->block_SDOtimeoutTime_us - SDO_C->block_timeoutTimer; if (*timerNext_us > diff) { *timerNext_us = diff; } } #endif } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */ if (SDO_C->CANtxBuff->bufferFull) { ret = CO_SDO_RT_transmittBufferFull; } } /* Transmit CAN data ******************************************************/ if (ret == CO_SDO_RT_waitingResponse) { #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK size_t count; #endif memset((void *)&SDO_C->CANtxBuff->data[0], 0, 8); switch (SDO_C->state) { case CO_SDO_ST_UPLOAD_INITIATE_REQ: { SDO_C->CANtxBuff->data[0] = 0x40; SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index; SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8); SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_UPLOAD_INITIATE_RSP; break; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED case CO_SDO_ST_UPLOAD_SEGMENT_REQ: { /* verify, if there is enough space in data buffer */ if (CO_fifo_getSpace(&SDO_C->bufFifo) < 7) { ret = CO_SDO_RT_uploadDataBufferFull; break; } SDO_C->CANtxBuff->data[0] = 0x60 | SDO_C->toggle; /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_RSP; break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */ #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ: { SDO_C->CANtxBuff->data[0] = 0xA4; SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index; SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8); SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; /* calculate number of block segments from free buffer space */ count = CO_fifo_getSpace(&SDO_C->bufFifo) / 7; if (count > 127) { count = 127; } else if (count == 0) { abortCode = CO_SDO_AB_OUT_OF_MEM; SDO_C->state = CO_SDO_ST_ABORT; break; } SDO_C->block_blksize = (uint8_t)count; SDO_C->CANtxBuff->data[4] = SDO_C->block_blksize; SDO_C->CANtxBuff->data[5] = CO_CONFIG_SDO_CLI_PST; /* reset timeout timer and send message */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP; break; } case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2: { SDO_C->CANtxBuff->data[0] = 0xA3; /* reset timeout timers, seqno and send message */ SDO_C->timeoutTimer = 0; SDO_C->block_timeoutTimer = 0; SDO_C->block_seqno = 0; SDO_C->block_crc = 0; /* Block segments will be received in different thread. Make memory * barrier here with CO_FLAG_CLEAR() call. */ SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ; CO_FLAG_CLEAR(SDO_C->CANrxNew); CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } 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) { SDO_C->state = CO_SDO_ST_UPLOAD_BLK_END_SREQ; } else { /* verify if size of data uploaded is too large */ if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd) { abortCode = CO_SDO_AB_DATA_LONG; SDO_C->state = CO_SDO_ST_ABORT; break; } /* calculate number of block segments from free buffer space */ count = CO_fifo_getSpace(&SDO_C->bufFifo) / 7; if (count >= 127) { count = 127; } 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) { *timerNext_us = 0; } #endif break; } SDO_C->block_blksize = (uint8_t)count; SDO_C->block_seqno = 0; /* Block segments will be received in different thread. Make * memory barrier here with CO_FLAG_CLEAR() call. */ SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ; CO_FLAG_CLEAR(SDO_C->CANrxNew); } SDO_C->CANtxBuff->data[2] = SDO_C->block_blksize; /* 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; } case CO_SDO_ST_UPLOAD_BLK_END_CRSP: { SDO_C->CANtxBuff->data[0] = 0xA1; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_ok_communicationEnd; break; } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */ default: { break; } } } if (ret == CO_SDO_RT_waitingResponse) { if (SDO_C->state == CO_SDO_ST_ABORT) { uint32_t code = CO_SWAP_32((uint32_t)abortCode); /* Send SDO abort message */ SDO_C->CANtxBuff->data[0] = 0x80; SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index; SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8); SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; memcpy(&SDO_C->CANtxBuff->data[4], &code, sizeof(code)); CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = CO_SDO_ST_IDLE; ret = CO_SDO_RT_endedWithClientAbort; } #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK else if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) { ret = CO_SDO_RT_blockUploadInProgress; } #endif } if (sizeIndicated != NULL) { *sizeIndicated = SDO_C->sizeInd; } if (sizeTransferred != NULL) { *sizeTransferred = SDO_C->sizeTran; } if (SDOabortCode != NULL) { *SDOabortCode = abortCode; } return ret; } /******************************************************************************/ size_t CO_SDOclientUploadBufRead(CO_SDOclient_t *SDO_C, char *buf, size_t count) { size_t ret = 0; if (SDO_C != NULL && buf != NULL) { ret = CO_fifo_read(&SDO_C->bufFifo, buf, count, NULL); } return ret; } /******************************************************************************/ void CO_SDOclientClose(CO_SDOclient_t *SDO_C) { if (SDO_C != NULL) { SDO_C->state = CO_SDO_ST_IDLE; } } #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE */