1156 { |
1156 { |
1157 ec_datagram_t *datagram = fsm->datagram; |
1157 ec_datagram_t *datagram = fsm->datagram; |
1158 ec_slave_t *slave = fsm->slave; |
1158 ec_slave_t *slave = fsm->slave; |
1159 ec_master_t *master = slave->master; |
1159 ec_master_t *master = slave->master; |
1160 ec_sdo_request_t *request = fsm->request; |
1160 ec_sdo_request_t *request = fsm->request; |
1161 ec_sdo_t *sdo = request->sdo; |
|
1162 ec_sdo_entry_t *entry = request->entry; |
1161 ec_sdo_entry_t *entry = request->entry; |
1163 uint8_t *data; |
1162 uint8_t *data; |
1164 |
1163 |
1165 if (master->debug_level) |
1164 if (master->debug_level) |
1166 EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n", |
1165 EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n", |
1167 sdo->index, entry->subindex, slave->ring_position); |
1166 entry->sdo->index, entry->subindex, slave->ring_position); |
1168 |
1167 |
1169 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1168 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1170 fsm->state = ec_fsm_coe_error; |
1169 fsm->state = ec_fsm_coe_error; |
1171 return; |
1170 return; |
1172 } |
1171 } |
1173 |
1172 |
1174 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1173 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1175 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1174 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1176 EC_WRITE_U16(data + 3, sdo->index); |
1175 EC_WRITE_U16(data + 3, entry->sdo->index); |
1177 EC_WRITE_U8 (data + 5, entry->subindex); |
1176 EC_WRITE_U8 (data + 5, entry->subindex); |
1178 memset(data + 6, 0x00, 4); |
1177 memset(data + 6, 0x00, 4); |
1179 |
1178 |
1180 if (master->debug_level) { |
1179 if (master->debug_level) { |
1181 EC_DBG("Upload request:\n"); |
1180 EC_DBG("Upload request:\n"); |
1287 ec_slave_t *slave = fsm->slave; |
1286 ec_slave_t *slave = fsm->slave; |
1288 ec_master_t *master = slave->master; |
1287 ec_master_t *master = slave->master; |
1289 uint8_t *data, mbox_prot; |
1288 uint8_t *data, mbox_prot; |
1290 size_t rec_size, data_size; |
1289 size_t rec_size, data_size; |
1291 ec_sdo_request_t *request = fsm->request; |
1290 ec_sdo_request_t *request = fsm->request; |
1292 ec_sdo_t *sdo = request->sdo; |
|
1293 ec_sdo_entry_t *entry = request->entry; |
1291 ec_sdo_entry_t *entry = request->entry; |
1294 uint32_t complete_size; |
1292 uint32_t complete_size; |
1295 unsigned int expedited, size_specified; |
1293 unsigned int expedited, size_specified; |
1296 |
1294 |
1297 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1295 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1338 } |
1336 } |
1339 |
1337 |
1340 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1338 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1341 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1339 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1342 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
1340 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
1343 sdo->index, entry->subindex, slave->ring_position); |
1341 entry->sdo->index, entry->subindex, slave->ring_position); |
1344 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1342 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1345 fsm->state = ec_fsm_coe_error; |
1343 fsm->state = ec_fsm_coe_error; |
1346 return; |
1344 return; |
1347 } |
1345 } |
1348 |
1346 |
1349 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1347 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1350 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1348 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1351 EC_READ_U16(data + 3) != sdo->index || // index |
1349 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1352 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1350 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1353 EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex); |
1351 EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1354 EC_ERR("Invalid SDO upload response at slave %i!\n", |
1352 EC_ERR("Invalid SDO upload response at slave %i!\n", |
1355 slave->ring_position); |
1353 slave->ring_position); |
1356 ec_print_data(data, rec_size); |
1354 ec_print_data(data, rec_size); |
1357 fsm->state = ec_fsm_coe_error; |
1355 fsm->state = ec_fsm_coe_error; |
1358 return; |
1356 return; |
1529 ec_slave_t *slave = fsm->slave; |
1527 ec_slave_t *slave = fsm->slave; |
1530 ec_master_t *master = slave->master; |
1528 ec_master_t *master = slave->master; |
1531 uint8_t *data, mbox_prot; |
1529 uint8_t *data, mbox_prot; |
1532 size_t rec_size, data_size; |
1530 size_t rec_size, data_size; |
1533 ec_sdo_request_t *request = fsm->request; |
1531 ec_sdo_request_t *request = fsm->request; |
1534 ec_sdo_t *sdo = request->sdo; |
|
1535 ec_sdo_entry_t *entry = request->entry; |
1532 ec_sdo_entry_t *entry = request->entry; |
1536 uint32_t seg_size; |
1533 uint32_t seg_size; |
1537 unsigned int last_segment; |
1534 unsigned int last_segment; |
1538 |
1535 |
1539 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1536 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1580 } |
1577 } |
1581 |
1578 |
1582 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1579 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1583 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1580 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1584 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
1581 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
1585 sdo->index, entry->subindex, slave->ring_position); |
1582 entry->sdo->index, entry->subindex, slave->ring_position); |
1586 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1583 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1587 fsm->state = ec_fsm_coe_error; |
1584 fsm->state = ec_fsm_coe_error; |
1588 return; |
1585 return; |
1589 } |
1586 } |
1590 |
1587 |
1591 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1588 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1592 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1589 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1593 EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex); |
1590 EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1594 EC_ERR("Invalid SDO upload segment response at slave %i!\n", |
1591 EC_ERR("Invalid SDO upload segment response at slave %i!\n", |
1595 slave->ring_position); |
1592 slave->ring_position); |
1596 ec_print_data(data, rec_size); |
1593 ec_print_data(data, rec_size); |
1597 fsm->state = ec_fsm_coe_error; |
1594 fsm->state = ec_fsm_coe_error; |
1598 return; |
1595 return; |