428 fsm->retries = EC_FSM_RETRIES; |
428 fsm->retries = EC_FSM_RETRIES; |
429 fsm->state = ec_fsm_coe_dict_check; |
429 fsm->state = ec_fsm_coe_dict_check; |
430 return; |
430 return; |
431 } |
431 } |
432 |
432 |
|
433 if (rec_size < 3) { |
|
434 EC_ERR("Received corrupted Sdo dictionary response (size %u).\n", |
|
435 rec_size); |
|
436 fsm->state = ec_fsm_coe_error; |
|
437 return; |
|
438 } |
|
439 |
433 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
440 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
434 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
441 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
435 EC_ERR("Sdo information error response at slave %u!\n", |
442 EC_ERR("Sdo information error response at slave %u!\n", |
436 slave->ring_position); |
443 slave->ring_position); |
437 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
444 if (rec_size < 10) { |
|
445 EC_ERR("Incomplete Sdo information error response:\n"); |
|
446 ec_print_data(data, rec_size); |
|
447 } else { |
|
448 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
449 } |
438 fsm->state = ec_fsm_coe_error; |
450 fsm->state = ec_fsm_coe_error; |
439 return; |
451 return; |
440 } |
452 } |
441 |
453 |
442 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
454 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
446 ec_print_data(data, rec_size); |
458 ec_print_data(data, rec_size); |
447 fsm->state = ec_fsm_coe_error; |
459 fsm->state = ec_fsm_coe_error; |
448 return; |
460 return; |
449 } |
461 } |
450 |
462 |
451 if (rec_size < 8) { |
463 if (rec_size < 8 || rec_size % 2) { |
452 EC_ERR("Invalid data size!\n"); |
464 EC_ERR("Invalid data size %u!\n", rec_size); |
453 ec_print_data(data, rec_size); |
465 ec_print_data(data, rec_size); |
454 fsm->state = ec_fsm_coe_error; |
466 fsm->state = ec_fsm_coe_error; |
455 return; |
467 return; |
456 } |
468 } |
457 |
469 |
654 fsm->retries = EC_FSM_RETRIES; |
666 fsm->retries = EC_FSM_RETRIES; |
655 fsm->state = ec_fsm_coe_dict_desc_check; |
667 fsm->state = ec_fsm_coe_dict_desc_check; |
656 return; |
668 return; |
657 } |
669 } |
658 |
670 |
|
671 if (rec_size < 3) { |
|
672 EC_ERR("Received corrupted Sdo description response (size %u).\n", |
|
673 rec_size); |
|
674 fsm->state = ec_fsm_coe_error; |
|
675 return; |
|
676 } |
|
677 |
659 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
678 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
660 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
679 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
661 EC_ERR("Sdo information error response at slave %u while" |
680 EC_ERR("Sdo information error response at slave %u while" |
662 " fetching Sdo 0x%04X!\n", slave->ring_position, |
681 " fetching Sdo 0x%04X!\n", slave->ring_position, |
663 sdo->index); |
682 sdo->index); |
664 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
683 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
684 fsm->state = ec_fsm_coe_error; |
|
685 return; |
|
686 } |
|
687 |
|
688 if (rec_size < 8) { |
|
689 EC_ERR("Received corrupted Sdo description response (size %u).\n", |
|
690 rec_size); |
665 fsm->state = ec_fsm_coe_error; |
691 fsm->state = ec_fsm_coe_error; |
666 return; |
692 return; |
667 } |
693 } |
668 |
694 |
669 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
695 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
870 fsm->retries = EC_FSM_RETRIES; |
896 fsm->retries = EC_FSM_RETRIES; |
871 fsm->state = ec_fsm_coe_dict_entry_check; |
897 fsm->state = ec_fsm_coe_dict_entry_check; |
872 return; |
898 return; |
873 } |
899 } |
874 |
900 |
|
901 if (rec_size < 3) { |
|
902 EC_ERR("Received corrupted Sdo entry description response " |
|
903 "(size %u).\n", rec_size); |
|
904 fsm->state = ec_fsm_coe_error; |
|
905 return; |
|
906 } |
|
907 |
875 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
908 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
876 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
909 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
877 EC_ERR("Sdo information error response at slave %u while" |
910 EC_ERR("Sdo information error response at slave %u while" |
878 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
911 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
879 sdo->index, fsm->subindex); |
912 sdo->index, fsm->subindex); |
880 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
913 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
914 fsm->state = ec_fsm_coe_error; |
|
915 return; |
|
916 } |
|
917 |
|
918 if (rec_size < 9) { |
|
919 EC_ERR("Received corrupted Sdo entry description response " |
|
920 "(size %u).\n", rec_size); |
881 fsm->state = ec_fsm_coe_error; |
921 fsm->state = ec_fsm_coe_error; |
882 return; |
922 return; |
883 } |
923 } |
884 |
924 |
885 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
925 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
1189 ec_print_data(data, rec_size); |
1229 ec_print_data(data, rec_size); |
1190 } |
1230 } |
1191 |
1231 |
1192 if (rec_size < 6) { |
1232 if (rec_size < 6) { |
1193 fsm->state = ec_fsm_coe_error; |
1233 fsm->state = ec_fsm_coe_error; |
1194 EC_ERR("Received data is too small (%u bytes):\n", rec_size); |
1234 EC_ERR("Received data are too small (%u bytes):\n", rec_size); |
1195 ec_print_data(data, rec_size); |
1235 ec_print_data(data, rec_size); |
1196 return; |
1236 return; |
1197 } |
1237 } |
1198 |
1238 |
1199 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1239 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1203 request->index, request->subindex, request->data_size, |
1243 request->index, request->subindex, request->data_size, |
1204 slave->ring_position); |
1244 slave->ring_position); |
1205 if (rec_size < 10) { |
1245 if (rec_size < 10) { |
1206 EC_ERR("Incomplete Abort command:\n"); |
1246 EC_ERR("Incomplete Abort command:\n"); |
1207 ec_print_data(data, rec_size); |
1247 ec_print_data(data, rec_size); |
1208 } |
1248 } else { |
1209 else |
1249 fsm->request->abort_code = EC_READ_U32(data + 6); |
1210 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1250 ec_canopen_abort_msg(fsm->request->abort_code); |
|
1251 } |
1211 return; |
1252 return; |
1212 } |
1253 } |
1213 |
1254 |
1214 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1255 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1215 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1256 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1444 |
1485 |
1445 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1486 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1446 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1487 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1447 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1488 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1448 request->index, request->subindex, slave->ring_position); |
1489 request->index, request->subindex, slave->ring_position); |
1449 if (rec_size >= 10) |
1490 if (rec_size >= 10) { |
1450 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1491 request->abort_code = EC_READ_U32(data + 6); |
1451 else |
1492 ec_canopen_abort_msg(request->abort_code); |
|
1493 } else { |
1452 EC_ERR("No abort message.\n"); |
1494 EC_ERR("No abort message.\n"); |
|
1495 } |
1453 fsm->state = ec_fsm_coe_error; |
1496 fsm->state = ec_fsm_coe_error; |
1454 return; |
1497 return; |
1455 } |
1498 } |
1456 |
1499 |
1457 // normal or expedited? |
1500 // normal or expedited? |
1735 |
1778 |
1736 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1779 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1737 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1780 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1738 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1781 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1739 request->index, request->subindex, slave->ring_position); |
1782 request->index, request->subindex, slave->ring_position); |
1740 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1783 request->abort_code = EC_READ_U32(data + 6); |
|
1784 ec_canopen_abort_msg(request->abort_code); |
1741 fsm->state = ec_fsm_coe_error; |
1785 fsm->state = ec_fsm_coe_error; |
1742 return; |
1786 return; |
1743 } |
1787 } |
1744 |
1788 |
1745 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1789 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |