1074     uint8_t size;

  1076     uint8_t data_set_size;

  1101         size = 4 - request->data_size;

  1109         fsm->remaining = 0;

  1110 

  1111         data_set_size = 4 - request->data_size;

  1105                     | size << 2

  1115                     | data_set_size << 2

  1120         if (slave->configured_rx_mailbox_size < 6 + 10 + request->data_size) {

  1130         size_t data_size;

  1121             EC_ERR("SDO fragmenting not supported yet!\n"); // FIXME

  1131 

  1122             fsm->state = ec_fsm_coe_error;

  1132         if (slave->configured_rx_mailbox_size <

  1123             return;

  1133                 6 + 10 + request->data_size) {

  1134             // segmenting needed

  1135             data_size = slave->configured_rx_mailbox_size - 6;

  1136         } else {

  1137             data_size = 10 + request->data_size;

  1127                 request->data_size + 10);

  1141                 data_size);

  1141         memcpy(data + 10, request->data, request->data_size);

  1158 

  1159         if (data_size > 10) {

  1160             size_t segment_size = data_size - 10;

  1161             fsm->remaining -= segment_size;

  1162             memcpy(data + 10, request->data, segment_size);

  1163             fsm->offset += segment_size;

  1164         }

  1145             ec_print_data(data, 10 + request->data_size);

  1168             ec_print_data(data, data_size);

  1360     fsm->state = ec_fsm_coe_end; // success

  1383     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;

  1412 

  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 {

  1433         fsm->state = ec_fsm_coe_end; // success

  1434     }

  1435 }

  1436 

  1437 /*****************************************************************************/

  1438 

  1439 /**

  1440    CoE state: DOWN SEG CHECK.

  1441 */

  1442 

  1443 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1444 {

  1445     ec_datagram_t *datagram = fsm->datagram;

  1446     ec_slave_t *slave = fsm->slave;

  1447 

  1448     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

  1449         return;

  1450 

  1451     if (datagram->state != EC_DATAGRAM_RECEIVED) {

  1452         fsm->state = ec_fsm_coe_error;

  1453         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"

  1454                 " (datagram state %u).\n",

  1455                slave->ring_position, datagram->state);

  1456         return;

  1457     }

  1458 

  1459     if (datagram->working_counter != 1) {

  1460         fsm->state = ec_fsm_coe_error;

  1461         EC_ERR("Reception of CoE mailbox segment check"

  1462                 " datagram failed on slave %u: ", slave->ring_position);

  1463         ec_datagram_print_wc_error(datagram);

  1464         return;

  1465     }

  1466 

  1467     if (!ec_slave_mbox_check(datagram)) {

  1468         unsigned long diff_ms =

  1469             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1470         if (diff_ms >= fsm->request->response_timeout) {

  1471             fsm->state = ec_fsm_coe_error;

  1472             EC_ERR("Timeout while waiting for SDO download segment response "

  1473                     "on slave %u.\n", slave->ring_position);

  1474             return;

  1475         }

  1476 

  1477         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  1478         fsm->retries = EC_FSM_RETRIES;

  1479         return;

  1480     }

  1481 

  1482     // Fetch response

  1483     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

  1484     fsm->retries = EC_FSM_RETRIES;

  1485     fsm->state = ec_fsm_coe_down_seg_response;

  1486 }

  1487 

  1488 /*****************************************************************************/

  1489 

  1490 /**

  1491    CoE state: DOWN SEG RESPONSE.

  1492    \todo Timeout behavior

  1493 */

  1494 

  1495 void ec_fsm_coe_down_seg_response(

  1496         ec_fsm_coe_t *fsm /**< finite state machine */

  1497         )

  1498 {

  1499     ec_datagram_t *datagram = fsm->datagram;

  1500     ec_slave_t *slave = fsm->slave;

  1501     uint8_t *data, mbox_prot;

  1502     size_t rec_size;

  1503     ec_sdo_request_t *request = fsm->request;

  1504 

  1505     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

  1506         return; // FIXME: request again?

  1507 

  1508     if (datagram->state != EC_DATAGRAM_RECEIVED) {

  1509         fsm->state = ec_fsm_coe_error;

  1510         EC_ERR("Failed to receive CoE download response datagram from"

  1511                " slave %u (datagram state %u).\n",

  1512                slave->ring_position, datagram->state);

  1513         return;

  1514     }

  1515 

  1516     if (datagram->working_counter != 1) {

  1517         fsm->state = ec_fsm_coe_error;

  1518         EC_ERR("Reception of CoE download response failed on slave %u: ",

  1519                 slave->ring_position);

  1520         ec_datagram_print_wc_error(datagram);

  1521         return;

  1522     }

  1523 

  1524     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);

  1525     if (IS_ERR(data)) {

  1526         fsm->state = ec_fsm_coe_error;

  1527         return;

  1528     }

  1529 

  1530     if (mbox_prot != 0x03) { // CoE

  1531         fsm->state = ec_fsm_coe_error;

  1532         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

  1533         return;

  1534     }

  1535 

  1536     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1537         // check for CoE response again

  1538         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  1539         fsm->retries = EC_FSM_RETRIES;

  1540         fsm->state = ec_fsm_coe_down_check;

  1541         return;

  1542     }

  1543 

  1544     if (slave->master->debug_level) {

  1545         EC_DBG("Download response:\n");

  1546         ec_print_data(data, rec_size);

  1547     }

  1548 

  1549     if (rec_size < 6) {

  1550         fsm->state = ec_fsm_coe_error;

  1551         EC_ERR("Received data are too small (%zu bytes):\n", rec_size);

  1552         ec_print_data(data, rec_size);

  1553         return;

  1554     }

  1555 

  1556     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

  1557         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

  1558         char subidxstr[10];

  1559         fsm->state = ec_fsm_coe_error;

  1560         if (request->complete_access) {

  1561             subidxstr[0] = 0x00;

  1562         } else {

  1563             sprintf(subidxstr, ":%02X", request->subindex);

  1564         }

  1565         EC_ERR("SDO download 0x%04X%s (%zu bytes) aborted on slave %u.\n",

  1566                 request->index, subidxstr, request->data_size,

  1567                 slave->ring_position);

  1568         if (rec_size < 10) {

  1569             EC_ERR("Incomplete abort command:\n");

  1570             ec_print_data(data, rec_size);

  1571         } else {

  1572             fsm->request->abort_code = EC_READ_U32(data + 6);

  1573             ec_canopen_abort_msg(fsm->request->abort_code);

  1574         }

  1575         return;

  1576     }

  1577 

  1578     if (EC_READ_U16(data) >> 12 != 0x3 ||

  1579             ((EC_READ_U8(data + 2) >> 5) != 0x01)) { // segment response

  1580         if (slave->master->debug_level) {

  1581             EC_DBG("Invalid SDO download response at slave %u! Retrying...\n",

  1582                     slave->ring_position);

  1583             ec_print_data(data, rec_size);

  1584         }

  1585         // check for CoE response again

  1586         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  1587         fsm->retries = EC_FSM_RETRIES;

  1588         fsm->state = ec_fsm_coe_down_seg_check;

  1589         return;

  1590     }

  1591 

  1592     if (((EC_READ_U8(data + 2) >> 4) & 0x01) != fsm->toggle) {

  1593         EC_ERR("Invalid toggle received during segmented download:\n");

  1594         ec_print_data(data, rec_size);

  1595         fsm->state = ec_fsm_coe_error;

  1596         return;

  1597     }

  1598 

  1599     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 

  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 {

  1649         fsm->state = ec_fsm_coe_end; // success

  1650     }