Newer
Older
EC_WRITE_U32(data + 6, sdodata->size);
memcpy(data + 10, sdodata->data, sdodata->size);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_request;
}
/*****************************************************************************/
/**
CoE state: DOWN REQUEST.
*/
void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE download request datagram for"
" slave %i (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
EC_ERR("Reception of CoE download request failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_check;
}
/*****************************************************************************/
/**
CoE state: DOWN CHECK.
*/
void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive CoE mailbox check datagram for slave %i"
" (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (datagram->cycles_received
- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Timeout while checking SDO configuration on slave %i.\n",
slave->ring_position);
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_response;
}
/*****************************************************************************/
/**
CoE state: DOWN RESPONSE.
*/
void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_data_t *sdodata = fsm->sdodata;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE download response datagram from"
" slave %i (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
EC_ERR("Reception of CoE download response failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram,
&mbox_prot, &rec_size))) {
fsm->state = ec_fsm_coe_error;
return;
}
if (mbox_prot != 0x03) { // CoE
EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
fsm->state = ec_fsm_coe_error;
return;
}
if (rec_size < 6) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Received data is too small (%i bytes):\n", rec_size);
ec_print_data(data, rec_size);
return;
}
if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
fsm->state = ec_fsm_coe_error;
EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",
sdodata->index, sdodata->subindex, sdodata->size,
slave->ring_position);
if (rec_size < 10) {
EC_ERR("Incomplete Abort command:\n");
ec_print_data(data, rec_size);
}
else
ec_canopen_abort_msg(EC_READ_U32(data + 6));
return;
}
if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
EC_READ_U16(data + 3) != sdodata->index || // index
EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex
fsm->state = ec_fsm_coe_error;
EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n",
sdodata->index, sdodata->subindex, sdodata->size);
EC_ERR("Invalid SDO download response at slave %i!\n",
slave->ring_position);
ec_print_data(data, rec_size);
return;
}
fsm->state = ec_fsm_coe_end; // success
}
/*****************************************************************************/
/**
CoE state: UP START.
*/
void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_sdo_request_t *request = fsm->request;
ec_sdo_t *sdo = request->sdo;
ec_sdo_entry_t *entry = request->entry;
uint8_t *data;
if (master->debug_level)
EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n",
sdo->index, entry->subindex, slave->ring_position);
if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {
fsm->state = ec_fsm_coe_error;
return;
}
EC_WRITE_U16(data, 0x2 << 12); // SDO request
EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
EC_WRITE_U16(data + 3, sdo->index);
EC_WRITE_U8 (data + 5, entry->subindex);
memset(data + 6, 0x00, 4);
if (master->debug_level) {
EC_DBG("Upload request:\n");
ec_print_data(data, 10);
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_request;
}
/*****************************************************************************/
/**
CoE state: UP REQUEST.
*/
void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive CoE upload request for slave %i"
" (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload request failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_check;
}
/*****************************************************************************/
/**
CoE state: UP CHECK.
*/
void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive CoE mailbox check datagram from slave %i"
" (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (datagram->cycles_received
- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Timeout while checking SDO upload on slave %i.\n",
slave->ring_position);
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_response;
}
/*****************************************************************************/
/**
CoE state: UP RESPONSE.
*/
void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
ec_sdo_t *sdo = request->sdo;
ec_sdo_entry_t *entry = request->entry;
uint32_t complete_size;
unsigned int expedited, size_specified;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive CoE upload response datagram for"
" slave %i (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload response failed - slave %i did not"
" respond.\n", slave->ring_position);
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram,
&mbox_prot, &rec_size))) {
fsm->state = ec_fsm_coe_error;
return;
}
if (master->debug_level) {
EC_DBG("Upload response:\n");
ec_print_data(data, rec_size);
}
if (mbox_prot != 0x03) { // CoE
EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
fsm->state = ec_fsm_coe_error;
return;
}
if (rec_size < 10) {
EC_ERR("Received currupted SDO upload response!\n");
ec_print_data(data, rec_size);
fsm->state = ec_fsm_coe_error;
return;
}
if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
sdo->index, entry->subindex, slave->ring_position);
ec_canopen_abort_msg(EC_READ_U32(data + 6));
fsm->state = ec_fsm_coe_error;
return;
}
if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
EC_READ_U16(data + 3) != sdo->index || // index
EC_READ_U8 (data + 5) != entry->subindex) { // subindex
EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex);
EC_ERR("Invalid SDO upload response at slave %i!\n",
slave->ring_position);
ec_print_data(data, rec_size);
fsm->state = ec_fsm_coe_error;
return;
}
data_size = rec_size - 10;
expedited = EC_READ_U8(data + 2) & 0x02;
if (expedited) {
size_specified = EC_READ_U8(data + 2) & 0x01;
if (size_specified) {
complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
}
else {
complete_size = 4;
}
}
else {
complete_size = EC_READ_U32(data + 6);
}
if (request->data) {
kfree(request->data);
request->data = NULL;
}
request->size = 0;
if (complete_size) {
if (!(request->data = (uint8_t *)
kmalloc(complete_size + 1, GFP_ATOMIC))) {
EC_ERR("Failed to allocate %i bytes of SDO data!\n",
complete_size);
fsm->state = ec_fsm_coe_error;
return;
}
request->data[complete_size] = 0x00; // just to be sure...
}
if (expedited) {
memcpy(request->data, data + 6, complete_size);
request->size = complete_size;
}
else {
memcpy(request->data, data + 10, data_size);
request->size = data_size;
fsm->toggle = 0;
if (data_size < complete_size) {
EC_WARN("SDO data incomplete (%i / %i).\n",
data_size, complete_size);
if (!(data = ec_slave_mbox_prepare_send(slave, datagram,
0x03, 3))) {
fsm->state = ec_fsm_coe_error;
return;
}
EC_WRITE_U16(data, 0x2 << 12); // SDO request
EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
| 0x3 << 5)); // upload segment request
if (master->debug_level) {
EC_DBG("Upload segment request:\n");
ec_print_data(data, 3);
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
}
}
fsm->state = ec_fsm_coe_end; // success
}
/*****************************************************************************/
/**
CoE state: UP REQUEST.
*/
void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE upload segment request datagram for"
" slave %i (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
EC_ERR("Reception of CoE upload segment request failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_check;
}
/*****************************************************************************/
/**
CoE state: UP CHECK.
*/
void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive CoE mailbox check datagram for slave %i"
" (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (datagram->cycles_received
- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Timeout while checking SDO upload segment on slave %i.\n",
slave->ring_position);
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_response;
}
/*****************************************************************************/
/**
CoE state: UP RESPONSE.
*/
void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
ec_sdo_t *sdo = request->sdo;
ec_sdo_entry_t *entry = request->entry;
uint32_t seg_size;
unsigned int last_segment;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE upload segment response datagram for"
" slave %i (datagram state %i).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
EC_ERR("Reception of CoE upload segment response failed - slave %i"
" did not respond.\n", slave->ring_position);
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram,
&mbox_prot, &rec_size))) {
fsm->state = ec_fsm_coe_error;
return;
}
if (master->debug_level) {
EC_DBG("Upload segment response:\n");
ec_print_data(data, rec_size);
}
if (mbox_prot != 0x03) { // CoE
EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
1629
1630
1631
1632
1633
1634
1635
1636
1637
1638
1639
1640
1641
1642
1643
1644
1645
1646
1647
1648
1649
1650
1651
1652
1653
1654
1655
1656
1657
1658
1659
1660
1661
1662
1663
1664
1665
1666
1667
1668
1669
1670
1671
1672
1673
1674
1675
1676
1677
1678
1679
1680
1681
1682
1683
1684
1685
1686
1687
1688
1689
fsm->state = ec_fsm_coe_error;
return;
}
if (rec_size < 10) {
EC_ERR("Received currupted SDO upload segment response!\n");
ec_print_data(data, rec_size);
fsm->state = ec_fsm_coe_error;
return;
}
if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
sdo->index, entry->subindex, slave->ring_position);
ec_canopen_abort_msg(EC_READ_U32(data + 6));
fsm->state = ec_fsm_coe_error;
return;
}
if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response
EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex);
EC_ERR("Invalid SDO upload segment response at slave %i!\n",
slave->ring_position);
ec_print_data(data, rec_size);
fsm->state = ec_fsm_coe_error;
return;
}
last_segment = EC_READ_U8(data + 2) & 0x01;
seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
data_size = rec_size - 10;
if (data_size != seg_size) {
EC_WARN("SDO segment data invalid (%i / %i)"
" - Fragmenting not implemented.\n",
data_size, seg_size);
}
memcpy(request->data + request->size, data + 10, data_size);
request->size += data_size;
if (!last_segment) {
fsm->toggle = !fsm->toggle;
if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) {
fsm->state = ec_fsm_coe_error;
return;
}
EC_WRITE_U16(data, 0x2 << 12); // SDO request
EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
| 0x3 << 5)); // upload segment request
if (master->debug_level) {
EC_DBG("Upload segment request:\n");
ec_print_data(data, 3);
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
1691
1692
1693
1694
1695
1696
1697
1698
1699
1700
1701
1702
1703
1704
1705
1706
1707
1708
1709
1710
1711
1712
1713
1714
1715
1716
1717
1718
fsm->state = ec_fsm_coe_up_seg_request;
return;
}
fsm->state = ec_fsm_coe_end; // success
}
/*****************************************************************************/
/**
State: ERROR.
*/
void ec_fsm_coe_error(ec_fsm_coe_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/
/**
State: END.
*/
void ec_fsm_coe_end(ec_fsm_coe_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/