--- 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. */
+ )
+{
+}
+
+/*****************************************************************************/