/* * CANopen Service Data Object - client. * * @file CO_SDOmaster.c * @ingroup CO_SDOmaster * @author Janez Paternoster * @author Matej Severkar * @copyright 2004 - 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 "CO_driver.h" #include "CO_SDO.h" #include "CO_SDOmaster.h" #include "crc16-ccitt.h" /* Client command specifier */ #define CCS_DOWNLOAD_INITIATE 1 #define CCS_DOWNLOAD_SEGMENT 0 #define CCS_UPLOAD_INITIATE 2 #define CCS_UPLOAD_SEGMENT 3 #define CCS_ABORT 4 #define CCS_UPLOAD_BLOCK 5 #define CCS_DOWNLOAD_BLOCK 6 /* Server Command Specifier */ #define SCS_UPLOAD_INITIATE 2 #define SCS_UPLOAD_SEGMENT 0 #define SCS_DOWNLOAD_INITIATED 3 #define SCS_DOWNLOAD_SEGMENT 1 #define SCS_ABORT 4 #define SCS_DOWNLOAD_BLOCK 5 #define SCS_UPLOAD_BLOCK 6 /* client states */ #define SDO_STATE_NOTDEFINED 0 #define SDO_STATE_ABORT 1 /* DOWNLOAD EXPEDITED/SEGMENTED */ #define SDO_STATE_DOWNLOAD_INITIATE 10 #define SDO_STATE_DOWNLOAD_REQUEST 11 #define SDO_STATE_DOWNLOAD_RESPONSE 12 /* UPLOAD EXPEDITED/SEGMENTED */ #define SDO_STATE_UPLOAD_INITIATED 20 #define SDO_STATE_UPLOAD_REQUEST 21 #define SDO_STATE_UPLOAD_RESPONSE 22 /* DOWNLOAD BLOCK */ #define SDO_STATE_BLOCKDOWNLOAD_INITIATE 100 #define SDO_STATE_BLOCKDOWNLOAD_INPROGRES 101 #define SDO_STATE_BLOCKDOWNLOAD_BLOCK_ACK 102 #define SDO_STATE_BLOCKDOWNLOAD_CRC 103 #define SDO_STATE_BLOCKDOWNLOAD_CRC_ACK 104 /* UPLOAD BLOCK */ #define SDO_STATE_BLOCKUPLOAD_INITIATE 200 #define SDO_STATE_BLOCKUPLOAD_INITIATE_ACK 201 #define SDO_STATE_BLOCKUPLOAD_INPROGRES 202 #define SDO_STATE_BLOCKUPLOAD_SUB_END 203 #define SDO_STATE_BLOCKUPLOAD_BLOCK_ACK 204 #define SDO_STATE_BLOCKUPLOAD_BLOCK_ACK_LAST 205 #define SDO_STATE_BLOCKUPLOAD_BLOCK_CRC 206 #define SDO_STATE_BLOCKUPLOAD_BLOCK_END 207 /* * 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, const CO_CANrxMsg_t *msg){ CO_SDOclient_t *SDO_C; SDO_C = (CO_SDOclient_t*)object; /* this is the correct pointer type of the first argument */ /* verify message length and message overflow (previous message was not processed yet) */ if((msg->DLC == 8U) && (!IS_CANrxNew(SDO_C->CANrxNew)) && (SDO_C->state != SDO_STATE_NOTDEFINED)){ if(SDO_C->state != SDO_STATE_BLOCKUPLOAD_INPROGRES) { /* copy data and set 'new message' flag */ SDO_C->CANrxData[0] = msg->data[0]; SDO_C->CANrxData[1] = msg->data[1]; SDO_C->CANrxData[2] = msg->data[2]; SDO_C->CANrxData[3] = msg->data[3]; SDO_C->CANrxData[4] = msg->data[4]; SDO_C->CANrxData[5] = msg->data[5]; SDO_C->CANrxData[6] = msg->data[6]; SDO_C->CANrxData[7] = msg->data[7]; SET_CANrxNew(SDO_C->CANrxNew); } else { /* block upload, copy data directly */ uint8_t seqno; SDO_C->CANrxData[0] = msg->data[0]; seqno = SDO_C->CANrxData[0] & 0x7f; SDO_C->timeoutTimer = 0; SDO_C->timeoutTimerBLOCK = 0; /* check correct sequence number. */ if(seqno == (SDO_C->block_seqno + 1)) { /* block_seqno is correct */ uint8_t i; SDO_C->block_seqno++; /* copy data */ for(i=1; i<8; i++) { SDO_C->buffer[SDO_C->dataSizeTransfered++] = msg->data[i]; if(SDO_C->dataSizeTransfered >= SDO_C->bufferSize) { /* buffer full, break reception */ SDO_C->state = SDO_STATE_BLOCKUPLOAD_SUB_END; SET_CANrxNew(SDO_C->CANrxNew); break; } } /* break reception if last segment or block sequence is too large */ if(((SDO_C->CANrxData[0] & 0x80U) == 0x80U) || (SDO_C->block_seqno >= SDO_C->block_blksize)) { SDO_C->state = SDO_STATE_BLOCKUPLOAD_SUB_END; SET_CANrxNew(SDO_C->CANrxNew); } } else if((seqno == SDO_C->block_seqno) || (SDO_C->block_seqno == 0U)){ /* Ignore message, if it is duplicate or if sequence didn't started yet. */ } else { /* seqno is totally wrong, break reception. */ SDO_C->state = SDO_STATE_BLOCKUPLOAD_SUB_END; SET_CANrxNew(SDO_C->CANrxNew); } } /* Optional signal to RTOS, which can resume task, which handles SDO client. */ if(IS_CANrxNew(SDO_C->CANrxNew) && SDO_C->pFunctSignal != NULL) { SDO_C->pFunctSignal(); } } } /******************************************************************************/ CO_ReturnError_t CO_SDOclient_init( CO_SDOclient_t *SDO_C, CO_SDO_t *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 || SDO==NULL || SDOClientPar==NULL || SDOClientPar->maxSubIndex!=3 || CANdevRx==NULL || CANdevTx==NULL){ return CO_ERROR_ILLEGAL_ARGUMENT; } /* Configure object variables */ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); SDO_C->pst = 21; /* block transfer */ SDO_C->block_size_max = 127; /* block transfer */ SDO_C->SDO = SDO; SDO_C->SDOClientPar = SDOClientPar; SDO_C->pFunctSignal = NULL; SDO_C->CANdevRx = CANdevRx; SDO_C->CANdevRxIdx = CANdevRxIdx; SDO_C->CANdevTx = CANdevTx; SDO_C->CANdevTxIdx = CANdevTxIdx; 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; } /******************************************************************************/ void CO_SDOclient_initCallback( CO_SDOclient_t *SDOclient, void (*pFunctSignal)(void)) { if(SDOclient != NULL){ SDOclient->pFunctSignal = pFunctSignal; } } /******************************************************************************/ CO_SDOclient_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_SDOcli_wrongArguments; } /* Configure object variables */ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(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_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 received message */ /* configure SDO client CAN transmission */ SDO_C->CANtxBuff = CO_CANtxBufferInit( SDO_C->CANdevTx, /* CAN device */ SDO_C->CANdevTxIdx, /* index of specific 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; } return CO_SDOcli_ok_communicationEnd; } /******************************************************************************/ static void CO_SDOclient_abort(CO_SDOclient_t *SDO_C, uint32_t code){ SDO_C->CANtxBuff->data[0] = 0x80; SDO_C->CANtxBuff->data[1] = SDO_C->index & 0xFF; SDO_C->CANtxBuff->data[2] = (SDO_C->index>>8) & 0xFF; SDO_C->CANtxBuff->data[3] = SDO_C->subIndex; CO_memcpySwap4(&SDO_C->CANtxBuff->data[4], &code); CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); } /******************************************************************************/ static void CO_SDOTxBufferClear(CO_SDOclient_t *SDO_C) { uint16_t i; for(i=0; i<8; i++) { SDO_C->CANtxBuff->data[i] = 0; } SDO_C->CANtxBuff->bufferFull = 0; } /******************************************************************************* * * DOWNLOAD * * ******************************************************************************/ CO_SDOclient_return_t CO_SDOclientDownloadInitiate( CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, uint8_t *dataTx, uint32_t dataSize, uint8_t blockEnable) { /* verify parameters */ if(SDO_C == NULL || dataTx == 0 || dataSize == 0) { return CO_SDOcli_wrongArguments; } /* save parameters */ SDO_C->buffer = dataTx; SDO_C->bufferSize = dataSize; SDO_C->state = SDO_STATE_DOWNLOAD_INITIATE; /* prepare CAN tx message */ CO_SDOTxBufferClear(SDO_C); SDO_C->index = index; SDO_C->subIndex = subIndex; SDO_C->CANtxBuff->data[1] = index & 0xFF; SDO_C->CANtxBuff->data[2] = index >> 8; SDO_C->CANtxBuff->data[3] = subIndex; /* if nodeIDOfTheSDOServer == node-ID of this node, then exchange data with this node */ if(SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId){ /* Optional signal to RTOS. We can immediately continue SDO Client */ if(SDO_C->pFunctSignal != NULL) { SDO_C->pFunctSignal(); } return CO_SDOcli_ok_communicationEnd; } if(dataSize <= 4){ uint16_t i; /* expedited transfer */ SDO_C->CANtxBuff->data[0] = 0x23 | ((4-dataSize) << 2); /* copy data */ for(i=dataSize+3; i>=4; i--) SDO_C->CANtxBuff->data[i] = dataTx[i-4]; } else if((SDO_C->bufferSize > SDO_C->pst) && blockEnable != 0){ /* BLOCK transfer */ /* set state of block transfer */ SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_INITIATE; /* init HEAD of packet */ SDO_C->CANtxBuff->data[0] = CCS_DOWNLOAD_BLOCK<<5; /* set flag for CRC */ SDO_C->CANtxBuff->data[0] |= 0x01<<2; /* size indicator: */ SDO_C->CANtxBuff->data[0] |= 0x01<<1; /* set length of data */ SDO_C->CANtxBuff->data[4] = (uint8_t) dataSize; SDO_C->CANtxBuff->data[5] = (uint8_t) (dataSize >> 8); SDO_C->CANtxBuff->data[6] = (uint8_t) (dataSize >> 16); SDO_C->CANtxBuff->data[7] = (uint8_t) (dataSize >> 24); } else{ uint32_t len; /* segmented transfer */ SDO_C->CANtxBuff->data[0] = 0x21; len = dataSize; CO_memcpySwap4(&SDO_C->CANtxBuff->data[4], &len); } /* empty receive buffer, reset timeout timer and send message */ CLEAR_CANrxNew(SDO_C->CANrxNew); SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); return CO_SDOcli_ok_communicationEnd; } /******************************************************************************/ CO_SDOclient_return_t CO_SDOclientDownload( CO_SDOclient_t *SDO_C, uint16_t timeDifference_ms, uint16_t SDOtimeoutTime, uint32_t *pSDOabortCode) { CO_SDOclient_return_t ret = CO_SDOcli_waitingServerResponse; /* verify parameters */ if(SDO_C == NULL) { return CO_SDOcli_wrongArguments; } /* clear abort code */ *pSDOabortCode = CO_SDO_AB_NONE; /* if nodeIDOfTheSDOServer == node-ID of this node, then exchange data with this node */ if(SDO_C->SDO && SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId){ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); /* If SDO server is busy return error */ if(SDO_C->SDO->state != 0){ return CO_SDOcli_endedWithClientAbort; } /* init ODF_arg */ *pSDOabortCode = CO_SDO_initTransfer(SDO_C->SDO, SDO_C->index, SDO_C->subIndex); if((*pSDOabortCode) != CO_SDO_AB_NONE){ return CO_SDOcli_endedWithServerAbort; } /* set buffer */ SDO_C->SDO->ODF_arg.data = SDO_C->buffer; /* write data to the Object dictionary */ *pSDOabortCode = CO_SDO_writeOD(SDO_C->SDO, SDO_C->bufferSize); if((*pSDOabortCode) != CO_SDO_AB_NONE){ return CO_SDOcli_endedWithServerAbort; } return CO_SDOcli_ok_communicationEnd; } /* RX data ****************************************************************************************** */ if(IS_CANrxNew(SDO_C->CANrxNew)){ uint8_t SCS = SDO_C->CANrxData[0]>>5; /* Client command specifier */ /* ABORT */ if (SDO_C->CANrxData[0] == (SCS_ABORT<<5)){ SDO_C->state = SDO_STATE_NOTDEFINED; CO_memcpySwap4(pSDOabortCode , &SDO_C->CANrxData[4]); CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_endedWithServerAbort; } switch (SDO_C->state){ case SDO_STATE_DOWNLOAD_INITIATE:{ if (SCS == SCS_DOWNLOAD_INITIATED){ if(SDO_C->bufferSize <= 4){ /* expedited transfer */ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_ok_communicationEnd; } else{ /* segmented transfer - prepare the first segment */ SDO_C->bufferOffset = 0; SDO_C->toggle =0; SDO_C->state = SDO_STATE_DOWNLOAD_REQUEST; } } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_DOWNLOAD_RESPONSE:{ if (SCS == SCS_DOWNLOAD_SEGMENT){ /* verify toggle bit */ if((SDO_C->CANrxData[0]&0x10) != (SDO_C->toggle<<4)){ *pSDOabortCode = CO_SDO_AB_TOGGLE_BIT; SDO_C->state = SDO_STATE_ABORT; break; } /* alternate toggle bit */ if (SDO_C->toggle ==0x00) SDO_C->toggle =0x01; else SDO_C->toggle =0x00; /* is end of transfer? */ if(SDO_C->bufferOffset == SDO_C->bufferSize){ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_ok_communicationEnd; } SDO_C->state = SDO_STATE_DOWNLOAD_REQUEST; } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_BLOCKDOWNLOAD_INITIATE:{ /* waiting on reply on block download initiated */ if (SCS == SCS_DOWNLOAD_BLOCK){ uint16_t IndexTmp; IndexTmp = SDO_C->CANrxData[2]; IndexTmp = IndexTmp << 8; IndexTmp |= SDO_C->CANrxData[1]; if(IndexTmp != SDO_C->index || SDO_C->CANrxData[3] != SDO_C->subIndex) { /* wrong index */ *pSDOabortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = SDO_STATE_ABORT; break; } /* set blksize */ SDO_C->block_blksize = SDO_C->CANrxData[4]; SDO_C->block_seqno = 0; SDO_C->bufferOffset = 0; SDO_C->bufferOffsetACK = 0; SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_INPROGRES; } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_BLOCKDOWNLOAD_INPROGRES: case SDO_STATE_BLOCKDOWNLOAD_BLOCK_ACK:{ /* waiting block ACK */ if (SCS == SCS_DOWNLOAD_BLOCK){ /* check server subcommand */ if((SDO_C->CANrxData[0] & 0x02) == 0){ /* wrong server sub command */ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; break; } /* check number of segments */ if(SDO_C->CANrxData[1] != SDO_C->block_blksize){ /* NOT all segments transferred successfully */ SDO_C->bufferOffsetACK += SDO_C->CANrxData[1] * 7; SDO_C->bufferOffset = SDO_C->bufferOffsetACK; } else{ SDO_C->bufferOffsetACK = SDO_C->bufferOffset; } /* set size of next block */ SDO_C->block_blksize = SDO_C->CANrxData[2]; SDO_C->block_seqno = 0; if(SDO_C->bufferOffset >= SDO_C->bufferSize) SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_CRC; else SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_INPROGRES; } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_BLOCKDOWNLOAD_CRC_ACK:{ if (SCS == SCS_DOWNLOAD_BLOCK){ if((SDO_C->CANrxData[0] & 0x01) == 0){ /* wrong server sub command */ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; break; } /* SDO block download successfully transferred */ SDO_C->state = SDO_STATE_NOTDEFINED; SDO_C->timeoutTimer = 0; CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_ok_communicationEnd; } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; break; } } default:{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; break; } } SDO_C->timeoutTimer = 0; CLEAR_CANrxNew(SDO_C->CANrxNew); } /* TMO *********************************************************************************************** */ if(SDO_C->timeoutTimer < SDOtimeoutTime){ SDO_C->timeoutTimer += timeDifference_ms; } if(SDO_C->timeoutTimer >= SDOtimeoutTime){ /* communication TMO */ *pSDOabortCode = CO_SDO_AB_TIMEOUT; CO_SDOclient_abort(SDO_C, *pSDOabortCode); return CO_SDOcli_endedWithTimeout; } /* TX data ******************************************************************************************* */ if(SDO_C->CANtxBuff->bufferFull) { return CO_SDOcli_transmittBufferFull; } CO_SDOTxBufferClear(SDO_C); switch (SDO_C->state){ /* ABORT */ case SDO_STATE_ABORT:{ SDO_C->state = SDO_STATE_NOTDEFINED; CO_SDOclient_abort (SDO_C, *pSDOabortCode); ret = CO_SDOcli_endedWithClientAbort; break; } /* SEGMENTED */ case SDO_STATE_DOWNLOAD_REQUEST:{ uint16_t i, j; /* calculate length to be sent */ j = SDO_C->bufferSize - SDO_C->bufferOffset; if(j > 7) j = 7; /* fill data bytes */ for(i=0; iCANtxBuff->data[i+1] = SDO_C->buffer[SDO_C->bufferOffset + i]; for(; i<7; i++) SDO_C->CANtxBuff->data[i+1] = 0; SDO_C->bufferOffset += j; /* SDO command specifier */ SDO_C->CANtxBuff->data[0] = CCS_DOWNLOAD_SEGMENT | ((SDO_C->toggle)<<4) | ((7-j)<<1); /* is end of transfer? */ if(SDO_C->bufferOffset == SDO_C->bufferSize){ SDO_C->CANtxBuff->data[0] |= 1; } /* Send next SDO message */ CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = SDO_STATE_DOWNLOAD_RESPONSE; break; } /* BLOCK */ case SDO_STATE_BLOCKDOWNLOAD_INPROGRES:{ SDO_C->block_seqno += 1; SDO_C->CANtxBuff->data[0] = SDO_C->block_seqno; if(SDO_C->block_seqno >= SDO_C->block_blksize){ SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_BLOCK_ACK; } /* set data */ SDO_C->block_noData = 0; uint8_t i; for(i = 1; i < 8; i++){ if(SDO_C->bufferOffset < SDO_C->bufferSize){ SDO_C->CANtxBuff->data[i] = *(SDO_C->buffer + SDO_C->bufferOffset); } else{ SDO_C->CANtxBuff->data[i] = 0; SDO_C->block_noData += 1; } SDO_C->bufferOffset += 1; } if(SDO_C->bufferOffset >= SDO_C->bufferSize){ SDO_C->CANtxBuff->data[0] |= 0x80; SDO_C->block_blksize = SDO_C->block_seqno; SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_BLOCK_ACK; } /* tx data */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } case SDO_STATE_BLOCKDOWNLOAD_CRC:{ SDO_C->CANtxBuff->data[0] = (CCS_DOWNLOAD_BLOCK<<5) | (SDO_C->block_noData << 2) | 0x01; uint16_t tmp16; tmp16 = crc16_ccitt((unsigned char *)SDO_C->buffer, (unsigned int)SDO_C->bufferSize, 0); SDO_C->CANtxBuff->data[1] = (uint8_t) tmp16; SDO_C->CANtxBuff->data[2] = (uint8_t) (tmp16>>8); /* set state */ SDO_C->state = SDO_STATE_BLOCKDOWNLOAD_CRC_ACK; /* tx data */ SDO_C->timeoutTimer = 0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } default:{ break; } } if(SDO_C->state == SDO_STATE_BLOCKDOWNLOAD_INPROGRES) { ret = CO_SDOcli_blockDownldInProgress; } return ret; } /******************************************************************************* * * UPLOAD * ******************************************************************************/ CO_SDOclient_return_t CO_SDOclientUploadInitiate( CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, uint8_t *dataRx, uint32_t dataRxSize, uint8_t blockEnable) { /* verify parameters */ if(SDO_C == NULL || dataRx == 0 || dataRxSize < 4) { return CO_SDOcli_wrongArguments; } /* save parameters */ SDO_C->buffer = dataRx; SDO_C->bufferSize = dataRxSize; /* prepare CAN tx message */ CO_SDOTxBufferClear(SDO_C); SDO_C->index = index; SDO_C->subIndex = subIndex; SDO_C->CANtxBuff->data[1] = index & 0xFF; SDO_C->CANtxBuff->data[2] = index >> 8; SDO_C->CANtxBuff->data[3] = subIndex; if(blockEnable == 0){ SDO_C->state = SDO_STATE_UPLOAD_INITIATED; SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_INITIATE<<5); } else{ SDO_C->state = SDO_STATE_BLOCKUPLOAD_INITIATE; /* header */ SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_BLOCK<<5); /* set CRC */ SDO_C->CANtxBuff->data[0] |= 0x04; /* set number of segments in block */ SDO_C->block_blksize = SDO_C->block_size_max; if ((SDO_C->block_blksize *7) > SDO_C->bufferSize){ return CO_SDOcli_wrongArguments; } SDO_C->CANtxBuff->data[4] = SDO_C->block_blksize; SDO_C->CANtxBuff->data[5] = SDO_C->pst; SDO_C->block_seqno = 0; } /* if nodeIDOfTheSDOServer == node-ID of this node, then exchange data with this node */ if(SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId){ /* Optional signal to RTOS. We can immediately continue SDO Client */ if(SDO_C->pFunctSignal != NULL) { SDO_C->pFunctSignal(); } return CO_SDOcli_ok_communicationEnd; } /* empty receive buffer, reset timeout timer and send message */ CLEAR_CANrxNew(SDO_C->CANrxNew); SDO_C->timeoutTimer = 0; SDO_C->timeoutTimerBLOCK =0; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); return CO_SDOcli_ok_communicationEnd; } /******************************************************************************/ CO_SDOclient_return_t CO_SDOclientUpload( CO_SDOclient_t *SDO_C, uint16_t timeDifference_ms, uint16_t SDOtimeoutTime, uint32_t *pDataSize, uint32_t *pSDOabortCode) { uint16_t indexTmp; uint32_t tmp32; CO_SDOclient_return_t ret = CO_SDOcli_waitingServerResponse; /* verify parameters */ if(SDO_C == NULL) { return CO_SDOcli_wrongArguments; } /* clear abort code */ *pSDOabortCode = CO_SDO_AB_NONE; /* if nodeIDOfTheSDOServer == node-ID of this node, then exchange data with this node */ if(SDO_C->SDO && SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId){ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); /* If SDO server is busy return error */ if(SDO_C->SDO->state != 0){ *pSDOabortCode = CO_SDO_AB_DEVICE_INCOMPAT; return CO_SDOcli_endedWithClientAbort; } /* init ODF_arg */ *pSDOabortCode = CO_SDO_initTransfer(SDO_C->SDO, SDO_C->index, SDO_C->subIndex); if((*pSDOabortCode) != CO_SDO_AB_NONE){ return CO_SDOcli_endedWithServerAbort; } /* set buffer and length if domain */ SDO_C->SDO->ODF_arg.data = SDO_C->buffer; if(SDO_C->SDO->ODF_arg.ODdataStorage == 0) SDO_C->SDO->ODF_arg.dataLength = SDO_C->bufferSize; /* read data from the Object dictionary */ *pSDOabortCode = CO_SDO_readOD(SDO_C->SDO, SDO_C->bufferSize); if((*pSDOabortCode) != CO_SDO_AB_NONE){ return CO_SDOcli_endedWithServerAbort; } /* set data size */ *pDataSize = SDO_C->SDO->ODF_arg.dataLength; /* is SDO buffer too small */ if(SDO_C->SDO->ODF_arg.lastSegment == 0){ *pSDOabortCode = CO_SDO_AB_OUT_OF_MEM; /* Out of memory */ return CO_SDOcli_endedWithServerAbort; } return CO_SDOcli_ok_communicationEnd; } /* RX data ******************************************************************************** */ if(IS_CANrxNew(SDO_C->CANrxNew)){ uint8_t SCS = SDO_C->CANrxData[0]>>5; /* Client command specifier */ /* ABORT */ if (SDO_C->CANrxData[0] == (SCS_ABORT<<5)){ SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); CO_memcpySwap4(pSDOabortCode , &SDO_C->CANrxData[4]); return CO_SDOcli_endedWithServerAbort; } switch (SDO_C->state){ case SDO_STATE_UPLOAD_INITIATED:{ if (SCS == SCS_UPLOAD_INITIATE){ if(SDO_C->CANrxData[0] & 0x02){ uint8_t size; /* Expedited transfer */ if(SDO_C->CANrxData[0] & 0x01)/* is size indicated */ size = 4 - ((SDO_C->CANrxData[0]>>2)&0x03); /* size */ else size = 4; *pDataSize = size; /* copy data */ while(size--) SDO_C->buffer[size] = SDO_C->CANrxData[4+size]; SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_ok_communicationEnd; } else{ /* segmented transfer - prepare first segment */ SDO_C->bufferOffset = 0; SDO_C->state = SDO_STATE_UPLOAD_REQUEST; SDO_C->toggle =0; /* continue with segmented upload */ } } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_UPLOAD_RESPONSE:{ if (SCS == SCS_UPLOAD_SEGMENT){ uint16_t size, i; /* verify toggle bit */ if((SDO_C->CANrxData[0] &0x10) != (~SDO_C->toggle &0x10)){ *pSDOabortCode = CO_SDO_AB_TOGGLE_BIT; SDO_C->state = SDO_STATE_ABORT; break; } /* get size */ size = 7 - ((SDO_C->CANrxData[0]>>1)&0x07); /* verify length */ if((SDO_C->bufferOffset + size) > SDO_C->bufferSize){ *pSDOabortCode = CO_SDO_AB_OUT_OF_MEM; /* Out of memory */ SDO_C->state = SDO_STATE_ABORT; break; } /* copy data to buffer */ for(i=0; ibuffer[SDO_C->bufferOffset + i] = SDO_C->CANrxData[1 + i]; SDO_C->bufferOffset += size; /* If no more segments to be uploaded, finish communication */ if(SDO_C->CANrxData[0] & 0x01){ *pDataSize = SDO_C->bufferOffset; SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_ok_communicationEnd; } /* set state */ SDO_C->state = SDO_STATE_UPLOAD_REQUEST; break; } else{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_BLOCKUPLOAD_INITIATE:{ if (SCS == SCS_UPLOAD_BLOCK){ /* block upload initiate response */ SDO_C->state = SDO_STATE_BLOCKUPLOAD_INITIATE_ACK; /* SCR support */ if((SDO_C->CANrxData[0] & 0x04) != 0) SDO_C->crcEnabled = 1; /* CRC suported */ else SDO_C->crcEnabled = 0; /* CRC not suported */ /* chech Index ans subnindex */ indexTmp = SDO_C->CANrxData[2]; indexTmp = indexTmp << 8; indexTmp |= SDO_C->CANrxData[1]; if(indexTmp != SDO_C->index || SDO_C->CANrxData[3] != SDO_C->subIndex){ *pSDOabortCode = CO_SDO_AB_PRAM_INCOMPAT; SDO_C->state = SDO_STATE_ABORT; } /* set length */ if(SDO_C->CANrxData[0]&0x02){ uint32_t len; CO_memcpySwap4(&len, &SDO_C->CANrxData[4]); SDO_C->dataSize = len; } else{ SDO_C->dataSize = 0; } /* check available buffer size */ if (SDO_C->dataSize > SDO_C->bufferSize){ *pSDOabortCode = CO_SDO_AB_OUT_OF_MEM; SDO_C->state = SDO_STATE_ABORT; } SDO_C->dataSizeTransfered =0; } else if (SCS == SCS_UPLOAD_INITIATE){ /* switch to regular segmented transfer */ if(SDO_C->CANrxData[0] & 0x02){ uint8_t size; /* Expedited transfer */ if(SDO_C->CANrxData[0] & 0x01)/* is size indicated */ size = 4 - ((SDO_C->CANrxData[0]>>2)&0x03); /* size */ else size = 4; *pDataSize = size; /* copy data */ while(size--) SDO_C->buffer[size] = SDO_C->CANrxData[4+size]; SDO_C->state = SDO_STATE_NOTDEFINED; CLEAR_CANrxNew(SDO_C->CANrxNew); return CO_SDOcli_ok_communicationEnd; } else{ /* segmented transfer - prepare first segment */ SDO_C->bufferOffset = 0; SDO_C->state = SDO_STATE_UPLOAD_REQUEST; SDO_C->toggle =0; /* continue with segmented upload */ } } else{ /* unknown SCS */ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; } break; } case SDO_STATE_BLOCKUPLOAD_INPROGRES:{ /* data are copied directly in receive function */ break; } case SDO_STATE_BLOCKUPLOAD_SUB_END:{ /* data was copied by receive function, sub-block is finished */ /* Is last segment? */ if(SDO_C->CANrxData[0] & 0x80) { /* Is data size indicated and wrong? */ if((SDO_C->dataSize != 0) && (SDO_C->dataSize > SDO_C->dataSizeTransfered)) { *pSDOabortCode = CO_SDO_AB_TYPE_MISMATCH; SDO_C->state = SDO_STATE_ABORT; } else { SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_ACK_LAST; } } else { /* Is SDO buffer overflow? */ if(SDO_C->dataSizeTransfered >= SDO_C->bufferSize) { *pSDOabortCode = CO_SDO_AB_OUT_OF_MEM; SDO_C->state = SDO_STATE_ABORT; } else { SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_ACK; } } break; } case SDO_STATE_BLOCKUPLOAD_BLOCK_CRC:{ if (SCS == SCS_UPLOAD_BLOCK){ tmp32 = ((SDO_C->CANrxData[0]>>2) & 0x07); SDO_C->dataSizeTransfered -= tmp32; SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_END; if (SDO_C->crcEnabled){ uint16_t tmp16; CO_memcpySwap2(&tmp16, &SDO_C->CANrxData[1]); if (tmp16 != crc16_ccitt((unsigned char *)SDO_C->buffer, (unsigned int)SDO_C->dataSizeTransfered, 0)){ *pSDOabortCode = CO_SDO_AB_CRC; SDO_C->state = SDO_STATE_ABORT; } else{ SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_END; } } else{ SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_END; } } else{ *pSDOabortCode = CO_SDO_AB_GENERAL; SDO_C->state = SDO_STATE_ABORT; } break; } default:{ *pSDOabortCode = CO_SDO_AB_CMD; SDO_C->state = SDO_STATE_ABORT; break; } } SDO_C->timeoutTimer = 0; CLEAR_CANrxNew(SDO_C->CANrxNew); } /* TMO *************************************************************************************************** */ if(SDO_C->timeoutTimer < SDOtimeoutTime){ SDO_C->timeoutTimer += timeDifference_ms; if (SDO_C->state == SDO_STATE_BLOCKUPLOAD_INPROGRES) SDO_C->timeoutTimerBLOCK += timeDifference_ms; } if(SDO_C->timeoutTimer >= SDOtimeoutTime){ /* communication TMO */ *pSDOabortCode = CO_SDO_AB_TIMEOUT; CO_SDOclient_abort(SDO_C, *pSDOabortCode); return CO_SDOcli_endedWithTimeout; } if(SDO_C->timeoutTimerBLOCK >= (SDOtimeoutTime/2)){ /* block TMO */ SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_ACK; } /* TX data ******************************************************************************** */ if(SDO_C->CANtxBuff->bufferFull) { return CO_SDOcli_transmittBufferFull; } CO_SDOTxBufferClear(SDO_C); switch (SDO_C->state){ case SDO_STATE_ABORT:{ SDO_C->state = SDO_STATE_NOTDEFINED; CO_SDOclient_abort (SDO_C, *pSDOabortCode); ret = CO_SDOcli_endedWithClientAbort; break; } /* SEGMENTED UPLOAD */ case SDO_STATE_UPLOAD_REQUEST:{ SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_SEGMENT<<5) | (SDO_C->toggle & 0x10); CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); SDO_C->state = SDO_STATE_UPLOAD_RESPONSE; SDO_C->toggle = ~SDO_C->toggle; break; } /* BLOCK */ case SDO_STATE_BLOCKUPLOAD_INITIATE_ACK:{ SDO_C->timeoutTimerBLOCK = 0; SDO_C->block_seqno = 0; SDO_C->state = SDO_STATE_BLOCKUPLOAD_INPROGRES; /* header */ SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_BLOCK<<5) | 0x03; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } case SDO_STATE_BLOCKUPLOAD_BLOCK_ACK_LAST:{ /* header */ SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_BLOCK<<5) | 0x02; SDO_C->CANtxBuff->data[1] = SDO_C->block_seqno; SDO_C->block_seqno = 0; SDO_C->timeoutTimerBLOCK = 0; SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_CRC; SDO_C->CANtxBuff->data[2] = SDO_C->block_blksize; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } case SDO_STATE_BLOCKUPLOAD_BLOCK_ACK:{ /* header */ SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_BLOCK<<5) | 0x02; SDO_C->CANtxBuff->data[1] = SDO_C->block_seqno; /* set next block size */ if (SDO_C->dataSize != 0){ if(SDO_C->dataSizeTransfered >= SDO_C->dataSize){ SDO_C->block_blksize = 0; SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_CRC; } else{ tmp32 = ((SDO_C->dataSize - SDO_C->dataSizeTransfered) / 7); if(tmp32 >= SDO_C->block_size_max){ SDO_C->block_blksize = SDO_C->block_size_max; } else{ if((SDO_C->dataSize - SDO_C->dataSizeTransfered) % 7 == 0) SDO_C->block_blksize = tmp32; else SDO_C->block_blksize = tmp32 + 1; } SDO_C->block_seqno = 0; SDO_C->timeoutTimerBLOCK = 0; SDO_C->state = SDO_STATE_BLOCKUPLOAD_INPROGRES; } } else{ SDO_C->block_seqno = 0; SDO_C->timeoutTimerBLOCK = 0; SDO_C->state = SDO_STATE_BLOCKUPLOAD_INPROGRES; } SDO_C->CANtxBuff->data[2] = SDO_C->block_blksize; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); break; } case SDO_STATE_BLOCKUPLOAD_BLOCK_END:{ SDO_C->CANtxBuff->data[0] = (CCS_UPLOAD_BLOCK<<5) | 0x01; CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff); *pDataSize = SDO_C->dataSizeTransfered; SDO_C->state = SDO_STATE_NOTDEFINED; ret = CO_SDOcli_ok_communicationEnd; break; } default: break; } if(SDO_C->state == SDO_STATE_BLOCKUPLOAD_INPROGRES) { ret = CO_SDOcli_blockUploadInProgress; } return ret; } /******************************************************************************/ void CO_SDOclientClose(CO_SDOclient_t *SDO_C){ if(SDO_C != NULL) { SDO_C->state = SDO_STATE_NOTDEFINED; } }