Mirrors_Framework/ethercat
1
0
Fork 0
mirror of https://gitlab.com/etherlab.org/ethercat.git synced 2026-09-22 18:13:46 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Avoided io_sem locking from userspace/rtdm library.

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
NEWS
+1
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:
master/fsm_coe.c
+619 -449
View File
File diff suppressed because it is too large Load Diff
master/fsm_coe.h
+6 -5
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 *);
/*****************************************************************************/
master/fsm_foe.c
+174 -148
View File
File diff suppressed because it is too large Load Diff
master/fsm_foe.h
+12 -11
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 *);
master/fsm_master.c
+12 -11
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");
master/fsm_pdo.c
+120 -88
View File
File diff suppressed because it is too large Load Diff
master/fsm_pdo.h
+2 -2
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 *);
/*****************************************************************************/
master/fsm_pdo_entry.c
+65 -41
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. */
)
{
}
master/fsm_pdo_entry.h
+3 -2
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 *);
/*****************************************************************************/
master/fsm_slave.c
+124 -81
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;
}
master/fsm_slave.h
+11 -9
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 *);
/*****************************************************************************/
master/fsm_slave_config.c
+11 -10
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;
}
}
master/fsm_slave_scan.c
+3 -2
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;
master/fsm_soe.c
+224 -137
View File
File diff suppressed because it is too large Load Diff
master/fsm_soe.h
+7 -6
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 *);
/*****************************************************************************/
master/ioctl.c
+10 -15
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;
}
master/master.c
+230 -114
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;
}
master/master.h
+17 -3
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);
master/slave.c
+3 -18
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);
}
/*****************************************************************************/
master/slave.h
-1
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. */
};
/*****************************************************************************/