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

Avoided io_sem locking from userspace/rtdm library.

This commit is contained in:
Florian Pose 2013-01-10 17:36:41 +01:00
parent 52938b943f
commit 032acb3d85
21 changed files with 1654 additions and 1153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
NEWS
View File

@ -17,6 +17,7 @@ Changes in 1.5.1:
* Added fix for ESC port order (DC delay calculation).
* Added e1000 driver for kernels 2.6.35, 3.0, 3.4.
* Added 8139too driver for kernel 3.4.
* Avoided slave FSM datagram queue; implemented datagram ring instead.
Changes in 1.5.0:

1068
master/fsm_coe.c
View File

File diff suppressed because it is too large Load Diff

11
master/fsm_coe.h
View File

@ -51,10 +51,10 @@ 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 */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_coe_t *); /**< CoE state function */
void (*state)(ec_fsm_coe_t *, ec_datagram_t *); /**< CoE state function */
ec_datagram_t *datagram; /**< Datagram used in last step. */
unsigned long jiffies_start; /**< CoE timestamp. */
ec_sdo_t *sdo; /**< current SDO */
uint8_t subindex; /**< current subindex */
@ -63,18 +63,19 @@ struct ec_fsm_coe {
uint8_t toggle; /**< toggle bit for segment commands */
uint32_t offset; /**< Data offset during segmented download. */
uint32_t remaining; /**< Remaining bytes during segmented download. */
size_t segment_size; /**< Current segment size. */
};
/*****************************************************************************/
void ec_fsm_coe_init(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_init(ec_fsm_coe_t *);
void ec_fsm_coe_clear(ec_fsm_coe_t *);
void ec_fsm_coe_dictionary(ec_fsm_coe_t *, ec_slave_t *);
void ec_fsm_coe_transfer(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_request_t *);
int ec_fsm_coe_exec(ec_fsm_coe_t *);
int ec_fsm_coe_success(ec_fsm_coe_t *);
int ec_fsm_coe_exec(ec_fsm_coe_t *, ec_datagram_t *);
int ec_fsm_coe_success(const ec_fsm_coe_t *);
/*****************************************************************************/

322
master/fsm_foe.c
View File

@ -74,44 +74,42 @@ enum {
/*****************************************************************************/
int ec_foe_prepare_data_send(ec_fsm_foe_t *);
int ec_foe_prepare_wrq_send(ec_fsm_foe_t *);
int ec_foe_prepare_rrq_send(ec_fsm_foe_t *);
int ec_foe_prepare_send_ack(ec_fsm_foe_t *);
int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);
int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);
int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);
int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);
void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);
void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);
void ec_fsm_foe_write(ec_fsm_foe_t *);
void ec_fsm_foe_read(ec_fsm_foe_t *);
void ec_fsm_foe_end(ec_fsm_foe_t *);
void ec_fsm_foe_error(ec_fsm_foe_t *);
void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *);
void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *);
void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
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 *);
void ec_fsm_foe_state_ack_read(ec_fsm_foe_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_data_sent(ec_fsm_foe_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 *);
void ec_fsm_foe_state_data_read(ec_fsm_foe_t *);
void ec_fsm_foe_state_sent_ack(ec_fsm_foe_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_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_write_start(ec_fsm_foe_t *);
void ec_fsm_foe_read_start(ec_fsm_foe_t *);
void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);
/*****************************************************************************/
/** Constructor.
*/
void ec_fsm_foe_init(ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< datagram */
)
void ec_fsm_foe_init(
ec_fsm_foe_t *fsm /**< finite state machine */
)
{
fsm->state = NULL;
fsm->datagram = datagram;
fsm->state = NULL;
fsm->datagram = NULL;
}
/*****************************************************************************/
@ -125,13 +123,36 @@ void ec_fsm_foe_clear(ec_fsm_foe_t *fsm /**< finite state machine */)
/*****************************************************************************/
/** Executes the current state of the state machine.
* \return false, if state machine has terminated
*
* \return 1, if the datagram was used, else 0.
*/
int ec_fsm_foe_exec(ec_fsm_foe_t *fsm /**< finite state machine */)
int ec_fsm_foe_exec(
ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
fsm->state(fsm);
int datagram_used = 0;
return fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
if (fsm->datagram &&
(fsm->datagram->state == EC_DATAGRAM_INIT ||
fsm->datagram->state == EC_DATAGRAM_QUEUED ||
fsm->datagram->state == EC_DATAGRAM_SENT)) {
// datagram not received yet
return datagram_used;
}
fsm->state(fsm, datagram);
datagram_used =
fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
if (datagram_used) {
fsm->datagram = datagram;
} else {
fsm->datagram = NULL;
}
return datagram_used;
}
/*****************************************************************************/
@ -139,7 +160,7 @@ int ec_fsm_foe_exec(ec_fsm_foe_t *fsm /**< finite state machine */)
/** Returns, if the state machine terminated with success.
* \return non-zero if successful.
*/
int ec_fsm_foe_success(ec_fsm_foe_t *fsm /**< Finite state machine */)
int ec_fsm_foe_success(const ec_fsm_foe_t *fsm /**< Finite state machine */)
{
return fsm->state == ec_fsm_foe_end;
}
@ -156,11 +177,25 @@ void ec_fsm_foe_transfer(
{
fsm->slave = slave;
fsm->request = request;
if (request->dir == EC_DIR_OUTPUT) {
fsm->state = ec_fsm_foe_write;
fsm->tx_buffer = fsm->request->buffer;
fsm->tx_buffer_size = fsm->request->data_size;
fsm->tx_buffer_offset = 0;
fsm->tx_filename = fsm->request->file_name;
fsm->tx_filename_len = strlen(fsm->tx_filename);
fsm->state = ec_fsm_foe_write_start;
}
else {
fsm->state = ec_fsm_foe_read;
fsm->rx_buffer = fsm->request->buffer;
fsm->rx_buffer_size = fsm->request->buffer_size;
fsm->rx_filename = fsm->request->file_name;
fsm->rx_filename_len = strlen(fsm->rx_filename);
fsm->state = ec_fsm_foe_read_start;
}
}
@ -168,7 +203,10 @@ void ec_fsm_foe_transfer(
/** State: ERROR.
*/
void ec_fsm_foe_error(ec_fsm_foe_t *fsm /**< finite state machine */)
void ec_fsm_foe_error(
ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
#ifdef DEBUG_FOE
printk("ec_fsm_foe_error()\n");
@ -179,7 +217,10 @@ void ec_fsm_foe_error(ec_fsm_foe_t *fsm /**< finite state machine */)
/** State: END.
*/
void ec_fsm_foe_end(ec_fsm_foe_t *fsm /**< finite state machine */)
void ec_fsm_foe_end(
ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
#ifdef DEBUG_FOE
printk("ec_fsm_foe_end\n");
@ -190,7 +231,10 @@ void ec_fsm_foe_end(ec_fsm_foe_t *fsm /**< finite state machine */)
/** Sends a file or the next fragment.
*/
int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)
int ec_foe_prepare_data_send(
ec_fsm_foe_t *fsm,
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t remaining_size, current_size;
uint8_t *data;
@ -207,13 +251,14 @@ 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,
datagram, EC_MBOX_TYPE_FILEACCESS,
current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U8 ( data, EC_FOE_OPCODE_DATA ); // OpCode = DataBlock req.
EC_WRITE_U32( data + 2, fsm->tx_packet_no ); // PacketNo, Password
EC_WRITE_U8 (data, EC_FOE_OPCODE_DATA); // OpCode = DataBlock req.
EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password
memcpy(data + EC_FOE_HEADER_SIZE,
fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
@ -226,7 +271,10 @@ int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)
/** Prepare a write request (WRQ) with filename
*/
int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)
int ec_foe_prepare_wrq_send(
ec_fsm_foe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t current_size;
uint8_t *data;
@ -238,10 +286,11 @@ 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, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
EC_WRITE_U32( data + 2, fsm->tx_packet_no );
@ -253,27 +302,12 @@ int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)
/*****************************************************************************/
/** Start a write operation.
*/
void ec_fsm_foe_write(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
)
{
fsm->tx_buffer = fsm->request->buffer;
fsm->tx_buffer_size = fsm->request->data_size;
fsm->tx_buffer_offset = 0;
fsm->tx_filename = fsm->request->file_name;
fsm->tx_filename_len = strlen(fsm->tx_filename);
fsm->state = ec_fsm_foe_write_start;
}
/*****************************************************************************/
/** Initializes the FoE write state machine.
*/
void ec_fsm_foe_write_start(ec_fsm_foe_t *fsm /**< finite state machine */)
void ec_fsm_foe_write_start(
ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -292,7 +326,7 @@ void ec_fsm_foe_write_start(ec_fsm_foe_t *fsm /**< finite state machine */)
return;
}
if (ec_foe_prepare_wrq_send(fsm)) {
if (ec_foe_prepare_wrq_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
@ -305,33 +339,33 @@ void ec_fsm_foe_write_start(ec_fsm_foe_t *fsm /**< finite state machine */)
/** Check for acknowledge.
*/
void ec_fsm_foe_state_ack_check(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->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 (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
// slave did not put anything in the mailbox yet
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
" failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(fsm->datagram)) {
// slave did not put anything in the mailbox yet
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
@ -357,10 +391,10 @@ void ec_fsm_foe_state_ack_check(
/** Acknowledge a read operation.
*/
void ec_fsm_foe_state_ack_read(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
@ -370,22 +404,22 @@ void ec_fsm_foe_state_ack_read(
printk("ec_fsm_foe_ack_read()\n");
#endif
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
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(datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!(data = ec_slave_mbox_fetch(fsm->slave, datagram,
&mbox_prot, &rec_size))) {
data = ec_slave_mbox_fetch(fsm->slave, datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
@ -401,7 +435,7 @@ void ec_fsm_foe_state_ack_read(
if (opCode == EC_FOE_OPCODE_BUSY) {
// slave not ready
if (ec_foe_prepare_data_send(fsm)) {
if (ec_foe_prepare_data_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
EC_SLAVE_ERR(slave, "Slave is busy.\n");
return;
@ -419,7 +453,7 @@ void ec_fsm_foe_state_ack_read(
return;
}
if (ec_foe_prepare_data_send(fsm)) {
if (ec_foe_prepare_data_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
@ -437,32 +471,32 @@ void ec_fsm_foe_state_ack_read(
* fragment, if necessary.
*/
void ec_fsm_foe_state_wrq_sent(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->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 (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
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_SLAVE_ERR(slave, "Reception of FoE WRQ failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
@ -478,10 +512,10 @@ void ec_fsm_foe_state_wrq_sent(
* fragment, if necessary.
*/
void ec_fsm_foe_state_data_sent(
ec_fsm_foe_t *fsm /**< Foe statemachine. */
ec_fsm_foe_t *fsm, /**< Foe statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
@ -491,18 +525,18 @@ void ec_fsm_foe_state_data_sent(
if (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 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);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
ec_slave_mbox_prepare_check(fsm->slave, fsm->datagram);
ec_slave_mbox_prepare_check(fsm->slave, datagram);
fsm->jiffies_start = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
@ -512,17 +546,21 @@ void ec_fsm_foe_state_data_sent(
/** Prepare a read request (RRQ) with filename
*/
int ec_foe_prepare_rrq_send(ec_fsm_foe_t *fsm)
int ec_foe_prepare_rrq_send(
ec_fsm_foe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t current_size;
uint8_t *data;
current_size = fsm->rx_filename_len;
data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
EC_WRITE_U32(data + 2, 0x00000000); // no passwd
@ -541,18 +579,20 @@ int ec_foe_prepare_rrq_send(ec_fsm_foe_t *fsm)
/** Prepare to send an acknowledge.
*/
int ec_foe_prepare_send_ack(
ec_fsm_foe_t *foe /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data;
data = ec_slave_mbox_prepare_send(foe->slave, foe->datagram,
data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE);
if (IS_ERR(data))
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
EC_WRITE_U32(data + 2, foe->rx_expected_packet_no);
EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);
return 0;
}
@ -561,36 +601,36 @@ int ec_foe_prepare_send_ack(
/** State: RRQ SENT.
*
* Checks is the previous transmit datagram succeded and sends the next
* Checks is the previous transmit datagram succeeded and sends the next
* fragment, if necessary.
*/
void ec_fsm_foe_state_rrq_sent(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->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 (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
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_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
@ -600,26 +640,11 @@ void ec_fsm_foe_state_rrq_sent(
/*****************************************************************************/
/** Start a read operation.
*/
void ec_fsm_foe_read(
ec_fsm_foe_t *fsm /**< FoE state machine. */
)
{
fsm->state = ec_fsm_foe_read_start;
fsm->rx_filename = fsm->request->file_name;
fsm->rx_filename_len = strlen(fsm->rx_filename);
fsm->rx_buffer = fsm->request->buffer;
fsm->rx_buffer_size = fsm->request->buffer_size;
}
/*****************************************************************************/
/** Starting state for read operations.
*/
void ec_fsm_foe_read_start(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -638,7 +663,7 @@ void ec_fsm_foe_read_start(
return;
}
if (ec_foe_prepare_rrq_send(fsm)) {
if (ec_foe_prepare_rrq_send(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
return;
}
@ -651,33 +676,34 @@ void ec_fsm_foe_read_start(
/** Check for data.
*/
void ec_fsm_foe_state_data_check(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->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 (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
if (fsm->datagram->working_counter != 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);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
@ -702,35 +728,35 @@ void ec_fsm_foe_state_data_check(
/** Start reading data.
*/
void ec_fsm_foe_state_data_read(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t rec_size;
uint8_t *data, opCode, packet_no, mbox_prot;
ec_datagram_t *datagram = fsm->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 (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
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(datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot,
&rec_size))) {
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
}
@ -745,7 +771,7 @@ void ec_fsm_foe_state_data_read(
opCode = EC_READ_U8(data);
if (opCode == EC_FOE_OPCODE_BUSY) {
if (ec_foe_prepare_send_ack(fsm)) {
if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
}
return;
@ -800,7 +826,7 @@ void ec_fsm_foe_state_data_read(
#ifdef DEBUG_FOE
printk ("last_packet=true\n");
#endif
if (ec_foe_prepare_send_ack(fsm)) {
if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR);
return;
}
@ -826,32 +852,32 @@ void ec_fsm_foe_state_data_read(
/** Sent an acknowledge.
*/
void ec_fsm_foe_state_sent_ack(
ec_fsm_foe_t *fsm /**< FoE statemachine. */
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->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 (fsm->datagram->state != 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);
ec_datagram_print_state(fsm->datagram);
return;
}
if (datagram->working_counter != 1) {
if (fsm->datagram->working_counter != 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: ");
ec_datagram_print_wc_error(datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.

23
master/fsm_foe.h
View File

@ -2,7 +2,8 @@
*
* $Id$
*
* Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
* Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
* 2009-2012 Florian Pose <fp@igh-essen.com>
*
* This file is part of the IgH EtherCAT Master.
*
@ -50,15 +51,15 @@ typedef struct ec_fsm_foe ec_fsm_foe_t; /**< \see ec_fsm_foe */
/** Finite state machines for the CANopen-over-EtherCAT protocol.
*/
struct ec_fsm_foe {
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
ec_slave_t *slave; /**< Slave the FSM runs on. */
unsigned int retries; /**< Retries upon datagram timeout */
void (*state)(ec_fsm_foe_t *); /**< FoE state function */
void (*state)(ec_fsm_foe_t *, ec_datagram_t *); /**< FoE state function. */
ec_datagram_t *datagram; /**< Datagram used in previous step. */
unsigned long jiffies_start; /**< FoE timestamp. */
uint8_t subindex; /**< current subindex */
ec_foe_request_t *request; /**< FoE request */
uint8_t toggle; /**< toggle bit for segment commands */
uint8_t subindex; /**< Current subindex. */
ec_foe_request_t *request; /**< FoE request. */
uint8_t toggle; /**< Toggle bit for segment commands. */
uint8_t *tx_buffer; /**< Buffer with data to transmit. */
uint32_t tx_buffer_size; /**< Size of data to transmit. */
@ -80,11 +81,11 @@ 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 *);
void ec_fsm_foe_clear(ec_fsm_foe_t *);
int ec_fsm_foe_exec(ec_fsm_foe_t *);
int ec_fsm_foe_success(ec_fsm_foe_t *);
int ec_fsm_foe_exec(ec_fsm_foe_t *, ec_datagram_t *);
int ec_fsm_foe_success(const ec_fsm_foe_t *);
void ec_fsm_foe_transfer(ec_fsm_foe_t *, ec_slave_t *, ec_foe_request_t *);

23
master/fsm_master.c
View File

@ -85,8 +85,8 @@ void ec_fsm_master_init(
ec_fsm_master_reset(fsm);
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
ec_fsm_coe_init(&fsm->fsm_coe);
ec_fsm_soe_init(&fsm->fsm_soe);
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,
@ -342,6 +342,7 @@ void ec_fsm_master_state_broadcast(
}
}
master->slave_count = count;
master->fsm_slave = master->slaves;
/* start with first device with slaves responding; at least one
* has responding slaves, otherwise count would be zero. */
@ -465,7 +466,7 @@ int ec_fsm_master_action_process_sdo(
fsm->slave = slave;
fsm->state = ec_fsm_master_state_sdo_request;
ec_fsm_coe_transfer(&fsm->fsm_coe, slave, req);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram);
return 1;
}
}
@ -495,7 +496,7 @@ void ec_fsm_master_action_idle(
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
ec_fsm_slave_ready(&slave->fsm);
ec_fsm_slave_set_ready(&slave->fsm);
}
// check, if slaves have an SDO dictionary to read out.
@ -520,7 +521,7 @@ void ec_fsm_master_action_idle(
fsm->slave = slave;
fsm->state = ec_fsm_master_state_sdo_dictionary;
ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram); // execute immediately
fsm->datagram->device_index = fsm->slave->device_index;
return;
}
@ -1138,7 +1139,7 @@ void ec_fsm_master_state_write_sii(
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
EC_SLAVE_ERR(slave, "Failed to write SII data.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&master->request_queue);
wake_up_all(&master->request_queue);
ec_fsm_master_restart(fsm);
return;
}
@ -1166,7 +1167,7 @@ void ec_fsm_master_state_write_sii(
// TODO: Evaluate other SII contents!
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&master->request_queue);
wake_up_all(&master->request_queue);
// check for another SII write request
if (ec_fsm_master_action_process_sii(fsm))
@ -1186,7 +1187,7 @@ void ec_fsm_master_state_sdo_dictionary(
ec_slave_t *slave = fsm->slave;
ec_master_t *master = fsm->master;
if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
return;
}
@ -1226,7 +1227,7 @@ void ec_fsm_master_state_sdo_request(
return;
}
if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
return;
}
@ -1234,14 +1235,14 @@ void ec_fsm_master_state_sdo_request(
EC_SLAVE_DBG(fsm->slave, 1,
"Failed to process internal SDO request.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&fsm->master->request_queue);
wake_up_all(&fsm->master->request_queue);
ec_fsm_master_restart(fsm);
return;
}
// SDO request finished
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&fsm->master->request_queue);
wake_up_all(&fsm->master->request_queue);
EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n");

208
master/fsm_pdo.c
View File

@ -27,8 +27,7 @@
*
*****************************************************************************/
/** \file
* EtherCAT PDO configuration state machine.
/** \file EtherCAT PDO configuration state machine.
*/
/*****************************************************************************/
@ -42,30 +41,30 @@
/*****************************************************************************/
void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);
void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);
void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *);
void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_t *);
/*****************************************************************************/
@ -162,10 +161,12 @@ int ec_fsm_pdo_running(
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_exec(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
fsm->state(fsm);
fsm->state(fsm, datagram);
return ec_fsm_pdo_running(fsm);
}
@ -189,12 +190,13 @@ int ec_fsm_pdo_success(
/** Start reading PDO assignment.
*/
void ec_fsm_pdo_read_state_start(
ec_fsm_pdo_t *fsm /**< finite state machine */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
// read PDO assignment for first sync manager not reserved for mailbox
fsm->sync_index = 1; // next is 2
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@ -202,7 +204,8 @@ void ec_fsm_pdo_read_state_start(
/** Read PDO assignment of next sync manager.
*/
void ec_fsm_pdo_read_action_next_sync(
ec_fsm_pdo_t *fsm /**< Finite state machine */
ec_fsm_pdo_t *fsm, /**< finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -222,7 +225,7 @@ void ec_fsm_pdo_read_action_next_sync(
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_read_state_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
@ -237,15 +240,18 @@ void ec_fsm_pdo_read_action_next_sync(
/** Count assigned PDOs.
*/
void ec_fsm_pdo_read_state_pdo_count(
ec_fsm_pdo_t *fsm /**< finite state machine */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs"
" for SM%u.\n", fsm->sync_index);
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
@ -253,7 +259,7 @@ void ec_fsm_pdo_read_state_pdo_count(
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
" when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex);
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
fsm->pdo_count = EC_READ_U8(fsm->request.data);
@ -262,7 +268,7 @@ void ec_fsm_pdo_read_state_pdo_count(
// read first PDO
fsm->pdo_pos = 1;
ec_fsm_pdo_read_action_next_pdo(fsm);
ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
}
/*****************************************************************************/
@ -270,7 +276,8 @@ void ec_fsm_pdo_read_state_pdo_count(
/** Read next PDO.
*/
void ec_fsm_pdo_read_action_next_pdo(
ec_fsm_pdo_t *fsm /**< finite state machine */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (fsm->pdo_pos <= fsm->pdo_count) {
@ -279,7 +286,7 @@ void ec_fsm_pdo_read_action_next_pdo(
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_read_state_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
@ -289,7 +296,7 @@ void ec_fsm_pdo_read_action_next_pdo(
ec_pdo_list_clear_pdos(&fsm->pdos);
// next sync manager
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@ -297,16 +304,19 @@ void ec_fsm_pdo_read_action_next_pdo(
/** Fetch PDO information.
*/
void ec_fsm_pdo_read_state_pdo(
ec_fsm_pdo_t *fsm /**< finite state machine */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read index of"
" assigned PDO %u from SM%u.\n",
fsm->pdo_pos, fsm->sync_index);
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
@ -314,14 +324,14 @@ void ec_fsm_pdo_read_state_pdo(
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
" when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex);
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
if (!(fsm->pdo = (ec_pdo_t *)
kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n");
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
@ -335,7 +345,7 @@ void ec_fsm_pdo_read_state_pdo(
fsm->state = ec_fsm_pdo_read_state_pdo_entries;
ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
fsm->state(fsm); // execute immediately
fsm->state(fsm, datagram); // execute immediately
}
/*****************************************************************************/
@ -343,22 +353,24 @@ void ec_fsm_pdo_read_state_pdo(
/** Fetch PDO information.
*/
void ec_fsm_pdo_read_state_pdo_entries(
ec_fsm_pdo_t *fsm /**< finite state machine */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
return;
}
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries"
" for PDO 0x%04X.\n", fsm->pdo->index);
ec_fsm_pdo_read_action_next_sync(fsm);
ec_fsm_pdo_read_action_next_sync(fsm, datagram);
return;
}
// next PDO
fsm->pdo_pos++;
ec_fsm_pdo_read_action_next_pdo(fsm);
ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
}
/******************************************************************************
@ -368,7 +380,8 @@ void ec_fsm_pdo_read_state_pdo_entries(
/** Start PDO configuration.
*/
void ec_fsm_pdo_conf_state_start(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (!fsm->slave->config) {
@ -377,7 +390,7 @@ void ec_fsm_pdo_conf_state_start(
}
fsm->sync_index = 1; // next is 2
ec_fsm_pdo_conf_action_next_sync(fsm);
ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@ -400,7 +413,8 @@ ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(
/** Get the next sync manager for a pdo configuration.
*/
void ec_fsm_pdo_conf_action_next_sync(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
fsm->sync_index++;
@ -432,11 +446,11 @@ void ec_fsm_pdo_conf_action_next_sync(
if (!(fsm->pdo =
ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
// no pdos configured
ec_fsm_pdo_conf_action_check_assignment(fsm);
ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
return;
}
ec_fsm_pdo_conf_action_pdo_mapping(fsm);
ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
return;
}
@ -448,7 +462,8 @@ void ec_fsm_pdo_conf_action_next_sync(
/** Check if the mapping has to be read, otherwise start to configure it.
*/
void ec_fsm_pdo_conf_action_pdo_mapping(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
const ec_pdo_t *assigned_pdo;
@ -469,12 +484,12 @@ void ec_fsm_pdo_conf_action_pdo_mapping(
ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
&fsm->slave_pdo);
fsm->state = ec_fsm_pdo_conf_state_read_mapping;
fsm->state(fsm); // execute immediately
fsm->state(fsm, datagram); // execute immediately
return;
}
// pdo mapping is known, check if it most be re-configured
ec_fsm_pdo_conf_action_check_mapping(fsm);
ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
}
/*****************************************************************************/
@ -482,11 +497,13 @@ void ec_fsm_pdo_conf_action_pdo_mapping(
/** Execute the PDO entry state machine to read the current PDO's mapping.
*/
void ec_fsm_pdo_conf_state_read_mapping(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
return;
}
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
EC_SLAVE_WARN(fsm->slave,
@ -494,7 +511,7 @@ void ec_fsm_pdo_conf_state_read_mapping(
fsm->pdo->index);
// check if the mapping must be re-configured
ec_fsm_pdo_conf_action_check_mapping(fsm);
ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
}
/*****************************************************************************/
@ -504,7 +521,8 @@ void ec_fsm_pdo_conf_state_read_mapping(
* \todo Display mapping differences.
*/
void ec_fsm_pdo_conf_action_check_mapping(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
// check, if slave supports PDO configuration
@ -516,7 +534,7 @@ void ec_fsm_pdo_conf_action_check_mapping(
ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
fsm->pdo, &fsm->slave_pdo);
fsm->state = ec_fsm_pdo_conf_state_mapping;
fsm->state(fsm); // execure immediately
fsm->state(fsm, datagram); // execure immediately
return;
}
else if (!ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
@ -530,7 +548,7 @@ void ec_fsm_pdo_conf_action_check_mapping(
printk("\n");
}
ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
}
/*****************************************************************************/
@ -538,18 +556,20 @@ void ec_fsm_pdo_conf_action_check_mapping(
/** Let the PDO entry state machine configure the current PDO's mapping.
*/
void ec_fsm_pdo_conf_state_mapping(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
return;
}
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
EC_SLAVE_WARN(fsm->slave,
"Failed to configure mapping of PDO 0x%04X.\n",
fsm->pdo->index);
ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
}
/*****************************************************************************/
@ -557,17 +577,18 @@ void ec_fsm_pdo_conf_state_mapping(
/** Check mapping of next PDO, otherwise configure assignment.
*/
void ec_fsm_pdo_conf_action_next_pdo_mapping(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
// get next configured PDO
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
// no more configured pdos
ec_fsm_pdo_conf_action_check_assignment(fsm);
ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
return;
}
ec_fsm_pdo_conf_action_pdo_mapping(fsm);
ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
}
/*****************************************************************************/
@ -575,7 +596,8 @@ void ec_fsm_pdo_conf_action_next_pdo_mapping(
/** Check if the PDO assignment of the current SM has to be re-configured.
*/
void ec_fsm_pdo_conf_action_check_assignment(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
@ -605,7 +627,7 @@ void ec_fsm_pdo_conf_action_check_assignment(
fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
@ -613,7 +635,7 @@ void ec_fsm_pdo_conf_action_check_assignment(
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
}
ec_fsm_pdo_conf_action_next_sync(fsm);
ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
}
/*****************************************************************************/
@ -621,17 +643,20 @@ void ec_fsm_pdo_conf_action_check_assignment(
/** Set the number of assigned PDOs to zero.
*/
void ec_fsm_pdo_conf_state_zero_pdo_count(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe))
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n",
fsm->sync_index);
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
ec_fsm_pdo_conf_action_next_sync(fsm);
EC_SLAVE_WARN(fsm->slave, "");
ec_fsm_pdo_print(fsm);
ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
return;
}
@ -642,15 +667,14 @@ void ec_fsm_pdo_conf_state_zero_pdo_count(
// find first PDO
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
// check for mapping to be altered
ec_fsm_pdo_conf_action_next_sync(fsm);
ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
return;
}
// assign first PDO
fsm->pdo_pos = 1;
ec_fsm_pdo_conf_action_assign_pdo(fsm);
ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
}
/*****************************************************************************/
@ -658,7 +682,8 @@ void ec_fsm_pdo_conf_state_zero_pdo_count(
/** Assign a PDO.
*/
void ec_fsm_pdo_conf_action_assign_pdo(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
EC_WRITE_U16(fsm->request.data, fsm->pdo->index);
@ -672,7 +697,7 @@ void ec_fsm_pdo_conf_action_assign_pdo(
fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@ -680,10 +705,13 @@ void ec_fsm_pdo_conf_action_assign_pdo(
/** Add a PDO to the sync managers PDO assignment.
*/
void ec_fsm_pdo_conf_state_assign_pdo(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u"
@ -696,7 +724,6 @@ void ec_fsm_pdo_conf_state_assign_pdo(
// find next PDO
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
// no more PDOs to assign, set PDO count
EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
fsm->request.data_size = 1;
@ -709,13 +736,13 @@ void ec_fsm_pdo_conf_state_assign_pdo(
fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
// add next PDO to assignment
fsm->pdo_pos++;
ec_fsm_pdo_conf_action_assign_pdo(fsm);
ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
}
/*****************************************************************************/
@ -723,10 +750,13 @@ void ec_fsm_pdo_conf_state_assign_pdo(
/** Set the number of assigned PDOs.
*/
void ec_fsm_pdo_conf_state_set_pdo_count(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to set number of"
@ -743,7 +773,7 @@ void ec_fsm_pdo_conf_state_set_pdo_count(
" PDO assignment of SM%u.\n", fsm->sync_index);
// check if PDO mapping has to be altered
ec_fsm_pdo_conf_action_next_sync(fsm);
ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
}
/******************************************************************************
@ -753,7 +783,8 @@ void ec_fsm_pdo_conf_state_set_pdo_count(
/** State: ERROR.
*/
void ec_fsm_pdo_state_error(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
}
@ -763,7 +794,8 @@ void ec_fsm_pdo_state_error(
/** State: END.
*/
void ec_fsm_pdo_state_end(
ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
}

4
master/fsm_pdo.h
View File

@ -53,7 +53,7 @@ typedef struct ec_fsm_pdo ec_fsm_pdo_t;
*/
struct ec_fsm_pdo
{
void (*state)(ec_fsm_pdo_t *); /**< State function. */
void (*state)(ec_fsm_pdo_t *, ec_datagram_t *); /**< State function. */
ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */
ec_fsm_pdo_entry_t fsm_pdo_entry; /**< PDO entry state machine. */
ec_pdo_list_t pdos; /**< PDO configuration. */
@ -76,7 +76,7 @@ void ec_fsm_pdo_clear(ec_fsm_pdo_t *);
void ec_fsm_pdo_start_reading(ec_fsm_pdo_t *, ec_slave_t *);
void ec_fsm_pdo_start_configuration(ec_fsm_pdo_t *, ec_slave_t *);
int ec_fsm_pdo_exec(ec_fsm_pdo_t *);
int ec_fsm_pdo_exec(ec_fsm_pdo_t *, ec_datagram_t *);
int ec_fsm_pdo_success(const ec_fsm_pdo_t *);
/*****************************************************************************/

106
master/fsm_pdo_entry.c
View File

@ -42,21 +42,24 @@
/*****************************************************************************/
void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *,
ec_datagram_t *);
void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *,
ec_datagram_t *);
void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *,
ec_datagram_t *);
void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *, ec_datagram_t *);
void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *, ec_datagram_t *);
/*****************************************************************************/
@ -87,7 +90,7 @@ void ec_fsm_pdo_entry_clear(
/** Print the current and desired PDO mapping.
*/
void ec_fsm_pdo_entry_print(
ec_fsm_pdo_entry_t *fsm /**< PDO configuration state machine. */
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
)
{
printk("Currently mapped PDO entries: ");
@ -103,7 +106,7 @@ void ec_fsm_pdo_entry_print(
*/
void ec_fsm_pdo_entry_start_reading(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_slave_t *slave, /**< slave to configure */
ec_slave_t *slave, /**< Slave to configure. */
ec_pdo_t *pdo /**< PDO to read entries for. */
)
{
@ -121,7 +124,7 @@ void ec_fsm_pdo_entry_start_reading(
*/
void ec_fsm_pdo_entry_start_configuration(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_slave_t *slave, /**< slave to configure */
ec_slave_t *slave, /**< Slave to configure. */
const ec_pdo_t *pdo, /**< PDO with the desired entries. */
const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
)
@ -160,10 +163,12 @@ int ec_fsm_pdo_entry_running(
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_entry_exec(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
fsm->state(fsm);
fsm->state(fsm, datagram);
return ec_fsm_pdo_entry_running(fsm);
}
@ -187,7 +192,8 @@ int ec_fsm_pdo_entry_success(
/** Request reading the number of mapped PDO entries.
*/
void ec_fsm_pdo_entry_read_state_start(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, 0);
@ -195,7 +201,7 @@ void ec_fsm_pdo_entry_read_state_start(
fsm->state = ec_fsm_pdo_entry_read_state_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@ -203,11 +209,13 @@ void ec_fsm_pdo_entry_read_state_start(
/** Read number of mapped PDO entries.
*/
void ec_fsm_pdo_entry_read_state_count(
ec_fsm_pdo_entry_t *fsm /**< finite state machine */
ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe))
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave,
@ -231,7 +239,7 @@ void ec_fsm_pdo_entry_read_state_count(
// read first PDO entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_read_action_next(fsm);
ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
/*****************************************************************************/
@ -239,7 +247,8 @@ void ec_fsm_pdo_entry_read_state_count(
/** Read next PDO entry.
*/
void ec_fsm_pdo_entry_read_action_next(
ec_fsm_pdo_entry_t *fsm /**< finite state machine */
ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (fsm->entry_pos <= fsm->entry_count) {
@ -248,7 +257,7 @@ void ec_fsm_pdo_entry_read_action_next(
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_entry_read_state_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
@ -261,10 +270,13 @@ void ec_fsm_pdo_entry_read_action_next(
/** Read PDO entry information.
*/
void ec_fsm_pdo_entry_read_state_entry(
ec_fsm_pdo_entry_t *fsm /**< finite state machine */
ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n");
@ -315,7 +327,7 @@ void ec_fsm_pdo_entry_read_state_entry(
// next PDO entry
fsm->entry_pos++;
ec_fsm_pdo_entry_read_action_next(fsm);
ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
}
@ -326,7 +338,8 @@ void ec_fsm_pdo_entry_read_state_entry(
/** Start PDO mapping.
*/
void ec_fsm_pdo_entry_conf_state_start(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_sdo_request_alloc(&fsm->request, 4)) {
@ -344,7 +357,7 @@ void ec_fsm_pdo_entry_conf_state_start(
fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@ -367,11 +380,13 @@ ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(
/** Set the number of mapped entries to zero.
*/
void ec_fsm_pdo_entry_conf_state_zero_entry_count(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe))
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");
@ -392,7 +407,7 @@ void ec_fsm_pdo_entry_conf_state_zero_entry_count(
// add first entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_conf_action_map(fsm);
ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/*****************************************************************************/
@ -400,7 +415,8 @@ void ec_fsm_pdo_entry_conf_state_zero_entry_count(
/** Starts to add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_action_map(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint32_t value;
@ -420,7 +436,7 @@ void ec_fsm_pdo_entry_conf_action_map(
fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/*****************************************************************************/
@ -428,10 +444,13 @@ void ec_fsm_pdo_entry_conf_action_map(
/** Add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_state_map_entry(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
@ -458,13 +477,13 @@ void ec_fsm_pdo_entry_conf_state_map_entry(
fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
// add next entry
fsm->entry_pos++;
ec_fsm_pdo_entry_conf_action_map(fsm);
ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/*****************************************************************************/
@ -472,10 +491,13 @@ void ec_fsm_pdo_entry_conf_state_map_entry(
/** Set the number of entries.
*/
void ec_fsm_pdo_entry_conf_state_set_entry_count(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");
@ -497,7 +519,8 @@ void ec_fsm_pdo_entry_conf_state_set_entry_count(
/** State: ERROR.
*/
void ec_fsm_pdo_entry_state_error(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
}
@ -507,7 +530,8 @@ void ec_fsm_pdo_entry_state_error(
/** State: END.
*/
void ec_fsm_pdo_entry_state_end(
ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
}

5
master/fsm_pdo_entry.h
View File

@ -51,7 +51,8 @@ typedef struct ec_fsm_pdo_entry ec_fsm_pdo_entry_t;
*/
struct ec_fsm_pdo_entry
{
void (*state)(ec_fsm_pdo_entry_t *); /**< state function */
void (*state)(ec_fsm_pdo_entry_t *, ec_datagram_t *); /**< state function
*/
ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */
ec_sdo_request_t request; /**< SDO request. */
@ -74,7 +75,7 @@ void ec_fsm_pdo_entry_start_reading(ec_fsm_pdo_entry_t *, ec_slave_t *,
void ec_fsm_pdo_entry_start_configuration(ec_fsm_pdo_entry_t *, ec_slave_t *,
const ec_pdo_t *, const ec_pdo_t *);
int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *);
int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *, ec_datagram_t *);
int ec_fsm_pdo_entry_success(const ec_fsm_pdo_entry_t *);
/*****************************************************************************/

205
master/fsm_slave.c
View File

@ -42,16 +42,16 @@
/*****************************************************************************/
void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *);
void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *);
int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);
int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);
int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);
int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);
/*****************************************************************************/
@ -59,22 +59,23 @@ 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_slave_t *slave /**< EtherCAT slave. */
)
{
fsm->slave = slave;
fsm->datagram = datagram;
fsm->datagram->data_size = 0;
EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
INIT_LIST_HEAD(&fsm->list); // mark as unlisted
fsm->state = ec_fsm_slave_state_idle;
fsm->datagram = NULL;
fsm->sdo_request = NULL;
fsm->reg_request = NULL;
fsm->foe_request = NULL;
fsm->soe_request = NULL;
// 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);
// Init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe);
ec_fsm_foe_init(&fsm->fsm_foe);
ec_fsm_soe_init(&fsm->fsm_soe);
}
/*****************************************************************************/
@ -85,6 +86,28 @@ void ec_fsm_slave_clear(
ec_fsm_slave_t *fsm /**< Master state machine. */
)
{
// signal requests that are currently in operation
if (fsm->sdo_request) {
fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;
wake_up_all(&fsm->slave->master->request_queue);
}
if (fsm->reg_request) {
fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
wake_up_all(&fsm->slave->master->request_queue);
}
if (fsm->foe_request) {
fsm->foe_request->state = EC_INT_REQUEST_FAILURE;
wake_up_all(&fsm->slave->master->request_queue);
}
if (fsm->soe_request) {
fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
wake_up_all(&fsm->slave->master->request_queue);
}
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_fsm_foe_clear(&fsm->fsm_foe);
@ -95,29 +118,34 @@ void ec_fsm_slave_clear(
/** Executes the current state of the state machine.
*
* If the state machine's datagram is not sent or received yet, the execution
* of the state machine is delayed to the next cycle.
* \return 1 if \a datagram was used, else 0.
*/
void ec_fsm_slave_exec(
ec_fsm_slave_t *fsm /**< Slave state machine. */
int ec_fsm_slave_exec(
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< New datagram to use. */
)
{
if (fsm->datagram->state == EC_DATAGRAM_SENT
|| fsm->datagram->state == EC_DATAGRAM_QUEUED) {
// datagram was not sent or received yet.
return;
int datagram_used;
fsm->state(fsm, datagram);
datagram_used = fsm->state != ec_fsm_slave_state_idle &&
fsm->state != ec_fsm_slave_state_ready;
if (datagram_used) {
fsm->datagram = datagram;
} else {
fsm->datagram = NULL;
}
fsm->state(fsm);
ec_datagram_output_stats(fsm->datagram);
return datagram_used;
}
/*****************************************************************************/
/** Sets the current state of the state machine to READY
*/
void ec_fsm_slave_ready(
void ec_fsm_slave_set_ready(
ec_fsm_slave_t *fsm /**< Slave state machine. */
)
{
@ -127,6 +155,17 @@ void ec_fsm_slave_ready(
}
}
/*****************************************************************************/
/** Returns, if the FSM is currently not busy and ready to execute.
*/
int ec_fsm_slave_is_ready(
const ec_fsm_slave_t *fsm /**< Slave state machine. */
)
{
return fsm->state == ec_fsm_slave_state_ready;
}
/******************************************************************************
* Slave state machine
*****************************************************************************/
@ -134,7 +173,8 @@ void ec_fsm_slave_ready(
/** Slave state: IDLE.
*/
void ec_fsm_slave_state_idle(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
// do nothing
@ -145,26 +185,27 @@ void ec_fsm_slave_state_idle(
/** Slave state: READY.
*/
void ec_fsm_slave_state_ready(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
// Check for pending external SDO requests
if (ec_fsm_slave_action_process_sdo(fsm)) {
if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
return;
}
// Check for pending external register requests
if (ec_fsm_slave_action_process_reg(fsm)) {
if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
return;
}
// Check for pending FoE requests
if (ec_fsm_slave_action_process_foe(fsm)) {
if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
return;
}
// Check for pending SoE requests
if (ec_fsm_slave_action_process_soe(fsm)) {
if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
return;
}
}
@ -176,7 +217,8 @@ void ec_fsm_slave_state_ready(
* \return non-zero, if an SDO request is processed.
*/
int ec_fsm_slave_action_process_sdo(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -194,7 +236,7 @@ int ec_fsm_slave_action_process_sdo(
EC_SLAVE_WARN(slave, "Aborting SDO request,"
" slave has error flag set.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->state = ec_fsm_slave_state_idle;
return 1;
}
@ -202,22 +244,21 @@ int ec_fsm_slave_action_process_sdo(
if (slave->current_state == EC_SLAVE_STATE_INIT) {
EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->state = ec_fsm_slave_state_idle;
return 1;
}
fsm->sdo_request = request;
request->state = EC_INT_REQUEST_BUSY;
// Found pending SDO request. Execute it!
EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
// Start SDO transfer
fsm->sdo_request = request;
fsm->state = ec_fsm_slave_state_sdo_request;
ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
return 1;
}
@ -226,21 +267,21 @@ int ec_fsm_slave_action_process_sdo(
/** Slave state: SDO_REQUEST.
*/
void ec_fsm_slave_state_sdo_request(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_sdo_request_t *request = fsm->sdo_request;
if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->sdo_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
return;
@ -250,7 +291,7 @@ void ec_fsm_slave_state_sdo_request(
// SDO request finished
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->sdo_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}
@ -262,7 +303,8 @@ void ec_fsm_slave_state_sdo_request(
* \return non-zero, if a register request is processed.
*/
int ec_fsm_slave_action_process_reg(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -295,7 +337,7 @@ int ec_fsm_slave_action_process_reg(
EC_SLAVE_WARN(slave, "Aborting register request,"
" slave has error flag set.\n");
fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->reg_request = NULL;
fsm->state = ec_fsm_slave_state_idle;
return 1;
@ -308,17 +350,16 @@ int ec_fsm_slave_action_process_reg(
// Start register access
if (fsm->reg_request->dir == EC_DIR_INPUT) {
ec_datagram_fprd(fsm->datagram, slave->station_address,
ec_datagram_fprd(datagram, slave->station_address,
fsm->reg_request->address, fsm->reg_request->transfer_size);
ec_datagram_zero(fsm->datagram);
ec_datagram_zero(datagram);
} else {
ec_datagram_fpwr(fsm->datagram, slave->station_address,
ec_datagram_fpwr(datagram, slave->station_address,
fsm->reg_request->address, fsm->reg_request->transfer_size);
memcpy(fsm->datagram->data, fsm->reg_request->data,
memcpy(datagram->data, fsm->reg_request->data,
fsm->reg_request->transfer_size);
}
fsm->datagram->device_index = slave->device_index;
ec_master_queue_external_datagram(slave->master, fsm->datagram);
datagram->device_index = slave->device_index;
fsm->state = ec_fsm_slave_state_reg_request;
return 1;
}
@ -328,7 +369,8 @@ int ec_fsm_slave_action_process_reg(
/** Slave state: Register request.
*/
void ec_fsm_slave_state_reg_request(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -337,6 +379,7 @@ void ec_fsm_slave_state_reg_request(
if (!reg) {
// configuration was cleared in the meantime
fsm->state = ec_fsm_slave_state_ready;
fsm->reg_request = NULL;
return;
}
@ -345,7 +388,7 @@ void ec_fsm_slave_state_reg_request(
" request datagram: ");
ec_datagram_print_state(fsm->datagram);
reg->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->reg_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
return;
@ -366,7 +409,7 @@ void ec_fsm_slave_state_reg_request(
fsm->datagram->working_counter);
}
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->reg_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}
@ -378,7 +421,8 @@ void ec_fsm_slave_state_reg_request(
* \return non-zero, if an FoE request is processed.
*/
int ec_fsm_slave_action_process_foe(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -396,20 +440,19 @@ int ec_fsm_slave_action_process_foe(
EC_SLAVE_WARN(slave, "Aborting FoE request,"
" slave has error flag set.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->state = ec_fsm_slave_state_idle;
return 1;
}
request->state = EC_INT_REQUEST_BUSY;
fsm->foe_request = request;
EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
fsm->foe_request = request;
fsm->state = ec_fsm_slave_state_foe_request;
ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
ec_fsm_foe_exec(&fsm->fsm_foe);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
return 1;
}
@ -418,21 +461,21 @@ int ec_fsm_slave_action_process_foe(
/** Slave state: FOE REQUEST.
*/
void ec_fsm_slave_state_foe_request(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_foe_request_t *request = fsm->foe_request;
if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {
return;
}
if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->foe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
return;
@ -443,7 +486,7 @@ void ec_fsm_slave_state_foe_request(
" data.\n", request->data_size);
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->foe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}
@ -455,7 +498,8 @@ void ec_fsm_slave_state_foe_request(
* \return non-zero, if a request is processed.
*/
int ec_fsm_slave_action_process_soe(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
@ -473,7 +517,7 @@ int ec_fsm_slave_action_process_soe(
EC_SLAVE_WARN(slave, "Aborting SoE request,"
" slave has error flag set.\n");
req->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->state = ec_fsm_slave_state_idle;
return 1;
}
@ -481,22 +525,21 @@ int ec_fsm_slave_action_process_soe(
if (slave->current_state == EC_SLAVE_STATE_INIT) {
EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
req->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->state = ec_fsm_slave_state_idle;
return 0;
}
fsm->soe_request = req;
req->state = EC_INT_REQUEST_BUSY;
// Found pending request. Execute it!
EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
// Start SoE transfer
fsm->soe_request = req;
fsm->state = ec_fsm_slave_state_soe_request;
ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
return 1;
}
@ -505,21 +548,21 @@ int ec_fsm_slave_action_process_soe(
/** Slave state: SOE_REQUEST.
*/
void ec_fsm_slave_state_soe_request(
ec_fsm_slave_t *fsm /**< Slave state machine. */
ec_fsm_slave_t *fsm, /**< Slave state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *request = fsm->soe_request;
if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) {
return;
}
if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->soe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
return;
@ -529,7 +572,7 @@ void ec_fsm_slave_state_soe_request(
// SoE request finished
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&slave->master->request_queue);
wake_up_all(&slave->master->request_queue);
fsm->soe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}

20
master/fsm_slave.h
View File

@ -53,27 +53,29 @@ 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 */
struct list_head list; /**< Used for execution list. */
void (*state)(ec_fsm_slave_t *); /**< master state function */
void (*state)(ec_fsm_slave_t *, ec_datagram_t *); /**< State function. */
ec_datagram_t *datagram; /**< Previous state datagram. */
ec_sdo_request_t *sdo_request; /**< SDO request to process. */
ec_reg_request_t *reg_request; /**< Register request to process. */
ec_foe_request_t *foe_request; /**< FoE request to process. */
off_t foe_index; /**< index to FoE write request data */
off_t foe_index; /**< Index to FoE write request data. */
ec_soe_request_t *soe_request; /**< SoE request to process. */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
ec_fsm_foe_t fsm_foe; /**< FoE state machine */
ec_fsm_soe_t fsm_soe; /**< SoE state machine */
ec_fsm_coe_t fsm_coe; /**< CoE state machine. */
ec_fsm_foe_t fsm_foe; /**< FoE state machine. */
ec_fsm_soe_t fsm_soe; /**< SoE state machine. */
};
/*****************************************************************************/
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 *);
void ec_fsm_slave_clear(ec_fsm_slave_t *);
void ec_fsm_slave_exec(ec_fsm_slave_t *);
void ec_fsm_slave_ready(ec_fsm_slave_t *);
int ec_fsm_slave_exec(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_set_ready(ec_fsm_slave_t *);
int ec_fsm_slave_is_ready(const ec_fsm_slave_t *);
/*****************************************************************************/

21
master/fsm_slave_config.c
View File

@ -679,7 +679,7 @@ void ec_fsm_slave_config_enter_sdo_conf(
ec_sdo_request_copy(&fsm->request_copy, fsm->request);
ecrt_sdo_request_write(&fsm->request_copy);
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
}
/*****************************************************************************/
@ -690,7 +690,7 @@ void ec_fsm_slave_config_state_sdo_conf(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) {
if (ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram)) {
return;
}
@ -713,7 +713,7 @@ void ec_fsm_slave_config_state_sdo_conf(
ec_sdo_request_copy(&fsm->request_copy, fsm->request);
ecrt_sdo_request_write(&fsm->request_copy);
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
return;
}
@ -746,7 +746,7 @@ void ec_fsm_slave_config_enter_soe_conf_preop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
@ -765,7 +765,7 @@ void ec_fsm_slave_config_state_soe_conf_preop(
{
ec_slave_t *slave = fsm->slave;
if (ec_fsm_soe_exec(fsm->fsm_soe)) {
if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
return;
}
@ -790,7 +790,7 @@ void ec_fsm_slave_config_state_soe_conf_preop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
@ -823,8 +823,9 @@ void ec_fsm_slave_config_state_pdo_conf(
{
// TODO check for config here
if (ec_fsm_pdo_exec(fsm->fsm_pdo))
if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
return;
}
if (!fsm->slave->config) { // config removed in the meantime
ec_fsm_slave_config_reconfigure(fsm);
@ -1470,7 +1471,7 @@ void ec_fsm_slave_config_enter_soe_conf_safeop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}
@ -1489,7 +1490,7 @@ void ec_fsm_slave_config_state_soe_conf_safeop(
{
ec_slave_t *slave = fsm->slave;
if (ec_fsm_soe_exec(fsm->fsm_soe)) {
if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
return;
}
@ -1514,7 +1515,7 @@ void ec_fsm_slave_config_state_soe_conf_safeop(
ec_soe_request_write(&fsm->soe_request_copy);
ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
&fsm->soe_request_copy);
ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
return;
}
}

5
master/fsm_slave_scan.c
View File

@ -894,7 +894,7 @@ void ec_fsm_slave_scan_enter_pdos(
EC_SLAVE_DBG(slave, 1, "Scanning PDO assignment and mapping.\n");
fsm->state = ec_fsm_slave_scan_state_pdos;
ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave);
ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately
ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram); // execute immediately
}
/*****************************************************************************/
@ -905,8 +905,9 @@ void ec_fsm_slave_scan_state_pdos(
ec_fsm_slave_scan_t *fsm /**< slave state machine */
)
{
if (ec_fsm_pdo_exec(fsm->fsm_pdo))
if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
return;
}
if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
fsm->state = ec_fsm_slave_scan_state_error;

361
master/fsm_soe.c
View File

@ -64,18 +64,18 @@ enum ec_soe_opcodes {
/*****************************************************************************/
void ec_fsm_soe_read_start(ec_fsm_soe_t *);
void ec_fsm_soe_read_request(ec_fsm_soe_t *);
void ec_fsm_soe_read_check(ec_fsm_soe_t *);
void ec_fsm_soe_read_response(ec_fsm_soe_t *);
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_write_start(ec_fsm_soe_t *);
void ec_fsm_soe_write_request(ec_fsm_soe_t *);
void ec_fsm_soe_write_check(ec_fsm_soe_t *);
void ec_fsm_soe_write_response(ec_fsm_soe_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_end(ec_fsm_soe_t *);
void ec_fsm_soe_error(ec_fsm_soe_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 *);
/*****************************************************************************/
@ -105,12 +105,12 @@ void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
/** Constructor.
*/
void ec_fsm_soe_init(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< datagram */
ec_fsm_soe_t *fsm /**< finite state machine */
)
{
fsm->state = NULL;
fsm->datagram = datagram;
fsm->datagram = NULL;
fsm->fragment_size = 0;
}
/*****************************************************************************/
@ -135,6 +135,7 @@ void ec_fsm_soe_transfer(
{
fsm->slave = slave;
fsm->request = request;
if (request->dir == EC_DIR_OUTPUT) {
fsm->state = ec_fsm_soe_write_start;
} else {
@ -144,26 +145,46 @@ void ec_fsm_soe_transfer(
/*****************************************************************************/
/**
Executes the current state of the state machine.
\return false, if state machine has terminated
*/
int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)
/** Executes the current state of the state machine.
*
* \return 1 if the datagram was used, else 0.
*/
int ec_fsm_soe_exec(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
fsm->state(fsm);
int datagram_used = 0;
return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
if (fsm->datagram &&
(fsm->datagram->state == EC_DATAGRAM_INIT ||
fsm->datagram->state == EC_DATAGRAM_QUEUED ||
fsm->datagram->state == EC_DATAGRAM_SENT)) {
// datagram not received yet
return datagram_used;
}
fsm->state(fsm, datagram);
datagram_used =
fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
if (datagram_used) {
fsm->datagram = datagram;
} else {
fsm->datagram = NULL;
}
return datagram_used;
}
/*****************************************************************************/
/**
Returns, if the state machine terminated with success.
\return non-zero if successful.
*/
int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)
/** Returns, if the state machine terminated with success.
*
* \return non-zero if successful.
*/
int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
{
return fsm->state == ec_fsm_soe_end;
}
@ -191,32 +212,24 @@ void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
* SoE read state machine
*****************************************************************************/
/** SoE state: READ START.
/** Prepare a read operation.
*
* \return 0 on success, otherwise a negative error code.
*/
void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
int ec_fsm_soe_prepare_read(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
uint8_t *data;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *request = fsm->request;
uint8_t *data;
EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
request->drive_no);
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
return;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
EC_SOE_SIZE);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
return;
return PTR_ERR(data);
}
EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
@ -228,51 +241,93 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
ec_print_data(data, EC_SOE_SIZE);
}
fsm->request->data_size = 0;
fsm->request->jiffies_sent = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_request;
return 0;
}
/*****************************************************************************/
/** SoE state: READ START.
*/
void ec_fsm_soe_read_start(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *request = fsm->request;
EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
request->drive_no);
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
return;
}
request->data_size = 0;
fsm->retries = EC_FSM_RETRIES;
if (ec_fsm_soe_prepare_read(fsm, datagram)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
}
}
/*****************************************************************************/
/** SoE state: READ REQUEST.
*/
void ec_fsm_soe_read_request(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 /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return; // FIXME: check for response first?
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
if (ec_fsm_soe_prepare_read(fsm, datagram)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
}
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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(fsm->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 (fsm->datagram->working_counter != 1) {
if (!fsm->datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
if (ec_fsm_soe_prepare_read(fsm, datagram)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
}
return;
}
}
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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
fsm->jiffies_start = datagram->jiffies_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_soe_read_check;
@ -282,34 +337,39 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
/** CoE state: READ CHECK.
*/
void ec_fsm_soe_read_check(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 /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
if (fsm->datagram->working_counter != 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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
(fsm->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"
@ -333,9 +393,11 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
/** SoE state: READ RESPONSE.
*/
void ec_fsm_soe_read_response(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 /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
@ -343,26 +405,28 @@ 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--)
return; // FIXME: request again?
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
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(datagram);
ec_datagram_print_wc_error(fsm->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, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@ -435,7 +499,7 @@ 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;
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;
@ -456,15 +520,15 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
/** Write next fragment.
*/
void ec_fsm_soe_write_next_fragment(
ec_fsm_soe_t *fsm /**< finite state machine */
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
uint8_t incomplete, *data;
size_t header_size, max_fragment_size, remaining_size, fragment_size;
size_t header_size, max_fragment_size, remaining_size;
uint16_t fragments_left;
header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
@ -479,14 +543,14 @@ void ec_fsm_soe_write_next_fragment(
remaining_size = req->data_size - fsm->offset;
max_fragment_size = slave->configured_rx_mailbox_size - header_size;
incomplete = remaining_size > max_fragment_size;
fragment_size = incomplete ? max_fragment_size : remaining_size;
fragments_left = remaining_size / fragment_size - 1;
if (remaining_size % fragment_size) {
fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
fragments_left = remaining_size / fsm->fragment_size - 1;
if (remaining_size % fsm->fragment_size) {
fragments_left++;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
EC_SOE_SIZE + fragment_size);
EC_SOE_SIZE + fsm->fragment_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@ -497,16 +561,14 @@ void ec_fsm_soe_write_next_fragment(
(req->drive_no & 0x07) << 5);
EC_WRITE_U8(data + 1, 1 << 6); // only value included
EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
memcpy(data + 4, req->data + fsm->offset, fragment_size);
fsm->offset += fragment_size;
memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size);
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
ec_print_data(data, EC_SOE_SIZE + fragment_size);
ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
}
req->jiffies_sent = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_request;
}
@ -514,7 +576,10 @@ void ec_fsm_soe_write_next_fragment(
/** SoE state: WRITE START.
*/
void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
void ec_fsm_soe_write_start(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *req = fsm->request;
@ -530,48 +595,54 @@ void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
}
fsm->offset = 0;
ec_fsm_soe_write_next_fragment(fsm);
fsm->retries = EC_FSM_RETRIES;
ec_fsm_soe_write_next_fragment(fsm, datagram);
}
/*****************************************************************************/
/** SoE state: WRITE REQUEST.
*/
void ec_fsm_soe_write_request(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 /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return; // FIXME: check for response first?
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_fsm_soe_write_next_fragment(fsm, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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(fsm->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 (fsm->datagram->working_counter != 1) {
if (!fsm->datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
ec_fsm_soe_write_next_fragment(fsm, datagram);
return;
}
}
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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
@ -582,64 +653,65 @@ void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
/** CoE state: WRITE CHECK.
*/
void ec_fsm_soe_write_check(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 /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *req = fsm->request;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
if (fsm->datagram->working_counter != 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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (fsm->offset < req->data_size) {
ec_fsm_soe_write_next_fragment(fsm);
} else {
if (!ec_slave_mbox_check(datagram)) {
unsigned long diff_ms =
(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 write response.\n", diff_ms);
ec_fsm_soe_print_error(fsm);
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(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 write response.\n", diff_ms);
ec_fsm_soe_print_error(fsm);
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_response;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_response;
}
/*****************************************************************************/
/** SoE state: WRITE RESPONSE.
*/
void ec_fsm_soe_write_response(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 /**< Datagram to use. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
@ -647,27 +719,29 @@ 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 (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
return; // FIXME: request again?
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (fsm->datagram->state != 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(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
if (datagram->working_counter != 1) {
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(datagram);
ec_datagram_print_wc_error(fsm->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, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@ -689,7 +763,7 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
if (rec_size < EC_SOE_SIZE) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Received currupted SoE write response"
EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
" (%zu bytes)!\n", rec_size);
ec_print_data(data, rec_size);
ec_fsm_soe_print_error(fsm);
@ -735,14 +809,24 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
req->error_code = 0x0000;
}
fsm->state = ec_fsm_soe_end; // success
fsm->offset += fsm->fragment_size;
if (fsm->offset < req->data_size) {
fsm->retries = EC_FSM_RETRIES;
ec_fsm_soe_write_next_fragment(fsm, datagram);
} else {
fsm->state = ec_fsm_soe_end; // success
}
}
/*****************************************************************************/
/** State: ERROR.
*/
void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
void ec_fsm_soe_error(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
}
@ -750,7 +834,10 @@ void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
/** State: END.
*/
void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)
void ec_fsm_soe_end(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
}

13
master/fsm_soe.h
View File

@ -50,24 +50,25 @@ 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 */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_soe_t *); /**< CoE state function */
unsigned long jiffies_start; /**< CoE timestamp. */
void (*state)(ec_fsm_soe_t *, ec_datagram_t *); /**< CoE state function */
ec_datagram_t *datagram; /**< Datagram used in the previous step. */
unsigned long jiffies_start; /**< Timestamp. */
ec_soe_request_t *request; /**< SoE request */
off_t offset; /**< IDN data offset during fragmented write. */
size_t fragment_size; /**< Size of the current fragment. */
};
/*****************************************************************************/
void ec_fsm_soe_init(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_init(ec_fsm_soe_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 *);
int ec_fsm_soe_exec(ec_fsm_soe_t *);
int ec_fsm_soe_success(ec_fsm_soe_t *);
int ec_fsm_soe_exec(ec_fsm_soe_t *, ec_datagram_t *);
int ec_fsm_soe_success(const ec_fsm_soe_t *);
/*****************************************************************************/

25
master/ioctl.c
View File

@ -1753,12 +1753,11 @@ static ATTRIBUTES int ec_ioctl_send(
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
if (unlikely(!ctx->requested))
if (unlikely(!ctx->requested)) {
return -EPERM;
}
down(&master->io_sem);
ecrt_master_send(master);
up(&master->io_sem);
return 0;
}
@ -1772,12 +1771,11 @@ static ATTRIBUTES int ec_ioctl_receive(
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
if (unlikely(!ctx->requested))
if (unlikely(!ctx->requested)) {
return -EPERM;
}
down(&master->io_sem);
ecrt_master_receive(master);
up(&master->io_sem);
return 0;
}
@ -1864,12 +1862,11 @@ static ATTRIBUTES int ec_ioctl_sync_ref(
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
if (unlikely(!ctx->requested))
if (unlikely(!ctx->requested)) {
return -EPERM;
}
down(&master->io_sem);
ecrt_master_sync_reference_clock(master);
up(&master->io_sem);
return 0;
}
@ -1883,12 +1880,11 @@ static ATTRIBUTES int ec_ioctl_sync_slaves(
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
if (unlikely(!ctx->requested))
if (unlikely(!ctx->requested)) {
return -EPERM;
}
down(&master->io_sem);
ecrt_master_sync_slave_clocks(master);
up(&master->io_sem);
return 0;
}
@ -1931,12 +1927,11 @@ static ATTRIBUTES int ec_ioctl_sync_mon_queue(
ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
)
{
if (unlikely(!ctx->requested))
if (unlikely(!ctx->requested)) {
return -EPERM;
}
down(&master->io_sem);
ecrt_master_sync_monitor_queue(master);
up(&master->io_sem);
return 0;
}

344
master/master.c
View File

@ -136,7 +136,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
)
{
int ret;
unsigned int dev_idx;
unsigned int dev_idx, i;
master->index = index;
master->reserved = 0;
@ -193,11 +193,23 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
INIT_LIST_HEAD(&master->ext_datagram_queue);
sema_init(&master->ext_queue_sem, 1);
INIT_LIST_HEAD(&master->external_datagram_queue);
master->ext_ring_idx_rt = 0;
master->ext_ring_idx_fsm = 0;
// init external datagram ring
for (i = 0; i < EC_EXT_RING_SIZE; i++) {
ec_datagram_t *datagram = &master->ext_datagram_ring[i];
ec_datagram_init(datagram);
snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, "ext-%u", i);
}
// send interval in IDLE phase
ec_master_set_send_interval(master, 1000000 / HZ);
master->fsm_slave = NULL;
INIT_LIST_HEAD(&master->fsm_exec_list);
master->fsm_exec_count = 0U;
master->debug_level = debug_level;
master->stats.timeouts = 0;
master->stats.corrupted = 0;
@ -245,6 +257,17 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
// create state machine object
ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
// alloc external datagram ring
for (i = 0; i < EC_EXT_RING_SIZE; i++) {
ec_datagram_t *datagram = &master->ext_datagram_ring[i];
ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE);
if (ret) {
EC_MASTER_ERR(master, "Failed to allocate external"
" datagram %u.\n", i);
goto out_clear_ext_datagrams;
}
}
// init reference sync datagram
ec_datagram_init(&master->ref_sync_datagram);
snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE,
@ -254,7 +277,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
ec_datagram_clear(&master->ref_sync_datagram);
EC_MASTER_ERR(master, "Failed to allocate reference"
" synchronisation datagram.\n");
goto out_clear_fsm;
goto out_clear_ext_datagrams;
}
// init sync datagram
@ -337,7 +360,10 @@ out_clear_sync:
ec_datagram_clear(&master->sync_datagram);
out_clear_ref_sync:
ec_datagram_clear(&master->ref_sync_datagram);
out_clear_fsm:
out_clear_ext_datagrams:
for (i = 0; i < EC_EXT_RING_SIZE; i++) {
ec_datagram_clear(&master->ext_datagram_ring[i]);
}
ec_fsm_master_clear(&master->fsm);
ec_datagram_clear(&master->fsm_datagram);
out_clear_devices:
@ -355,7 +381,7 @@ void ec_master_clear(
ec_master_t *master /**< EtherCAT master */
)
{
unsigned int dev_idx;
unsigned int dev_idx, i;
#ifdef EC_RTDM
ec_rtdm_dev_clear(&master->rtdm_dev);
@ -379,6 +405,11 @@ void ec_master_clear(
ec_datagram_clear(&master->sync_mon_datagram);
ec_datagram_clear(&master->sync_datagram);
ec_datagram_clear(&master->ref_sync_datagram);
for (i = 0; i < EC_EXT_RING_SIZE; i++) {
ec_datagram_clear(&master->ext_datagram_ring[i]);
}
ec_fsm_master_clear(&master->fsm);
ec_datagram_clear(&master->fsm_datagram);
@ -446,9 +477,13 @@ void ec_master_clear_slaves(ec_master_t *master)
EC_MASTER_WARN(master, "Discarding SII request, slave %u about"
" to be deleted.\n", request->slave->ring_position);
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&master->request_queue);
wake_up_all(&master->request_queue);
}
master->fsm_slave = NULL;
INIT_LIST_HEAD(&master->fsm_exec_list);
master->fsm_exec_count = 0;
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
@ -743,75 +778,104 @@ void ec_master_inject_external_datagrams(
ec_master_t *master /**< EtherCAT master */
)
{
ec_datagram_t *datagram, *n;
size_t queue_size = 0;
ec_datagram_t *datagram;
size_t queue_size = 0, new_queue_size = 0;
#if DEBUG_INJECT
unsigned int datagram_count = 0;
#endif
list_for_each_entry(datagram, &master->datagram_queue, queue) {
queue_size += datagram->data_size;
if (master->ext_ring_idx_rt == master->ext_ring_idx_fsm) {
// nothing to inject
return;
}
list_for_each_entry(datagram, &master->datagram_queue, queue) {
if (datagram->state == EC_DATAGRAM_QUEUED) {
queue_size += datagram->data_size;
}
}
list_for_each_entry_safe(datagram, n, &master->external_datagram_queue,
queue) {
queue_size += datagram->data_size;
if (queue_size <= master->max_queue_size) {
list_del_init(&datagram->queue);
#if DEBUG_INJECT
EC_MASTER_DBG(master, 0, "Injecting external datagram %08x"
" size=%u, queue_size=%u\n", (unsigned int) datagram,
datagram->data_size, queue_size);
EC_MASTER_DBG(master, 1, "Injecting datagrams, queue_size=%zu\n",
queue_size);
#endif
while (master->ext_ring_idx_rt != master->ext_ring_idx_fsm) {
datagram = &master->ext_datagram_ring[master->ext_ring_idx_rt];
if (datagram->state != EC_DATAGRAM_INIT) {
// skip datagram
master->ext_ring_idx_rt =
(master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
continue;
}
new_queue_size = queue_size + datagram->data_size;
if (new_queue_size <= master->max_queue_size) {
#if DEBUG_INJECT
EC_MASTER_DBG(master, 1, "Injecting datagram %s"
" size=%zu, queue_size=%zu\n", datagram->name,
datagram->data_size, new_queue_size);
datagram_count++;
#endif
#ifdef EC_HAVE_CYCLES
datagram->cycles_sent = 0;
#endif
datagram->jiffies_sent = 0;
ec_master_queue_datagram(master, datagram);
} else {
if (datagram->data_size > master->max_queue_size) {
list_del_init(&datagram->queue);
queue_size = new_queue_size;
}
else if (datagram->data_size > master->max_queue_size) {
datagram->state = EC_DATAGRAM_ERROR;
EC_MASTER_ERR(master, "External datagram %s is too large,"
" size=%zu, max_queue_size=%zu\n",
datagram->name, datagram->data_size,
master->max_queue_size);
}
else { // datagram does not fit in the current cycle
#ifdef EC_HAVE_CYCLES
cycles_t cycles_now = get_cycles();
if (cycles_now - datagram->cycles_sent
> ext_injection_timeout_cycles)
#else
if (jiffies - datagram->jiffies_sent
> ext_injection_timeout_jiffies)
#endif
{
unsigned int time_us;
datagram->state = EC_DATAGRAM_ERROR;
EC_MASTER_ERR(master, "External datagram %p is too large,"
" size=%zu, max_queue_size=%zu\n",
datagram, datagram->data_size,
master->max_queue_size);
} else {
#ifdef EC_HAVE_CYCLES
cycles_t cycles_now = get_cycles();
if (cycles_now - datagram->cycles_sent
> ext_injection_timeout_cycles)
time_us = (unsigned int)
((cycles_now - datagram->cycles_sent) * 1000LL)
/ cpu_khz;
#else
if (jiffies - datagram->jiffies_sent
> ext_injection_timeout_jiffies)
time_us = (unsigned int)
((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
#endif
{
unsigned int time_us;
list_del_init(&datagram->queue);
datagram->state = EC_DATAGRAM_ERROR;
#ifdef EC_HAVE_CYCLES
time_us = (unsigned int)
((cycles_now - datagram->cycles_sent) * 1000LL)
/ cpu_khz;
#else
time_us = (unsigned int)
((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
#endif
EC_MASTER_ERR(master, "Timeout %u us: Injecting"
" external datagram %p size=%zu,"
" max_queue_size=%zu\n", time_us, datagram,
datagram->data_size, master->max_queue_size);
}
EC_MASTER_ERR(master, "Timeout %u us: Injecting"
" external datagram %s size=%zu,"
" max_queue_size=%zu\n", time_us, datagram->name,
datagram->data_size, master->max_queue_size);
}
else {
#if DEBUG_INJECT
else {
EC_MASTER_DBG(master, 0, "Deferred injecting"
" of external datagram %p"
" size=%u, queue_size=%u\n",
datagram, datagram->data_size, queue_size);
}
EC_MASTER_DBG(master, 1, "Deferred injecting"
" external datagram %s size=%u, queue_size=%u\n",
datagram->name, datagram->data_size, queue_size);
#endif
break;
}
}
master->ext_ring_idx_rt =
(master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
}
#if DEBUG_INJECT
EC_MASTER_DBG(master, 1, "Injected %u datagrams.\n", datagram_count);
#endif
}
/*****************************************************************************/
@ -832,42 +896,21 @@ void ec_master_set_send_interval(
/*****************************************************************************/
/** Places an external datagram in the sdo datagram queue.
/** Searches for a free datagram in the external datagram ring.
*/
void ec_master_queue_external_datagram(
ec_master_t *master, /**< EtherCAT master */
ec_datagram_t *datagram /**< datagram */
ec_datagram_t *ec_master_get_external_datagram(
ec_master_t *master /**< EtherCAT master */
)
{
ec_datagram_t *queued_datagram;
down(&master->io_sem);
// check, if the datagram is already queued
list_for_each_entry(queued_datagram, &master->external_datagram_queue,
queue) {
if (queued_datagram == datagram) {
up(&master->io_sem);
datagram->state = EC_DATAGRAM_QUEUED;
return;
}
if ((master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE !=
master->ext_ring_idx_rt) {
ec_datagram_t *datagram =
&master->ext_datagram_ring[master->ext_ring_idx_fsm];
return datagram;
}
else {
return NULL;
}
#if DEBUG_INJECT
EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n",
datagram, datagram->data_size);
#endif
list_add_tail(&datagram->queue, &master->external_datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
#ifdef EC_HAVE_CYCLES
datagram->cycles_sent = get_cycles();
#endif
datagram->jiffies_sent = jiffies;
up(&master->io_sem);
master->fsm.idle = 0;
}
/*****************************************************************************/
@ -1366,12 +1409,101 @@ void ec_master_nanosleep(const unsigned long nsecs)
/*****************************************************************************/
/** Execute slave FSMs.
*/
void ec_master_exec_slave_fsms(
ec_master_t *master /**< EtherCAT master. */
)
{
ec_datagram_t *datagram;
ec_fsm_slave_t *fsm, *next;
unsigned int count = 0;
list_for_each_entry_safe(fsm, next, &master->fsm_exec_list, list) {
if (!fsm->datagram) {
EC_MASTER_WARN(master, "Slave %u FSM has zero datagram."
"This is a bug!\n", fsm->slave->ring_position);
list_del_init(&fsm->list);
master->fsm_exec_count--;
return;
}
if (fsm->datagram->state == EC_DATAGRAM_INIT ||
fsm->datagram->state == EC_DATAGRAM_QUEUED ||
fsm->datagram->state == EC_DATAGRAM_SENT) {
// previous datagram was not sent or received yet.
// wait until next thread execution
return;
}
datagram = ec_master_get_external_datagram(master);
if (!datagram) {
// no free datagrams at the moment
EC_MASTER_WARN(master, "No free datagram during"
" slave FSM execution. This is a bug!\n");
continue;
}
#if DEBUG_INJECT
EC_MASTER_DBG(master, 1, "Executing slave %u FSM.\n",
fsm->slave->ring_position);
#endif
if (ec_fsm_slave_exec(fsm, datagram)) {
// FSM consumed datagram
#if DEBUG_INJECT
EC_MASTER_DBG(master, 1, "FSM consumed datagram %s\n",
datagram->name);
#endif
master->ext_ring_idx_fsm =
(master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
}
else {
// FSM finished
list_del_init(&fsm->list);
master->fsm_exec_count--;
#if DEBUG_INJECT
EC_MASTER_DBG(master, 1, "FSM finished. %u remaining.\n",
master->fsm_exec_count);
#endif
}
}
while (master->fsm_exec_count < EC_EXT_RING_SIZE / 2
&& count < master->slave_count) {
if (ec_fsm_slave_is_ready(&master->fsm_slave->fsm)) {
datagram = ec_master_get_external_datagram(master);
if (ec_fsm_slave_exec(&master->fsm_slave->fsm, datagram)) {
master->ext_ring_idx_fsm =
(master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
list_add_tail(&master->fsm_slave->fsm.list,
&master->fsm_exec_list);
master->fsm_exec_count++;
#if DEBUG_INJECT
EC_MASTER_DBG(master, 1, "New slave %u FSM"
" consumed datagram %s, now %u FSMs in list.\n",
master->fsm_slave->ring_position, datagram->name,
master->fsm_exec_count);
#endif
}
}
master->fsm_slave++;
if (master->fsm_slave >= master->slaves + master->slave_count) {
master->fsm_slave = master->slaves;
}
count++;
}
}
/*****************************************************************************/
/** Master kernel thread function for IDLE phase.
*/
static int ec_master_idle_thread(void *priv_data)
{
ec_master_t *master = (ec_master_t *) priv_data;
ec_slave_t *slave = NULL;
int fsm_exec;
#ifdef EC_USE_HRTIMER
size_t sent_bytes;
@ -1392,8 +1524,6 @@ static int ec_master_idle_thread(void *priv_data)
ecrt_master_receive(master);
up(&master->io_sem);
fsm_exec = 0;
// execute master & slave state machines
if (down_interruptible(&master->master_sem)) {
break;
@ -1401,11 +1531,7 @@ static int ec_master_idle_thread(void *priv_data)
fsm_exec = ec_fsm_master_exec(&master->fsm);
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
ec_fsm_slave_exec(&slave->fsm);
}
ec_master_exec_slave_fsms(master);
up(&master->master_sem);
@ -1449,8 +1575,6 @@ static int ec_master_idle_thread(void *priv_data)
static int ec_master_operation_thread(void *priv_data)
{
ec_master_t *master = (ec_master_t *) priv_data;
ec_slave_t *slave = NULL;
int fsm_exec;
EC_MASTER_DBG(master, 1, "Operation thread running"
" with fsm interval = %u us, max data size=%zu\n",
@ -1463,28 +1587,20 @@ static int ec_master_operation_thread(void *priv_data)
// output statistics
ec_master_output_stats(master);
fsm_exec = 0;
// execute master & slave state machines
if (down_interruptible(&master->master_sem)) {
break;
}
fsm_exec += ec_fsm_master_exec(&master->fsm);
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
ec_fsm_slave_exec(&slave->fsm);
}
up(&master->master_sem);
// Inject datagrams (let the RT thread queue them, see
// ecrt_master_send())
if (fsm_exec) {
if (ec_fsm_master_exec(&master->fsm)) {
// Inject datagrams (let the RT thread queue them, see
// ecrt_master_send())
master->injection_seq_fsm++;
}
ec_master_exec_slave_fsms(master);
up(&master->master_sem);
}
#ifdef EC_USE_HRTIMER
@ -2282,7 +2398,7 @@ void ecrt_master_send(ec_master_t *master)
ec_device_index_t dev_idx;
if (master->injection_seq_rt != master->injection_seq_fsm) {
// inject datagrams produced by master FSM
// inject datagram produced by master FSM
ec_master_queue_datagram(master, &master->fsm_datagram);
master->injection_seq_rt = master->injection_seq_fsm;
}

20
master/master.h
View File

@ -114,6 +114,13 @@
} \
} while (0)
/** Size of the external datagram ring.
*
* The external datagram ring is used for slave FSMs.
*/
#define EC_EXT_RING_SIZE 32
/*****************************************************************************/
/** EtherCAT master phase.
@ -260,11 +267,20 @@ struct ec_master {
struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
ext_datagram_queue. */
struct list_head external_datagram_queue; /**< External Datagram queue. */
ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /**< External datagram
ring. */
unsigned int ext_ring_idx_rt; /**< Index in external datagram ring for RT
side. */
unsigned int ext_ring_idx_fsm; /**< Index in external datagram ring for
FSM side. */
unsigned int send_interval; /**< Interval between two calls to
ecrt_master_send(). */
size_t max_queue_size; /**< Maximum size of datagram queue */
ec_slave_t *fsm_slave; /**< Slave that is queried next for FSM exec. */
struct list_head fsm_exec_list; /**< Slave FSM execution list. */
unsigned int fsm_exec_count; /**< Number of entries in execution list. */
unsigned int debug_level; /**< Master debug level. */
ec_stats_t stats; /**< Cyclic statistics. */
@ -324,8 +340,6 @@ void ec_master_eoe_stop(ec_master_t *);
void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t);
void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *);
void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *);
void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
void ec_master_inject_external_datagrams(ec_master_t *);
// misc.
void ec_master_set_send_interval(ec_master_t *, unsigned int);

21
master/slave.c
View File

@ -68,7 +68,6 @@ void ec_slave_init(
)
{
unsigned int i;
int ret;
slave->master = master;
slave->device_index = dev_idx;
@ -157,19 +156,8 @@ void ec_slave_init(
INIT_LIST_HEAD(&slave->foe_requests);
INIT_LIST_HEAD(&slave->soe_requests);
// init state machine datagram
ec_datagram_init(&slave->fsm_datagram);
snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE,
"slave%u-fsm", slave->ring_position);
ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE);
if (ret < 0) {
ec_datagram_clear(&slave->fsm_datagram);
EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n");
return;
}
// create state machine object
ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram);
ec_fsm_slave_init(&slave->fsm, slave);
}
/*****************************************************************************/
@ -194,7 +182,6 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
EC_SLAVE_WARN(slave, "Discarding SDO request,"
" slave about to be deleted.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
}
while (!list_empty(&slave->reg_requests)) {
@ -204,7 +191,6 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
EC_SLAVE_WARN(slave, "Discarding register request,"
" slave about to be deleted.\n");
reg->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
}
while (!list_empty(&slave->foe_requests)) {
@ -214,7 +200,6 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
EC_SLAVE_WARN(slave, "Discarding FoE request,"
" slave about to be deleted.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
}
while (!list_empty(&slave->soe_requests)) {
@ -224,9 +209,10 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
EC_SLAVE_WARN(slave, "Discarding SoE request,"
" slave about to be deleted.\n");
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->master->request_queue);
}
wake_up_all(&slave->master->request_queue);
if (slave->config) {
ec_slave_config_detach(slave->config);
}
@ -260,7 +246,6 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
}
ec_fsm_slave_clear(&slave->fsm);
ec_datagram_clear(&slave->fsm_datagram);
}
/*****************************************************************************/

1
master/slave.h
View File

@ -230,7 +230,6 @@ struct ec_slave
struct list_head soe_requests; /**< SoE write requests. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
};
/*****************************************************************************/