40 |
40 |
41 #include "globals.h" |
41 #include "globals.h" |
42 #include "master.h" |
42 #include "master.h" |
43 #include "mailbox.h" |
43 #include "mailbox.h" |
44 #include "fsm_coe.h" |
44 #include "fsm_coe.h" |
|
45 |
|
46 /*****************************************************************************/ |
|
47 |
|
48 /** Maximum time in ms to wait for responses when reading out the dictionary. |
|
49 */ |
|
50 #define EC_FSM_COE_DICT_TIMEOUT 3000 |
45 |
51 |
46 /*****************************************************************************/ |
52 /*****************************************************************************/ |
47 |
53 |
48 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
54 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
49 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
55 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
355 } |
361 } |
356 |
362 |
357 if (!ec_slave_mbox_check(datagram)) { |
363 if (!ec_slave_mbox_check(datagram)) { |
358 unsigned long diff_ms = |
364 unsigned long diff_ms = |
359 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
365 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
360 if (diff_ms >= 100) { |
366 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
361 fsm->state = ec_fsm_coe_error; |
367 fsm->state = ec_fsm_coe_error; |
362 EC_ERR("Timeout while checking Sdo dictionary on slave %u.\n", |
368 EC_ERR("Timeout while checking Sdo dictionary on slave %u.\n", |
363 slave->ring_position); |
369 slave->ring_position); |
364 return; |
370 return; |
365 } |
371 } |
452 return; |
458 return; |
453 } |
459 } |
454 |
460 |
455 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
461 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
456 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
462 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
457 EC_ERR("Invalid Sdo list response at slave %u!\n", |
463 if (fsm->slave->master->debug_level) { |
458 slave->ring_position); |
464 EC_DBG("Invalid Sdo list response at slave %u! Retrying...\n", |
459 ec_print_data(data, rec_size); |
465 slave->ring_position); |
460 fsm->state = ec_fsm_coe_error; |
466 ec_print_data(data, rec_size); |
|
467 } |
|
468 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
469 fsm->retries = EC_FSM_RETRIES; |
|
470 fsm->state = ec_fsm_coe_dict_check; |
461 return; |
471 return; |
462 } |
472 } |
463 |
473 |
464 if (rec_size < 8 || rec_size % 2) { |
474 if (rec_size < 8 || rec_size % 2) { |
465 EC_ERR("Invalid data size %u!\n", rec_size); |
475 EC_ERR("Invalid data size %u!\n", rec_size); |
595 } |
605 } |
596 |
606 |
597 if (!ec_slave_mbox_check(datagram)) { |
607 if (!ec_slave_mbox_check(datagram)) { |
598 unsigned long diff_ms = |
608 unsigned long diff_ms = |
599 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
609 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
600 if (diff_ms >= 100) { |
610 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
601 fsm->state = ec_fsm_coe_error; |
611 fsm->state = ec_fsm_coe_error; |
602 EC_ERR("Timeout while checking Sdo description on slave %u.\n", |
612 EC_ERR("Timeout while checking Sdo description on slave %u.\n", |
603 slave->ring_position); |
613 slave->ring_position); |
604 return; |
614 return; |
605 } |
615 } |
695 } |
705 } |
696 |
706 |
697 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
707 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
698 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
708 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
699 EC_READ_U16(data + 6) != sdo->index) { // Sdo index |
709 EC_READ_U16(data + 6) != sdo->index) { // Sdo index |
700 EC_ERR("Invalid object description response at slave %u while" |
710 if (fsm->slave->master->debug_level) { |
701 " fetching Sdo 0x%04X!\n", slave->ring_position, |
711 EC_DBG("Invalid object description response at slave %u while" |
702 sdo->index); |
712 " fetching Sdo 0x%04X!\n", slave->ring_position, |
703 ec_print_data(data, rec_size); |
713 sdo->index); |
704 fsm->state = ec_fsm_coe_error; |
714 ec_print_data(data, rec_size); |
|
715 } |
|
716 // check for CoE response again |
|
717 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
718 fsm->retries = EC_FSM_RETRIES; |
|
719 fsm->state = ec_fsm_coe_dict_desc_check; |
705 return; |
720 return; |
706 } |
721 } |
707 |
722 |
708 if (rec_size < 12) { |
723 if (rec_size < 12) { |
709 EC_ERR("Invalid data size!\n"); |
724 EC_ERR("Invalid data size!\n"); |
825 } |
840 } |
826 |
841 |
827 if (!ec_slave_mbox_check(datagram)) { |
842 if (!ec_slave_mbox_check(datagram)) { |
828 unsigned long diff_ms = |
843 unsigned long diff_ms = |
829 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
844 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
830 if (diff_ms >= 100) { |
845 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
831 fsm->state = ec_fsm_coe_error; |
846 fsm->state = ec_fsm_coe_error; |
832 EC_ERR("Timeout while checking Sdo entry on slave %u.\n", |
847 EC_ERR("Timeout while checking Sdo entry on slave %u.\n", |
833 slave->ring_position); |
848 slave->ring_position); |
834 return; |
849 return; |
835 } |
850 } |
927 |
942 |
928 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
943 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
929 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
944 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
930 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
945 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
931 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
946 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
932 EC_ERR("Invalid entry description response at slave %u while" |
947 if (fsm->slave->master->debug_level) { |
933 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
948 EC_DBG("Invalid entry description response at slave %u while" |
934 sdo->index, fsm->subindex); |
949 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
935 ec_print_data(data, rec_size); |
950 sdo->index, fsm->subindex); |
936 fsm->state = ec_fsm_coe_error; |
951 ec_print_data(data, rec_size); |
|
952 } |
|
953 // check for CoE response again |
|
954 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
955 fsm->retries = EC_FSM_RETRIES; |
|
956 fsm->state = ec_fsm_coe_dict_entry_check; |
937 return; |
957 return; |
938 } |
958 } |
939 |
959 |
940 if (rec_size < 16) { |
960 if (rec_size < 16) { |
941 EC_ERR("Invalid data size %u!\n", rec_size); |
961 EC_ERR("Invalid data size %u!\n", rec_size); |
1099 if (!datagram->working_counter) { |
1119 if (!datagram->working_counter) { |
1100 unsigned long diff_ms = |
1120 unsigned long diff_ms = |
1101 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1121 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1102 if (diff_ms < fsm->request->response_timeout) { |
1122 if (diff_ms < fsm->request->response_timeout) { |
1103 if (fsm->slave->master->debug_level) { |
1123 if (fsm->slave->master->debug_level) { |
1104 EC_DBG("Slave %u did no respond to Sdo download request. " |
1124 EC_DBG("Slave %u did not respond to Sdo download request. " |
1105 "Retrying after %u ms...\n", |
1125 "Retrying after %u ms...\n", |
1106 slave->ring_position, (u32) diff_ms); |
1126 slave->ring_position, (u32) diff_ms); |
1107 // no response; send request datagram again |
1127 // no response; send request datagram again |
1108 return; |
1128 return; |
1109 } |
1129 } |
1154 } |
1174 } |
1155 |
1175 |
1156 if (!ec_slave_mbox_check(datagram)) { |
1176 if (!ec_slave_mbox_check(datagram)) { |
1157 unsigned long diff_ms = |
1177 unsigned long diff_ms = |
1158 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1178 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1159 if (diff_ms >= 100) { |
1179 if (diff_ms >= fsm->request->response_timeout) { |
1160 fsm->state = ec_fsm_coe_error; |
1180 fsm->state = ec_fsm_coe_error; |
1161 EC_ERR("Timeout while checking Sdo configuration on slave %u.\n", |
1181 EC_ERR("Timeout while waiting for Sdo download response on " |
1162 slave->ring_position); |
1182 "slave %u.\n", slave->ring_position); |
1163 return; |
1183 return; |
1164 } |
1184 } |
1165 |
1185 |
1166 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1186 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1167 fsm->retries = EC_FSM_RETRIES; |
1187 fsm->retries = EC_FSM_RETRIES; |
1258 |
1278 |
1259 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1279 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1260 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1280 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1261 EC_READ_U16(data + 3) != request->index || // index |
1281 EC_READ_U16(data + 3) != request->index || // index |
1262 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1282 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1263 fsm->state = ec_fsm_coe_error; |
1283 if (slave->master->debug_level) { |
1264 EC_ERR("Sdo download 0x%04X:%02X (%u bytes) failed:\n", |
1284 EC_DBG("Invalid Sdo download response at slave %u! Retrying...\n", |
1265 request->index, request->subindex, request->data_size); |
1285 slave->ring_position); |
1266 EC_ERR("Invalid Sdo download response at slave %u!\n", |
1286 ec_print_data(data, rec_size); |
1267 slave->ring_position); |
1287 } |
1268 ec_print_data(data, rec_size); |
1288 // check for CoE response again |
|
1289 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1290 fsm->retries = EC_FSM_RETRIES; |
|
1291 fsm->state = ec_fsm_coe_down_check; |
1269 return; |
1292 return; |
1270 } |
1293 } |
1271 |
1294 |
1272 fsm->state = ec_fsm_coe_end; // success |
1295 fsm->state = ec_fsm_coe_end; // success |
1273 } |
1296 } |
1516 |
1539 |
1517 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1540 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1518 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1541 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1519 EC_READ_U16(data + 3) != request->index || // index |
1542 EC_READ_U16(data + 3) != request->index || // index |
1520 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1543 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1521 fsm->state = ec_fsm_coe_error; |
1544 if (fsm->slave->master->debug_level) { |
1522 EC_ERR("Sdo upload 0x%04X:%02X failed:\n", |
1545 EC_DBG("Invalid Sdo upload expedited response at slave %u!\n", |
1523 request->index, request->subindex); |
1546 slave->ring_position); |
1524 EC_ERR("Invalid Sdo upload expedited response at slave %u!\n", |
1547 ec_print_data(data, rec_size); |
1525 slave->ring_position); |
1548 } |
1526 ec_print_data(data, rec_size); |
1549 // check for CoE response again |
|
1550 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1551 fsm->retries = EC_FSM_RETRIES; |
|
1552 fsm->state = ec_fsm_coe_up_check; |
1527 return; |
1553 return; |
1528 } |
1554 } |
1529 |
1555 |
1530 size_specified = EC_READ_U8(data + 2) & 0x01; |
1556 size_specified = EC_READ_U8(data + 2) & 0x01; |
1531 if (size_specified) { |
1557 if (size_specified) { |
1557 |
1583 |
1558 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1584 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1559 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1585 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1560 EC_READ_U16(data + 3) != request->index || // index |
1586 EC_READ_U16(data + 3) != request->index || // index |
1561 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1587 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1562 fsm->state = ec_fsm_coe_error; |
1588 if (fsm->slave->master->debug_level) { |
1563 EC_ERR("Sdo upload 0x%04X:%02X failed:\n", |
1589 EC_DBG("Invalid Sdo normal upload response at slave %u!\n", |
1564 request->index, request->subindex); |
1590 slave->ring_position); |
1565 EC_ERR("Invalid Sdo normal upload response at slave %u!\n", |
1591 ec_print_data(data, rec_size); |
1566 slave->ring_position); |
1592 } |
1567 ec_print_data(data, rec_size); |
1593 // check for CoE response again |
|
1594 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1595 fsm->retries = EC_FSM_RETRIES; |
|
1596 fsm->state = ec_fsm_coe_up_check; |
1568 return; |
1597 return; |
1569 } |
1598 } |
1570 |
1599 |
1571 data_size = rec_size - 10; |
1600 data_size = rec_size - 10; |
1572 complete_size = EC_READ_U32(data + 6); |
1601 complete_size = EC_READ_U32(data + 6); |
1692 } |
1721 } |
1693 |
1722 |
1694 if (!ec_slave_mbox_check(datagram)) { |
1723 if (!ec_slave_mbox_check(datagram)) { |
1695 unsigned long diff_ms = |
1724 unsigned long diff_ms = |
1696 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1725 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1697 if (diff_ms >= 100) { |
1726 if (diff_ms >= fsm->request->response_timeout) { |
1698 fsm->state = ec_fsm_coe_error; |
1727 fsm->state = ec_fsm_coe_error; |
1699 EC_ERR("Timeout while checking Sdo upload segment on slave %u.\n", |
1728 EC_ERR("Timeout while waiting for Sdo upload segment response " |
1700 slave->ring_position); |
1729 "on slave %u.\n", slave->ring_position); |
1701 return; |
1730 return; |
1702 } |
1731 } |
1703 |
1732 |
1704 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1733 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1705 fsm->retries = EC_FSM_RETRIES; |
1734 fsm->retries = EC_FSM_RETRIES; |
1792 return; |
1821 return; |
1793 } |
1822 } |
1794 |
1823 |
1795 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1824 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1796 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1825 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1797 EC_ERR("Sdo upload 0x%04X:%02X failed:\n", request->index, request->subindex); |
1826 if (fsm->slave->master->debug_level) { |
1798 EC_ERR("Invalid Sdo upload segment response at slave %u!\n", |
1827 EC_DBG("Invalid Sdo upload segment response at slave %u!\n", |
1799 slave->ring_position); |
1828 slave->ring_position); |
1800 ec_print_data(data, rec_size); |
1829 ec_print_data(data, rec_size); |
1801 fsm->state = ec_fsm_coe_error; |
1830 } |
|
1831 // check for CoE response again |
|
1832 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1833 fsm->retries = EC_FSM_RETRIES; |
|
1834 fsm->state = ec_fsm_coe_up_seg_check; |
1802 return; |
1835 return; |
1803 } |
1836 } |
1804 |
1837 |
1805 last_segment = EC_READ_U8(data + 2) & 0x01; |
1838 last_segment = EC_READ_U8(data + 2) & 0x01; |
1806 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1839 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |