master/fsm_slave_config.c
changeset 2066 b544025bd696
parent 2045 ff2a13a4603c
child 2149 98e1e773bed0
equal deleted inserted replaced
2065:4d8c9a441ef6 2066:b544025bd696
   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.