etherlabmaster: comparison master/fsm

equal deleted inserted replaced

-:505cf41488a4
+:9cdd7669dc0b
 #include "fsm_slave.h"
 /*****************************************************************************/
-void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
+void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);
-void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
+void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);
-int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
+int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);
-void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);
-int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *);
+int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);
-void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);
-int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
+int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
-void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
-int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
+int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
-void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);
 /*****************************************************************************/
 /** Constructor.
 */
 void ec_fsm_slave_init(
 ec_fsm_slave_t *fsm, /**< Slave state machine. */
-ec_slave_t *slave, /**< EtherCAT slave. */
+ec_slave_t *slave /**< EtherCAT slave. */
-ec_datagram_t *datagram /**< Datagram object to use. */
 )
 {
 fsm->slave = slave;
-fsm->datagram = datagram;
+INIT_LIST_HEAD(&fsm->list); // mark as unlisted
-fsm->datagram->data_size = 0;
-EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
 fsm->state = ec_fsm_slave_state_idle;
+fsm->datagram = NULL;
-// init sub-state-machines
+fsm->sdo_request = NULL;
-ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
+fsm->reg_request = NULL;
-ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
+fsm->foe_request = NULL;
-ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
+fsm->soe_request = NULL;
+// Init sub-state-machines
+ec_fsm_coe_init(&fsm->fsm_coe);
+ec_fsm_foe_init(&fsm->fsm_foe);
+ec_fsm_soe_init(&fsm->fsm_soe);
 }
 /*****************************************************************************/
 /** Destructor.
 */
 void ec_fsm_slave_clear(
 ec_fsm_slave_t *fsm /**< Master state machine. */
 )
 {
+// signal requests that are currently in operation
+if (fsm->sdo_request) {
+fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;
+wake_up_all(&fsm->slave->master->request_queue);
+}
+if (fsm->reg_request) {
+fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
+wake_up_all(&fsm->slave->master->request_queue);
+}
+if (fsm->foe_request) {
+fsm->foe_request->state = EC_INT_REQUEST_FAILURE;
+wake_up_all(&fsm->slave->master->request_queue);
+}
+if (fsm->soe_request) {
+fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
+wake_up_all(&fsm->slave->master->request_queue);
+}
 // clear sub-state machines
 ec_fsm_coe_clear(&fsm->fsm_coe);
 ec_fsm_foe_clear(&fsm->fsm_foe);
 ec_fsm_soe_clear(&fsm->fsm_soe);
 }
 /*****************************************************************************/
 /** Executes the current state of the state machine.
 *
-* If the state machine's datagram is not sent or received yet, the execution
+* \return 1 if \a datagram was used, else 0.
-* of the state machine is delayed to the next cycle.
+*/
-*/
+int ec_fsm_slave_exec(
-void ec_fsm_slave_exec(
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_datagram_t *datagram /**< New datagram to use. */
 )
 {
-if (fsm->datagram->state == EC_DATAGRAM_SENT
+int datagram_used;
-|| fsm->datagram->state == EC_DATAGRAM_QUEUED) {
-// datagram was not sent or received yet.
+fsm->state(fsm, datagram);
-return;
-}
+datagram_used = fsm->state != ec_fsm_slave_state_idle &&
+fsm->state != ec_fsm_slave_state_ready;
-fsm->state(fsm);
+if (datagram_used) {
-ec_datagram_output_stats(fsm->datagram);
+fsm->datagram = datagram;
+} else {
+fsm->datagram = NULL;
+}
+return datagram_used;
 }
 /*****************************************************************************/
 /** Sets the current state of the state machine to READY
 */
-void ec_fsm_slave_ready(
+void ec_fsm_slave_set_ready(
 ec_fsm_slave_t *fsm /**< Slave state machine. */
 )
 {
 if (fsm->state == ec_fsm_slave_state_idle) {
 EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n");
 fsm->state = ec_fsm_slave_state_ready;
 }
 }
+/*****************************************************************************/
+/** Returns, if the FSM is currently not busy and ready to execute.
+*/
+int ec_fsm_slave_is_ready(
+const ec_fsm_slave_t *fsm /**< Slave state machine. */
+)
+{
+return fsm->state == ec_fsm_slave_state_ready;
+}
 /******************************************************************************
 * Slave state machine
 *****************************************************************************/
 /** Slave state: IDLE.
 */
 void ec_fsm_slave_state_idle(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 // do nothing
 }
 /*****************************************************************************/
 /** Slave state: READY.
 */
 void ec_fsm_slave_state_ready(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 // Check for pending external SDO requests
-if (ec_fsm_slave_action_process_sdo(fsm)) {
+if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
 return;
 }
 // Check for pending external register requests
-if (ec_fsm_slave_action_process_reg(fsm)) {
+if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
 return;
 }
 // Check for pending FoE requests
-if (ec_fsm_slave_action_process_foe(fsm)) {
+if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
 return;
 }
 // Check for pending SoE requests
-if (ec_fsm_slave_action_process_soe(fsm)) {
+if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
 return;
 }
 }
 /*****************************************************************************/
 /** Check for pending SDO requests and process one.
 *
 * \return non-zero, if an SDO request is processed.
 */
 int ec_fsm_slave_action_process_sdo(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_sdo_request_t *request;
 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 EC_SLAVE_WARN(slave, "Aborting SDO request,"
 " slave has error flag set.\n");
 request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->state = ec_fsm_slave_state_idle;
 return 1;
 }
 if (slave->current_state == EC_SLAVE_STATE_INIT) {
 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
 request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->state = ec_fsm_slave_state_idle;
 return 1;
 }
+fsm->sdo_request = request;
 request->state = EC_INT_REQUEST_BUSY;
 // Found pending SDO request. Execute it!
 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
 // Start SDO transfer
-fsm->sdo_request = request;
 fsm->state = ec_fsm_slave_state_sdo_request;
 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
-ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
-ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
 return 1;
 }
 /*****************************************************************************/
 /** Slave state: SDO_REQUEST.
 */
 void ec_fsm_slave_state_sdo_request(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_sdo_request_t *request = fsm->sdo_request;
-if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
+if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) {
-ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
 return;
 }
 if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
 request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->sdo_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 return;
 }
 EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");
 // SDO request finished
 request->state = EC_INT_REQUEST_SUCCESS;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->sdo_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 }
 /*****************************************************************************/
 /** Check for pending register requests and process one.
 *
 * \return non-zero, if a register request is processed.
 */
 int ec_fsm_slave_action_process_reg(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_reg_request_t *reg;
 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 EC_SLAVE_WARN(slave, "Aborting register request,"
 " slave has error flag set.\n");
 fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->reg_request = NULL;
 fsm->state = ec_fsm_slave_state_idle;
 return 1;
 }
 fsm->reg_request->state = EC_INT_REQUEST_BUSY;
 // Start register access
 if (fsm->reg_request->dir == EC_DIR_INPUT) {
-ec_datagram_fprd(fsm->datagram, slave->station_address,
+ec_datagram_fprd(datagram, slave->station_address,
 fsm->reg_request->address, fsm->reg_request->transfer_size);
-ec_datagram_zero(fsm->datagram);
+ec_datagram_zero(datagram);
 } else {
-ec_datagram_fpwr(fsm->datagram, slave->station_address,
+ec_datagram_fpwr(datagram, slave->station_address,
 fsm->reg_request->address, fsm->reg_request->transfer_size);
-memcpy(fsm->datagram->data, fsm->reg_request->data,
+memcpy(datagram->data, fsm->reg_request->data,
 fsm->reg_request->transfer_size);
 }
-fsm->datagram->device_index = slave->device_index;
+datagram->device_index = slave->device_index;
-ec_master_queue_external_datagram(slave->master, fsm->datagram);
 fsm->state = ec_fsm_slave_state_reg_request;
 return 1;
 }
 /*****************************************************************************/
 /** Slave state: Register request.
 */
 void ec_fsm_slave_state_reg_request(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_reg_request_t *reg = fsm->reg_request;
 if (!reg) {
 // configuration was cleared in the meantime
 fsm->state = ec_fsm_slave_state_ready;
+fsm->reg_request = NULL;
 return;
 }
 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
 EC_SLAVE_ERR(slave, "Failed to receive register"
 " request datagram: ");
 ec_datagram_print_state(fsm->datagram);
 reg->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->reg_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 return;
 }
 EC_SLAVE_ERR(slave, "Register request failed"
 " (working counter is %u).\n",
 fsm->datagram->working_counter);
 }
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->reg_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 }
 /*****************************************************************************/
 /** Check for pending FoE requests and process one.
 *
 * \return non-zero, if an FoE request is processed.
 */
 int ec_fsm_slave_action_process_foe(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_foe_request_t *request;
 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 EC_SLAVE_WARN(slave, "Aborting FoE request,"
 " slave has error flag set.\n");
 request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->state = ec_fsm_slave_state_idle;
 return 1;
 }
 request->state = EC_INT_REQUEST_BUSY;
