1502 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1502 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1503 { |
1503 { |
1504 ec_datagram_t *datagram = fsm->datagram; |
1504 ec_datagram_t *datagram = fsm->datagram; |
1505 ec_slave_t *slave = fsm->slave; |
1505 ec_slave_t *slave = fsm->slave; |
1506 ec_master_t *master = slave->master; |
1506 ec_master_t *master = slave->master; |
1507 uint8_t *data, mbox_prot; |
1507 uint16_t rec_index; |
|
1508 uint8_t *data, mbox_prot, rec_subindex; |
1508 size_t rec_size, data_size; |
1509 size_t rec_size, data_size; |
1509 ec_sdo_request_t *request = fsm->request; |
1510 ec_sdo_request_t *request = fsm->request; |
1510 unsigned int expedited, size_specified; |
1511 unsigned int expedited, size_specified; |
1511 |
1512 |
1512 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1513 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1551 fsm->retries = EC_FSM_RETRIES; |
1552 fsm->retries = EC_FSM_RETRIES; |
1552 fsm->state = ec_fsm_coe_up_check; |
1553 fsm->state = ec_fsm_coe_up_check; |
1553 return; |
1554 return; |
1554 } |
1555 } |
1555 |
1556 |
1556 if (rec_size < 3) { |
1557 if (rec_size < 6) { |
1557 fsm->state = ec_fsm_coe_error; |
1558 fsm->state = ec_fsm_coe_error; |
1558 EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size); |
1559 EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size); |
1559 ec_print_data(data, rec_size); |
1560 ec_print_data(data, rec_size); |
1560 return; |
1561 return; |
1561 } |
1562 } |
1562 |
1563 |
1563 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1564 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1564 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1565 EC_READ_U8(data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1565 EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", |
1566 EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", |
1566 request->index, request->subindex, slave->ring_position); |
1567 request->index, request->subindex, slave->ring_position); |
1567 if (rec_size >= 10) { |
1568 if (rec_size >= 10) { |
1568 request->abort_code = EC_READ_U32(data + 6); |
1569 request->abort_code = EC_READ_U32(data + 6); |
1569 ec_canopen_abort_msg(request->abort_code); |
1570 ec_canopen_abort_msg(request->abort_code); |
1572 } |
1573 } |
1573 fsm->state = ec_fsm_coe_error; |
1574 fsm->state = ec_fsm_coe_error; |
1574 return; |
1575 return; |
1575 } |
1576 } |
1576 |
1577 |
|
1578 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
|
1579 EC_READ_U8(data + 2) >> 5 != 0x2) { // upload response |
|
1580 EC_ERR("Received unknown response while uploading SDO 0x%04X:%02X" |
|
1581 " from slave %u.\n", request->index, request->subindex, |
|
1582 slave->ring_position); |
|
1583 ec_print_data(data, rec_size); |
|
1584 fsm->state = ec_fsm_coe_error; |
|
1585 return; |
|
1586 } |
|
1587 |
|
1588 rec_index = EC_READ_U16(data + 3); |
|
1589 rec_subindex = EC_READ_U8(data + 5); |
|
1590 |
|
1591 if (rec_index != request->index || rec_subindex != request->subindex) { |
|
1592 EC_ERR("Received upload response for wrong SDO (0x%04X:%02X," |
|
1593 " requested: 0x%04X:%02X) from slave %u.\n", |
|
1594 rec_index, rec_subindex, request->index, request->subindex, |
|
1595 slave->ring_position); |
|
1596 ec_print_data(data, rec_size); |
|
1597 |
|
1598 // check for CoE response again |
|
1599 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1600 fsm->retries = EC_FSM_RETRIES; |
|
1601 fsm->state = ec_fsm_coe_up_check; |
|
1602 return; |
|
1603 } |
|
1604 |
1577 // normal or expedited? |
1605 // normal or expedited? |
1578 expedited = EC_READ_U8(data + 2) & 0x02; |
1606 expedited = EC_READ_U8(data + 2) & 0x02; |
1579 |
1607 |
1580 if (expedited) { |
1608 if (expedited) { |
1581 if (rec_size < 7) { |
|
1582 fsm->state = ec_fsm_coe_error; |
|
1583 EC_ERR("Received currupted SDO expedited upload" |
|
1584 " response (only %u bytes)!\n", rec_size); |
|
1585 ec_print_data(data, rec_size); |
|
1586 return; |
|
1587 } |
|
1588 |
|
1589 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
|
1590 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
|
1591 EC_READ_U16(data + 3) != request->index || // index |
|
1592 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
|
1593 if (fsm->slave->master->debug_level) { |
|
1594 EC_DBG("Invalid SDO upload expedited response at slave %u!\n", |
|
1595 slave->ring_position); |
|
1596 ec_print_data(data, rec_size); |
|
1597 } |
|
1598 // check for CoE response again |
|
1599 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1600 fsm->retries = EC_FSM_RETRIES; |
|
1601 fsm->state = ec_fsm_coe_up_check; |
|
1602 return; |
|
1603 } |
|
1604 |
|
1605 size_specified = EC_READ_U8(data + 2) & 0x01; |
1609 size_specified = EC_READ_U8(data + 2) & 0x01; |
1606 if (size_specified) { |
1610 if (size_specified) { |
1607 fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1611 fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1608 } else { |
1612 } else { |
1609 fsm->complete_size = 4; |
1613 fsm->complete_size = 4; |
1625 if (rec_size < 10) { |
1629 if (rec_size < 10) { |
1626 fsm->state = ec_fsm_coe_error; |
1630 fsm->state = ec_fsm_coe_error; |
1627 EC_ERR("Received currupted SDO normal upload" |
1631 EC_ERR("Received currupted SDO normal upload" |
1628 " response (only %u bytes)!\n", rec_size); |
1632 " response (only %u bytes)!\n", rec_size); |
1629 ec_print_data(data, rec_size); |
1633 ec_print_data(data, rec_size); |
1630 return; |
|
1631 } |
|
1632 |
|
1633 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
|
1634 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
|
1635 EC_READ_U16(data + 3) != request->index || // index |
|
1636 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
|
1637 if (fsm->slave->master->debug_level) { |
|
1638 EC_DBG("Invalid SDO normal upload response at slave %u!\n", |
|
1639 slave->ring_position); |
|
1640 ec_print_data(data, rec_size); |
|
1641 } |
|
1642 // check for CoE response again |
|
1643 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1644 fsm->retries = EC_FSM_RETRIES; |
|
1645 fsm->state = ec_fsm_coe_up_check; |
|
1646 return; |
1634 return; |
1647 } |
1635 } |
1648 |
1636 |
1649 data_size = rec_size - 10; |
1637 data_size = rec_size - 10; |
1650 fsm->complete_size = EC_READ_U32(data + 6); |
1638 fsm->complete_size = EC_READ_U32(data + 6); |
1860 fsm->state = ec_fsm_coe_error; |
1848 fsm->state = ec_fsm_coe_error; |
1861 return; |
1849 return; |
1862 } |
1850 } |
1863 |
1851 |
1864 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1852 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1865 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1853 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1866 EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", |
1854 EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", |
1867 request->index, request->subindex, slave->ring_position); |
1855 request->index, request->subindex, slave->ring_position); |
1868 request->abort_code = EC_READ_U32(data + 6); |
1856 request->abort_code = EC_READ_U32(data + 6); |
1869 ec_canopen_abort_msg(request->abort_code); |
1857 ec_canopen_abort_msg(request->abort_code); |
1870 fsm->state = ec_fsm_coe_error; |
1858 fsm->state = ec_fsm_coe_error; |