1492 fsm->state = ec_fsm_coe_up_response; |
1492 fsm->state = ec_fsm_coe_up_response; |
1493 } |
1493 } |
1494 |
1494 |
1495 /*****************************************************************************/ |
1495 /*****************************************************************************/ |
1496 |
1496 |
|
1497 /** Prepare an SDO upload segment request. |
|
1498 */ |
|
1499 void ec_fsm_coe_up_prepare_segment_request( |
|
1500 ec_fsm_coe_t *fsm /**< Finite state machine */ |
|
1501 ) |
|
1502 { |
|
1503 uint8_t *data = |
|
1504 ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram, 0x03, 10); |
|
1505 if (IS_ERR(data)) { |
|
1506 fsm->state = ec_fsm_coe_error; |
|
1507 return; |
|
1508 } |
|
1509 |
|
1510 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
1511 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
|
1512 | 0x3 << 5)); // upload segment request |
|
1513 |
|
1514 if (fsm->slave->master->debug_level) { |
|
1515 EC_DBG("Upload segment request:\n"); |
|
1516 ec_print_data(data, 10); |
|
1517 } |
|
1518 } |
|
1519 |
|
1520 /*****************************************************************************/ |
|
1521 |
1497 /** |
1522 /** |
1498 CoE state: UP RESPONSE. |
1523 CoE state: UP RESPONSE. |
1499 \todo Timeout behavior |
1524 \todo Timeout behavior |
1500 */ |
1525 */ |
1501 |
1526 |
1659 if (data_size < fsm->complete_size) { |
1684 if (data_size < fsm->complete_size) { |
1660 if (master->debug_level) |
1685 if (master->debug_level) |
1661 EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n", |
1686 EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n", |
1662 data_size, fsm->complete_size); |
1687 data_size, fsm->complete_size); |
1663 |
1688 |
1664 data = ec_slave_mbox_prepare_send(slave, datagram, |
1689 ec_fsm_coe_up_prepare_segment_request(fsm); |
1665 0x03, 3); |
|
1666 if (IS_ERR(data)) { |
|
1667 fsm->state = ec_fsm_coe_error; |
|
1668 return; |
|
1669 } |
|
1670 |
|
1671 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
1672 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
|
1673 | 0x3 << 5)); // upload segment request |
|
1674 |
|
1675 if (master->debug_level) { |
|
1676 EC_DBG("Upload segment request:\n"); |
|
1677 ec_print_data(data, 3); |
|
1678 } |
|
1679 |
|
1680 fsm->retries = EC_FSM_RETRIES; |
1690 fsm->retries = EC_FSM_RETRIES; |
1681 fsm->state = ec_fsm_coe_up_seg_request; |
1691 fsm->state = ec_fsm_coe_up_seg_request; |
1682 return; |
1692 return; |
1683 } |
1693 } |
1684 } |
1694 } |
1874 } |
1884 } |
1875 |
1885 |
1876 last_segment = EC_READ_U8(data + 2) & 0x01; |
1886 last_segment = EC_READ_U8(data + 2) & 0x01; |
1877 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1887 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1878 if (rec_size > 10) { |
1888 if (rec_size > 10) { |
1879 data_size = rec_size - 10; |
1889 data_size = rec_size - 3; /* Header of segment upload is smaller than |
|
1890 normal upload */ |
1880 } else { // == 10 |
1891 } else { // == 10 |
1881 /* seg_size contains the number of trailing bytes to ignore. */ |
1892 /* seg_size contains the number of trailing bytes to ignore. */ |
1882 data_size = rec_size - seg_size; |
1893 data_size = rec_size - seg_size; |
1883 } |
1894 } |
1884 |
1895 |
1888 request->index, request->subindex, slave->ring_position); |
1899 request->index, request->subindex, slave->ring_position); |
1889 fsm->state = ec_fsm_coe_error; |
1900 fsm->state = ec_fsm_coe_error; |
1890 return; |
1901 return; |
1891 } |
1902 } |
1892 |
1903 |
1893 memcpy(request->data + request->data_size, data + 10, data_size); |
1904 memcpy(request->data + request->data_size, data + 3, data_size); |
1894 request->data_size += data_size; |
1905 request->data_size += data_size; |
1895 |
1906 |
1896 if (!last_segment) { |
1907 if (!last_segment) { |
1897 fsm->toggle = !fsm->toggle; |
1908 fsm->toggle = !fsm->toggle; |
1898 |
1909 ec_fsm_coe_up_prepare_segment_request(fsm); |
1899 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3); |
|
1900 if (IS_ERR(data)) { |
|
1901 fsm->state = ec_fsm_coe_error; |
|
1902 return; |
|
1903 } |
|
1904 |
|
1905 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
1906 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
|
1907 | 0x3 << 5)); // upload segment request |
|
1908 |
|
1909 if (master->debug_level) { |
|
1910 EC_DBG("Upload segment request:\n"); |
|
1911 ec_print_data(data, 3); |
|
1912 } |
|
1913 |
|
1914 fsm->retries = EC_FSM_RETRIES; |
1910 fsm->retries = EC_FSM_RETRIES; |
1915 fsm->state = ec_fsm_coe_up_seg_request; |
1911 fsm->state = ec_fsm_coe_up_seg_request; |
1916 return; |
1912 return; |
1917 } |
1913 } |
1918 |
1914 |