equal
deleted
inserted
replaced
1491 ec_slave_t *slave = fsm->slave; |
1491 ec_slave_t *slave = fsm->slave; |
1492 ec_master_t *master = slave->master; |
1492 ec_master_t *master = slave->master; |
1493 uint8_t *data, mbox_prot; |
1493 uint8_t *data, mbox_prot; |
1494 size_t rec_size, data_size; |
1494 size_t rec_size, data_size; |
1495 ec_sdo_request_t *request = fsm->request; |
1495 ec_sdo_request_t *request = fsm->request; |
1496 uint32_t complete_size; |
|
1497 unsigned int expedited, size_specified; |
1496 unsigned int expedited, size_specified; |
1498 |
1497 |
1499 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1498 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1500 return; // FIXME: request again? |
1499 return; // FIXME: request again? |
1501 |
1500 |
1589 return; |
1588 return; |
1590 } |
1589 } |
1591 |
1590 |
1592 size_specified = EC_READ_U8(data + 2) & 0x01; |
1591 size_specified = EC_READ_U8(data + 2) & 0x01; |
1593 if (size_specified) { |
1592 if (size_specified) { |
1594 complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1593 fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1595 } else { |
1594 } else { |
1596 complete_size = 4; |
1595 fsm->complete_size = 4; |
1597 } |
1596 } |
1598 |
1597 |
1599 if (rec_size < 6 + complete_size) { |
1598 if (rec_size < 6 + fsm->complete_size) { |
1600 fsm->state = ec_fsm_coe_error; |
1599 fsm->state = ec_fsm_coe_error; |
1601 EC_ERR("Received currupted SDO expedited upload" |
1600 EC_ERR("Received currupted SDO expedited upload" |
1602 " response (only %u bytes)!\n", rec_size); |
1601 " response (only %u bytes)!\n", rec_size); |
1603 ec_print_data(data, rec_size); |
1602 ec_print_data(data, rec_size); |
1604 return; |
1603 return; |
1605 } |
1604 } |
1606 |
1605 |
1607 if (ec_sdo_request_copy_data(request, data + 6, complete_size)) { |
1606 if (ec_sdo_request_copy_data(request, data + 6, fsm->complete_size)) { |
1608 fsm->state = ec_fsm_coe_error; |
1607 fsm->state = ec_fsm_coe_error; |
1609 return; |
1608 return; |
1610 } |
1609 } |
1611 } else { // normal |
1610 } else { // normal |
1612 if (rec_size < 10) { |
1611 if (rec_size < 10) { |
1632 fsm->state = ec_fsm_coe_up_check; |
1631 fsm->state = ec_fsm_coe_up_check; |
1633 return; |
1632 return; |
1634 } |
1633 } |
1635 |
1634 |
1636 data_size = rec_size - 10; |
1635 data_size = rec_size - 10; |
1637 complete_size = EC_READ_U32(data + 6); |
1636 fsm->complete_size = EC_READ_U32(data + 6); |
1638 |
1637 |
1639 if (!complete_size) { |
1638 if (!fsm->complete_size) { |
1640 fsm->state = ec_fsm_coe_error; |
1639 fsm->state = ec_fsm_coe_error; |
1641 EC_ERR("No complete size supplied!\n"); |
1640 EC_ERR("No complete size supplied!\n"); |
1642 ec_print_data(data, rec_size); |
1641 ec_print_data(data, rec_size); |
1643 return; |
1642 return; |
1644 } |
1643 } |
1645 |
1644 |
1646 if (ec_sdo_request_alloc(request, complete_size)) { |
1645 if (ec_sdo_request_alloc(request, fsm->complete_size)) { |
1647 fsm->state = ec_fsm_coe_error; |
1646 fsm->state = ec_fsm_coe_error; |
1648 return; |
1647 return; |
1649 } |
1648 } |
1650 |
1649 |
1651 if (ec_sdo_request_copy_data(request, data + 10, data_size)) { |
1650 if (ec_sdo_request_copy_data(request, data + 10, data_size)) { |
1653 return; |
1652 return; |
1654 } |
1653 } |
1655 |
1654 |
1656 fsm->toggle = 0; |
1655 fsm->toggle = 0; |
1657 |
1656 |
1658 if (data_size < complete_size) { |
1657 if (data_size < fsm->complete_size) { |
1659 EC_WARN("SDO data incomplete (%u / %u).\n", |
1658 if (master->debug_level) |
1660 data_size, complete_size); |
1659 EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n", |
|
1660 data_size, fsm->complete_size); |
1661 |
1661 |
1662 data = ec_slave_mbox_prepare_send(slave, datagram, |
1662 data = ec_slave_mbox_prepare_send(slave, datagram, |
1663 0x03, 3); |
1663 0x03, 3); |
1664 if (IS_ERR(data)) { |
1664 if (IS_ERR(data)) { |
1665 fsm->state = ec_fsm_coe_error; |
1665 fsm->state = ec_fsm_coe_error; |
1781 /*****************************************************************************/ |
1781 /*****************************************************************************/ |
1782 |
1782 |
1783 /** |
1783 /** |
1784 CoE state: UP RESPONSE. |
1784 CoE state: UP RESPONSE. |
1785 \todo Timeout behavior |
1785 \todo Timeout behavior |
1786 \todo Check for \a data_size exceeding \a complete_size. |
|
1787 */ |
1786 */ |
1788 |
1787 |
1789 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1788 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1790 { |
1789 { |
1791 ec_datagram_t *datagram = fsm->datagram; |
1790 ec_datagram_t *datagram = fsm->datagram; |
1872 return; |
1871 return; |
1873 } |
1872 } |
1874 |
1873 |
1875 last_segment = EC_READ_U8(data + 2) & 0x01; |
1874 last_segment = EC_READ_U8(data + 2) & 0x01; |
1876 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1875 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1877 data_size = rec_size - 10; |
1876 if (rec_size > 10) { |
1878 |
1877 data_size = rec_size - 10; |
1879 if (data_size != seg_size) { |
1878 } else { // == 10 |
1880 EC_WARN("SDO segment data invalid (%u / %u)" |
1879 /* seg_size contains the number of trailing bytes to ignore. */ |
1881 " - Fragmenting not implemented.\n", |
1880 data_size = rec_size - seg_size; |
1882 data_size, seg_size); |
1881 } |
|
1882 |
|
1883 if (request->data_size + data_size > fsm->complete_size) { |
|
1884 EC_ERR("SDO upload 0x%04X:%02X failed on slave %u: Fragment" |
|
1885 " exceeding complete size!\n", |
|
1886 request->index, request->subindex, slave->ring_position); |
|
1887 fsm->state = ec_fsm_coe_error; |
|
1888 return; |
1883 } |
1889 } |
1884 |
1890 |
1885 memcpy(request->data + request->data_size, data + 10, data_size); |
1891 memcpy(request->data + request->data_size, data + 10, data_size); |
1886 request->data_size += data_size; |
1892 request->data_size += data_size; |
1887 |
1893 |
1906 fsm->retries = EC_FSM_RETRIES; |
1912 fsm->retries = EC_FSM_RETRIES; |
1907 fsm->state = ec_fsm_coe_up_seg_request; |
1913 fsm->state = ec_fsm_coe_up_seg_request; |
1908 return; |
1914 return; |
1909 } |
1915 } |
1910 |
1916 |
|
1917 if (request->data_size != fsm->complete_size) { |
|
1918 EC_WARN("SDO upload 0x%04X:%02X on slave %u: Assembled data" |
|
1919 " size (%u) does not match complete size (%u)!\n", |
|
1920 request->index, request->subindex, slave->ring_position, |
|
1921 request->data_size, fsm->complete_size); |
|
1922 } |
|
1923 |
1911 if (master->debug_level) { |
1924 if (master->debug_level) { |
1912 EC_DBG("Uploaded data:\n"); |
1925 EC_DBG("Uploaded data:\n"); |
1913 ec_print_data(request->data, request->data_size); |
1926 ec_print_data(request->data, request->data_size); |
1914 } |
1927 } |
1915 |
1928 |