equal
deleted
inserted
replaced
1483 ec_slave_t *slave = fsm->slave; |
1483 ec_slave_t *slave = fsm->slave; |
1484 ec_master_t *master = slave->master; |
1484 ec_master_t *master = slave->master; |
1485 uint8_t *data, mbox_prot; |
1485 uint8_t *data, mbox_prot; |
1486 size_t rec_size, data_size; |
1486 size_t rec_size, data_size; |
1487 ec_sdo_request_t *request = fsm->request; |
1487 ec_sdo_request_t *request = fsm->request; |
1488 uint32_t complete_size; |
|
1489 unsigned int expedited, size_specified; |
1488 unsigned int expedited, size_specified; |
1490 |
1489 |
1491 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1490 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1492 return; // FIXME: request again? |
1491 return; // FIXME: request again? |
1493 |
1492 |
1581 return; |
1580 return; |
1582 } |
1581 } |
1583 |
1582 |
1584 size_specified = EC_READ_U8(data + 2) & 0x01; |
1583 size_specified = EC_READ_U8(data + 2) & 0x01; |
1585 if (size_specified) { |
1584 if (size_specified) { |
1586 complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1585 fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1587 } else { |
1586 } else { |
1588 complete_size = 4; |
1587 fsm->complete_size = 4; |
1589 } |
1588 } |
1590 |
1589 |
1591 if (rec_size < 6 + complete_size) { |
1590 if (rec_size < 6 + fsm->complete_size) { |
1592 fsm->state = ec_fsm_coe_error; |
1591 fsm->state = ec_fsm_coe_error; |
1593 EC_ERR("Received currupted SDO expedited upload" |
1592 EC_ERR("Received currupted SDO expedited upload" |
1594 " response (only %u bytes)!\n", rec_size); |
1593 " response (only %u bytes)!\n", rec_size); |
1595 ec_print_data(data, rec_size); |
1594 ec_print_data(data, rec_size); |
1596 return; |
1595 return; |
1597 } |
1596 } |
1598 |
1597 |
1599 if (ec_sdo_request_copy_data(request, data + 6, complete_size)) { |
1598 if (ec_sdo_request_copy_data(request, data + 6, fsm->complete_size)) { |
1600 fsm->state = ec_fsm_coe_error; |
1599 fsm->state = ec_fsm_coe_error; |
1601 return; |
1600 return; |
1602 } |
1601 } |
1603 } else { // normal |
1602 } else { // normal |
1604 if (rec_size < 10) { |
1603 if (rec_size < 10) { |
1624 fsm->state = ec_fsm_coe_up_check; |
1623 fsm->state = ec_fsm_coe_up_check; |
1625 return; |
1624 return; |
1626 } |
1625 } |
1627 |
1626 |
1628 data_size = rec_size - 10; |
1627 data_size = rec_size - 10; |
1629 complete_size = EC_READ_U32(data + 6); |
1628 fsm->complete_size = EC_READ_U32(data + 6); |
1630 |
1629 |
1631 if (!complete_size) { |
1630 if (!fsm->complete_size) { |
1632 fsm->state = ec_fsm_coe_error; |
1631 fsm->state = ec_fsm_coe_error; |
1633 EC_ERR("No complete size supplied!\n"); |
1632 EC_ERR("No complete size supplied!\n"); |
1634 ec_print_data(data, rec_size); |
1633 ec_print_data(data, rec_size); |
1635 return; |
1634 return; |
1636 } |
1635 } |
1637 |
1636 |
1638 if (ec_sdo_request_alloc(request, complete_size)) { |
1637 if (ec_sdo_request_alloc(request, fsm->complete_size)) { |
1639 fsm->state = ec_fsm_coe_error; |
1638 fsm->state = ec_fsm_coe_error; |
1640 return; |
1639 return; |
1641 } |
1640 } |
1642 |
1641 |
1643 if (ec_sdo_request_copy_data(request, data + 10, data_size)) { |
1642 if (ec_sdo_request_copy_data(request, data + 10, data_size)) { |
1645 return; |
1644 return; |
1646 } |
1645 } |
1647 |
1646 |
1648 fsm->toggle = 0; |
1647 fsm->toggle = 0; |
1649 |
1648 |
1650 if (data_size < complete_size) { |
1649 if (data_size < fsm->complete_size) { |
1651 EC_WARN("SDO data incomplete (%u / %u).\n", |
1650 if (master->debug_level) |
1652 data_size, complete_size); |
1651 EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n", |
|
1652 data_size, fsm->complete_size); |
1653 |
1653 |
1654 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, |
1654 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, |
1655 0x03, 3))) { |
1655 0x03, 3))) { |
1656 fsm->state = ec_fsm_coe_error; |
1656 fsm->state = ec_fsm_coe_error; |
1657 return; |
1657 return; |
1772 /*****************************************************************************/ |
1772 /*****************************************************************************/ |
1773 |
1773 |
1774 /** |
1774 /** |
1775 CoE state: UP RESPONSE. |
1775 CoE state: UP RESPONSE. |
1776 \todo Timeout behavior |
1776 \todo Timeout behavior |
1777 \todo Check for \a data_size exceeding \a complete_size. |
|
1778 */ |
1777 */ |
1779 |
1778 |
1780 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1779 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1781 { |
1780 { |
1782 ec_datagram_t *datagram = fsm->datagram; |
1781 ec_datagram_t *datagram = fsm->datagram; |
1863 return; |
1862 return; |
1864 } |
1863 } |
1865 |
1864 |
1866 last_segment = EC_READ_U8(data + 2) & 0x01; |
1865 last_segment = EC_READ_U8(data + 2) & 0x01; |
1867 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1866 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1868 data_size = rec_size - 10; |
1867 if (rec_size > 10) { |
1869 |
1868 data_size = rec_size - 10; |
1870 if (data_size != seg_size) { |
1869 } else { // == 10 |
1871 EC_WARN("SDO segment data invalid (%u / %u)" |
1870 /* seg_size contains the number of trailing bytes to ignore. */ |
1872 " - Fragmenting not implemented.\n", |
1871 data_size = rec_size - seg_size; |
1873 data_size, seg_size); |
1872 } |
|
1873 |
|
1874 if (request->data_size + data_size > fsm->complete_size) { |
|
1875 EC_ERR("SDO upload 0x%04X:%02X failed on slave %u: Fragment" |
|
1876 " exceeding complete size!\n", |
|
1877 request->index, request->subindex, slave->ring_position); |
|
1878 fsm->state = ec_fsm_coe_error; |
|
1879 return; |
1874 } |
1880 } |
1875 |
1881 |
1876 memcpy(request->data + request->data_size, data + 10, data_size); |
1882 memcpy(request->data + request->data_size, data + 10, data_size); |
1877 request->data_size += data_size; |
1883 request->data_size += data_size; |
1878 |
1884 |
1896 fsm->retries = EC_FSM_RETRIES; |
1902 fsm->retries = EC_FSM_RETRIES; |
1897 fsm->state = ec_fsm_coe_up_seg_request; |
1903 fsm->state = ec_fsm_coe_up_seg_request; |
1898 return; |
1904 return; |
1899 } |
1905 } |
1900 |
1906 |
|
1907 if (request->data_size != fsm->complete_size) { |
|
1908 EC_WARN("SDO upload 0x%04X:%02X on slave %u: Assembled data" |
|
1909 " size (%u) does not match complete size (%u)!\n", |
|
1910 request->index, request->subindex, slave->ring_position, |
|
1911 request->data_size, fsm->complete_size); |
|
1912 } |
|
1913 |
1901 if (master->debug_level) { |
1914 if (master->debug_level) { |
1902 EC_DBG("Uploaded data:\n"); |
1915 EC_DBG("Uploaded data:\n"); |
1903 ec_print_data(request->data, request->data_size); |
1916 ec_print_data(request->data, request->data_size); |
1904 } |
1917 } |
1905 |
1918 |