42 /*****************************************************************************/ |
42 /*****************************************************************************/ |
43 |
43 |
44 /** Maximum time in ms to wait for responses when reading out the dictionary. |
44 /** Maximum time in ms to wait for responses when reading out the dictionary. |
45 */ |
45 */ |
46 #define EC_FSM_COE_DICT_TIMEOUT 3000 |
46 #define EC_FSM_COE_DICT_TIMEOUT 3000 |
|
47 |
|
48 #define EC_COE_DOWN_REQ_HEADER_SIZE 10 |
|
49 #define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3 |
|
50 #define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7 |
47 |
51 |
48 /*****************************************************************************/ |
52 /*****************************************************************************/ |
49 |
53 |
50 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
54 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
51 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
55 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
1061 |
1065 |
1062 /****************************************************************************** |
1066 /****************************************************************************** |
1063 * CoE state machine |
1067 * CoE state machine |
1064 *****************************************************************************/ |
1068 *****************************************************************************/ |
1065 |
1069 |
1066 /** |
1070 /** CoE state: DOWN START. |
1067 CoE state: DOWN START. |
1071 */ |
1068 */ |
1072 void ec_fsm_coe_down_start( |
1069 |
1073 ec_fsm_coe_t *fsm /**< finite state machine */ |
1070 void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */) |
1074 ) |
1071 { |
1075 { |
1072 ec_datagram_t *datagram = fsm->datagram; |
1076 ec_datagram_t *datagram = fsm->datagram; |
1073 ec_slave_t *slave = fsm->slave; |
1077 ec_slave_t *slave = fsm->slave; |
1074 ec_sdo_request_t *request = fsm->request; |
1078 ec_sdo_request_t *request = fsm->request; |
1075 uint8_t *data; |
1079 uint8_t *data; |
1091 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1095 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1092 fsm->state = ec_fsm_coe_error; |
1096 fsm->state = ec_fsm_coe_error; |
1093 return; |
1097 return; |
1094 } |
1098 } |
1095 |
1099 |
1096 if (slave->configured_rx_mailbox_size < 16) { |
1100 if (slave->configured_rx_mailbox_size < |
|
1101 EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { |
1097 EC_ERR("Mailbox too small!\n"); |
1102 EC_ERR("Mailbox too small!\n"); |
1098 fsm->state = ec_fsm_coe_error; |
1103 fsm->state = ec_fsm_coe_error; |
1099 return; |
1104 return; |
1100 } |
1105 } |
1101 |
1106 |
1102 if (request->data_size <= 4) { // use expedited transfer type |
1107 if (request->data_size <= 4) { // use expedited transfer type |
1103 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); |
1108 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
|
1109 EC_COE_DOWN_REQ_HEADER_SIZE); |
1104 if (IS_ERR(data)) { |
1110 if (IS_ERR(data)) { |
1105 fsm->state = ec_fsm_coe_error; |
1111 fsm->state = ec_fsm_coe_error; |
1106 return; |
1112 return; |
1107 } |
1113 } |
1108 |
1114 |
1121 memcpy(data + 6, request->data, request->data_size); |
1127 memcpy(data + 6, request->data, request->data_size); |
1122 memset(data + 6 + request->data_size, 0x00, 4 - request->data_size); |
1128 memset(data + 6 + request->data_size, 0x00, 4 - request->data_size); |
1123 |
1129 |
1124 if (slave->master->debug_level) { |
1130 if (slave->master->debug_level) { |
1125 EC_DBG("Expedited download request:\n"); |
1131 EC_DBG("Expedited download request:\n"); |
1126 ec_print_data(data, 10); |
1132 ec_print_data(data, EC_COE_DOWN_REQ_HEADER_SIZE); |
1127 } |
1133 } |
1128 } |
1134 } |
1129 else { // request->data_size > 4, use normal transfer type |
1135 else { // request->data_size > 4, use normal transfer type |
1130 size_t data_size; |
1136 size_t data_size, |
1131 |
1137 max_data_size = |
1132 if (slave->configured_rx_mailbox_size < |
1138 slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE, |
1133 6 + 10 + request->data_size) { |
1139 required_data_size = |
|
1140 EC_COE_DOWN_REQ_HEADER_SIZE + request->data_size; |
|
1141 |
|
1142 if (max_data_size < required_data_size) { |
1134 // segmenting needed |
1143 // segmenting needed |
1135 data_size = slave->configured_rx_mailbox_size - 6; |
1144 data_size = max_data_size; |
1136 } else { |
1145 } else { |
1137 data_size = 10 + request->data_size; |
1146 data_size = required_data_size; |
1138 } |
1147 } |
1139 |
1148 |
1140 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1149 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1141 data_size); |
1150 data_size); |
1142 if (IS_ERR(data)) { |
1151 if (IS_ERR(data)) { |
1146 |
1155 |
1147 fsm->offset = 0; |
1156 fsm->offset = 0; |
1148 fsm->remaining = request->data_size; |
1157 fsm->remaining = request->data_size; |
1149 |
1158 |
1150 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1159 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1151 EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal |
1160 EC_WRITE_U8(data + 2, |
1152 | ((request->complete_access ? 1 : 0) << 4) |
1161 0x1 // size indicator, normal |
1153 | 0x1 << 5)); // Download request |
1162 | ((request->complete_access ? 1 : 0) << 4) |
|
1163 | 0x1 << 5); // Download request |
1154 EC_WRITE_U16(data + 3, request->index); |
1164 EC_WRITE_U16(data + 3, request->index); |
1155 EC_WRITE_U8 (data + 5, |
1165 EC_WRITE_U8 (data + 5, |
1156 request->complete_access ? 0x00 : request->subindex); |
1166 request->complete_access ? 0x00 : request->subindex); |
1157 EC_WRITE_U32(data + 6, request->data_size); |
1167 EC_WRITE_U32(data + 6, request->data_size); |
1158 |
1168 |
1159 if (data_size > 10) { |
1169 if (data_size > EC_COE_DOWN_REQ_HEADER_SIZE) { |
1160 size_t segment_size = data_size - 10; |
1170 size_t segment_size = data_size - EC_COE_DOWN_REQ_HEADER_SIZE; |
|
1171 memcpy(data + EC_COE_DOWN_REQ_HEADER_SIZE, |
|
1172 request->data, segment_size); |
|
1173 fsm->offset += segment_size; |
1161 fsm->remaining -= segment_size; |
1174 fsm->remaining -= segment_size; |
1162 memcpy(data + 10, request->data, segment_size); |
|
1163 fsm->offset += segment_size; |
|
1164 } |
1175 } |
1165 |
1176 |
1166 if (slave->master->debug_level) { |
1177 if (slave->master->debug_level) { |
1167 EC_DBG("Normal download request:\n"); |
1178 EC_DBG("Normal download request:\n"); |
1168 ec_print_data(data, data_size); |
1179 ec_print_data(data, data_size); |
1276 fsm->state = ec_fsm_coe_down_response; |
1287 fsm->state = ec_fsm_coe_down_response; |
1277 } |
1288 } |
1278 |
1289 |
1279 /*****************************************************************************/ |
1290 /*****************************************************************************/ |
1280 |
1291 |
|
1292 void ec_fsm_coe_down_prepare_segment_request( |
|
1293 ec_fsm_coe_t *fsm /**< finite state machine */ |
|
1294 ) |
|
1295 { |
|
1296 ec_datagram_t *datagram = fsm->datagram; |
|
1297 ec_slave_t *slave = fsm->slave; |
|
1298 ec_sdo_request_t *request = fsm->request; |
|
1299 size_t max_segment_size = |
|
1300 slave->configured_rx_mailbox_size |
|
1301 - EC_MBOX_HEADER_SIZE |
|
1302 - EC_COE_DOWN_SEG_REQ_HEADER_SIZE; |
|
1303 size_t segment_size, data_size; |
|
1304 uint8_t last_segment, seg_data_size, *data; |
|
1305 |
|
1306 if (fsm->remaining > max_segment_size) { |
|
1307 segment_size = max_segment_size; |
|
1308 last_segment = 0; |
|
1309 } else { |
|
1310 segment_size = fsm->remaining; |
|
1311 last_segment = 1; |
|
1312 } |
|
1313 |
|
1314 if (segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) { |
|
1315 seg_data_size = 0x00; |
|
1316 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size; |
|
1317 } else { |
|
1318 seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size; |
|
1319 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE |
|
1320 + EC_COE_DOWN_SEG_MIN_DATA_SIZE; |
|
1321 } |
|
1322 |
|
1323 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
|
1324 data_size); |
|
1325 if (IS_ERR(data)) { |
|
1326 fsm->state = ec_fsm_coe_error; |
|
1327 return; |
|
1328 } |
|
1329 |
|
1330 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
1331 EC_WRITE_U8(data + 2, (last_segment ? 1 : 0) |
|
1332 | (seg_data_size << 1) |
|
1333 | (fsm->toggle << 4) |
|
1334 | (0x00 << 5)); // Download segment request |
|
1335 memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE, |
|
1336 request->data + fsm->offset, segment_size); |
|
1337 if (segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) { |
|
1338 memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size, 0x00, |
|
1339 EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size); |
|
1340 } |
|
1341 |
|
1342 fsm->offset += segment_size; |
|
1343 fsm->remaining -= segment_size; |
|
1344 |
|
1345 if (slave->master->debug_level) { |
|
1346 EC_DBG("Download segment request:\n"); |
|
1347 ec_print_data(data, data_size); |
|
1348 } |
|
1349 |
|
1350 fsm->state = ec_fsm_coe_down_seg_check; |
|
1351 } |
|
1352 |
|
1353 /*****************************************************************************/ |
|
1354 |
1281 /** |
1355 /** |
1282 CoE state: DOWN RESPONSE. |
1356 CoE state: DOWN RESPONSE. |
1283 \todo Timeout behavior |
1357 \todo Timeout behavior |
1284 */ |
1358 */ |
1285 |
1359 |
1379 fsm->state = ec_fsm_coe_down_check; |
1453 fsm->state = ec_fsm_coe_down_check; |
1380 return; |
1454 return; |
1381 } |
1455 } |
1382 |
1456 |
1383 if (fsm->remaining) { // more segments to download |
1457 if (fsm->remaining) { // more segments to download |
1384 size_t max_segment_size = slave->configured_rx_mailbox_size - 9; |
|
1385 size_t segment_size, data_size; |
|
1386 uint8_t last_segment, seg_data_size; |
|
1387 |
|
1388 if (fsm->remaining > max_segment_size) { |
|
1389 segment_size = max_segment_size; |
|
1390 last_segment = 0; |
|
1391 } else { |
|
1392 segment_size = fsm->remaining; |
|
1393 last_segment = 1; |
|
1394 } |
|
1395 |
|
1396 if (segment_size > 7) { |
|
1397 seg_data_size = 0x00; |
|
1398 data_size = 3 + segment_size; |
|
1399 } else { |
|
1400 seg_data_size = 7 - segment_size; |
|
1401 data_size = 10; |
|
1402 } |
|
1403 |
|
1404 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
|
1405 data_size); |
|
1406 if (IS_ERR(data)) { |
|
1407 fsm->state = ec_fsm_coe_error; |
|
1408 return; |
|
1409 } |
|
1410 |
|
1411 fsm->toggle = 0; |
1458 fsm->toggle = 0; |
1412 |
1459 ec_fsm_coe_down_prepare_segment_request(fsm); |
1413 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
1414 EC_WRITE_U8(data + 2, (last_segment ? 1 : 0) |
|
1415 | (seg_data_size << 1) |
|
1416 | (fsm->toggle << 4) |
|
1417 | (0x00 << 5)); // Download segment request |
|
1418 memcpy(data + 3, request->data + fsm->offset, segment_size); |
|
1419 if (segment_size < 7) { |
|
1420 memset(data + 3 + segment_size, 0x00, 7 - segment_size); |
|
1421 } |
|
1422 |
|
1423 fsm->offset += segment_size; |
|
1424 fsm->remaining -= segment_size; |
|
1425 |
|
1426 if (slave->master->debug_level) { |
|
1427 EC_DBG("Download segment request:\n"); |
|
1428 ec_print_data(data, data_size); |
|
1429 } |
|
1430 |
|
1431 fsm->state = ec_fsm_coe_down_seg_check; // success |
|
1432 } else { |
1460 } else { |
1433 fsm->state = ec_fsm_coe_end; // success |
1461 fsm->state = ec_fsm_coe_end; // success |
1434 } |
1462 } |
1435 } |
1463 } |
1436 |
1464 |
1595 fsm->state = ec_fsm_coe_error; |
1623 fsm->state = ec_fsm_coe_error; |
1596 return; |
1624 return; |
1597 } |
1625 } |
1598 |
1626 |
1599 if (fsm->remaining) { // more segments to download |
1627 if (fsm->remaining) { // more segments to download |
1600 size_t max_segment_size = slave->configured_rx_mailbox_size - 9; |
|
1601 size_t segment_size, data_size; |
|
1602 uint8_t last_segment, seg_data_size; |
|
1603 |
|
1604 if (fsm->remaining > max_segment_size) { |
|
1605 segment_size = max_segment_size; |
|
1606 last_segment = 0; |
|
1607 } else { |
|
1608 segment_size = fsm->remaining; |
|
1609 last_segment = 1; |
|
1610 } |
|
1611 |
|
1612 if (segment_size > 7) { |
|
1613 seg_data_size = 0x00; |
|
1614 data_size = 3 + segment_size; |
|
1615 } else { |
|
1616 seg_data_size = 7 - segment_size; |
|
1617 data_size = 10; |
|
1618 } |
|
1619 |
|
1620 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
|
1621 data_size); |
|
1622 if (IS_ERR(data)) { |
|
1623 fsm->state = ec_fsm_coe_error; |
|
1624 return; |
|
1625 } |
|
1626 |
|
1627 fsm->toggle = !fsm->toggle; |
1628 fsm->toggle = !fsm->toggle; |
1628 |
1629 ec_fsm_coe_down_prepare_segment_request(fsm); |
1629 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
1630 EC_WRITE_U8(data + 2, (last_segment ? 1 : 0) |
|
1631 | (seg_data_size << 1) |
|
1632 | (fsm->toggle << 4) |
|
1633 | (0x00 << 5)); // Download segment request |
|
1634 memcpy(data + 3, request->data + fsm->offset, segment_size); |
|
1635 if (segment_size < 7) { |
|
1636 memset(data + 3 + segment_size, 0x00, 7 - segment_size); |
|
1637 } |
|
1638 |
|
1639 fsm->offset += segment_size; |
|
1640 fsm->remaining -= segment_size; |
|
1641 |
|
1642 if (slave->master->debug_level) { |
|
1643 EC_DBG("Download segment request:\n"); |
|
1644 ec_print_data(data, data_size); |
|
1645 } |
|
1646 |
|
1647 fsm->state = ec_fsm_coe_down_seg_check; // success |
|
1648 } else { |
1630 } else { |
1649 fsm->state = ec_fsm_coe_end; // success |
1631 fsm->state = ec_fsm_coe_end; // success |
1650 } |
1632 } |
1651 } |
1633 } |
1652 |
1634 |