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

merged

This commit is contained in:
Martin Troxler 2011-03-14 16:57:47 +01:00
parent dda18aa1f6 ae09c143c9
commit b65f4f7cde
23 changed files with 579 additions and 370 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
configure.ac
View File

@ -512,6 +512,31 @@ if test "x${hrtimer}" = "x1"; then
AC_DEFINE([EC_USE_HRTIMER], [1], [Use hrtimer for scheduling])
fi
#------------------------------------------------------------------------------
# Use 2 datagrams (payload+last-byte) when sending to mailbox (reduces frame size)
#------------------------------------------------------------------------------
AC_ARG_ENABLE([mboxframesize],
AS_HELP_STRING([--enable-mboxframesize],
[Reduced frame size when sending to mailbox, uses 2 datagrams (default: no)]),
[
case "${enableval}" in
yes) mboxframesize=1
;;
no) mboxframesize=0
;;
*) AC_MSG_ERROR([Invalid value for --enable-mboxframesize])
;;
esac
],
[mboxframesize=0]
)
if test "x${mboxframesize}" = "x1"; then
AC_DEFINE([EC_REDUCE_MBOXFRAMESIZE], [1], [Reduced frame size when sending to mailbox])
fi
#------------------------------------------------------------------------------
# Read alias address from register
#------------------------------------------------------------------------------

25
master/ethernet.c
View File

