41 |
41 |
42 /*****************************************************************************/ |
42 /*****************************************************************************/ |
43 |
43 |
44 /** Maximum time in ms to wait for responses when reading out the dictionary. |
44 /** Maximum time in ms to wait for responses when reading out the dictionary. |
45 */ |
45 */ |
46 #define EC_FSM_COE_DICT_TIMEOUT 3000 |
46 #define EC_FSM_COE_DICT_TIMEOUT 1000 |
47 |
47 |
48 #define EC_COE_DOWN_REQ_HEADER_SIZE 10 |
48 #define EC_COE_DOWN_REQ_HEADER_SIZE 10 |
49 #define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3 |
49 #define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3 |
50 #define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7 |
50 #define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7 |
|
51 |
|
52 #define DEBUG_RETRIES 0 |
|
53 #define DEBUG_LONG 0 |
51 |
54 |
52 /*****************************************************************************/ |
55 /*****************************************************************************/ |
53 |
56 |
54 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
57 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
55 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
58 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
622 if (!ec_slave_mbox_check(datagram)) { |
625 if (!ec_slave_mbox_check(datagram)) { |
623 unsigned long diff_ms = |
626 unsigned long diff_ms = |
624 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
627 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
625 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
628 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
626 fsm->state = ec_fsm_coe_error; |
629 fsm->state = ec_fsm_coe_error; |
627 EC_ERR("Timeout while waiting for SDO object description " |
630 EC_ERR("Timeout while waiting for SDO 0x%04x object description " |
628 "response on slave %u.\n", slave->ring_position); |
631 "response on slave %u.\n", fsm->sdo->index, |
|
632 slave->ring_position); |
629 return; |
633 return; |
630 } |
634 } |
631 |
635 |
632 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
636 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
633 fsm->retries = EC_FSM_RETRIES; |
637 fsm->retries = EC_FSM_RETRIES; |
858 if (!ec_slave_mbox_check(datagram)) { |
862 if (!ec_slave_mbox_check(datagram)) { |
859 unsigned long diff_ms = |
863 unsigned long diff_ms = |
860 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
864 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
861 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
865 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
862 fsm->state = ec_fsm_coe_error; |
866 fsm->state = ec_fsm_coe_error; |
863 EC_ERR("Timeout while waiting for SDO entry description response " |
867 EC_ERR("Timeout while waiting for SDO entry 0x%04x:%x" |
864 "on slave %u.\n", slave->ring_position); |
868 " description response on slave %u.\n", |
|
869 fsm->sdo->index, fsm->subindex, slave->ring_position); |
865 return; |
870 return; |
866 } |
871 } |
867 |
872 |
868 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
873 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
869 fsm->retries = EC_FSM_RETRIES; |
874 fsm->retries = EC_FSM_RETRIES; |
1194 |
1199 |
1195 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1200 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1196 { |
1201 { |
1197 ec_datagram_t *datagram = fsm->datagram; |
1202 ec_datagram_t *datagram = fsm->datagram; |
1198 ec_slave_t *slave = fsm->slave; |
1203 ec_slave_t *slave = fsm->slave; |
|
1204 unsigned long diff_ms; |
1199 |
1205 |
1200 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1206 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1201 return; // FIXME: check for response first? |
1207 return; // FIXME: check for response first? |
1202 |
1208 |
1203 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1209 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1206 " slave %u (datagram state %u).\n", |
1212 " slave %u (datagram state %u).\n", |
1207 slave->ring_position, datagram->state); |
1213 slave->ring_position, datagram->state); |
1208 return; |
1214 return; |
1209 } |
1215 } |
1210 |
1216 |
|
1217 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
|
1218 |
1211 if (datagram->working_counter != 1) { |
1219 if (datagram->working_counter != 1) { |
1212 if (!datagram->working_counter) { |
1220 if (!datagram->working_counter) { |
1213 unsigned long diff_ms = |
|
1214 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
|
1215 if (diff_ms < fsm->request->response_timeout) { |
1221 if (diff_ms < fsm->request->response_timeout) { |
|
1222 #if DEBUG_RETRIES |
1216 if (fsm->slave->master->debug_level) { |
1223 if (fsm->slave->master->debug_level) { |
1217 EC_DBG("Slave %u did not respond to SDO download request. " |
1224 EC_DBG("Slave %u did not respond to SDO download request. " |
1218 "Retrying after %u ms...\n", |
1225 "Retrying after %u ms...\n", |
1219 slave->ring_position, (u32) diff_ms); |
1226 slave->ring_position, (u32) diff_ms); |
1220 } |
1227 } |
|
1228 #endif |
1221 // no response; send request datagram again |
1229 // no response; send request datagram again |
1222 return; |
1230 return; |
1223 } |
1231 } |
1224 } |
1232 } |
1225 fsm->state = ec_fsm_coe_error; |
1233 fsm->state = ec_fsm_coe_error; |
1226 EC_ERR("Reception of CoE download request failed on slave %u: ", |
1234 EC_ERR("Reception of CoE download request for SDO 0x%04x:%x failed" |
1227 slave->ring_position); |
1235 " with timeout after %u ms on slave %u: ", |
|
1236 fsm->request->index, fsm->request->subindex, (u32) diff_ms, |
|
1237 fsm->slave->ring_position); |
1228 ec_datagram_print_wc_error(datagram); |
1238 ec_datagram_print_wc_error(datagram); |
1229 return; |
1239 return; |
1230 } |
1240 } |
|
1241 |
|
1242 #if DEBUG_LONG |
|
1243 if (diff_ms > 200) { |
|
1244 EC_WARN("SDO 0x%04x:%x download took %u ms on slave %u.\n", |
|
1245 fsm->request->index, fsm->request->subindex, (u32) diff_ms, |
|
1246 fsm->slave->ring_position); |
|
1247 } |
|
1248 #endif |
1231 |
1249 |
1232 fsm->jiffies_start = datagram->jiffies_sent; |
1250 fsm->jiffies_start = datagram->jiffies_sent; |
1233 |
1251 |
1234 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1252 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1235 fsm->retries = EC_FSM_RETRIES; |
1253 fsm->retries = EC_FSM_RETRIES; |
1269 if (!ec_slave_mbox_check(datagram)) { |
1287 if (!ec_slave_mbox_check(datagram)) { |
1270 unsigned long diff_ms = |
1288 unsigned long diff_ms = |
1271 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1289 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1272 if (diff_ms >= fsm->request->response_timeout) { |
1290 if (diff_ms >= fsm->request->response_timeout) { |
1273 fsm->state = ec_fsm_coe_error; |
1291 fsm->state = ec_fsm_coe_error; |
1274 EC_ERR("Timeout while waiting for SDO download response on " |
1292 EC_ERR("Timeout after %u ms while waiting for SDO 0x%04x:%x" |
1275 "slave %u.\n", slave->ring_position); |
1293 " download response on slave %u.\n", (u32) diff_ms, |
|
1294 fsm->request->index, fsm->request->subindex, |
|
1295 slave->ring_position); |
1276 return; |
1296 return; |
1277 } |
1297 } |
1278 |
1298 |
1279 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1299 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1280 fsm->retries = EC_FSM_RETRIES; |
1300 fsm->retries = EC_FSM_RETRIES; |
1687 |
1707 |
1688 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1708 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1689 { |
1709 { |
1690 ec_datagram_t *datagram = fsm->datagram; |
1710 ec_datagram_t *datagram = fsm->datagram; |
1691 ec_slave_t *slave = fsm->slave; |
1711 ec_slave_t *slave = fsm->slave; |
|
1712 unsigned long diff_ms; |
1692 |
1713 |
1693 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1714 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1694 return; // FIXME: check for response first? |
1715 return; // FIXME: check for response first? |
1695 |
1716 |
1696 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1717 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1699 " (datagram state %u).\n", |
1720 " (datagram state %u).\n", |
1700 slave->ring_position, datagram->state); |
1721 slave->ring_position, datagram->state); |
1701 return; |
1722 return; |
1702 } |
1723 } |
1703 |
1724 |
|
1725 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
|
1726 |
1704 if (datagram->working_counter != 1) { |
1727 if (datagram->working_counter != 1) { |
1705 if (!datagram->working_counter) { |
1728 if (!datagram->working_counter) { |
1706 unsigned long diff_ms = |
|
1707 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
|
1708 if (diff_ms < fsm->request->response_timeout) { |
1729 if (diff_ms < fsm->request->response_timeout) { |
|
1730 #if DEBUG_RETRIES |
1709 if (fsm->slave->master->debug_level) { |
1731 if (fsm->slave->master->debug_level) { |
1710 EC_DBG("Slave %u did not respond to SDO upload request. " |
1732 EC_DBG("Slave %u did not respond to SDO upload request. " |
1711 "Retrying after %u ms...\n", |
1733 "Retrying after %u ms...\n", |
1712 slave->ring_position, (u32) diff_ms); |
1734 slave->ring_position, (u32) diff_ms); |
1713 } |
1735 } |
|
1736 #endif |
1714 // no response; send request datagram again |
1737 // no response; send request datagram again |
1715 return; |
1738 return; |
1716 } |
1739 } |
1717 } |
1740 } |
1718 fsm->state = ec_fsm_coe_error; |
1741 fsm->state = ec_fsm_coe_error; |
1719 EC_ERR("Reception of CoE upload request failed on slave %u: ", |
1742 EC_ERR("Reception of CoE upload request for SDO 0x%04x:%x failed" |
1720 slave->ring_position); |
1743 " with timeout after %u ms on slave %u: ", |
|
1744 fsm->request->index, fsm->request->subindex, (u32) diff_ms, |
|
1745 fsm->slave->ring_position); |
1721 ec_datagram_print_wc_error(datagram); |
1746 ec_datagram_print_wc_error(datagram); |
1722 return; |
1747 return; |
1723 } |
1748 } |
|
1749 |
|
1750 #if DEBUG_LONG |
|
1751 if (diff_ms > 200) { |
|
1752 EC_WARN("SDO 0x%04x:%x upload took %u ms on slave %u.\n", |
|
1753 fsm->request->index, fsm->request->subindex, (u32) diff_ms, |
|
1754 fsm->slave->ring_position); |
|
1755 } |
|
1756 #endif |
1724 |
1757 |
1725 fsm->jiffies_start = datagram->jiffies_sent; |
1758 fsm->jiffies_start = datagram->jiffies_sent; |
1726 |
1759 |
1727 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1760 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1728 fsm->retries = EC_FSM_RETRIES; |
1761 fsm->retries = EC_FSM_RETRIES; |
1762 if (!ec_slave_mbox_check(datagram)) { |
1795 if (!ec_slave_mbox_check(datagram)) { |
1763 unsigned long diff_ms = |
1796 unsigned long diff_ms = |
1764 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1797 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1765 if (diff_ms >= fsm->request->response_timeout) { |
1798 if (diff_ms >= fsm->request->response_timeout) { |
1766 fsm->state = ec_fsm_coe_error; |
1799 fsm->state = ec_fsm_coe_error; |
1767 EC_ERR("Timeout while waiting for SDO upload response on " |
1800 EC_ERR("Timeout after %u ms while waiting for SDO 0x%04x:%x" |
1768 "slave %u.\n", slave->ring_position); |
1801 " upload response on slave %u.\n", (u32) diff_ms, |
|
1802 fsm->request->index, fsm->request->subindex, |
|
1803 slave->ring_position); |
1769 return; |
1804 return; |
1770 } |
1805 } |
1771 |
1806 |
1772 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1807 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1773 fsm->retries = EC_FSM_RETRIES; |
1808 fsm->retries = EC_FSM_RETRIES; |