Lines Matching refs:SDO
163 static void CO_SDO_receive_done(CO_SDO_t *SDO){ in CO_SDO_receive_done() argument
165 uint8_t rcv = SDO->CANrxRcv; in CO_SDO_receive_done()
170 SDO->CANrxRcv = newRcv; in CO_SDO_receive_done()
171 SET_CANrxNew(SDO->CANrxNew[rcv]); in CO_SDO_receive_done()
173 SET_CANrxNew(SDO->CANrxNew[0]); in CO_SDO_receive_done()
186 CO_SDO_t *SDO; in CO_SDO_receive() local
189 SDO = (CO_SDO_t*)object; /* this is the correct pointer type of the first argument */ in CO_SDO_receive()
190 rcv = SDO->CANrxRcv; in CO_SDO_receive()
191 CANrxData = SDO->CANrxData[rcv]; in CO_SDO_receive()
194 if((msg->DLC == 8U) && (!IS_CANrxNew(SDO->CANrxNew[rcv]))){ in CO_SDO_receive()
195 if(SDO->state != CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK) { in CO_SDO_receive()
206 CO_SDO_receive_done(SDO); in CO_SDO_receive()
214 SDO->timeoutTimer = 0; in CO_SDO_receive()
216 if (SDO->timeoutSubblockDownolad) in CO_SDO_receive()
217 SDO->timeoutSubblockDownolad = false; in CO_SDO_receive()
220 if(seqno == (SDO->sequence + 1U)) { in CO_SDO_receive()
224 if (CO_SDO_BUFFER_SIZE - SDO->bufferOffset >= 7) { in CO_SDO_receive()
227 SDO->sequence++; in CO_SDO_receive()
231 …SDO->ODF_arg.data[SDO->bufferOffset++] = msg->data[i]; //SDO->ODF_arg.data is equal as SDO->databu… in CO_SDO_receive()
235 if(((CANrxData[0] & 0x80U) == 0x80U) || (SDO->sequence >= SDO->blksize)) { in CO_SDO_receive()
236 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP; in CO_SDO_receive()
237 CO_SDO_receive_done(SDO); in CO_SDO_receive()
241 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2; in CO_SDO_receive()
242 CO_SDO_receive_done(SDO); in CO_SDO_receive()
245 else if((seqno == SDO->sequence) || (SDO->sequence == 0U)) { in CO_SDO_receive()
250 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2; in CO_SDO_receive()
251 CO_SDO_receive_done(SDO); in CO_SDO_receive()
256 if((IS_CANrxNew(SDO->CANrxNew[rcv])) && (SDO->pFunctSignal != NULL)) { in CO_SDO_receive()
257 SDO->pFunctSignal(); in CO_SDO_receive()
289 CO_SDO_t *SDO, in CO_SDO_init() argument
304 if(SDO==NULL || CANdevRx==NULL || CANdevTx==NULL){ in CO_SDO_init()
312 SDO->ownOD = true; in CO_SDO_init()
313 SDO->OD = OD; in CO_SDO_init()
314 SDO->ODSize = ODSize; in CO_SDO_init()
315 SDO->ODExtensions = ODExtensions; in CO_SDO_init()
319 SDO->ODExtensions[i].pODFunc = NULL; in CO_SDO_init()
320 SDO->ODExtensions[i].object = NULL; in CO_SDO_init()
321 SDO->ODExtensions[i].flags = NULL; in CO_SDO_init()
326 SDO->ownOD = false; in CO_SDO_init()
327 SDO->OD = parentSDO->OD; in CO_SDO_init()
328 SDO->ODSize = parentSDO->ODSize; in CO_SDO_init()
329 SDO->ODExtensions = parentSDO->ODExtensions; in CO_SDO_init()
333 SDO->nodeId = nodeId; in CO_SDO_init()
334 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_init()
338 CLEAR_CANrxNew(SDO->CANrxNew[i]); in CO_SDO_init()
340 SDO->CANrxRcv = 0; in CO_SDO_init()
341 SDO->CANrxProc = 0; in CO_SDO_init()
343 SDO->pFunctSignal = NULL; in CO_SDO_init()
348 … CO_OD_configure(SDO, ObjDictIndex_SDOServerParameter, CO_ODF_1200, (void*)&SDO->nodeId, 0U, 0U); in CO_SDO_init()
363 (void*)SDO, /* object passed to receive function */ in CO_SDO_init()
367 SDO->CANdevTx = CANdevTx; in CO_SDO_init()
368 SDO->CANtxBuff = CO_CANtxBufferInit( in CO_SDO_init()
382 CO_SDO_t *SDO, in CO_SDO_initCallback() argument
385 if(SDO != NULL){ in CO_SDO_initCallback()
386 SDO->pFunctSignal = pFunctSignal; in CO_SDO_initCallback()
393 CO_SDO_t *SDO, in CO_OD_configure() argument
402 entryNo = CO_OD_find(SDO, index); in CO_OD_configure()
404 CO_OD_extension_t *ext = &SDO->ODExtensions[entryNo]; in CO_OD_configure()
405 uint8_t maxSubIndex = SDO->OD[entryNo].maxSubIndex; in CO_OD_configure()
424 uint16_t CO_OD_find(CO_SDO_t *SDO, uint16_t index){ in CO_OD_find() argument
431 max = SDO->ODSize - 1U; in CO_OD_find()
434 object = &SDO->OD[cur]; in CO_OD_find()
448 object = &SDO->OD[min]; in CO_OD_find()
460 uint16_t CO_OD_getLength(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){ in CO_OD_getLength() argument
461 const CO_OD_entry_t* object = &SDO->OD[entryNo]; in CO_OD_getLength()
500 uint16_t CO_OD_getAttribute(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){ in CO_OD_getAttribute() argument
501 const CO_OD_entry_t* object = &SDO->OD[entryNo]; in CO_OD_getAttribute()
534 void* CO_OD_getDataPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){ in CO_OD_getDataPointer() argument
535 const CO_OD_entry_t* object = &SDO->OD[entryNo]; in CO_OD_getDataPointer()
568 uint8_t* CO_OD_getFlagsPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){ in CO_OD_getFlagsPointer() argument
571 if((entryNo == 0xFFFFU) || (SDO->ODExtensions == 0)){ in CO_OD_getFlagsPointer()
575 ext = &SDO->ODExtensions[entryNo]; in CO_OD_getFlagsPointer()
582 uint32_t CO_SDO_initTransfer(CO_SDO_t *SDO, uint16_t index, uint8_t subIndex){ in CO_SDO_initTransfer() argument
584 SDO->ODF_arg.index = index; in CO_SDO_initTransfer()
585 SDO->ODF_arg.subIndex = subIndex; in CO_SDO_initTransfer()
588 SDO->entryNo = CO_OD_find(SDO, index); in CO_SDO_initTransfer()
589 if(SDO->entryNo == 0xFFFFU){ in CO_SDO_initTransfer()
594 if(subIndex > SDO->OD[SDO->entryNo].maxSubIndex && in CO_SDO_initTransfer()
595 SDO->OD[SDO->entryNo].pData != NULL) in CO_SDO_initTransfer()
601 SDO->ODF_arg.ODdataStorage = CO_OD_getDataPointer(SDO, SDO->entryNo, subIndex); in CO_SDO_initTransfer()
604 SDO->ODF_arg.object = NULL; in CO_SDO_initTransfer()
605 if(SDO->ODExtensions){ in CO_SDO_initTransfer()
606 CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo]; in CO_SDO_initTransfer()
607 SDO->ODF_arg.object = ext->object; in CO_SDO_initTransfer()
609 SDO->ODF_arg.data = SDO->databuffer; in CO_SDO_initTransfer()
610 SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, subIndex); in CO_SDO_initTransfer()
611 SDO->ODF_arg.attribute = CO_OD_getAttribute(SDO, SDO->entryNo, subIndex); in CO_SDO_initTransfer()
612 SDO->ODF_arg.pFlags = CO_OD_getFlagsPointer(SDO, SDO->entryNo, subIndex); in CO_SDO_initTransfer()
614 SDO->ODF_arg.firstSegment = true; in CO_SDO_initTransfer()
615 SDO->ODF_arg.lastSegment = true; in CO_SDO_initTransfer()
618 SDO->ODF_arg.dataLengthTotal = (SDO->ODF_arg.ODdataStorage) ? SDO->ODF_arg.dataLength : 0U; in CO_SDO_initTransfer()
620 SDO->ODF_arg.offset = 0U; in CO_SDO_initTransfer()
623 if(SDO->ODF_arg.dataLength > CO_SDO_BUFFER_SIZE){ in CO_SDO_initTransfer()
632 uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize){ in CO_SDO_readOD() argument
633 uint8_t *SDObuffer = SDO->ODF_arg.data; in CO_SDO_readOD()
634 uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage; in CO_SDO_readOD()
635 uint16_t length = SDO->ODF_arg.dataLength; in CO_SDO_readOD()
639 if((SDO->ODF_arg.attribute & CO_ODA_READABLE) == 0) in CO_SDO_readOD()
643 if(SDO->ODExtensions != NULL){ in CO_SDO_readOD()
644 ext = &SDO->ODExtensions[SDO->entryNo]; in CO_SDO_readOD()
662 SDO->ODF_arg.reading = true; in CO_SDO_readOD()
664 uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg); in CO_SDO_readOD()
671 if((SDO->ODF_arg.dataLength == 0U) || (SDO->ODF_arg.dataLength > SDOBufferSize)){ in CO_SDO_readOD()
679 SDO->ODF_arg.offset += SDO->ODF_arg.dataLength; in CO_SDO_readOD()
680 SDO->ODF_arg.firstSegment = false; in CO_SDO_readOD()
684 if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){ in CO_SDO_readOD()
685 uint16_t len = SDO->ODF_arg.dataLength; in CO_SDO_readOD()
686 uint8_t *buf1 = SDO->ODF_arg.data; in CO_SDO_readOD()
703 uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length){ in CO_SDO_writeOD() argument
704 uint8_t *SDObuffer = SDO->ODF_arg.data; in CO_SDO_writeOD()
705 uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage; in CO_SDO_writeOD()
709 if((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0){ in CO_SDO_writeOD()
715 SDO->ODF_arg.dataLength = length; in CO_SDO_writeOD()
719 else if(SDO->ODF_arg.dataLength != length){ in CO_SDO_writeOD()
725 if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){ in CO_SDO_writeOD()
726 uint16_t len = SDO->ODF_arg.dataLength; in CO_SDO_writeOD()
727 uint8_t *buf1 = SDO->ODF_arg.data; in CO_SDO_writeOD()
742 SDO->ODF_arg.reading = false; in CO_SDO_writeOD()
743 if(SDO->ODExtensions != NULL){ in CO_SDO_writeOD()
744 CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo]; in CO_SDO_writeOD()
747 uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg); in CO_SDO_writeOD()
754 SDO->ODF_arg.offset += SDO->ODF_arg.dataLength; in CO_SDO_writeOD()
755 SDO->ODF_arg.firstSegment = false; in CO_SDO_writeOD()
758 if(SDO->ODF_arg.index == 0x1003 && SDO->ODF_arg.subIndex == 0) { in CO_SDO_writeOD()
775 static void CO_SDO_process_done(CO_SDO_t *SDO, uint16_t *timerNext_ms) { in CO_SDO_process_done() argument
777 uint8_t proc = SDO->CANrxProc; in CO_SDO_process_done()
781 if (!IS_CANrxNew(SDO->CANrxNew[proc])){ in CO_SDO_process_done()
788 SDO->CANrxProc = newProc; in CO_SDO_process_done()
789 CLEAR_CANrxNew(SDO->CANrxNew[proc]); in CO_SDO_process_done()
791 if ((timerNext_ms != NULL) && (IS_CANrxNew(SDO->CANrxNew[newProc]))){ in CO_SDO_process_done()
797 CLEAR_CANrxNew(SDO->CANrxNew[0]); in CO_SDO_process_done()
802 static void CO_SDO_abort(CO_SDO_t *SDO, uint32_t code){ in CO_SDO_abort() argument
803 SDO->CANtxBuff->data[0] = 0x80; in CO_SDO_abort()
804 SDO->CANtxBuff->data[1] = SDO->ODF_arg.index & 0xFF; in CO_SDO_abort()
805 SDO->CANtxBuff->data[2] = (SDO->ODF_arg.index>>8) & 0xFF; in CO_SDO_abort()
806 SDO->CANtxBuff->data[3] = SDO->ODF_arg.subIndex; in CO_SDO_abort()
807 CO_memcpySwap4(&SDO->CANtxBuff->data[4], &code); in CO_SDO_abort()
808 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_abort()
811 while (IS_CANrxNew(SDO->CANrxNew[SDO->CANrxProc])) in CO_SDO_abort()
812 CO_SDO_process_done(SDO, NULL); in CO_SDO_abort()
814 CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff); in CO_SDO_abort()
819 CO_SDO_t *SDO, in CO_SDO_process() argument
830 proc = SDO->CANrxProc; in CO_SDO_process()
831 isNew = IS_CANrxNew(SDO->CANrxNew[proc]); in CO_SDO_process()
834 if((SDO->state == CO_SDO_ST_IDLE) && (!isNew)){ in CO_SDO_process()
840 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
843 CO_SDO_process_done(SDO, timerNext_ms); in CO_SDO_process()
847 CANrxData = SDO->CANrxData[proc]; in CO_SDO_process()
850 if((!SDO->CANtxBuff->bufferFull) && (isNew || (SDO->state == CO_SDO_ST_UPLOAD_BL_SUBBLOCK))){ in CO_SDO_process()
852 if(SDO->state != CO_SDO_ST_UPLOAD_BL_SUBBLOCK) in CO_SDO_process()
853 SDO->timeoutTimer = 0; in CO_SDO_process()
856 …SDO->CANtxBuff->data[0] = SDO->CANtxBuff->data[1] = SDO->CANtxBuff->data[2] = SDO->CANtxBuff->data… in CO_SDO_process()
857 …SDO->CANtxBuff->data[4] = SDO->CANtxBuff->data[5] = SDO->CANtxBuff->data[6] = SDO->CANtxBuff->data… in CO_SDO_process()
861 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
862 CO_SDO_process_done(SDO, timerNext_ms); in CO_SDO_process()
867 if(SDO->state != CO_SDO_ST_IDLE){ in CO_SDO_process()
868 state = SDO->state; in CO_SDO_process()
878 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
885 abortCode = CO_SDO_initTransfer(SDO, index, CANrxData[3]); in CO_SDO_process()
887 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
893 if((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0U){ in CO_SDO_process()
894 CO_SDO_abort(SDO, CO_SDO_AB_READONLY); /* attempt to write a read-only object */ in CO_SDO_process()
909 abortCode = CO_SDO_readOD(SDO, CO_SDO_BUFFER_SIZE); in CO_SDO_process()
911 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
916 if((CCS == CCS_UPLOAD_BLOCK) && (SDO->ODF_arg.dataLength > CANrxData[5])){ in CO_SDO_process()
927 if(SDO->timeoutTimer < SDOtimeoutTime){ in CO_SDO_process()
928 SDO->timeoutTimer += timeDifference_ms; in CO_SDO_process()
930 if(SDO->timeoutTimer >= SDOtimeoutTime){ in CO_SDO_process()
931 …if((SDO->state == CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK) && (!SDO->timeoutSubblockDownolad) && (!SDO->CAN… in CO_SDO_process()
933 SDO->timeoutSubblockDownolad = true; in CO_SDO_process()
934 SDO->timeoutTimer = 0; in CO_SDO_process()
939 CO_SDO_abort(SDO, CO_SDO_AB_TIMEOUT); /* SDO protocol timed out */ in CO_SDO_process()
957 SDO->CANtxBuff->data[0] = 0x60; in CO_SDO_process()
958 SDO->CANtxBuff->data[1] = CANrxData[1]; in CO_SDO_process()
959 SDO->CANtxBuff->data[2] = CANrxData[2]; in CO_SDO_process()
960 SDO->CANtxBuff->data[3] = CANrxData[3]; in CO_SDO_process()
969 len = SDO->ODF_arg.dataLength; in CO_SDO_process()
973 SDO->ODF_arg.data[0] = CANrxData[4]; in CO_SDO_process()
974 SDO->ODF_arg.data[1] = CANrxData[5]; in CO_SDO_process()
975 SDO->ODF_arg.data[2] = CANrxData[6]; in CO_SDO_process()
976 SDO->ODF_arg.data[3] = CANrxData[7]; in CO_SDO_process()
979 abortCode = CO_SDO_writeOD(SDO, len); in CO_SDO_process()
981 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
986 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
996 SDO->ODF_arg.dataLengthTotal = lenRx; in CO_SDO_process()
999 if((lenRx != SDO->ODF_arg.dataLength) && (SDO->ODF_arg.ODdataStorage != 0)){ in CO_SDO_process()
1000 … CO_SDO_abort(SDO, CO_SDO_AB_TYPE_MISMATCH); /* Length of service parameter does not match */ in CO_SDO_process()
1004 SDO->bufferOffset = 0U; in CO_SDO_process()
1005 SDO->sequence = 0U; in CO_SDO_process()
1006 SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENTED; in CO_SDO_process()
1015 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1021 if(i != SDO->sequence){ in CO_SDO_process()
1022 CO_SDO_abort(SDO, CO_SDO_AB_TOGGLE_BIT);/* toggle bit not alternated */ in CO_SDO_process()
1030 if((SDO->bufferOffset + len) > SDO->ODF_arg.dataLength){ in CO_SDO_process()
1031 if(SDO->ODF_arg.ODdataStorage != 0){ in CO_SDO_process()
1032 … CO_SDO_abort(SDO, CO_SDO_AB_DATA_LONG); /* Length of service parameter too high */ in CO_SDO_process()
1037 SDO->ODF_arg.lastSegment = false; in CO_SDO_process()
1038 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset); in CO_SDO_process()
1040 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
1044 SDO->ODF_arg.dataLength = CO_SDO_BUFFER_SIZE; in CO_SDO_process()
1045 SDO->bufferOffset = 0U; in CO_SDO_process()
1051 SDO->ODF_arg.data[SDO->bufferOffset++] = CANrxData[i+1]; in CO_SDO_process()
1055 SDO->ODF_arg.lastSegment = true; in CO_SDO_process()
1056 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset); in CO_SDO_process()
1058 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
1063 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
1067 SDO->CANtxBuff->data[0] = 0x20 | (SDO->sequence ? 0x10 : 0x00); in CO_SDO_process()
1068 SDO->sequence = (SDO->sequence) ? 0 : 1; in CO_SDO_process()
1076 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1081 SDO->CANtxBuff->data[0] = 0xA4; in CO_SDO_process()
1082 SDO->CANtxBuff->data[1] = CANrxData[1]; in CO_SDO_process()
1083 SDO->CANtxBuff->data[2] = CANrxData[2]; in CO_SDO_process()
1084 SDO->CANtxBuff->data[3] = CANrxData[3]; in CO_SDO_process()
1087 SDO->blksize = (CO_SDO_BUFFER_SIZE > (7*127)) ? 127 : (CO_SDO_BUFFER_SIZE / 7); in CO_SDO_process()
1088 SDO->CANtxBuff->data[4] = SDO->blksize; in CO_SDO_process()
1091 SDO->crcEnabled = (CANrxData[0] & 0x04) ? true : false; in CO_SDO_process()
1092 SDO->crc = 0; in CO_SDO_process()
1098 SDO->ODF_arg.dataLengthTotal = lenRx; in CO_SDO_process()
1101 if((lenRx != SDO->ODF_arg.dataLength) && (SDO->ODF_arg.ODdataStorage != 0)){ in CO_SDO_process()
1102 … CO_SDO_abort(SDO, CO_SDO_AB_TYPE_MISMATCH); /* Length of service parameter does not match */ in CO_SDO_process()
1107 SDO->bufferOffset = 0U; in CO_SDO_process()
1108 SDO->sequence = 0U; in CO_SDO_process()
1109 SDO->timeoutSubblockDownolad = false; in CO_SDO_process()
1110 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK; in CO_SDO_process()
1125 lastSegmentInSubblock = (!SDO->timeoutSubblockDownolad && in CO_SDO_process()
1129 SDO->CANtxBuff->data[0] = 0xA2; in CO_SDO_process()
1130 SDO->CANtxBuff->data[1] = SDO->sequence; in CO_SDO_process()
1134 SDO->sequence = 0U; in CO_SDO_process()
1137 … if((SDO->ODF_arg.ODdataStorage == 0) && (SDO->bufferOffset != 0) && !lastSegmentInSubblock){ in CO_SDO_process()
1139 if(SDO->crcEnabled){ in CO_SDO_process()
1140 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc); in CO_SDO_process()
1144 SDO->ODF_arg.lastSegment = false; in CO_SDO_process()
1145 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset); in CO_SDO_process()
1147 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
1151 SDO->ODF_arg.dataLength = CO_SDO_BUFFER_SIZE; in CO_SDO_process()
1152 SDO->bufferOffset = 0U; in CO_SDO_process()
1156 len = CO_SDO_BUFFER_SIZE - SDO->bufferOffset; in CO_SDO_process()
1157 SDO->blksize = (len > (7*127)) ? 127 : (len / 7); in CO_SDO_process()
1158 SDO->CANtxBuff->data[2] = SDO->blksize; in CO_SDO_process()
1162 SDO->state = CO_SDO_ST_DOWNLOAD_BL_END; in CO_SDO_process()
1164 else if(SDO->bufferOffset >= CO_SDO_BUFFER_SIZE) { in CO_SDO_process()
1165 CO_SDO_abort(SDO, CO_SDO_AB_DEVICE_INCOMPAT); in CO_SDO_process()
1169 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK; in CO_SDO_process()
1181 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1187 SDO->bufferOffset -= len; in CO_SDO_process()
1190 if(SDO->crcEnabled){ in CO_SDO_process()
1192 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc); in CO_SDO_process()
1196 if(SDO->crc != crc){ in CO_SDO_process()
1197 CO_SDO_abort(SDO, CO_SDO_AB_CRC); /* CRC error (block mode only). */ in CO_SDO_process()
1203 SDO->ODF_arg.lastSegment = true; in CO_SDO_process()
1204 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset); in CO_SDO_process()
1206 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
1211 SDO->CANtxBuff->data[0] = 0xA1; in CO_SDO_process()
1212 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
1219 SDO->CANtxBuff->data[1] = CANrxData[1]; in CO_SDO_process()
1220 SDO->CANtxBuff->data[2] = CANrxData[2]; in CO_SDO_process()
1221 SDO->CANtxBuff->data[3] = CANrxData[3]; in CO_SDO_process()
1224 if(SDO->ODF_arg.dataLength <= 4U){ in CO_SDO_process()
1225 for(i=0U; i<SDO->ODF_arg.dataLength; i++) in CO_SDO_process()
1226 SDO->CANtxBuff->data[4U+i] = SDO->ODF_arg.data[i]; in CO_SDO_process()
1228 SDO->CANtxBuff->data[0] = 0x43U | ((4U-SDO->ODF_arg.dataLength) << 2U); in CO_SDO_process()
1229 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
1236 SDO->bufferOffset = 0U; in CO_SDO_process()
1237 SDO->sequence = 0U; in CO_SDO_process()
1238 SDO->state = CO_SDO_ST_UPLOAD_SEGMENTED; in CO_SDO_process()
1241 if(SDO->ODF_arg.dataLengthTotal != 0U){ in CO_SDO_process()
1242 uint32_t dlentot = SDO->ODF_arg.dataLengthTotal; in CO_SDO_process()
1243 CO_memcpySwap4(&SDO->CANtxBuff->data[4], &dlentot); in CO_SDO_process()
1244 SDO->CANtxBuff->data[0] = 0x41U; in CO_SDO_process()
1247 SDO->CANtxBuff->data[0] = 0x40U; in CO_SDO_process()
1259 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1265 if(i != SDO->sequence){ in CO_SDO_process()
1266 CO_SDO_abort(SDO, CO_SDO_AB_TOGGLE_BIT);/* toggle bit not alternated */ in CO_SDO_process()
1271 len = SDO->ODF_arg.dataLength - SDO->bufferOffset; in CO_SDO_process()
1275 if((SDO->ODF_arg.ODdataStorage == 0) && (len < 7U) && (!SDO->ODF_arg.lastSegment)){ in CO_SDO_process()
1278 SDO->ODF_arg.data[i] = SDO->ODF_arg.data[SDO->bufferOffset+i]; in CO_SDO_process()
1282 SDO->ODF_arg.data += len; in CO_SDO_process()
1283 … SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, SDO->ODF_arg.subIndex) - len; in CO_SDO_process()
1286 abortCode = CO_SDO_readOD(SDO, CO_SDO_BUFFER_SIZE); in CO_SDO_process()
1288 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
1293 SDO->ODF_arg.data -= len; in CO_SDO_process()
1294 SDO->ODF_arg.dataLength += len; in CO_SDO_process()
1295 SDO->bufferOffset = 0; in CO_SDO_process()
1298 len = SDO->ODF_arg.dataLength; in CO_SDO_process()
1304 SDO->CANtxBuff->data[i+1] = SDO->ODF_arg.data[SDO->bufferOffset++]; in CO_SDO_process()
1307 SDO->CANtxBuff->data[0] = 0x00 | (SDO->sequence ? 0x10 : 0x00) | ((7-len)<<1); in CO_SDO_process()
1308 SDO->sequence = (SDO->sequence) ? 0 : 1; in CO_SDO_process()
1311 if((SDO->bufferOffset == SDO->ODF_arg.dataLength) && (SDO->ODF_arg.lastSegment)){ in CO_SDO_process()
1312 SDO->CANtxBuff->data[0] |= 0x01; in CO_SDO_process()
1313 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
1323 SDO->CANtxBuff->data[1] = CANrxData[1]; in CO_SDO_process()
1324 SDO->CANtxBuff->data[2] = CANrxData[2]; in CO_SDO_process()
1325 SDO->CANtxBuff->data[3] = CANrxData[3]; in CO_SDO_process()
1329 SDO->crcEnabled = true; in CO_SDO_process()
1330 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->ODF_arg.dataLength, 0); in CO_SDO_process()
1333 SDO->crcEnabled = false; in CO_SDO_process()
1334 SDO->crc = 0; in CO_SDO_process()
1338 SDO->blksize = CANrxData[4]; in CO_SDO_process()
1342 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1347 if((SDO->blksize < 1U) || (SDO->blksize > 127U) || in CO_SDO_process()
1348 (((SDO->blksize*7U) > SDO->ODF_arg.dataLength) && (!SDO->ODF_arg.lastSegment))){ in CO_SDO_process()
1349 CO_SDO_abort(SDO, CO_SDO_AB_BLOCK_SIZE); /* Invalid block size (block mode only). */ in CO_SDO_process()
1354 if(SDO->ODF_arg.dataLengthTotal != 0U){ in CO_SDO_process()
1355 uint32_t dlentot = SDO->ODF_arg.dataLengthTotal; in CO_SDO_process()
1356 CO_memcpySwap4(&SDO->CANtxBuff->data[4], &dlentot); in CO_SDO_process()
1357 SDO->CANtxBuff->data[0] = 0xC6U; in CO_SDO_process()
1360 SDO->CANtxBuff->data[0] = 0xC4U; in CO_SDO_process()
1364 SDO->state = CO_SDO_ST_UPLOAD_BL_INITIATE_2; in CO_SDO_process()
1372 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1376 SDO->bufferOffset = 0U; in CO_SDO_process()
1377 SDO->sequence = 0U; in CO_SDO_process()
1378 SDO->endOfTransfer = false; in CO_SDO_process()
1379 CO_SDO_process_done(SDO, timerNext_ms); in CO_SDO_process()
1381 SDO->state = CO_SDO_ST_UPLOAD_BL_SUBBLOCK; in CO_SDO_process()
1394 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1401 if(ackseq > SDO->sequence){ in CO_SDO_process()
1402 CO_SDO_abort(SDO, CO_SDO_AB_SEQ_NUM); /* Invalid sequence */ in CO_SDO_process()
1407 if((SDO->endOfTransfer) && (ackseq == SDO->blksize)){ in CO_SDO_process()
1409 SDO->CANtxBuff->data[0] = 0xC1 | ((7 - SDO->lastLen) << 2); in CO_SDO_process()
1412 if(SDO->crcEnabled) in CO_SDO_process()
1413 CO_memcpySwap2(&SDO->CANtxBuff->data[1], &SDO->crc); in CO_SDO_process()
1415 SDO->state = CO_SDO_ST_UPLOAD_BL_END; in CO_SDO_process()
1423 for(i=ackseq*7, j=0; i<SDO->ODF_arg.dataLength; i++, j++) in CO_SDO_process()
1424 SDO->ODF_arg.data[j] = SDO->ODF_arg.data[i]; in CO_SDO_process()
1427 SDO->ODF_arg.dataLength -= ackseq * 7U; in CO_SDO_process()
1430 SDO->blksize = CANrxData[2]; in CO_SDO_process()
1433 …if((SDO->ODF_arg.ODdataStorage == 0) && (SDO->ODF_arg.dataLength < (SDO->blksize*7U)) && (!SDO->OD… in CO_SDO_process()
1435 len = SDO->ODF_arg.dataLength; /* length of valid data in buffer */ in CO_SDO_process()
1436 SDO->ODF_arg.data += len; in CO_SDO_process()
1437 … SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, SDO->ODF_arg.subIndex) - len; in CO_SDO_process()
1440 abortCode = CO_SDO_readOD(SDO, CO_SDO_BUFFER_SIZE); in CO_SDO_process()
1442 CO_SDO_abort(SDO, abortCode); in CO_SDO_process()
1447 if(SDO->crcEnabled){ in CO_SDO_process()
1448 … SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->ODF_arg.dataLength, SDO->crc); in CO_SDO_process()
1452 SDO->ODF_arg.data -= len; in CO_SDO_process()
1453 SDO->ODF_arg.dataLength += len; in CO_SDO_process()
1457 if(((SDO->blksize*7U) > SDO->ODF_arg.dataLength) && (!SDO->ODF_arg.lastSegment)){ in CO_SDO_process()
1458 … CO_SDO_abort(SDO, CO_SDO_AB_BLOCK_SIZE); /* Invalid block size (block mode only). */ in CO_SDO_process()
1462 SDO->bufferOffset = 0U; in CO_SDO_process()
1463 SDO->sequence = 0U; in CO_SDO_process()
1464 SDO->endOfTransfer = false; in CO_SDO_process()
1468 if((SDO->sequence == SDO->blksize) || (SDO->endOfTransfer)){ in CO_SDO_process()
1473 SDO->timeoutTimer = 0; in CO_SDO_process()
1476 len = SDO->ODF_arg.dataLength - SDO->bufferOffset; in CO_SDO_process()
1483 SDO->CANtxBuff->data[i+1] = SDO->ODF_arg.data[SDO->bufferOffset++]; in CO_SDO_process()
1487 SDO->CANtxBuff->data[0] = ++SDO->sequence; in CO_SDO_process()
1490 if((SDO->bufferOffset == SDO->ODF_arg.dataLength) && (SDO->ODF_arg.lastSegment)){ in CO_SDO_process()
1491 SDO->CANtxBuff->data[0] |= 0x80; in CO_SDO_process()
1492 SDO->lastLen = len; in CO_SDO_process()
1493 SDO->blksize = SDO->sequence; in CO_SDO_process()
1494 SDO->endOfTransfer = true; in CO_SDO_process()
1511 … CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */ in CO_SDO_process()
1515 SDO->state = CO_SDO_ST_IDLE; in CO_SDO_process()
1526 … CO_SDO_abort(SDO, CO_SDO_AB_DEVICE_INCOMPAT);/* general internal incompatibility in the device */ in CO_SDO_process()
1532 CO_SDO_process_done(SDO, timerNext_ms); in CO_SDO_process()
1536 CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff); in CO_SDO_process()
1539 if(SDO->state != CO_SDO_ST_IDLE){ in CO_SDO_process()