+fsm->foe_request = request;
 EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
-fsm->foe_request = request;
 fsm->state = ec_fsm_slave_state_foe_request;
 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
-ec_fsm_foe_exec(&fsm->fsm_foe);
+ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
-ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
 return 1;
 }
 /*****************************************************************************/
 /** Slave state: FOE REQUEST.
 */
 void ec_fsm_slave_state_foe_request(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_foe_request_t *request = fsm->foe_request;
-if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
+if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {
-ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
 return;
 }
 if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
 EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
 request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->foe_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 return;
 }
 // finished transferring FoE
 EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
 " data.\n", request->data_size);
 request->state = EC_INT_REQUEST_SUCCESS;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->foe_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 }
 /*****************************************************************************/
 /** Check for pending SoE requests and process one.
 *
 * \return non-zero, if a request is processed.
 */
 int ec_fsm_slave_action_process_soe(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_soe_request_t *req;
 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 EC_SLAVE_WARN(slave, "Aborting SoE request,"
 " slave has error flag set.\n");
 req->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->state = ec_fsm_slave_state_idle;
 return 1;
 }
 if (slave->current_state == EC_SLAVE_STATE_INIT) {
 EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
 req->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->state = ec_fsm_slave_state_idle;
 return 0;
 }
+fsm->soe_request = req;
 req->state = EC_INT_REQUEST_BUSY;
 // Found pending request. Execute it!
 EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
 // Start SoE transfer
-fsm->soe_request = req;
 fsm->state = ec_fsm_slave_state_soe_request;
 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
-ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
+ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
-ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
 return 1;
 }
 /*****************************************************************************/
 /** Slave state: SOE_REQUEST.
 */
 void ec_fsm_slave_state_soe_request(
-ec_fsm_slave_t *fsm /**< Slave state machine. */
+ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ec_datagram_t *datagram /**< Datagram to use. */
 )
 {
 ec_slave_t *slave = fsm->slave;
 ec_soe_request_t *request = fsm->soe_request;
-if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
+if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) {
-ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
 return;
 }
 if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
 request->state = EC_INT_REQUEST_FAILURE;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->soe_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 return;
 }
 EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
 // SoE request finished
 request->state = EC_INT_REQUEST_SUCCESS;
-wake_up(&slave->master->request_queue);
+wake_up_all(&slave->master->request_queue);
 fsm->soe_request = NULL;
 fsm->state = ec_fsm_slave_state_ready;
 }
 /*****************************************************************************/

branch	stable-1.5
changeset 2498	9cdd7669dc0b
parent 2467	74ede087bc85
child 2522	ec403cf308eb