diff --git a/master/fsm_master.c b/master/fsm_master.c index c1cb5b46..47ddb94d 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -1135,6 +1135,9 @@ void ec_fsm_master_state_scan_slave( if (ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan)) { return; } + // Assume that the slaves mailbox data is valid even if the slave scanning skipped + // the clear mailbox state, e.g. if the slave refused to enter state INIT. + fsm->slave->valid_mbox_data = 1; #ifdef EC_EOE if (slave->sii.mailbox_protocols & EC_MBOX_EOE) { diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c index 6637a285..66a7795d 100644 --- a/master/fsm_slave_scan.c +++ b/master/fsm_slave_scan.c @@ -55,6 +55,7 @@ void ec_fsm_slave_scan_state_assign_sii(ec_fsm_slave_scan_t *); #endif void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *); void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *); +void ec_fsm_slave_scan_state_mailbox_cleared(ec_fsm_slave_scan_t *); #ifdef EC_REGALIAS void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *); #endif @@ -70,6 +71,7 @@ void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *); void ec_fsm_slave_scan_enter_regalias(ec_fsm_slave_scan_t *); #endif void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *); +void ec_fsm_slave_scan_enter_clear_mailbox(ec_fsm_slave_scan_t *); void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *); /*****************************************************************************/ @@ -979,7 +981,7 @@ void ec_fsm_slave_scan_state_preop( return; } - ec_fsm_slave_scan_enter_pdos(fsm); + ec_fsm_slave_scan_enter_clear_mailbox(fsm); } /*****************************************************************************/ @@ -1050,6 +1052,51 @@ void ec_fsm_slave_scan_state_sync( // 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_clear_mailbox(fsm); +} + +/*****************************************************************************/ + +/** Enter slave scan state: Clear Mailbox. + */ +void ec_fsm_slave_scan_enter_clear_mailbox( + ec_fsm_slave_scan_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + + // If there is some old data in the slave's mailbox, read it out and + // discard it. We don't need to check the mailbox first, we just ignore + // an error or empty mailbox response. + ec_datagram_t *datagram = fsm->datagram; + ec_slave_mbox_prepare_fetch(fsm->slave, datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_scan_state_mailbox_cleared; + + slave->valid_mbox_data = 0; +} + +/*****************************************************************************/ + +/** Slave scan state: Mailbox cleared. + */ +void ec_fsm_slave_scan_state_mailbox_cleared(ec_fsm_slave_scan_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = fsm->datagram; + + if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { + ec_slave_mbox_prepare_fetch(fsm->slave, datagram); + return; + } + + if (unlikely(slave->master->debug_level > 0) + && datagram->state == EC_DATAGRAM_RECEIVED + && datagram->working_counter == 1) + EC_SLAVE_INFO(slave, "Cleared old data from the mailbox\n"); + + slave->valid_mbox_data = 1; + ec_fsm_slave_scan_enter_pdos(fsm); } diff --git a/master/master.c b/master/master.c index 5ba49378..a2a02dc8 100644 --- a/master/master.c +++ b/master/master.c @@ -1286,67 +1286,72 @@ void ec_master_receive_datagrams( 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) { + if (slave->valid_mbox_data) { + datagram_mbox_prot = EC_READ_U8(cur_data + 5) & 0x0F; + switch (datagram_mbox_prot) { #ifdef EC_EOE - case EC_MBOX_TYPE_EOE: - // check EOE type and store in correct handlers mbox data cache - eoe_type = EC_READ_U8(cur_data + 6) & 0x0F; + case EC_MBOX_TYPE_EOE: + // check EOE type and store in correct handlers mbox data cache + eoe_type = EC_READ_U8(cur_data + 6) & 0x0F; - switch (eoe_type) { + switch (eoe_type) { - case EC_EOE_TYPE_FRAME_FRAG: - // EoE Frame Fragment handler - if ((slave->mbox_eoe_frag_data.data) && (data_size <= slave->mbox_eoe_frag_data.data_size)) { - memcpy(slave->mbox_eoe_frag_data.data, cur_data, data_size); - slave->mbox_eoe_frag_data.payload_size = data_size; - } - break; - case EC_EOE_TYPE_INIT_RES: - // EoE Init / Set IP response handler - if ((slave->mbox_eoe_init_data.data) && (data_size <= slave->mbox_eoe_init_data.data_size)) { - memcpy(slave->mbox_eoe_init_data.data, cur_data, data_size); - slave->mbox_eoe_init_data.payload_size = data_size; - } - break; - default: - EC_MASTER_DBG(master, 1, "Unhandled EoE protocol type from slave: %u Protocol: %u, Type: %x\n", - datagram_slave_addr, datagram_mbox_prot, eoe_type); - // copy instead received data into the datagram memory. - memcpy(datagram->data, cur_data, data_size); - break; - } - break; + case EC_EOE_TYPE_FRAME_FRAG: + // EoE Frame Fragment handler + if ((slave->mbox_eoe_frag_data.data) && (data_size <= slave->mbox_eoe_frag_data.data_size)) { + memcpy(slave->mbox_eoe_frag_data.data, cur_data, data_size); + slave->mbox_eoe_frag_data.payload_size = data_size; + } + break; + case EC_EOE_TYPE_INIT_RES: + // EoE Init / Set IP response handler + if ((slave->mbox_eoe_init_data.data) && (data_size <= slave->mbox_eoe_init_data.data_size)) { + memcpy(slave->mbox_eoe_init_data.data, cur_data, data_size); + slave->mbox_eoe_init_data.payload_size = data_size; + } + break; + default: + EC_MASTER_DBG(master, 1, "Unhandled EoE protocol type from slave: %u Protocol: %u, Type: %x\n", + datagram_slave_addr, datagram_mbox_prot, eoe_type); + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + break; + } + 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; + 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; } - 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); + } else { // 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. @@ -1405,7 +1410,6 @@ void ec_master_output_stats(ec_master_t *master /**< EtherCAT master */) { if (unlikely(jiffies - master->stats.output_jiffies >= HZ)) { master->stats.output_jiffies = jiffies; - if (master->stats.timeouts) { EC_MASTER_WARN(master, "%u datagram%s TIMED OUT!\n", master->stats.timeouts, diff --git a/master/slave.c b/master/slave.c index 46c73228..5a61d99d 100644 --- a/master/slave.c +++ b/master/slave.c @@ -176,6 +176,8 @@ void ec_slave_init( ec_mbox_data_init(&slave->mbox_foe_data); ec_mbox_data_init(&slave->mbox_soe_data); ec_mbox_data_init(&slave->mbox_voe_data); + + slave->valid_mbox_data = 0; } diff --git a/master/slave.h b/master/slave.h index 5eb25276..6fe5da5c 100644 --- a/master/slave.h +++ b/master/slave.h @@ -268,6 +268,8 @@ struct ec_slave 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. */ + + uint8_t valid_mbox_data; /**< Received mailbox data is valid. */ }; /*****************************************************************************/