diff --git a/master/datagram.c b/master/datagram.c index da508509..e051ac8b 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(KERN_CONT "invalid"); + break; default: printk("???"); } @@ -648,3 +651,104 @@ 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 96af32fa..eea19a37 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 f51dc92e..b70094e8 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 *); @@ -494,9 +495,14 @@ 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; + } } /*****************************************************************************/ @@ -515,12 +521,20 @@ 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; } @@ -538,6 +552,32 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */) * datagram. */ 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; @@ -550,17 +590,19 @@ 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 b3116b6a..b0d7d2da 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,16 @@ 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 +438,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 +446,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 +454,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 +527,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 +535,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 +543,58 @@ 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 +747,16 @@ 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 +779,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 +787,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 +795,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 +871,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 +879,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 +887,54 @@ 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 +1069,16 @@ 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 +1101,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 +1109,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 +1117,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 +1163,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 +1171,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 +1179,56 @@ 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 +1578,16 @@ 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 +1609,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 +1619,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 +1627,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 +1737,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 +1754,55 @@ 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 +1904,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 +1913,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 +1921,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 +1967,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 +1977,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 +1985,54 @@ 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 +2257,16 @@ 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 +2290,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 +2299,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 +2307,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 +2383,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 +2393,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 +2401,60 @@ 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 +2652,16 @@ 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 +2685,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 +2695,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 +2703,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 +2749,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 +2759,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 +2767,59 @@ 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 4788ab67..873c49c5 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,17 @@ 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 +357,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 +365,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 +373,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 +414,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 +422,58 @@ 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 4470c91c..dff70cd5 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,57 @@ 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 +571,16 @@ 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 +615,18 @@ 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 +715,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 +724,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 +732,16 @@ 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 +794,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 +802,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 +846,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 +854,56 @@ 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 +1038,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 +1045,17 @@ 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 80956432..e9014d84 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 e24fefe2..6637a285 100644 --- a/master/fsm_slave_scan.c +++ b/master/fsm_slave_scan.c @@ -1047,6 +1047,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 8ffbd5c3..a626de7d 100644 --- a/master/fsm_soe.c +++ b/master/fsm_soe.c @@ -66,11 +66,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 *); @@ -326,9 +328,17 @@ 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; + } } /*****************************************************************************/ @@ -349,6 +359,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); @@ -357,6 +368,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); @@ -365,11 +377,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); @@ -397,11 +420,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. @@ -410,6 +428,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); @@ -417,14 +436,57 @@ 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); @@ -650,9 +712,17 @@ void ec_fsm_soe_write_request( } else { // all fragments sent; query response 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; + } } } @@ -674,6 +744,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); @@ -682,6 +753,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); @@ -689,10 +761,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); @@ -720,11 +803,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. @@ -733,6 +811,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); @@ -741,14 +820,57 @@ 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 d894bae5..8ceda622 100644 --- a/master/mailbox.c +++ b/master/mailbox.c @@ -164,18 +164,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); } @@ -186,12 +191,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 - "); @@ -208,13 +213,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 18a5d4ab..fc1f0a6f 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 db1e1082..2936a646 100644 --- a/master/master.c +++ b/master/master.c @@ -48,6 +48,7 @@ #include "slave_config.h" #include "device.h" #include "datagram.h" +#include "mailbox.h" #ifdef EC_EOE #if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0) @@ -969,8 +970,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; + } } /*****************************************************************************/ @@ -1163,10 +1166,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) { @@ -1208,6 +1212,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; @@ -1264,9 +1270,84 @@ 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; @@ -2528,6 +2609,9 @@ void ecrt_master_deactivate(ec_master_t *master) // set states for all slaves ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); + // clear read_mbox_busy flag in case slave CoE FSM were interrupted + ec_read_mbox_lock_clear(slave); + // mark for reconfiguration, because the master could have no // possibility for a reconfiguration between two sequential operation // phases. diff --git a/master/slave.c b/master/slave.c index db294c6a..0736e586 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 8a0ddbac..46ece170 100644 --- a/master/slave.h +++ b/master/slave.h @@ -39,6 +39,7 @@ #include #include +#include #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 6e0a39b1..ea67c2e4 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,19 @@ 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 +333,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 +342,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 +351,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 +390,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 +404,53 @@ 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 +480,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, rec_size); voe->request_state = EC_INT_REQUEST_SUCCESS; voe->state = ec_voe_handler_state_end; // success } @@ -489,8 +549,10 @@ 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 +581,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, rec_size); voe->request_state = EC_INT_REQUEST_SUCCESS; voe->state = ec_voe_handler_state_end; // success }