878 } |
878 } |
879 |
879 |
880 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
880 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
881 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
881 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
882 EC_ERR("Sdo information error response at slave %u while" |
882 EC_ERR("Sdo information error response at slave %u while" |
883 " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position, |
883 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
884 sdo->index, fsm->subindex); |
884 sdo->index, fsm->subindex); |
885 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
885 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
886 fsm->state = ec_fsm_coe_error; |
886 fsm->state = ec_fsm_coe_error; |
887 return; |
887 return; |
888 } |
888 } |
890 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
890 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
891 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
891 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
892 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
892 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
893 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
893 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
894 EC_ERR("Invalid entry description response at slave %u while" |
894 EC_ERR("Invalid entry description response at slave %u while" |
895 " fetching Sdo entry 0x%04X:%u!\n", slave->ring_position, |
895 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
896 sdo->index, fsm->subindex); |
896 sdo->index, fsm->subindex); |
897 ec_print_data(data, rec_size); |
897 ec_print_data(data, rec_size); |
898 fsm->state = ec_fsm_coe_error; |
898 fsm->state = ec_fsm_coe_error; |
899 return; |
899 return; |
900 } |
900 } |
996 ec_slave_t *slave = fsm->slave; |
996 ec_slave_t *slave = fsm->slave; |
997 ec_sdo_request_t *request = fsm->request; |
997 ec_sdo_request_t *request = fsm->request; |
998 uint8_t *data; |
998 uint8_t *data; |
999 |
999 |
1000 if (fsm->slave->master->debug_level) { |
1000 if (fsm->slave->master->debug_level) { |
1001 EC_DBG("Downloading Sdo 0x%04X:%u to slave %u.\n", |
1001 EC_DBG("Downloading Sdo 0x%04X:%02X to slave %u.\n", |
1002 request->index, request->subindex, slave->ring_position); |
1002 request->index, request->subindex, slave->ring_position); |
1003 ec_print_data(request->data, request->data_size); |
1003 ec_print_data(request->data, request->data_size); |
1004 } |
1004 } |
1005 |
1005 |
1006 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1006 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1183 } |
1183 } |
1184 |
1184 |
1185 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1185 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1186 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1186 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1187 fsm->state = ec_fsm_coe_error; |
1187 fsm->state = ec_fsm_coe_error; |
1188 EC_ERR("Sdo download 0x%04X:%X (%u bytes) aborted on slave %u.\n", |
1188 EC_ERR("Sdo download 0x%04X:%02X (%u bytes) aborted on slave %u.\n", |
1189 request->index, request->subindex, request->data_size, |
1189 request->index, request->subindex, request->data_size, |
1190 slave->ring_position); |
1190 slave->ring_position); |
1191 if (rec_size < 10) { |
1191 if (rec_size < 10) { |
1192 EC_ERR("Incomplete Abort command:\n"); |
1192 EC_ERR("Incomplete Abort command:\n"); |
1193 ec_print_data(data, rec_size); |
1193 ec_print_data(data, rec_size); |
1200 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1200 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1201 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1201 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1202 EC_READ_U16(data + 3) != request->index || // index |
1202 EC_READ_U16(data + 3) != request->index || // index |
1203 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1203 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1204 fsm->state = ec_fsm_coe_error; |
1204 fsm->state = ec_fsm_coe_error; |
1205 EC_ERR("Sdo download 0x%04X:%X (%u bytes) failed:\n", |
1205 EC_ERR("Sdo download 0x%04X:%02X (%u bytes) failed:\n", |
1206 request->index, request->subindex, request->data_size); |
1206 request->index, request->subindex, request->data_size); |
1207 EC_ERR("Invalid Sdo download response at slave %u!\n", |
1207 EC_ERR("Invalid Sdo download response at slave %u!\n", |
1208 slave->ring_position); |
1208 slave->ring_position); |
1209 ec_print_data(data, rec_size); |
1209 ec_print_data(data, rec_size); |
1210 return; |
1210 return; |
1226 ec_master_t *master = slave->master; |
1226 ec_master_t *master = slave->master; |
1227 ec_sdo_request_t *request = fsm->request; |
1227 ec_sdo_request_t *request = fsm->request; |
1228 uint8_t *data; |
1228 uint8_t *data; |
1229 |
1229 |
1230 if (master->debug_level) |
1230 if (master->debug_level) |
1231 EC_DBG("Uploading Sdo 0x%04X:%u from slave %u.\n", |
1231 EC_DBG("Uploading Sdo 0x%04X:%02X from slave %u.\n", |
1232 request->index, request->subindex, slave->ring_position); |
1232 request->index, request->subindex, slave->ring_position); |
1233 |
1233 |
1234 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1234 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1235 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1235 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1236 fsm->state = ec_fsm_coe_error; |
1236 fsm->state = ec_fsm_coe_error; |
1414 return; |
1414 return; |
1415 } |
1415 } |
1416 |
1416 |
1417 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1417 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1418 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1418 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1419 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %u.\n", |
1419 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1420 request->index, request->subindex, slave->ring_position); |
1420 request->index, request->subindex, slave->ring_position); |
1421 if (rec_size >= 10) |
1421 if (rec_size >= 10) |
1422 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1422 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1423 else |
1423 else |
1424 EC_ERR("No abort message.\n"); |
1424 EC_ERR("No abort message.\n"); |
1441 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1441 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1442 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1442 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1443 EC_READ_U16(data + 3) != request->index || // index |
1443 EC_READ_U16(data + 3) != request->index || // index |
1444 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1444 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1445 fsm->state = ec_fsm_coe_error; |
1445 fsm->state = ec_fsm_coe_error; |
1446 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1446 EC_ERR("Sdo upload 0x%04X:%02X failed:\n", |
1447 request->index, request->subindex); |
1447 request->index, request->subindex); |
1448 EC_ERR("Invalid Sdo upload expedited response at slave %u!\n", |
1448 EC_ERR("Invalid Sdo upload expedited response at slave %u!\n", |
1449 slave->ring_position); |
1449 slave->ring_position); |
1450 ec_print_data(data, rec_size); |
1450 ec_print_data(data, rec_size); |
1451 return; |
1451 return; |
1482 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1482 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1483 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1483 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1484 EC_READ_U16(data + 3) != request->index || // index |
1484 EC_READ_U16(data + 3) != request->index || // index |
1485 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1485 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1486 fsm->state = ec_fsm_coe_error; |
1486 fsm->state = ec_fsm_coe_error; |
1487 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1487 EC_ERR("Sdo upload 0x%04X:%02X failed:\n", |
1488 request->index, request->subindex); |
1488 request->index, request->subindex); |
1489 EC_ERR("Invalid Sdo normal upload response at slave %u!\n", |
1489 EC_ERR("Invalid Sdo normal upload response at slave %u!\n", |
1490 slave->ring_position); |
1490 slave->ring_position); |
1491 ec_print_data(data, rec_size); |
1491 ec_print_data(data, rec_size); |
1492 return; |
1492 return; |
1705 return; |
1705 return; |
1706 } |
1706 } |
1707 |
1707 |
1708 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1708 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1709 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1709 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1710 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %u.\n", |
1710 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1711 request->index, request->subindex, slave->ring_position); |
1711 request->index, request->subindex, slave->ring_position); |
1712 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1712 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1713 fsm->state = ec_fsm_coe_error; |
1713 fsm->state = ec_fsm_coe_error; |
1714 return; |
1714 return; |
1715 } |
1715 } |
1716 |
1716 |
1717 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1717 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1718 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1718 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1719 EC_ERR("Sdo upload 0x%04X:%X failed:\n", request->index, request->subindex); |
1719 EC_ERR("Sdo upload 0x%04X:%02X failed:\n", request->index, request->subindex); |
1720 EC_ERR("Invalid Sdo upload segment response at slave %u!\n", |
1720 EC_ERR("Invalid Sdo upload segment response at slave %u!\n", |
1721 slave->ring_position); |
1721 slave->ring_position); |
1722 ec_print_data(data, rec_size); |
1722 ec_print_data(data, rec_size); |
1723 fsm->state = ec_fsm_coe_error; |
1723 fsm->state = ec_fsm_coe_error; |
1724 return; |
1724 return; |