1173 { |
1173 { |
1174 ec_datagram_t *datagram = fsm->datagram; |
1174 ec_datagram_t *datagram = fsm->datagram; |
1175 ec_slave_t *slave = fsm->slave; |
1175 ec_slave_t *slave = fsm->slave; |
1176 ec_master_t *master = slave->master; |
1176 ec_master_t *master = slave->master; |
1177 ec_sdo_request_t *request = fsm->request; |
1177 ec_sdo_request_t *request = fsm->request; |
1178 ec_sdo_entry_t *entry = request->entry; |
|
1179 uint8_t *data; |
1178 uint8_t *data; |
1180 |
1179 |
1181 if (master->debug_level) |
1180 if (master->debug_level) |
1182 EC_DBG("Uploading Sdo 0x%04X:%i from slave %i.\n", |
1181 EC_DBG("Uploading Sdo 0x%04X:%i from slave %i.\n", |
1183 entry->sdo->index, entry->subindex, slave->ring_position); |
1182 request->index, request->subindex, slave->ring_position); |
1184 |
1183 |
1185 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
1184 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
1186 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1185 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1187 fsm->state = ec_fsm_coe_error; |
1186 fsm->state = ec_fsm_coe_error; |
1188 return; |
1187 return; |
1193 return; |
1192 return; |
1194 } |
1193 } |
1195 |
1194 |
1196 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1195 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1197 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1196 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1198 EC_WRITE_U16(data + 3, entry->sdo->index); |
1197 EC_WRITE_U16(data + 3, request->index); |
1199 EC_WRITE_U8 (data + 5, entry->subindex); |
1198 EC_WRITE_U8 (data + 5, request->subindex); |
1200 memset(data + 6, 0x00, 4); |
1199 memset(data + 6, 0x00, 4); |
1201 |
1200 |
1202 if (master->debug_level) { |
1201 if (master->debug_level) { |
1203 EC_DBG("Upload request:\n"); |
1202 EC_DBG("Upload request:\n"); |
1204 ec_print_data(data, 10); |
1203 ec_print_data(data, 10); |
1309 ec_slave_t *slave = fsm->slave; |
1308 ec_slave_t *slave = fsm->slave; |
1310 ec_master_t *master = slave->master; |
1309 ec_master_t *master = slave->master; |
1311 uint8_t *data, mbox_prot; |
1310 uint8_t *data, mbox_prot; |
1312 size_t rec_size, data_size; |
1311 size_t rec_size, data_size; |
1313 ec_sdo_request_t *request = fsm->request; |
1312 ec_sdo_request_t *request = fsm->request; |
1314 ec_sdo_entry_t *entry = request->entry; |
|
1315 uint32_t complete_size; |
1313 uint32_t complete_size; |
1316 unsigned int expedited, size_specified; |
1314 unsigned int expedited, size_specified; |
1317 |
1315 |
1318 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1316 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1319 return; // FIXME: request again? |
1317 return; // FIXME: request again? |
1359 } |
1357 } |
1360 |
1358 |
1361 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1359 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1362 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1360 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1363 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1361 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1364 entry->sdo->index, entry->subindex, slave->ring_position); |
1362 request->index, request->subindex, slave->ring_position); |
1365 if (rec_size >= 10) |
1363 if (rec_size >= 10) |
1366 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1364 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1367 else |
1365 else |
1368 EC_ERR("No abort message.\n"); |
1366 EC_ERR("No abort message.\n"); |
1369 fsm->state = ec_fsm_coe_error; |
1367 fsm->state = ec_fsm_coe_error; |
1388 return; |
1386 return; |
1389 } |
1387 } |
1390 |
1388 |
1391 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1389 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1392 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1390 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1393 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1391 EC_READ_U16(data + 3) != request->index || // index |
1394 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1392 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1395 EC_ERR("Sdo upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1393 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
|
1394 request->index, request->subindex); |
1396 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1395 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1397 slave->ring_position); |
1396 slave->ring_position); |
1398 ec_print_data(data, rec_size); |
1397 ec_print_data(data, rec_size); |
1399 fsm->state = ec_fsm_coe_error; |
1398 fsm->state = ec_fsm_coe_error; |
1400 return; |
1399 return; |
1436 return; |
1435 return; |
1437 } |
1436 } |
1438 |
1437 |
1439 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1438 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1440 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1439 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1441 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1440 EC_READ_U16(data + 3) != request->index || // index |
1442 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1441 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1443 EC_ERR("Sdo upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1442 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
|
1443 request->index, request->subindex); |
1444 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1444 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1445 slave->ring_position); |
1445 slave->ring_position); |
1446 ec_print_data(data, rec_size); |
1446 ec_print_data(data, rec_size); |
1447 fsm->state = ec_fsm_coe_error; |
1447 fsm->state = ec_fsm_coe_error; |
1448 return; |
1448 return; |
1600 ec_slave_t *slave = fsm->slave; |
1600 ec_slave_t *slave = fsm->slave; |
1601 ec_master_t *master = slave->master; |
1601 ec_master_t *master = slave->master; |
1602 uint8_t *data, mbox_prot; |
1602 uint8_t *data, mbox_prot; |
1603 size_t rec_size, data_size; |
1603 size_t rec_size, data_size; |
1604 ec_sdo_request_t *request = fsm->request; |
1604 ec_sdo_request_t *request = fsm->request; |
1605 ec_sdo_entry_t *entry = request->entry; |
|
1606 uint32_t seg_size; |
1605 uint32_t seg_size; |
1607 unsigned int last_segment; |
1606 unsigned int last_segment; |
1608 |
1607 |
1609 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1608 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1610 return; // FIXME: request again? |
1609 return; // FIXME: request again? |
1650 } |
1649 } |
1651 |
1650 |
1652 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1651 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1653 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1652 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1654 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1653 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1655 entry->sdo->index, entry->subindex, slave->ring_position); |
1654 request->index, request->subindex, slave->ring_position); |
1656 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1655 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1657 fsm->state = ec_fsm_coe_error; |
1656 fsm->state = ec_fsm_coe_error; |
1658 return; |
1657 return; |
1659 } |
1658 } |
1660 |
1659 |
1661 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1660 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1662 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1661 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1663 EC_ERR("Sdo upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1662 EC_ERR("Sdo upload 0x%04X:%X failed:\n", request->index, request->subindex); |
1664 EC_ERR("Invalid Sdo upload segment response at slave %i!\n", |
1663 EC_ERR("Invalid Sdo upload segment response at slave %i!\n", |
1665 slave->ring_position); |
1664 slave->ring_position); |
1666 ec_print_data(data, rec_size); |
1665 ec_print_data(data, rec_size); |
1667 fsm->state = ec_fsm_coe_error; |
1666 fsm->state = ec_fsm_coe_error; |
1668 return; |
1667 return; |