Hi Gavin,
that worked, I had to add some minor changes:
memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, data_size);
=>
memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, voe->data_size +
EC_VOE_HEADER_SIZE);
The VoE header is taken into account here by ecrt_voe_handler_data and
there is also ecrt_voe_handler_received_header so the VoE header has to
be copied too.
Best regards,
Christoph
On 01/19/2017 01:09 AM, Gavin Lambert wrote:
On 19 January 2017 05:26, quoth Christoph Schroeder:
I am currently testing the patchset 20160804 and encountered a problem
introduced by 0005-Support-for-multiple-mailbox-protocols.patch. It's not
possible to retrieve the data via the libraries "ecrt_voe_handler_data"
function anymore after a successful ecrt_voe_handler_read. I still get the
correct data size but there is no data found at the returned pointer address.
I guess this is the problem:
"The mailbox state machines will check and fetch the data from their own
buffer instead of the datagram buffer (that is no longer used for mailbox
read data)."
The data seems to be stored somewhere else now and the function still
returns a pointer to a datagram buffer. Communication still works and the
data is there inside the master as debuglevel=1 prints the correct data into
the kernel log. I found the lines were this is done in the new introduced
"ec_voe_handler_state_read_response_data", but how can I access the
data from the library without using ioctl calls from my user application? I
would appreciate any hint or an update of the patch.
Try the following changes (sorry, can't generate a patch right now):
* voe_handler.c:372: "mbox_coe_data" should be "mbox_voe_data".
* voe_handler.c:500: add:
memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, data_size);
* voe_handler.c:608: same here.
Give this a try and let me know if it helps, or if further changes are
required, and I can update the patchset accordingly.
________________________________
Helmholtz-Zentrum Berlin für Materialien und Energie GmbH
Mitglied der Hermann von Helmholtz-Gemeinschaft Deutscher Forschungszentren e.V.
Aufsichtsrat: Vorsitzender Dr. Karl Eugen Huthmacher, stv. Vorsitzende Dr.
Jutta Koch-Unterseher
Geschäftsführung: Prof. Dr. Anke Rita Kaysser-Pyzalla, Thomas Frederking
Sitz Berlin, AG Charlottenburg, 89 HRB 5583
Postadresse:
Hahn-Meitner-Platz 1
D-14109 Berlin
http://www.helmholtz-berlin.de
commit 72ebe5133d382b99b945711f47f9b1c3dbe0170d
Author: Christoph Schröder <christoph.schroe...@helmholtz-berlin.de>
Date: Thu Jan 19 10:38:57 2017 +0100
fixed 0005-Support-for-multiple-mailbox-protocols.patch
diff --git a/master/datagram.c b/master/datagram.c
index da50850..974d081 100644
--- a/master/datagram.c
+++ b/master/datagram.c
@@ -587,6 +587,9 @@ void ec_datagram_print_state(
case EC_DATAGRAM_ERROR:
printk("error");
break;
+ case EC_DATAGRAM_INVALID:
+ printk("invalid");
+ break;
default:
printk("???");
}
@@ -648,3 +651,109 @@ const char *ec_datagram_type_string(
}
/*****************************************************************************/
+
+/** Initialize mailbox response data.
+ *
+ */
+void ec_mbox_data_init(
+ ec_mbox_data_t *mbox_data /**< Mailbox response data. */
+ )
+{
+ mbox_data->data = NULL;
+ mbox_data->data_size = 0;
+ mbox_data->payload_size = 0;
+}
+
+
+/*****************************************************************************/
+
+/** Free internal memory for mailbox response data.
+ *
+ */
+void ec_mbox_data_clear(
+ ec_mbox_data_t *mbox_data /**< Mailbox response data. */
+ )
+{
+ if (mbox_data->data) {
+ kfree(mbox_data->data);
+ mbox_data->data = NULL;
+ mbox_data->data_size = 0;
+ }
+}
+
+
+/*****************************************************************************/
+
+/** Allocates internal memory for mailbox response data.
+ *
+ * \return 0 in case of success, otherwise \a -ENOMEM.
+ */
+int ec_mbox_data_prealloc(
+ ec_mbox_data_t *mbox_data, /**< Mailbox response data. */
+ size_t size /**< Mailbox size in bytes. */
+ )
+{
+ if (mbox_data->data) {
+ kfree(mbox_data->data);
+ mbox_data->data = NULL;
+ mbox_data->data_size = 0;
+ }
+
+ if (!(mbox_data->data = kmalloc(size, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate %zu bytes of mailbox data memory!\n", size);
+ return -ENOMEM;
+ }
+ mbox_data->data_size = size;
+ return 0;
+}
+
+
+/*****************************************************************************/
+
+/** Allocates internal memory for mailbox response data for all slave
+ * supported mailbox protocols .
+ *
+ */
+void ec_mbox_prot_data_prealloc(
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ uint16_t protocols, /**< Supported protocols. */
+ size_t size /**< Mailbox size in bytes. */
+ )
+{
+ if ((size > 0) && (size <= EC_MAX_DATA_SIZE)) {
+#ifdef EC_EOE
+ if (protocols & EC_MBOX_EOE) {
+ ec_mbox_data_prealloc(&slave->mbox_eoe_data, size);
+ }
+ else {
+ ec_mbox_data_clear(&slave->mbox_eoe_data);
+ }
+#endif
+ if (protocols & EC_MBOX_COE) {
+ ec_mbox_data_prealloc(&slave->mbox_coe_data, size);
+ }
+ else {
+ ec_mbox_data_clear(&slave->mbox_coe_data);
+ }
+ if (protocols & EC_MBOX_FOE) {
+ ec_mbox_data_prealloc(&slave->mbox_foe_data, size);
+ }
+ else {
+ ec_mbox_data_clear(&slave->mbox_foe_data);
+ }
+ if (protocols & EC_MBOX_SOE) {
+ ec_mbox_data_prealloc(&slave->mbox_soe_data, size);
+ }
+ else {
+ ec_mbox_data_clear(&slave->mbox_soe_data);
+ }
+ if (protocols & EC_MBOX_VOE) {
+ ec_mbox_data_prealloc(&slave->mbox_voe_data, size);
+ }
+ else {
+ ec_mbox_data_clear(&slave->mbox_voe_data);
+ }
+ }
+}
+
+/*****************************************************************************/
diff --git a/master/datagram.h b/master/datagram.h
index 96af32f..eea19a3 100644
--- a/master/datagram.h
+++ b/master/datagram.h
@@ -77,7 +77,8 @@ typedef enum {
EC_DATAGRAM_SENT, /**< Sent (still in the queue). */
EC_DATAGRAM_RECEIVED, /**< Received (dequeued). */
EC_DATAGRAM_TIMED_OUT, /**< Timed out (dequeued). */
- EC_DATAGRAM_ERROR /**< Error while sending/receiving (dequeued). */
+ EC_DATAGRAM_ERROR, /**< Error while sending/receiving (dequeued). */
+ EC_DATAGRAM_INVALID /**< Unused and should not be queued (dequeued). */
} ec_datagram_state_t;
/*****************************************************************************/
@@ -113,6 +114,17 @@ typedef struct {
char name[EC_DATAGRAM_NAME_SIZE]; /**< Description of the datagram. */
} ec_datagram_t;
+
+/*****************************************************************************/
+
+/** EtherCAT mailbox response data.
+ */
+typedef struct {
+ uint8_t *data; /**< Mailbox response data. */
+ size_t data_size; /**< Size of the mailbox response data buffer. */
+ size_t payload_size; /**< Size of the mailbox response payload data. */
+} ec_mbox_data_t;
+
/*****************************************************************************/
void ec_datagram_init(ec_datagram_t *);
@@ -144,6 +156,10 @@ void ec_datagram_print_wc_error(const ec_datagram_t *);
void ec_datagram_output_stats(ec_datagram_t *);
const char *ec_datagram_type_string(const ec_datagram_t *);
+void ec_mbox_data_init(ec_mbox_data_t *);
+void ec_mbox_data_clear(ec_mbox_data_t *);
+void ec_mbox_prot_data_prealloc(ec_slave_t *, uint16_t, size_t);
+
/*****************************************************************************/
#endif
diff --git a/master/ethernet.c b/master/ethernet.c
index e8a2206..d990bde 100644
--- a/master/ethernet.c
+++ b/master/ethernet.c
@@ -71,6 +71,7 @@ void ec_eoe_flush(ec_eoe_t *);
void ec_eoe_state_rx_start(ec_eoe_t *);
void ec_eoe_state_rx_check(ec_eoe_t *);
void ec_eoe_state_rx_fetch(ec_eoe_t *);
+void ec_eoe_state_rx_fetch_data(ec_eoe_t *);
void ec_eoe_state_tx_start(ec_eoe_t *);
void ec_eoe_state_tx_sent(ec_eoe_t *);
@@ -490,9 +491,15 @@ void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */)
return;
}
- ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
- eoe->queue_datagram = 1;
- eoe->state = ec_eoe_state_rx_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(eoe->slave)) {
+ eoe->state = ec_eoe_state_rx_fetch_data;
+ }
+ else {
+ ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
+ eoe->queue_datagram = 1;
+ eoe->state = ec_eoe_state_rx_check;
+ }
}
/*****************************************************************************/
@@ -511,12 +518,21 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
" check datagram for %s.\n", eoe->dev->name);
#endif
eoe->state = ec_eoe_state_tx_start;
+ ec_read_mbox_lock_clear(eoe->slave);
return;
}
if (!ec_slave_mbox_check(&eoe->datagram)) {
eoe->rx_idle = 1;
- eoe->state = ec_eoe_state_tx_start;
+ ec_read_mbox_lock_clear(eoe->slave);
+ // check that data is not already received by another read request
+ if (eoe->slave->mbox_eoe_data.payload_size > 0) {
+ eoe->state = ec_eoe_state_rx_fetch_data;
+ eoe->state(eoe);
+ }
+ else {
+ eoe->state = ec_eoe_state_tx_start;
+ }
return;
}
@@ -535,6 +551,32 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
*/
void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
{
+
+ if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
+ eoe->stats.rx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
+ " fetch datagram for %s.\n", eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_tx_start;
+ ec_read_mbox_lock_clear(eoe->slave);
+ return;
+ }
+ ec_read_mbox_lock_clear(eoe->slave);
+ eoe->state = ec_eoe_state_rx_fetch_data;
+ eoe->state(eoe);
+}
+
+
+
+/*****************************************************************************/
+
+/** State: RX_FETCH DATA.
+ *
+ * Processes the EoE data.
+ */
+void ec_eoe_state_rx_fetch_data(ec_eoe_t *eoe /**< EoE handler */)
+{
size_t rec_size, data_size;
uint8_t *data, frame_type, last_fragment, time_appended, mbox_prot;
uint8_t fragment_offset, fragment_number;
@@ -546,17 +588,21 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
unsigned int i;
#endif
- if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
- eoe->stats.rx_errors++;
-#if EOE_DEBUG_LEVEL >= 1
- EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
- " fetch datagram for %s.\n", eoe->dev->name);
-#endif
- eoe->state = ec_eoe_state_tx_start;
+ if (eoe->slave->mbox_eoe_data.payload_size > 0) {
+ eoe->slave->mbox_eoe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (!ec_read_mbox_locked(eoe->slave)) {
+ ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
+ eoe->queue_datagram = 1;
+ eoe->state = ec_eoe_state_rx_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram,
+ data = ec_slave_mbox_fetch(eoe->slave, &eoe->slave->mbox_eoe_data,
&mbox_prot, &rec_size);
if (IS_ERR(data)) {
eoe->stats.rx_errors++;
diff --git a/master/fsm_coe.c b/master/fsm_coe.c
index b3116b6..43962fc 100644
--- a/master/fsm_coe.c
+++ b/master/fsm_coe.c
@@ -71,27 +71,34 @@ 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_response_data(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_desc_response_data(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_dict_entry_response_data(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_response_data(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_down_seg_response_data(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_response_data(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_up_seg_response_data(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 *);
@@ -401,9 +408,17 @@ void ec_fsm_coe_dict_request(
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;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_coe_dict_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_check;
+ }
}
/*****************************************************************************/
@@ -424,6 +439,7 @@ void ec_fsm_coe_dict_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -431,6 +447,7 @@ void ec_fsm_coe_dict_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -438,11 +455,22 @@ void ec_fsm_coe_dict_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_dict_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for"
" SDO dictionary list response.\n");
return;
@@ -500,13 +528,6 @@ void ec_fsm_coe_dict_response(
)
{
ec_slave_t *slave = fsm->slave;
- uint8_t *data, mbox_prot;
- size_t rec_size;
- unsigned int sdo_count, i;
- uint16_t sdo_index, fragments_left;
- ec_sdo_t *sdo;
- bool first_segment;
- size_t index_list_offset;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -515,6 +536,7 @@ void ec_fsm_coe_dict_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -522,13 +544,61 @@ void ec_fsm_coe_dict_response(
}
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(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_dict_response_data;
+ fsm->state(fsm, datagram);
+}
+
+
+/*****************************************************************************/
+
+/**
+ CoE state: DICT RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_dict_response_data(
+ 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;
+ unsigned int sdo_count, i;
+ uint16_t sdo_index, fragments_left;
+ ec_sdo_t *sdo;
+ bool first_segment;
+ size_t index_list_offset;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_dict_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@@ -681,9 +751,17 @@ void ec_fsm_coe_dict_desc_request(
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_desc_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_coe_dict_desc_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_desc_check;
+ }
}
/*****************************************************************************/
@@ -706,6 +784,7 @@ void ec_fsm_coe_dict_desc_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -713,6 +792,7 @@ void ec_fsm_coe_dict_desc_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -720,11 +800,22 @@ void ec_fsm_coe_dict_desc_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_dict_desc_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for"
" SDO 0x%04x object description response.\n",
fsm->sdo->index);
@@ -785,9 +876,6 @@ void ec_fsm_coe_dict_desc_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_sdo_t *sdo = fsm->sdo;
- uint8_t *data, mbox_prot;
- size_t rec_size, name_size;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -796,6 +884,7 @@ void ec_fsm_coe_dict_desc_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -803,14 +892,57 @@ void ec_fsm_coe_dict_desc_response(
}
if (fsm->datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" response failed: ");
- ec_datagram_print_wc_error(fsm->datagram);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_dict_desc_response_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/**
+ CoE state: DICT DESC RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_dict_desc_response_data(
+ 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;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_dict_desc_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@@ -945,9 +1077,17 @@ void ec_fsm_coe_dict_entry_request(
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_entry_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_coe_dict_entry_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_entry_check;
+ }
}
/*****************************************************************************/
@@ -970,6 +1110,7 @@ void ec_fsm_coe_dict_entry_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -977,6 +1118,7 @@ void ec_fsm_coe_dict_entry_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -984,11 +1126,22 @@ void ec_fsm_coe_dict_entry_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_dict_entry_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for"
" SDO entry 0x%04x:%x description response.\n",
fsm->sdo->index, fsm->subindex);
@@ -1019,11 +1172,6 @@ void ec_fsm_coe_dict_entry_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_sdo_t *sdo = fsm->sdo;
- uint8_t *data, mbox_prot;
- size_t rec_size, data_size;
- ec_sdo_entry_t *entry;
- u16 word;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -1032,6 +1180,7 @@ void ec_fsm_coe_dict_entry_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
" description response datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -1039,14 +1188,59 @@ void ec_fsm_coe_dict_entry_response(
}
if (fsm->datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" response failed: ");
- ec_datagram_print_wc_error(fsm->datagram);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_dict_entry_response_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/**
+ CoE state: DICT ENTRY RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_dict_entry_response_data(
+ 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, data_size;
+ ec_sdo_entry_t *entry;
+ u16 word;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_dict_entry_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@@ -1396,9 +1590,17 @@ void ec_fsm_coe_down_request(
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_down_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_coe_down_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_check;
+ }
}
/*****************************************************************************/
@@ -1420,6 +1622,7 @@ void ec_fsm_coe_down_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -1429,6 +1632,7 @@ void ec_fsm_coe_down_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -1436,12 +1640,23 @@ void ec_fsm_coe_down_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_down_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (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;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
" for SDO 0x%04x:%x download response.\n", diff_ms,
fsm->request->index, fsm->request->subindex);
@@ -1535,8 +1750,6 @@ void ec_fsm_coe_down_response(
)
{
ec_slave_t *slave = fsm->slave;
- uint8_t *data, mbox_prot;
- size_t rec_size;
ec_sdo_request_t *request = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
@@ -1554,14 +1767,58 @@ void ec_fsm_coe_down_response(
}
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(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_down_response_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/**
+ CoE state: DOWN RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_down_response_data(
+ 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;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_down_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
+
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -1663,6 +1920,7 @@ void ec_fsm_coe_down_seg_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -1671,6 +1929,7 @@ void ec_fsm_coe_down_seg_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -1678,12 +1937,23 @@ void ec_fsm_coe_down_seg_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_down_seg_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (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;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download"
" segment response.\n");
return;
@@ -1713,8 +1983,6 @@ void ec_fsm_coe_down_seg_response(
)
{
ec_slave_t *slave = fsm->slave;
- uint8_t *data, mbox_prot;
- size_t rec_size;
ec_sdo_request_t *request = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
@@ -1725,6 +1993,7 @@ void ec_fsm_coe_down_seg_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE download response"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -1732,14 +2001,57 @@ void ec_fsm_coe_down_seg_response(
}
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(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_down_seg_response_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/**
+ CoE state: DOWN SEG RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_down_seg_response_data(
+ 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;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_down_seg_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -1964,10 +2276,17 @@ void ec_fsm_coe_up_request(
#endif
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_up_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_coe_up_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_check;
+ }
}
/*****************************************************************************/
@@ -1991,6 +2310,7 @@ void ec_fsm_coe_up_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -1999,6 +2319,7 @@ void ec_fsm_coe_up_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -2006,12 +2327,23 @@ void ec_fsm_coe_up_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_up_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (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;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" SDO 0x%04x:%x upload response.\n", diff_ms,
fsm->request->index, fsm->request->subindex);
@@ -2071,13 +2403,7 @@ void ec_fsm_coe_up_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_master_t *master = slave->master;
- uint16_t rec_index;
- uint8_t *data, mbox_prot, rec_subindex;
- size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
- unsigned int expedited, size_specified;
- int ret;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -2087,6 +2413,7 @@ void ec_fsm_coe_up_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -2094,14 +2421,63 @@ void ec_fsm_coe_up_response(
}
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(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_up_response_data;
+ fsm->state(fsm, datagram);
+}
+
+
+/*****************************************************************************/
+
+/**
+ CoE state: UP RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_up_response_data(
+ 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;
+ uint8_t *data, mbox_prot, rec_subindex;
+ size_t rec_size, data_size;
+ ec_sdo_request_t *request = fsm->request;
+ unsigned int expedited, size_specified;
+ int ret;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_up_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
+
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@@ -2299,10 +2675,17 @@ void ec_fsm_coe_up_seg_request(
}
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_up_seg_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_coe_up_seg_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_check;
+ }
}
/*****************************************************************************/
@@ -2326,6 +2709,7 @@ void ec_fsm_coe_up_seg_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -2335,6 +2719,7 @@ void ec_fsm_coe_up_seg_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"
" failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -2342,12 +2727,23 @@ void ec_fsm_coe_up_seg_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_coe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_up_seg_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (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;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload"
" segment response.\n");
return;
@@ -2377,11 +2773,7 @@ void ec_fsm_coe_up_seg_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_master_t *master = slave->master;
- uint8_t *data, mbox_prot;
- size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
- unsigned int last_segment;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -2391,6 +2783,7 @@ void ec_fsm_coe_up_seg_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -2398,15 +2791,62 @@ void ec_fsm_coe_up_seg_response(
}
if (fsm->datagram->working_counter != 1) {
- request->errno = EIO;
- fsm->state = ec_fsm_coe_error;
- EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_coe_data.payload_size == 0) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
" response failed: ");
- ec_datagram_print_wc_error(fsm->datagram);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_coe_up_seg_response_data;
+ fsm->state(fsm, datagram);
+}
+
+
+/*****************************************************************************/
+
+/**
+ CoE state: UP RESPONSE DATA.
+
+*/
+
+void ec_fsm_coe_up_seg_response_data(
+ 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;
+ size_t rec_size, data_size;
+ ec_sdo_request_t *request = fsm->request;
+ unsigned int last_segment;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_coe_data.payload_size > 0) {
+ slave->mbox_coe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_coe_up_seg_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
+
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
diff --git a/master/fsm_eoe.c b/master/fsm_eoe.c
index 4788ab6..0e6c4d8 100644
--- a/master/fsm_eoe.c
+++ b/master/fsm_eoe.c
@@ -51,6 +51,7 @@ void ec_fsm_eoe_set_ip_start(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_request(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_check(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_response(ec_fsm_eoe_t *, ec_datagram_t *);
+void ec_fsm_eoe_set_ip_response_data(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_end(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_error(ec_fsm_eoe_t *, ec_datagram_t *);
@@ -325,9 +326,18 @@ void ec_fsm_eoe_set_ip_request(
}
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_eoe_set_ip_check;
+
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_eoe_set_ip_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_eoe_set_ip_check;
+ }
}
/*****************************************************************************/
@@ -348,6 +358,7 @@ void ec_fsm_eoe_set_ip_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive EoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -355,6 +366,7 @@ void ec_fsm_eoe_set_ip_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_eoe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of EoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -362,11 +374,21 @@ void ec_fsm_eoe_set_ip_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ unsigned long diff_ms;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_eoe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_eoe_set_ip_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_EOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_eoe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" set IP parameter response.\n", diff_ms);
return;
@@ -393,10 +415,6 @@ void ec_fsm_eoe_set_ip_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_master_t *master = slave->master;
- uint8_t *data, mbox_prot, frame_type;
- size_t rec_size;
- ec_eoe_request_t *req = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -405,19 +423,61 @@ void ec_fsm_eoe_set_ip_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
- fsm->state = ec_fsm_eoe_error;
- EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
- ec_datagram_print_wc_error(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_eoe_data.payload_size == 0) {
+ fsm->state = ec_fsm_eoe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_eoe_set_ip_response_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/** EoE state: SET IP RESPONSE DATA.
+ */
+void ec_fsm_eoe_set_ip_response_data(
+ ec_fsm_eoe_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, frame_type;
+ size_t rec_size;
+ ec_eoe_request_t *req = fsm->request;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_eoe_data.payload_size > 0) {
+ slave->mbox_eoe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_eoe_set_ip_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_eoe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_eoe_error;
return;
diff --git a/master/fsm_foe.c b/master/fsm_foe.c
index 4470c91..ad2dbb9 100644
--- a/master/fsm_foe.c
+++ b/master/fsm_foe.c
@@ -87,11 +87,13 @@ 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_ack_read_data(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_data_read_data(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 *);
@@ -359,6 +361,7 @@ void ec_fsm_foe_state_ack_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -366,6 +369,7 @@ void ec_fsm_foe_state_ack_check(
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
" failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -374,9 +378,19 @@ void ec_fsm_foe_state_ack_check(
if (!ec_slave_mbox_check(fsm->datagram)) {
// slave did not put anything in the mailbox yet
+
+ // check that data is not already received by another read request
+ if (slave->mbox_foe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_foe_state_ack_read_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
if (time_after(fsm->datagram->jiffies_received,
fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
return;
}
@@ -403,9 +417,6 @@ void ec_fsm_foe_state_ack_read(
)
{
ec_slave_t *slave = fsm->slave;
- uint8_t *data, mbox_prot;
- uint8_t opCode;
- size_t rec_size;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
@@ -413,19 +424,60 @@ void ec_fsm_foe_state_ack_read(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
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(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_foe_data.payload_size == 0) {
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_foe_state_ack_read_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/** Process a read operation.
+ */
+void ec_fsm_foe_state_ack_read_data(
+ 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;
+ size_t rec_size;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_foe_data.payload_size > 0) {
+ slave->mbox_foe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_foe_state_ack_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_foe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
@@ -522,10 +574,17 @@ void ec_fsm_foe_state_wrq_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_foe_state_ack_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_foe_state_ack_read_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_ack_check;
+ }
}
/*****************************************************************************/
@@ -560,10 +619,19 @@ void ec_fsm_foe_state_data_sent(
return;
}
- ec_slave_mbox_prepare_check(slave, datagram);
fsm->jiffies_start = jiffies;
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_foe_state_ack_check;
+
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_foe_state_ack_read_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_ack_check;
+ }
}
/*****************************************************************************/
@@ -652,6 +720,7 @@ void ec_fsm_foe_state_rrq_sent(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -660,6 +729,7 @@ void ec_fsm_foe_state_rrq_sent(
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_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
@@ -667,10 +737,17 @@ void ec_fsm_foe_state_rrq_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_foe_state_data_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_foe_state_data_read_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_data_check;
+ }
}
/*****************************************************************************/
@@ -723,6 +800,7 @@ void ec_fsm_foe_state_data_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: ");
ec_datagram_print_state(fsm->datagram);
return;
@@ -730,15 +808,25 @@ void ec_fsm_foe_state_data_check(
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!ec_slave_mbox_check(fsm->datagram)) {
+ // check that data is not already received by another read request
+ if (slave->mbox_foe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_foe_state_data_read_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
if (time_after(fsm->datagram->jiffies_received,
fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
return;
}
@@ -764,10 +852,6 @@ void ec_fsm_foe_state_data_read(
ec_datagram_t *datagram /**< Datagram to use. */
)
{
- size_t rec_size;
- uint32_t packet_no;
- uint8_t *data, opCode, mbox_prot;
-
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
@@ -776,19 +860,59 @@ void ec_fsm_foe_state_data_read(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
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(fsm->datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_foe_data.payload_size == 0) {
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_foe_state_data_read_data;
+ fsm->state(fsm, datagram);
+}
+
+/*****************************************************************************/
+
+/** Process a read data operation.
+ */
+void ec_fsm_foe_state_data_read_data(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ size_t rec_size;
+ uint8_t *data, opCode, packet_no, mbox_prot;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_foe_data.payload_size > 0) {
+ slave->mbox_foe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_foe_state_data_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_foe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
@@ -923,8 +1047,6 @@ void ec_fsm_foe_state_sent_ack(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
- ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
-
if (fsm->rx_last_packet) {
fsm->rx_expected_packet_no = 0;
fsm->request->data_size = fsm->rx_buffer_offset;
@@ -932,8 +1054,18 @@ void ec_fsm_foe_state_sent_ack(
}
else {
fsm->rx_expected_packet_no++;
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_foe_state_data_check;
+
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_foe_state_data_read_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_data_check;
+ }
}
}
diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c
index 192ade3..71f7033 100755
--- a/master/fsm_slave_config.c
+++ b/master/fsm_slave_config.c
@@ -272,6 +272,10 @@ void ec_fsm_slave_config_state_init(
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_slave_config_state_clear_fmmus;
+
+ EC_SLAVE_DBG(slave, 1, "Clearing mailbox check flag...\n");
+
+ ec_read_mbox_lock_clear(slave);
}
/*****************************************************************************/
@@ -542,6 +546,9 @@ void ec_fsm_slave_config_enter_mbox_sync(
slave->sii.std_tx_mailbox_size;
}
+ // allocate memory for mailbox response data for supported mailbox protocols
+ ec_mbox_prot_data_prealloc(slave, slave->sii.mailbox_protocols, slave->configured_tx_mailbox_size);
+
fsm->take_time = 1;
fsm->retries = EC_FSM_RETRIES;
diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c
index 442e98c..39f9381 100644
--- a/master/fsm_slave_scan.c
+++ b/master/fsm_slave_scan.c
@@ -953,6 +953,9 @@ void ec_fsm_slave_scan_state_sync(
slave->configured_tx_mailbox_offset,
slave->configured_tx_mailbox_size);
+ // allocate memory for mailbox response data for supported mailbox protocols
+ ec_mbox_prot_data_prealloc(slave, slave->sii.mailbox_protocols, slave->configured_tx_mailbox_size);
+
ec_fsm_slave_scan_enter_pdos(fsm);
}
diff --git a/master/fsm_soe.c b/master/fsm_soe.c
index 2c55512..2ab72c2 100644
--- a/master/fsm_soe.c
+++ b/master/fsm_soe.c
@@ -64,11 +64,13 @@ 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_read_response_data(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_write_response_data(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 *);
@@ -324,9 +326,18 @@ void ec_fsm_soe_read_request(
}
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;
+
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_soe_read_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_read_check;
+ }
}
/*****************************************************************************/
@@ -347,6 +358,7 @@ void ec_fsm_soe_read_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@@ -355,6 +367,7 @@ void ec_fsm_soe_read_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@@ -363,11 +376,22 @@ void ec_fsm_soe_read_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (fsm->datagram->jiffies_received - fsm->jiffies_start) *
- 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_soe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_soe_read_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" read response.\n", diff_ms);
ec_fsm_soe_print_error(fsm);
@@ -395,11 +419,6 @@ void ec_fsm_soe_read_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_master_t *master = slave->master;
- uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
- value_included;
- size_t rec_size, data_size;
- ec_soe_request_t *req = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -408,6 +427,7 @@ void ec_fsm_soe_read_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@@ -415,14 +435,60 @@ void ec_fsm_soe_read_response(
}
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(fsm->datagram);
- ec_fsm_soe_print_error(fsm);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_soe_data.payload_size == 0) {
+ fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_soe_read_response_data;
+ fsm->state(fsm, datagram);
+}
+
+
+/*****************************************************************************/
+
+/**
+ SoE state: READ RESPONSE DATA.
+
+*/
+
+void ec_fsm_soe_read_response_data(
+ 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,
+ value_included;
+ size_t rec_size, data_size;
+ ec_soe_request_t *req = fsm->request;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_soe_data.payload_size > 0) {
+ slave->mbox_soe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_soe_read_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_soe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -640,9 +706,17 @@ void ec_fsm_soe_write_request(
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_write_check;
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ fsm->state = ec_fsm_soe_write_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_check;
+ }
}
/*****************************************************************************/
@@ -663,6 +737,7 @@ void ec_fsm_soe_write_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@@ -671,6 +746,7 @@ void ec_fsm_soe_write_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@@ -678,10 +754,21 @@ void ec_fsm_soe_write_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
- unsigned long diff_ms =
- (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_soe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_soe_write_response_data;
+ fsm->state(fsm, datagram);
+ return;
+ }
+
+ diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
" for write response.\n", diff_ms);
ec_fsm_soe_print_error(fsm);
@@ -709,11 +796,6 @@ void ec_fsm_soe_write_response(
)
{
ec_slave_t *slave = fsm->slave;
- ec_master_t *master = slave->master;
- ec_soe_request_t *req = fsm->request;
- uint8_t *data, mbox_prot, opcode, error_flag;
- uint16_t idn;
- size_t rec_size;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@@ -722,6 +804,7 @@ void ec_fsm_soe_write_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE write"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@@ -730,14 +813,60 @@ void ec_fsm_soe_write_response(
}
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(fsm->datagram);
- ec_fsm_soe_print_error(fsm);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_soe_data.payload_size == 0) {
+ fsm->state = ec_fsm_soe_error;
+ ec_read_mbox_lock_clear(slave);
+ EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ fsm->state = ec_fsm_soe_write_response_data;
+ fsm->state(fsm, datagram);
+}
+
+
+/*****************************************************************************/
+
+/**
+ SoE state: WRITE RESPONSE DATA.
+
+*/
+
+void ec_fsm_soe_write_response_data(
+ 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 *data, mbox_prot, opcode, error_flag;
+ uint16_t idn;
+ size_t rec_size;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_soe_data.payload_size > 0) {
+ slave->mbox_soe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->state = ec_fsm_soe_write_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_soe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
diff --git a/master/mailbox.c b/master/mailbox.c
index 3af07bc..2300928 100644
--- a/master/mailbox.c
+++ b/master/mailbox.c
@@ -163,18 +163,23 @@ const ec_code_msg_t mbox_error_messages[] = {
* \return Pointer to the received data, or ERR_PTR() code.
*/
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
- const ec_datagram_t *datagram, /**< datagram */
+ ec_mbox_data_t *response_data, /**< response data */
uint8_t *type, /**< expected mailbox protocol */
size_t *size /**< size of the received data */
)
{
size_t data_size;
- data_size = EC_READ_U16(datagram->data);
+ if (!response_data->data) {
+ EC_SLAVE_ERR(slave, "No mailbox response data received!\n");
+ return ERR_PTR(-EPROTO);
+ }
+
+ data_size = EC_READ_U16(response_data->data);
if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) {
EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n");
- ec_print_data(datagram->data, slave->configured_tx_mailbox_size);
+ ec_print_data(response_data->data, slave->configured_tx_mailbox_size);
return ERR_PTR(-EPROTO);
}
@@ -185,12 +190,12 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
}
#endif
- *type = EC_READ_U8(datagram->data + 5) & 0x0F;
+ *type = EC_READ_U8(response_data->data + 5) & 0x0F;
*size = data_size;
if (*type == 0x00) {
const ec_code_msg_t *mbox_msg;
- uint16_t code = EC_READ_U16(datagram->data + 8);
+ uint16_t code = EC_READ_U16(response_data->data + 8);
EC_SLAVE_ERR(slave, "Mailbox error response received - ");
@@ -207,13 +212,13 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
}
if (slave->master->debug_level && data_size > 0) {
- ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size);
+ ec_print_data(response_data->data + EC_MBOX_HEADER_SIZE, data_size);
}
return ERR_PTR(-EPROTO);
}
- return datagram->data + EC_MBOX_HEADER_SIZE;
+ return response_data->data + EC_MBOX_HEADER_SIZE;
}
/*****************************************************************************/
diff --git a/master/mailbox.h b/master/mailbox.h
index 18a5d4a..fc1f0a6 100644
--- a/master/mailbox.h
+++ b/master/mailbox.h
@@ -64,7 +64,7 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_datagram_t *,
int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_datagram_t *);
int ec_slave_mbox_check(const ec_datagram_t *);
int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_datagram_t *);
-uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, const ec_datagram_t *,
+uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, ec_mbox_data_t *,
uint8_t *, size_t *);
/*****************************************************************************/
diff --git a/master/master.c b/master/master.c
index 836e1c5..c933475 100644
--- a/master/master.c
+++ b/master/master.c
@@ -49,6 +49,7 @@
#include "slave_config.h"
#include "device.h"
#include "datagram.h"
+#include "mailbox.h"
#ifdef EC_EOE
#include "ethernet.h"
#endif
@@ -959,8 +960,10 @@ void ec_master_queue_datagram(
}
}
- list_add_tail(&datagram->queue, &master->datagram_queue);
- datagram->state = EC_DATAGRAM_QUEUED;
+ if (datagram->state != EC_DATAGRAM_INVALID) {
+ list_add_tail(&datagram->queue, &master->datagram_queue);
+ datagram->state = EC_DATAGRAM_QUEUED;
+ }
}
/*****************************************************************************/
@@ -1153,10 +1156,11 @@ void ec_master_receive_datagrams(
)
{
size_t frame_size, data_size;
- uint8_t datagram_type, datagram_index;
- unsigned int cmd_follows, matched;
+ uint8_t datagram_type, datagram_index, datagram_mbox_prot;
+ unsigned int cmd_follows, datagram_slave_addr, datagram_offset_addr, datagram_wc, matched;
const uint8_t *cur_data;
ec_datagram_t *datagram;
+ ec_slave_t *slave;
if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
if (master->debug_level || FORCE_OUTPUT_CORRUPTED) {
@@ -1198,6 +1202,8 @@ void ec_master_receive_datagrams(
// process datagram header
datagram_type = EC_READ_U8 (cur_data);
datagram_index = EC_READ_U8 (cur_data + 1);
+ datagram_slave_addr = EC_READ_U16(cur_data + 2);
+ datagram_offset_addr = EC_READ_U16(cur_data + 4);
data_size = EC_READ_U16(cur_data + 6) & 0x07FF;
cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000;
cur_data += EC_DATAGRAM_HEADER_SIZE;
@@ -1254,9 +1260,90 @@ void ec_master_receive_datagrams(
datagram->type != EC_DATAGRAM_FPWR &&
datagram->type != EC_DATAGRAM_BWR &&
datagram->type != EC_DATAGRAM_LWR) {
- // copy received data into the datagram memory,
- // if something has been read
- memcpy(datagram->data, cur_data, data_size);
+
+ // common mailbox dispatcher for mailboxes read using the physical slave address
+ if (datagram->type == EC_DATAGRAM_FPRD) {
+ datagram_wc = EC_READ_U16(cur_data + data_size);
+ if (datagram_wc) {
+ if (master->slaves != NULL) {
+ for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) {
+ if (slave->station_address == datagram_slave_addr) {
+ break;
+ }
+ }
+ if (slave->station_address == datagram_slave_addr) {
+ if (slave->configured_tx_mailbox_offset != 0) {
+ if (datagram_offset_addr == slave->configured_tx_mailbox_offset) {
+ datagram_mbox_prot = EC_READ_U8(cur_data + 5) & 0x0F;
+ switch (datagram_mbox_prot) {
+#ifdef EC_EOE
+ case EC_MBOX_TYPE_EOE:
+ if ((slave->mbox_eoe_data.data) && (data_size <= slave->mbox_eoe_data.data_size)) {
+ memcpy(slave->mbox_eoe_data.data, cur_data, data_size);
+ slave->mbox_eoe_data.payload_size = data_size;
+ }
+ break;
+#endif
+ case EC_MBOX_TYPE_COE:
+ if ((slave->mbox_coe_data.data) && (data_size <= slave->mbox_coe_data.data_size)) {
+ memcpy(slave->mbox_coe_data.data, cur_data, data_size);
+ slave->mbox_coe_data.payload_size = data_size;
+ }
+ break;
+ case EC_MBOX_TYPE_FOE:
+ if ((slave->mbox_foe_data.data) && (data_size <= slave->mbox_foe_data.data_size)) {
+ memcpy(slave->mbox_foe_data.data, cur_data, data_size);
+ slave->mbox_foe_data.payload_size = data_size;
+ }
+ break;
+ case EC_MBOX_TYPE_SOE:
+ if ((slave->mbox_soe_data.data) && (data_size <= slave->mbox_soe_data.data_size)) {
+ memcpy(slave->mbox_soe_data.data, cur_data, data_size);
+ slave->mbox_soe_data.payload_size = data_size;
+ }
+ break;
+ case EC_MBOX_TYPE_VOE:
+ if ((slave->mbox_voe_data.data) && (data_size <= slave->mbox_voe_data.data_size)) {
+ memcpy(slave->mbox_voe_data.data, cur_data, data_size);
+ slave->mbox_voe_data.payload_size = data_size;
+ }
+ break;
+ default:
+ EC_MASTER_DBG(master, 1, "Unknown mailbox protocol from slave: %u Protocol: %u\n", datagram_slave_addr, datagram_mbox_prot);
+ // copy instead received data into the datagram memory.
+ memcpy(datagram->data, cur_data, data_size);
+ break;
+ }
+ }
+ else {
+ // copy instead received data into the datagram memory.
+ memcpy(datagram->data, cur_data, data_size);
+ }
+ }
+ else {
+ // copy instead received data into the datagram memory.
+ memcpy(datagram->data, cur_data, data_size);
+ }
+ }
+ else {
+ EC_MASTER_DBG(master, 1, "No slave matching datagram slave address: %u\n", datagram_slave_addr);
+ }
+ }
+ else {
+ EC_MASTER_DBG(master, 1, "No configured slaves!\n");
+ // copy instead received data into the datagram memory.
+ memcpy(datagram->data, cur_data, data_size);
+ }
+ }
+ else {
+ // copy instead received data into the datagram memory.
+ memcpy(datagram->data, cur_data, data_size);
+ }
+ }
+ else {
+ // copy instead received data into the datagram memory.
+ memcpy(datagram->data, cur_data, data_size);
+ }
}
cur_data += data_size;
diff --git a/master/slave.c b/master/slave.c
index db294c6..0736e58 100644
--- a/master/slave.c
+++ b/master/slave.c
@@ -164,8 +164,52 @@ void ec_slave_init(
// create state machine object
ec_fsm_slave_init(&slave->fsm, slave);
+
+ slave->read_mbox_busy = 0;
+ rt_mutex_init(&slave->mbox_sem);
+
+#ifdef EC_EOE
+ ec_mbox_data_init(&slave->mbox_eoe_data);
+#endif
+ ec_mbox_data_init(&slave->mbox_coe_data);
+ ec_mbox_data_init(&slave->mbox_foe_data);
+ ec_mbox_data_init(&slave->mbox_soe_data);
+ ec_mbox_data_init(&slave->mbox_voe_data);
+}
+
+
+/*****************************************************************************/
+
+/**
+ Clears the mailbox lock.
+*/
+void ec_read_mbox_lock_clear(ec_slave_t *slave)
+{
+ rt_mutex_lock(&slave->mbox_sem);
+ slave->read_mbox_busy = 0;
+ rt_mutex_unlock(&slave->mbox_sem);
+}
+
+
+/*****************************************************************************/
+
+/**
+ Return the current mailbox lock status and lock it if not locked.
+*/
+int ec_read_mbox_locked(ec_slave_t *slave)
+{
+ int rc;
+
+ rt_mutex_lock(&slave->mbox_sem);
+ rc = slave->read_mbox_busy;
+ if (!slave->read_mbox_busy) {
+ slave->read_mbox_busy = 1;
+ }
+ rt_mutex_unlock(&slave->mbox_sem);
+ return rc;
}
+
/*****************************************************************************/
/**
@@ -260,6 +304,15 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
kfree(slave->sii_words);
}
+ // free mailbox response data
+#ifdef EC_EOE
+ ec_mbox_data_clear(&slave->mbox_eoe_data);
+#endif
+ ec_mbox_data_clear(&slave->mbox_coe_data);
+ ec_mbox_data_clear(&slave->mbox_foe_data);
+ ec_mbox_data_clear(&slave->mbox_soe_data);
+ ec_mbox_data_clear(&slave->mbox_voe_data);
+
ec_fsm_slave_clear(&slave->fsm);
}
diff --git a/master/slave.h b/master/slave.h
index 8a0ddba..46ece17 100644
--- a/master/slave.h
+++ b/master/slave.h
@@ -39,6 +39,7 @@
#include <linux/list.h>
#include <linux/kobject.h>
+#include <linux/rtmutex.h>
#include "globals.h"
#include "datagram.h"
@@ -255,6 +256,17 @@ struct ec_slave
struct list_head eoe_requests; /**< EoE set IP parameter requests. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
+
+ uint8_t read_mbox_busy; /**< Flag set during a mailbox read request. */
+ struct rt_mutex mbox_sem; /**< Semaphore protecting the check_mbox variable. */
+
+#ifdef EC_EOE
+ ec_mbox_data_t mbox_eoe_data; /**< Received mailbox data for EoE. */
+#endif
+ ec_mbox_data_t mbox_coe_data; /**< Received mailbox data for CoE. */
+ ec_mbox_data_t mbox_foe_data; /**< Received mailbox data for FoE. */
+ ec_mbox_data_t mbox_soe_data; /**< Received mailbox data for SoE. */
+ ec_mbox_data_t mbox_voe_data; /**< Received mailbox data for VoE. */
};
/*****************************************************************************/
@@ -292,6 +304,9 @@ void ec_slave_attach_pdo_names(ec_slave_t *);
void ec_slave_calc_port_delays(ec_slave_t *);
void ec_slave_calc_transmission_delays_rec(ec_slave_t *, uint32_t *);
+void ec_read_mbox_lock_clear(ec_slave_t *);
+int ec_read_mbox_locked(ec_slave_t *);
+
/*****************************************************************************/
#endif
diff --git a/master/voe_handler.c b/master/voe_handler.c
index 6e0a39b..65a2468 100644
--- a/master/voe_handler.c
+++ b/master/voe_handler.c
@@ -56,6 +56,7 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *);
void ec_voe_handler_state_read_start(ec_voe_handler_t *);
void ec_voe_handler_state_read_check(ec_voe_handler_t *);
void ec_voe_handler_state_read_response(ec_voe_handler_t *);
+void ec_voe_handler_state_read_response_data(ec_voe_handler_t *);
void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);
void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);
@@ -303,11 +304,20 @@ void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
return;
}
- ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
-
voe->jiffies_start = jiffies;
- voe->retries = EC_FSM_RETRIES;
- voe->state = ec_voe_handler_state_read_check;
+
+ // mailbox read check is skipped if a read request is already ongoing
+ if (ec_read_mbox_locked(slave)) {
+ voe->state = ec_voe_handler_state_read_response_data;
+ // the datagram is not used and marked as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ voe->jiffies_start = jiffies;
+ voe->retries = EC_FSM_RETRIES;
+ voe->state = ec_voe_handler_state_read_check;
+ }
}
/*****************************************************************************/
@@ -324,6 +334,7 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
if (datagram->state != EC_DATAGRAM_RECEIVED) {
voe->state = ec_voe_handler_state_error;
+ ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: ");
ec_datagram_print_state(datagram);
@@ -332,6 +343,7 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
if (datagram->working_counter != 1) {
voe->state = ec_voe_handler_state_error;
+ ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"
" datagram failed: ");
@@ -340,10 +352,21 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
}
if (!ec_slave_mbox_check(datagram)) {
- unsigned long diff_ms =
- (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
+ unsigned long diff_ms = 0;
+
+ // check that data is not already received by another read request
+ if (slave->mbox_voe_data.payload_size > 0) {
+ ec_read_mbox_lock_clear(slave);
+ voe->state = ec_voe_handler_state_read_response_data;
+ voe->state(voe);
+ return;
+ }
+
+ diff_ms = (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
+
if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
voe->state = ec_voe_handler_state_error;
+ ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n");
return;
@@ -368,15 +391,13 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_slave_t *slave = voe->config->slave;
- ec_master_t *master = voe->config->master;
- uint8_t *data, mbox_prot;
- size_t rec_size;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
voe->state = ec_voe_handler_state_error;
+ ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
ec_datagram_print_state(datagram);
@@ -384,14 +405,56 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
}
if (datagram->working_counter != 1) {
- voe->state = ec_voe_handler_state_error;
- voe->request_state = EC_INT_REQUEST_FAILURE;
- EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");
- ec_datagram_print_wc_error(datagram);
+ // only an error if data has not already been read by another read request
+ if (slave->mbox_voe_data.payload_size == 0) {
+ voe->state = ec_voe_handler_state_error;
+ ec_read_mbox_lock_clear(slave);
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+ }
+ ec_read_mbox_lock_clear(slave);
+ voe->state = ec_voe_handler_state_read_response_data;
+ voe->state(voe);
+}
+
+
+/*****************************************************************************/
+
+/**
+ VoE state: READ RESPONSE DATA.
+
+*/
+
+void ec_voe_handler_state_read_response_data(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+ ec_master_t *master = voe->config->master;
+ uint8_t *data, mbox_prot;
+ size_t rec_size;
+
+ // process the data available or initiate a new mailbox read check
+ if (slave->mbox_voe_data.payload_size > 0) {
+ slave->mbox_voe_data.payload_size = 0;
+ }
+ else
+ {
+ // initiate a new mailbox read check if required data is not available
+ if (ec_read_mbox_locked(slave)) {
+ // await current read request and mark the datagram as invalid
+ datagram->state = EC_DATAGRAM_INVALID;
+ }
+ else {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ voe->state = ec_voe_handler_state_read_check;
+ }
return;
}
- data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_voe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
@@ -421,6 +484,7 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
}
voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
+ memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, voe->data_size + EC_VOE_HEADER_SIZE);
voe->request_state = EC_INT_REQUEST_SUCCESS;
voe->state = ec_voe_handler_state_end; // success
}
@@ -489,8 +553,11 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
return;
}
- if (!(data = ec_slave_mbox_fetch(slave, datagram,
- &mbox_prot, &rec_size))) {
+ if (slave->mbox_voe_data.payload_size > 0) {
+ slave->mbox_voe_data.payload_size = 0;
+ data = ec_slave_mbox_fetch(slave, &slave->mbox_voe_data, &mbox_prot, &rec_size);
+ }
+ else {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
return;
@@ -519,6 +586,7 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
}
voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
+ memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, voe->data_size + EC_VOE_HEADER_SIZE);
voe->request_state = EC_INT_REQUEST_SUCCESS;
voe->state = ec_voe_handler_state_end; // success
}
_______________________________________________
etherlab-dev mailing list
etherlab-dev@etherlab.org
http://lists.etherlab.org/mailman/listinfo/etherlab-dev