@ -111,6 +111,7 @@ int ec_eoe_init(
eoe->slave = slave;
ec_datagram_init(&eoe->datagram);
ec_mbox_init(&eoe->mbox,&eoe->datagram);
eoe->queue_datagram = 0;
eoe->state = ec_eoe_state_rx_start;
eoe->opened = 0;
@ -220,6 +221,7 @@ void ec_eoe_clear(ec_eoe_t *eoe /**< EoE handler */)
free_netdev(eoe->dev);
ec_mbox_clear(&eoe->mbox);
ec_datagram_clear(&eoe->datagram);
}
@ -294,7 +296,7 @@ int ec_eoe_send(ec_eoe_t *eoe /**< EoE handler */)
printk("\n");
#endif
data = ec_slave_mbox_prepare_send(eoe->slave, &eoe->datagram,
data = ec_slave_mbox_prepare_send(eoe->slave, &eoe->mbox,
0x02, current_size + 4);
if (IS_ERR(data))
return PTR_ERR(data);
@ -324,7 +326,8 @@ void ec_eoe_run(ec_eoe_t *eoe /**< EoE handler */)
// if the datagram was not sent, or is not yet received, skip this cycle
if (eoe->queue_datagram ||
eoe->datagram.state == EC_DATAGRAM_SENT || eoe->datagram.state == EC_DATAGRAM_QUEUED)
ec_mbox_is_datagram_state(&eoe->mbox,EC_DATAGRAM_QUEUED) ||
ec_mbox_is_datagram_state(&eoe->mbox,EC_DATAGRAM_SENT))
return;
// call state function
@ -349,7 +352,7 @@ void ec_eoe_run(ec_eoe_t *eoe /**< EoE handler */)
void ec_eoe_queue(ec_eoe_t *eoe /**< EoE handler */)
{
if (eoe->queue_datagram) {
ec_master_queue_fsm_datagram(eoe->slave->master, &eoe->datagram);
ec_master_mbox_queue_datagrams(eoe->slave->master, &eoe->mbox);
eoe->queue_datagram = 0;
}
}
@ -395,7 +398,7 @@ void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */)
return;
}
ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
ec_slave_mbox_prepare_check(eoe->slave, &eoe->mbox);
eoe->queue_datagram = 1;
eoe->state = ec_eoe_state_rx_check;
}
@ -409,7 +412,7 @@ void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */)
*/
void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
{
if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(&eoe->mbox,EC_DATAGRAM_RECEIVED)) {
eoe->stats.rx_errors++;
#if EOE_DEBUG_LEVEL >= 1
EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
@ -419,14 +422,14 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
return;
}
if (!ec_slave_mbox_check(&eoe->datagram)) {
if (!ec_slave_mbox_check(&eoe->mbox)) {
eoe->rx_idle = 1;
eoe->state = ec_eoe_state_tx_start;
return;
}
eoe->rx_idle = 0;
ec_slave_mbox_prepare_fetch(eoe->slave, &eoe->datagram);
ec_slave_mbox_prepare_fetch(eoe->slave, &eoe->mbox);
eoe->queue_datagram = 1;
eoe->state = ec_eoe_state_rx_fetch;
}
@ -448,7 +451,7 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
unsigned int i;
#endif
if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(&eoe->mbox,EC_DATAGRAM_RECEIVED)) {
eoe->stats.rx_errors++;
#if EOE_DEBUG_LEVEL >= 1
EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
@ -458,7 +461,7 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
return;
}
data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram,
data = ec_slave_mbox_fetch(eoe->slave, &eoe->mbox,
&mbox_prot, &rec_size);
if (IS_ERR(data)) {
eoe->stats.rx_errors++;
@ -685,7 +688,7 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */)
*/
void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */)
{
if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(&eoe->mbox,EC_DATAGRAM_RECEIVED)) {
if (eoe->tries) {
eoe->tries--; // try again
eoe->queue_datagram = 1;
@ -701,7 +704,7 @@ void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */)
return;
}
if (eoe->datagram.working_counter != 1) {
if (!ec_mbox_is_datagram_wc(&eoe->mbox,1)) {
if (eoe->tries) {
eoe->tries--; // try again
eoe->queue_datagram = 1;

1
master/ethernet.h
View File

@ -73,6 +73,7 @@ struct ec_eoe
struct list_head list; /**< list item */
ec_slave_t *slave; /**< pointer to the corresponding slave */
ec_datagram_t datagram; /**< datagram */
ec_mailbox_t mbox; /**< mailbox */
unsigned int queue_datagram; /**< the datagram is ready for queuing */
void (*state)(ec_eoe_t *); /**< state function for the state machine */
struct net_device *dev; /**< net_device for virtual ethernet device */

320
master/fsm_coe.c
View File

File diff suppressed because it is too large Load Diff

5
master/fsm_coe.h
View File

@ -39,6 +39,7 @@
#include "globals.h"
#include "datagram.h"
#include "mailbox.h"
#include "slave.h"
#include "sdo.h"
#include "sdo_request.h"
@ -51,7 +52,7 @@ typedef struct ec_fsm_coe ec_fsm_coe_t; /**< \see ec_fsm_coe */
*/
struct ec_fsm_coe {
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
ec_mailbox_t *mbox; /**< mailbox used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_coe_t *); /**< CoE state function */
@ -67,7 +68,7 @@ struct ec_fsm_coe {
/*****************************************************************************/
void ec_fsm_coe_init(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_init(ec_fsm_coe_t *, ec_mailbox_t *);
void ec_fsm_coe_clear(ec_fsm_coe_t *);
void ec_fsm_coe_dictionary(ec_fsm_coe_t *, ec_slave_t *);

94
master/fsm_foe.c
View File

@ -107,11 +107,11 @@ void ec_fsm_foe_read_start(ec_fsm_foe_t *);
/** Constructor.
*/
void ec_fsm_foe_init(ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< datagram */
ec_mailbox_t *mbox /**< mailbox */
)
{
fsm->state = NULL;
fsm->datagram = datagram;
fsm->state = NULL;
fsm->mbox = mbox;
}
/*****************************************************************************/
@ -207,7 +207,7 @@ int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)
}
data = ec_slave_mbox_prepare_send(fsm->slave,
fsm->datagram, EC_MBOX_TYPE_FILEACCESS,
fsm->mbox,EC_MBOX_TYPE_FILEACCESS,
current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
return -1;
@ -238,7 +238,7 @@ int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)
current_size = fsm->tx_filename_len;
data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
data = ec_slave_mbox_prepare_send(fsm->slave, fsm->mbox,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
return -1;
@ -308,21 +308,22 @@ void ec_fsm_foe_state_ack_check(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_ack_check()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
@ -331,7 +332,7 @@ void ec_fsm_foe_state_ack_check(
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(mbox)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
@ -340,13 +341,13 @@ void ec_fsm_foe_state_ack_check(
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_read;
@ -360,7 +361,8 @@ void ec_fsm_foe_state_ack_read(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
@ -370,21 +372,21 @@ void ec_fsm_foe_state_ack_read(
printk("ec_fsm_foe_ack_read()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,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(datagram);
return;
}
if (!(data = ec_slave_mbox_fetch(fsm->slave, datagram,
if (!(data = ec_slave_mbox_fetch(fsm->slave, fsm->mbox,
&mbox_prot, &rec_size))) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
@ -440,21 +442,22 @@ void ec_fsm_foe_state_wrq_sent(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_foe_state_sent_wrq()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE WRQ: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE WRQ failed: ");
@ -464,7 +467,7 @@ void ec_fsm_foe_state_wrq_sent(
fsm->jiffies_start = datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(fsm->slave, fsm->mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
@ -481,28 +484,29 @@ void ec_fsm_foe_state_data_sent(
ec_fsm_foe_t *fsm /**< Foe statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_state_data_sent()\n");
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
ec_foe_set_tx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE data send failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
ec_slave_mbox_prepare_check(fsm->slave, fsm->datagram);
ec_slave_mbox_prepare_check(slave, mbox);
fsm->jiffies_start = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
@ -519,7 +523,7 @@ int ec_foe_prepare_rrq_send(ec_fsm_foe_t *fsm)
current_size = fsm->rx_filename_len;
data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
data = ec_slave_mbox_prepare_send(fsm->slave, fsm->mbox,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
return -1;
@ -546,7 +550,7 @@ int ec_foe_prepare_send_ack(
{
uint8_t *data;
data = ec_slave_mbox_prepare_send(foe->slave, foe->datagram,
data = ec_slave_mbox_prepare_send(foe->slave, foe->mbox,
EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
return -1;
@ -568,21 +572,22 @@ void ec_fsm_foe_state_rrq_sent(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_foe_state_rrq_sent()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
@ -592,7 +597,7 @@ void ec_fsm_foe_state_rrq_sent(
fsm->jiffies_start = datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(fsm->slave, fsm->mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
@ -657,28 +662,29 @@ void ec_fsm_foe_state_data_check(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_state_data_check()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: ");
ec_datagram_print_wc_error(datagram);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(mbox)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
@ -687,13 +693,13 @@ void ec_fsm_foe_state_data_check(
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_read;
@ -711,28 +717,29 @@ void ec_fsm_foe_state_data_read(
size_t rec_size;
uint8_t *data, opCode, packet_no, mbox_prot;
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_fsm_foe_state_data_read()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,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(datagram);
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size))) {
if (!(data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size))) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
}
@ -831,21 +838,22 @@ void ec_fsm_foe_state_sent_ack(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_datagram_t *datagram = mbox->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
printk("ec_foe_state_sent_ack()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE ACK: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
// slave did not put anything into the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ACK failed: ");
@ -855,7 +863,7 @@ void ec_fsm_foe_state_sent_ack(
fsm->jiffies_start = datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(fsm->slave, fsm->mbox); // can not fail.
if (fsm->rx_last_packet) {
fsm->rx_expected_packet_no = 0;

4
master/fsm_foe.h
View File

@ -51,7 +51,7 @@ typedef struct ec_fsm_foe ec_fsm_foe_t; /**< \see ec_fsm_foe */
*/
struct ec_fsm_foe {
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
ec_mailbox_t *mbox; /**< mailbox used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_foe_t *); /**< FoE state function */
@ -80,7 +80,7 @@ struct ec_fsm_foe {
/*****************************************************************************/
void ec_fsm_foe_init(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_init(ec_fsm_foe_t *, ec_mailbox_t *);
void ec_fsm_foe_clear(ec_fsm_foe_t *);
int ec_fsm_foe_exec(ec_fsm_foe_t *);

8
master/fsm_master.c
View File

@ -85,6 +85,7 @@ void ec_fsm_master_init(
{
fsm->master = master;
fsm->datagram = datagram;
fsm->mbox = &master->fsm_mbox;
fsm->state = ec_fsm_master_state_start;
fsm->idle = 0;
fsm->link_state = 0;
@ -93,7 +94,7 @@ void ec_fsm_master_init(
fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
ec_fsm_coe_init(&fsm->fsm_coe, fsm->mbox);
ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);
ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,
@ -133,12 +134,11 @@ int ec_fsm_master_exec(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
if (fsm->datagram->state == EC_DATAGRAM_SENT
|| fsm->datagram->state == EC_DATAGRAM_QUEUED) {
if (ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_QUEUED)
|| ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_SENT)) {
// datagram was not sent or received yet.
return 0;
}
fsm->state(fsm);
return 1;
}

2
master/fsm_master.h
View File

@ -39,6 +39,7 @@
#include "globals.h"
#include "datagram.h"
#include "mailbox.h"
#include "foe_request.h"
#include "sdo_request.h"
#include "soe_request.h"
@ -127,6 +128,7 @@ typedef struct ec_fsm_master ec_fsm_master_t; /**< \see ec_fsm_master */
struct ec_fsm_master {
ec_master_t *master; /**< master the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
ec_mailbox_t* mbox; /**< mailbox used in the CoE state machine */
unsigned int retries; /**< retries on datagram timeout. */
void (*state)(ec_fsm_master_t *); /**< master state function */

28
master/fsm_slave.c
View File

@ -57,21 +57,21 @@ void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
void ec_fsm_slave_init(
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_slave_t *slave, /**< EtherCAT slave. */
ec_datagram_t *datagram /**< Datagram object to use. */
ec_mailbox_t *mbox/**< Datagram object to use. */
)
{
fsm->slave = slave;
fsm->datagram = datagram;
fsm->datagram->data_size = 0;
fsm->mbox = mbox;
slave->datagram.data_size = 0;
EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
fsm->state = ec_fsm_slave_state_idle;
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
ec_fsm_coe_init(&fsm->fsm_coe, fsm->mbox);
ec_fsm_foe_init(&fsm->fsm_foe, fsm->mbox);
ec_fsm_soe_init(&fsm->fsm_soe, fsm->mbox);
}
/*****************************************************************************/
@ -101,8 +101,8 @@ int ec_fsm_slave_exec(
ec_fsm_slave_t *fsm /**< Slave state machine. */
)
{
if (fsm->datagram->state == EC_DATAGRAM_SENT
|| fsm->datagram->state == EC_DATAGRAM_QUEUED) {
if (ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_QUEUED)
|| ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_SENT)) {
// datagram was not sent or received yet.
return 0;
}
@ -208,7 +208,7 @@ int ec_fsm_slave_action_process_sdo(
fsm->state = ec_fsm_slave_state_sdo_request;
ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
return 1;
}
return 0;
@ -227,7 +227,7 @@ void ec_fsm_slave_state_sdo_request(
if (ec_fsm_coe_exec(&fsm->fsm_coe))
{
ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
return;
}
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
@ -285,7 +285,7 @@ int ec_fsm_slave_action_process_foe(
fsm->state = ec_fsm_slave_state_foe_request;
ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
ec_fsm_foe_exec(&fsm->fsm_foe);
ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
return 1;
}
return 0;
@ -304,7 +304,7 @@ void ec_fsm_slave_state_foe_request(
if (ec_fsm_foe_exec(&fsm->fsm_foe))
{
ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
return;
}
@ -376,7 +376,7 @@ int ec_fsm_slave_action_process_soe(
fsm->state = ec_fsm_slave_state_soe_request;
ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req);
ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
ec_master_queue_request_fsm_datagram(fsm->slave->master, fsm->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
return 1;
}
return 0;
@ -394,7 +394,7 @@ void ec_fsm_slave_state_soe_request(
ec_master_soe_request_t *request = fsm->soe_request;
if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
ec_master_queue_request_fsm_datagram(fsm->slave->master, fsm->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
return;
}

4
master/fsm_slave.h
View File

@ -50,7 +50,7 @@ typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */
*/
struct ec_fsm_slave {
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
ec_mailbox_t *mbox; /**< mailbox used in the state machine */
void (*state)(ec_fsm_slave_t *); /**< master state function */
ec_master_sdo_request_t *sdo_request; /**< SDO request to process. */
@ -65,7 +65,7 @@ struct ec_fsm_slave {
/*****************************************************************************/
void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *, ec_datagram_t *);
void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *, ec_mailbox_t *);
void ec_fsm_slave_clear(ec_fsm_slave_t *);
int ec_fsm_slave_exec(ec_fsm_slave_t *);

20
master/fsm_slave_config.c
View File

@ -177,8 +177,8 @@ int ec_fsm_slave_config_exec(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
if (fsm->datagram->state == EC_DATAGRAM_SENT
|| fsm->datagram->state == EC_DATAGRAM_QUEUED) {
if (fsm->datagram->state == EC_DATAGRAM_QUEUED
|| fsm->datagram->state == EC_DATAGRAM_SENT) {
// datagram was not sent or received yet.
return ec_fsm_slave_config_running(fsm);
}
@ -738,8 +738,7 @@ void ec_fsm_slave_config_enter_soe_conf_preop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_request_fsm_datagram(slave->master,
fsm_soe->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox);
return;
}
}
@ -760,7 +759,7 @@ void ec_fsm_slave_config_state_soe_conf_preop(
ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
if (ec_fsm_soe_exec(fsm_soe)) {
ec_master_queue_request_fsm_datagram(slave->master, fsm_soe->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox);
return;
}
@ -785,8 +784,7 @@ void ec_fsm_slave_config_state_soe_conf_preop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_request_fsm_datagram(slave->master,
fsm_soe->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox);
return;
}
}
@ -1454,8 +1452,7 @@ void ec_fsm_slave_config_enter_soe_conf_safeop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_request_fsm_datagram(slave->master,
fsm_soe->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox);
return;
}
}
@ -1476,7 +1473,7 @@ void ec_fsm_slave_config_state_soe_conf_safeop(
ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
if (ec_fsm_soe_exec(fsm_soe)) {
ec_master_queue_request_fsm_datagram(slave->master, fsm_soe->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox);
return;
}
@ -1501,8 +1498,7 @@ void ec_fsm_slave_config_state_soe_conf_safeop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_request_fsm_datagram(slave->master,
fsm_soe->datagram);
ec_slave_mbox_queue_datagrams(slave, fsm_soe->mbox);
return;
}
}

4
master/fsm_slave_scan.c
View File

@ -137,8 +137,8 @@ int ec_fsm_slave_scan_running(const ec_fsm_slave_scan_t *fsm /**< slave state ma
int ec_fsm_slave_scan_exec(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
{
if (fsm->datagram->state == EC_DATAGRAM_SENT
|| fsm->datagram->state == EC_DATAGRAM_QUEUED) {
if (fsm->datagram->state == EC_DATAGRAM_QUEUED
|| fsm->datagram->state == EC_DATAGRAM_SENT) {
// datagram was not sent or received yet.
return ec_fsm_slave_scan_running(fsm);
}

120
master/fsm_soe.c
View File

@ -106,11 +106,11 @@ void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
*/
void ec_fsm_soe_init(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< datagram */
ec_mailbox_t *mbox /**< mailbox */
)
{
fsm->state = NULL;
fsm->datagram = datagram;
fsm->mbox = mbox;
}
/*****************************************************************************/
@ -195,7 +195,7 @@ void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
*/
void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *request = fsm->request;
@ -211,7 +211,7 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
return;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,
EC_SOE_SIZE);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
@ -240,25 +240,25 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
*/
void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
if (datagram->working_counter != 1) {
if (!datagram->working_counter) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
if (ec_mbox_is_datagram_wc(mbox,0)) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
return;
@ -267,13 +267,13 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read request"
" failed after %lu ms: ", diff_ms);
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->jiffies_start = mbox->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
}
@ -284,32 +284,32 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
*/
void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(mbox)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
(mbox->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
@ -318,13 +318,13 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_response;
}
@ -335,7 +335,7 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
*/
void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
@ -343,26 +343,26 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
size_t rec_size, data_size;
ec_soe_request_t *req = fsm->request;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@ -435,8 +435,8 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
if (incomplete) {
EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
" at offset %zu.\n", req->data_size);
fsm->jiffies_start = datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->jiffies_start = mbox->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
} else {
@ -459,7 +459,7 @@ void ec_fsm_soe_write_next_fragment(
ec_fsm_soe_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
@ -485,7 +485,7 @@ void ec_fsm_soe_write_next_fragment(
fragments_left++;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,
EC_SOE_SIZE + fragment_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
@ -539,25 +539,25 @@ void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
*/
void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
if (datagram->working_counter != 1) {
if (!datagram->working_counter) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
if (ec_mbox_is_datagram_wc(mbox,0)) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
return;
@ -566,14 +566,14 @@ void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write request"
" failed after %lu ms: ", diff_ms);
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->jiffies_start = mbox->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_check;
}
@ -584,25 +584,25 @@ void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
*/
void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *req = fsm->request;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
@ -610,9 +610,9 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
if (fsm->offset < req->data_size) {
ec_fsm_soe_write_next_fragment(fsm);
} else {
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(mbox)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
(mbox->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
@ -621,13 +621,13 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_response;
}
@ -639,7 +639,7 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
*/
void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_mailbox_t *mbox = fsm->mbox;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
@ -647,27 +647,27 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
uint16_t idn;
size_t rec_size;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write"
" response datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);

4
master/fsm_soe.h
View File

@ -50,7 +50,7 @@ typedef struct ec_fsm_soe ec_fsm_soe_t; /**< \see ec_fsm_soe */
*/
struct ec_fsm_soe {
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
ec_mailbox_t *mbox; /**< mailbox used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_soe_t *); /**< CoE state function */
@ -61,7 +61,7 @@ struct ec_fsm_soe {
/*****************************************************************************/
void ec_fsm_soe_init(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_init(ec_fsm_soe_t *, ec_mailbox_t *);
void ec_fsm_soe_clear(ec_fsm_soe_t *);
void ec_fsm_soe_transfer(ec_fsm_soe_t *, ec_slave_t *, ec_soe_request_t *);

124
master/mailbox.c
View File

@ -37,10 +37,84 @@
#include <linux/slab.h>
#include <linux/delay.h>
#include "slave.h"
#include "mailbox.h"
#include "datagram.h"
#include "master.h"
/*****************************************************************************/
/**
Mailbox constructor.
*/
void ec_mbox_init(ec_mailbox_t* mbox, /** mailbox */
ec_datagram_t* datagram /**< Datagram used for the mailbox content. */
)
{
mbox->datagram = datagram;
#ifdef EC_REDUCE_MBOXFRAMESIZE
ec_datagram_init(&mbox->end_datagram);
#endif
}
/*****************************************************************************/
/**
Clears mailbox datagrams.
*/
void ec_mbox_clear(ec_mailbox_t* mbox /** mailbox */
)
{
#ifdef EC_REDUCE_MBOXFRAMESIZE
ec_datagram_clear(&mbox->end_datagram);
#endif
}
/*****************************************************************************/
/**
Queues the slave datagrams.
*/
void ec_slave_mbox_queue_datagrams(const ec_slave_t* slave, /** slave */
ec_mailbox_t* mbox /** mailbox */
)
{
ec_master_queue_request_fsm_datagram(slave->master, mbox->datagram);
#ifdef EC_REDUCE_MBOXFRAMESIZE
if (mbox->end_datagram.type != EC_DATAGRAM_NONE)
{
ec_master_queue_request_fsm_datagram(slave->master, &mbox->end_datagram);
}
#endif
}
/*****************************************************************************/
/**
Queues the datagrams.
*/
void ec_master_mbox_queue_datagrams(ec_master_t* master, /** master */
ec_mailbox_t* mbox /** mailbox */
)
{
ec_master_queue_fsm_datagram(master, mbox->datagram);
#ifdef EC_REDUCE_MBOXFRAMESIZE
if (mbox->end_datagram.type != EC_DATAGRAM_NONE)
{
ec_master_queue_fsm_datagram(master, &mbox->end_datagram);
}
#endif
}
/*****************************************************************************/
/**
@ -48,12 +122,13 @@
\return Pointer to mailbox datagram data, or ERR_PTR() code.
*/
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
ec_datagram_t *datagram, /**< datagram */
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t* slave, /** slave */
ec_mailbox_t* mbox, /** mailbox */
uint8_t type, /**< mailbox protocol */
size_t size /**< size of the data */
)
{
ec_datagram_t* datagram = mbox->datagram;
size_t total_size;
int ret;
@ -72,8 +147,13 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
}
ret = ec_datagram_fpwr(datagram, slave->station_address,
slave->configured_rx_mailbox_offset,
slave->configured_rx_mailbox_size);
slave->configured_rx_mailbox_offset,
#ifdef EC_REDUCE_MBOXFRAMESIZE
total_size
#else
slave->configured_rx_mailbox_size
#endif
);
if (ret)
return ERR_PTR(ret);
@ -82,6 +162,17 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority
EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type
#ifdef EC_REDUCE_MBOXFRAMESIZE
/* in order to fulfil the ESC's mailbox protocol,
at least the last byte of the mailbox must be written */
if (total_size < slave->configured_rx_mailbox_size) {
ret = ec_datagram_fpwr(&mbox->end_datagram, slave->station_address,
slave->configured_rx_mailbox_offset+slave->configured_rx_mailbox_size-1,
1);
if (ret)
return ERR_PTR(ret);
}
#endif
return datagram->data + EC_MBOX_HEADER_SIZE;
}
@ -93,15 +184,19 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
\return 0 in case of success, else < 0
*/
int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */
ec_datagram_t *datagram /**< datagram */
int ec_slave_mbox_prepare_check(const ec_slave_t* slave, /** slave */
ec_mailbox_t* mbox /** mailbox */
)
{
ec_datagram_t* datagram = mbox->datagram;
int ret = ec_datagram_fprd(datagram, slave->station_address, 0x808, 8);
if (ret)
return ret;
ec_datagram_zero(datagram);
#ifdef EC_REDUCE_MBOXFRAMESIZE
mbox->end_datagram.type = EC_DATAGRAM_NONE;
#endif
return 0;
}
@ -112,9 +207,9 @@ int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */
\return 0 in case of success, else < 0
*/
int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */)
int ec_slave_mbox_check(ec_mailbox_t* mbox /** mailbox */)
{
return EC_READ_U8(datagram->data + 5) & 8 ? 1 : 0;
return EC_READ_U8(mbox->datagram->data + 5) & 8 ? 1 : 0;
}
/*****************************************************************************/
@ -124,10 +219,11 @@ int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */)
\return 0 in case of success, else < 0
*/
int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */
ec_datagram_t *datagram /**< datagram */
int ec_slave_mbox_prepare_fetch(const ec_slave_t* slave, /** slave */
ec_mailbox_t* mbox /** mailbox */
)
{
ec_datagram_t* datagram = mbox->datagram;
int ret = ec_datagram_fprd(datagram, slave->station_address,
slave->configured_tx_mailbox_offset,
slave->configured_tx_mailbox_size);
@ -135,6 +231,9 @@ int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */
return ret;
ec_datagram_zero(datagram);
#ifdef EC_REDUCE_MBOXFRAMESIZE
mbox->end_datagram.type = EC_DATAGRAM_NONE;
#endif
return 0;
}
@ -162,12 +261,13 @@ 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 */
ec_datagram_t *datagram, /**< datagram */
uint8_t *ec_slave_mbox_fetch(const ec_slave_t* slave, /** slave */
ec_mailbox_t* mbox, /** mailbox */
uint8_t *type, /**< expected mailbox protocol */
size_t *size /**< size of the received data */
)
{
ec_datagram_t* datagram = mbox->datagram;
size_t data_size;
data_size = EC_READ_U16(datagram->data);

54
master/mailbox.h
View File

@ -37,7 +37,8 @@
#ifndef __EC_MAILBOX_H__
#define __EC_MAILBOX_H__
#include "slave.h"
#include "globals.h"
#include "datagram.h"
/*****************************************************************************/
@ -47,12 +48,53 @@
/*****************************************************************************/
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_datagram_t *,
/** EtherCAT slave mailbox.
*/
struct ec_mailbox
{
ec_datagram_t* datagram; /**< Datagram used for the mailbox content. */
#ifdef EC_REDUCE_MBOXFRAMESIZE
ec_datagram_t end_datagram; /**< Datagram used for writing the end byte to the mailbox. */
#endif
};
typedef struct ec_mailbox ec_mailbox_t; /**< \see ec_mailbox. */
/*****************************************************************************/
void ec_mbox_init(ec_mailbox_t *, ec_datagram_t*);
void ec_mbox_clear(ec_mailbox_t*);
/**
Checks the datagrams states.
*/
static inline int ec_mbox_is_datagram_state(ec_mailbox_t*mbox,
ec_datagram_state_t state) {
return (mbox->datagram->state == state)
#ifdef EC_REDUCE_MBOXFRAMESIZE
&& (mbox->end_datagram.type == EC_DATAGRAM_NONE || mbox->end_datagram.state == state)
#endif
;
}
/**
Checks the datagrams working counter.
*/
static inline int ec_mbox_is_datagram_wc(ec_mailbox_t*mbox,
size_t wc) {
return (mbox->datagram->working_counter == wc)
#ifdef EC_REDUCE_MBOXFRAMESIZE
&& (mbox->end_datagram.type == EC_DATAGRAM_NONE || mbox->end_datagram.working_counter == wc)
#endif
;
}
void ec_slave_mbox_queue_datagrams(const ec_slave_t*,ec_mailbox_t*);
void ec_master_mbox_queue_datagrams(ec_master_t*, ec_mailbox_t*);
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t*,ec_mailbox_t *,
uint8_t, size_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 *, ec_datagram_t *,
int ec_slave_mbox_prepare_check(const ec_slave_t*,ec_mailbox_t *);
int ec_slave_mbox_check(ec_mailbox_t *);
int ec_slave_mbox_prepare_fetch(const ec_slave_t*,ec_mailbox_t *);
uint8_t *ec_slave_mbox_fetch(const ec_slave_t*,ec_mailbox_t *,
uint8_t *, size_t *);
/*****************************************************************************/

8
master/master.c
View File

@ -228,6 +228,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
}
// create state machine object
ec_mbox_init(&master->fsm_mbox,&master->fsm_datagram);
ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
// init reference sync datagram
@ -329,7 +330,7 @@ void ec_master_clear(
#endif
ec_cdev_clear(&master->cdev);
#ifdef EC_EOE
ec_master_clear_eoe_handlers(master);
#endif
@ -341,6 +342,7 @@ void ec_master_clear(
ec_datagram_clear(&master->sync_datagram);
ec_datagram_clear(&master->ref_sync_datagram);
ec_fsm_master_clear(&master->fsm);
ec_mbox_clear(&master->fsm_mbox);
ec_datagram_clear(&master->fsm_datagram);
ec_device_clear(&master->backup_device);
ec_device_clear(&master->main_device);
@ -1279,7 +1281,7 @@ static int ec_master_idle_thread(void *priv_data)
if (ec_mutex_lock_interruptible(&master->master_mutex))
break;
if (ec_fsm_master_exec(&master->fsm)) {
ec_master_queue_fsm_datagram(master, &master->fsm_datagram);
ec_master_mbox_queue_datagrams(master, &master->fsm_mbox);
}
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
@ -1343,7 +1345,7 @@ static int ec_master_operation_thread(void *priv_data)
if (ec_mutex_lock_interruptible(&master->master_mutex))
break;
if (ec_fsm_master_exec(&master->fsm))
ec_master_queue_fsm_datagram(master, &master->fsm_datagram);
ec_master_mbox_queue_datagrams(master, &master->fsm_mbox);
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {

1
master/master.h
View File

@ -193,6 +193,7 @@ struct ec_master {
ec_fsm_master_t fsm; /**< Master state machine. */
ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
ec_mailbox_t fsm_mbox; /**< Mailbox used for state machines. */
ec_master_phase_t phase; /**< Master phase. */
unsigned int active; /**< Master has been activated. */
unsigned int config_changed; /**< The configuration changed. */

15
master/slave.c
View File

@ -159,19 +159,20 @@ void ec_slave_init(
INIT_LIST_HEAD(&slave->soe_requests);
init_waitqueue_head(&slave->soe_queue);
// init state machine datagram
ec_datagram_init(&slave->fsm_datagram);
snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE,
// init datagram
ec_datagram_init(&slave->datagram);
snprintf(slave->datagram.name, EC_DATAGRAM_NAME_SIZE,
"slave%u-fsm", slave->ring_position);
ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE);
ret = ec_datagram_prealloc(&slave->datagram, EC_MAX_DATA_SIZE);
if (ret < 0) {
ec_datagram_clear(&slave->fsm_datagram);
ec_datagram_clear(&slave->datagram);
EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n");
return;
}
ec_mbox_init(&slave->mbox,&slave->datagram);
// create state machine object
ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram);
ec_fsm_slave_init(&slave->fsm, slave, &slave->mbox);
}
/*****************************************************************************/
@ -255,7 +256,7 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
if (slave->sii_words)
kfree(slave->sii_words);
ec_fsm_slave_clear(&slave->fsm);
ec_datagram_clear(&slave->fsm_datagram);
ec_mbox_clear(&slave->mbox);
}
/*****************************************************************************/

4
master/slave.h
View File

@ -46,6 +46,7 @@
#include "sync.h"
#include "sdo.h"
#include "fsm_slave.h"
#include "mailbox.h"
/*****************************************************************************/
@ -274,7 +275,8 @@ struct ec_slave
wait_queue_head_t soe_queue; /**< Wait queue for SoE requests from user
space. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
ec_datagram_t datagram; /** Datagram used for data transfers */
ec_mailbox_t mbox; /**< Mailbox used for data transfers. */
};
/*****************************************************************************/

78
master/voe_handler.c
View File

@ -88,6 +88,7 @@ int ec_voe_handler_init(
voe->request_state = EC_INT_REQUEST_INIT;
ec_datagram_init(&voe->datagram);
ec_mbox_init(&voe->mbox,&voe->datagram);
return ec_datagram_prealloc(&voe->datagram,
size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
}
@ -100,6 +101,7 @@ void ec_voe_handler_clear(
ec_voe_handler_t *voe /**< VoE handler. */
)
{
ec_mbox_clear(&voe->mbox);
ec_datagram_clear(&voe->datagram);
}
@ -191,7 +193,7 @@ ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
if (voe->config->slave) { // FIXME locking?
voe->state(voe);
if (voe->request_state == EC_INT_REQUEST_BUSY)
ec_master_queue_datagram(voe->config->master, &voe->datagram);
ec_slave_mbox_queue_datagrams(voe->config->slave,&voe->mbox);
} else {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
@ -224,7 +226,7 @@ void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
return;
}
data = ec_slave_mbox_prepare_send(slave, &voe->datagram,
data = ec_slave_mbox_prepare_send(slave, &voe->mbox,
EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);
if (IS_ERR(data)) {
voe->state = ec_voe_handler_state_error;
@ -247,22 +249,22 @@ void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
*/
void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_mailbox_t *mbox = &voe->mbox;
ec_slave_t *slave = voe->config->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE write request datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
return;
}
if (datagram->working_counter != 1) {
if (!datagram->working_counter) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
if (ec_mbox_is_datagram_wc(mbox,0)) {
unsigned long diff_ms =
(jiffies - voe->jiffies_start) * 1000 / HZ;
if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {
@ -276,7 +278,7 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Reception of VoE write request failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
return;
}
@ -292,7 +294,7 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
*/
void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_mailbox_t *mbox = &voe->mbox;
ec_slave_t *slave = voe->config->slave;
EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");
@ -304,7 +306,7 @@ void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
voe->jiffies_start = jiffies;
voe->retries = EC_FSM_RETRIES;
@ -317,32 +319,32 @@ void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
*/
void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_mailbox_t *mbox = &voe->mbox;
ec_slave_t *slave = voe->config->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(mbox)) {
unsigned long diff_ms =
(datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
(mbox->datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
@ -350,13 +352,13 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
voe->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
voe->retries = EC_FSM_RETRIES;
voe->state = ec_voe_handler_state_read_response;
}
@ -367,32 +369,32 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
*/
void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_mailbox_t *mbox = &voe->mbox;
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--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,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);
ec_datagram_print_wc_error(mbox->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
@ -432,7 +434,7 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
*/
void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_mailbox_t *mbox = &voe->mbox;
ec_slave_t *slave = voe->config->slave;
EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");
@ -444,7 +446,7 @@ void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)
return;
}
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
voe->jiffies_start = jiffies;
voe->retries = EC_FSM_RETRIES;
@ -458,39 +460,39 @@ void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)
*/
void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_mailbox_t *mbox = &voe->mbox;
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--)
if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
ec_datagram_print_state(datagram);
ec_datagram_print_state(mbox->datagram);
return;
}
if (datagram->working_counter == 0) {
if (ec_mbox_is_datagram_wc(mbox,0)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_DBG(slave, 1, "Slave did not send VoE data.\n");
return;
}
if (datagram->working_counter != 1) {
if (!ec_mbox_is_datagram_wc(mbox,1)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_WARN(slave, "Reception of VoE read response failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(mbox->datagram);
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram,
if (!(data = ec_slave_mbox_fetch(slave, mbox,
&mbox_prot, &rec_size))) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;

1
master/voe_handler.h
View File

@ -50,6 +50,7 @@ struct ec_voe_handler {
struct list_head list; /**< List item. */
ec_slave_config_t *config; /**< Parent slave configuration. */
ec_datagram_t datagram; /**< State machine datagram. */
ec_mailbox_t mbox; /**< State machine mailbox. */
uint32_t vendor_id; /**< Vendor ID for the header. */
uint16_t vendor_type; /**< Vendor type for the header. */
size_t data_size; /**< Size of VoE data. */