1611 EC_ERR("SII: Reception of read datagram failed.\n"); |
1611 EC_ERR("SII: Reception of read datagram failed.\n"); |
1612 fsm->sii_state = ec_fsm_error; |
1612 fsm->sii_state = ec_fsm_error; |
1613 return; |
1613 return; |
1614 } |
1614 } |
1615 |
1615 |
|
1616 fsm->sii_start = datagram->cycles_sent; |
1616 fsm->sii_check_once_more = 1; |
1617 fsm->sii_check_once_more = 1; |
1617 fsm->sii_start = get_cycles(); |
|
1618 |
1618 |
1619 // issue check/fetch datagram |
1619 // issue check/fetch datagram |
1620 if (fsm->sii_mode) { |
1620 if (fsm->sii_mode) { |
1621 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); |
1621 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); |
1622 } |
1622 } |
1647 } |
1647 } |
1648 |
1648 |
1649 // check "busy bit" |
1649 // check "busy bit" |
1650 if (EC_READ_U8(datagram->data + 1) & 0x81) { |
1650 if (EC_READ_U8(datagram->data + 1) & 0x81) { |
1651 // still busy... timeout? |
1651 // still busy... timeout? |
1652 if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { |
1652 if (datagram->cycles_received |
|
1653 - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { |
1653 if (!fsm->sii_check_once_more) { |
1654 if (!fsm->sii_check_once_more) { |
1654 EC_ERR("SII: Read timeout.\n"); |
1655 EC_ERR("SII: Read timeout.\n"); |
1655 fsm->sii_state = ec_fsm_error; |
1656 fsm->sii_state = ec_fsm_error; |
1656 #if 0 |
1657 #if 0 |
1657 EC_DBG("SII busy: %02X %02X %02X %02X\n", |
1658 EC_DBG("SII busy: %02X %02X %02X %02X\n", |
1725 EC_ERR("SII: Reception of write datagram failed.\n"); |
1726 EC_ERR("SII: Reception of write datagram failed.\n"); |
1726 fsm->sii_state = ec_fsm_error; |
1727 fsm->sii_state = ec_fsm_error; |
1727 return; |
1728 return; |
1728 } |
1729 } |
1729 |
1730 |
1730 fsm->sii_start = get_cycles(); |
1731 fsm->sii_start = datagram->cycles_sent; |
|
1732 fsm->sii_check_once_more = 1; |
1731 |
1733 |
1732 // issue check/fetch datagram |
1734 // issue check/fetch datagram |
1733 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2); |
1735 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2); |
1734 ec_master_queue_datagram(fsm->master, datagram); |
1736 ec_master_queue_datagram(fsm->master, datagram); |
1735 fsm->sii_state = ec_fsm_sii_write_check2; |
1737 fsm->sii_state = ec_fsm_sii_write_check2; |
1752 return; |
1754 return; |
1753 } |
1755 } |
1754 |
1756 |
1755 if (EC_READ_U8(datagram->data + 1) & 0x82) { |
1757 if (EC_READ_U8(datagram->data + 1) & 0x82) { |
1756 // still busy... timeout? |
1758 // still busy... timeout? |
1757 if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { |
1759 if (datagram->cycles_received |
1758 EC_ERR("SII: Write timeout.\n"); |
1760 - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { |
1759 fsm->sii_state = ec_fsm_error; |
1761 if (!fsm->sii_check_once_more) { |
1760 return; |
1762 EC_ERR("SII: Write timeout.\n"); |
|
1763 fsm->sii_state = ec_fsm_error; |
|
1764 return; |
|
1765 } |
|
1766 fsm->sii_check_once_more = 0; |
1761 } |
1767 } |
1762 |
1768 |
1763 // issue check/fetch datagram again |
1769 // issue check/fetch datagram again |
1764 ec_master_queue_datagram(fsm->master, datagram); |
1770 ec_master_queue_datagram(fsm->master, datagram); |
1765 return; |
1771 return; |
1786 void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */) |
1792 void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */) |
1787 { |
1793 { |
1788 ec_datagram_t *datagram = &fsm->datagram; |
1794 ec_datagram_t *datagram = &fsm->datagram; |
1789 ec_slave_t *slave = fsm->slave; |
1795 ec_slave_t *slave = fsm->slave; |
1790 |
1796 |
1791 fsm->change_jiffies = jiffies; |
1797 fsm->change_take_time = 1; |
1792 |
1798 |
1793 // write new state to slave |
1799 // write new state to slave |
1794 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
1800 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
1795 EC_WRITE_U16(datagram->data, fsm->change_new); |
1801 EC_WRITE_U16(datagram->data, fsm->change_new); |
1796 ec_master_queue_datagram(fsm->master, datagram); |
1802 ec_master_queue_datagram(fsm->master, datagram); |
1813 EC_ERR("Failed to send state datagram to slave %i!\n", |
1819 EC_ERR("Failed to send state datagram to slave %i!\n", |
1814 fsm->slave->ring_position); |
1820 fsm->slave->ring_position); |
1815 return; |
1821 return; |
1816 } |
1822 } |
1817 |
1823 |
|
1824 if (fsm->change_take_time) { |
|
1825 fsm->change_take_time = 0; |
|
1826 fsm->change_jiffies = datagram->jiffies_sent; |
|
1827 } |
|
1828 |
1818 if (datagram->working_counter != 1) { |
1829 if (datagram->working_counter != 1) { |
1819 if (jiffies - fsm->change_jiffies >= 3 * HZ) { |
1830 if (datagram->jiffies_received - fsm->change_jiffies >= 3 * HZ) { |
1820 fsm->change_state = ec_fsm_error; |
1831 fsm->change_state = ec_fsm_error; |
1821 EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" |
1832 EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" |
1822 " respond.\n", fsm->change_new, fsm->slave->ring_position); |
1833 " respond.\n", fsm->change_new, fsm->slave->ring_position); |
1823 return; |
1834 return; |
1824 } |
1835 } |
1828 EC_WRITE_U16(datagram->data, fsm->change_new); |
1839 EC_WRITE_U16(datagram->data, fsm->change_new); |
1829 ec_master_queue_datagram(fsm->master, datagram); |
1840 ec_master_queue_datagram(fsm->master, datagram); |
1830 return; |
1841 return; |
1831 } |
1842 } |
1832 |
1843 |
1833 fsm->change_jiffies = jiffies; |
1844 fsm->change_take_time = 1; |
1834 |
1845 |
1835 // read AL status from slave |
1846 // read AL status from slave |
1836 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
1847 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
1837 ec_master_queue_datagram(fsm->master, datagram); |
1848 ec_master_queue_datagram(fsm->master, datagram); |
1838 fsm->change_state = ec_fsm_change_status; |
1849 fsm->change_state = ec_fsm_change_status; |
1853 || datagram->working_counter != 1) { |
1864 || datagram->working_counter != 1) { |
1854 fsm->change_state = ec_fsm_error; |
1865 fsm->change_state = ec_fsm_error; |
1855 EC_ERR("Failed to check state 0x%02X on slave %i.\n", |
1866 EC_ERR("Failed to check state 0x%02X on slave %i.\n", |
1856 fsm->change_new, slave->ring_position); |
1867 fsm->change_new, slave->ring_position); |
1857 return; |
1868 return; |
|
1869 } |
|
1870 |
|
1871 if (fsm->change_take_time) { |
|
1872 fsm->change_take_time = 0; |
|
1873 fsm->change_jiffies = datagram->jiffies_sent; |
1858 } |
1874 } |
1859 |
1875 |
1860 slave->current_state = EC_READ_U8(datagram->data); |
1876 slave->current_state = EC_READ_U8(datagram->data); |
1861 |
1877 |
1862 if (slave->current_state == fsm->change_new) { |
1878 if (slave->current_state == fsm->change_new) { |
1876 ec_master_queue_datagram(fsm->master, datagram); |
1892 ec_master_queue_datagram(fsm->master, datagram); |
1877 fsm->change_state = ec_fsm_change_code; |
1893 fsm->change_state = ec_fsm_change_code; |
1878 return; |
1894 return; |
1879 } |
1895 } |
1880 |
1896 |
1881 if (jiffies - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms |
1897 if (datagram->jiffies_received |
|
1898 - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms |
1882 // timeout while checking |
1899 // timeout while checking |
1883 fsm->change_state = ec_fsm_error; |
1900 fsm->change_state = ec_fsm_error; |
1884 EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", |
1901 EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", |
1885 fsm->change_new, slave->ring_position); |
1902 fsm->change_new, slave->ring_position); |
1886 return; |
1903 return; |
1985 fsm->change_state = ec_fsm_error; |
2002 fsm->change_state = ec_fsm_error; |
1986 EC_ERR("Reception of state ack datagram failed.\n"); |
2003 EC_ERR("Reception of state ack datagram failed.\n"); |
1987 return; |
2004 return; |
1988 } |
2005 } |
1989 |
2006 |
1990 fsm->change_jiffies = jiffies; |
2007 fsm->change_take_time = 1; |
1991 |
2008 |
1992 // read new AL status |
2009 // read new AL status |
1993 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
2010 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
1994 ec_master_queue_datagram(fsm->master, datagram); |
2011 ec_master_queue_datagram(fsm->master, datagram); |
1995 fsm->change_state = ec_fsm_change_check_ack; |
2012 fsm->change_state = ec_fsm_change_check_ack; |
2012 fsm->change_state = ec_fsm_error; |
2029 fsm->change_state = ec_fsm_error; |
2013 EC_ERR("Reception of state ack check datagram failed.\n"); |
2030 EC_ERR("Reception of state ack check datagram failed.\n"); |
2014 return; |
2031 return; |
2015 } |
2032 } |
2016 |
2033 |
|
2034 if (fsm->change_take_time) { |
|
2035 fsm->change_take_time = 0; |
|
2036 fsm->change_jiffies = datagram->jiffies_sent; |
|
2037 } |
|
2038 |
2017 ack_state = EC_READ_U8(datagram->data); |
2039 ack_state = EC_READ_U8(datagram->data); |
2018 |
2040 |
2019 if (ack_state == slave->current_state) { |
2041 if (ack_state == slave->current_state) { |
2020 fsm->change_state = ec_fsm_error; |
2042 fsm->change_state = ec_fsm_error; |
2021 EC_INFO("Acknowleged state 0x%02X on slave %i.\n", |
2043 EC_INFO("Acknowleged state 0x%02X on slave %i.\n", |
2022 slave->current_state, slave->ring_position); |
2044 slave->current_state, slave->ring_position); |
2023 return; |
2045 return; |
2024 } |
2046 } |
2025 |
2047 |
2026 if (jiffies - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms |
2048 if (datagram->jiffies_received |
|
2049 - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms |
2027 // timeout while checking |
2050 // timeout while checking |
2028 slave->current_state = EC_SLAVE_STATE_UNKNOWN; |
2051 slave->current_state = EC_SLAVE_STATE_UNKNOWN; |
2029 fsm->change_state = ec_fsm_error; |
2052 fsm->change_state = ec_fsm_error; |
2030 EC_ERR("Timeout while acknowleging state 0x%02X on slave %i.\n", |
2053 EC_ERR("Timeout while acknowleging state 0x%02X on slave %i.\n", |
2031 fsm->change_new, slave->ring_position); |
2054 fsm->change_new, slave->ring_position); |
2095 fsm->coe_state = ec_fsm_error; |
2118 fsm->coe_state = ec_fsm_error; |
2096 EC_ERR("Reception of CoE download request failed.\n"); |
2119 EC_ERR("Reception of CoE download request failed.\n"); |
2097 return; |
2120 return; |
2098 } |
2121 } |
2099 |
2122 |
2100 fsm->coe_start = get_cycles(); |
2123 fsm->coe_start = datagram->cycles_sent; |
2101 |
2124 |
2102 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2125 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2103 ec_master_queue_datagram(fsm->master, datagram); |
2126 ec_master_queue_datagram(fsm->master, datagram); |
2104 fsm->coe_state = ec_fsm_coe_down_check; |
2127 fsm->coe_state = ec_fsm_coe_down_check; |
2105 } |
2128 } |
2121 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
2144 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
2122 return; |
2145 return; |
2123 } |
2146 } |
2124 |
2147 |
2125 if (!ec_slave_mbox_check(datagram)) { |
2148 if (!ec_slave_mbox_check(datagram)) { |
2126 if (get_cycles() - fsm->coe_start >= (cycles_t) 100 * cpu_khz) { |
2149 if (datagram->cycles_received |
|
2150 - fsm->coe_start >= (cycles_t) 100 * cpu_khz) { |
2127 fsm->coe_state = ec_fsm_error; |
2151 fsm->coe_state = ec_fsm_error; |
2128 EC_ERR("Timeout while checking SDO configuration on slave %i.\n", |
2152 EC_ERR("Timeout while checking SDO configuration on slave %i.\n", |
2129 slave->ring_position); |
2153 slave->ring_position); |
2130 return; |
2154 return; |
2131 } |
2155 } |