bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Applied 0019-Support-for-multiple-mailbox-protocols.patch

This commit is contained in:
Florian Pose 2022-06-23 11:54:10 +02:00
parent 429ba9356e
commit f943348d98
15 changed files with 1325 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
master/datagram.c
View File

@ -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);
}
}
}
/*****************************************************************************/

18
master/datagram.h
View File

@ -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

66
master/ethernet.c
View File

@ -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++;

617
master/fsm_coe.c
View File

@ -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;

82
master/fsm_eoe.c
View File

@ -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;

182
master/fsm_foe.c
View File

@ -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;
}
}
}

7
master/fsm_slave_config.c
View File

@ -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;

3
master/fsm_slave_scan.c
View File

@ -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);
}

184
master/fsm_soe.c
View File

@ -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);

19
master/mailbox.c
View File

@ -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;
}
/*****************************************************************************/

2
master/mailbox.h
View File

@ -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 *);
/*****************************************************************************/

98
master/master.c
View File

@ -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.

53
master/slave.c
View File

@ -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);
}

15
master/slave.h
View File

@ -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

95
master/voe_handler.c
View File

@ -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
}