--- a/NEWS Thu Jan 10 12:34:58 2013 +0100
+++ b/NEWS Thu Jan 10 17:36:41 2013 +0100
@@ -17,6 +17,7 @@
* Added fix for ESC port order (DC delay calculation).
* Added e1000 driver for kernels 2.6.35, 3.0, 3.4.
* Added 8139too driver for kernel 3.4.
+* Avoided slave FSM datagram queue; implemented datagram ring instead.
Changes in 1.5.0:
--- a/master/fsm_coe.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_coe.c Thu Jan 10 17:36:41 2013 +0100
@@ -27,10 +27,8 @@
*
*****************************************************************************/
-/**
- \file
- EtherCAT CoE state machines.
-*/
+/** \file EtherCAT CoE state machines.
+ */
/*****************************************************************************/
@@ -68,44 +66,43 @@
/*****************************************************************************/
-void ec_fsm_coe_dict_start(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_request(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_check(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_response(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);
-
-void ec_fsm_coe_down_start(ec_fsm_coe_t *);
-void ec_fsm_coe_down_request(ec_fsm_coe_t *);
-void ec_fsm_coe_down_check(ec_fsm_coe_t *);
-void ec_fsm_coe_down_response(ec_fsm_coe_t *);
-void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);
-void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);
-
-void ec_fsm_coe_up_start(ec_fsm_coe_t *);
-void ec_fsm_coe_up_request(ec_fsm_coe_t *);
-void ec_fsm_coe_up_check(ec_fsm_coe_t *);
-void ec_fsm_coe_up_response(ec_fsm_coe_t *);
-void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);
-void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);
-void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);
-
-void ec_fsm_coe_end(ec_fsm_coe_t *);
-void ec_fsm_coe_error(ec_fsm_coe_t *);
-
-/*****************************************************************************/
-
-/**
- SDO abort messages.
- The "abort SDO transfer request" supplies an abort code,
- which can be translated to clear text. This table does
- the mapping of the codes and messages.
-*/
-
+void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *);
+
+/*****************************************************************************/
+
+/** SDO abort messages.
+ *
+ * The "abort SDO transfer request" supplies an abort code, which can be
+ * translated to clear text. This table does the mapping of the codes and
+ * messages.
+ */
const ec_code_msg_t sdo_abort_messages[] = {
{0x05030000, "Toggle bit not changed"},
{0x05040000, "SDO protocol timeout"},
@@ -147,7 +144,10 @@
/** Outputs an SDO abort message.
*/
-void ec_canopen_abort_msg(const ec_slave_t *slave, uint32_t abort_code)
+void ec_canopen_abort_msg(
+ const ec_slave_t *slave, /**< Slave. */
+ uint32_t abort_code /**< Abort code to search for. */
+ )
{
const ec_code_msg_t *abort_msg;
@@ -164,37 +164,34 @@
/*****************************************************************************/
-/**
- Constructor.
-*/
-
-void ec_fsm_coe_init(ec_fsm_coe_t *fsm, /**< finite state machine */
- ec_datagram_t *datagram /**< datagram */
- )
+/** Constructor.
+ */
+void ec_fsm_coe_init(
+ ec_fsm_coe_t *fsm /**< Finite state machine */
+ )
{
fsm->state = NULL;
- fsm->datagram = datagram;
-}
-
-/*****************************************************************************/
-
-/**
- Destructor.
-*/
-
-void ec_fsm_coe_clear(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
-
-/**
- Starts reading a slaves' SDO dictionary.
-*/
-
-void ec_fsm_coe_dictionary(ec_fsm_coe_t *fsm, /**< finite state machine */
- ec_slave_t *slave /**< EtherCAT slave */
- )
+ fsm->datagram = NULL;
+}
+
+/*****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_coe_clear(
+ ec_fsm_coe_t *fsm /**< Finite state machine */
+ )
+{
+}
+
+/*****************************************************************************/
+
+/** Starts reading a slaves' SDO dictionary.
+ */
+void ec_fsm_coe_dictionary(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_slave_t *slave /**< EtherCAT slave */
+ )
{
fsm->slave = slave;
fsm->state = ec_fsm_coe_dict_start;
@@ -202,10 +199,8 @@
/*****************************************************************************/
-/**
- Starts to transfer an SDO to/from a slave.
-*/
-
+/** Starts to transfer an SDO to/from a slave.
+ */
void ec_fsm_coe_transfer(
ec_fsm_coe_t *fsm, /**< State machine. */
ec_slave_t *slave, /**< EtherCAT slave. */
@@ -214,34 +209,58 @@
{
fsm->slave = slave;
fsm->request = request;
- if (request->dir == EC_DIR_OUTPUT)
+
+ if (request->dir == EC_DIR_OUTPUT) {
fsm->state = ec_fsm_coe_down_start;
- else
+ }
+ else {
fsm->state = ec_fsm_coe_up_start;
-}
-
-/*****************************************************************************/
-
-/**
- Executes the current state of the state machine.
- \return false, if state machine has terminated
-*/
-
-int ec_fsm_coe_exec(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- fsm->state(fsm);
-
- return fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;
-}
-
-/*****************************************************************************/
-
-/**
- Returns, if the state machine terminated with success.
- \return non-zero if successful.
-*/
-
-int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */)
+ }
+}
+
+/*****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_coe_exec(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/*****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ * \return non-zero if successful.
+ */
+int ec_fsm_coe_success(
+ const ec_fsm_coe_t *fsm /**< Finite state machine */
+ )
{
return fsm->state == ec_fsm_coe_end;
}
@@ -288,33 +307,15 @@
* CoE dictionary state machine
*****************************************************************************/
-/**
- CoE state: DICT START.
-*/
-
-void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
- uint8_t *data;
-
- if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
- EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
- if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) {
- EC_SLAVE_ERR(slave, "Slave does not support"
- " SDO information service!\n");
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
- data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
+int ec_fsm_coe_prepare_dict(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
if (IS_ERR(data)) {
- fsm->state = ec_fsm_coe_error;
- return;
+ return PTR_ERR(data);
}
EC_WRITE_U16(data, 0x8 << 12); // SDO information
@@ -323,41 +324,76 @@
EC_WRITE_U16(data + 4, 0x0000);
EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
+ fsm->state = ec_fsm_coe_dict_request;
+ return 0;
+}
+
+/*****************************************************************************/
+
+/** CoE state: DICT START.
+ */
+void ec_fsm_coe_dict_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) {
+ EC_SLAVE_ERR(slave, "Slave does not support"
+ " SDO information service!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_dict_request;
-}
-
-/*****************************************************************************/
-
-/**
- CoE state: DICT REQUEST.
- \todo Timeout behavior
-*/
-
-void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+
+ if (ec_fsm_coe_prepare_dict(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/*****************************************************************************/
+
+/** CoE state: DICT REQUEST.
+ * \todo Timeout behavior
+ */
+void ec_fsm_coe_dict_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_prepare_dict(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
" request datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@@ -366,36 +402,39 @@
/*****************************************************************************/
-/**
- CoE state: DICT CHECK.
-*/
-
-void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+/** CoE state: DICT CHECK.
+ */
+void ec_fsm_coe_dict_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Timeout while waiting for"
@@ -416,14 +455,39 @@
/*****************************************************************************/
+int ec_fsm_coe_dict_prepare_desc(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U16(data, 0x8 << 12); // SDO information
+ EC_WRITE_U8 (data + 2, 0x03); // Get object description request
+ EC_WRITE_U8 (data + 3, 0x00);
+ EC_WRITE_U16(data + 4, 0x0000);
+ EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
+
+ fsm->state = ec_fsm_coe_dict_desc_request;
+ return 0;
+}
+
+/*****************************************************************************/
+
/**
CoE state: DICT RESPONSE.
\todo Timeout behavior
*/
-void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_dict_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
@@ -433,25 +497,27 @@
bool first_segment;
size_t index_list_offset;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
" response datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@@ -543,7 +609,7 @@
if (EC_READ_U8(data + 2) & 0x80 || fragments_left) {
// more messages waiting. check again.
- fsm->jiffies_start = datagram->jiffies_sent;
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
@@ -559,20 +625,10 @@
// fetch SDO descriptions
fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);
- data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
- if (IS_ERR(data)) {
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
- EC_WRITE_U16(data, 0x8 << 12); // SDO information
- EC_WRITE_U8 (data + 2, 0x03); // Get object description request
- EC_WRITE_U8 (data + 3, 0x00);
- EC_WRITE_U16(data + 4, 0x0000);
- EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
-
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_dict_desc_request;
+ if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
}
/*****************************************************************************/
@@ -582,31 +638,37 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_desc_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
" description request datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" request failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@@ -619,32 +681,37 @@
CoE state: DICT DESC CHECK.
*/
-void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_desc_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Timeout while waiting for"
@@ -666,40 +733,68 @@
/*****************************************************************************/
+int ec_fsm_coe_dict_prepare_entry(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U16(data, 0x8 << 12); // SDO information
+ EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
+ EC_WRITE_U8 (data + 3, 0x00);
+ EC_WRITE_U16(data + 4, 0x0000);
+ EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
+ EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
+ EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only)
+
+ fsm->state = ec_fsm_coe_dict_entry_request;
+ return 0;
+}
+
+/*****************************************************************************/
+
/**
CoE state: DICT DESC RESPONSE.
\todo Timeout behavior
*/
-void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
- /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_dict_desc_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_sdo_t *sdo = fsm->sdo;
uint8_t *data, mbox_prot;
size_t rec_size, name_size;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"
" response datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@@ -789,23 +884,11 @@
// start fetching entries
fsm->subindex = 0;
-
- data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
- if (IS_ERR(data)) {
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
- EC_WRITE_U16(data, 0x8 << 12); // SDO information
- EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
- EC_WRITE_U8 (data + 3, 0x00);
- EC_WRITE_U16(data + 4, 0x0000);
- EC_WRITE_U16(data + 6, sdo->index); // SDO index
- EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
- EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only)
-
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_dict_entry_request;
+
+ if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
}
/*****************************************************************************/
@@ -815,31 +898,36 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm
- /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_entry_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry"
" request datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail
fsm->retries = EC_FSM_RETRIES;
@@ -852,33 +940,37 @@
CoE state: DICT ENTRY CHECK.
*/
-void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm
- /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_entry_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Timeout while waiting for"
@@ -905,10 +997,11 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
- /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_dict_entry_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_sdo_t *sdo = fsm->sdo;
uint8_t *data, mbox_prot;
@@ -916,26 +1009,28 @@
ec_sdo_entry_t *entry;
u16 word;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
" description response datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@@ -1046,45 +1141,27 @@
}
if (fsm->subindex < sdo->max_subindex) {
+
fsm->subindex++;
-
- data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
- if (IS_ERR(data)) {
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
fsm->state = ec_fsm_coe_error;
- return;
- }
-
- EC_WRITE_U16(data, 0x8 << 12); // SDO information
- EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
- EC_WRITE_U8 (data + 3, 0x00);
- EC_WRITE_U16(data + 4, 0x0000);
- EC_WRITE_U16(data + 6, sdo->index); // SDO index
- EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
- EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
-
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_dict_entry_request;
+ }
+
return;
}
// another SDO description to fetch?
if (fsm->sdo->list.next != &slave->sdo_dictionary) {
+
fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list);
-
- data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
- if (IS_ERR(data)) {
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
fsm->state = ec_fsm_coe_error;
- return;
- }
-
- EC_WRITE_U16(data, 0x8 << 12); // SDO information
- EC_WRITE_U8 (data + 2, 0x03); // Get object description request
- EC_WRITE_U8 (data + 3, 0x00);
- EC_WRITE_U16(data + 4, 0x0000);
- EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
-
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_dict_desc_request;
+ }
+
return;
}
@@ -1095,52 +1172,22 @@
* CoE state machine
*****************************************************************************/
-/** CoE state: DOWN START.
- */
-void ec_fsm_coe_down_start(
- ec_fsm_coe_t *fsm /**< finite state machine */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+int ec_fsm_coe_prepare_down_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ u8 *data;
ec_slave_t *slave = fsm->slave;
ec_sdo_request_t *request = fsm->request;
- uint8_t *data;
uint8_t data_set_size;
- if (fsm->slave->master->debug_level) {
- char subidxstr[10];
- if (request->complete_access) {
- subidxstr[0] = 0x00;
- } else {
- sprintf(subidxstr, ":%02X", request->subindex);
- }
- EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n",
- request->index, subidxstr);
- ec_print_data(request->data, request->data_size);
- }
-
- if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
- EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
- request->errno = EPROTONOSUPPORT;
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
- if (slave->configured_rx_mailbox_size <
- EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
- EC_SLAVE_ERR(slave, "Mailbox too small!\n");
- request->errno = EOVERFLOW;
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
if (request->data_size <= 4) { // use expedited transfer type
data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
EC_COE_DOWN_REQ_HEADER_SIZE);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
- fsm->state = ec_fsm_coe_error;
- return;
+ return PTR_ERR(data);
}
fsm->remaining = 0;
@@ -1181,8 +1228,7 @@
data_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
- fsm->state = ec_fsm_coe_error;
- return;
+ return PTR_ERR(data);
}
fsm->offset = 0;
@@ -1212,9 +1258,56 @@
}
}
+ fsm->state = ec_fsm_coe_down_request;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** CoE state: DOWN START.
+ */
+void ec_fsm_coe_down_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+
+ if (fsm->slave->master->debug_level) {
+ char subidxstr[10];
+ if (request->complete_access) {
+ subidxstr[0] = 0x00;
+ } else {
+ sprintf(subidxstr, ":%02X", request->subindex);
+ }
+ EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n",
+ request->index, subidxstr);
+ ec_print_data(request->data, request->data_size);
+ }
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+ request->errno = EPROTONOSUPPORT;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (slave->configured_rx_mailbox_size <
+ EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
+ EC_SLAVE_ERR(slave, "Mailbox too small!\n");
+ request->errno = EOVERFLOW;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+
fsm->request->jiffies_sent = jiffies;
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_down_request;
+
+ if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
}
/*****************************************************************************/
@@ -1224,28 +1317,34 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_down_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE download"
" request datagram: ");
- ec_datagram_print_state(datagram);
+ ec_datagram_print_state(fsm->datagram);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
- if (datagram->working_counter != 1) {
- if (!datagram->working_counter) {
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
if (diff_ms < fsm->request->response_timeout) {
#if DEBUG_RETRIES
EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO"
@@ -1253,6 +1352,9 @@
diff_ms);
#endif
// no response; send request datagram again
+ if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
return;
}
}
@@ -1261,7 +1363,7 @@
EC_SLAVE_ERR(slave, "Reception of CoE download request"
" for SDO 0x%04x:%x failed with timeout after %lu ms: ",
fsm->request->index, fsm->request->subindex, diff_ms);
- ec_datagram_print_wc_error(datagram);
+ ec_datagram_print_wc_error(fsm->datagram);
return;
}
@@ -1272,7 +1374,7 @@
}
#endif
- fsm->jiffies_start = datagram->jiffies_sent;
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@@ -1283,35 +1385,40 @@
/** CoE state: DOWN CHECK.
*/
-void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_down_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
" datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
@@ -1337,32 +1444,32 @@
/** Prepare a download segment request.
*/
void ec_fsm_coe_down_prepare_segment_request(
- ec_fsm_coe_t *fsm /**< finite state machine */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_sdo_request_t *request = fsm->request;
size_t max_segment_size =
slave->configured_rx_mailbox_size
- EC_MBOX_HEADER_SIZE
- EC_COE_DOWN_SEG_REQ_HEADER_SIZE;
- size_t segment_size, data_size;
+ size_t data_size;
uint8_t last_segment, seg_data_size, *data;
if (fsm->remaining > max_segment_size) {
- segment_size = max_segment_size;
+ fsm->segment_size = max_segment_size;
last_segment = 0;
} else {
- segment_size = fsm->remaining;
+ fsm->segment_size = fsm->remaining;
last_segment = 1;
}
- if (segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
+ if (fsm->segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
seg_data_size = 0x00;
- data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size;
+ data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size;
} else {
- seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size;
+ seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size;
data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE
+ EC_COE_DOWN_SEG_MIN_DATA_SIZE;
}
@@ -1381,14 +1488,11 @@
| (fsm->toggle << 4)
| (0x00 << 5)); // Download segment request
memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE,
- request->data + fsm->offset, segment_size);
- if (segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
- memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size, 0x00,
- EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size);
- }
-
- fsm->offset += segment_size;
- fsm->remaining -= segment_size;
+ request->data + fsm->offset, fsm->segment_size);
+ if (fsm->segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
+ memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size,
+ 0x00, EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size);
+ }
if (slave->master->debug_level) {
EC_SLAVE_DBG(slave, 1, "Download segment request:\n");
@@ -1405,35 +1509,39 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_down_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_request_t *request = fsm->request;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE download"
" response datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -1510,7 +1618,7 @@
if (fsm->remaining) { // more segments to download
fsm->toggle = 0;
- ec_fsm_coe_down_prepare_segment_request(fsm);
+ ec_fsm_coe_down_prepare_segment_request(fsm, datagram);
} else {
fsm->state = ec_fsm_coe_end; // success
}
@@ -1522,34 +1630,37 @@
CoE state: DOWN SEG CHECK.
*/
-void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_down_seg_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
@@ -1577,36 +1688,38 @@
*/
void ec_fsm_coe_down_seg_response(
- ec_fsm_coe_t *fsm /**< finite state machine */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_request_t *request = fsm->request;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE download response"
" datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -1688,9 +1801,12 @@
return;
}
+ fsm->offset += fsm->segment_size;
+ fsm->remaining -= fsm->segment_size;
+
if (fsm->remaining) { // more segments to download
fsm->toggle = !fsm->toggle;
- ec_fsm_coe_down_prepare_segment_request(fsm);
+ ec_fsm_coe_down_prepare_segment_request(fsm, datagram);
} else {
fsm->state = ec_fsm_coe_end; // success
}
@@ -1698,33 +1814,19 @@
/*****************************************************************************/
-/**
- CoE state: UP START.
-*/
-
-void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
+int ec_fsm_coe_prepare_up(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
ec_master_t *master = slave->master;
- ec_sdo_request_t *request = fsm->request;
- uint8_t *data;
-
- EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n",
- request->index, request->subindex);
-
- if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
- EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
- request->errno = EPROTONOSUPPORT;
- fsm->state = ec_fsm_coe_error;
- return;
- }
-
- data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
+
+ u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
- fsm->state = ec_fsm_coe_error;
- return;
+ return PTR_ERR(data);
}
EC_WRITE_U16(data, 0x2 << 12); // SDO request
@@ -1738,39 +1840,75 @@
ec_print_data(data, 10);
}
+ fsm->state = ec_fsm_coe_up_request;
+ return 0;
+}
+
+/*****************************************************************************/
+
+/**
+ CoE state: UP START.
+*/
+
+void ec_fsm_coe_up_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+
+ EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n",
+ request->index, request->subindex);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+ request->errno = EPROTONOSUPPORT;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ fsm->retries = EC_FSM_RETRIES;
fsm->request->jiffies_sent = jiffies;
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_up_request;
-}
-
-/*****************************************************************************/
-
+
+ if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/*****************************************************************************/
/**
CoE state: UP REQUEST.
\todo Timeout behavior
*/
-void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_up_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: ");
- ec_datagram_print_state(datagram);
+ ec_datagram_print_state(fsm->datagram);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
- if (datagram->working_counter != 1) {
- if (!datagram->working_counter) {
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
if (diff_ms < fsm->request->response_timeout) {
#if DEBUG_RETRIES
EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
@@ -1778,6 +1916,9 @@
diff_ms);
#endif
// no response; send request datagram again
+ if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
return;
}
}
@@ -1786,7 +1927,7 @@
EC_SLAVE_ERR(slave, "Reception of CoE upload request for"
" SDO 0x%04x:%x failed with timeout after %lu ms: ",
fsm->request->index, fsm->request->subindex, diff_ms);
- ec_datagram_print_wc_error(datagram);
+ ec_datagram_print_wc_error(fsm->datagram);
return;
}
@@ -1797,7 +1938,7 @@
}
#endif
- fsm->jiffies_start = datagram->jiffies_sent;
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@@ -1810,34 +1951,39 @@
CoE state: UP CHECK.
*/
-void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_up_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
@@ -1863,11 +2009,12 @@
/** Prepare an SDO upload segment request.
*/
void ec_fsm_coe_up_prepare_segment_request(
- ec_fsm_coe_t *fsm /**< Finite state machine */
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data =
- ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram, 0x03, 10);
+ ec_slave_mbox_prepare_send(fsm->slave, datagram, 0x03, 10);
if (IS_ERR(data)) {
fsm->request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -1892,9 +2039,11 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_up_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint16_t rec_index;
@@ -1904,27 +2053,29 @@
unsigned int expedited, size_specified;
int ret;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"
" datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -2069,7 +2220,7 @@
if (data_size < fsm->complete_size) {
EC_SLAVE_DBG(slave, 1, "SDO data incomplete (%zu / %u)."
" Segmenting...\n", data_size, fsm->complete_size);
- ec_fsm_coe_up_prepare_segment_request(fsm);
+ ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
@@ -2091,33 +2242,37 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_up_seg_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
" request datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
" request failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@@ -2130,35 +2285,40 @@
CoE state: UP CHECK.
*/
-void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_up_seg_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
" datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"
" failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
@@ -2185,9 +2345,11 @@
\todo Timeout behavior
*/
-void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_up_seg_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot;
@@ -2195,28 +2357,30 @@
ec_sdo_request_t *request = fsm->request;
unsigned int last_segment;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
" response datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
" response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -2299,7 +2463,7 @@
last_segment = EC_READ_U8(data + 2) & 0x01;
if (!last_segment) {
fsm->toggle = !fsm->toggle;
- ec_fsm_coe_up_prepare_segment_request(fsm);
+ ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
@@ -2326,7 +2490,10 @@
State: ERROR.
*/
-void ec_fsm_coe_error(ec_fsm_coe_t *fsm /**< finite state machine */)
+void ec_fsm_coe_error(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
}
@@ -2336,8 +2503,11 @@
State: END.
*/
-void ec_fsm_coe_end(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
+void ec_fsm_coe_end(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/*****************************************************************************/
--- a/master/fsm_coe.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_coe.h Thu Jan 10 17:36:41 2013 +0100
@@ -51,10 +51,10 @@
*/
struct ec_fsm_coe {
ec_slave_t *slave; /**< slave the FSM runs on */
- ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
- void (*state)(ec_fsm_coe_t *); /**< CoE state function */
+ void (*state)(ec_fsm_coe_t *, ec_datagram_t *); /**< CoE state function */
+ ec_datagram_t *datagram; /**< Datagram used in last step. */
unsigned long jiffies_start; /**< CoE timestamp. */
ec_sdo_t *sdo; /**< current SDO */
uint8_t subindex; /**< current subindex */
@@ -63,18 +63,19 @@
uint8_t toggle; /**< toggle bit for segment commands */
uint32_t offset; /**< Data offset during segmented download. */
uint32_t remaining; /**< Remaining bytes during segmented download. */
+ size_t segment_size; /**< Current segment size. */
};
/*****************************************************************************/
-void ec_fsm_coe_init(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_init(ec_fsm_coe_t *);
void ec_fsm_coe_clear(ec_fsm_coe_t *);
void ec_fsm_coe_dictionary(ec_fsm_coe_t *, ec_slave_t *);
void ec_fsm_coe_transfer(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_request_t *);
-int ec_fsm_coe_exec(ec_fsm_coe_t *);
-int ec_fsm_coe_success(ec_fsm_coe_t *);
+int ec_fsm_coe_exec(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_success(const ec_fsm_coe_t *);
/*****************************************************************************/
--- a/master/fsm_foe.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_foe.c Thu Jan 10 17:36:41 2013 +0100
@@ -74,44 +74,42 @@
/*****************************************************************************/
-int ec_foe_prepare_data_send(ec_fsm_foe_t *);
-int ec_foe_prepare_wrq_send(ec_fsm_foe_t *);
-int ec_foe_prepare_rrq_send(ec_fsm_foe_t *);
-int ec_foe_prepare_send_ack(ec_fsm_foe_t *);
+int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);
void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);
void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);
-void ec_fsm_foe_write(ec_fsm_foe_t *);
-void ec_fsm_foe_read(ec_fsm_foe_t *);
-void ec_fsm_foe_end(ec_fsm_foe_t *);
-void ec_fsm_foe_error(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *);
-void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *);
-void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_data_check(ec_fsm_foe_t *);
-void ec_fsm_foe_state_data_read(ec_fsm_foe_t *);
-void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *);
-
-void ec_fsm_foe_write_start(ec_fsm_foe_t *);
-void ec_fsm_foe_read_start(ec_fsm_foe_t *);
+void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);
/*****************************************************************************/
/** Constructor.
*/
-void ec_fsm_foe_init(ec_fsm_foe_t *fsm, /**< finite state machine */
- ec_datagram_t *datagram /**< datagram */
- )
-{
- fsm->state = NULL;
- fsm->datagram = datagram;
+void ec_fsm_foe_init(
+ ec_fsm_foe_t *fsm /**< finite state machine */
+ )
+{
+ fsm->state = NULL;
+ fsm->datagram = NULL;
}
/*****************************************************************************/
@@ -125,13 +123,36 @@
/*****************************************************************************/
/** Executes the current state of the state machine.
- * \return false, if state machine has terminated
- */
-int ec_fsm_foe_exec(ec_fsm_foe_t *fsm /**< finite state machine */)
-{
- fsm->state(fsm);
-
- return fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
+ *
+ * \return 1, if the datagram was used, else 0.
+ */
+int ec_fsm_foe_exec(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
}
/*****************************************************************************/
@@ -139,7 +160,7 @@
/** Returns, if the state machine terminated with success.
* \return non-zero if successful.
*/
-int ec_fsm_foe_success(ec_fsm_foe_t *fsm /**< Finite state machine */)
+int ec_fsm_foe_success(const ec_fsm_foe_t *fsm /**< Finite state machine */)
{
return fsm->state == ec_fsm_foe_end;
}
@@ -156,11 +177,25 @@
{
fsm->slave = slave;
fsm->request = request;
+
if (request->dir == EC_DIR_OUTPUT) {
- fsm->state = ec_fsm_foe_write;
+ fsm->tx_buffer = fsm->request->buffer;
+ fsm->tx_buffer_size = fsm->request->data_size;
+ fsm->tx_buffer_offset = 0;
+
+ fsm->tx_filename = fsm->request->file_name;
+ fsm->tx_filename_len = strlen(fsm->tx_filename);
+
+ fsm->state = ec_fsm_foe_write_start;
}
else {
- fsm->state = ec_fsm_foe_read;
+ fsm->rx_buffer = fsm->request->buffer;
+ fsm->rx_buffer_size = fsm->request->buffer_size;
+
+ fsm->rx_filename = fsm->request->file_name;
+ fsm->rx_filename_len = strlen(fsm->rx_filename);
+
+ fsm->state = ec_fsm_foe_read_start;
}
}
@@ -168,7 +203,10 @@
/** State: ERROR.
*/
-void ec_fsm_foe_error(ec_fsm_foe_t *fsm /**< finite state machine */)
+void ec_fsm_foe_error(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
#ifdef DEBUG_FOE
printk("ec_fsm_foe_error()\n");
@@ -179,7 +217,10 @@
/** State: END.
*/
-void ec_fsm_foe_end(ec_fsm_foe_t *fsm /**< finite state machine */)
+void ec_fsm_foe_end(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
#ifdef DEBUG_FOE
printk("ec_fsm_foe_end\n");
@@ -190,7 +231,10 @@
/** Sends a file or the next fragment.
*/
-int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)
+int ec_foe_prepare_data_send(
+ ec_fsm_foe_t *fsm,
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
size_t remaining_size, current_size;
uint8_t *data;
@@ -207,13 +251,14 @@
}
data = ec_slave_mbox_prepare_send(fsm->slave,
- fsm->datagram, EC_MBOX_TYPE_FILEACCESS,
+ datagram, EC_MBOX_TYPE_FILEACCESS,
current_size + EC_FOE_HEADER_SIZE);
- if (IS_ERR(data))
+ if (IS_ERR(data)) {
return -1;
-
- EC_WRITE_U8 ( data, EC_FOE_OPCODE_DATA ); // OpCode = DataBlock req.
- EC_WRITE_U32( data + 2, fsm->tx_packet_no ); // PacketNo, Password
+ }
+
+ EC_WRITE_U8 (data, EC_FOE_OPCODE_DATA); // OpCode = DataBlock req.
+ EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password
memcpy(data + EC_FOE_HEADER_SIZE,
fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
@@ -226,7 +271,10 @@
/** Prepare a write request (WRQ) with filename
*/
-int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)
+int ec_foe_prepare_wrq_send(
+ ec_fsm_foe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
size_t current_size;
uint8_t *data;
@@ -238,10 +286,11 @@
current_size = fsm->tx_filename_len;
- data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
+ data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
- if (IS_ERR(data))
+ if (IS_ERR(data)) {
return -1;
+ }
EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
EC_WRITE_U32( data + 2, fsm->tx_packet_no );
@@ -253,27 +302,12 @@
/*****************************************************************************/
-/** Start a write operation.
- */
-void ec_fsm_foe_write(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- fsm->tx_buffer = fsm->request->buffer;
- fsm->tx_buffer_size = fsm->request->data_size;
- fsm->tx_buffer_offset = 0;
-
- fsm->tx_filename = fsm->request->file_name;
- fsm->tx_filename_len = strlen(fsm->tx_filename);
-
- fsm->state = ec_fsm_foe_write_start;
-}
-
-/*****************************************************************************/
-
/** Initializes the FoE write state machine.
*/
-void ec_fsm_foe_write_start(ec_fsm_foe_t *fsm /**< finite state machine */)
+void ec_fsm_foe_write_start(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
ec_slave_t *slave = fsm->slave;
@@ -292,7 +326,7 @@
return;
}
- if (ec_foe_prepare_wrq_send(fsm)) {
+ if (ec_foe_prepare_wrq_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
@@ -305,33 +339,33 @@
/** Check for acknowledge.
*/
void ec_fsm_foe_state_ack_check(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_ack_check()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
- // slave did not put anything in the mailbox yet
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
" failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ // slave did not put anything in the mailbox yet
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
@@ -357,10 +391,10 @@
/** Acknowledge a read operation.
*/
void ec_fsm_foe_state_ack_read(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
@@ -370,22 +404,22 @@
printk("ec_fsm_foe_ack_read()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!(data = ec_slave_mbox_fetch(fsm->slave, datagram,
- &mbox_prot, &rec_size))) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(fsm->slave, datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
@@ -401,7 +435,7 @@
if (opCode == EC_FOE_OPCODE_BUSY) {
// slave not ready
- if (ec_foe_prepare_data_send(fsm)) {
+ if (ec_foe_prepare_data_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
EC_SLAVE_ERR(slave, "Slave is busy.\n");
return;
@@ -419,7 +453,7 @@
return;
}
- if (ec_foe_prepare_data_send(fsm)) {
+ if (ec_foe_prepare_data_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
@@ -437,32 +471,32 @@
* fragment, if necessary.
*/
void ec_fsm_foe_state_wrq_sent(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_foe_state_sent_wrq()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE WRQ: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE WRQ failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
@@ -478,10 +512,10 @@
* fragment, if necessary.
*/
void ec_fsm_foe_state_data_sent(
- ec_fsm_foe_t *fsm /**< Foe statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< Foe statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
@@ -491,18 +525,18 @@
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
- ec_datagram_print_state(datagram);
+ ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_tx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE data send failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- ec_slave_mbox_prepare_check(fsm->slave, fsm->datagram);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(fsm->slave, datagram);
fsm->jiffies_start = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
@@ -512,17 +546,21 @@
/** Prepare a read request (RRQ) with filename
*/
-int ec_foe_prepare_rrq_send(ec_fsm_foe_t *fsm)
+int ec_foe_prepare_rrq_send(
+ ec_fsm_foe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
size_t current_size;
uint8_t *data;
current_size = fsm->rx_filename_len;
- data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
+ data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
- if (IS_ERR(data))
+ if (IS_ERR(data)) {
return -1;
+ }
EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
EC_WRITE_U32(data + 2, 0x00000000); // no passwd
@@ -541,18 +579,20 @@
/** Prepare to send an acknowledge.
*/
int ec_foe_prepare_send_ack(
- ec_fsm_foe_t *foe /**< FoE statemachine. */
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data;
- data = ec_slave_mbox_prepare_send(foe->slave, foe->datagram,
+ data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE);
- if (IS_ERR(data))
+ if (IS_ERR(data)) {
return -1;
+ }
EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
- EC_WRITE_U32(data + 2, foe->rx_expected_packet_no);
+ EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);
return 0;
}
@@ -561,36 +601,36 @@
/** State: RRQ SENT.
*
- * Checks is the previous transmit datagram succeded and sends the next
+ * Checks is the previous transmit datagram succeeded and sends the next
* fragment, if necessary.
*/
void ec_fsm_foe_state_rrq_sent(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_foe_state_rrq_sent()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
@@ -600,26 +640,11 @@
/*****************************************************************************/
-/** Start a read operation.
- */
-void ec_fsm_foe_read(
- ec_fsm_foe_t *fsm /**< FoE state machine. */
- )
-{
- fsm->state = ec_fsm_foe_read_start;
- fsm->rx_filename = fsm->request->file_name;
- fsm->rx_filename_len = strlen(fsm->rx_filename);
-
- fsm->rx_buffer = fsm->request->buffer;
- fsm->rx_buffer_size = fsm->request->buffer_size;
-}
-
-/*****************************************************************************/
-
/** Starting state for read operations.
*/
void ec_fsm_foe_read_start(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@@ -638,7 +663,7 @@
return;
}
- if (ec_foe_prepare_rrq_send(fsm)) {
+ if (ec_foe_prepare_rrq_send(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
return;
}
@@ -651,33 +676,34 @@
/** Check for data.
*/
void ec_fsm_foe_state_data_check(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_state_data_check()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
@@ -702,35 +728,35 @@
/** Start reading data.
*/
void ec_fsm_foe_state_data_read(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t rec_size;
uint8_t *data, opCode, packet_no, mbox_prot;
- ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_state_data_read()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- if (!(data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot,
- &rec_size))) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
}
@@ -745,7 +771,7 @@
opCode = EC_READ_U8(data);
if (opCode == EC_FOE_OPCODE_BUSY) {
- if (ec_foe_prepare_send_ack(fsm)) {
+ if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
}
return;
@@ -800,7 +826,7 @@
#ifdef DEBUG_FOE
printk ("last_packet=true\n");
#endif
- if (ec_foe_prepare_send_ack(fsm)) {
+ if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR);
return;
}
@@ -826,32 +852,32 @@
/** Sent an acknowledge.
*/
void ec_fsm_foe_state_sent_ack(
- ec_fsm_foe_t *fsm /**< FoE statemachine. */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_foe_state_sent_ack()\n");
#endif
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE ACK: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
// slave did not put anything into the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ACK failed: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
--- a/master/fsm_foe.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_foe.h Thu Jan 10 17:36:41 2013 +0100
@@ -2,7 +2,8 @@
*
* $Id$
*
- * Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
+ * Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
+ * 2009-2012 Florian Pose <fp@igh-essen.com>
*
* This file is part of the IgH EtherCAT Master.
*
@@ -50,15 +51,15 @@
/** Finite state machines for the CANopen-over-EtherCAT protocol.
*/
struct ec_fsm_foe {
- ec_slave_t *slave; /**< slave the FSM runs on */
- ec_datagram_t *datagram; /**< datagram used in the state machine */
- unsigned int retries; /**< retries upon datagram timeout */
+ ec_slave_t *slave; /**< Slave the FSM runs on. */
+ unsigned int retries; /**< Retries upon datagram timeout */
- void (*state)(ec_fsm_foe_t *); /**< FoE state function */
+ void (*state)(ec_fsm_foe_t *, ec_datagram_t *); /**< FoE state function. */
+ ec_datagram_t *datagram; /**< Datagram used in previous step. */
unsigned long jiffies_start; /**< FoE timestamp. */
- uint8_t subindex; /**< current subindex */
- ec_foe_request_t *request; /**< FoE request */
- uint8_t toggle; /**< toggle bit for segment commands */
+ uint8_t subindex; /**< Current subindex. */
+ ec_foe_request_t *request; /**< FoE request. */
+ uint8_t toggle; /**< Toggle bit for segment commands. */
uint8_t *tx_buffer; /**< Buffer with data to transmit. */
uint32_t tx_buffer_size; /**< Size of data to transmit. */
@@ -80,11 +81,11 @@
/*****************************************************************************/
-void ec_fsm_foe_init(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_init(ec_fsm_foe_t *);
void ec_fsm_foe_clear(ec_fsm_foe_t *);
-int ec_fsm_foe_exec(ec_fsm_foe_t *);
-int ec_fsm_foe_success(ec_fsm_foe_t *);
+int ec_fsm_foe_exec(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_fsm_foe_success(const ec_fsm_foe_t *);
void ec_fsm_foe_transfer(ec_fsm_foe_t *, ec_slave_t *, ec_foe_request_t *);
--- a/master/fsm_master.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_master.c Thu Jan 10 17:36:41 2013 +0100
@@ -85,8 +85,8 @@
ec_fsm_master_reset(fsm);
// init sub-state-machines
- ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
- ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
+ ec_fsm_coe_init(&fsm->fsm_coe);
+ ec_fsm_soe_init(&fsm->fsm_soe);
ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);
ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,
@@ -342,6 +342,7 @@
}
}
master->slave_count = count;
+ master->fsm_slave = master->slaves;
/* start with first device with slaves responding; at least one
* has responding slaves, otherwise count would be zero. */
@@ -465,7 +466,7 @@
fsm->slave = slave;
fsm->state = ec_fsm_master_state_sdo_request;
ec_fsm_coe_transfer(&fsm->fsm_coe, slave, req);
- ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram);
return 1;
}
}
@@ -495,7 +496,7 @@
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
- ec_fsm_slave_ready(&slave->fsm);
+ ec_fsm_slave_set_ready(&slave->fsm);
}
// check, if slaves have an SDO dictionary to read out.
@@ -520,7 +521,7 @@
fsm->slave = slave;
fsm->state = ec_fsm_master_state_sdo_dictionary;
ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);
- ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram); // execute immediately
fsm->datagram->device_index = fsm->slave->device_index;
return;
}
@@ -1138,7 +1139,7 @@
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
EC_SLAVE_ERR(slave, "Failed to write SII data.\n");
request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&master->request_queue);
+ wake_up_all(&master->request_queue);
ec_fsm_master_restart(fsm);
return;
}
@@ -1166,7 +1167,7 @@
// TODO: Evaluate other SII contents!
request->state = EC_INT_REQUEST_SUCCESS;
- wake_up(&master->request_queue);
+ wake_up_all(&master->request_queue);
// check for another SII write request
if (ec_fsm_master_action_process_sii(fsm))
@@ -1186,7 +1187,7 @@
ec_slave_t *slave = fsm->slave;
ec_master_t *master = fsm->master;
- if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
+ if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
return;
}
@@ -1226,7 +1227,7 @@
return;
}
- if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
+ if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
return;
}
@@ -1234,14 +1235,14 @@
EC_SLAVE_DBG(fsm->slave, 1,
"Failed to process internal SDO request.\n");
request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&fsm->master->request_queue);
+ wake_up_all(&fsm->master->request_queue);
ec_fsm_master_restart(fsm);
return;
}
// SDO request finished
request->state = EC_INT_REQUEST_SUCCESS;
- wake_up(&fsm->master->request_queue);
+ wake_up_all(&fsm->master->request_queue);
EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n");
--- a/master/fsm_pdo.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo.c Thu Jan 10 17:36:41 2013 +0100
@@ -27,8 +27,7 @@
*
*****************************************************************************/
-/** \file
- * EtherCAT PDO configuration state machine.
+/** \file EtherCAT PDO configuration state machine.
*/
/*****************************************************************************/
@@ -42,30 +41,30 @@
/*****************************************************************************/
-void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);
-void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);
+void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_t *);
/*****************************************************************************/
@@ -162,10 +161,12 @@
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_exec(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
- fsm->state(fsm);
+ ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ fsm->state(fsm, datagram);
+
return ec_fsm_pdo_running(fsm);
}
@@ -189,12 +190,13 @@
/** Start reading PDO assignment.
*/
void ec_fsm_pdo_read_state_start(
- ec_fsm_pdo_t *fsm /**< finite state machine */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
// read PDO assignment for first sync manager not reserved for mailbox
fsm->sync_index = 1; // next is 2
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@@ -202,7 +204,8 @@
/** Read PDO assignment of next sync manager.
*/
void ec_fsm_pdo_read_action_next_sync(
- ec_fsm_pdo_t *fsm /**< Finite state machine */
+ ec_fsm_pdo_t *fsm, /**< finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@@ -222,7 +225,7 @@
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_read_state_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
@@ -237,15 +240,18 @@
/** Count assigned PDOs.
*/
void ec_fsm_pdo_read_state_pdo_count(
- ec_fsm_pdo_t *fsm /**< finite state machine */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs"
" for SM%u.\n", fsm->sync_index);
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
@@ -253,7 +259,7 @@
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
" when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex);
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
fsm->pdo_count = EC_READ_U8(fsm->request.data);
@@ -262,7 +268,7 @@
// read first PDO
fsm->pdo_pos = 1;
- ec_fsm_pdo_read_action_next_pdo(fsm);
+ ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
}
/*****************************************************************************/
@@ -270,7 +276,8 @@
/** Read next PDO.
*/
void ec_fsm_pdo_read_action_next_pdo(
- ec_fsm_pdo_t *fsm /**< finite state machine */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (fsm->pdo_pos <= fsm->pdo_count) {
@@ -279,7 +286,7 @@
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_read_state_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
@@ -289,7 +296,7 @@
ec_pdo_list_clear_pdos(&fsm->pdos);
// next sync manager
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@@ -297,16 +304,19 @@
/** Fetch PDO information.
*/
void ec_fsm_pdo_read_state_pdo(
- ec_fsm_pdo_t *fsm /**< finite state machine */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read index of"
" assigned PDO %u from SM%u.\n",
fsm->pdo_pos, fsm->sync_index);
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
@@ -314,14 +324,14 @@
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
" when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex);
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
if (!(fsm->pdo = (ec_pdo_t *)
kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n");
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
@@ -335,7 +345,7 @@
fsm->state = ec_fsm_pdo_read_state_pdo_entries;
ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
- fsm->state(fsm); // execute immediately
+ fsm->state(fsm, datagram); // execute immediately
}
/*****************************************************************************/
@@ -343,22 +353,24 @@
/** Fetch PDO information.
*/
void ec_fsm_pdo_read_state_pdo_entries(
- ec_fsm_pdo_t *fsm /**< finite state machine */
- )
-{
- if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
- return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+ return;
+ }
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries"
" for PDO 0x%04X.\n", fsm->pdo->index);
- ec_fsm_pdo_read_action_next_sync(fsm);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
// next PDO
fsm->pdo_pos++;
- ec_fsm_pdo_read_action_next_pdo(fsm);
+ ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
}
/******************************************************************************
@@ -368,7 +380,8 @@
/** Start PDO configuration.
*/
void ec_fsm_pdo_conf_state_start(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (!fsm->slave->config) {
@@ -377,7 +390,7 @@
}
fsm->sync_index = 1; // next is 2
- ec_fsm_pdo_conf_action_next_sync(fsm);
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@@ -400,7 +413,8 @@
/** Get the next sync manager for a pdo configuration.
*/
void ec_fsm_pdo_conf_action_next_sync(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
fsm->sync_index++;
@@ -432,11 +446,11 @@
if (!(fsm->pdo =
ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
// no pdos configured
- ec_fsm_pdo_conf_action_check_assignment(fsm);
+ ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
return;
}
- ec_fsm_pdo_conf_action_pdo_mapping(fsm);
+ ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
return;
}
@@ -448,7 +462,8 @@
/** Check if the mapping has to be read, otherwise start to configure it.
*/
void ec_fsm_pdo_conf_action_pdo_mapping(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
const ec_pdo_t *assigned_pdo;
@@ -469,12 +484,12 @@
ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
&fsm->slave_pdo);
fsm->state = ec_fsm_pdo_conf_state_read_mapping;
- fsm->state(fsm); // execute immediately
+ fsm->state(fsm, datagram); // execute immediately
return;
}
// pdo mapping is known, check if it most be re-configured
- ec_fsm_pdo_conf_action_check_mapping(fsm);
+ ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
}
/*****************************************************************************/
@@ -482,11 +497,13 @@
/** Execute the PDO entry state machine to read the current PDO's mapping.
*/
void ec_fsm_pdo_conf_state_read_mapping(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
- if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
- return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+ return;
+ }
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
EC_SLAVE_WARN(fsm->slave,
@@ -494,7 +511,7 @@
fsm->pdo->index);
// check if the mapping must be re-configured
- ec_fsm_pdo_conf_action_check_mapping(fsm);
+ ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
}
/*****************************************************************************/
@@ -504,7 +521,8 @@
* \todo Display mapping differences.
*/
void ec_fsm_pdo_conf_action_check_mapping(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
// check, if slave supports PDO configuration
@@ -516,7 +534,7 @@
ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
fsm->pdo, &fsm->slave_pdo);
fsm->state = ec_fsm_pdo_conf_state_mapping;
- fsm->state(fsm); // execure immediately
+ fsm->state(fsm, datagram); // execure immediately
return;
}
else if (!ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
@@ -530,7 +548,7 @@
printk("\n");
}
- ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
+ ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
}
/*****************************************************************************/
@@ -538,18 +556,20 @@
/** Let the PDO entry state machine configure the current PDO's mapping.
*/
void ec_fsm_pdo_conf_state_mapping(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
- if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
- return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+ return;
+ }
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
EC_SLAVE_WARN(fsm->slave,
"Failed to configure mapping of PDO 0x%04X.\n",
fsm->pdo->index);
- ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
+ ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
}
/*****************************************************************************/
@@ -557,17 +577,18 @@
/** Check mapping of next PDO, otherwise configure assignment.
*/
void ec_fsm_pdo_conf_action_next_pdo_mapping(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
// get next configured PDO
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
// no more configured pdos
- ec_fsm_pdo_conf_action_check_assignment(fsm);
- return;
- }
-
- ec_fsm_pdo_conf_action_pdo_mapping(fsm);
+ ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
+ return;
+ }
+
+ ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
}
/*****************************************************************************/
@@ -575,7 +596,8 @@
/** Check if the PDO assignment of the current SM has to be re-configured.
*/
void ec_fsm_pdo_conf_action_check_assignment(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
@@ -605,7 +627,7 @@
fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
@@ -613,7 +635,7 @@
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
}
- ec_fsm_pdo_conf_action_next_sync(fsm);
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@@ -621,17 +643,20 @@
/** Set the number of assigned PDOs to zero.
*/
void ec_fsm_pdo_conf_state_zero_pdo_count(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe))
- return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n",
fsm->sync_index);
- EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
- ec_fsm_pdo_conf_action_next_sync(fsm);
+ EC_SLAVE_WARN(fsm->slave, "");
+ ec_fsm_pdo_print(fsm);
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
return;
}
@@ -642,15 +667,14 @@
// find first PDO
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
-
// check for mapping to be altered
- ec_fsm_pdo_conf_action_next_sync(fsm);
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
return;
}
// assign first PDO
fsm->pdo_pos = 1;
- ec_fsm_pdo_conf_action_assign_pdo(fsm);
+ ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
}
/*****************************************************************************/
@@ -658,7 +682,8 @@
/** Assign a PDO.
*/
void ec_fsm_pdo_conf_action_assign_pdo(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
EC_WRITE_U16(fsm->request.data, fsm->pdo->index);
@@ -672,7 +697,7 @@
fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@@ -680,10 +705,13 @@
/** Add a PDO to the sync managers PDO assignment.
*/
void ec_fsm_pdo_conf_state_assign_pdo(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u"
@@ -696,7 +724,6 @@
// find next PDO
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
-
// no more PDOs to assign, set PDO count
EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
fsm->request.data_size = 1;
@@ -709,13 +736,13 @@
fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
// add next PDO to assignment
fsm->pdo_pos++;
- ec_fsm_pdo_conf_action_assign_pdo(fsm);
+ ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
}
/*****************************************************************************/
@@ -723,10 +750,13 @@
/** Set the number of assigned PDOs.
*/
void ec_fsm_pdo_conf_state_set_pdo_count(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to set number of"
@@ -743,7 +773,7 @@
" PDO assignment of SM%u.\n", fsm->sync_index);
// check if PDO mapping has to be altered
- ec_fsm_pdo_conf_action_next_sync(fsm);
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
}
/******************************************************************************
@@ -753,7 +783,8 @@
/** State: ERROR.
*/
void ec_fsm_pdo_state_error(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
}
@@ -763,9 +794,10 @@
/** State: END.
*/
void ec_fsm_pdo_state_end(
- ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
- )
-{
-}
-
-/*****************************************************************************/
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/*****************************************************************************/
--- a/master/fsm_pdo.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo.h Thu Jan 10 17:36:41 2013 +0100
@@ -53,7 +53,7 @@
*/
struct ec_fsm_pdo
{
- void (*state)(ec_fsm_pdo_t *); /**< State function. */
+ void (*state)(ec_fsm_pdo_t *, ec_datagram_t *); /**< State function. */
ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */
ec_fsm_pdo_entry_t fsm_pdo_entry; /**< PDO entry state machine. */
ec_pdo_list_t pdos; /**< PDO configuration. */
@@ -76,7 +76,7 @@
void ec_fsm_pdo_start_reading(ec_fsm_pdo_t *, ec_slave_t *);
void ec_fsm_pdo_start_configuration(ec_fsm_pdo_t *, ec_slave_t *);
-int ec_fsm_pdo_exec(ec_fsm_pdo_t *);
+int ec_fsm_pdo_exec(ec_fsm_pdo_t *, ec_datagram_t *);
int ec_fsm_pdo_success(const ec_fsm_pdo_t *);
/*****************************************************************************/
--- a/master/fsm_pdo_entry.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo_entry.c Thu Jan 10 17:36:41 2013 +0100
@@ -42,21 +42,24 @@
/*****************************************************************************/
-void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);
+void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *,
+ ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *,
+ ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *,
+ ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *, ec_datagram_t *);
/*****************************************************************************/
@@ -87,7 +90,7 @@
/** Print the current and desired PDO mapping.
*/
void ec_fsm_pdo_entry_print(
- ec_fsm_pdo_entry_t *fsm /**< PDO configuration state machine. */
+ ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
)
{
printk("Currently mapped PDO entries: ");
@@ -103,7 +106,7 @@
*/
void ec_fsm_pdo_entry_start_reading(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
- ec_slave_t *slave, /**< slave to configure */
+ ec_slave_t *slave, /**< Slave to configure. */
ec_pdo_t *pdo /**< PDO to read entries for. */
)
{
@@ -121,7 +124,7 @@
*/
void ec_fsm_pdo_entry_start_configuration(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
- ec_slave_t *slave, /**< slave to configure */
+ ec_slave_t *slave, /**< Slave to configure. */
const ec_pdo_t *pdo, /**< PDO with the desired entries. */
const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
)
@@ -160,10 +163,12 @@
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_entry_exec(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
- )
-{
- fsm->state(fsm);
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ fsm->state(fsm, datagram);
+
return ec_fsm_pdo_entry_running(fsm);
}
@@ -187,7 +192,8 @@
/** Request reading the number of mapped PDO entries.
*/
void ec_fsm_pdo_entry_read_state_start(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, 0);
@@ -195,7 +201,7 @@
fsm->state = ec_fsm_pdo_entry_read_state_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@@ -203,11 +209,13 @@
/** Read number of mapped PDO entries.
*/
void ec_fsm_pdo_entry_read_state_count(
- ec_fsm_pdo_entry_t *fsm /**< finite state machine */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe))
- return;
+ ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave,
@@ -231,7 +239,7 @@
// read first PDO entry
fsm->entry_pos = 1;
- ec_fsm_pdo_entry_read_action_next(fsm);
+ ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
/*****************************************************************************/
@@ -239,7 +247,8 @@
/** Read next PDO entry.
*/
void ec_fsm_pdo_entry_read_action_next(
- ec_fsm_pdo_entry_t *fsm /**< finite state machine */
+ ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (fsm->entry_pos <= fsm->entry_count) {
@@ -248,7 +257,7 @@
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_entry_read_state_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
@@ -261,10 +270,13 @@
/** Read PDO entry information.
*/
void ec_fsm_pdo_entry_read_state_entry(
- ec_fsm_pdo_entry_t *fsm /**< finite state machine */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n");
@@ -315,7 +327,7 @@
// next PDO entry
fsm->entry_pos++;
- ec_fsm_pdo_entry_read_action_next(fsm);
+ ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
}
@@ -326,7 +338,8 @@
/** Start PDO mapping.
*/
void ec_fsm_pdo_entry_conf_state_start(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_sdo_request_alloc(&fsm->request, 4)) {
@@ -344,7 +357,7 @@
fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@@ -367,11 +380,13 @@
/** Set the number of mapped entries to zero.
*/
void ec_fsm_pdo_entry_conf_state_zero_entry_count(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe))
- return;
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");
@@ -392,7 +407,7 @@
// add first entry
fsm->entry_pos = 1;
- ec_fsm_pdo_entry_conf_action_map(fsm);
+ ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/*****************************************************************************/
@@ -400,7 +415,8 @@
/** Starts to add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_action_map(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint32_t value;
@@ -420,7 +436,7 @@
fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@@ -428,10 +444,13 @@
/** Add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_state_map_entry(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
@@ -458,13 +477,13 @@
fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
// add next entry
fsm->entry_pos++;
- ec_fsm_pdo_entry_conf_action_map(fsm);
+ ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/*****************************************************************************/
@@ -472,10 +491,13 @@
/** Set the number of entries.
*/
void ec_fsm_pdo_entry_conf_state_set_entry_count(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
- )
-{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");
@@ -497,7 +519,8 @@
/** State: ERROR.
*/
void ec_fsm_pdo_entry_state_error(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
)
{
}
@@ -507,9 +530,10 @@
/** State: END.
*/
void ec_fsm_pdo_entry_state_end(
- ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
- )
-{
-}
-
-/*****************************************************************************/
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/*****************************************************************************/
--- a/master/fsm_pdo_entry.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo_entry.h Thu Jan 10 17:36:41 2013 +0100
@@ -51,7 +51,8 @@
*/
struct ec_fsm_pdo_entry
{
- void (*state)(ec_fsm_pdo_entry_t *); /**< state function */
+ void (*state)(ec_fsm_pdo_entry_t *, ec_datagram_t *); /**< state function
+ */
ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */
ec_sdo_request_t request; /**< SDO request. */
@@ -74,7 +75,7 @@
void ec_fsm_pdo_entry_start_configuration(ec_fsm_pdo_entry_t *, ec_slave_t *,
const ec_pdo_t *, const ec_pdo_t *);
-int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *);
+int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *, ec_datagram_t *);
int ec_fsm_pdo_entry_success(const ec_fsm_pdo_entry_t *);
/*****************************************************************************/
--- a/master/fsm_slave.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave.c Thu Jan 10 17:36:41 2013 +0100
@@ -42,16 +42,16 @@
/*****************************************************************************/
-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_reg(ec_fsm_slave_t *);
-void ec_fsm_slave_state_reg_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 *);
/*****************************************************************************/
@@ -59,22 +59,23 @@
*/
void ec_fsm_slave_init(
ec_fsm_slave_t *fsm, /**< Slave state machine. */
- ec_slave_t *slave, /**< EtherCAT slave. */
- ec_datagram_t *datagram /**< Datagram object to use. */
+ ec_slave_t *slave /**< EtherCAT slave. */
)
{
fsm->slave = slave;
- fsm->datagram = datagram;
- fsm->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->datagram);
- ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
- ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
+ 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);
}
/*****************************************************************************/
@@ -85,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);
@@ -95,29 +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.
- */
-void ec_fsm_slave_exec(
- ec_fsm_slave_t *fsm /**< Slave state machine. */
- )
-{
- if (fsm->datagram->state == EC_DATAGRAM_SENT
- || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
- // datagram was not sent or received yet.
- return;
- }
-
- fsm->state(fsm);
-
- ec_datagram_output_stats(fsm->datagram);
+ * \return 1 if \a datagram was used, else 0.
+ */
+int ec_fsm_slave_exec(
+ 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. */
)
{
@@ -127,6 +155,17 @@
}
}
+/*****************************************************************************/
+
+/** 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
*****************************************************************************/
@@ -134,7 +173,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
@@ -145,26 +185,27 @@
/** 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;
}
}
@@ -176,7 +217,8 @@
* \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;
@@ -194,7 +236,7 @@
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;
}
@@ -202,22 +244,21 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+ ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
return 1;
}
@@ -226,21 +267,21 @@
/** 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)) {
- ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+ 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.\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;
@@ -250,7 +291,7 @@
// 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;
}
@@ -262,7 +303,8 @@
* \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;
@@ -295,7 +337,7 @@
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;
@@ -308,17 +350,16 @@
// 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;
- ec_master_queue_external_datagram(slave->master, fsm->datagram);
+ datagram->device_index = slave->device_index;
fsm->state = ec_fsm_slave_state_reg_request;
return 1;
}
@@ -328,7 +369,8 @@
/** 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;
@@ -337,6 +379,7 @@
if (!reg) {
// configuration was cleared in the meantime
fsm->state = ec_fsm_slave_state_ready;
+ fsm->reg_request = NULL;
return;
}
@@ -345,7 +388,7 @@
" 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;
@@ -366,7 +409,7 @@
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;
}
@@ -378,7 +421,8 @@
* \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;
@@ -396,20 +440,19 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+ ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
return 1;
}
@@ -418,21 +461,21 @@
/** 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)) {
- ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+ 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.\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;
@@ -443,7 +486,7 @@
" 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;
}
@@ -455,7 +498,8 @@
* \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;
@@ -473,7 +517,7 @@
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;
}
@@ -481,22 +525,21 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+ ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
return 1;
}
@@ -505,21 +548,21 @@
/** 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)) {
- ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+ 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->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;
@@ -529,7 +572,7 @@
// 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;
}
--- a/master/fsm_slave.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave.h Thu Jan 10 17:36:41 2013 +0100
@@ -53,27 +53,29 @@
*/
struct ec_fsm_slave {
ec_slave_t *slave; /**< slave the FSM runs on */
- ec_datagram_t *datagram; /**< datagram used in the state machine */
+ struct list_head list; /**< Used for execution list. */
- void (*state)(ec_fsm_slave_t *); /**< master state function */
+ void (*state)(ec_fsm_slave_t *, ec_datagram_t *); /**< State function. */
+ ec_datagram_t *datagram; /**< Previous state datagram. */
ec_sdo_request_t *sdo_request; /**< SDO request to process. */
ec_reg_request_t *reg_request; /**< Register request to process. */
ec_foe_request_t *foe_request; /**< FoE request to process. */
- off_t foe_index; /**< index to FoE write request data */
+ off_t foe_index; /**< Index to FoE write request data. */
ec_soe_request_t *soe_request; /**< SoE request to process. */
- ec_fsm_coe_t fsm_coe; /**< CoE state machine */
- ec_fsm_foe_t fsm_foe; /**< FoE state machine */
- ec_fsm_soe_t fsm_soe; /**< SoE state machine */
+ ec_fsm_coe_t fsm_coe; /**< CoE state machine. */
+ ec_fsm_foe_t fsm_foe; /**< FoE state machine. */
+ ec_fsm_soe_t fsm_soe; /**< SoE state machine. */
};
/*****************************************************************************/
-void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *);
void ec_fsm_slave_clear(ec_fsm_slave_t *);
-void ec_fsm_slave_exec(ec_fsm_slave_t *);
-void ec_fsm_slave_ready(ec_fsm_slave_t *);
+int ec_fsm_slave_exec(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_set_ready(ec_fsm_slave_t *);
+int ec_fsm_slave_is_ready(const ec_fsm_slave_t *);
/*****************************************************************************/
--- a/master/fsm_slave_config.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave_config.c Thu Jan 10 17:36:41 2013 +0100
@@ -679,7 +679,7 @@
ec_sdo_request_copy(&fsm->request_copy, fsm->request);
ecrt_sdo_request_write(&fsm->request_copy);
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
}
/*****************************************************************************/
@@ -690,7 +690,7 @@
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
- if (ec_fsm_coe_exec(fsm->fsm_coe)) {
+ if (ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram)) {
return;
}
@@ -713,7 +713,7 @@
ec_sdo_request_copy(&fsm->request_copy, fsm->request);
ecrt_sdo_request_write(&fsm->request_copy);
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
- ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+ ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
return;
}
@@ -746,7 +746,7 @@
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
- ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
@@ -765,7 +765,7 @@
{
ec_slave_t *slave = fsm->slave;
- if (ec_fsm_soe_exec(fsm->fsm_soe)) {
+ if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
return;
}
@@ -790,7 +790,7 @@
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
- ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
@@ -823,8 +823,9 @@
{
// TODO check for config here
- if (ec_fsm_pdo_exec(fsm->fsm_pdo))
- return;
+ if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
+ return;
+ }
if (!fsm->slave->config) { // config removed in the meantime
ec_fsm_slave_config_reconfigure(fsm);
@@ -1470,7 +1471,7 @@
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
- ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
@@ -1489,7 +1490,7 @@
{
ec_slave_t *slave = fsm->slave;
- if (ec_fsm_soe_exec(fsm->fsm_soe)) {
+ if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
return;
}
@@ -1514,7 +1515,7 @@
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
- ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
--- a/master/fsm_slave_scan.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave_scan.c Thu Jan 10 17:36:41 2013 +0100
@@ -894,7 +894,7 @@
EC_SLAVE_DBG(slave, 1, "Scanning PDO assignment and mapping.\n");
fsm->state = ec_fsm_slave_scan_state_pdos;
ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave);
- ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately
+ ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram); // execute immediately
}
/*****************************************************************************/
@@ -905,8 +905,9 @@
ec_fsm_slave_scan_t *fsm /**< slave state machine */
)
{
- if (ec_fsm_pdo_exec(fsm->fsm_pdo))
- return;
+ if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
+ return;
+ }
if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
fsm->state = ec_fsm_slave_scan_state_error;
--- a/master/fsm_soe.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_soe.c Thu Jan 10 17:36:41 2013 +0100
@@ -64,18 +64,18 @@
/*****************************************************************************/
-void ec_fsm_soe_read_start(ec_fsm_soe_t *);
-void ec_fsm_soe_read_request(ec_fsm_soe_t *);
-void ec_fsm_soe_read_check(ec_fsm_soe_t *);
-void ec_fsm_soe_read_response(ec_fsm_soe_t *);
-
-void ec_fsm_soe_write_start(ec_fsm_soe_t *);
-void ec_fsm_soe_write_request(ec_fsm_soe_t *);
-void ec_fsm_soe_write_check(ec_fsm_soe_t *);
-void ec_fsm_soe_write_response(ec_fsm_soe_t *);
-
-void ec_fsm_soe_end(ec_fsm_soe_t *);
-void ec_fsm_soe_error(ec_fsm_soe_t *);
+void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
/*****************************************************************************/
@@ -105,12 +105,12 @@
/** Constructor.
*/
void ec_fsm_soe_init(
- ec_fsm_soe_t *fsm, /**< finite state machine */
- ec_datagram_t *datagram /**< datagram */
+ ec_fsm_soe_t *fsm /**< finite state machine */
)
{
fsm->state = NULL;
- fsm->datagram = datagram;
+ fsm->datagram = NULL;
+ fsm->fragment_size = 0;
}
/*****************************************************************************/
@@ -135,6 +135,7 @@
{
fsm->slave = slave;
fsm->request = request;
+
if (request->dir == EC_DIR_OUTPUT) {
fsm->state = ec_fsm_soe_write_start;
} else {
@@ -144,26 +145,46 @@
/*****************************************************************************/
-/**
- Executes the current state of the state machine.
- \return false, if state machine has terminated
-*/
-
-int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- fsm->state(fsm);
-
- return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
-}
-
-/*****************************************************************************/
-
-/**
- Returns, if the state machine terminated with success.
- \return non-zero if successful.
-*/
-
-int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_soe_exec(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/*****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ *
+ * \return non-zero if successful.
+ */
+int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
{
return fsm->state == ec_fsm_soe_end;
}
@@ -191,32 +212,24 @@
* SoE read state machine
*****************************************************************************/
-/** SoE state: READ START.
- */
-void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+/** Prepare a read operation.
+ *
+ * \return 0 on success, otherwise a negative error code.
+ */
+int ec_fsm_soe_prepare_read(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint8_t *data;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *request = fsm->request;
- uint8_t *data;
-
- EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
- request->drive_no);
-
- if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
- EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
- fsm->state = ec_fsm_soe_error;
- ec_fsm_soe_print_error(fsm);
- return;
- }
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
EC_SOE_SIZE);
if (IS_ERR(data)) {
- fsm->state = ec_fsm_soe_error;
- ec_fsm_soe_print_error(fsm);
- return;
+ return PTR_ERR(data);
}
EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
@@ -228,51 +241,93 @@
ec_print_data(data, EC_SOE_SIZE);
}
- fsm->request->data_size = 0;
fsm->request->jiffies_sent = jiffies;
+ fsm->state = ec_fsm_soe_read_request;
+
+ return 0;
+}
+
+/*****************************************************************************/
+
+/** SoE state: READ START.
+ */
+void ec_fsm_soe_read_start(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *request = fsm->request;
+
+ EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
+ request->drive_no);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ request->data_size = 0;
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_soe_read_request;
+
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
}
/*****************************************************************************/
/** SoE state: READ REQUEST.
*/
-void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_read_request(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
- ec_datagram_print_state(datagram);
+ ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
- if (datagram->working_counter != 1) {
- if (!datagram->working_counter) {
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
return;
}
}
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read request"
" failed after %lu ms: ", diff_ms);
- ec_datagram_print_wc_error(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
@@ -282,34 +337,39 @@
/** CoE state: READ CHECK.
*/
-void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_soe_read_check(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
- ec_datagram_print_state(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (!ec_slave_mbox_check(datagram)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
@@ -333,9 +393,11 @@
/** SoE state: READ RESPONSE.
*/
-void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_read_response(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
@@ -343,26 +405,28 @@
size_t rec_size, data_size;
ec_soe_request_t *req = fsm->request;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
- ec_datagram_print_state(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
- ec_datagram_print_wc_error(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -435,7 +499,7 @@
if (incomplete) {
EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
" at offset %zu.\n", req->data_size);
- fsm->jiffies_start = datagram->jiffies_sent;
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
@@ -456,15 +520,15 @@
/** Write next fragment.
*/
void ec_fsm_soe_write_next_fragment(
- ec_fsm_soe_t *fsm /**< finite state machine */
- )
-{
- ec_datagram_t *datagram = fsm->datagram;
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
uint8_t incomplete, *data;
- size_t header_size, max_fragment_size, remaining_size, fragment_size;
+ size_t header_size, max_fragment_size, remaining_size;
uint16_t fragments_left;
header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
@@ -479,14 +543,14 @@
remaining_size = req->data_size - fsm->offset;
max_fragment_size = slave->configured_rx_mailbox_size - header_size;
incomplete = remaining_size > max_fragment_size;
- fragment_size = incomplete ? max_fragment_size : remaining_size;
- fragments_left = remaining_size / fragment_size - 1;
- if (remaining_size % fragment_size) {
+ fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
+ fragments_left = remaining_size / fsm->fragment_size - 1;
+ if (remaining_size % fsm->fragment_size) {
fragments_left++;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
- EC_SOE_SIZE + fragment_size);
+ EC_SOE_SIZE + fsm->fragment_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -497,16 +561,14 @@
(req->drive_no & 0x07) << 5);
EC_WRITE_U8(data + 1, 1 << 6); // only value included
EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
- memcpy(data + 4, req->data + fsm->offset, fragment_size);
- fsm->offset += fragment_size;
+ memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size);
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
- ec_print_data(data, EC_SOE_SIZE + fragment_size);
+ ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
}
req->jiffies_sent = jiffies;
- fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_request;
}
@@ -514,7 +576,10 @@
/** SoE state: WRITE START.
*/
-void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
+void ec_fsm_soe_write_start(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *req = fsm->request;
@@ -530,48 +595,54 @@
}
fsm->offset = 0;
- ec_fsm_soe_write_next_fragment(fsm);
+ fsm->retries = EC_FSM_RETRIES;
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
}
/*****************************************************************************/
/** SoE state: WRITE REQUEST.
*/
-void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_write_request(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
- ec_datagram_print_state(datagram);
+ ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
- if (datagram->working_counter != 1) {
- if (!datagram->working_counter) {
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
return;
}
}
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write request"
" failed after %lu ms: ", diff_ms);
- ec_datagram_print_wc_error(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- fsm->jiffies_start = datagram->jiffies_sent;
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@@ -582,64 +653,65 @@
/** CoE state: WRITE CHECK.
*/
-void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
- ec_slave_t *slave = fsm->slave;
- ec_soe_request_t *req = fsm->request;
-
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
- return;
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_soe_write_check(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
- ec_datagram_print_state(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
- ec_datagram_print_wc_error(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (fsm->offset < req->data_size) {
- ec_fsm_soe_write_next_fragment(fsm);
- } else {
- if (!ec_slave_mbox_check(datagram)) {
- unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
- if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
- fsm->state = ec_fsm_soe_error;
- EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
- " for write response.\n", diff_ms);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- fsm->retries = EC_FSM_RETRIES;
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+ " for write response.\n", diff_ms);
+ ec_fsm_soe_print_error(fsm);
return;
}
- // Fetch response
- ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_soe_write_response;
- }
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_response;
}
/*****************************************************************************/
/** SoE state: WRITE RESPONSE.
*/
-void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_write_response(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
@@ -647,27 +719,29 @@
uint16_t idn;
size_t rec_size;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
return; // FIXME: request again?
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write"
" response datagram: ");
- ec_datagram_print_state(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (datagram->working_counter != 1) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
- ec_datagram_print_wc_error(datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -689,7 +763,7 @@
if (rec_size < EC_SOE_SIZE) {
fsm->state = ec_fsm_soe_error;
- EC_SLAVE_ERR(slave, "Received currupted SoE write response"
+ EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
" (%zu bytes)!\n", rec_size);
ec_print_data(data, rec_size);
ec_fsm_soe_print_error(fsm);
@@ -735,14 +809,24 @@
req->error_code = 0x0000;
}
- fsm->state = ec_fsm_soe_end; // success
+ fsm->offset += fsm->fragment_size;
+
+ if (fsm->offset < req->data_size) {
+ fsm->retries = EC_FSM_RETRIES;
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ } else {
+ fsm->state = ec_fsm_soe_end; // success
+ }
}
/*****************************************************************************/
/** State: ERROR.
*/
-void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
+void ec_fsm_soe_error(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
}
@@ -750,8 +834,11 @@
/** State: END.
*/
-void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
+void ec_fsm_soe_end(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/*****************************************************************************/
--- a/master/fsm_soe.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_soe.h Thu Jan 10 17:36:41 2013 +0100
@@ -50,24 +50,25 @@
*/
struct ec_fsm_soe {
ec_slave_t *slave; /**< slave the FSM runs on */
- ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
- void (*state)(ec_fsm_soe_t *); /**< CoE state function */
- unsigned long jiffies_start; /**< CoE timestamp. */
+ void (*state)(ec_fsm_soe_t *, ec_datagram_t *); /**< CoE state function */
+ ec_datagram_t *datagram; /**< Datagram used in the previous step. */
+ unsigned long jiffies_start; /**< Timestamp. */
ec_soe_request_t *request; /**< SoE request */
off_t offset; /**< IDN data offset during fragmented write. */
+ size_t fragment_size; /**< Size of the current fragment. */
};
/*****************************************************************************/
-void ec_fsm_soe_init(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_init(ec_fsm_soe_t *);
void ec_fsm_soe_clear(ec_fsm_soe_t *);
void ec_fsm_soe_transfer(ec_fsm_soe_t *, ec_slave_t *, ec_soe_request_t *);
-int ec_fsm_soe_exec(ec_fsm_soe_t *);
-int ec_fsm_soe_success(ec_fsm_soe_t *);
+int ec_fsm_soe_exec(ec_fsm_soe_t *, ec_datagram_t *);
+int ec_fsm_soe_success(const ec_fsm_soe_t *);
/*****************************************************************************/
--- a/master/ioctl.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/ioctl.c Thu Jan 10 17:36:41 2013 +0100
@@ -1753,12 +1753,11 @@
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
- if (unlikely(!ctx->requested))
- return -EPERM;
-
- down(&master->io_sem);
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
ecrt_master_send(master);
- up(&master->io_sem);
return 0;
}
@@ -1772,12 +1771,11 @@
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
- if (unlikely(!ctx->requested))
- return -EPERM;
-
- down(&master->io_sem);
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
ecrt_master_receive(master);
- up(&master->io_sem);
return 0;
}
@@ -1864,12 +1862,11 @@
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
- if (unlikely(!ctx->requested))
- return -EPERM;
-
- down(&master->io_sem);
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
ecrt_master_sync_reference_clock(master);
- up(&master->io_sem);
return 0;
}
@@ -1883,12 +1880,11 @@
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
- if (unlikely(!ctx->requested))
- return -EPERM;
-
- down(&master->io_sem);
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
ecrt_master_sync_slave_clocks(master);
- up(&master->io_sem);
return 0;
}
@@ -1931,12 +1927,11 @@
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
- if (unlikely(!ctx->requested))
- return -EPERM;
-
- down(&master->io_sem);
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
ecrt_master_sync_monitor_queue(master);
- up(&master->io_sem);
return 0;
}
--- a/master/master.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/master.c Thu Jan 10 17:36:41 2013 +0100
@@ -136,7 +136,7 @@
)
{
int ret;
- unsigned int dev_idx;
+ unsigned int dev_idx, i;
master->index = index;
master->reserved = 0;
@@ -193,11 +193,23 @@
INIT_LIST_HEAD(&master->ext_datagram_queue);
sema_init(&master->ext_queue_sem, 1);
- INIT_LIST_HEAD(&master->external_datagram_queue);
+ master->ext_ring_idx_rt = 0;
+ master->ext_ring_idx_fsm = 0;
+
+ // init external datagram ring
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_t *datagram = &master->ext_datagram_ring[i];
+ ec_datagram_init(datagram);
+ snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, "ext-%u", i);
+ }
// send interval in IDLE phase
ec_master_set_send_interval(master, 1000000 / HZ);
+ master->fsm_slave = NULL;
+ INIT_LIST_HEAD(&master->fsm_exec_list);
+ master->fsm_exec_count = 0U;
+
master->debug_level = debug_level;
master->stats.timeouts = 0;
master->stats.corrupted = 0;
@@ -245,6 +257,17 @@
// create state machine object
ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
+ // alloc external datagram ring
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_t *datagram = &master->ext_datagram_ring[i];
+ ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE);
+ if (ret) {
+ EC_MASTER_ERR(master, "Failed to allocate external"
+ " datagram %u.\n", i);
+ goto out_clear_ext_datagrams;
+ }
+ }
+
// init reference sync datagram
ec_datagram_init(&master->ref_sync_datagram);
snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE,
@@ -254,7 +277,7 @@
ec_datagram_clear(&master->ref_sync_datagram);
EC_MASTER_ERR(master, "Failed to allocate reference"
" synchronisation datagram.\n");
- goto out_clear_fsm;
+ goto out_clear_ext_datagrams;
}
// init sync datagram
@@ -337,7 +360,10 @@
ec_datagram_clear(&master->sync_datagram);
out_clear_ref_sync:
ec_datagram_clear(&master->ref_sync_datagram);
-out_clear_fsm:
+out_clear_ext_datagrams:
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_clear(&master->ext_datagram_ring[i]);
+ }
ec_fsm_master_clear(&master->fsm);
ec_datagram_clear(&master->fsm_datagram);
out_clear_devices:
@@ -355,7 +381,7 @@
ec_master_t *master /**< EtherCAT master */
)
{
- unsigned int dev_idx;
+ unsigned int dev_idx, i;
#ifdef EC_RTDM
ec_rtdm_dev_clear(&master->rtdm_dev);
@@ -379,6 +405,11 @@
ec_datagram_clear(&master->sync_mon_datagram);
ec_datagram_clear(&master->sync_datagram);
ec_datagram_clear(&master->ref_sync_datagram);
+
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_clear(&master->ext_datagram_ring[i]);
+ }
+
ec_fsm_master_clear(&master->fsm);
ec_datagram_clear(&master->fsm_datagram);
@@ -446,8 +477,12 @@
EC_MASTER_WARN(master, "Discarding SII request, slave %u about"
" to be deleted.\n", request->slave->ring_position);
request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&master->request_queue);
- }
+ wake_up_all(&master->request_queue);
+ }
+
+ master->fsm_slave = NULL;
+ INIT_LIST_HEAD(&master->fsm_exec_list);
+ master->fsm_exec_count = 0;
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
@@ -743,75 +778,104 @@
ec_master_t *master /**< EtherCAT master */
)
{
- ec_datagram_t *datagram, *n;
- size_t queue_size = 0;
+ ec_datagram_t *datagram;
+ size_t queue_size = 0, new_queue_size = 0;
+#if DEBUG_INJECT
+ unsigned int datagram_count = 0;
+#endif
+
+ if (master->ext_ring_idx_rt == master->ext_ring_idx_fsm) {
+ // nothing to inject
+ return;
+ }
list_for_each_entry(datagram, &master->datagram_queue, queue) {
- queue_size += datagram->data_size;
- }
-
- list_for_each_entry_safe(datagram, n, &master->external_datagram_queue,
- queue) {
- queue_size += datagram->data_size;
- if (queue_size <= master->max_queue_size) {
- list_del_init(&datagram->queue);
+ if (datagram->state == EC_DATAGRAM_QUEUED) {
+ queue_size += datagram->data_size;
+ }
+ }
+
#if DEBUG_INJECT
- EC_MASTER_DBG(master, 0, "Injecting external datagram %08x"
- " size=%u, queue_size=%u\n", (unsigned int) datagram,
- datagram->data_size, queue_size);
+ EC_MASTER_DBG(master, 1, "Injecting datagrams, queue_size=%zu\n",
+ queue_size);
+#endif
+
+ while (master->ext_ring_idx_rt != master->ext_ring_idx_fsm) {
+ datagram = &master->ext_datagram_ring[master->ext_ring_idx_rt];
+
+ if (datagram->state != EC_DATAGRAM_INIT) {
+ // skip datagram
+ master->ext_ring_idx_rt =
+ (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
+ continue;
+ }
+
+ new_queue_size = queue_size + datagram->data_size;
+ if (new_queue_size <= master->max_queue_size) {
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Injecting datagram %s"
+ " size=%zu, queue_size=%zu\n", datagram->name,
+ datagram->data_size, new_queue_size);
+ datagram_count++;
#endif
#ifdef EC_HAVE_CYCLES
datagram->cycles_sent = 0;
#endif
datagram->jiffies_sent = 0;
ec_master_queue_datagram(master, datagram);
- } else {
- if (datagram->data_size > master->max_queue_size) {
- list_del_init(&datagram->queue);
+ queue_size = new_queue_size;
+ }
+ else if (datagram->data_size > master->max_queue_size) {
+ datagram->state = EC_DATAGRAM_ERROR;
+ EC_MASTER_ERR(master, "External datagram %s is too large,"
+ " size=%zu, max_queue_size=%zu\n",
+ datagram->name, datagram->data_size,
+ master->max_queue_size);
+ }
+ else { // datagram does not fit in the current cycle
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_now = get_cycles();
+
+ if (cycles_now - datagram->cycles_sent
+ > ext_injection_timeout_cycles)
+#else
+ if (jiffies - datagram->jiffies_sent
+ > ext_injection_timeout_jiffies)
+#endif
+ {
+ unsigned int time_us;
+
datagram->state = EC_DATAGRAM_ERROR;
- EC_MASTER_ERR(master, "External datagram %p is too large,"
- " size=%zu, max_queue_size=%zu\n",
- datagram, datagram->data_size,
- master->max_queue_size);
- } else {
#ifdef EC_HAVE_CYCLES
- cycles_t cycles_now = get_cycles();
-
- if (cycles_now - datagram->cycles_sent
- > ext_injection_timeout_cycles)
+ time_us = (unsigned int)
+ ((cycles_now - datagram->cycles_sent) * 1000LL)
+ / cpu_khz;
#else
- if (jiffies - datagram->jiffies_sent
- > ext_injection_timeout_jiffies)
-#endif
- {
- unsigned int time_us;
-
- list_del_init(&datagram->queue);
- datagram->state = EC_DATAGRAM_ERROR;
-#ifdef EC_HAVE_CYCLES
- time_us = (unsigned int)
- ((cycles_now - datagram->cycles_sent) * 1000LL)
- / cpu_khz;
-#else
- time_us = (unsigned int)
- ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
-#endif
- EC_MASTER_ERR(master, "Timeout %u us: Injecting"
- " external datagram %p size=%zu,"
- " max_queue_size=%zu\n", time_us, datagram,
- datagram->data_size, master->max_queue_size);
- }
+ time_us = (unsigned int)
+ ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
+#endif
+ EC_MASTER_ERR(master, "Timeout %u us: Injecting"
+ " external datagram %s size=%zu,"
+ " max_queue_size=%zu\n", time_us, datagram->name,
+ datagram->data_size, master->max_queue_size);
+ }
+ else {
#if DEBUG_INJECT
- else {
- EC_MASTER_DBG(master, 0, "Deferred injecting"
- " of external datagram %p"
- " size=%u, queue_size=%u\n",
- datagram, datagram->data_size, queue_size);
- }
-#endif
+ EC_MASTER_DBG(master, 1, "Deferred injecting"
+ " external datagram %s size=%u, queue_size=%u\n",
+ datagram->name, datagram->data_size, queue_size);
+#endif
+ break;
}
}
- }
+
+ master->ext_ring_idx_rt =
+ (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
+ }
+
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Injected %u datagrams.\n", datagram_count);
+#endif
}
/*****************************************************************************/
@@ -832,42 +896,21 @@
/*****************************************************************************/
-/** Places an external datagram in the sdo datagram queue.
- */
-void ec_master_queue_external_datagram(
- ec_master_t *master, /**< EtherCAT master */
- ec_datagram_t *datagram /**< datagram */
- )
-{
- ec_datagram_t *queued_datagram;
-
- down(&master->io_sem);
-
- // check, if the datagram is already queued
- list_for_each_entry(queued_datagram, &master->external_datagram_queue,
- queue) {
- if (queued_datagram == datagram) {
- up(&master->io_sem);
- datagram->state = EC_DATAGRAM_QUEUED;
- return;
- }
- }
-
-#if DEBUG_INJECT
- EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n",
- datagram, datagram->data_size);
-#endif
-
- list_add_tail(&datagram->queue, &master->external_datagram_queue);
- datagram->state = EC_DATAGRAM_QUEUED;
-#ifdef EC_HAVE_CYCLES
- datagram->cycles_sent = get_cycles();
-#endif
- datagram->jiffies_sent = jiffies;
-
- up(&master->io_sem);
-
- master->fsm.idle = 0;
+/** Searches for a free datagram in the external datagram ring.
+ */
+ec_datagram_t *ec_master_get_external_datagram(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ if ((master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE !=
+ master->ext_ring_idx_rt) {
+ ec_datagram_t *datagram =
+ &master->ext_datagram_ring[master->ext_ring_idx_fsm];
+ return datagram;
+ }
+ else {
+ return NULL;
+ }
}
/*****************************************************************************/
@@ -1366,12 +1409,101 @@
/*****************************************************************************/
+/** Execute slave FSMs.
+ */
+void ec_master_exec_slave_fsms(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ ec_datagram_t *datagram;
+ ec_fsm_slave_t *fsm, *next;
+ unsigned int count = 0;
+
+ list_for_each_entry_safe(fsm, next, &master->fsm_exec_list, list) {
+ if (!fsm->datagram) {
+ EC_MASTER_WARN(master, "Slave %u FSM has zero datagram."
+ "This is a bug!\n", fsm->slave->ring_position);
+ list_del_init(&fsm->list);
+ master->fsm_exec_count--;
+ return;
+ }
+
+ if (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT) {
+ // previous datagram was not sent or received yet.
+ // wait until next thread execution
+ return;
+ }
+
+ datagram = ec_master_get_external_datagram(master);
+ if (!datagram) {
+ // no free datagrams at the moment
+ EC_MASTER_WARN(master, "No free datagram during"
+ " slave FSM execution. This is a bug!\n");
+ continue;
+ }
+
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Executing slave %u FSM.\n",
+ fsm->slave->ring_position);
+#endif
+ if (ec_fsm_slave_exec(fsm, datagram)) {
+ // FSM consumed datagram
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "FSM consumed datagram %s\n",
+ datagram->name);
+#endif
+ master->ext_ring_idx_fsm =
+ (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
+ }
+ else {
+ // FSM finished
+ list_del_init(&fsm->list);
+ master->fsm_exec_count--;
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "FSM finished. %u remaining.\n",
+ master->fsm_exec_count);
+#endif
+ }
+ }
+
+ while (master->fsm_exec_count < EC_EXT_RING_SIZE / 2
+ && count < master->slave_count) {
+
+ if (ec_fsm_slave_is_ready(&master->fsm_slave->fsm)) {
+ datagram = ec_master_get_external_datagram(master);
+
+ if (ec_fsm_slave_exec(&master->fsm_slave->fsm, datagram)) {
+ master->ext_ring_idx_fsm =
+ (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
+ list_add_tail(&master->fsm_slave->fsm.list,
+ &master->fsm_exec_list);
+ master->fsm_exec_count++;
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "New slave %u FSM"
+ " consumed datagram %s, now %u FSMs in list.\n",
+ master->fsm_slave->ring_position, datagram->name,
+ master->fsm_exec_count);
+#endif
+ }
+ }
+
+ master->fsm_slave++;
+ if (master->fsm_slave >= master->slaves + master->slave_count) {
+ master->fsm_slave = master->slaves;
+ }
+ count++;
+ }
+}
+
+/*****************************************************************************/
+
/** Master kernel thread function for IDLE phase.
*/
static int ec_master_idle_thread(void *priv_data)
{
ec_master_t *master = (ec_master_t *) priv_data;
- ec_slave_t *slave = NULL;
int fsm_exec;
#ifdef EC_USE_HRTIMER
size_t sent_bytes;
@@ -1392,8 +1524,6 @@
ecrt_master_receive(master);
up(&master->io_sem);
- fsm_exec = 0;
-
// execute master & slave state machines
if (down_interruptible(&master->master_sem)) {
break;
@@ -1401,11 +1531,7 @@
fsm_exec = ec_fsm_master_exec(&master->fsm);
- for (slave = master->slaves;
- slave < master->slaves + master->slave_count;
- slave++) {
- ec_fsm_slave_exec(&slave->fsm);
- }
+ ec_master_exec_slave_fsms(master);
up(&master->master_sem);
@@ -1449,8 +1575,6 @@
static int ec_master_operation_thread(void *priv_data)
{
ec_master_t *master = (ec_master_t *) priv_data;
- ec_slave_t *slave = NULL;
- int fsm_exec;
EC_MASTER_DBG(master, 1, "Operation thread running"
" with fsm interval = %u us, max data size=%zu\n",
@@ -1463,28 +1587,20 @@
// output statistics
ec_master_output_stats(master);
- fsm_exec = 0;
-
// execute master & slave state machines
if (down_interruptible(&master->master_sem)) {
break;
}
- fsm_exec += ec_fsm_master_exec(&master->fsm);
-
- for (slave = master->slaves;
- slave < master->slaves + master->slave_count;
- slave++) {
- ec_fsm_slave_exec(&slave->fsm);
- }
-
- up(&master->master_sem);
-
- // Inject datagrams (let the RT thread queue them, see
- // ecrt_master_send())
- if (fsm_exec) {
+ if (ec_fsm_master_exec(&master->fsm)) {
+ // Inject datagrams (let the RT thread queue them, see
+ // ecrt_master_send())
master->injection_seq_fsm++;
}
+
+ ec_master_exec_slave_fsms(master);
+
+ up(&master->master_sem);
}
#ifdef EC_USE_HRTIMER
@@ -2282,7 +2398,7 @@
ec_device_index_t dev_idx;
if (master->injection_seq_rt != master->injection_seq_fsm) {
- // inject datagrams produced by master FSM
+ // inject datagram produced by master FSM
ec_master_queue_datagram(master, &master->fsm_datagram);
master->injection_seq_rt = master->injection_seq_fsm;
}
--- a/master/master.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/master.h Thu Jan 10 17:36:41 2013 +0100
@@ -114,6 +114,13 @@
} \
} while (0)
+
+/** Size of the external datagram ring.
+ *
+ * The external datagram ring is used for slave FSMs.
+ */
+#define EC_EXT_RING_SIZE 32
+
/*****************************************************************************/
/** EtherCAT master phase.
@@ -260,11 +267,20 @@
struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
ext_datagram_queue. */
- struct list_head external_datagram_queue; /**< External Datagram queue. */
+ ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /**< External datagram
+ ring. */
+ unsigned int ext_ring_idx_rt; /**< Index in external datagram ring for RT
+ side. */
+ unsigned int ext_ring_idx_fsm; /**< Index in external datagram ring for
+ FSM side. */
unsigned int send_interval; /**< Interval between two calls to
ecrt_master_send(). */
size_t max_queue_size; /**< Maximum size of datagram queue */
+ ec_slave_t *fsm_slave; /**< Slave that is queried next for FSM exec. */
+ struct list_head fsm_exec_list; /**< Slave FSM execution list. */
+ unsigned int fsm_exec_count; /**< Number of entries in execution list. */
+
unsigned int debug_level; /**< Master debug level. */
ec_stats_t stats; /**< Cyclic statistics. */
@@ -324,8 +340,6 @@
void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t);
void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *);
void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *);
-void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
-void ec_master_inject_external_datagrams(ec_master_t *);
// misc.
void ec_master_set_send_interval(ec_master_t *, unsigned int);
--- a/master/slave.c Thu Jan 10 12:34:58 2013 +0100
+++ b/master/slave.c Thu Jan 10 17:36:41 2013 +0100
@@ -68,7 +68,6 @@
)
{
unsigned int i;
- int ret;
slave->master = master;
slave->device_index = dev_idx;
@@ -157,19 +156,8 @@
INIT_LIST_HEAD(&slave->foe_requests);
INIT_LIST_HEAD(&slave->soe_requests);
- // init state machine datagram
- ec_datagram_init(&slave->fsm_datagram);
- snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE,
- "slave%u-fsm", slave->ring_position);
- ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE);
- if (ret < 0) {
- ec_datagram_clear(&slave->fsm_datagram);
- EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n");
- return;
- }
-
// create state machine object
- ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram);
+ ec_fsm_slave_init(&slave->fsm, slave);
}
/*****************************************************************************/
@@ -194,7 +182,6 @@
EC_SLAVE_WARN(slave, "Discarding SDO request,"
" slave about to be deleted.\n");
request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&slave->master->request_queue);
}
while (!list_empty(&slave->reg_requests)) {
@@ -204,7 +191,6 @@
EC_SLAVE_WARN(slave, "Discarding register request,"
" slave about to be deleted.\n");
reg->state = EC_INT_REQUEST_FAILURE;
- wake_up(&slave->master->request_queue);
}
while (!list_empty(&slave->foe_requests)) {
@@ -214,7 +200,6 @@
EC_SLAVE_WARN(slave, "Discarding FoE request,"
" slave about to be deleted.\n");
request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&slave->master->request_queue);
}
while (!list_empty(&slave->soe_requests)) {
@@ -224,8 +209,9 @@
EC_SLAVE_WARN(slave, "Discarding SoE request,"
" slave about to be deleted.\n");
request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&slave->master->request_queue);
- }
+ }
+
+ wake_up_all(&slave->master->request_queue);
if (slave->config) {
ec_slave_config_detach(slave->config);
@@ -260,7 +246,6 @@
}
ec_fsm_slave_clear(&slave->fsm);
- ec_datagram_clear(&slave->fsm_datagram);
}
/*****************************************************************************/
--- a/master/slave.h Thu Jan 10 12:34:58 2013 +0100
+++ b/master/slave.h Thu Jan 10 17:36:41 2013 +0100
@@ -230,7 +230,6 @@
struct list_head soe_requests; /**< SoE write requests. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
- ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
};
/*****************************************************************************/