591 } |
591 } |
592 |
592 |
593 // Check, if EoE processing has to be started |
593 // Check, if EoE processing has to be started |
594 ec_master_eoe_start(master); |
594 ec_master_eoe_start(master); |
595 |
595 |
|
596 // check, if slaves have an SDO dictionary to read out. |
|
597 list_for_each_entry(slave, &master->slaves, list) { |
|
598 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE) |
|
599 || slave->sdo_dictionary_fetched |
|
600 || slave->current_state == EC_SLAVE_STATE_INIT |
|
601 || jiffies - slave->jiffies_preop < 5 * HZ |
|
602 || !slave->online |
|
603 || slave->error_flag) continue; |
|
604 |
|
605 if (master->debug_level) { |
|
606 EC_DBG("Fetching SDO dictionary of slave %i.\n", |
|
607 slave->ring_position); |
|
608 } |
|
609 |
|
610 if (kobject_add(&slave->sdo_kobj)) { |
|
611 EC_ERR("Failed to add SDO kobj of slave %i.\n", |
|
612 slave->ring_position); |
|
613 slave->error_flag = 1; |
|
614 fsm->master_state = ec_fsm_master_start; |
|
615 fsm->master_state(fsm); // execute immediately |
|
616 return; |
|
617 } |
|
618 |
|
619 slave->sdo_dictionary_fetched = 1; |
|
620 |
|
621 // start fetching SDO dictionary |
|
622 fsm->slave = slave; |
|
623 fsm->master_state = ec_fsm_master_sdodict; |
|
624 fsm->coe_state = ec_fsm_coe_dict_start; |
|
625 fsm->coe_state(fsm); // execute immediately |
|
626 return; |
|
627 } |
|
628 |
596 if (master->mode == EC_MASTER_MODE_IDLE) { |
629 if (master->mode == EC_MASTER_MODE_IDLE) { |
597 // nothing to configure. check for pending EEPROM write operations. |
630 // nothing to configure. check for pending EEPROM write operations. |
598 list_for_each_entry(slave, &master->slaves, list) { |
631 list_for_each_entry(slave, &master->slaves, list) { |
599 if (!slave->new_eeprom_data) continue; |
632 if (!slave->new_eeprom_data) continue; |
600 |
633 |
1432 } |
1497 } |
1433 |
1498 |
1434 if (fsm->change_state != ec_fsm_end) return; |
1499 if (fsm->change_state != ec_fsm_end) return; |
1435 |
1500 |
1436 // slave is now in PREOP |
1501 // slave is now in PREOP |
|
1502 slave->jiffies_preop = fsm->datagram.jiffies_received; |
1437 |
1503 |
1438 if (master->debug_level) { |
1504 if (master->debug_level) { |
1439 EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position); |
1505 EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position); |
1440 } |
1506 } |
1441 |
1507 |
1442 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
1508 if (slave->current_state == slave->requested_state) { |
1443 // slave does not support CoE. skip dictionary fetching |
1509 fsm->slave_state = ec_fsm_end; // successful |
1444 // and SDO configuration |
1510 if (master->debug_level) { |
1445 |
1511 EC_DBG("Finished configuration of slave %i.\n", |
1446 if (slave->current_state == slave->requested_state) { |
1512 slave->ring_position); |
1447 fsm->slave_state = ec_fsm_end; // successful |
1513 } |
1448 if (master->debug_level) { |
1514 return; |
1449 EC_DBG("Finished configuration of slave %i.\n", |
1515 } |
1450 slave->ring_position); |
1516 |
1451 } |
1517 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1452 return; |
1518 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
1453 } |
|
1454 |
|
1455 if (!slave->base_fmmu_count) { |
|
1456 // slave has no FMMUs. skip configuration and go to SAVEOP |
|
1457 fsm->slave_state = ec_fsm_slaveconf_saveop; |
1519 fsm->slave_state = ec_fsm_slaveconf_saveop; |
1458 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
1520 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
1459 fsm->change_state = ec_fsm_change_start; |
1521 fsm->change_state = ec_fsm_change_start; |
1460 fsm->change_state(fsm); // execute immediately |
1522 fsm->change_state(fsm); // execute immediately |
1461 return; |
1523 return; |
1462 } |
1524 } |
1463 |
1525 |
1464 // configure FMMUs |
|
1465 ec_datagram_npwr(datagram, slave->station_address, |
|
1466 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1467 memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1468 for (j = 0; j < slave->fmmu_count; j++) { |
|
1469 ec_fmmu_config(&slave->fmmus[j], slave, |
|
1470 datagram->data + EC_FMMU_SIZE * j); |
|
1471 } |
|
1472 |
|
1473 ec_master_queue_datagram(master, datagram); |
|
1474 fsm->slave_state = ec_fsm_slaveconf_fmmu; |
|
1475 return; |
|
1476 } |
|
1477 |
|
1478 if (!list_empty(&slave->sdo_dictionary)) { |
|
1479 // SDO dictionary already fetched |
|
1480 |
|
1481 if (master->debug_level) { |
|
1482 EC_DBG("SDO dictionary already present for slave %i.\n", |
|
1483 slave->ring_position); |
|
1484 } |
|
1485 |
|
1486 if (slave->current_state == slave->requested_state) { |
|
1487 fsm->slave_state = ec_fsm_end; // successful |
|
1488 if (master->debug_level) { |
|
1489 EC_DBG("Finished configuration of slave %i.\n", |
|
1490 slave->ring_position); |
|
1491 } |
|
1492 return; |
|
1493 } |
|
1494 |
|
1495 if (list_empty(&slave->sdo_confs)) { |
|
1496 // skip SDO configuration |
|
1497 |
|
1498 if (!slave->base_fmmu_count) { |
|
1499 // slave has no FMMUs. skip configuration and go to SAVEOP |
|
1500 fsm->slave_state = ec_fsm_slaveconf_saveop; |
|
1501 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
|
1502 fsm->change_state = ec_fsm_change_start; |
|
1503 fsm->change_state(fsm); // execute immediately |
|
1504 return; |
|
1505 } |
|
1506 |
|
1507 // configure FMMUs |
|
1508 ec_datagram_npwr(datagram, slave->station_address, |
|
1509 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1510 memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1511 for (j = 0; j < slave->fmmu_count; j++) { |
|
1512 ec_fmmu_config(&slave->fmmus[j], slave, |
|
1513 datagram->data + EC_FMMU_SIZE * j); |
|
1514 } |
|
1515 |
|
1516 ec_master_queue_datagram(master, datagram); |
|
1517 fsm->slave_state = ec_fsm_slaveconf_fmmu; |
|
1518 return; |
|
1519 } |
|
1520 |
|
1521 // start SDO configuration |
1526 // start SDO configuration |
1522 fsm->slave_state = ec_fsm_slaveconf_sdoconf; |
1527 fsm->slave_state = ec_fsm_slaveconf_sdoconf; |
1523 fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); |
1528 fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); |
1524 fsm->coe_state = ec_fsm_coe_down_start; |
1529 fsm->coe_state = ec_fsm_coe_down_start; |
1525 fsm->coe_state(fsm); // execute immediately |
1530 fsm->coe_state(fsm); // execute immediately |
1526 return; |
|
1527 } |
|
1528 |
|
1529 if (master->debug_level) { |
|
1530 EC_DBG("Fetching SDO dictionary of slave %i.\n", |
|
1531 slave->ring_position); |
|
1532 } |
|
1533 |
|
1534 // start fetching SDO dictionary |
|
1535 fsm->slave_state = ec_fsm_slaveconf_sdodict; |
|
1536 fsm->coe_state = ec_fsm_coe_dict_start; |
|
1537 fsm->coe_state(fsm); // execute immediately |
|
1538 } |
|
1539 |
|
1540 /*****************************************************************************/ |
|
1541 |
|
1542 /** |
|
1543 Slave configuration state: SDODICT. |
|
1544 */ |
|
1545 |
|
1546 void ec_fsm_slaveconf_sdodict(ec_fsm_t *fsm /**< finite state machine */) |
|
1547 { |
|
1548 ec_slave_t *slave = fsm->slave; |
|
1549 ec_datagram_t *datagram = &fsm->datagram; |
|
1550 ec_master_t *master = fsm->master; |
|
1551 unsigned int j; |
|
1552 |
|
1553 fsm->coe_state(fsm); // execute CoE state machine |
|
1554 |
|
1555 if (fsm->coe_state == ec_fsm_error) { |
|
1556 fsm->slave->error_flag = 1; |
|
1557 fsm->slave_state = ec_fsm_error; |
|
1558 return; |
|
1559 } |
|
1560 |
|
1561 if (fsm->coe_state != ec_fsm_end) return; |
|
1562 |
|
1563 // SDO dictionary fetching finished |
|
1564 |
|
1565 if (master->debug_level) { |
|
1566 EC_DBG("Finished fetching SDO dictionary of slave %i.\n", |
|
1567 slave->ring_position); |
|
1568 } |
|
1569 |
|
1570 if (slave->current_state == slave->requested_state) { |
|
1571 fsm->slave_state = ec_fsm_end; // successful |
|
1572 if (master->debug_level) { |
|
1573 EC_DBG("Finished configuration of slave %i.\n", |
|
1574 slave->ring_position); |
|
1575 } |
|
1576 return; |
|
1577 } |
|
1578 |
|
1579 if (list_empty(&slave->sdo_confs)) { |
|
1580 // skip SDO configuration |
|
1581 |
|
1582 if (!slave->base_fmmu_count) { |
|
1583 // slave has no FMMUs. skip configuration. |
|
1584 |
|
1585 // set state to SAVEOP |
|
1586 fsm->slave_state = ec_fsm_slaveconf_saveop; |
|
1587 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
|
1588 fsm->change_state = ec_fsm_change_start; |
|
1589 fsm->change_state(fsm); // execute immediately |
|
1590 return; |
|
1591 } |
|
1592 |
|
1593 // configure FMMUs |
|
1594 ec_datagram_npwr(datagram, slave->station_address, |
|
1595 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1596 memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1597 for (j = 0; j < slave->fmmu_count; j++) { |
|
1598 ec_fmmu_config(&slave->fmmus[j], slave, |
|
1599 datagram->data + EC_FMMU_SIZE * j); |
|
1600 } |
|
1601 |
|
1602 ec_master_queue_datagram(master, datagram); |
|
1603 fsm->slave_state = ec_fsm_slaveconf_fmmu; |
|
1604 return; |
|
1605 } |
|
1606 |
|
1607 // start SDO configuration |
|
1608 fsm->slave_state = ec_fsm_slaveconf_sdoconf; |
|
1609 fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); |
|
1610 fsm->coe_state = ec_fsm_coe_down_start; |
|
1611 fsm->coe_state(fsm); // execute immediately |
|
1612 } |
|
1613 |
|
1614 /*****************************************************************************/ |
|
1615 |
|
1616 /** |
|
1617 Slave configuration state: SDOCONF. |
|
1618 */ |
|
1619 |
|
1620 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
|
1621 { |
|
1622 ec_slave_t *slave = fsm->slave; |
|
1623 ec_datagram_t *datagram = &fsm->datagram; |
|
1624 ec_master_t *master = fsm->master; |
|
1625 unsigned int j; |
|
1626 |
|
1627 fsm->coe_state(fsm); // execute CoE state machine |
|
1628 |
|
1629 if (fsm->coe_state == ec_fsm_error) { |
|
1630 fsm->slave->error_flag = 1; |
|
1631 fsm->slave_state = ec_fsm_error; |
|
1632 return; |
|
1633 } |
|
1634 |
|
1635 if (fsm->coe_state != ec_fsm_end) return; |
|
1636 |
|
1637 // Another SDO to configure? |
|
1638 if (fsm->coe_sdodata->list.next != &fsm->slave->sdo_confs) { |
|
1639 fsm->coe_sdodata = list_entry(fsm->coe_sdodata->list.next, |
|
1640 ec_sdo_data_t, list); |
|
1641 fsm->coe_state = ec_fsm_coe_down_start; |
|
1642 fsm->coe_state(fsm); // execute immediately |
|
1643 return; |
|
1644 } |
|
1645 |
|
1646 // All SDOs are now configured. |
|
1647 |
|
1648 if (!slave->base_fmmu_count) { |
|
1649 // slave has no FMMUs. skip configuration. |
|
1650 |
|
1651 // set state to SAVEOP |
|
1652 fsm->slave_state = ec_fsm_slaveconf_saveop; |
|
1653 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
|
1654 fsm->change_state = ec_fsm_change_start; |
|
1655 fsm->change_state(fsm); // execute immediately |
|
1656 return; |
1531 return; |
1657 } |
1532 } |
1658 |
1533 |
1659 // configure FMMUs |
1534 // configure FMMUs |
1660 ec_datagram_npwr(datagram, slave->station_address, |
1535 ec_datagram_npwr(datagram, slave->station_address, |
1676 */ |
1551 */ |
1677 |
1552 |
1678 void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */) |
1553 void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */) |
1679 { |
1554 { |
1680 ec_datagram_t *datagram = &fsm->datagram; |
1555 ec_datagram_t *datagram = &fsm->datagram; |
|
1556 ec_slave_t *slave = fsm->slave; |
1681 |
1557 |
1682 if (datagram->state != EC_DATAGRAM_RECEIVED |
1558 if (datagram->state != EC_DATAGRAM_RECEIVED |
1683 || datagram->working_counter != 1) { |
1559 || datagram->working_counter != 1) { |
1684 fsm->slave->error_flag = 1; |
1560 fsm->slave->error_flag = 1; |
1685 fsm->slave_state = ec_fsm_error; |
1561 fsm->slave_state = ec_fsm_error; |
1686 EC_ERR("Failed to set FMMUs on slave %i.\n", |
1562 EC_ERR("Failed to set FMMUs on slave %i.\n", |
1687 fsm->slave->ring_position); |
1563 fsm->slave->ring_position); |
1688 return; |
1564 return; |
1689 } |
1565 } |
|
1566 |
|
1567 // No CoE configuration to be applied? Jump to SAVEOP state. |
|
1568 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
|
1569 // set state to SAVEOP |
|
1570 fsm->slave_state = ec_fsm_slaveconf_saveop; |
|
1571 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
|
1572 fsm->change_state = ec_fsm_change_start; |
|
1573 fsm->change_state(fsm); // execute immediately |
|
1574 return; |
|
1575 } |
|
1576 |
|
1577 // start SDO configuration |
|
1578 fsm->slave_state = ec_fsm_slaveconf_sdoconf; |
|
1579 fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); |
|
1580 fsm->coe_state = ec_fsm_coe_down_start; |
|
1581 fsm->coe_state(fsm); // execute immediately |
|
1582 } |
|
1583 |
|
1584 /*****************************************************************************/ |
|
1585 |
|
1586 /** |
|
1587 Slave configuration state: SDOCONF. |
|
1588 */ |
|
1589 |
|
1590 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
|
1591 { |
|
1592 fsm->coe_state(fsm); // execute CoE state machine |
|
1593 |
|
1594 if (fsm->coe_state == ec_fsm_error) { |
|
1595 fsm->slave->error_flag = 1; |
|
1596 fsm->slave_state = ec_fsm_error; |
|
1597 return; |
|
1598 } |
|
1599 |
|
1600 if (fsm->coe_state != ec_fsm_end) return; |
|
1601 |
|
1602 // Another SDO to configure? |
|
1603 if (fsm->coe_sdodata->list.next != &fsm->slave->sdo_confs) { |
|
1604 fsm->coe_sdodata = list_entry(fsm->coe_sdodata->list.next, |
|
1605 ec_sdo_data_t, list); |
|
1606 fsm->coe_state = ec_fsm_coe_down_start; |
|
1607 fsm->coe_state(fsm); // execute immediately |
|
1608 return; |
|
1609 } |
|
1610 |
|
1611 // All SDOs are now configured. |
1690 |
1612 |
1691 // set state to SAVEOP |
1613 // set state to SAVEOP |
1692 fsm->slave_state = ec_fsm_slaveconf_saveop; |
1614 fsm->slave_state = ec_fsm_slaveconf_saveop; |
1693 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
1615 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
1694 fsm->change_state = ec_fsm_change_start; |
1616 fsm->change_state = ec_fsm_change_start; |