67 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
74 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
74 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
75 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
75 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
76 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
76 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
77 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
77 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
78 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); |
78 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); |
79 void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *); |
79 void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *); |
80 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); |
80 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); |
81 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); |
81 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); |
82 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
82 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
|
83 void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *); |
83 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
84 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
84 |
85 |
85 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
86 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
86 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
87 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
87 void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *); |
88 void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *); |
88 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
89 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
89 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
90 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
90 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
91 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
91 void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *); |
92 void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *); |
92 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
93 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
93 void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *); |
94 void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *); |
94 void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *); |
95 void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *); |
95 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
96 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
96 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); |
97 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); |
97 void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *); |
98 void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *); |
98 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); |
99 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); |
|
100 void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *); |
|
101 void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *); |
99 |
102 |
100 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); |
103 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); |
101 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *); |
104 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *); |
102 |
105 |
103 void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *); |
106 void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *); |
704 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
707 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
705 return; |
708 return; |
706 } |
709 } |
707 |
710 |
708 // All SDOs are now configured. |
711 // All SDOs are now configured. |
709 ec_fsm_slave_config_enter_soe_conf(fsm); |
712 ec_fsm_slave_config_enter_soe_conf_preop(fsm); |
710 } |
713 } |
711 |
714 |
712 /*****************************************************************************/ |
715 /*****************************************************************************/ |
713 |
716 |
714 /** Check for SoE configurations to be applied. |
717 /** Check for SoE configurations to be applied. |
715 */ |
718 */ |
716 void ec_fsm_slave_config_enter_soe_conf( |
719 void ec_fsm_slave_config_enter_soe_conf_preop( |
717 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
720 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
718 ) |
721 ) |
719 { |
722 { |
720 ec_slave_t *slave = fsm->slave; |
723 ec_slave_t *slave = fsm->slave; |
721 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
724 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
|
725 ec_soe_request_t *req; |
722 |
726 |
723 if (!slave->config) { |
727 if (!slave->config) { |
724 ec_fsm_slave_config_enter_pdo_sync(fsm); |
728 ec_fsm_slave_config_enter_pdo_sync(fsm); |
725 return; |
729 return; |
726 } |
730 } |
727 |
731 |
728 // No SoE configuration to be applied? |
732 list_for_each_entry(req, &slave->config->soe_configs, list) { |
729 if (list_empty(&slave->config->soe_configs)) { // skip configuration |
733 if (req->al_state == EC_AL_STATE_PREOP) { |
730 ec_fsm_slave_config_enter_pdo_conf(fsm); |
734 // start SoE configuration |
731 return; |
735 fsm->state = ec_fsm_slave_config_state_soe_conf_preop; |
732 } |
736 fsm->soe_request = req; |
733 |
737 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
734 // start SoE configuration |
738 ec_soe_request_write(&fsm->soe_request_copy); |
735 fsm->state = ec_fsm_slave_config_state_soe_conf; |
739 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
736 fsm->soe_request = list_entry(fsm->slave->config->soe_configs.next, |
740 ec_fsm_soe_exec(fsm_soe); // execute immediately |
737 ec_soe_request_t, list); |
741 ec_master_queue_external_datagram(slave->master, |
738 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
742 fsm_soe->datagram); |
739 ec_soe_request_write(&fsm->soe_request_copy); |
743 return; |
740 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
744 } |
741 ec_fsm_soe_exec(fsm_soe); // execute immediately |
745 } |
742 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
746 |
|
747 // No SoE configuration to be applied in PREOP |
|
748 ec_fsm_slave_config_enter_pdo_conf(fsm); |
743 } |
749 } |
744 |
750 |
745 /*****************************************************************************/ |
751 /*****************************************************************************/ |
746 |
752 |
747 /** Slave configuration state: SOE_CONF. |
753 /** Slave configuration state: SOE_CONF. |
748 */ |
754 */ |
749 void ec_fsm_slave_config_state_soe_conf( |
755 void ec_fsm_slave_config_state_soe_conf_preop( |
750 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
756 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
751 ) |
757 ) |
752 { |
758 { |
753 ec_slave_t *slave = fsm->slave; |
759 ec_slave_t *slave = fsm->slave; |
754 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
760 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
768 if (!fsm->slave->config) { // config removed in the meantime |
774 if (!fsm->slave->config) { // config removed in the meantime |
769 ec_fsm_slave_config_reconfigure(fsm); |
775 ec_fsm_slave_config_reconfigure(fsm); |
770 return; |
776 return; |
771 } |
777 } |
772 |
778 |
773 // Another IDN to configure? |
779 // Another IDN to configure in PREOP? |
774 if (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { |
780 while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { |
775 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
781 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
776 ec_soe_request_t, list); |
782 ec_soe_request_t, list); |
777 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
783 if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { |
778 ec_soe_request_write(&fsm->soe_request_copy); |
784 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
779 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
785 ec_soe_request_write(&fsm->soe_request_copy); |
780 ec_fsm_soe_exec(fsm_soe); // execute immediately |
786 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
781 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
787 ec_fsm_soe_exec(fsm_soe); // execute immediately |
782 return; |
788 ec_master_queue_external_datagram(slave->master, |
783 } |
789 fsm_soe->datagram); |
784 |
790 return; |
785 // All SDOs are now configured. |
791 } |
|
792 } |
|
793 |
|
794 // All PREOP IDNs are now configured. |
786 ec_fsm_slave_config_enter_pdo_conf(fsm); |
795 ec_fsm_slave_config_enter_pdo_conf(fsm); |
787 } |
796 } |
788 |
797 |
789 /*****************************************************************************/ |
798 /*****************************************************************************/ |
790 |
799 |
1240 diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1249 diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1241 |
1250 |
1242 if (0 && abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { |
1251 if (0 && abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { |
1243 |
1252 |
1244 if (diff_ms >= EC_DC_SYNC_WAIT_MS) { |
1253 if (diff_ms >= EC_DC_SYNC_WAIT_MS) { |
1245 EC_SLAVE_WARN(slave, "Slave did not sync after %u ms.\n", |
1254 EC_SLAVE_WARN(slave, "Slave did not sync after %lu ms.\n", |
1246 (u32) diff_ms); |
1255 diff_ms); |
1247 } else { |
1256 } else { |
1248 EC_SLAVE_DBG(slave, 1, "Sync after %4u ms: %10u ns\n", |
1257 EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n", |
1249 (u32) diff_ms, abs_sync_diff); |
1258 diff_ms, abs_sync_diff); |
1250 |
1259 |
1251 // check synchrony again |
1260 // check synchrony again |
1252 ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); |
1261 ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); |
1253 fsm->retries = EC_FSM_RETRIES; |
1262 fsm->retries = EC_FSM_RETRIES; |
1254 return; |
1263 return; |
1255 } |
1264 } |
1256 } else { |
1265 } else { |
1257 EC_SLAVE_DBG(slave, 1, "%u ns difference after %u ms.\n", |
1266 EC_SLAVE_DBG(slave, 1, "%u ns difference after %lu ms.\n", |
1258 abs_sync_diff, (u32) diff_ms); |
1267 abs_sync_diff, diff_ms); |
1259 } |
1268 } |
1260 |
1269 |
1261 // set DC start time |
1270 // set DC start time |
1262 start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns |
1271 start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns |
1263 // FIXME use slave's local system time here? |
1272 // FIXME use slave's local system time here? |
1414 fsm->state = ec_fsm_slave_config_state_end; // successful |
1423 fsm->state = ec_fsm_slave_config_state_end; // successful |
1415 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
1424 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
1416 return; |
1425 return; |
1417 } |
1426 } |
1418 |
1427 |
|
1428 ec_fsm_slave_config_enter_soe_conf_safeop(fsm); |
|
1429 } |
|
1430 |
|
1431 /*****************************************************************************/ |
|
1432 |
|
1433 /** Check for SoE configurations to be applied in SAFEOP. |
|
1434 */ |
|
1435 void ec_fsm_slave_config_enter_soe_conf_safeop( |
|
1436 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
1437 ) |
|
1438 { |
|
1439 ec_slave_t *slave = fsm->slave; |
|
1440 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
|
1441 ec_soe_request_t *req; |
|
1442 |
|
1443 if (!slave->config) { |
|
1444 ec_fsm_slave_config_enter_op(fsm); |
|
1445 return; |
|
1446 } |
|
1447 |
|
1448 list_for_each_entry(req, &slave->config->soe_configs, list) { |
|
1449 if (req->al_state == EC_AL_STATE_SAFEOP) { |
|
1450 // start SoE configuration |
|
1451 fsm->state = ec_fsm_slave_config_state_soe_conf_safeop; |
|
1452 fsm->soe_request = req; |
|
1453 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
|
1454 ec_soe_request_write(&fsm->soe_request_copy); |
|
1455 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
|
1456 ec_fsm_soe_exec(fsm_soe); // execute immediately |
|
1457 ec_master_queue_external_datagram(slave->master, |
|
1458 fsm_soe->datagram); |
|
1459 return; |
|
1460 } |
|
1461 } |
|
1462 |
|
1463 // No SoE configuration to be applied in SAFEOP |
|
1464 ec_fsm_slave_config_enter_op(fsm); |
|
1465 } |
|
1466 |
|
1467 /*****************************************************************************/ |
|
1468 |
|
1469 /** Slave configuration state: SOE_CONF. |
|
1470 */ |
|
1471 void ec_fsm_slave_config_state_soe_conf_safeop( |
|
1472 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
1473 ) |
|
1474 { |
|
1475 ec_slave_t *slave = fsm->slave; |
|
1476 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
|
1477 |
|
1478 if (ec_fsm_soe_exec(fsm_soe)) { |
|
1479 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
|
1480 return; |
|
1481 } |
|
1482 |
|
1483 if (!ec_fsm_soe_success(fsm_soe)) { |
|
1484 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
|
1485 fsm->slave->error_flag = 1; |
|
1486 fsm->state = ec_fsm_slave_config_state_error; |
|
1487 return; |
|
1488 } |
|
1489 |
|
1490 if (!fsm->slave->config) { // config removed in the meantime |
|
1491 ec_fsm_slave_config_reconfigure(fsm); |
|
1492 return; |
|
1493 } |
|
1494 |
|
1495 // Another IDN to configure in SAFEOP? |
|
1496 while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { |
|
1497 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
|
1498 ec_soe_request_t, list); |
|
1499 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
|
1500 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
|
1501 ec_soe_request_write(&fsm->soe_request_copy); |
|
1502 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
|
1503 ec_fsm_soe_exec(fsm_soe); // execute immediately |
|
1504 ec_master_queue_external_datagram(slave->master, |
|
1505 fsm_soe->datagram); |
|
1506 return; |
|
1507 } |
|
1508 } |
|
1509 |
|
1510 // All SAFEOP IDNs are now configured. |
|
1511 ec_fsm_slave_config_enter_op(fsm); |
|
1512 } |
|
1513 |
|
1514 /*****************************************************************************/ |
|
1515 |
|
1516 /** Bring slave to OP. |
|
1517 */ |
|
1518 void ec_fsm_slave_config_enter_op( |
|
1519 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
1520 ) |
|
1521 { |
1419 // set state to OP |
1522 // set state to OP |
1420 fsm->state = ec_fsm_slave_config_state_op; |
1523 fsm->state = ec_fsm_slave_config_state_op; |
1421 ec_fsm_change_start(fsm->fsm_change, slave, EC_SLAVE_STATE_OP); |
1524 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_OP); |
1422 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
1525 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
1423 } |
1526 } |
1424 |
1527 |
1425 /*****************************************************************************/ |
1528 /*****************************************************************************/ |
1426 |
1529 |