Slave configuration exclusively from master thread; removed
ec_master_sync_io(); introduced events in the master state machine; locking of slave scanning and slave configuration through allow_scan and allow_config flags.
This commit is contained in:
parent
a5f7899df7
commit
f5a7e78786
3
NEWS
3
NEWS
|
|
@ -37,6 +37,9 @@ Changes in version 1.3.0:
|
|||
* Implemented setting of secondary slave address (alias) via sysfs.
|
||||
* Implemented SDO reading in operation mode via sysfs.
|
||||
* Removed annoying eeprom_write_enable file. EEPROM writing always enabled.
|
||||
* Slave configuration is now done exclusively from the master thread. Removed
|
||||
ec_master_sync_io(). Userspace threads are now waiting for events in the
|
||||
state machine.
|
||||
* Master state machine scheduled with timeout if idle, otherwise is executed
|
||||
as fast as possible (with schedule()).
|
||||
* Removed EtherCAT line comments from 8139too drivers.
|
||||
|
|
|
|||
4
TODO
4
TODO
|
|
@ -28,6 +28,10 @@ $Id$
|
|||
- SDO write access in sysfs.
|
||||
- Configure slave ports to automatically open on link detection.
|
||||
- Calculate cycle time of operation state machine.
|
||||
- Locking for serveral slave variables for sysfs access.
|
||||
- Interrupt master state machines state scan for other jobs.
|
||||
- Master state machine, slave configuration: Do not check every slave on
|
||||
a cycle.
|
||||
|
||||
* Less important issues:
|
||||
- Implement all EtherCAT datagram types.
|
||||
|
|
|
|||
|
|
@ -221,53 +221,68 @@ void ec_fsm_master_state_broadcast(ec_fsm_master_t *fsm /**< master state machin
|
|||
EC_INFO("Slave states: %s.\n", states);
|
||||
}
|
||||
|
||||
// topology change in idle mode: clear all slaves and scan the bus
|
||||
if (fsm->topology_change_pending &&
|
||||
master->mode == EC_MASTER_MODE_IDLE) {
|
||||
fsm->topology_change_pending = 0;
|
||||
fsm->tainted = 0;
|
||||
fsm->idle = 0;
|
||||
fsm->scan_jiffies = jiffies;
|
||||
if (fsm->topology_change_pending) {
|
||||
down(&master->scan_sem);
|
||||
if (!master->allow_scan) {
|
||||
up(&master->scan_sem);
|
||||
}
|
||||
else {
|
||||
master->scan_state = EC_REQUEST_IN_PROGRESS;
|
||||
up(&master->scan_sem);
|
||||
|
||||
// topology change when scan is allowed:
|
||||
// clear all slaves and scan the bus
|
||||
fsm->topology_change_pending = 0;
|
||||
fsm->tainted = 0;
|
||||
fsm->idle = 0;
|
||||
fsm->scan_jiffies = jiffies;
|
||||
|
||||
ec_master_eoe_stop(master);
|
||||
ec_master_destroy_slaves(master);
|
||||
ec_master_eoe_stop(master);
|
||||
ec_master_destroy_slaves(master);
|
||||
|
||||
master->slave_count = datagram->working_counter;
|
||||
master->slave_count = datagram->working_counter;
|
||||
|
||||
if (!master->slave_count) {
|
||||
// no slaves present -> finish state machine.
|
||||
fsm->state = ec_fsm_master_state_end;
|
||||
if (!master->slave_count) {
|
||||
// no slaves present -> finish state machine.
|
||||
master->scan_state = EC_REQUEST_COMPLETE;
|
||||
wake_up_interruptible(&master->scan_queue);
|
||||
fsm->state = ec_fsm_master_state_end;
|
||||
return;
|
||||
}
|
||||
|
||||
// init slaves
|
||||
for (i = 0; i < master->slave_count; i++) {
|
||||
if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),
|
||||
GFP_ATOMIC))) {
|
||||
EC_ERR("Failed to allocate slave %i!\n", i);
|
||||
ec_master_destroy_slaves(master);
|
||||
master->scan_state = EC_REQUEST_FAILURE;
|
||||
wake_up_interruptible(&master->scan_queue);
|
||||
fsm->state = ec_fsm_master_state_error;
|
||||
return;
|
||||
}
|
||||
|
||||
if (ec_slave_init(slave, master, i, i + 1)) {
|
||||
// freeing of "slave" already done
|
||||
ec_master_destroy_slaves(master);
|
||||
master->scan_state = EC_REQUEST_FAILURE;
|
||||
wake_up_interruptible(&master->scan_queue);
|
||||
fsm->state = ec_fsm_master_state_error;
|
||||
return;
|
||||
}
|
||||
|
||||
list_add_tail(&slave->list, &master->slaves);
|
||||
}
|
||||
|
||||
EC_INFO("Scanning bus.\n");
|
||||
|
||||
// begin scanning of slaves
|
||||
fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
|
||||
fsm->state = ec_fsm_master_state_scan_slaves;
|
||||
ec_fsm_slave_start_scan(&fsm->fsm_slave, fsm->slave);
|
||||
ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately
|
||||
return;
|
||||
}
|
||||
|
||||
// init slaves
|
||||
for (i = 0; i < master->slave_count; i++) {
|
||||
if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),
|
||||
GFP_ATOMIC))) {
|
||||
EC_ERR("Failed to allocate slave %i!\n", i);
|
||||
ec_master_destroy_slaves(master);
|
||||
fsm->state = ec_fsm_master_state_error;
|
||||
return;
|
||||
}
|
||||
|
||||
if (ec_slave_init(slave, master, i, i + 1)) {
|
||||
// freeing of "slave" already done
|
||||
ec_master_destroy_slaves(master);
|
||||
fsm->state = ec_fsm_master_state_error;
|
||||
return;
|
||||
}
|
||||
|
||||
list_add_tail(&slave->list, &master->slaves);
|
||||
}
|
||||
|
||||
EC_INFO("Scanning bus.\n");
|
||||
|
||||
// begin scanning of slaves
|
||||
fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
|
||||
fsm->state = ec_fsm_master_state_scan_slaves;
|
||||
ec_fsm_slave_start_scan(&fsm->fsm_slave, fsm->slave);
|
||||
ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately
|
||||
return;
|
||||
}
|
||||
|
||||
// fetch state from each slave
|
||||
|
|
@ -391,6 +406,57 @@ int ec_fsm_master_action_process_sdo(
|
|||
|
||||
/*****************************************************************************/
|
||||
|
||||
/**
|
||||
*/
|
||||
|
||||
int ec_fsm_master_action_configure(
|
||||
ec_fsm_master_t *fsm /**< master state machine */
|
||||
)
|
||||
{
|
||||
ec_slave_t *slave;
|
||||
ec_master_t *master = fsm->master;
|
||||
char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];
|
||||
|
||||
// check if any slaves are not in the state, they're supposed to be
|
||||
// FIXME do not check all slaves in every cycle...
|
||||
list_for_each_entry(slave, &master->slaves, list) {
|
||||
if (slave->error_flag
|
||||
|| slave->online_state == EC_SLAVE_OFFLINE
|
||||
|| slave->requested_state == EC_SLAVE_STATE_UNKNOWN
|
||||
|| (slave->current_state == slave->requested_state
|
||||
&& slave->self_configured)) continue;
|
||||
|
||||
if (master->debug_level) {
|
||||
ec_state_string(slave->current_state, old_state);
|
||||
if (slave->current_state != slave->requested_state) {
|
||||
ec_state_string(slave->requested_state, new_state);
|
||||
EC_DBG("Changing state of slave %i (%s -> %s).\n",
|
||||
slave->ring_position, old_state, new_state);
|
||||
}
|
||||
else if (!slave->self_configured) {
|
||||
EC_DBG("Reconfiguring slave %i (%s).\n",
|
||||
slave->ring_position, old_state);
|
||||
}
|
||||
}
|
||||
|
||||
fsm->idle = 0;
|
||||
fsm->slave = slave;
|
||||
fsm->state = ec_fsm_master_state_configure_slave;
|
||||
ec_fsm_slave_start_conf(&fsm->fsm_slave, slave);
|
||||
ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (fsm->config_error)
|
||||
master->config_state = EC_REQUEST_FAILURE;
|
||||
else
|
||||
master->config_state = EC_REQUEST_COMPLETE;
|
||||
wake_up_interruptible(&master->config_queue);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/**
|
||||
Master action: PROC_STATES.
|
||||
Processes the slave states.
|
||||
|
|
@ -402,35 +468,19 @@ void ec_fsm_master_action_process_states(ec_fsm_master_t *fsm
|
|||
{
|
||||
ec_master_t *master = fsm->master;
|
||||
ec_slave_t *slave;
|
||||
char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];
|
||||
|
||||
// check if any slaves are not in the state, they're supposed to be
|
||||
list_for_each_entry(slave, &master->slaves, list) {
|
||||
if (slave->error_flag
|
||||
|| slave->online_state == EC_SLAVE_OFFLINE
|
||||
|| slave->requested_state == EC_SLAVE_STATE_UNKNOWN
|
||||
|| (slave->current_state == slave->requested_state
|
||||
&& slave->self_configured)) continue;
|
||||
|
||||
if (master->debug_level) {
|
||||
ec_state_string(slave->current_state, old_state);
|
||||
if (slave->current_state != slave->requested_state) {
|
||||
ec_state_string(slave->requested_state, new_state);
|
||||
EC_DBG("Changing state of slave %i (%s -> %s).\n",
|
||||
slave->ring_position, old_state, new_state);
|
||||
}
|
||||
else if (!slave->self_configured) {
|
||||
EC_DBG("Reconfiguring slave %i (%s).\n",
|
||||
slave->ring_position, old_state);
|
||||
}
|
||||
}
|
||||
|
||||
fsm->idle = 0;
|
||||
fsm->slave = slave;
|
||||
fsm->state = ec_fsm_master_state_configure_slave;
|
||||
ec_fsm_slave_start_conf(&fsm->fsm_slave, slave);
|
||||
ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately
|
||||
return;
|
||||
down(&master->config_sem);
|
||||
if (!master->allow_config) {
|
||||
up(&master->config_sem);
|
||||
}
|
||||
else {
|
||||
master->config_state = EC_REQUEST_IN_PROGRESS;
|
||||
fsm->config_error = 0;
|
||||
up(&master->config_sem);
|
||||
|
||||
// check for pending slave configurations
|
||||
if (ec_fsm_master_action_configure(fsm))
|
||||
return;
|
||||
}
|
||||
|
||||
// Check, if EoE processing has to be started
|
||||
|
|
@ -769,13 +819,8 @@ void ec_fsm_master_state_scan_slaves(
|
|||
|
||||
EC_INFO("Bus scanning completed in %u ms.\n",
|
||||
(u32) (jiffies - fsm->scan_jiffies) * 1000 / HZ);
|
||||
|
||||
// set initial states of all slaves to PREOP to make mailbox
|
||||
// communication possible
|
||||
list_for_each_entry(slave, &master->slaves, list) {
|
||||
if (slave->requested_state == EC_SLAVE_STATE_UNKNOWN)
|
||||
ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
|
||||
}
|
||||
master->scan_state = EC_REQUEST_COMPLETE;
|
||||
wake_up_interruptible(&master->scan_queue);
|
||||
|
||||
fsm->state = ec_fsm_master_state_end;
|
||||
}
|
||||
|
|
@ -794,7 +839,14 @@ void ec_fsm_master_state_configure_slave(ec_fsm_master_t *fsm
|
|||
if (ec_fsm_slave_exec(&fsm->fsm_slave)) // execute slave's state machine
|
||||
return;
|
||||
|
||||
ec_fsm_master_action_process_states(fsm);
|
||||
if (!ec_fsm_slave_success(&fsm->fsm_slave))
|
||||
fsm->config_error = 1;
|
||||
|
||||
// configure next slave, if necessary
|
||||
if (ec_fsm_master_action_configure(fsm))
|
||||
return;
|
||||
|
||||
fsm->state = ec_fsm_master_state_end;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -89,6 +89,7 @@ struct ec_fsm_master
|
|||
unsigned int validate; /**< non-zero, if validation to do */
|
||||
unsigned int tainted; /**< non-zero, if the current bus topology does
|
||||
not meet the initial conditions */
|
||||
unsigned int config_error; /**< error during slave configuration */
|
||||
ec_slave_t *slave; /**< current slave */
|
||||
ec_eeprom_write_request_t *eeprom_request; /**< EEPROM write request */
|
||||
off_t eeprom_index; /**< index to EEPROM write request data */
|
||||
|
|
|
|||
272
master/master.c
272
master/master.c
|
|
@ -121,7 +121,17 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
|
|||
|
||||
INIT_LIST_HEAD(&master->slaves);
|
||||
master->slave_count = 0;
|
||||
|
||||
master->scan_state = EC_REQUEST_IN_PROGRESS;
|
||||
master->allow_scan = 1;
|
||||
init_MUTEX(&master->scan_sem);
|
||||
init_waitqueue_head(&master->scan_queue);
|
||||
|
||||
master->config_state = EC_REQUEST_COMPLETE;
|
||||
master->allow_config = 1;
|
||||
init_MUTEX(&master->config_sem);
|
||||
init_waitqueue_head(&master->config_queue);
|
||||
|
||||
INIT_LIST_HEAD(&master->datagram_queue);
|
||||
master->datagram_index = 0;
|
||||
|
||||
|
|
@ -320,24 +330,13 @@ void ec_master_release_cb(void *master /**< callback data */)
|
|||
|
||||
/**
|
||||
* Starts the master thread.
|
||||
*/
|
||||
*/
|
||||
|
||||
int ec_master_thread_start(ec_master_t *master /**< EtherCAT master */)
|
||||
int ec_master_thread_start(
|
||||
ec_master_t *master, /**< EtherCAT master */
|
||||
int (*thread_func)(ec_master_t *) /**< thread function to start */
|
||||
)
|
||||
{
|
||||
int (*thread_func)(ec_master_t *);
|
||||
|
||||
switch (master->mode) {
|
||||
case EC_MASTER_MODE_IDLE:
|
||||
thread_func = ec_master_idle_thread;
|
||||
break;
|
||||
case EC_MASTER_MODE_OPERATION:
|
||||
thread_func = ec_master_operation_thread;
|
||||
break;
|
||||
default:
|
||||
EC_ERR("Invalid master mode while starting thread!\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
init_completion(&master->thread_exit);
|
||||
|
||||
EC_INFO("Starting master thread.\n");
|
||||
|
|
@ -352,22 +351,32 @@ int ec_master_thread_start(ec_master_t *master /**< EtherCAT master */)
|
|||
|
||||
/**
|
||||
* Stops the master thread.
|
||||
*/
|
||||
*/
|
||||
|
||||
void ec_master_thread_stop(ec_master_t *master /**< EtherCAT master */)
|
||||
{
|
||||
if (master->thread_id) {
|
||||
kill_proc(master->thread_id, SIGTERM, 1);
|
||||
wait_for_completion(&master->thread_exit);
|
||||
EC_INFO("Master thread exited.\n");
|
||||
}
|
||||
if (!master->thread_id) {
|
||||
EC_WARN("ec_master_thread_stop: Already finished!\n");
|
||||
return;
|
||||
}
|
||||
|
||||
kill_proc(master->thread_id, SIGTERM, 1);
|
||||
wait_for_completion(&master->thread_exit);
|
||||
EC_INFO("Master thread exited.\n");
|
||||
|
||||
if (master->fsm_datagram.state != EC_DATAGRAM_SENT) return;
|
||||
|
||||
// wait for FSM datagram
|
||||
while (get_cycles() - master->fsm_datagram.cycles_sent
|
||||
< (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000))
|
||||
schedule();
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/**
|
||||
* Transition function from ORPHANED to IDLE mode.
|
||||
*/
|
||||
*/
|
||||
|
||||
int ec_master_enter_idle_mode(ec_master_t *master /**< EtherCAT master */)
|
||||
{
|
||||
|
|
@ -376,7 +385,7 @@ int ec_master_enter_idle_mode(ec_master_t *master /**< EtherCAT master */)
|
|||
master->cb_data = master;
|
||||
|
||||
master->mode = EC_MASTER_MODE_IDLE;
|
||||
if (ec_master_thread_start(master)) {
|
||||
if (ec_master_thread_start(master, ec_master_idle_thread)) {
|
||||
master->mode = EC_MASTER_MODE_ORPHANED;
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -388,7 +397,7 @@ int ec_master_enter_idle_mode(ec_master_t *master /**< EtherCAT master */)
|
|||
|
||||
/**
|
||||
* Transition function from IDLE to ORPHANED mode.
|
||||
*/
|
||||
*/
|
||||
|
||||
void ec_master_leave_idle_mode(ec_master_t *master /**< EtherCAT master */)
|
||||
{
|
||||
|
|
@ -403,57 +412,55 @@ void ec_master_leave_idle_mode(ec_master_t *master /**< EtherCAT master */)
|
|||
|
||||
/**
|
||||
* Transition function from IDLE to OPERATION mode.
|
||||
*/
|
||||
*/
|
||||
|
||||
int ec_master_enter_operation_mode(ec_master_t *master /**< EtherCAT master */)
|
||||
{
|
||||
ec_slave_t *slave;
|
||||
|
||||
ec_master_eoe_stop(master); // stop EoE timer
|
||||
master->eoe_checked = 1; // prevent from starting again by FSM
|
||||
ec_master_thread_stop(master);
|
||||
down(&master->config_sem);
|
||||
master->allow_config = 0; // temporarily disable slave configuration
|
||||
up(&master->config_sem);
|
||||
|
||||
master->mode = EC_MASTER_MODE_OPERATION;
|
||||
|
||||
// wait for FSM datagram
|
||||
if (master->fsm_datagram.state == EC_DATAGRAM_SENT) {
|
||||
while (get_cycles() - master->fsm_datagram.cycles_sent
|
||||
< (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)) {}
|
||||
ecrt_master_receive(master);
|
||||
// wait for slave configuration to complete
|
||||
if (wait_event_interruptible(master->config_queue,
|
||||
master->config_state != EC_REQUEST_IN_PROGRESS)) {
|
||||
EC_INFO("Finishing slave configuration interrupted by signal.\n");
|
||||
goto out_allow;
|
||||
}
|
||||
|
||||
// finish running IDLE FSM
|
||||
if (ec_fsm_master_running(&master->fsm)) {
|
||||
while (ec_fsm_master_exec(&master->fsm)) {
|
||||
ec_master_queue_datagram(master, &master->fsm_datagram);
|
||||
ec_master_sync_io(master);
|
||||
}
|
||||
if (master->debug_level)
|
||||
EC_DBG("Waiting for pending slave configuration returned.\n");
|
||||
|
||||
down(&master->scan_sem);
|
||||
master->allow_scan = 0; // 'lock' the slave list
|
||||
up(&master->scan_sem);
|
||||
|
||||
// wait for slave scan to complete
|
||||
if (wait_event_interruptible(master->scan_queue,
|
||||
master->scan_state != EC_REQUEST_IN_PROGRESS)) {
|
||||
EC_INFO("Waiting for slave scan interrupted by signal.\n");
|
||||
goto out_allow;
|
||||
}
|
||||
|
||||
if (master->debug_level) {
|
||||
if (ec_master_measure_bus_time(master)) {
|
||||
EC_ERR("Bus time measuring failed!\n");
|
||||
goto out_idle;
|
||||
}
|
||||
}
|
||||
if (master->debug_level)
|
||||
EC_DBG("Waiting for pending slave scan returned.\n");
|
||||
|
||||
// set initial slave states
|
||||
// set states for all slaves
|
||||
list_for_each_entry(slave, &master->slaves, list) {
|
||||
ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
|
||||
}
|
||||
|
||||
master->eoe_checked = 0; // allow starting EoE again
|
||||
if (master->debug_level)
|
||||
EC_DBG("Switching to operation mode.\n");
|
||||
|
||||
master->mode = EC_MASTER_MODE_OPERATION;
|
||||
master->pdo_slaves_offline = 0; // assume all PDO slaves online
|
||||
master->injection_seq_fsm = 0;
|
||||
master->injection_seq_rt = 0;
|
||||
|
||||
return 0;
|
||||
|
||||
out_idle:
|
||||
master->mode = EC_MASTER_MODE_IDLE;
|
||||
if (ec_master_thread_start(master)) {
|
||||
EC_WARN("Failed to start master thread again!\n");
|
||||
}
|
||||
|
||||
out_allow:
|
||||
master->allow_scan = 1;
|
||||
master->allow_config = 1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -461,73 +468,35 @@ int ec_master_enter_operation_mode(ec_master_t *master /**< EtherCAT master */)
|
|||
|
||||
/**
|
||||
* Transition function from OPERATION to IDLE mode.
|
||||
*/
|
||||
*/
|
||||
|
||||
void ec_master_leave_operation_mode(ec_master_t *master
|
||||
/**< EtherCAT master */)
|
||||
{
|
||||
ec_slave_t *slave;
|
||||
ec_fsm_master_t *fsm = &master->fsm;
|
||||
ec_fsm_slave_t fsm_slave;
|
||||
|
||||
ec_master_eoe_stop(master); // stop EoE timer
|
||||
master->eoe_checked = 1; // prevent from starting again by FSM
|
||||
ec_master_thread_stop(master); // stop master thread
|
||||
master->injection_seq_fsm = 0;
|
||||
master->injection_seq_rt = 0;
|
||||
master->mode = EC_MASTER_MODE_IDLE;
|
||||
|
||||
// wait for FSM datagram
|
||||
if (master->fsm_datagram.state == EC_DATAGRAM_SENT) {
|
||||
// active waiting
|
||||
while (get_cycles() - master->fsm_datagram.cycles_sent
|
||||
< (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000));
|
||||
ecrt_master_receive(master);
|
||||
}
|
||||
|
||||
// finish running master FSM
|
||||
if (ec_fsm_master_running(fsm)) {
|
||||
while (ec_fsm_master_exec(fsm)) {
|
||||
ec_master_queue_datagram(master, &master->fsm_datagram);
|
||||
ec_master_sync_io(master);
|
||||
}
|
||||
}
|
||||
|
||||
ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram);
|
||||
ec_master_eoe_stop(master);
|
||||
ec_master_thread_stop(master);
|
||||
|
||||
master->request_cb = ec_master_request_cb;
|
||||
master->release_cb = ec_master_release_cb;
|
||||
master->cb_data = master;
|
||||
|
||||
// set states for all slaves
|
||||
list_for_each_entry(slave, &master->slaves, list) {
|
||||
ec_slave_reset(slave);
|
||||
ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
|
||||
|
||||
// don't try to set PREOP for slaves that don't respond,
|
||||
// because of 3 second timeout.
|
||||
if (slave->online_state == EC_SLAVE_OFFLINE) {
|
||||
if (master->debug_level)
|
||||
EC_DBG("Skipping to configure offline slave %i.\n",
|
||||
slave->ring_position);
|
||||
continue;
|
||||
}
|
||||
|
||||
ec_fsm_slave_start_conf(&fsm_slave, slave);
|
||||
while (ec_fsm_slave_exec(&fsm_slave)) {
|
||||
ec_master_queue_datagram(master, &master->fsm_datagram);
|
||||
ec_master_sync_io(master);
|
||||
}
|
||||
}
|
||||
|
||||
ec_fsm_slave_clear(&fsm_slave);
|
||||
|
||||
ec_master_destroy_domains(master);
|
||||
|
||||
master->allow_scan = 1;
|
||||
master->allow_config = 1;
|
||||
|
||||
master->request_cb = ec_master_request_cb;
|
||||
master->release_cb = ec_master_release_cb;
|
||||
master->cb_data = master;
|
||||
|
||||
master->eoe_checked = 0; // allow EoE again
|
||||
|
||||
master->mode = EC_MASTER_MODE_IDLE;
|
||||
if (ec_master_thread_start(master)) {
|
||||
EC_WARN("Failed to start master thread!\n");
|
||||
if (ec_master_thread_start(master, ec_master_idle_thread)) {
|
||||
EC_WARN("Failed to restart master thread!\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -811,7 +780,7 @@ static int ec_master_idle_thread(ec_master_t *master)
|
|||
daemonize("EtherCAT-IDLE");
|
||||
allow_signal(SIGTERM);
|
||||
|
||||
while (!signal_pending(current) && master->mode == EC_MASTER_MODE_IDLE) {
|
||||
while (!signal_pending(current)) {
|
||||
cycles_start = get_cycles();
|
||||
|
||||
if (ec_fsm_master_running(&master->fsm)) {
|
||||
|
|
@ -850,6 +819,8 @@ schedule:
|
|||
}
|
||||
|
||||
master->thread_id = 0;
|
||||
if (master->debug_level)
|
||||
EC_DBG("Master IDLE thread exiting...\n");
|
||||
complete_and_exit(&master->thread_exit, 0);
|
||||
}
|
||||
|
||||
|
|
@ -866,9 +837,7 @@ static int ec_master_operation_thread(ec_master_t *master)
|
|||
daemonize("EtherCAT-OP");
|
||||
allow_signal(SIGTERM);
|
||||
|
||||
while (!signal_pending(current) &&
|
||||
master->mode == EC_MASTER_MODE_OPERATION) {
|
||||
|
||||
while (!signal_pending(current)) {
|
||||
if (master->injection_seq_rt != master->injection_seq_fsm ||
|
||||
master->fsm_datagram.state == EC_DATAGRAM_SENT ||
|
||||
master->fsm_datagram.state == EC_DATAGRAM_QUEUED)
|
||||
|
|
@ -902,6 +871,8 @@ schedule:
|
|||
}
|
||||
|
||||
master->thread_id = 0;
|
||||
if (master->debug_level)
|
||||
EC_DBG("Master OP thread exiting...\n");
|
||||
complete_and_exit(&master->thread_exit, 0);
|
||||
}
|
||||
|
||||
|
|
@ -1455,8 +1426,6 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */)
|
|||
{
|
||||
uint32_t domain_offset;
|
||||
ec_domain_t *domain;
|
||||
ec_fsm_slave_t fsm_slave;
|
||||
ec_slave_t *slave;
|
||||
|
||||
// allocate all domains
|
||||
domain_offset = 0;
|
||||
|
|
@ -1468,30 +1437,37 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */)
|
|||
domain_offset += domain->data_size;
|
||||
}
|
||||
|
||||
ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram);
|
||||
// request slave configuration
|
||||
down(&master->config_sem);
|
||||
master->allow_config = 1; // request the current configuration
|
||||
master->config_state = EC_REQUEST_IN_PROGRESS;
|
||||
up(&master->config_sem);
|
||||
|
||||
// configure all slaves
|
||||
list_for_each_entry(slave, &master->slaves, list) {
|
||||
ec_fsm_slave_start_conf(&fsm_slave, slave);
|
||||
while (ec_fsm_slave_exec(&fsm_slave)) {
|
||||
ec_master_queue_datagram(master, &master->fsm_datagram);
|
||||
ec_master_sync_io(master);
|
||||
}
|
||||
|
||||
if (!ec_fsm_slave_success(&fsm_slave)) {
|
||||
ec_fsm_slave_clear(&fsm_slave);
|
||||
EC_ERR("Failed to configure slave %i!\n", slave->ring_position);
|
||||
return -1;
|
||||
}
|
||||
// wait for configuration to complete
|
||||
if (wait_event_interruptible(master->config_queue,
|
||||
master->config_state != EC_REQUEST_IN_PROGRESS)) {
|
||||
EC_INFO("Waiting for configuration interrupted by signal.\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
ec_fsm_slave_clear(&fsm_slave);
|
||||
if (master->config_state != EC_REQUEST_COMPLETE) {
|
||||
EC_ERR("Failed to configure slaves.\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// restart EoE process and master thread with new locking
|
||||
ec_master_eoe_stop(master);
|
||||
ec_master_thread_stop(master);
|
||||
|
||||
ec_master_prepare(master); // prepare asynchronous IO
|
||||
|
||||
if (master->debug_level)
|
||||
EC_DBG("FSM datagram is %x.\n", (unsigned int) &master->fsm_datagram);
|
||||
|
||||
if (ec_master_thread_start(master)) {
|
||||
master->injection_seq_fsm = 0;
|
||||
master->injection_seq_rt = 0;
|
||||
|
||||
if (ec_master_thread_start(master, ec_master_operation_thread)) {
|
||||
EC_ERR("Failed to start master thread!\n");
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -1501,38 +1477,6 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */)
|
|||
|
||||
/*****************************************************************************/
|
||||
|
||||
/**
|
||||
Sends queued datagrams and waits for their reception.
|
||||
*/
|
||||
|
||||
void ec_master_sync_io(ec_master_t *master /**< EtherCAT master */)
|
||||
{
|
||||
ec_datagram_t *datagram;
|
||||
unsigned int datagrams_sent;
|
||||
|
||||
// send all datagrams
|
||||
ecrt_master_send(master);
|
||||
|
||||
while (1) { // active waiting
|
||||
schedule(); // schedule other processes while waiting.
|
||||
ecrt_master_receive(master); // receive and dequeue datagrams
|
||||
|
||||
// count number of datagrams still waiting for response
|
||||
datagrams_sent = 0;
|
||||
list_for_each_entry(datagram, &master->datagram_queue, queue) {
|
||||
// there may be another process that queued commands
|
||||
// in the meantime.
|
||||
if (datagram->state == EC_DATAGRAM_QUEUED) continue;
|
||||
datagrams_sent++;
|
||||
}
|
||||
|
||||
// abort loop if there are no more datagrams marked as sent.
|
||||
if (!datagrams_sent) break;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/**
|
||||
Asynchronous sending of datagrams.
|
||||
\ingroup RealtimeInterface
|
||||
|
|
|
|||
|
|
@ -112,6 +112,20 @@ struct ec_master
|
|||
|
||||
struct list_head slaves; /**< list of slaves on the bus */
|
||||
unsigned int slave_count; /**< number of slaves on the bus */
|
||||
|
||||
ec_request_state_t scan_state; /**< current scanning state */
|
||||
unsigned int allow_scan; /**< non-zero, if slave scanning is allowed */
|
||||
struct semaphore scan_sem; /**< semaphore protecting the scan_state
|
||||
variable and the allow_scan flag */
|
||||
wait_queue_head_t scan_queue; /**< queue for processes that wait for
|
||||
slave scanning */
|
||||
|
||||
ec_request_state_t config_state; /**< state of slave configuration */
|
||||
unsigned int allow_config; /**< non-zero, if slave scanning is allowed */
|
||||
struct semaphore config_sem; /**< semaphore protecting the config_state
|
||||
variable and the allow_config flag */
|
||||
wait_queue_head_t config_queue; /**< queue for processes that wait for
|
||||
slave configuration */
|
||||
|
||||
struct list_head datagram_queue; /**< datagram queue */
|
||||
uint8_t datagram_index; /**< current datagram index */
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ int ec_slave_init(ec_slave_t *slave, /**< EtherCAT slave */
|
|||
|
||||
slave->master = master;
|
||||
|
||||
slave->requested_state = EC_SLAVE_STATE_UNKNOWN;
|
||||
slave->requested_state = EC_SLAVE_STATE_PREOP;
|
||||
slave->current_state = EC_SLAVE_STATE_UNKNOWN;
|
||||
slave->self_configured = 0;
|
||||
slave->error_flag = 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue