--- a/master/fsm_slave.c Thu Sep 06 14:21:02 2012 +0200
+++ b/master/fsm_slave.c Mon Nov 03 15:20:05 2014 +0100
@@ -2,7 +2,7 @@
*
* $Id$
*
- * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
@@ -36,19 +36,22 @@
#include "globals.h"
#include "master.h"
#include "mailbox.h"
+#include "slave_config.h"
#include "fsm_slave.h"
/*****************************************************************************/
-void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
-void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
-void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
-void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
-void ec_fsm_slave_state_soe_request(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 *, ec_datagram_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 *, ec_datagram_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 *, ec_datagram_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 *, ec_datagram_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 *, ec_datagram_t *);
/*****************************************************************************/
@@ -56,22 +59,23 @@
*/
void ec_fsm_slave_init(
ec_fsm_slave_t *fsm, /**< Slave state machine. */
- ec_slave_t *slave, /**< EtherCAT slave. */
- ec_mailbox_t *mbox/**< Datagram object to use. */
+ ec_slave_t *slave /**< EtherCAT slave. */
)
{
fsm->slave = slave;
- fsm->mbox = mbox;
- slave->datagram.data_size = 0;
-
- EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
+ INIT_LIST_HEAD(&fsm->list); // mark as unlisted
fsm->state = ec_fsm_slave_state_idle;
-
- // init sub-state-machines
- ec_fsm_coe_init(&fsm->fsm_coe, fsm->mbox);
- ec_fsm_foe_init(&fsm->fsm_foe, fsm->mbox);
- ec_fsm_soe_init(&fsm->fsm_soe, fsm->mbox);
+ fsm->datagram = NULL;
+ fsm->sdo_request = NULL;
+ fsm->reg_request = NULL;
+ fsm->foe_request = NULL;
+ 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);
}
/*****************************************************************************/
@@ -82,6 +86,28 @@
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);
@@ -92,30 +118,34 @@
/** Executes the current state of the state machine.
*
- * If the state machine's datagram is not sent or received yet, the execution
- * of the state machine is delayed to the next cycle.
- *
- * \return true, if the state machine was executed
+ * \return 1 if \a datagram was used, else 0.
*/
int ec_fsm_slave_exec(
- ec_fsm_slave_t *fsm /**< Slave state machine. */
- )
-{
- if (ec_mbox_is_datagram_state(fsm->mbox, EC_DATAGRAM_QUEUED)
- || ec_mbox_is_datagram_state(fsm->mbox, EC_DATAGRAM_SENT)) {
- // datagram was not sent or received yet.
- return 0;
- }
-
- fsm->state(fsm);
- return 1;
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< New datagram to use. */
+ )
+{
+ int datagram_used;
+
+ fsm->state(fsm, datagram);
+
+ datagram_used = fsm->state != ec_fsm_slave_state_idle &&
+ fsm->state != ec_fsm_slave_state_ready;
+
+ if (datagram_used) {
+ 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. */
)
{
@@ -125,6 +155,19 @@
}
}
+/*****************************************************************************/
+
+/** Returns, if the FSM is currently not busy and ready to execute.
+ *
+ * \return Non-zero if ready.
+ */
+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
*****************************************************************************/
@@ -132,7 +175,8 @@
/** 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
@@ -143,20 +187,29 @@
/** 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))
- return;
+ if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
+ return;
+ }
+
+ // Check for pending external register requests
+ if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
+ return;
+ }
// Check for pending FoE requests
- if (ec_fsm_slave_action_process_foe(fsm))
- return;
+ if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
+ return;
+ }
// Check for pending SoE requests
- if (ec_fsm_slave_action_process_soe(fsm))
- return;
+ if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
+ return;
+ }
}
/*****************************************************************************/
@@ -166,52 +219,49 @@
* \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_slave_t *slave = fsm->slave;
- ec_master_sdo_request_t *request, *next;
-
- // search the first external request to be processed
- list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
-
- list_del_init(&request->list); // dequeue
- if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
- EC_SLAVE_WARN(slave, "Aborting SDO request %p,"
- " slave has error flag set.\n",request);
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount,ec_master_sdo_request_release);
- wake_up(&slave->sdo_queue);
- fsm->sdo_request = NULL;
- fsm->state = ec_fsm_slave_state_idle;
- return 0;
- }
-
- if (slave->current_state == EC_SLAVE_STATE_INIT) {
- EC_SLAVE_WARN(slave, "Aborting SDO request %p,"
- " slave is in INIT.\n", request);
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount,ec_master_sdo_request_release);
- wake_up(&slave->sdo_queue);
- fsm->sdo_request = NULL;
- fsm->state = ec_fsm_slave_state_idle;
- return 0;
- }
-
- request->req.state = EC_INT_REQUEST_BUSY;
-
- // Found pending SDO request. Execute it!
- EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n", request);
-
- // Start SDO transfer
- fsm->sdo_request = request;
- fsm->state = ec_fsm_slave_state_sdo_request;
- ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
- ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
- ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+ 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 (list_empty(&slave->sdo_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);
+ list_del_init(&request->list); // dequeue
+
+ 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_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
return 1;
}
- return 0;
+
+ 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_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->state = ec_fsm_slave_state_sdo_request;
+ ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
+ ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
+ return 1;
}
/*****************************************************************************/
@@ -219,75 +269,193 @@
/** Slave state: SDO_REQUEST.
*/
void ec_fsm_slave_state_sdo_request(
- ec_fsm_slave_t *fsm /**< Slave state machine. */
- )
-{
- ec_slave_t *slave = fsm->slave;
- ec_master_sdo_request_t *request = fsm->sdo_request;
-
- if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
- ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
- return;
- }
+ 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, datagram)) {
+ return;
+ }
+
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
- EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n", request);
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount, ec_master_sdo_request_release);
- wake_up(&slave->sdo_queue);
+ EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
fsm->sdo_request = NULL;
- fsm->state = ec_fsm_slave_state_idle;
- return;
- }
-
- EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n", request);
+ fsm->state = ec_fsm_slave_state_ready;
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");
// SDO request finished
- request->req.state = EC_INT_REQUEST_SUCCESS;
- kref_put(&request->refcount, ec_master_sdo_request_release);
- wake_up(&slave->sdo_queue);
-
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&slave->master->request_queue);
fsm->sdo_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.
+/** 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_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_reg_request_t *reg;
+
+ fsm->reg_request = NULL;
+
+ if (slave->config) {
+ // search the first internal register request to be processed
+ list_for_each_entry(reg, &slave->config->reg_requests, list) {
+ if (reg->state == EC_INT_REQUEST_QUEUED) {
+ fsm->reg_request = reg;
+ break;
+ }
+ }
+ }
+
+ if (!fsm->reg_request && !list_empty(&slave->reg_requests)) {
+ // take the first external request to be processed
+ fsm->reg_request =
+ list_entry(slave->reg_requests.next, ec_reg_request_t, list);
+ list_del_init(&fsm->reg_request->list); // dequeue
+ }
+
+ if (!fsm->reg_request) { // no register request to process
+ return 0;
+ }
+
+ 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_all(&slave->master->request_queue);
+ fsm->reg_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return 1;
+ }
+
+ // Found pending register request. Execute it!
+ EC_SLAVE_DBG(slave, 1, "Processing register request...\n");
+
+ fsm->reg_request->state = EC_INT_REQUEST_BUSY;
+
+ // Start register access
+ if (fsm->reg_request->dir == EC_DIR_INPUT) {
+ ec_datagram_fprd(datagram, slave->station_address,
+ fsm->reg_request->address, fsm->reg_request->transfer_size);
+ ec_datagram_zero(datagram);
+ } else {
+ ec_datagram_fpwr(datagram, slave->station_address,
+ fsm->reg_request->address, fsm->reg_request->transfer_size);
+ memcpy(datagram->data, fsm->reg_request->data,
+ fsm->reg_request->transfer_size);
+ }
+ datagram->device_index = slave->device_index;
+ 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_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_all(&slave->master->request_queue);
+ fsm->reg_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+ return;
+ }
+
+ if (fsm->datagram->working_counter == 1) {
+ if (reg->dir == EC_DIR_INPUT) { // read request
+ memcpy(reg->data, fsm->datagram->data, reg->transfer_size);
+ }
+
+ reg->state = EC_INT_REQUEST_SUCCESS;
+ EC_SLAVE_DBG(slave, 1, "Register request successful.\n");
+ } else {
+ reg->state = EC_INT_REQUEST_FAILURE;
+ ec_datagram_print_state(fsm->datagram);
+ EC_SLAVE_ERR(slave, "Register request failed"
+ " (working counter is %u).\n",
+ fsm->datagram->working_counter);
+ }
+
+ 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_slave_t *slave = fsm->slave;
- ec_master_foe_request_t *request, *next;
-
- // search the first request to be processed
- list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
- if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
- EC_SLAVE_WARN(slave, "Aborting FOE request %p,"
- " slave has error flag set.\n", request);
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount, ec_master_foe_request_release);
- wake_up(&slave->foe_queue);
- fsm->sdo_request = NULL;
- fsm->state = ec_fsm_slave_state_idle;
- return 0;
- }
- list_del_init(&request->list); // dequeue
- request->req.state = EC_INT_REQUEST_BUSY;
-
- EC_SLAVE_DBG(slave, 1, "Processing FoE request %p.\n", request);
-
- fsm->foe_request = request;
- fsm->state = ec_fsm_slave_state_foe_request;
- ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
- ec_fsm_foe_exec(&fsm->fsm_foe);
- ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+ 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 (list_empty(&slave->foe_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ request = list_entry(slave->foe_requests.next, ec_foe_request_t, list);
+ list_del_init(&request->list); // dequeue
+
+ 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_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
return 1;
}
- return 0;
+
+ request->state = EC_INT_REQUEST_BUSY;
+ fsm->foe_request = request;
+
+ EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
+
+ fsm->state = ec_fsm_slave_state_foe_request;
+ ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
+ ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
+ return 1;
}
/*****************************************************************************/
@@ -295,35 +463,32 @@
/** Slave state: FOE REQUEST.
*/
void ec_fsm_slave_state_foe_request(
- ec_fsm_slave_t *fsm /**< Slave state machine. */
- )
-{
- ec_slave_t *slave = fsm->slave;
- ec_master_foe_request_t *request = fsm->foe_request;
-
- if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
- ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+ 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, datagram)) {
return;
}
if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
- EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n", request);
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount, ec_master_foe_request_release);
- wake_up(&slave->foe_queue);
+ EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
fsm->foe_request = NULL;
- fsm->state = ec_fsm_slave_state_idle;
+ fsm->state = ec_fsm_slave_state_ready;
return;
}
// finished transferring FoE
- EC_SLAVE_DBG(slave, 1, "FoE request %p successfully"
- " transferred %zu bytes.\n", request, request->req.data_size);
-
- request->req.state = EC_INT_REQUEST_SUCCESS;
- kref_put(&request->refcount, ec_master_foe_request_release);
- wake_up(&slave->foe_queue);
-
+ EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
+ " data.\n", request->data_size);
+
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&slave->master->request_queue);
fsm->foe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}
@@ -335,49 +500,49 @@
* \return non-zero, if a request is processed.
*/
int ec_fsm_slave_action_process_soe(
- ec_fsm_slave_t *fsm /**< Slave state machine. */
- )
-{
- ec_slave_t *slave = fsm->slave;
- ec_master_soe_request_t *request, *next;
-
- // search the first request to be processed
- list_for_each_entry_safe(request, next, &slave->soe_requests, list) {
-
- list_del_init(&request->list); // dequeue
- if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
- EC_SLAVE_WARN(slave, "Aborting SoE request,"
- " slave has error flag set.\n");
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount, ec_master_soe_request_release);
- wake_up(&slave->soe_queue);
- fsm->state = ec_fsm_slave_state_idle;
- return 0;
- }
-
- if (slave->current_state == EC_SLAVE_STATE_INIT) {
- EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount, ec_master_soe_request_release);
- wake_up(&slave->soe_queue);
- fsm->state = ec_fsm_slave_state_idle;
- return 0;
- }
-
- request->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 = request;
- fsm->state = ec_fsm_slave_state_soe_request;
- ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req);
- ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
- ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+ 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 (list_empty(&slave->soe_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ req = list_entry(slave->soe_requests.next, ec_soe_request_t, list);
+ list_del_init(&req->list); // dequeue
+
+ 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_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
return 1;
}
- return 0;
+
+ 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_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->state = ec_fsm_slave_state_soe_request;
+ ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
+ ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
+ return 1;
}
/*****************************************************************************/
@@ -385,34 +550,31 @@
/** Slave state: SOE_REQUEST.
*/
void ec_fsm_slave_state_soe_request(
- ec_fsm_slave_t *fsm /**< Slave state machine. */
- )
-{
- ec_slave_t *slave = fsm->slave;
- ec_master_soe_request_t *request = fsm->soe_request;
-
- if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
- ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+ 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, datagram)) {
return;
}
if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
- request->req.state = EC_INT_REQUEST_FAILURE;
- kref_put(&request->refcount, ec_master_soe_request_release);
- wake_up(&slave->soe_queue);
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
fsm->soe_request = NULL;
- fsm->state = ec_fsm_slave_state_idle;
+ fsm->state = ec_fsm_slave_state_ready;
return;
}
EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
// SoE request finished
- request->req.state = EC_INT_REQUEST_SUCCESS;
- kref_put(&request->refcount, ec_master_soe_request_release);
- wake_up(&slave->soe_queue);
-
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&slave->master->request_queue);
fsm->soe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}