equal
deleted
inserted
replaced
175 */ |
175 */ |
176 int ec_fsm_slave_config_exec( |
176 int ec_fsm_slave_config_exec( |
177 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
177 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
178 ) |
178 ) |
179 { |
179 { |
180 if (fsm->datagram->state == EC_DATAGRAM_SENT |
180 if (fsm->datagram->state == EC_DATAGRAM_QUEUED |
181 || fsm->datagram->state == EC_DATAGRAM_QUEUED) { |
181 || fsm->datagram->state == EC_DATAGRAM_SENT) { |
182 // datagram was not sent or received yet. |
182 // datagram was not sent or received yet. |
183 return ec_fsm_slave_config_running(fsm); |
183 return ec_fsm_slave_config_running(fsm); |
184 } |
184 } |
185 |
185 |
186 fsm->state(fsm); |
186 fsm->state(fsm); |
736 fsm->soe_request = req; |
736 fsm->soe_request = req; |
737 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
737 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
738 ec_soe_request_write(&fsm->soe_request_copy); |
738 ec_soe_request_write(&fsm->soe_request_copy); |
739 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
739 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
740 ec_fsm_soe_exec(fsm_soe); // execute immediately |
740 ec_fsm_soe_exec(fsm_soe); // execute immediately |
741 ec_master_queue_external_datagram(slave->master, |
741 ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox); |
742 fsm_soe->datagram); |
|
743 return; |
742 return; |
744 } |
743 } |
745 } |
744 } |
746 |
745 |
747 // No SoE configuration to be applied in PREOP |
746 // No SoE configuration to be applied in PREOP |
758 { |
757 { |
759 ec_slave_t *slave = fsm->slave; |
758 ec_slave_t *slave = fsm->slave; |
760 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
759 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
761 |
760 |
762 if (ec_fsm_soe_exec(fsm_soe)) { |
761 if (ec_fsm_soe_exec(fsm_soe)) { |
763 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
762 ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox); |
764 return; |
763 return; |
765 } |
764 } |
766 |
765 |
767 if (!ec_fsm_soe_success(fsm_soe)) { |
766 if (!ec_fsm_soe_success(fsm_soe)) { |
768 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
767 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
783 if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { |
782 if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { |
784 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
783 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
785 ec_soe_request_write(&fsm->soe_request_copy); |
784 ec_soe_request_write(&fsm->soe_request_copy); |
786 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
785 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
787 ec_fsm_soe_exec(fsm_soe); // execute immediately |
786 ec_fsm_soe_exec(fsm_soe); // execute immediately |
788 ec_master_queue_external_datagram(slave->master, |
787 ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox); |
789 fsm_soe->datagram); |
|
790 return; |
788 return; |
791 } |
789 } |
792 } |
790 } |
793 |
791 |
794 // All PREOP IDNs are now configured. |
792 // All PREOP IDNs are now configured. |
1246 } |
1244 } |
1247 |
1245 |
1248 abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; |
1246 abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; |
1249 diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1247 diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1250 |
1248 |
1251 if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { |
1249 if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { |
1252 |
1250 |
1253 if (diff_ms >= EC_DC_SYNC_WAIT_MS) { |
1251 if (diff_ms >= EC_DC_SYNC_WAIT_MS) { |
1254 EC_SLAVE_WARN(slave, "Slave did not sync after %lu ms.\n", |
1252 EC_SLAVE_WARN(slave, "Slave did not sync after %lu ms.\n", |
1255 diff_ms); |
1253 diff_ms); |
1256 } else { |
1254 } else { |
1452 fsm->soe_request = req; |
1450 fsm->soe_request = req; |
1453 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1451 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1454 ec_soe_request_write(&fsm->soe_request_copy); |
1452 ec_soe_request_write(&fsm->soe_request_copy); |
1455 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
1453 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
1456 ec_fsm_soe_exec(fsm_soe); // execute immediately |
1454 ec_fsm_soe_exec(fsm_soe); // execute immediately |
1457 ec_master_queue_external_datagram(slave->master, |
1455 ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox); |
1458 fsm_soe->datagram); |
|
1459 return; |
1456 return; |
1460 } |
1457 } |
1461 } |
1458 } |
1462 |
1459 |
1463 // No SoE configuration to be applied in SAFEOP |
1460 // No SoE configuration to be applied in SAFEOP |
1474 { |
1471 { |
1475 ec_slave_t *slave = fsm->slave; |
1472 ec_slave_t *slave = fsm->slave; |
1476 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
1473 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
1477 |
1474 |
1478 if (ec_fsm_soe_exec(fsm_soe)) { |
1475 if (ec_fsm_soe_exec(fsm_soe)) { |
1479 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
1476 ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox); |
1480 return; |
1477 return; |
1481 } |
1478 } |
1482 |
1479 |
1483 if (!ec_fsm_soe_success(fsm_soe)) { |
1480 if (!ec_fsm_soe_success(fsm_soe)) { |
1484 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
1481 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
1499 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
1496 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
1500 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1497 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1501 ec_soe_request_write(&fsm->soe_request_copy); |
1498 ec_soe_request_write(&fsm->soe_request_copy); |
1502 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
1499 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
1503 ec_fsm_soe_exec(fsm_soe); // execute immediately |
1500 ec_fsm_soe_exec(fsm_soe); // execute immediately |
1504 ec_master_queue_external_datagram(slave->master, |
1501 ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox); |
1505 fsm_soe->datagram); |
|
1506 return; |
1502 return; |
1507 } |
1503 } |
1508 } |
1504 } |
1509 |
1505 |
1510 // All SAFEOP IDNs are now configured. |
1506 // All SAFEOP IDNs are now configured. |