Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm.c
changeset 436 63214beb641d
parent 435 779a18d12e6c
child 437 ef80f2faa2c5
equal deleted inserted replaced
435:779a18d12e6c 436:63214beb641d 
    78 void ec_fsm_slaveconf_fmmu(ec_fsm_t *);
 
    78 void ec_fsm_slaveconf_fmmu(ec_fsm_t *);
 
    79 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *);
 
    79 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *);
 
    80 void ec_fsm_slaveconf_saveop(ec_fsm_t *);
 
    80 void ec_fsm_slaveconf_saveop(ec_fsm_t *);
 
    81 void ec_fsm_slaveconf_op(ec_fsm_t *);
 
    81 void ec_fsm_slaveconf_op(ec_fsm_t *);
 
    82 
    82 
    83 void ec_fsm_coe_dict_start(ec_fsm_t *);
 
       
    84 void ec_fsm_coe_dict_request(ec_fsm_t *);
 
       
    85 void ec_fsm_coe_dict_check(ec_fsm_t *);
 
       
    86 void ec_fsm_coe_dict_response(ec_fsm_t *);
 
       
    87 void ec_fsm_coe_dict_desc_request(ec_fsm_t *);
 
       
    88 void ec_fsm_coe_dict_desc_check(ec_fsm_t *);
 
       
    89 void ec_fsm_coe_dict_desc_response(ec_fsm_t *);
 
       
    90 void ec_fsm_coe_dict_entry_request(ec_fsm_t *);
 
       
    91 void ec_fsm_coe_dict_entry_check(ec_fsm_t *);
 
       
    92 void ec_fsm_coe_dict_entry_response(ec_fsm_t *);
 
       
    93 
       
    94 void ec_fsm_coe_down_start(ec_fsm_t *);
 
       
    95 void ec_fsm_coe_down_request(ec_fsm_t *);
 
       
    96 void ec_fsm_coe_down_check(ec_fsm_t *);
 
       
    97 void ec_fsm_coe_down_response(ec_fsm_t *);
 
       
    98 
       
    99 void ec_fsm_coe_up_start(ec_fsm_t *);
 
       
   100 void ec_fsm_coe_up_request(ec_fsm_t *);
 
       
   101 void ec_fsm_coe_up_check(ec_fsm_t *);
 
       
   102 void ec_fsm_coe_up_response(ec_fsm_t *);
 
       
   103 void ec_fsm_coe_up_seg_request(ec_fsm_t *);
 
       
   104 void ec_fsm_coe_up_seg_check(ec_fsm_t *);
 
       
   105 void ec_fsm_coe_up_seg_response(ec_fsm_t *);
 
       
   106 
       
   107 void ec_fsm_end(ec_fsm_t *);
 
    83 void ec_fsm_end(ec_fsm_t *);
 
   108 void ec_fsm_error(ec_fsm_t *);
 
    84 void ec_fsm_error(ec_fsm_t *);
 
   109 
       
   110 void ec_canopen_abort_msg(uint32_t);
 
       
   111 
    85 
   112 /*****************************************************************************/
 
    86 /*****************************************************************************/
 
   113 
    87 
   114 /**
 
    88 /**
 
   115    Constructor.
 
    89    Constructor.
 
   129     if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) {
 
   103     if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) {
 
   130         EC_ERR("Failed to allocate FSM datagram.\n");
 
   104         EC_ERR("Failed to allocate FSM datagram.\n");
 
   131         return -1;
 
   105         return -1;
 
   132     }
 
   106     }
 
   133 
   107 
       
   108     // init sub-state-machines
 
   134     ec_fsm_sii_init(&fsm->fsm_sii, &fsm->datagram);
 
   109     ec_fsm_sii_init(&fsm->fsm_sii, &fsm->datagram);
 
   135     ec_fsm_change_init(&fsm->fsm_change, &fsm->datagram);
 
   110     ec_fsm_change_init(&fsm->fsm_change, &fsm->datagram);
 
       
   111     ec_fsm_coe_init(&fsm->fsm_coe, &fsm->datagram);
 
   136 
   112 
   137     return 0;
 
   113     return 0;
 
   138 }
 
   114 }
 
   139 
   115 
   140 /*****************************************************************************/
 
   116 /*****************************************************************************/
 
   143    Destructor.
 
   119    Destructor.
 
   144 */
 
   120 */
 
   145 
   121 
   146 void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)
 
   122 void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)
 
   147 {
 
   123 {
 
       
   124     // clear sub-state machines
 
   148     ec_fsm_sii_clear(&fsm->fsm_sii);
 
   125     ec_fsm_sii_clear(&fsm->fsm_sii);
 
   149     ec_fsm_change_clear(&fsm->fsm_change);
 
   126     ec_fsm_change_clear(&fsm->fsm_change);
 
       
   127     ec_fsm_coe_clear(&fsm->fsm_coe);
 
   150 
   128 
   151     ec_datagram_clear(&fsm->datagram);
 
   129     ec_datagram_clear(&fsm->datagram);
 
   152 }
 
   130 }
 
   153 
   131 
   154 /*****************************************************************************/
 
   132 /*****************************************************************************/
 
   598         }
 
   576         }
 
   599 
   577 
   600         // start uploading SDO
 
   578         // start uploading SDO
 
   601         fsm->slave = slave;
 
   579         fsm->slave = slave;
 
   602         fsm->master_state = ec_fsm_master_sdo_request;
 
   580         fsm->master_state = ec_fsm_master_sdo_request;
 
   603         fsm->coe_request = request;
 
   581         fsm->sdo_request = request;
 
   604         fsm->coe_state = ec_fsm_coe_up_start;
 
   582         ec_fsm_coe_upload(&fsm->fsm_coe, slave, request);
 
   605         fsm->coe_state(fsm); // execute immediately
 
   583         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
   606         return;
 
   584         return;
 
   607     }
 
   585     }
 
   608 
   586 
   609     // check, if slaves have an SDO dictionary to read out.
 
   587     // check, if slaves have an SDO dictionary to read out.
 
   610     list_for_each_entry(slave, &master->slaves, list) {
 
   588     list_for_each_entry(slave, &master->slaves, list) {
 
   632         slave->sdo_dictionary_fetched = 1;
 
   610         slave->sdo_dictionary_fetched = 1;
 
   633 
   611 
   634         // start fetching SDO dictionary
 
   612         // start fetching SDO dictionary
 
   635         fsm->slave = slave;
 
   613         fsm->slave = slave;
 
   636         fsm->master_state = ec_fsm_master_sdodict;
 
   614         fsm->master_state = ec_fsm_master_sdodict;
 
   637         fsm->coe_state = ec_fsm_coe_dict_start;
 
   615         ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);
 
   638         fsm->coe_state(fsm); // execute immediately
 
   616         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
   639         return;
 
   617         return;
 
   640     }
 
   618     }
 
   641 
   619 
   642     if (master->mode == EC_MASTER_MODE_IDLE) {
 
   620     if (master->mode == EC_MASTER_MODE_IDLE) {
 
   643         // nothing to configure. check for pending EEPROM write operations.
 
   621         // nothing to configure. check for pending EEPROM write operations.
 
  1018 void ec_fsm_master_sdodict(ec_fsm_t *fsm /**< finite state machine */)
 
   996 void ec_fsm_master_sdodict(ec_fsm_t *fsm /**< finite state machine */)
 
  1019 {
 
   997 {
 
  1020     ec_slave_t *slave = fsm->slave;
 
   998     ec_slave_t *slave = fsm->slave;
 
  1021     ec_master_t *master = fsm->master;
 
   999     ec_master_t *master = fsm->master;
 
  1022 
  1000 
  1023     fsm->coe_state(fsm); // execute CoE state machine
 
  1001     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
  1024 
  1002 
  1025     if (fsm->coe_state == ec_fsm_error) {
 
  1003     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
  1026         fsm->master_state = ec_fsm_master_start;
 
  1004         fsm->master_state = ec_fsm_master_start;
 
  1027         fsm->master_state(fsm); // execute immediately
 
  1005         fsm->master_state(fsm); // execute immediately
 
  1028         return;
 
  1006         return;
 
  1029     }
 
  1007     }
 
  1030 
       
  1031     if (fsm->coe_state != ec_fsm_end) return;
 
       
  1032 
  1008 
  1033     // SDO dictionary fetching finished
 
  1009     // SDO dictionary fetching finished
 
  1034 
  1010 
  1035     if (master->debug_level) {
 
  1011     if (master->debug_level) {
 
  1036         unsigned int sdo_count, entry_count;
 
  1012         unsigned int sdo_count, entry_count;
 
  1051 */
 
  1027 */
 
  1052 
  1028 
  1053 void ec_fsm_master_sdo_request(ec_fsm_t *fsm /**< finite state machine */)
 
  1029 void ec_fsm_master_sdo_request(ec_fsm_t *fsm /**< finite state machine */)
 
  1054 {
 
  1030 {
 
  1055     ec_master_t *master = fsm->master;
 
  1031     ec_master_t *master = fsm->master;
 
  1056     ec_sdo_request_t *request = fsm->coe_request;
 
  1032     ec_sdo_request_t *request = fsm->sdo_request;
 
  1057 
  1033 
  1058     fsm->coe_state(fsm); // execute CoE state machine
 
  1034     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
  1059 
  1035 
  1060     if (fsm->coe_state == ec_fsm_error) {
 
  1036     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
  1061         request->return_code = -1;
 
  1037         request->return_code = -1;
 
  1062         wake_up_interruptible(&master->sdo_wait_queue);
 
  1038         wake_up_interruptible(&master->sdo_wait_queue);
 
  1063         fsm->master_state = ec_fsm_master_start;
 
  1039         fsm->master_state = ec_fsm_master_start;
 
  1064         fsm->master_state(fsm); // execute immediately
 
  1040         fsm->master_state(fsm); // execute immediately
 
  1065         return;
 
  1041         return;
 
  1066     }
 
  1042     }
 
  1067 
       
  1068     if (fsm->coe_state != ec_fsm_end) return;
 
       
  1069 
  1043 
  1070     // SDO dictionary fetching finished
 
  1044     // SDO dictionary fetching finished
 
  1071 
  1045 
  1072     request->return_code = 1;
 
  1046     request->return_code = 1;
 
  1073     wake_up_interruptible(&master->sdo_wait_queue);
 
  1047     wake_up_interruptible(&master->sdo_wait_queue);
 
  1567             return;
 
  1541             return;
 
  1568         }
 
  1542         }
 
  1569 
  1543 
  1570         // start SDO configuration
 
  1544         // start SDO configuration
 
  1571         fsm->slave_state = ec_fsm_slaveconf_sdoconf;
 
  1545         fsm->slave_state = ec_fsm_slaveconf_sdoconf;
 
  1572         fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list);
 
  1546         fsm->sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list);
 
  1573         fsm->coe_state = ec_fsm_coe_down_start;
 
  1547         ec_fsm_coe_download(&fsm->fsm_coe, slave, fsm->sdodata);
 
  1574         fsm->coe_state(fsm); // execute immediately
 
  1548         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
  1575         return;
 
  1549         return;
 
  1576     }
 
  1550     }
 
  1577 
  1551 
  1578     // configure FMMUs
 
  1552     // configure FMMUs
 
  1579     ec_datagram_npwr(datagram, slave->station_address,
 
  1553     ec_datagram_npwr(datagram, slave->station_address,
 
  1617         return;
 
  1591         return;
 
  1618     }
 
  1592     }
 
  1619 
  1593 
  1620     // start SDO configuration
 
  1594     // start SDO configuration
 
  1621     fsm->slave_state = ec_fsm_slaveconf_sdoconf;
 
  1595     fsm->slave_state = ec_fsm_slaveconf_sdoconf;
 
  1622     fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list);
 
  1596     fsm->sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list);
 
  1623     fsm->coe_state = ec_fsm_coe_down_start;
 
  1597     ec_fsm_coe_download(&fsm->fsm_coe, slave, fsm->sdodata);
 
  1624     fsm->coe_state(fsm); // execute immediately
 
  1598     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
  1625 }
 
  1599 }
 
  1626 
  1600 
  1627 /*****************************************************************************/
 
  1601 /*****************************************************************************/
 
  1628 
  1602 
  1629 /**
 
  1603 /**
 
  1630    Slave configuration state: SDOCONF.
 
  1604    Slave configuration state: SDOCONF.
 
  1631 */
 
  1605 */
 
  1632 
  1606 
  1633 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */)
 
  1607 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */)
 
  1634 {
 
  1608 {
 
  1635     fsm->coe_state(fsm); // execute CoE state machine
 
  1609     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
  1636 
  1610 
  1637     if (fsm->coe_state == ec_fsm_error) {
 
  1611     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
  1638         fsm->slave->error_flag = 1;
 
  1612         fsm->slave->error_flag = 1;
 
  1639         fsm->slave_state = ec_fsm_error;
 
  1613         fsm->slave_state = ec_fsm_error;
 
  1640         return;
 
  1614         return;
 
  1641     }
 
  1615     }
 
  1642 
  1616 
  1643     if (fsm->coe_state != ec_fsm_end) return;
 
       
  1644 
       
  1645     // Another SDO to configure?
 
  1617     // Another SDO to configure?
 
  1646     if (fsm->coe_sdodata->list.next != &fsm->slave->sdo_confs) {
 
  1618     if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) {
 
  1647         fsm->coe_sdodata = list_entry(fsm->coe_sdodata->list.next,
 
  1619         fsm->sdodata = list_entry(fsm->sdodata->list.next,
 
  1648                                       ec_sdo_data_t, list);
 
  1620                                   ec_sdo_data_t, list);
 
  1649         fsm->coe_state = ec_fsm_coe_down_start;
 
  1621         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
 
  1650         fsm->coe_state(fsm); // execute immediately
 
  1622         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
  1651         return;
 
  1623         return;
 
  1652     }
 
  1624     }
 
  1653 
  1625 
  1654     // All SDOs are now configured.
 
  1626     // All SDOs are now configured.
 
  1655 
  1627 
  1727 
  1699 
  1728     fsm->slave_state = ec_fsm_end; // successful
 
  1700     fsm->slave_state = ec_fsm_end; // successful
 
  1729 }
 
  1701 }
 
  1730 
  1702 
  1731 /******************************************************************************
 
  1703 /******************************************************************************
 
  1732  *  CoE dictionary state machine
 
       
  1733  *****************************************************************************/
 
       
  1734 
       
  1735 /**
 
       
  1736    CoE state: DICT START.
 
       
  1737 */
 
       
  1738 
       
  1739 void ec_fsm_coe_dict_start(ec_fsm_t *fsm /**< finite state machine */)
 
       
  1740 {
 
       
  1741     ec_datagram_t *datagram = &fsm->datagram;
 
       
  1742     ec_slave_t *slave = fsm->slave;
 
       
  1743     uint8_t *data;
 
       
  1744 
       
  1745     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {
 
       
  1746         fsm->coe_state = ec_fsm_error;
 
       
  1747         return;
 
       
  1748     }
 
       
  1749 
       
  1750     EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
       
  1751     EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
 
       
  1752     EC_WRITE_U8 (data + 3, 0x00);
 
       
  1753     EC_WRITE_U16(data + 4, 0x0000);
 
       
  1754     EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
 
       
  1755 
       
  1756     ec_master_queue_datagram(fsm->master, datagram);
 
       
  1757     fsm->coe_state = ec_fsm_coe_dict_request;
 
       
  1758 }
 
       
  1759 
       
  1760 /*****************************************************************************/
 
       
  1761 
       
  1762 /**
 
       
  1763    CoE state: DICT REQUEST.
 
       
  1764 */
 
       
  1765 
       
  1766 void ec_fsm_coe_dict_request(ec_fsm_t *fsm /**< finite state machine */)
 
       
  1767 {
 
       
  1768     ec_datagram_t *datagram = &fsm->datagram;
 
       
  1769     ec_slave_t *slave = fsm->slave;
 
       
  1770 
       
  1771     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  1772         || datagram->working_counter != 1) {
 
       
  1773         fsm->coe_state = ec_fsm_error;
 
       
  1774         EC_ERR("Reception of CoE dictionary request failed on slave %i.\n",
 
       
  1775                slave->ring_position);
 
       
  1776         return;
 
       
  1777     }
 
       
  1778 
       
  1779     fsm->coe_start = datagram->cycles_sent;
 
       
  1780 
       
  1781     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1782     ec_master_queue_datagram(fsm->master, datagram);
 
       
  1783     fsm->coe_state = ec_fsm_coe_dict_check;
 
       
  1784 }
 
       
  1785 
       
  1786 /*****************************************************************************/
 
       
  1787 
       
  1788 /**
 
       
  1789    CoE state: DICT CHECK.
 
       
  1790 */
 
       
  1791 
       
  1792 void ec_fsm_coe_dict_check(ec_fsm_t *fsm /**< finite state machine */)
 
       
  1793 {
 
       
  1794     ec_datagram_t *datagram = &fsm->datagram;
 
       
  1795     ec_slave_t *slave = fsm->slave;
 
       
  1796 
       
  1797     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  1798         || datagram->working_counter != 1) {
 
       
  1799         fsm->coe_state = ec_fsm_error;
 
       
  1800         EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",
 
       
  1801                slave->ring_position);
 
       
  1802         return;
 
       
  1803     }
 
       
  1804 
       
  1805     if (!ec_slave_mbox_check(datagram)) {
 
       
  1806         if (datagram->cycles_received
 
       
  1807             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
 
       
  1808             fsm->coe_state = ec_fsm_error;
 
       
  1809             EC_ERR("Timeout while checking SDO dictionary on slave %i.\n",
 
       
  1810                    slave->ring_position);
 
       
  1811             return;
 
       
  1812         }
 
       
  1813 
       
  1814         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1815         ec_master_queue_datagram(fsm->master, datagram);
 
       
  1816         return;
 
       
  1817     }
 
       
  1818 
       
  1819     // Fetch response
 
       
  1820     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  1821     ec_master_queue_datagram(fsm->master, datagram);
 
       
  1822     fsm->coe_state = ec_fsm_coe_dict_response;
 
       
  1823 }
 
       
  1824 
       
  1825 /*****************************************************************************/
 
       
  1826 
       
  1827 /**
 
       
  1828    CoE state: DICT RESPONSE.
 
       
  1829 */
 
       
  1830 
       
  1831 void ec_fsm_coe_dict_response(ec_fsm_t *fsm /**< finite state machine */)
 
       
  1832 {
 
       
  1833     ec_datagram_t *datagram = &fsm->datagram;
 
       
  1834     ec_slave_t *slave = fsm->slave;
 
       
  1835     uint8_t *data, mbox_prot;
 
       
  1836     size_t rec_size;
 
       
  1837     unsigned int sdo_count, i;
 
       
  1838     uint16_t sdo_index;
 
       
  1839     ec_sdo_t *sdo;
 
       
  1840 
       
  1841     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  1842         || datagram->working_counter != 1) {
 
       
  1843         fsm->coe_state = ec_fsm_error;
 
       
  1844         EC_ERR("Reception of CoE dictionary response failed on slave %i.\n",
 
       
  1845                slave->ring_position);
 
       
  1846         return;
 
       
  1847     }
 
       
  1848 
       
  1849     if (!(data = ec_slave_mbox_fetch(slave, datagram,
 
       
  1850 				     &mbox_prot, &rec_size))) {
 
       
  1851         fsm->coe_state = ec_fsm_error;
 
       
  1852         return;
 
       
  1853     }
 
       
  1854 
       
  1855     if (mbox_prot != 0x03) { // CoE
 
       
  1856         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  1857         fsm->coe_state = ec_fsm_error;
 
       
  1858 	return;
 
       
  1859     }
 
       
  1860 
       
  1861     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
       
  1862         (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
 
       
  1863         EC_ERR("SDO information error response at slave %i!\n",
 
       
  1864                slave->ring_position);
 
       
  1865         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
       
  1866         fsm->coe_state = ec_fsm_error;
 
       
  1867 	return;
 
       
  1868     }
 
       
  1869 
       
  1870     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
       
  1871         (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response
 
       
  1872         EC_ERR("Invalid SDO list response at slave %i!\n",
 
       
  1873                slave->ring_position);
 
       
  1874         ec_print_data(data, rec_size);
 
       
  1875         fsm->coe_state = ec_fsm_error;
 
       
  1876 	return;
 
       
  1877     }
 
       
  1878 
       
  1879     if (rec_size < 8) {
 
       
  1880         EC_ERR("Invalid data size!\n");
 
       
  1881         ec_print_data(data, rec_size);
 
       
  1882         fsm->coe_state = ec_fsm_error;
 
       
  1883 	return;
 
       
  1884     }
 
       
  1885 
       
  1886     sdo_count = (rec_size - 8) / 2;
 
       
  1887 
       
  1888     for (i = 0; i < sdo_count; i++) {
 
       
  1889         sdo_index = EC_READ_U16(data + 8 + i * 2);
 
       
  1890         if (!sdo_index) {
 
       
  1891             EC_WARN("SDO dictionary of slave %i contains index 0x0000.\n",
 
       
  1892                     slave->ring_position);
 
       
  1893             continue;
 
       
  1894         }
 
       
  1895 
       
  1896         if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) {
 
       
  1897             EC_ERR("Failed to allocate memory for SDO!\n");
 
       
  1898             fsm->coe_state = ec_fsm_error;
 
       
  1899             return;
 
       
  1900         }
 
       
  1901 
       
  1902         if (ec_sdo_init(sdo, sdo_index, slave)) {
 
       
  1903             EC_ERR("Failed to init SDO!\n");
 
       
  1904             fsm->coe_state = ec_fsm_error;
 
       
  1905             return;
 
       
  1906         }
 
       
  1907 
       
  1908         if (kobject_add(&sdo->kobj)) {
 
       
  1909             EC_ERR("Failed to add kobject.\n");
 
       
  1910             kobject_put(&sdo->kobj); // free
 
       
  1911             fsm->coe_state = ec_fsm_error;
 
       
  1912             return;
 
       
  1913         }
 
       
  1914 
       
  1915         list_add_tail(&sdo->list, &slave->sdo_dictionary);
 
       
  1916     }
 
       
  1917 
       
  1918     if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again.
 
       
  1919         fsm->coe_start = datagram->cycles_sent;
 
       
  1920         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1921         ec_master_queue_datagram(fsm->master, datagram);
 
       
  1922         fsm->coe_state = ec_fsm_coe_dict_check;
 
       
  1923         return;
 
       
  1924     }
 
       
  1925 
       
  1926     if (list_empty(&slave->sdo_dictionary)) {
 
       
  1927         // no SDOs in dictionary. finished.
 
       
  1928         fsm->coe_state = ec_fsm_end; // success
 
       
  1929         return;
 
       
  1930     }
 
       
  1931 
       
  1932     // fetch SDO descriptions
 
       
  1933     fsm->coe_sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);
 
       
  1934 
       
  1935     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {
 
       
  1936         fsm->coe_state = ec_fsm_error;
 
       
  1937         return;
 
       
  1938     }
 
       
  1939 
       
  1940     EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
       
  1941     EC_WRITE_U8 (data + 2, 0x03); // Get object description request
 
       
  1942     EC_WRITE_U8 (data + 3, 0x00);
 
       
  1943     EC_WRITE_U16(data + 4, 0x0000);
 
       
  1944     EC_WRITE_U16(data + 6, fsm->coe_sdo->index); // SDO index
 
       
  1945 
       
  1946     ec_master_queue_datagram(fsm->master, datagram);
 
       
  1947     fsm->coe_state = ec_fsm_coe_dict_desc_request;
 
       
  1948 }
 
       
  1949 
       
  1950 /*****************************************************************************/
 
       
  1951 
       
  1952 /**
 
       
  1953    CoE state: DICT DESC REQUEST.
 
       
  1954 */
 
       
  1955 
       
  1956 void ec_fsm_coe_dict_desc_request(ec_fsm_t *fsm /**< finite state machine */)
 
       
  1957 {
 
       
  1958     ec_datagram_t *datagram = &fsm->datagram;
 
       
  1959     ec_slave_t *slave = fsm->slave;
 
       
  1960 
       
  1961     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  1962         || datagram->working_counter != 1) {
 
       
  1963         fsm->coe_state = ec_fsm_error;
 
       
  1964         EC_ERR("Reception of CoE SDO description"
 
       
  1965                " request failed on slave %i.\n", slave->ring_position);
 
       
  1966         return;
 
       
  1967     }
 
       
  1968 
       
  1969     fsm->coe_start = datagram->cycles_sent;
 
       
  1970 
       
  1971     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1972     ec_master_queue_datagram(fsm->master, datagram);
 
       
  1973     fsm->coe_state = ec_fsm_coe_dict_desc_check;
 
       
  1974 }
 
       
  1975 
       
  1976 /*****************************************************************************/
 
       
  1977 
       
  1978 /**
 
       
  1979    CoE state: DICT DESC CHECK.
 
       
  1980 */
 
       
  1981 
       
  1982 void ec_fsm_coe_dict_desc_check(ec_fsm_t *fsm /**< finite state machine */)
 
       
  1983 {
 
       
  1984     ec_datagram_t *datagram = &fsm->datagram;
 
       
  1985     ec_slave_t *slave = fsm->slave;
 
       
  1986 
       
  1987     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  1988         || datagram->working_counter != 1) {
 
       
  1989         fsm->coe_state = ec_fsm_error;
 
       
  1990         EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",
 
       
  1991                slave->ring_position);
 
       
  1992         return;
 
       
  1993     }
 
       
  1994 
       
  1995     if (!ec_slave_mbox_check(datagram)) {
 
       
  1996         if (datagram->cycles_received
 
       
  1997             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
 
       
  1998             fsm->coe_state = ec_fsm_error;
 
       
  1999             EC_ERR("Timeout while checking SDO description on slave %i.\n",
 
       
  2000                    slave->ring_position);
 
       
  2001             return;
 
       
  2002         }
 
       
  2003 
       
  2004         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2005         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2006         return;
 
       
  2007     }
 
       
  2008 
       
  2009     // Fetch response
 
       
  2010     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  2011     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2012     fsm->coe_state = ec_fsm_coe_dict_desc_response;
 
       
  2013 }
 
       
  2014 
       
  2015 /*****************************************************************************/
 
       
  2016 
       
  2017 /**
 
       
  2018    CoE state: DICT DESC RESPONSE.
 
       
  2019 */
 
       
  2020 
       
  2021 void ec_fsm_coe_dict_desc_response(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2022 {
 
       
  2023     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2024     ec_slave_t *slave = fsm->slave;
 
       
  2025     ec_sdo_t *sdo = fsm->coe_sdo;
 
       
  2026     uint8_t *data, mbox_prot;
 
       
  2027     size_t rec_size, name_size;
 
       
  2028 
       
  2029     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2030         || datagram->working_counter != 1) {
 
       
  2031         fsm->coe_state = ec_fsm_error;
 
       
  2032         EC_ERR("Reception of CoE SDO description"
 
       
  2033                "response failed on slave %i.\n", slave->ring_position);
 
       
  2034         return;
 
       
  2035     }
 
       
  2036 
       
  2037     if (!(data = ec_slave_mbox_fetch(slave, datagram,
 
       
  2038 				     &mbox_prot, &rec_size))) {
 
       
  2039         fsm->coe_state = ec_fsm_error;
 
       
  2040         return;
 
       
  2041     }
 
       
  2042 
       
  2043     if (mbox_prot != 0x03) { // CoE
 
       
  2044         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  2045         fsm->coe_state = ec_fsm_error;
 
       
  2046 	return;
 
       
  2047     }
 
       
  2048 
       
  2049     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
       
  2050         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
 
       
  2051         EC_ERR("SDO information error response at slave %i while"
 
       
  2052                " fetching SDO 0x%04X!\n", slave->ring_position,
 
       
  2053                sdo->index);
 
       
  2054         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
       
  2055         fsm->coe_state = ec_fsm_error;
 
       
  2056 	return;
 
       
  2057     }
 
       
  2058 
       
  2059     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
       
  2060         (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response
 
       
  2061         EC_READ_U16(data + 6) != sdo->index) { // SDO index
 
       
  2062         EC_ERR("Invalid object description response at slave %i while"
 
       
  2063                " fetching SDO 0x%04X!\n", slave->ring_position,
 
       
  2064                sdo->index);
 
       
  2065         ec_print_data(data, rec_size);
 
       
  2066         fsm->coe_state = ec_fsm_error;
 
       
  2067 	return;
 
       
  2068     }
 
       
  2069 
       
  2070     if (rec_size < 12) {
 
       
  2071         EC_ERR("Invalid data size!\n");
 
       
  2072         ec_print_data(data, rec_size);
 
       
  2073         fsm->coe_state = ec_fsm_error;
 
       
  2074 	return;
 
       
  2075     }
 
       
  2076 
       
  2077     sdo->subindices = EC_READ_U8(data + 10);
 
       
  2078     sdo->object_code = EC_READ_U8(data + 11);
 
       
  2079 
       
  2080     name_size = rec_size - 12;
 
       
  2081     if (name_size) {
 
       
  2082         if (!(sdo->name = kmalloc(name_size + 1, GFP_ATOMIC))) {
 
       
  2083             EC_ERR("Failed to allocate SDO name!\n");
 
       
  2084             fsm->coe_state = ec_fsm_error;
 
       
  2085             return;
 
       
  2086         }
 
       
  2087 
       
  2088         memcpy(sdo->name, data + 12, name_size);
 
       
  2089         sdo->name[name_size] = 0;
 
       
  2090     }
 
       
  2091 
       
  2092     if (EC_READ_U8(data + 2) & 0x80) {
 
       
  2093         EC_ERR("Fragment follows (not implemented)!\n");
 
       
  2094         fsm->coe_state = ec_fsm_error;
 
       
  2095 	return;
 
       
  2096     }
 
       
  2097 
       
  2098     // start fetching entries
 
       
  2099 
       
  2100     fsm->coe_subindex = 0;
 
       
  2101 
       
  2102     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {
 
       
  2103         fsm->coe_state = ec_fsm_error;
 
       
  2104         return;
 
       
  2105     }
 
       
  2106 
       
  2107     EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
       
  2108     EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
 
       
  2109     EC_WRITE_U8 (data + 3, 0x00);
 
       
  2110     EC_WRITE_U16(data + 4, 0x0000);
 
       
  2111     EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
       
  2112     EC_WRITE_U8 (data + 8, fsm->coe_subindex); // SDO subindex
 
       
  2113     EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
 
       
  2114 
       
  2115     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2116     fsm->coe_state = ec_fsm_coe_dict_entry_request;
 
       
  2117 }
 
       
  2118 
       
  2119 /*****************************************************************************/
 
       
  2120 
       
  2121 /**
 
       
  2122    CoE state: DICT ENTRY REQUEST.
 
       
  2123 */
 
       
  2124 
       
  2125 void ec_fsm_coe_dict_entry_request(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2126 {
 
       
  2127     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2128     ec_slave_t *slave = fsm->slave;
 
       
  2129 
       
  2130     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2131         || datagram->working_counter != 1) {
 
       
  2132         fsm->coe_state = ec_fsm_error;
 
       
  2133         EC_ERR("Reception of CoE SDO entry request failed on slave %i.\n",
 
       
  2134                slave->ring_position);
 
       
  2135         return;
 
       
  2136     }
 
       
  2137 
       
  2138     fsm->coe_start = datagram->cycles_sent;
 
       
  2139 
       
  2140     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2141     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2142     fsm->coe_state = ec_fsm_coe_dict_entry_check;
 
       
  2143 }
 
       
  2144 
       
  2145 /*****************************************************************************/
 
       
  2146 
       
  2147 /**
 
       
  2148    CoE state: DICT ENTRY CHECK.
 
       
  2149 */
 
       
  2150 
       
  2151 void ec_fsm_coe_dict_entry_check(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2152 {
 
       
  2153     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2154     ec_slave_t *slave = fsm->slave;
 
       
  2155 
       
  2156     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2157         || datagram->working_counter != 1) {
 
       
  2158         fsm->coe_state = ec_fsm_error;
 
       
  2159         EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",
 
       
  2160                slave->ring_position);
 
       
  2161         return;
 
       
  2162     }
 
       
  2163 
       
  2164     if (!ec_slave_mbox_check(datagram)) {
 
       
  2165         if (datagram->cycles_received
 
       
  2166             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
 
       
  2167             fsm->coe_state = ec_fsm_error;
 
       
  2168             EC_ERR("Timeout while checking SDO entry on slave %i.\n",
 
       
  2169                    slave->ring_position);
 
       
  2170             return;
 
       
  2171         }
 
       
  2172 
       
  2173         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2174         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2175         return;
 
       
  2176     }
 
       
  2177 
       
  2178     // Fetch response
 
       
  2179     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  2180     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2181     fsm->coe_state = ec_fsm_coe_dict_entry_response;
 
       
  2182 }
 
       
  2183 
       
  2184 /*****************************************************************************/
 
       
  2185 
       
  2186 /**
 
       
  2187    CoE state: DICT ENTRY RESPONSE.
 
       
  2188 */
 
       
  2189 
       
  2190 void ec_fsm_coe_dict_entry_response(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2191 {
 
       
  2192     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2193     ec_slave_t *slave = fsm->slave;
 
       
  2194     ec_sdo_t *sdo = fsm->coe_sdo;
 
       
  2195     uint8_t *data, mbox_prot;
 
       
  2196     size_t rec_size, data_size;
 
       
  2197     ec_sdo_entry_t *entry;
 
       
  2198 
       
  2199     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2200         || datagram->working_counter != 1) {
 
       
  2201         fsm->coe_state = ec_fsm_error;
 
       
  2202         EC_ERR("Reception of CoE SDO description"
 
       
  2203                " response failed on slave %i.\n", slave->ring_position);
 
       
  2204         return;
 
       
  2205     }
 
       
  2206 
       
  2207     if (!(data = ec_slave_mbox_fetch(slave, datagram,
 
       
  2208 				     &mbox_prot, &rec_size))) {
 
       
  2209         fsm->coe_state = ec_fsm_error;
 
       
  2210         return;
 
       
  2211     }
 
       
  2212 
       
  2213     if (mbox_prot != 0x03) { // CoE
 
       
  2214         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  2215         fsm->coe_state = ec_fsm_error;
 
       
  2216 	return;
 
       
  2217     }
 
       
  2218 
       
  2219     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
       
  2220         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
 
       
  2221         EC_ERR("SDO information error response at slave %i while"
 
       
  2222                " fetching SDO entry 0x%04X:%i!\n", slave->ring_position,
 
       
  2223                sdo->index, fsm->coe_subindex);
 
       
  2224         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
       
  2225         fsm->coe_state = ec_fsm_error;
 
       
  2226 	return;
 
       
  2227     }
 
       
  2228 
       
  2229     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
       
  2230         (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response
 
       
  2231         EC_READ_U16(data + 6) != sdo->index || // SDO index
 
       
  2232         EC_READ_U8(data + 8) != fsm->coe_subindex) { // SDO subindex
 
       
  2233         EC_ERR("Invalid entry description response at slave %i while"
 
       
  2234                " fetching SDO entry 0x%04X:%i!\n", slave->ring_position,
 
       
  2235                sdo->index, fsm->coe_subindex);
 
       
  2236         ec_print_data(data, rec_size);
 
       
  2237         fsm->coe_state = ec_fsm_error;
 
       
  2238 	return;
 
       
  2239     }
 
       
  2240 
       
  2241     if (rec_size < 16) {
 
       
  2242         EC_ERR("Invalid data size!\n");
 
       
  2243         ec_print_data(data, rec_size);
 
       
  2244         fsm->coe_state = ec_fsm_error;
 
       
  2245 	return;
 
       
  2246     }
 
       
  2247 
       
  2248     data_size = rec_size - 16;
 
       
  2249 
       
  2250     if (!(entry = (ec_sdo_entry_t *)
 
       
  2251           kmalloc(sizeof(ec_sdo_entry_t), GFP_ATOMIC))) {
 
       
  2252         EC_ERR("Failed to allocate entry!\n");
 
       
  2253         fsm->coe_state = ec_fsm_error;
 
       
  2254 	return;
 
       
  2255     }
 
       
  2256 
       
  2257     if (ec_sdo_entry_init(entry, fsm->coe_subindex, sdo)) {
 
       
  2258         EC_ERR("Failed to init entry!\n");
 
       
  2259         fsm->coe_state = ec_fsm_error;
 
       
  2260 	return;
 
       
  2261     }
 
       
  2262 
       
  2263     entry->data_type = EC_READ_U16(data + 10);
 
       
  2264     entry->bit_length = EC_READ_U16(data + 12);
 
       
  2265 
       
  2266     if (data_size) {
 
       
  2267         if (!(entry->description = kmalloc(data_size + 1, GFP_ATOMIC))) {
 
       
  2268             EC_ERR("Failed to allocate SDO entry name!\n");
 
       
  2269             fsm->coe_state = ec_fsm_error;
 
       
  2270             return;
 
       
  2271         }
 
       
  2272         memcpy(entry->description, data + 16, data_size);
 
       
  2273         entry->description[data_size] = 0;
 
       
  2274     }
 
       
  2275 
       
  2276     if (kobject_add(&entry->kobj)) {
 
       
  2277         EC_ERR("Failed to add kobject.\n");
 
       
  2278         kobject_put(&entry->kobj); // free
 
       
  2279         fsm->coe_state = ec_fsm_error;
 
       
  2280         return;
 
       
  2281     }
 
       
  2282 
       
  2283     list_add_tail(&entry->list, &sdo->entries);
 
       
  2284 
       
  2285     if (fsm->coe_subindex < sdo->subindices) {
 
       
  2286         fsm->coe_subindex++;
 
       
  2287 
       
  2288         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {
 
       
  2289             fsm->coe_state = ec_fsm_error;
 
       
  2290             return;
 
       
  2291         }
 
       
  2292 
       
  2293         EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
       
  2294         EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
 
       
  2295         EC_WRITE_U8 (data + 3, 0x00);
 
       
  2296         EC_WRITE_U16(data + 4, 0x0000);
 
       
  2297         EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
       
  2298         EC_WRITE_U8 (data + 8, fsm->coe_subindex); // SDO subindex
 
       
  2299         EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
 
       
  2300 
       
  2301         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2302         fsm->coe_state = ec_fsm_coe_dict_entry_request;
 
       
  2303         return;
 
       
  2304     }
 
       
  2305 
       
  2306     // another SDO description to fetch?
 
       
  2307     if (fsm->coe_sdo->list.next != &slave->sdo_dictionary) {
 
       
  2308         fsm->coe_sdo = list_entry(fsm->coe_sdo->list.next, ec_sdo_t, list);
 
       
  2309 
       
  2310         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {
 
       
  2311             fsm->coe_state = ec_fsm_error;
 
       
  2312             return;
 
       
  2313         }
 
       
  2314 
       
  2315         EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
       
  2316         EC_WRITE_U8 (data + 2, 0x03); // Get object description request
 
       
  2317         EC_WRITE_U8 (data + 3, 0x00);
 
       
  2318         EC_WRITE_U16(data + 4, 0x0000);
 
       
  2319         EC_WRITE_U16(data + 6, fsm->coe_sdo->index); // SDO index
 
       
  2320 
       
  2321         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2322         fsm->coe_state = ec_fsm_coe_dict_desc_request;
 
       
  2323         return;
 
       
  2324     }
 
       
  2325 
       
  2326     fsm->coe_state = ec_fsm_end;
 
       
  2327 }
 
       
  2328 
       
  2329 /******************************************************************************
 
       
  2330  *  CoE state machine
 
       
  2331  *****************************************************************************/
 
       
  2332 
       
  2333 /**
 
       
  2334    CoE state: DOWN START.
 
       
  2335 */
 
       
  2336 
       
  2337 void ec_fsm_coe_down_start(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2338 {
 
       
  2339     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2340     ec_slave_t *slave = fsm->slave;
 
       
  2341     ec_sdo_data_t *sdodata = fsm->coe_sdodata;
 
       
  2342     uint8_t *data;
 
       
  2343 
       
  2344     EC_INFO("Downloading SDO 0x%04X:%i to slave %i.\n",
 
       
  2345             sdodata->index, sdodata->subindex, slave->ring_position);
 
       
  2346 
       
  2347     if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) {
 
       
  2348         EC_ERR("SDO fragmenting not supported yet!\n");
 
       
  2349         fsm->coe_state = ec_fsm_error;
 
       
  2350         return;
 
       
  2351     }
 
       
  2352 
       
  2353     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
 
       
  2354                                             sdodata->size + 10))) {
 
       
  2355         fsm->coe_state = ec_fsm_error;
 
       
  2356         return;
 
       
  2357     }
 
       
  2358 
       
  2359     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
       
  2360     EC_WRITE_U8 (data + 2, (0x1 // size specified
 
       
  2361                             | 0x1 << 5)); // Download request
 
       
  2362     EC_WRITE_U16(data + 3, sdodata->index);
 
       
  2363     EC_WRITE_U8 (data + 5, sdodata->subindex);
 
       
  2364     EC_WRITE_U32(data + 6, sdodata->size);
 
       
  2365     memcpy(data + 10, sdodata->data, sdodata->size);
 
       
  2366 
       
  2367     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2368     fsm->coe_state = ec_fsm_coe_down_request;
 
       
  2369 }
 
       
  2370 
       
  2371 /*****************************************************************************/
 
       
  2372 
       
  2373 /**
 
       
  2374    CoE state: DOWN REQUEST.
 
       
  2375 */
 
       
  2376 
       
  2377 void ec_fsm_coe_down_request(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2378 {
 
       
  2379     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2380     ec_slave_t *slave = fsm->slave;
 
       
  2381 
       
  2382     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2383         || datagram->working_counter != 1) {
 
       
  2384         fsm->coe_state = ec_fsm_error;
 
       
  2385         EC_ERR("Reception of CoE download request failed.\n");
 
       
  2386         return;
 
       
  2387     }
 
       
  2388 
       
  2389     fsm->coe_start = datagram->cycles_sent;
 
       
  2390 
       
  2391     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2392     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2393     fsm->coe_state = ec_fsm_coe_down_check;
 
       
  2394 }
 
       
  2395 
       
  2396 /*****************************************************************************/
 
       
  2397 
       
  2398 /**
 
       
  2399    CoE state: DOWN CHECK.
 
       
  2400 */
 
       
  2401 
       
  2402 void ec_fsm_coe_down_check(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2403 {
 
       
  2404     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2405     ec_slave_t *slave = fsm->slave;
 
       
  2406 
       
  2407     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2408         || datagram->working_counter != 1) {
 
       
  2409         fsm->coe_state = ec_fsm_error;
 
       
  2410         EC_ERR("Reception of CoE mailbox check datagram failed.\n");
 
       
  2411         return;
 
       
  2412     }
 
       
  2413 
       
  2414     if (!ec_slave_mbox_check(datagram)) {
 
       
  2415         if (datagram->cycles_received
 
       
  2416             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
 
       
  2417             fsm->coe_state = ec_fsm_error;
 
       
  2418             EC_ERR("Timeout while checking SDO configuration on slave %i.\n",
 
       
  2419                    slave->ring_position);
 
       
  2420             return;
 
       
  2421         }
 
       
  2422 
       
  2423         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2424         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2425         return;
 
       
  2426     }
 
       
  2427 
       
  2428     // Fetch response
 
       
  2429     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  2430     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2431     fsm->coe_state = ec_fsm_coe_down_response;
 
       
  2432 }
 
       
  2433 
       
  2434 /*****************************************************************************/
 
       
  2435 
       
  2436 /**
 
       
  2437    CoE state: DOWN RESPONSE.
 
       
  2438 */
 
       
  2439 
       
  2440 void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2441 {
 
       
  2442     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2443     ec_slave_t *slave = fsm->slave;
 
       
  2444     uint8_t *data, mbox_prot;
 
       
  2445     size_t rec_size;
 
       
  2446     ec_sdo_data_t *sdodata = fsm->coe_sdodata;
 
       
  2447 
       
  2448     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2449         || datagram->working_counter != 1) {
 
       
  2450         fsm->coe_state = ec_fsm_error;
 
       
  2451         EC_ERR("Reception of CoE download response failed.\n");
 
       
  2452         return;
 
       
  2453     }
 
       
  2454 
       
  2455     if (!(data = ec_slave_mbox_fetch(slave, datagram,
 
       
  2456 				     &mbox_prot, &rec_size))) {
 
       
  2457         fsm->coe_state = ec_fsm_error;
 
       
  2458         return;
 
       
  2459     }
 
       
  2460 
       
  2461     if (mbox_prot != 0x03) { // CoE
 
       
  2462         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  2463         fsm->coe_state = ec_fsm_error;
 
       
  2464 	return;
 
       
  2465     }
 
       
  2466 
       
  2467     if (rec_size < 6) {
 
       
  2468         fsm->coe_state = ec_fsm_error;
 
       
  2469         EC_ERR("Received data is too small (%i bytes):\n", rec_size);
 
       
  2470         ec_print_data(data, rec_size);
 
       
  2471         return;
 
       
  2472     }
 
       
  2473 
       
  2474     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
       
  2475         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
       
  2476         fsm->coe_state = ec_fsm_error;
 
       
  2477         EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",
 
       
  2478                sdodata->index, sdodata->subindex, sdodata->size,
 
       
  2479                slave->ring_position);
 
       
  2480         if (rec_size < 10) {
 
       
  2481             EC_ERR("Incomplete Abort command:\n");
 
       
  2482             ec_print_data(data, rec_size);
 
       
  2483         }
 
       
  2484         else
 
       
  2485             ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
       
  2486         return;
 
       
  2487     }
 
       
  2488 
       
  2489     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
 
       
  2490         EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
 
       
  2491         EC_READ_U16(data + 3) != sdodata->index || // index
 
       
  2492         EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex
 
       
  2493         fsm->coe_state = ec_fsm_error;
 
       
  2494         EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n",
 
       
  2495                sdodata->index, sdodata->subindex, sdodata->size);
 
       
  2496         EC_ERR("Invalid SDO download response at slave %i!\n",
 
       
  2497                slave->ring_position);
 
       
  2498         ec_print_data(data, rec_size);
 
       
  2499         return;
 
       
  2500     }
 
       
  2501 
       
  2502     fsm->coe_state = ec_fsm_end; // success
 
       
  2503 }
 
       
  2504 
       
  2505 /*****************************************************************************/
 
       
  2506 
       
  2507 /**
 
       
  2508    CoE state: UP START.
 
       
  2509 */
 
       
  2510 
       
  2511 void ec_fsm_coe_up_start(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2512 {
 
       
  2513     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2514     ec_slave_t *slave = fsm->slave;
 
       
  2515     ec_master_t *master = slave->master;
 
       
  2516     ec_sdo_request_t *request = fsm->coe_request;
 
       
  2517     ec_sdo_t *sdo = request->sdo;
 
       
  2518     ec_sdo_entry_t *entry = request->entry;
 
       
  2519     uint8_t *data;
 
       
  2520 
       
  2521     EC_INFO("Uploading SDO 0x%04X:%i from slave %i.\n",
 
       
  2522             sdo->index, entry->subindex, slave->ring_position);
 
       
  2523 
       
  2524     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 6))) {
 
       
  2525         fsm->coe_state = ec_fsm_error;
 
       
  2526         return;
 
       
  2527     }
 
       
  2528 
       
  2529     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
       
  2530     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
 
       
  2531     EC_WRITE_U16(data + 3, sdo->index);
 
       
  2532     EC_WRITE_U8 (data + 5, entry->subindex);
 
       
  2533 
       
  2534     if (master->debug_level) {
 
       
  2535         EC_DBG("Upload request:\n");
 
       
  2536         ec_print_data(data, 6);
 
       
  2537     }
 
       
  2538 
       
  2539     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2540     fsm->coe_state = ec_fsm_coe_up_request;
 
       
  2541 }
 
       
  2542 
       
  2543 /*****************************************************************************/
 
       
  2544 
       
  2545 /**
 
       
  2546    CoE state: UP REQUEST.
 
       
  2547 */
 
       
  2548 
       
  2549 void ec_fsm_coe_up_request(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2550 {
 
       
  2551     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2552     ec_slave_t *slave = fsm->slave;
 
       
  2553 
       
  2554     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2555         || datagram->working_counter != 1) {
 
       
  2556         fsm->coe_state = ec_fsm_error;
 
       
  2557         EC_ERR("Reception of CoE upload request failed.\n");
 
       
  2558         return;
 
       
  2559     }
 
       
  2560 
       
  2561     fsm->coe_start = datagram->cycles_sent;
 
       
  2562 
       
  2563     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2564     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2565     fsm->coe_state = ec_fsm_coe_up_check;
 
       
  2566 }
 
       
  2567 
       
  2568 /*****************************************************************************/
 
       
  2569 
       
  2570 /**
 
       
  2571    CoE state: UP CHECK.
 
       
  2572 */
 
       
  2573 
       
  2574 void ec_fsm_coe_up_check(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2575 {
 
       
  2576     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2577     ec_slave_t *slave = fsm->slave;
 
       
  2578 
       
  2579     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2580         || datagram->working_counter != 1) {
 
       
  2581         fsm->coe_state = ec_fsm_error;
 
       
  2582         EC_ERR("Reception of CoE mailbox check datagram failed.\n");
 
       
  2583         return;
 
       
  2584     }
 
       
  2585 
       
  2586     if (!ec_slave_mbox_check(datagram)) {
 
       
  2587         if (datagram->cycles_received
 
       
  2588             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
 
       
  2589             fsm->coe_state = ec_fsm_error;
 
       
  2590             EC_ERR("Timeout while checking SDO upload on slave %i.\n",
 
       
  2591                    slave->ring_position);
 
       
  2592             return;
 
       
  2593         }
 
       
  2594 
       
  2595         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2596         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2597         return;
 
       
  2598     }
 
       
  2599 
       
  2600     // Fetch response
 
       
  2601     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  2602     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2603     fsm->coe_state = ec_fsm_coe_up_response;
 
       
  2604 }
 
       
  2605 
       
  2606 /*****************************************************************************/
 
       
  2607 
       
  2608 /**
 
       
  2609    CoE state: UP RESPONSE.
 
       
  2610 */
 
       
  2611 
       
  2612 void ec_fsm_coe_up_response(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2613 {
 
       
  2614     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2615     ec_slave_t *slave = fsm->slave;
 
       
  2616     ec_master_t *master = slave->master;
 
       
  2617     uint8_t *data, mbox_prot;
 
       
  2618     size_t rec_size, data_size;
 
       
  2619     ec_sdo_request_t *request = fsm->coe_request;
 
       
  2620     ec_sdo_t *sdo = request->sdo;
 
       
  2621     ec_sdo_entry_t *entry = request->entry;
 
       
  2622     uint32_t complete_size;
 
       
  2623     unsigned int expedited, size_specified;
 
       
  2624 
       
  2625     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2626         || datagram->working_counter != 1) {
 
       
  2627         fsm->coe_state = ec_fsm_error;
 
       
  2628         EC_ERR("Reception of CoE upload response failed.\n");
 
       
  2629         return;
 
       
  2630     }
 
       
  2631 
       
  2632     if (!(data = ec_slave_mbox_fetch(slave, datagram,
 
       
  2633 				     &mbox_prot, &rec_size))) {
 
       
  2634         fsm->coe_state = ec_fsm_error;
 
       
  2635         return;
 
       
  2636     }
 
       
  2637 
       
  2638     if (master->debug_level) {
 
       
  2639         EC_DBG("Upload response:\n");
 
       
  2640         ec_print_data(data, rec_size);
 
       
  2641     }
 
       
  2642 
       
  2643     if (mbox_prot != 0x03) { // CoE
 
       
  2644         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  2645         fsm->coe_state = ec_fsm_error;
 
       
  2646 	return;
 
       
  2647     }
 
       
  2648 
       
  2649     if (rec_size < 10) {
 
       
  2650         EC_ERR("Received currupted SDO upload response!\n");
 
       
  2651         ec_print_data(data, rec_size);
 
       
  2652         fsm->coe_state = ec_fsm_error;
 
       
  2653 	return;
 
       
  2654     }
 
       
  2655 
       
  2656     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
       
  2657         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
       
  2658         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
       
  2659                sdo->index, entry->subindex, slave->ring_position);
 
       
  2660         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
       
  2661         fsm->coe_state = ec_fsm_error;
 
       
  2662 	return;
 
       
  2663     }
 
       
  2664 
       
  2665     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
 
       
  2666         EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response
 
       
  2667         EC_READ_U16(data + 3) != sdo->index || // index
 
       
  2668         EC_READ_U8 (data + 5) != entry->subindex) { // subindex
 
       
  2669         EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex);
 
       
  2670         EC_ERR("Invalid SDO upload response at slave %i!\n",
 
       
  2671                slave->ring_position);
 
       
  2672         ec_print_data(data, rec_size);
 
       
  2673         fsm->coe_state = ec_fsm_error;
 
       
  2674 	return;
 
       
  2675     }
 
       
  2676 
       
  2677     data_size = rec_size - 10;
 
       
  2678     expedited = EC_READ_U8(data + 2) & 0x02;
 
       
  2679 
       
  2680     if (expedited) {
 
       
  2681         EC_WARN("Received expedited response upon normal request!\n");
 
       
  2682 
       
  2683         size_specified = EC_READ_U8(data + 2) & 0x01;
 
       
  2684         if (size_specified) {
 
       
  2685             complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
 
       
  2686         }
 
       
  2687         else {
 
       
  2688             complete_size = 4;
 
       
  2689         }
 
       
  2690     }
 
       
  2691     else {
 
       
  2692         complete_size = EC_READ_U32(data + 6);
 
       
  2693     }
 
       
  2694 
       
  2695     if (request->data) {
 
       
  2696         kfree(request->data);
 
       
  2697         request->data = NULL;
 
       
  2698     }
 
       
  2699     request->size = 0;
 
       
  2700 
       
  2701     if (complete_size) {
 
       
  2702         if (!(request->data = (uint8_t *) kmalloc(complete_size + 1, GFP_ATOMIC))) {
 
       
  2703             EC_ERR("Failed to allocate %i bytes of SDO data!\n", complete_size);
 
       
  2704             fsm->coe_state = ec_fsm_error;
 
       
  2705             return;
 
       
  2706         }
 
       
  2707         request->data[complete_size] = 0x00; // just to be sure...
 
       
  2708     }
 
       
  2709 
       
  2710     if (expedited) {
 
       
  2711         memcpy(request->data, data + 6, complete_size);
 
       
  2712         request->size = complete_size;
 
       
  2713     }
 
       
  2714     else {
 
       
  2715         memcpy(request->data, data + 10, data_size);
 
       
  2716         request->size = data_size;
 
       
  2717         fsm->coe_toggle = 0;
 
       
  2718 
       
  2719         if (data_size < complete_size) {
 
       
  2720             EC_WARN("SDO data incomplete (%i / %i).\n", data_size, complete_size);
 
       
  2721 
       
  2722             if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) {
 
       
  2723                 fsm->coe_state = ec_fsm_error;
 
       
  2724                 return;
 
       
  2725             }
 
       
  2726 
       
  2727             EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
       
  2728             EC_WRITE_U8 (data + 2, (fsm->coe_toggle << 4 // toggle
 
       
  2729                                     | 0x3 << 5)); // upload segment request
 
       
  2730 
       
  2731             if (master->debug_level) {
 
       
  2732                 EC_DBG("Upload segment request:\n");
 
       
  2733                 ec_print_data(data, 3);
 
       
  2734             }
 
       
  2735 
       
  2736             ec_master_queue_datagram(fsm->master, datagram);
 
       
  2737             fsm->coe_state = ec_fsm_coe_up_seg_request;
 
       
  2738             return;
 
       
  2739         }
 
       
  2740     }
 
       
  2741 
       
  2742     fsm->coe_state = ec_fsm_end; // success
 
       
  2743 }
 
       
  2744 
       
  2745 /*****************************************************************************/
 
       
  2746 
       
  2747 /**
 
       
  2748    CoE state: UP REQUEST.
 
       
  2749 */
 
       
  2750 
       
  2751 void ec_fsm_coe_up_seg_request(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2752 {
 
       
  2753     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2754     ec_slave_t *slave = fsm->slave;
 
       
  2755 
       
  2756     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2757         || datagram->working_counter != 1) {
 
       
  2758         fsm->coe_state = ec_fsm_error;
 
       
  2759         EC_ERR("Reception of CoE upload segment request failed.\n");
 
       
  2760         return;
 
       
  2761     }
 
       
  2762 
       
  2763     fsm->coe_start = datagram->cycles_sent;
 
       
  2764 
       
  2765     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2766     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2767     fsm->coe_state = ec_fsm_coe_up_seg_check;
 
       
  2768 }
 
       
  2769 
       
  2770 /*****************************************************************************/
 
       
  2771 
       
  2772 /**
 
       
  2773    CoE state: UP CHECK.
 
       
  2774 */
 
       
  2775 
       
  2776 void ec_fsm_coe_up_seg_check(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2777 {
 
       
  2778     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2779     ec_slave_t *slave = fsm->slave;
 
       
  2780 
       
  2781     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2782         || datagram->working_counter != 1) {
 
       
  2783         fsm->coe_state = ec_fsm_error;
 
       
  2784         EC_ERR("Reception of CoE mailbox check datagram failed.\n");
 
       
  2785         return;
 
       
  2786     }
 
       
  2787 
       
  2788     if (!ec_slave_mbox_check(datagram)) {
 
       
  2789         if (datagram->cycles_received
 
       
  2790             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
 
       
  2791             fsm->coe_state = ec_fsm_error;
 
       
  2792             EC_ERR("Timeout while checking SDO upload segment on slave %i.\n",
 
       
  2793                    slave->ring_position);
 
       
  2794             return;
 
       
  2795         }
 
       
  2796 
       
  2797         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  2798         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2799         return;
 
       
  2800     }
 
       
  2801 
       
  2802     // Fetch response
 
       
  2803     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  2804     ec_master_queue_datagram(fsm->master, datagram);
 
       
  2805     fsm->coe_state = ec_fsm_coe_up_seg_response;
 
       
  2806 }
 
       
  2807 
       
  2808 /*****************************************************************************/
 
       
  2809 
       
  2810 /**
 
       
  2811    CoE state: UP RESPONSE.
 
       
  2812 */
 
       
  2813 
       
  2814 void ec_fsm_coe_up_seg_response(ec_fsm_t *fsm /**< finite state machine */)
 
       
  2815 {
 
       
  2816     ec_datagram_t *datagram = &fsm->datagram;
 
       
  2817     ec_slave_t *slave = fsm->slave;
 
       
  2818     ec_master_t *master = slave->master;
 
       
  2819     uint8_t *data, mbox_prot;
 
       
  2820     size_t rec_size, data_size;
 
       
  2821     ec_sdo_request_t *request = fsm->coe_request;
 
       
  2822     ec_sdo_t *sdo = request->sdo;
 
       
  2823     ec_sdo_entry_t *entry = request->entry;
 
       
  2824     uint32_t seg_size;
 
       
  2825     unsigned int last_segment;
 
       
  2826 
       
  2827     if (datagram->state != EC_DATAGRAM_RECEIVED
 
       
  2828         || datagram->working_counter != 1) {
 
       
  2829         fsm->coe_state = ec_fsm_error;
 
       
  2830         EC_ERR("Reception of CoE upload segment response failed.\n");
 
       
  2831         return;
 
       
  2832     }
 
       
  2833 
       
  2834     if (!(data = ec_slave_mbox_fetch(slave, datagram,
 
       
  2835 				     &mbox_prot, &rec_size))) {
 
       
  2836         fsm->coe_state = ec_fsm_error;
 
       
  2837         return;
 
       
  2838     }
 
       
  2839 
       
  2840     if (master->debug_level) {
 
       
  2841         EC_DBG("Upload segment response:\n");
 
       
  2842         ec_print_data(data, rec_size);
 
       
  2843     }
 
       
  2844 
       
  2845     if (mbox_prot != 0x03) { // CoE
 
       
  2846         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  2847         fsm->coe_state = ec_fsm_error;
 
       
  2848 	return;
 
       
  2849     }
 
       
  2850 
       
  2851     if (rec_size < 10) {
 
       
  2852         EC_ERR("Received currupted SDO upload segment response!\n");
 
       
  2853         ec_print_data(data, rec_size);
 
       
  2854         fsm->coe_state = ec_fsm_error;
 
       
  2855 	return;
 
       
  2856     }
 
       
  2857 
       
  2858     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
       
  2859         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
       
  2860         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
       
  2861                sdo->index, entry->subindex, slave->ring_position);
 
       
  2862         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
       
  2863         fsm->coe_state = ec_fsm_error;
 
       
  2864 	return;
 
       
  2865     }
 
       
  2866 
       
  2867     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
 
       
  2868         EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response
 
       
  2869         EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex);
 
       
  2870         EC_ERR("Invalid SDO upload segment response at slave %i!\n",
 
       
  2871                slave->ring_position);
 
       
  2872         ec_print_data(data, rec_size);
 
       
  2873         fsm->coe_state = ec_fsm_error;
 
       
  2874 	return;
 
       
  2875     }
 
       
  2876 
       
  2877     last_segment = EC_READ_U8(data + 2) & 0x01;
 
       
  2878     seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
 
       
  2879     data_size = rec_size - 10;
 
       
  2880 
       
  2881     if (data_size != seg_size) {
 
       
  2882         EC_WARN("SDO segment data invalid (%i / %i)"
 
       
  2883                 " - Fragmenting not implemented.\n",
 
       
  2884                 data_size, seg_size);
 
       
  2885     }
 
       
  2886 
       
  2887     memcpy(request->data + request->size, data + 10, data_size);
 
       
  2888     request->size += data_size;
 
       
  2889 
       
  2890     if (!last_segment) {
 
       
  2891         fsm->coe_toggle = !fsm->coe_toggle;
 
       
  2892 
       
  2893         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) {
 
       
  2894             fsm->coe_state = ec_fsm_error;
 
       
  2895             return;
 
       
  2896         }
 
       
  2897 
       
  2898         EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
       
  2899         EC_WRITE_U8 (data + 2, (fsm->coe_toggle << 4 // toggle
 
       
  2900                                 | 0x3 << 5)); // upload segment request
 
       
  2901 
       
  2902         if (master->debug_level) {
 
       
  2903             EC_DBG("Upload segment request:\n");
 
       
  2904             ec_print_data(data, 3);
 
       
  2905         }
 
       
  2906 
       
  2907         ec_master_queue_datagram(fsm->master, datagram);
 
       
  2908         fsm->coe_state = ec_fsm_coe_up_seg_request;
 
       
  2909         return;
 
       
  2910     }
 
       
  2911 
       
  2912     fsm->coe_state = ec_fsm_end; // success
 
       
  2913 }
 
       
  2914 
       
  2915 /*****************************************************************************/
 
       
  2916 
       
  2917 /**
 
       
  2918    SDO abort messages.
 
       
  2919    The "abort SDO transfer request" supplies an abort code,
 
       
  2920    which can be translated to clear text. This table does
 
       
  2921    the mapping of the codes and messages.
 
       
  2922 */
 
       
  2923 
       
  2924 const ec_code_msg_t sdo_abort_messages[] = {
 
       
  2925     {0x05030000, "Toggle bit not changed"},
 
       
  2926     {0x05040000, "SDO protocol timeout"},
 
       
  2927     {0x05040001, "Client/Server command specifier not valid or unknown"},
 
       
  2928     {0x05040005, "Out of memory"},
 
       
  2929     {0x06010000, "Unsupported access to an object"},
 
       
  2930     {0x06010001, "Attempt to read a write-only object"},
 
       
  2931     {0x06010002, "Attempt to write a read-only object"},
 
       
  2932     {0x06020000, "This object does not exist in the object directory"},
 
       
  2933     {0x06040041, "The object cannot be mapped into the PDO"},
 
       
  2934     {0x06040042, "The number and length of the objects to be mapped would"
 
       
  2935      " exceed the PDO length"},
 
       
  2936     {0x06040043, "General parameter incompatibility reason"},
 
       
  2937     {0x06040047, "Gerneral internal incompatibility in device"},
 
       
  2938     {0x06060000, "Access failure due to a hardware error"},
 
       
  2939     {0x06070010, "Data type does not match, length of service parameter does"
 
       
  2940      " not match"},
 
       
  2941     {0x06070012, "Data type does not match, length of service parameter too"
 
       
  2942      " high"},
 
       
  2943     {0x06070013, "Data type does not match, length of service parameter too"
 
       
  2944      " low"},
 
       
  2945     {0x06090011, "Subindex does not exist"},
 
       
  2946     {0x06090030, "Value range of parameter exceeded"},
 
       
  2947     {0x06090031, "Value of parameter written too high"},
 
       
  2948     {0x06090032, "Value of parameter written too low"},
 
       
  2949     {0x06090036, "Maximum value is less than minimum value"},
 
       
  2950     {0x08000000, "General error"},
 
       
  2951     {0x08000020, "Data cannot be transferred or stored to the application"},
 
       
  2952     {0x08000021, "Data cannot be transferred or stored to the application"
 
       
  2953      " because of local control"},
 
       
  2954     {0x08000022, "Data cannot be transferred or stored to the application"
 
       
  2955      " because of the present device state"},
 
       
  2956     {0x08000023, "Object dictionary dynamic generation fails or no object"
 
       
  2957      " dictionary is present"},
 
       
  2958     {}
 
       
  2959 };
 
       
  2960 
       
  2961 /*****************************************************************************/
 
       
  2962 
       
  2963 /**
 
       
  2964    Outputs an SDO abort message.
 
       
  2965 */
 
       
  2966 
       
  2967 void ec_canopen_abort_msg(uint32_t abort_code)
 
       
  2968 {
 
       
  2969     const ec_code_msg_t *abort_msg;
 
       
  2970 
       
  2971     for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {
 
       
  2972         if (abort_msg->code == abort_code) {
 
       
  2973             EC_ERR("SDO abort message 0x%08X: \"%s\".\n",
 
       
  2974                    abort_msg->code, abort_msg->message);
 
       
  2975             return;
 
       
  2976         }
 
       
  2977     }
 
       
  2978 
       
  2979     EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code);
 
       
  2980 }
 
       
  2981 
       
  2982 /******************************************************************************
 
       
  2983  *  Common state functions
 
  1704  *  Common state functions
 
  2984  *****************************************************************************/
 
  1705  *****************************************************************************/
 
  2985 
  1706 
  2986 /**
 
  1707 /**
 
  2987    State: ERROR.
 
  1708    State: ERROR.
etherlabmaster