1 /*
2 * CANopen Service Data Object - server.
3 *
4 * @file CO_SDO.c
5 * @ingroup CO_SDO
6 * @author Janez Paternoster
7 * @copyright 2004 - 2020 Janez Paternoster
8 *
9 * This file is part of CANopenNode, an opensource CANopen Stack.
10 * Project home page is <https://github.com/CANopenNode/CANopenNode>.
11 * For more information on CANopen see <http://www.can-cia.org/>.
12 *
13 * Licensed under the Apache License, Version 2.0 (the "License");
14 * you may not use this file except in compliance with the License.
15 * You may obtain a copy of the License at
16 *
17 * http://www.apache.org/licenses/LICENSE-2.0
18 *
19 * Unless required by applicable law or agreed to in writing, software
20 * distributed under the License is distributed on an "AS IS" BASIS,
21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 * See the License for the specific language governing permissions and
23 * limitations under the License.
24 */
25
26
27 #include "CO_driver.h"
28 #include "CO_SDO.h"
29 #include "crc16-ccitt.h"
30
31
32 /* Client command specifier, see DS301 */
33 #define CCS_DOWNLOAD_INITIATE 1U
34 #define CCS_DOWNLOAD_SEGMENT 0U
35 #define CCS_UPLOAD_INITIATE 2U
36 #define CCS_UPLOAD_SEGMENT 3U
37 #define CCS_DOWNLOAD_BLOCK 6U
38 #define CCS_UPLOAD_BLOCK 5U
39 #define CCS_ABORT 0x80U
40
41
42 #if CO_SDO_BUFFER_SIZE < 7
43 #error CO_SDO_BUFFER_SIZE must be greater than 7
44 #endif
45
46
47 /* Helper functions. **********************************************************/
CO_memcpy(uint8_t dest[],const uint8_t src[],const uint16_t size)48 void CO_memcpy(uint8_t dest[], const uint8_t src[], const uint16_t size){
49 uint16_t i;
50 for(i = 0; i < size; i++){
51 dest[i] = src[i];
52 }
53 }
54
CO_memset(uint8_t dest[],uint8_t c,const uint16_t size)55 void CO_memset(uint8_t dest[], uint8_t c, const uint16_t size){
56 uint16_t i;
57 for(i = 0; i < size; i++){
58 dest[i] = c;
59 }
60 }
61
CO_getUint16(const uint8_t data[])62 uint16_t CO_getUint16(const uint8_t data[]){
63 CO_bytes_t b;
64 b.u8[0] = data[0];
65 b.u8[1] = data[1];
66 return b.u16[0];
67 }
68
CO_getUint32(const uint8_t data[])69 uint32_t CO_getUint32(const uint8_t data[]){
70 CO_bytes_t b;
71 b.u8[0] = data[0];
72 b.u8[1] = data[1];
73 b.u8[2] = data[2];
74 b.u8[3] = data[3];
75 return b.u32[0];
76 }
77
CO_setUint16(uint8_t data[],const uint16_t value)78 void CO_setUint16(uint8_t data[], const uint16_t value){
79 CO_bytes_t b;
80 b.u16[0] = value;
81 data[0] = b.u8[0];
82 data[1] = b.u8[1];
83 }
84
CO_setUint32(uint8_t data[],const uint32_t value)85 void CO_setUint32(uint8_t data[], const uint32_t value){
86 CO_bytes_t b;
87 b.u32[0] = value;
88 data[0] = b.u8[0];
89 data[1] = b.u8[1];
90 data[2] = b.u8[2];
91 data[3] = b.u8[3];
92 }
93
94 #ifdef CO_LITTLE_ENDIAN
CO_memcpySwap2(void * dest,const void * src)95 void CO_memcpySwap2(void* dest, const void* src){
96 char *cdest;
97 char *csrc;
98 cdest = (char *) dest;
99 csrc = (char *) src;
100 cdest[0] = csrc[0];
101 cdest[1] = csrc[1];
102 }
CO_memcpySwap4(void * dest,const void * src)103 void CO_memcpySwap4(void* dest, const void* src){
104 char *cdest;
105 char *csrc;
106 cdest = (char *) dest;
107 csrc = (char *) src;
108 cdest[0] = csrc[0];
109 cdest[1] = csrc[1];
110 cdest[2] = csrc[2];
111 cdest[3] = csrc[3];
112 }
CO_memcpySwap8(void * dest,const void * src)113 void CO_memcpySwap8(void* dest, const void* src){
114 char *cdest;
115 char *csrc;
116 cdest = (char *) dest;
117 csrc = (char *) src;
118 cdest[0] = csrc[0];
119 cdest[1] = csrc[1];
120 cdest[2] = csrc[2];
121 cdest[3] = csrc[3];
122 cdest[4] = csrc[4];
123 cdest[5] = csrc[5];
124 cdest[6] = csrc[6];
125 cdest[7] = csrc[7];
126 }
127 #endif
128 #ifdef CO_BIG_ENDIAN
CO_memcpySwap2(void * dest,const void * src)129 void CO_memcpySwap2(void* dest, const void* src){
130 char *cdest;
131 char *csrc;
132 cdest = (char *) dest;
133 csrc = (char *) src;
134 cdest[0] = csrc[1];
135 cdest[1] = csrc[0];
136 }
CO_memcpySwap4(void * dest,const void * src)137 void CO_memcpySwap4(void* dest, const void* src){
138 char *cdest;
139 char *csrc;
140 cdest = (char *) dest;
141 csrc = (char *) src;
142 cdest[0] = csrc[3];
143 cdest[1] = csrc[2];
144 cdest[2] = csrc[1];
145 cdest[3] = csrc[0];
146 }
CO_memcpySwap8(void * dest,const void * src)147 void CO_memcpySwap8(void* dest, const void* src){
148 char *cdest;
149 char *csrc;
150 cdest = (char *) dest;
151 csrc = (char *) src;
152 cdest[0] = csrc[7];
153 cdest[1] = csrc[6];
154 cdest[2] = csrc[5];
155 cdest[3] = csrc[4];
156 cdest[4] = csrc[3];
157 cdest[5] = csrc[2];
158 cdest[6] = csrc[1];
159 cdest[7] = csrc[0];
160 }
161 #endif
162
CO_SDO_receive_done(CO_SDO_t * SDO)163 static void CO_SDO_receive_done(CO_SDO_t *SDO){
164 #if CO_SDO_RX_DATA_SIZE > 1
165 uint8_t rcv = SDO->CANrxRcv;
166 uint8_t newRcv = rcv;
167
168 if (++newRcv >= CO_SDO_RX_DATA_SIZE)
169 newRcv = 0;
170 SDO->CANrxRcv = newRcv;
171 SET_CANrxNew(SDO->CANrxNew[rcv]);
172 #else
173 SET_CANrxNew(SDO->CANrxNew[0]);
174 #endif
175 }
176
177 /*
178 * Read received message from CAN module.
179 *
180 * Function will be called (by CAN receive interrupt) every time, when CAN
181 * message with correct identifier will be received. For more information and
182 * description of parameters see file CO_driver.h.
183 */
184 static void CO_SDO_receive(void *object, const CO_CANrxMsg_t *msg);
CO_SDO_receive(void * object,const CO_CANrxMsg_t * msg)185 static void CO_SDO_receive(void *object, const CO_CANrxMsg_t *msg){
186 CO_SDO_t *SDO;
187 uint8_t rcv, *CANrxData;
188
189 SDO = (CO_SDO_t*)object; /* this is the correct pointer type of the first argument */
190 rcv = SDO->CANrxRcv;
191 CANrxData = SDO->CANrxData[rcv];
192
193 /* verify message length and message queue overflow (if previous messages were not processed yet) */
194 if((msg->DLC == 8U) && (!IS_CANrxNew(SDO->CANrxNew[rcv]))){
195 if(SDO->state != CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK) {
196 /* copy data and set 'new message' flag */
197 CANrxData[0] = msg->data[0];
198 CANrxData[1] = msg->data[1];
199 CANrxData[2] = msg->data[2];
200 CANrxData[3] = msg->data[3];
201 CANrxData[4] = msg->data[4];
202 CANrxData[5] = msg->data[5];
203 CANrxData[6] = msg->data[6];
204 CANrxData[7] = msg->data[7];
205
206 CO_SDO_receive_done(SDO);
207 }
208 else {
209 /* block download, copy data directly */
210 uint8_t seqno;
211
212 CANrxData[0] = msg->data[0];
213 seqno = CANrxData[0] & 0x7fU;
214 SDO->timeoutTimer = 0;
215 /* clear timeout in sub-block transfer indication if set before */
216 if (SDO->timeoutSubblockDownolad)
217 SDO->timeoutSubblockDownolad = false;
218
219 /* check correct sequence number. */
220 if(seqno == (SDO->sequence + 1U)) {
221 /* sequence is correct */
222
223 /* check if buffer can store whole message just in case */
224 if (CO_SDO_BUFFER_SIZE - SDO->bufferOffset >= 7) {
225 uint8_t i;
226
227 SDO->sequence++;
228
229 /* copy data */
230 for(i=1; i<8; i++) {
231 SDO->ODF_arg.data[SDO->bufferOffset++] = msg->data[i]; //SDO->ODF_arg.data is equal as SDO->databuffer
232 }
233
234 /* break reception if last segment, block ends or block sequence is too large */
235 if(((CANrxData[0] & 0x80U) == 0x80U) || (SDO->sequence >= SDO->blksize)) {
236 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP;
237 CO_SDO_receive_done(SDO);
238 }
239 } else {
240 /* buffer is full, ignore this segment, send response without resetting sequence */
241 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2;
242 CO_SDO_receive_done(SDO);
243 }
244 }
245 else if((seqno == SDO->sequence) || (SDO->sequence == 0U)) {
246 /* Ignore message, if it is duplicate or if sequence didn't started yet. */
247 }
248 else {
249 /* seqno is wrong, send response without resetting sequence */
250 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2;
251 CO_SDO_receive_done(SDO);
252 }
253 }
254
255 /* Optional signal to RTOS, which can resume task, which handles SDO server. */
256 if((IS_CANrxNew(SDO->CANrxNew[rcv])) && (SDO->pFunctSignal != NULL)) {
257 SDO->pFunctSignal();
258 }
259 }
260 }
261
262
263 /*
264 * Function for accessing _SDO server parameter_ for default SDO (index 0x1200)
265 * from SDO server.
266 *
267 * For more information see file CO_SDO.h.
268 */
269 static CO_SDO_abortCode_t CO_ODF_1200(CO_ODF_arg_t *ODF_arg);
CO_ODF_1200(CO_ODF_arg_t * ODF_arg)270 static CO_SDO_abortCode_t CO_ODF_1200(CO_ODF_arg_t *ODF_arg){
271 uint8_t *nodeId;
272 uint32_t value;
273 CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
274
275 nodeId = (uint8_t*) ODF_arg->object;
276 value = CO_getUint32(ODF_arg->data);
277
278 /* if SDO reading Object dictionary 0x1200, add nodeId to the value */
279 if((ODF_arg->reading) && (ODF_arg->subIndex > 0U)){
280 CO_setUint32(ODF_arg->data, value + *nodeId);
281 }
282
283 return ret;
284 }
285
286
287 /******************************************************************************/
CO_SDO_init(CO_SDO_t * SDO,uint32_t COB_IDClientToServer,uint32_t COB_IDServerToClient,uint16_t ObjDictIndex_SDOServerParameter,CO_SDO_t * parentSDO,const CO_OD_entry_t OD[],uint16_t ODSize,CO_OD_extension_t * ODExtensions,uint8_t nodeId,CO_CANmodule_t * CANdevRx,uint16_t CANdevRxIdx,CO_CANmodule_t * CANdevTx,uint16_t CANdevTxIdx)288 CO_ReturnError_t CO_SDO_init(
289 CO_SDO_t *SDO,
290 uint32_t COB_IDClientToServer,
291 uint32_t COB_IDServerToClient,
292 uint16_t ObjDictIndex_SDOServerParameter,
293 CO_SDO_t *parentSDO,
294 const CO_OD_entry_t OD[],
295 uint16_t ODSize,
296 CO_OD_extension_t *ODExtensions,
297 uint8_t nodeId,
298 CO_CANmodule_t *CANdevRx,
299 uint16_t CANdevRxIdx,
300 CO_CANmodule_t *CANdevTx,
301 uint16_t CANdevTxIdx)
302 {
303 /* verify arguments */
304 if(SDO==NULL || CANdevRx==NULL || CANdevTx==NULL){
305 return CO_ERROR_ILLEGAL_ARGUMENT;
306 }
307
308 /* configure own object dictionary */
309 if(parentSDO == NULL){
310 uint16_t i;
311
312 SDO->ownOD = true;
313 SDO->OD = OD;
314 SDO->ODSize = ODSize;
315 SDO->ODExtensions = ODExtensions;
316
317 /* clear pointers in ODExtensions */
318 for(i=0U; i<ODSize; i++){
319 SDO->ODExtensions[i].pODFunc = NULL;
320 SDO->ODExtensions[i].object = NULL;
321 SDO->ODExtensions[i].flags = NULL;
322 }
323 }
324 /* copy object dictionary from parent */
325 else{
326 SDO->ownOD = false;
327 SDO->OD = parentSDO->OD;
328 SDO->ODSize = parentSDO->ODSize;
329 SDO->ODExtensions = parentSDO->ODExtensions;
330 }
331
332 /* Configure object variables */
333 SDO->nodeId = nodeId;
334 SDO->state = CO_SDO_ST_IDLE;
335
336 uint8_t i;
337 for(i=0U; i<CO_SDO_RX_DATA_SIZE; i++){
338 CLEAR_CANrxNew(SDO->CANrxNew[i]);
339 }
340 SDO->CANrxRcv = 0;
341 SDO->CANrxProc = 0;
342
343 SDO->pFunctSignal = NULL;
344
345
346 /* Configure Object dictionary entry at index 0x1200 */
347 if(ObjDictIndex_SDOServerParameter == OD_H1200_SDO_SERVER_PARAM){
348 CO_OD_configure(SDO, ObjDictIndex_SDOServerParameter, CO_ODF_1200, (void*)&SDO->nodeId, 0U, 0U);
349 }
350
351 if((COB_IDClientToServer & 0x80000000) != 0 || (COB_IDServerToClient & 0x80000000) != 0 ){
352 // SDO is invalid
353 COB_IDClientToServer = 0;
354 COB_IDServerToClient = 0;
355 }
356 /* configure SDO server CAN reception */
357 CO_CANrxBufferInit(
358 CANdevRx, /* CAN device */
359 CANdevRxIdx, /* rx buffer index */
360 COB_IDClientToServer, /* CAN identifier */
361 0x7FF, /* mask */
362 0, /* rtr */
363 (void*)SDO, /* object passed to receive function */
364 CO_SDO_receive); /* this function will process received message */
365
366 /* configure SDO server CAN transmission */
367 SDO->CANdevTx = CANdevTx;
368 SDO->CANtxBuff = CO_CANtxBufferInit(
369 CANdevTx, /* CAN device */
370 CANdevTxIdx, /* index of specific buffer inside CAN module */
371 COB_IDServerToClient, /* CAN identifier */
372 0, /* rtr */
373 8, /* number of data bytes */
374 0); /* synchronous message flag bit */
375
376 return CO_ERROR_NO;
377 }
378
379
380 /******************************************************************************/
CO_SDO_initCallback(CO_SDO_t * SDO,void (* pFunctSignal)(void))381 void CO_SDO_initCallback(
382 CO_SDO_t *SDO,
383 void (*pFunctSignal)(void))
384 {
385 if(SDO != NULL){
386 SDO->pFunctSignal = pFunctSignal;
387 }
388 }
389
390
391 /******************************************************************************/
CO_OD_configure(CO_SDO_t * SDO,uint16_t index,CO_SDO_abortCode_t (* pODFunc)(CO_ODF_arg_t * ODF_arg),void * object,uint8_t * flags,uint8_t flagsSize)392 void CO_OD_configure(
393 CO_SDO_t *SDO,
394 uint16_t index,
395 CO_SDO_abortCode_t (*pODFunc)(CO_ODF_arg_t *ODF_arg),
396 void *object,
397 uint8_t *flags,
398 uint8_t flagsSize)
399 {
400 uint16_t entryNo;
401
402 entryNo = CO_OD_find(SDO, index);
403 if(entryNo < 0xFFFFU){
404 CO_OD_extension_t *ext = &SDO->ODExtensions[entryNo];
405 uint8_t maxSubIndex = SDO->OD[entryNo].maxSubIndex;
406
407 ext->pODFunc = pODFunc;
408 ext->object = object;
409 if((flags != NULL) && (flagsSize != 0U) && (flagsSize == maxSubIndex)){
410 uint16_t i;
411 ext->flags = flags;
412 for(i=0U; i<=maxSubIndex; i++){
413 ext->flags[i] = 0U;
414 }
415 }
416 else{
417 ext->flags = NULL;
418 }
419 }
420 }
421
422
423 /******************************************************************************/
CO_OD_find(CO_SDO_t * SDO,uint16_t index)424 uint16_t CO_OD_find(CO_SDO_t *SDO, uint16_t index){
425 /* Fast search in ordered Object Dictionary. If indexes are mixed, this won't work. */
426 /* If Object Dictionary has up to 2^N entries, then N is max number of loop passes. */
427 uint16_t cur, min, max;
428 const CO_OD_entry_t* object;
429
430 min = 0U;
431 max = SDO->ODSize - 1U;
432 while(min < max){
433 cur = (min + max) / 2;
434 object = &SDO->OD[cur];
435 /* Is object matched */
436 if(index == object->index){
437 return cur;
438 }
439 if(index < object->index){
440 max = cur;
441 if(max) max--;
442 }
443 else
444 min = cur + 1U;
445 }
446
447 if(min == max){
448 object = &SDO->OD[min];
449 /* Is object matched */
450 if(index == object->index){
451 return min;
452 }
453 }
454
455 return 0xFFFFU; /* object does not exist in OD */
456 }
457
458
459 /******************************************************************************/
CO_OD_getLength(CO_SDO_t * SDO,uint16_t entryNo,uint8_t subIndex)460 uint16_t CO_OD_getLength(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
461 const CO_OD_entry_t* object = &SDO->OD[entryNo];
462
463 if(entryNo == 0xFFFFU){
464 return 0U;
465 }
466
467 if(object->maxSubIndex == 0U){ /* Object type is Var */
468 if(object->pData == 0){ /* data type is domain */
469 return CO_SDO_BUFFER_SIZE;
470 }
471 else{
472 return object->length;
473 }
474 }
475 else if(object->attribute != 0U){ /* Object type is Array */
476 if(subIndex == 0U){
477 return 1U;
478 }
479 else if(object->pData == 0){
480 /* data type is domain */
481 return CO_SDO_BUFFER_SIZE;
482 }
483 else{
484 return object->length;
485 }
486 }
487 else{ /* Object type is Record */
488 if(((const CO_OD_entryRecord_t*)(object->pData))[subIndex].pData == 0){
489 /* data type is domain */
490 return CO_SDO_BUFFER_SIZE;
491 }
492 else{
493 return ((const CO_OD_entryRecord_t*)(object->pData))[subIndex].length;
494 }
495 }
496 }
497
498
499 /******************************************************************************/
CO_OD_getAttribute(CO_SDO_t * SDO,uint16_t entryNo,uint8_t subIndex)500 uint16_t CO_OD_getAttribute(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
501 const CO_OD_entry_t* object = &SDO->OD[entryNo];
502
503 if(entryNo == 0xFFFFU){
504 return 0U;
505 }
506
507 if(object->maxSubIndex == 0U){ /* Object type is Var */
508 return object->attribute;
509 }
510 else if(object->attribute != 0U){/* Object type is Array */
511 bool_t exception_1003 = false;
512 uint16_t attr = object->attribute;
513
514 /* Special exception: Object 1003,00 should be writable */
515 if(object->index == 0x1003 && subIndex == 0) {
516 exception_1003 = true;
517 attr |= CO_ODA_WRITEABLE;
518 }
519
520 if(subIndex == 0U && !exception_1003){
521 /* First subIndex is readonly */
522 attr &= ~(CO_ODA_WRITEABLE | CO_ODA_RPDO_MAPABLE);
523 attr |= CO_ODA_READABLE;
524 }
525 return attr;
526 }
527 else{ /* Object type is Record */
528 return ((const CO_OD_entryRecord_t*)(object->pData))[subIndex].attribute;
529 }
530 }
531
532
533 /******************************************************************************/
CO_OD_getDataPointer(CO_SDO_t * SDO,uint16_t entryNo,uint8_t subIndex)534 void* CO_OD_getDataPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
535 const CO_OD_entry_t* object = &SDO->OD[entryNo];
536
537 if(entryNo == 0xFFFFU){
538 return 0;
539 }
540
541 if(object->maxSubIndex == 0U){ /* Object type is Var */
542 return object->pData;
543 }
544 else if(object->maxSubIndex < subIndex){
545 /* Object type Array/Record, request is out of bounds */
546 return 0;
547 }
548 else if(object->attribute != 0U){/* Object type is Array */
549 if(subIndex==0){
550 /* this is the data, for the subIndex 0 in the array */
551 return (void*) &object->maxSubIndex;
552 }
553 else if(object->pData == 0){
554 /* data type is domain */
555 return 0;
556 }
557 else{
558 return (void*)(((int8_t*)object->pData) + ((subIndex-1) * object->length));
559 }
560 }
561 else{ /* Object Type is Record */
562 return ((const CO_OD_entryRecord_t*)(object->pData))[subIndex].pData;
563 }
564 }
565
566
567 /******************************************************************************/
CO_OD_getFlagsPointer(CO_SDO_t * SDO,uint16_t entryNo,uint8_t subIndex)568 uint8_t* CO_OD_getFlagsPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
569 CO_OD_extension_t* ext;
570
571 if((entryNo == 0xFFFFU) || (SDO->ODExtensions == 0)){
572 return 0;
573 }
574
575 ext = &SDO->ODExtensions[entryNo];
576
577 return &ext->flags[subIndex];
578 }
579
580
581 /******************************************************************************/
CO_SDO_initTransfer(CO_SDO_t * SDO,uint16_t index,uint8_t subIndex)582 uint32_t CO_SDO_initTransfer(CO_SDO_t *SDO, uint16_t index, uint8_t subIndex){
583
584 SDO->ODF_arg.index = index;
585 SDO->ODF_arg.subIndex = subIndex;
586
587 /* find object in Object Dictionary */
588 SDO->entryNo = CO_OD_find(SDO, index);
589 if(SDO->entryNo == 0xFFFFU){
590 return CO_SDO_AB_NOT_EXIST ; /* object does not exist in OD */
591 }
592
593 /* verify existance of subIndex */
594 if(subIndex > SDO->OD[SDO->entryNo].maxSubIndex &&
595 SDO->OD[SDO->entryNo].pData != NULL)
596 {
597 return CO_SDO_AB_SUB_UNKNOWN; /* Sub-index does not exist. */
598 }
599
600 /* pointer to data in Object dictionary */
601 SDO->ODF_arg.ODdataStorage = CO_OD_getDataPointer(SDO, SDO->entryNo, subIndex);
602
603 /* fill ODF_arg */
604 SDO->ODF_arg.object = NULL;
605 if(SDO->ODExtensions){
606 CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo];
607 SDO->ODF_arg.object = ext->object;
608 }
609 SDO->ODF_arg.data = SDO->databuffer;
610 SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, subIndex);
611 SDO->ODF_arg.attribute = CO_OD_getAttribute(SDO, SDO->entryNo, subIndex);
612 SDO->ODF_arg.pFlags = CO_OD_getFlagsPointer(SDO, SDO->entryNo, subIndex);
613
614 SDO->ODF_arg.firstSegment = true;
615 SDO->ODF_arg.lastSegment = true;
616
617 /* indicate total data length, if not domain */
618 SDO->ODF_arg.dataLengthTotal = (SDO->ODF_arg.ODdataStorage) ? SDO->ODF_arg.dataLength : 0U;
619
620 SDO->ODF_arg.offset = 0U;
621
622 /* verify length */
623 if(SDO->ODF_arg.dataLength > CO_SDO_BUFFER_SIZE){
624 return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
625 }
626
627 return 0U;
628 }
629
630
631 /******************************************************************************/
CO_SDO_readOD(CO_SDO_t * SDO,uint16_t SDOBufferSize)632 uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize){
633 uint8_t *SDObuffer = SDO->ODF_arg.data;
634 uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
635 uint16_t length = SDO->ODF_arg.dataLength;
636 CO_OD_extension_t *ext = 0;
637
638 /* is object readable? */
639 if((SDO->ODF_arg.attribute & CO_ODA_READABLE) == 0)
640 return CO_SDO_AB_WRITEONLY; /* attempt to read a write-only object */
641
642 /* find extension */
643 if(SDO->ODExtensions != NULL){
644 ext = &SDO->ODExtensions[SDO->entryNo];
645 }
646
647 CO_LOCK_OD();
648
649 /* copy data from OD to SDO buffer if not domain */
650 if(ODdata != NULL){
651 while(length--) *(SDObuffer++) = *(ODdata++);
652 }
653 /* if domain, Object dictionary function MUST exist */
654 else{
655 if(ext->pODFunc == NULL){
656 CO_UNLOCK_OD();
657 return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
658 }
659 }
660
661 /* call Object dictionary function if registered */
662 SDO->ODF_arg.reading = true;
663 if(ext->pODFunc != NULL){
664 uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
665 if(abortCode != 0U){
666 CO_UNLOCK_OD();
667 return abortCode;
668 }
669
670 /* dataLength (upadted by pODFunc) must be inside limits */
671 if((SDO->ODF_arg.dataLength == 0U) || (SDO->ODF_arg.dataLength > SDOBufferSize)){
672 CO_UNLOCK_OD();
673 return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
674 }
675 }
676
677 CO_UNLOCK_OD();
678
679 SDO->ODF_arg.offset += SDO->ODF_arg.dataLength;
680 SDO->ODF_arg.firstSegment = false;
681
682 /* swap data if processor is not little endian (CANopen is) */
683 #ifdef CO_BIG_ENDIAN
684 if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
685 uint16_t len = SDO->ODF_arg.dataLength;
686 uint8_t *buf1 = SDO->ODF_arg.data;
687 uint8_t *buf2 = buf1 + len - 1;
688
689 len /= 2;
690 while(len--){
691 uint8_t b = *buf1;
692 *(buf1++) = *buf2;
693 *(buf2--) = b;
694 }
695 }
696 #endif
697
698 return 0U;
699 }
700
701
702 /******************************************************************************/
CO_SDO_writeOD(CO_SDO_t * SDO,uint16_t length)703 uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length){
704 uint8_t *SDObuffer = SDO->ODF_arg.data;
705 uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
706 bool_t exception_1003 = false;
707
708 /* is object writeable? */
709 if((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0){
710 return CO_SDO_AB_READONLY; /* attempt to write a read-only object */
711 }
712
713 /* length of domain data is application specific and not verified */
714 if(ODdata == 0){
715 SDO->ODF_arg.dataLength = length;
716 }
717
718 /* verify length except for domain data type */
719 else if(SDO->ODF_arg.dataLength != length){
720 return CO_SDO_AB_TYPE_MISMATCH; /* Length of service parameter does not match */
721 }
722
723 /* swap data if processor is not little endian (CANopen is) */
724 #ifdef CO_BIG_ENDIAN
725 if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
726 uint16_t len = SDO->ODF_arg.dataLength;
727 uint8_t *buf1 = SDO->ODF_arg.data;
728 uint8_t *buf2 = buf1 + len - 1;
729
730 len /= 2;
731 while(len--){
732 uint8_t b = *buf1;
733 *(buf1++) = *buf2;
734 *(buf2--) = b;
735 }
736 }
737 #endif
738
739 CO_LOCK_OD();
740
741 /* call Object dictionary function if registered */
742 SDO->ODF_arg.reading = false;
743 if(SDO->ODExtensions != NULL){
744 CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo];
745
746 if(ext->pODFunc != NULL){
747 uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
748 if(abortCode != 0U){
749 CO_UNLOCK_OD();
750 return abortCode;
751 }
752 }
753 }
754 SDO->ODF_arg.offset += SDO->ODF_arg.dataLength;
755 SDO->ODF_arg.firstSegment = false;
756
757 /* Special exception: 1003,00 is writable from network, but not in OD */
758 if(SDO->ODF_arg.index == 0x1003 && SDO->ODF_arg.subIndex == 0) {
759 exception_1003 = true;
760 }
761
762 /* copy data from SDO buffer to OD if not domain */
763 if((ODdata != NULL) && !exception_1003){
764 while(length--){
765 *(ODdata++) = *(SDObuffer++);
766 }
767 }
768
769 CO_UNLOCK_OD();
770
771 return 0;
772 }
773
774 /******************************************************************************/
CO_SDO_process_done(CO_SDO_t * SDO,uint16_t * timerNext_ms)775 static void CO_SDO_process_done(CO_SDO_t *SDO, uint16_t *timerNext_ms) {
776 #if CO_SDO_RX_DATA_SIZE > 1
777 uint8_t proc = SDO->CANrxProc;
778 uint8_t newProc = proc;
779
780 /* check if buffer needs to be free */
781 if (!IS_CANrxNew(SDO->CANrxNew[proc])){
782 return;
783 }
784
785 if (++newProc >= CO_SDO_RX_DATA_SIZE)
786 newProc = 0;
787
788 SDO->CANrxProc = newProc;
789 CLEAR_CANrxNew(SDO->CANrxNew[proc]);
790
791 if ((timerNext_ms != NULL) && (IS_CANrxNew(SDO->CANrxNew[newProc]))){
792 /* Set timerNext_ms to 0 to inform OS to call CO_SDO_process function again without delay */
793 timerNext_ms = 0;
794 }
795 #else
796 (void)(timerNext_ms);
797 CLEAR_CANrxNew(SDO->CANrxNew[0]);
798 #endif
799 }
800
801 /******************************************************************************/
CO_SDO_abort(CO_SDO_t * SDO,uint32_t code)802 static void CO_SDO_abort(CO_SDO_t *SDO, uint32_t code){
803 SDO->CANtxBuff->data[0] = 0x80;
804 SDO->CANtxBuff->data[1] = SDO->ODF_arg.index & 0xFF;
805 SDO->CANtxBuff->data[2] = (SDO->ODF_arg.index>>8) & 0xFF;
806 SDO->CANtxBuff->data[3] = SDO->ODF_arg.subIndex;
807 CO_memcpySwap4(&SDO->CANtxBuff->data[4], &code);
808 SDO->state = CO_SDO_ST_IDLE;
809
810 /* skip all received message in queue if any */
811 while (IS_CANrxNew(SDO->CANrxNew[SDO->CANrxProc]))
812 CO_SDO_process_done(SDO, NULL);
813
814 CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
815 }
816
817 /******************************************************************************/
CO_SDO_process(CO_SDO_t * SDO,bool_t NMTisPreOrOperational,uint16_t timeDifference_ms,uint16_t SDOtimeoutTime,uint16_t * timerNext_ms)818 int8_t CO_SDO_process(
819 CO_SDO_t *SDO,
820 bool_t NMTisPreOrOperational,
821 uint16_t timeDifference_ms,
822 uint16_t SDOtimeoutTime,
823 uint16_t *timerNext_ms)
824 {
825 CO_SDO_state_t state = CO_SDO_ST_IDLE;
826 bool_t sendResponse = false;
827 uint8_t proc, *CANrxData;
828 bool_t isNew;
829
830 proc = SDO->CANrxProc;
831 isNew = IS_CANrxNew(SDO->CANrxNew[proc]);
832
833 /* return if idle */
834 if((SDO->state == CO_SDO_ST_IDLE) && (!isNew)){
835 return 0;
836 }
837
838 /* SDO is allowed to work only in operational or pre-operational NMT state */
839 if(!NMTisPreOrOperational){
840 SDO->state = CO_SDO_ST_IDLE;
841
842 /* free receive buffer if it is not empty */
843 CO_SDO_process_done(SDO, timerNext_ms);
844 return 0;
845 }
846
847 CANrxData = SDO->CANrxData[proc];
848
849 /* Is something new to process? */
850 if((!SDO->CANtxBuff->bufferFull) && (isNew || (SDO->state == CO_SDO_ST_UPLOAD_BL_SUBBLOCK))){
851 /* reset timeout */
852 if(SDO->state != CO_SDO_ST_UPLOAD_BL_SUBBLOCK)
853 SDO->timeoutTimer = 0;
854
855 /* clear response buffer */
856 SDO->CANtxBuff->data[0] = SDO->CANtxBuff->data[1] = SDO->CANtxBuff->data[2] = SDO->CANtxBuff->data[3] = 0;
857 SDO->CANtxBuff->data[4] = SDO->CANtxBuff->data[5] = SDO->CANtxBuff->data[6] = SDO->CANtxBuff->data[7] = 0;
858
859 /* Is abort from client? */
860 if(isNew && (CANrxData[0] == CCS_ABORT)){
861 SDO->state = CO_SDO_ST_IDLE;
862 CO_SDO_process_done(SDO, timerNext_ms);
863 return -1;
864 }
865
866 /* continue with previous SDO communication or start new */
867 if(SDO->state != CO_SDO_ST_IDLE){
868 state = SDO->state;
869 }
870 else{
871 uint32_t abortCode;
872 uint16_t index;
873 uint8_t CCS = CANrxData[0] >> 5; /* Client command specifier */
874
875 /* Is client command specifier valid */
876 if((CCS != CCS_DOWNLOAD_INITIATE) && (CCS != CCS_UPLOAD_INITIATE) &&
877 (CCS != CCS_DOWNLOAD_BLOCK) && (CCS != CCS_UPLOAD_BLOCK)){
878 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
879 return -1;
880 }
881
882 /* init ODF_arg */
883 index = CANrxData[2];
884 index = index << 8 | CANrxData[1];
885 abortCode = CO_SDO_initTransfer(SDO, index, CANrxData[3]);
886 if(abortCode != 0U){
887 CO_SDO_abort(SDO, abortCode);
888 return -1;
889 }
890
891 /* download */
892 if((CCS == CCS_DOWNLOAD_INITIATE) || (CCS == CCS_DOWNLOAD_BLOCK)){
893 if((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0U){
894 CO_SDO_abort(SDO, CO_SDO_AB_READONLY); /* attempt to write a read-only object */
895 return -1;
896 }
897
898 /* set state machine to normal or block download */
899 if(CCS == CCS_DOWNLOAD_INITIATE){
900 state = CO_SDO_ST_DOWNLOAD_INITIATE;
901 }
902 else{
903 state = CO_SDO_ST_DOWNLOAD_BL_INITIATE;
904 }
905 }
906
907 /* upload */
908 else{
909 abortCode = CO_SDO_readOD(SDO, CO_SDO_BUFFER_SIZE);
910 if(abortCode != 0U){
911 CO_SDO_abort(SDO, abortCode);
912 return -1;
913 }
914
915 /* if data size is large enough set state machine to block upload, otherwise set to normal transfer */
916 if((CCS == CCS_UPLOAD_BLOCK) && (SDO->ODF_arg.dataLength > CANrxData[5])){
917 state = CO_SDO_ST_UPLOAD_BL_INITIATE;
918 }
919 else{
920 state = CO_SDO_ST_UPLOAD_INITIATE;
921 }
922 }
923 }
924 }
925
926 /* verify SDO timeout */
927 if(SDO->timeoutTimer < SDOtimeoutTime){
928 SDO->timeoutTimer += timeDifference_ms;
929 }
930 if(SDO->timeoutTimer >= SDOtimeoutTime){
931 if((SDO->state == CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK) && (!SDO->timeoutSubblockDownolad) && (!SDO->CANtxBuff->bufferFull)){
932 /* set indication timeout in sub-block transfer and reset timeout */
933 SDO->timeoutSubblockDownolad = true;
934 SDO->timeoutTimer = 0;
935 /* send response without resetting sequence */
936 state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2;
937 }
938 else{
939 CO_SDO_abort(SDO, CO_SDO_AB_TIMEOUT); /* SDO protocol timed out */
940 return -1;
941 }
942 }
943
944 /* return immediately if still idle */
945 if(state == CO_SDO_ST_IDLE){
946 return 0;
947 }
948
949 /* state machine (buffer is freed with process_done() at the end) */
950 switch(state){
951 uint32_t abortCode;
952 uint16_t len, i;
953 bool_t lastSegmentInSubblock;
954
955 case CO_SDO_ST_DOWNLOAD_INITIATE:{
956 /* default response */
957 SDO->CANtxBuff->data[0] = 0x60;
958 SDO->CANtxBuff->data[1] = CANrxData[1];
959 SDO->CANtxBuff->data[2] = CANrxData[2];
960 SDO->CANtxBuff->data[3] = CANrxData[3];
961
962 /* Expedited transfer */
963 if((CANrxData[0] & 0x02U) != 0U){
964 /* is size indicated? Get message length */
965 if((CANrxData[0] & 0x01U) != 0U){
966 len = 4U - ((CANrxData[0] >> 2U) & 0x03U);
967 }
968 else{
969 len = SDO->ODF_arg.dataLength;
970 }
971
972 /* copy data to SDO buffer */
973 SDO->ODF_arg.data[0] = CANrxData[4];
974 SDO->ODF_arg.data[1] = CANrxData[5];
975 SDO->ODF_arg.data[2] = CANrxData[6];
976 SDO->ODF_arg.data[3] = CANrxData[7];
977
978 /* write data to the Object dictionary */
979 abortCode = CO_SDO_writeOD(SDO, len);
980 if(abortCode != 0U){
981 CO_SDO_abort(SDO, abortCode);
982 return -1;
983 }
984
985 /* finish the communication */
986 SDO->state = CO_SDO_ST_IDLE;
987 sendResponse = true;
988 }
989
990 /* Segmented transfer */
991 else{
992 /* verify length if size is indicated */
993 if((CANrxData[0]&0x01) != 0){
994 uint32_t lenRx;
995 CO_memcpySwap4(&lenRx, &CANrxData[4]);
996 SDO->ODF_arg.dataLengthTotal = lenRx;
997
998 /* verify length except for domain data type */
999 if((lenRx != SDO->ODF_arg.dataLength) && (SDO->ODF_arg.ODdataStorage != 0)){
1000 CO_SDO_abort(SDO, CO_SDO_AB_TYPE_MISMATCH); /* Length of service parameter does not match */
1001 return -1;
1002 }
1003 }
1004 SDO->bufferOffset = 0U;
1005 SDO->sequence = 0U;
1006 SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENTED;
1007 sendResponse = true;
1008 }
1009 break;
1010 }
1011
1012 case CO_SDO_ST_DOWNLOAD_SEGMENTED:{
1013 /* verify client command specifier */
1014 if((CANrxData[0]&0xE0) != 0x00U){
1015 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1016 return -1;
1017 }
1018
1019 /* verify toggle bit */
1020 i = (CANrxData[0]&0x10U) ? 1U : 0U;
1021 if(i != SDO->sequence){
1022 CO_SDO_abort(SDO, CO_SDO_AB_TOGGLE_BIT);/* toggle bit not alternated */
1023 return -1;
1024 }
1025
1026 /* get size of data in message */
1027 len = 7U - ((CANrxData[0] >> 1U) & 0x07U);
1028
1029 /* verify length. Domain data type enables length larger than SDO buffer size */
1030 if((SDO->bufferOffset + len) > SDO->ODF_arg.dataLength){
1031 if(SDO->ODF_arg.ODdataStorage != 0){
1032 CO_SDO_abort(SDO, CO_SDO_AB_DATA_LONG); /* Length of service parameter too high */
1033 return -1;
1034 }
1035 else{
1036 /* empty buffer in domain data type */
1037 SDO->ODF_arg.lastSegment = false;
1038 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
1039 if(abortCode != 0U){
1040 CO_SDO_abort(SDO, abortCode);
1041 return -1;
1042 }
1043
1044 SDO->ODF_arg.dataLength = CO_SDO_BUFFER_SIZE;
1045 SDO->bufferOffset = 0U;
1046 }
1047 }
1048
1049 /* copy data to buffer */
1050 for(i=0U; i<len; i++)
1051 SDO->ODF_arg.data[SDO->bufferOffset++] = CANrxData[i+1];
1052
1053 /* If no more segments to be downloaded, write data to the Object dictionary */
1054 if((CANrxData[0] & 0x01U) != 0U){
1055 SDO->ODF_arg.lastSegment = true;
1056 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
1057 if(abortCode != 0U){
1058 CO_SDO_abort(SDO, abortCode);
1059 return -1;
1060 }
1061
1062 /* finish */
1063 SDO->state = CO_SDO_ST_IDLE;
1064 }
1065
1066 /* download segment response and alternate toggle bit */
1067 SDO->CANtxBuff->data[0] = 0x20 | (SDO->sequence ? 0x10 : 0x00);
1068 SDO->sequence = (SDO->sequence) ? 0 : 1;
1069 sendResponse = true;
1070 break;
1071 }
1072
1073 case CO_SDO_ST_DOWNLOAD_BL_INITIATE:{
1074 /* verify client command specifier and subcommand */
1075 if((CANrxData[0]&0xE1U) != 0xC0U){
1076 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1077 return -1;
1078 }
1079
1080 /* prepare response */
1081 SDO->CANtxBuff->data[0] = 0xA4;
1082 SDO->CANtxBuff->data[1] = CANrxData[1];
1083 SDO->CANtxBuff->data[2] = CANrxData[2];
1084 SDO->CANtxBuff->data[3] = CANrxData[3];
1085
1086 /* blksize */
1087 SDO->blksize = (CO_SDO_BUFFER_SIZE > (7*127)) ? 127 : (CO_SDO_BUFFER_SIZE / 7);
1088 SDO->CANtxBuff->data[4] = SDO->blksize;
1089
1090 /* is CRC enabled */
1091 SDO->crcEnabled = (CANrxData[0] & 0x04) ? true : false;
1092 SDO->crc = 0;
1093
1094 /* verify length if size is indicated */
1095 if((CANrxData[0]&0x02) != 0U){
1096 uint32_t lenRx;
1097 CO_memcpySwap4(&lenRx, &CANrxData[4]);
1098 SDO->ODF_arg.dataLengthTotal = lenRx;
1099
1100 /* verify length except for domain data type */
1101 if((lenRx != SDO->ODF_arg.dataLength) && (SDO->ODF_arg.ODdataStorage != 0)){
1102 CO_SDO_abort(SDO, CO_SDO_AB_TYPE_MISMATCH); /* Length of service parameter does not match */
1103 return -1;
1104 }
1105 }
1106
1107 SDO->bufferOffset = 0U;
1108 SDO->sequence = 0U;
1109 SDO->timeoutSubblockDownolad = false;
1110 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK;
1111
1112 /* send response */
1113 sendResponse = true;
1114 break;
1115 }
1116
1117 case CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK:{
1118 /* data are copied directly in receive function */
1119 break;
1120 }
1121
1122 case CO_SDO_ST_DOWNLOAD_BL_SUB_RESP:
1123 case CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2:{
1124 /* check if last segment received */
1125 lastSegmentInSubblock = (!SDO->timeoutSubblockDownolad &&
1126 ((CANrxData[0] & 0x80U) == 0x80U)) ? true : false;
1127
1128 /* prepare response */
1129 SDO->CANtxBuff->data[0] = 0xA2;
1130 SDO->CANtxBuff->data[1] = SDO->sequence;
1131
1132 /* reset sequence on reception break */
1133 if (state == CO_SDO_ST_DOWNLOAD_BL_SUB_RESP)
1134 SDO->sequence = 0U;
1135
1136 /* empty buffer in domain data type if not last segment */
1137 if((SDO->ODF_arg.ODdataStorage == 0) && (SDO->bufferOffset != 0) && !lastSegmentInSubblock){
1138 /* calculate CRC on next bytes, if enabled */
1139 if(SDO->crcEnabled){
1140 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc);
1141 }
1142
1143 /* write data to the Object dictionary */
1144 SDO->ODF_arg.lastSegment = false;
1145 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
1146 if(abortCode != 0U){
1147 CO_SDO_abort(SDO, abortCode);
1148 return -1;
1149 }
1150
1151 SDO->ODF_arg.dataLength = CO_SDO_BUFFER_SIZE;
1152 SDO->bufferOffset = 0U;
1153 }
1154
1155 /* blksize */
1156 len = CO_SDO_BUFFER_SIZE - SDO->bufferOffset;
1157 SDO->blksize = (len > (7*127)) ? 127 : (len / 7);
1158 SDO->CANtxBuff->data[2] = SDO->blksize;
1159
1160 /* set next state */
1161 if(lastSegmentInSubblock) {
1162 SDO->state = CO_SDO_ST_DOWNLOAD_BL_END;
1163 }
1164 else if(SDO->bufferOffset >= CO_SDO_BUFFER_SIZE) {
1165 CO_SDO_abort(SDO, CO_SDO_AB_DEVICE_INCOMPAT);
1166 return -1;
1167 }
1168 else {
1169 SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK;
1170 }
1171
1172 /* send response */
1173 sendResponse = true;
1174
1175 break;
1176 }
1177
1178 case CO_SDO_ST_DOWNLOAD_BL_END:{
1179 /* verify client command specifier and subcommand */
1180 if((CANrxData[0]&0xE1U) != 0xC1U){
1181 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1182 return -1;
1183 }
1184
1185 /* number of bytes in the last segment of the last block that do not contain data. */
1186 len = (CANrxData[0]>>2U) & 0x07U;
1187 SDO->bufferOffset -= len;
1188
1189 /* calculate and verify CRC, if enabled */
1190 if(SDO->crcEnabled){
1191 uint16_t crc;
1192 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc);
1193
1194 CO_memcpySwap2(&crc, &CANrxData[1]);
1195
1196 if(SDO->crc != crc){
1197 CO_SDO_abort(SDO, CO_SDO_AB_CRC); /* CRC error (block mode only). */
1198 return -1;
1199 }
1200 }
1201
1202 /* write data to the Object dictionary */
1203 SDO->ODF_arg.lastSegment = true;
1204 abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
1205 if(abortCode != 0U){
1206 CO_SDO_abort(SDO, abortCode);
1207 return -1;
1208 }
1209
1210 /* send response */
1211 SDO->CANtxBuff->data[0] = 0xA1;
1212 SDO->state = CO_SDO_ST_IDLE;
1213 sendResponse = true;
1214 break;
1215 }
1216
1217 case CO_SDO_ST_UPLOAD_INITIATE:{
1218 /* default response */
1219 SDO->CANtxBuff->data[1] = CANrxData[1];
1220 SDO->CANtxBuff->data[2] = CANrxData[2];
1221 SDO->CANtxBuff->data[3] = CANrxData[3];
1222
1223 /* Expedited transfer */
1224 if(SDO->ODF_arg.dataLength <= 4U){
1225 for(i=0U; i<SDO->ODF_arg.dataLength; i++)
1226 SDO->CANtxBuff->data[4U+i] = SDO->ODF_arg.data[i];
1227
1228 SDO->CANtxBuff->data[0] = 0x43U | ((4U-SDO->ODF_arg.dataLength) << 2U);
1229 SDO->state = CO_SDO_ST_IDLE;
1230
1231 sendResponse = true;
1232 }
1233
1234 /* Segmented transfer */
1235 else{
1236 SDO->bufferOffset = 0U;
1237 SDO->sequence = 0U;
1238 SDO->state = CO_SDO_ST_UPLOAD_SEGMENTED;
1239
1240 /* indicate data size, if known */
1241 if(SDO->ODF_arg.dataLengthTotal != 0U){
1242 uint32_t dlentot = SDO->ODF_arg.dataLengthTotal;
1243 CO_memcpySwap4(&SDO->CANtxBuff->data[4], &dlentot);
1244 SDO->CANtxBuff->data[0] = 0x41U;
1245 }
1246 else{
1247 SDO->CANtxBuff->data[0] = 0x40U;
1248 }
1249
1250 /* send response */
1251 sendResponse = true;
1252 }
1253 break;
1254 }
1255
1256 case CO_SDO_ST_UPLOAD_SEGMENTED:{
1257 /* verify client command specifier */
1258 if((CANrxData[0]&0xE0U) != 0x60U){
1259 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1260 return -1;
1261 }
1262
1263 /* verify toggle bit */
1264 i = ((CANrxData[0]&0x10U) != 0) ? 1U : 0U;
1265 if(i != SDO->sequence){
1266 CO_SDO_abort(SDO, CO_SDO_AB_TOGGLE_BIT);/* toggle bit not alternated */
1267 return -1;
1268 }
1269
1270 /* calculate length to be sent */
1271 len = SDO->ODF_arg.dataLength - SDO->bufferOffset;
1272 if(len > 7U) len = 7U;
1273
1274 /* If data type is domain, re-fill the data buffer if neccessary and indicated so. */
1275 if((SDO->ODF_arg.ODdataStorage == 0) && (len < 7U) && (!SDO->ODF_arg.lastSegment)){
1276 /* copy previous data to the beginning */
1277 for(i=0U; i<len; i++){
1278 SDO->ODF_arg.data[i] = SDO->ODF_arg.data[SDO->bufferOffset+i];
1279 }
1280
1281 /* move the beginning of the data buffer */
1282 SDO->ODF_arg.data += len;
1283 SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, SDO->ODF_arg.subIndex) - len;
1284
1285 /* read next data from Object dictionary function */
1286 abortCode = CO_SDO_readOD(SDO, CO_SDO_BUFFER_SIZE);
1287 if(abortCode != 0U){
1288 CO_SDO_abort(SDO, abortCode);
1289 return -1;
1290 }
1291
1292 /* return to the original data buffer */
1293 SDO->ODF_arg.data -= len;
1294 SDO->ODF_arg.dataLength += len;
1295 SDO->bufferOffset = 0;
1296
1297 /* re-calculate the length */
1298 len = SDO->ODF_arg.dataLength;
1299 if(len > 7U) len = 7U;
1300 }
1301
1302 /* fill response data bytes */
1303 for(i=0U; i<len; i++)
1304 SDO->CANtxBuff->data[i+1] = SDO->ODF_arg.data[SDO->bufferOffset++];
1305
1306 /* first response byte */
1307 SDO->CANtxBuff->data[0] = 0x00 | (SDO->sequence ? 0x10 : 0x00) | ((7-len)<<1);
1308 SDO->sequence = (SDO->sequence) ? 0 : 1;
1309
1310 /* verify end of transfer */
1311 if((SDO->bufferOffset == SDO->ODF_arg.dataLength) && (SDO->ODF_arg.lastSegment)){
1312 SDO->CANtxBuff->data[0] |= 0x01;
1313 SDO->state = CO_SDO_ST_IDLE;
1314 }
1315
1316 /* send response */
1317 sendResponse = true;
1318 break;
1319 }
1320
1321 case CO_SDO_ST_UPLOAD_BL_INITIATE:{
1322 /* default response */
1323 SDO->CANtxBuff->data[1] = CANrxData[1];
1324 SDO->CANtxBuff->data[2] = CANrxData[2];
1325 SDO->CANtxBuff->data[3] = CANrxData[3];
1326
1327 /* calculate CRC, if enabled */
1328 if((CANrxData[0] & 0x04U) != 0U){
1329 SDO->crcEnabled = true;
1330 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->ODF_arg.dataLength, 0);
1331 }
1332 else{
1333 SDO->crcEnabled = false;
1334 SDO->crc = 0;
1335 }
1336
1337 /* Number of segments per block */
1338 SDO->blksize = CANrxData[4];
1339
1340 /* verify client subcommand */
1341 if((CANrxData[0]&0x03U) != 0x00U){
1342 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1343 return -1;
1344 }
1345
1346 /* verify blksize and if SDO data buffer is large enough */
1347 if((SDO->blksize < 1U) || (SDO->blksize > 127U) ||
1348 (((SDO->blksize*7U) > SDO->ODF_arg.dataLength) && (!SDO->ODF_arg.lastSegment))){
1349 CO_SDO_abort(SDO, CO_SDO_AB_BLOCK_SIZE); /* Invalid block size (block mode only). */
1350 return -1;
1351 }
1352
1353 /* indicate data size, if known */
1354 if(SDO->ODF_arg.dataLengthTotal != 0U){
1355 uint32_t dlentot = SDO->ODF_arg.dataLengthTotal;
1356 CO_memcpySwap4(&SDO->CANtxBuff->data[4], &dlentot);
1357 SDO->CANtxBuff->data[0] = 0xC6U;
1358 }
1359 else{
1360 SDO->CANtxBuff->data[0] = 0xC4U;
1361 }
1362
1363 /* send response */
1364 SDO->state = CO_SDO_ST_UPLOAD_BL_INITIATE_2;
1365 sendResponse = true;
1366 break;
1367 }
1368
1369 case CO_SDO_ST_UPLOAD_BL_INITIATE_2:{
1370 /* verify client command specifier and subcommand */
1371 if((CANrxData[0]&0xE3U) != 0xA3U){
1372 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1373 return -1;
1374 }
1375
1376 SDO->bufferOffset = 0U;
1377 SDO->sequence = 0U;
1378 SDO->endOfTransfer = false;
1379 CO_SDO_process_done(SDO, timerNext_ms);
1380 isNew = false;
1381 SDO->state = CO_SDO_ST_UPLOAD_BL_SUBBLOCK;
1382 /* continue in next case */
1383 }
1384 // fallthrough
1385
1386 case CO_SDO_ST_UPLOAD_BL_SUBBLOCK:{
1387 /* is block confirmation received */
1388 if(isNew){
1389 uint8_t ackseq;
1390 uint16_t j;
1391
1392 /* verify client command specifier and subcommand */
1393 if((CANrxData[0]&0xE3U) != 0xA2U){
1394 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1395 return -1;
1396 }
1397
1398 ackseq = CANrxData[1]; /* sequence number of the last segment, that was received correctly. */
1399
1400 /* verify if response is too early */
1401 if(ackseq > SDO->sequence){
1402 CO_SDO_abort(SDO, CO_SDO_AB_SEQ_NUM); /* Invalid sequence */
1403 return -1;
1404 }
1405
1406 /* end of transfer */
1407 if((SDO->endOfTransfer) && (ackseq == SDO->blksize)){
1408 /* first response byte */
1409 SDO->CANtxBuff->data[0] = 0xC1 | ((7 - SDO->lastLen) << 2);
1410
1411 /* CRC */
1412 if(SDO->crcEnabled)
1413 CO_memcpySwap2(&SDO->CANtxBuff->data[1], &SDO->crc);
1414
1415 SDO->state = CO_SDO_ST_UPLOAD_BL_END;
1416
1417 /* send response */
1418 sendResponse = true;
1419 break;
1420 }
1421
1422 /* move remaining data to the beginning */
1423 for(i=ackseq*7, j=0; i<SDO->ODF_arg.dataLength; i++, j++)
1424 SDO->ODF_arg.data[j] = SDO->ODF_arg.data[i];
1425
1426 /* set remaining data length in buffer */
1427 SDO->ODF_arg.dataLength -= ackseq * 7U;
1428
1429 /* new block size */
1430 SDO->blksize = CANrxData[2];
1431
1432 /* If data type is domain, re-fill the data buffer if necessary and indicated so. */
1433 if((SDO->ODF_arg.ODdataStorage == 0) && (SDO->ODF_arg.dataLength < (SDO->blksize*7U)) && (!SDO->ODF_arg.lastSegment)){
1434 /* move the beginning of the data buffer */
1435 len = SDO->ODF_arg.dataLength; /* length of valid data in buffer */
1436 SDO->ODF_arg.data += len;
1437 SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, SDO->ODF_arg.subIndex) - len;
1438
1439 /* read next data from Object dictionary function */
1440 abortCode = CO_SDO_readOD(SDO, CO_SDO_BUFFER_SIZE);
1441 if(abortCode != 0U){
1442 CO_SDO_abort(SDO, abortCode);
1443 return -1;
1444 }
1445
1446 /* calculate CRC on next bytes, if enabled */
1447 if(SDO->crcEnabled){
1448 SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->ODF_arg.dataLength, SDO->crc);
1449 }
1450
1451 /* return to the original data buffer */
1452 SDO->ODF_arg.data -= len;
1453 SDO->ODF_arg.dataLength += len;
1454 }
1455
1456 /* verify if SDO data buffer is large enough */
1457 if(((SDO->blksize*7U) > SDO->ODF_arg.dataLength) && (!SDO->ODF_arg.lastSegment)){
1458 CO_SDO_abort(SDO, CO_SDO_AB_BLOCK_SIZE); /* Invalid block size (block mode only). */
1459 return -1;
1460 }
1461
1462 SDO->bufferOffset = 0U;
1463 SDO->sequence = 0U;
1464 SDO->endOfTransfer = false;
1465 }
1466
1467 /* return, if all segments was already transfered or on end of transfer */
1468 if((SDO->sequence == SDO->blksize) || (SDO->endOfTransfer)){
1469 break;
1470 }
1471
1472 /* reset timeout */
1473 SDO->timeoutTimer = 0;
1474
1475 /* calculate length to be sent */
1476 len = SDO->ODF_arg.dataLength - SDO->bufferOffset;
1477 if(len > 7U){
1478 len = 7U;
1479 }
1480
1481 /* fill response data bytes */
1482 for(i=0U; i<len; i++){
1483 SDO->CANtxBuff->data[i+1] = SDO->ODF_arg.data[SDO->bufferOffset++];
1484 }
1485
1486 /* first response byte */
1487 SDO->CANtxBuff->data[0] = ++SDO->sequence;
1488
1489 /* verify end of transfer */
1490 if((SDO->bufferOffset == SDO->ODF_arg.dataLength) && (SDO->ODF_arg.lastSegment)){
1491 SDO->CANtxBuff->data[0] |= 0x80;
1492 SDO->lastLen = len;
1493 SDO->blksize = SDO->sequence;
1494 SDO->endOfTransfer = true;
1495 }
1496
1497 /* send response */
1498 sendResponse = true;
1499
1500 /* Set timerNext_ms to 0 to inform OS to call this function again without delay. */
1501 if(timerNext_ms != NULL){
1502 *timerNext_ms = 0;
1503 }
1504
1505 break;
1506 }
1507
1508 case CO_SDO_ST_UPLOAD_BL_END:{
1509 /* verify client command specifier */
1510 if((CANrxData[0]&0xE1U) != 0xA1U){
1511 CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
1512 return -1;
1513 }
1514
1515 SDO->state = CO_SDO_ST_IDLE;
1516 break;
1517 }
1518
1519 case CO_SDO_ST_IDLE:
1520 {
1521 /* Nothing to do it seems */
1522 break;
1523 }
1524
1525 default:{
1526 CO_SDO_abort(SDO, CO_SDO_AB_DEVICE_INCOMPAT);/* general internal incompatibility in the device */
1527 return -1;
1528 }
1529 }
1530
1531 /* free receive buffer if it is not empty */
1532 CO_SDO_process_done(SDO, timerNext_ms);
1533
1534 /* send message */
1535 if(sendResponse) {
1536 CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
1537 }
1538
1539 if(SDO->state != CO_SDO_ST_IDLE){
1540 return 1;
1541 }
1542
1543 return 0;
1544 }
1545
