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

Slave scanning and configuration on all links.

This commit is contained in:
Florian Pose 2012-04-06 23:35:05 +02:00
parent eea71387d6
commit 4c3c5c2778
17 changed files with 239 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
master/cdev.c
View File

@ -294,6 +294,7 @@ int ec_cdev_ioctl_slave(
return -EINVAL;
}
data.device_index = slave->device_index;
data.vendor_id = slave->sii.vendor_id;
data.product_code = slave->sii.product_code;
data.revision_number = slave->sii.revision_number;

7
master/datagram_pair.c
View File

@ -52,17 +52,18 @@ int ec_datagram_pair_init(
const unsigned int used[] /**< input/output use count. */
)
{
unsigned int dev_idx;
ec_device_index_t dev_idx;
int ret;
INIT_LIST_HEAD(&pair->list);
pair->domain = domain;
for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
ec_datagram_init(&pair->datagrams[dev_idx]);
snprintf(pair->datagrams[dev_idx].name,
EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index,
logical_offset, dev_idx ? "backup" : "main");
logical_offset, ec_device_names[dev_idx]);
pair->datagrams[dev_idx].device_index = dev_idx;
}
pair->expected_working_counter = 0U;

6
master/domain.c
View File

@ -585,7 +585,7 @@ void ecrt_domain_process(ec_domain_t *domain)
void ecrt_domain_queue(ec_domain_t *domain)
{
ec_datagram_pair_t *datagram_pair;
unsigned int dev_idx;
ec_device_index_t dev_idx;
list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) {
@ -599,9 +599,9 @@ void ecrt_domain_queue(ec_domain_t *domain)
datagram_pair->datagrams[EC_DEVICE_MAIN].data,
datagram_pair->datagrams[EC_DEVICE_MAIN].data_size);
for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
ec_master_queue_datagram(domain->master,
&datagram_pair->datagrams[dev_idx], dev_idx);
&datagram_pair->datagrams[dev_idx]);
}
}
}

194
master/fsm_master.c
View File

@ -67,6 +67,7 @@ void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);
void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);
void ec_fsm_master_state_reg_request(ec_fsm_master_t *);
void ec_fsm_master_enter_clear_addresses(ec_fsm_master_t *);
void ec_fsm_master_enter_write_system_times(ec_fsm_master_t *);
/*****************************************************************************/
@ -79,14 +80,19 @@ void ec_fsm_master_init(
ec_datagram_t *datagram /**< Datagram object to use. */
)
{
ec_device_index_t dev_idx;
fsm->master = master;
fsm->datagram = datagram;
fsm->state = ec_fsm_master_state_start;
fsm->idle = 0;
fsm->link_state = 0;
fsm->slaves_responding = 0;
fsm->dev_idx = EC_DEVICE_MAIN;
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
fsm->link_state[dev_idx] = 0;
fsm->slaves_responding[dev_idx] = 0;
fsm->slave_states[dev_idx] = EC_SLAVE_STATE_UNKNOWN;
}
fsm->rescan_required = 0;
fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
@ -159,6 +165,7 @@ void ec_fsm_master_restart(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
fsm->dev_idx = EC_DEVICE_MAIN;
fsm->state = ec_fsm_master_state_start;
fsm->state(fsm); // execute immediately
}
@ -178,6 +185,7 @@ void ec_fsm_master_state_start(
fsm->idle = 1;
ec_datagram_brd(fsm->datagram, 0x0130, 2);
ec_datagram_zero(fsm->datagram);
fsm->datagram->device_index = fsm->dev_idx;
fsm->state = ec_fsm_master_state_broadcast;
}
@ -196,48 +204,58 @@ void ec_fsm_master_state_broadcast(
ec_slave_t *slave;
ec_master_t *master = fsm->master;
if (datagram->state == EC_DATAGRAM_TIMED_OUT)
return; // always retry
// bus topology change?
if (datagram->working_counter != fsm->slaves_responding) {
if (datagram->working_counter != fsm->slaves_responding[fsm->dev_idx]) {
fsm->rescan_required = 1;
fsm->slaves_responding = datagram->working_counter;
EC_MASTER_INFO(master, "%u slave(s) responding.\n",
fsm->slaves_responding);
fsm->slaves_responding[fsm->dev_idx] = datagram->working_counter;
EC_MASTER_INFO(master, "%u slave(s) responding on %s device.\n",
fsm->slaves_responding[fsm->dev_idx],
ec_device_names[fsm->dev_idx]);
}
if (fsm->link_state && !master->devices[EC_DEVICE_MAIN].link_state) {
// link went down
if (fsm->link_state[fsm->dev_idx] &&
!master->devices[fsm->dev_idx].link_state) {
ec_device_index_t dev_idx;
EC_MASTER_DBG(master, 1, "Master state machine detected "
"link down. Clearing slave list.\n");
"link down on %s device. Clearing slave list.\n",
ec_device_names[fsm->dev_idx]);
#ifdef EC_EOE
ec_master_eoe_stop(master);
ec_master_clear_eoe_handlers(master);
#endif
ec_master_clear_slaves(master);
fsm->slave_states = 0x00;
fsm->slaves_responding = 0; /* reset to trigger rescan on next link
up. */
}
fsm->link_state = master->devices[EC_DEVICE_MAIN].link_state;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
ec_fsm_master_restart(fsm);
return;
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
fsm->slave_states[dev_idx] = 0x00;
fsm->slaves_responding[dev_idx] = 0; /* Reset to trigger rescan on
next link up. */
}
}
fsm->link_state[fsm->dev_idx] = master->devices[fsm->dev_idx].link_state;
if (fsm->slaves_responding) {
if (datagram->state == EC_DATAGRAM_RECEIVED &&
fsm->slaves_responding[fsm->dev_idx]) {
uint8_t states = EC_READ_U8(datagram->data);
if (states != fsm->slave_states) { // slave states changed?
if (states != fsm->slave_states[fsm->dev_idx]) {
// slave states changed
char state_str[EC_STATE_STRING_SIZE];
fsm->slave_states = states;
ec_state_string(fsm->slave_states, state_str, 1);
EC_MASTER_INFO(master, "Slave states: %s.\n", state_str);
fsm->slave_states[fsm->dev_idx] = states;
ec_state_string(states, state_str, 1);
EC_MASTER_INFO(master, "Slave states on %s device: %s.\n",
ec_device_names[fsm->dev_idx], state_str);
}
} else {
fsm->slave_states = 0x00;
fsm->slave_states[fsm->dev_idx] = 0x00;
}
fsm->dev_idx++;
if (fsm->dev_idx < EC_NUM_DEVICES) {
// check number of responding slaves on next device
fsm->state = ec_fsm_master_state_start;
fsm->state(fsm); // execute immediately
return;
}
if (fsm->rescan_required) {
@ -245,6 +263,9 @@ void ec_fsm_master_state_broadcast(
if (!master->allow_scan) {
up(&master->scan_sem);
} else {
unsigned int count = 0, next_dev_slave, ring_position;
ec_device_index_t dev_idx;
master->scan_busy = 1;
up(&master->scan_sem);
@ -259,9 +280,12 @@ void ec_fsm_master_state_broadcast(
#endif
ec_master_clear_slaves(master);
master->slave_count = fsm->slaves_responding;
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES;
dev_idx++) {
count += fsm->slaves_responding[dev_idx];
}
if (!master->slave_count) {
if (!count) {
// no slaves present -> finish state machine.
master->scan_busy = 0;
wake_up_interruptible(&master->scan_queue);
@ -269,12 +293,11 @@ void ec_fsm_master_state_broadcast(
return;
}
size = sizeof(ec_slave_t) * master->slave_count;
size = sizeof(ec_slave_t) * count;
if (!(master->slaves =
(ec_slave_t *) kmalloc(size, GFP_KERNEL))) {
EC_MASTER_ERR(master, "Failed to allocate %u bytes"
" of slave memory!\n", size);
master->slave_count = 0; // TODO avoid retrying scan!
master->scan_busy = 0;
wake_up_interruptible(&master->scan_queue);
ec_fsm_master_restart(fsm);
@ -282,21 +305,35 @@ void ec_fsm_master_state_broadcast(
}
// init slaves
for (i = 0; i < master->slave_count; i++) {
dev_idx = EC_DEVICE_MAIN;
next_dev_slave = fsm->slaves_responding[dev_idx];
ring_position = 0;
for (i = 0; i < count; i++, ring_position++) {
slave = master->slaves + i;
ec_slave_init(slave, master, i, i + 1);
while (i >= next_dev_slave) {
dev_idx++;
next_dev_slave += fsm->slaves_responding[dev_idx];
ring_position = 0;
}
ec_slave_init(slave, master, dev_idx, ring_position, i + 1);
// do not force reconfiguration in operation phase to avoid
// unnecesssary process data interruptions
if (master->phase != EC_OPERATION)
if (master->phase != EC_OPERATION) {
slave->force_config = 1;
}
}
master->slave_count = count;
// broadcast clear all station addresses
ec_datagram_bwr(datagram, 0x0010, 2);
EC_WRITE_U16(datagram->data, 0x0000);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_clear_addresses;
/* start with first device with slaves responding; at least one
* has responding slaves, otherwise count would be zero. */
fsm->dev_idx = EC_DEVICE_MAIN;
while (!fsm->slaves_responding[fsm->dev_idx]) {
fsm->dev_idx++;
}
ec_fsm_master_enter_clear_addresses(fsm);
return;
}
}
@ -318,6 +355,7 @@ void ec_fsm_master_state_broadcast(
ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,
0x0130, 2);
ec_datagram_zero(datagram);
fsm->datagram->device_index = fsm->slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_read_state;
}
@ -411,6 +449,7 @@ int ec_fsm_master_action_process_register(
request->offset, request->length);
memcpy(fsm->datagram->data, request->data, request->length);
}
fsm->datagram->device_index = request->slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_reg_request;
return 1;
@ -518,6 +557,7 @@ void ec_fsm_master_action_idle(
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
fsm->datagram->device_index = fsm->slave->device_index;
return;
}
@ -550,6 +590,7 @@ void ec_fsm_master_action_next_slave_state(
ec_datagram_fprd(fsm->datagram,
fsm->slave->station_address, 0x0130, 2);
ec_datagram_zero(fsm->datagram);
fsm->datagram->device_index = fsm->slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_read_state;
return;
@ -607,6 +648,7 @@ void ec_fsm_master_action_configure(
fsm->state = ec_fsm_master_state_configure_slave;
ec_fsm_slave_config_start(&fsm->fsm_slave_config, slave);
fsm->state(fsm); // execute immediately
fsm->datagram->device_index = fsm->slave->device_index;
return;
}
@ -680,8 +722,9 @@ void ec_fsm_master_state_acknowledge(
{
ec_slave_t *slave = fsm->slave;
if (ec_fsm_change_exec(&fsm->fsm_change))
if (ec_fsm_change_exec(&fsm->fsm_change)) {
return;
}
if (!ec_fsm_change_success(&fsm->fsm_change)) {
fsm->slave->error_flag = 1;
@ -693,6 +736,22 @@ void ec_fsm_master_state_acknowledge(
/*****************************************************************************/
/** Start clearing slave addresses.
*/
void ec_fsm_master_enter_clear_addresses(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
// broadcast clear all station addresses
ec_datagram_bwr(fsm->datagram, 0x0010, 2);
EC_WRITE_U16(fsm->datagram->data, 0x0000);
fsm->datagram->device_index = fsm->dev_idx;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_clear_addresses;
}
/*****************************************************************************/
/** Master state: CLEAR ADDRESSES.
*/
void ec_fsm_master_state_clear_addresses(
@ -702,12 +761,14 @@ void ec_fsm_master_state_clear_addresses(
ec_master_t *master = fsm->master;
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_MASTER_ERR(master, "Failed to receive address"
" clearing datagram: ");
" clearing datagram on %s link: ",
ec_device_names[fsm->dev_idx]);
ec_datagram_print_state(datagram);
master->scan_busy = 0;
wake_up_interruptible(&master->scan_queue);
@ -715,17 +776,20 @@ void ec_fsm_master_state_clear_addresses(
return;
}
if (datagram->working_counter != master->slave_count) {
EC_MASTER_WARN(master, "Failed to clear all station addresses:"
if (datagram->working_counter != fsm->slaves_responding[fsm->dev_idx]) {
EC_MASTER_WARN(master, "Failed to clear station addresses on %s link:"
" Cleared %u of %u",
datagram->working_counter, master->slave_count);
ec_device_names[fsm->dev_idx], datagram->working_counter,
fsm->slaves_responding[fsm->dev_idx]);
}
EC_MASTER_DBG(master, 1, "Sending broadcast-write"
" to measure transmission delays.\n");
" to measure transmission delays on %s link.\n",
ec_device_names[fsm->dev_idx]);
ec_datagram_bwr(datagram, 0x0900, 1);
ec_datagram_zero(datagram);
fsm->datagram->device_index = fsm->dev_idx;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_dc_measure_delays;
}
@ -745,7 +809,8 @@ void ec_fsm_master_state_dc_measure_delays(
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_MASTER_ERR(master, "Failed to receive delay measuring datagram: ");
EC_MASTER_ERR(master, "Failed to receive delay measuring datagram"
" on %s link: ", ec_device_names[fsm->dev_idx]);
ec_datagram_print_state(datagram);
master->scan_busy = 0;
wake_up_interruptible(&master->scan_queue);
@ -753,16 +818,30 @@ void ec_fsm_master_state_dc_measure_delays(
return;
}
EC_MASTER_DBG(master, 1, "%u slaves responded to delay measuring.\n",
datagram->working_counter);
EC_MASTER_DBG(master, 1, "%u slaves responded to delay measuring"
" on %s link.\n",
datagram->working_counter, ec_device_names[fsm->dev_idx]);
do {
fsm->dev_idx++;
} while (fsm->dev_idx < EC_NUM_DEVICES &&
!fsm->slaves_responding[fsm->dev_idx]);
if (fsm->dev_idx < EC_NUM_DEVICES) {
ec_fsm_master_enter_clear_addresses(fsm);
return;
}
EC_MASTER_INFO(master, "Scanning bus.\n");
// begin scanning of slaves
fsm->slave = master->slaves;
EC_MASTER_DBG(master, 1, "Scanning slave %u on %s link.\n",
fsm->slave->ring_position,
ec_device_names[fsm->slave->device_index]);
fsm->state = ec_fsm_master_state_scan_slave;
ec_fsm_slave_scan_start(&fsm->fsm_slave_scan, fsm->slave);
ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan); // execute immediately
fsm->datagram->device_index = fsm->slave->device_index;
}
/*****************************************************************************/
@ -779,8 +858,10 @@ void ec_fsm_master_state_scan_slave(
#ifdef EC_EOE
ec_slave_t *slave = fsm->slave;
#endif
if (ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan))
if (ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan)) {
return;
}
#ifdef EC_EOE
if (slave->sii.mailbox_protocols & EC_MBOX_EOE) {
@ -800,8 +881,12 @@ void ec_fsm_master_state_scan_slave(
// another slave to fetch?
fsm->slave++;
if (fsm->slave < master->slaves + master->slave_count) {
EC_MASTER_DBG(master, 1, "Scanning slave %u on %s link.\n",
fsm->slave->ring_position,
ec_device_names[fsm->slave->device_index]);
ec_fsm_slave_scan_start(&fsm->fsm_slave_scan, fsm->slave);
ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan); // execute immediately
fsm->datagram->device_index = fsm->slave->device_index;
return;
}
@ -843,8 +928,9 @@ void ec_fsm_master_state_configure_slave(
{
ec_master_t *master = fsm->master;
if (ec_fsm_slave_config_exec(&fsm->fsm_slave_config))
if (ec_fsm_slave_config_exec(&fsm->fsm_slave_config)) {
return;
}
fsm->slave->force_config = 0;
@ -886,6 +972,7 @@ void ec_fsm_master_enter_write_system_times(
// and time offset (0x0920, 64 bit)
ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,
0x0910, 24);
fsm->datagram->device_index = fsm->slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_dc_read_offset;
return;
@ -1031,6 +1118,7 @@ void ec_fsm_master_state_dc_read_offset(
ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);
EC_WRITE_U64(datagram->data, new_offset);
EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);
fsm->datagram->device_index = slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_dc_write_offset;
}
@ -1135,7 +1223,9 @@ 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)) return;
if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
return;
}
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
ec_fsm_master_restart(fsm);

13
master/fsm_master.h
View File

@ -115,12 +115,19 @@ struct ec_fsm_master {
unsigned int retries; /**< retries on datagram timeout. */
void (*state)(ec_fsm_master_t *); /**< master state function */
ec_device_index_t dev_idx; /**< Current device index (for scanning etc.).
*/
int idle; /**< state machine is in idle phase */
unsigned long scan_jiffies; /**< beginning of slave scanning */
uint8_t link_state; /**< Last main device link state. */
unsigned int slaves_responding; /**< number of responding slaves */
uint8_t link_state[EC_NUM_DEVICES]; /**< Last link state for every device.
*/
unsigned int slaves_responding[EC_NUM_DEVICES]; /**< Number of responding
slaves for every device.
*/
unsigned int rescan_required; /**< A bus rescan is required. */
ec_slave_state_t slave_states; /**< states of responding slaves */
ec_slave_state_t slave_states[EC_NUM_DEVICES]; /**< AL states of
responding slaves for
every device. */
ec_slave_t *slave; /**< current slave */
ec_sii_write_request_t *sii_request; /**< SII write request */
off_t sii_index; /**< index to SII write request data */

18
master/fsm_slave.c
View File

@ -206,8 +206,7 @@ int ec_fsm_slave_action_process_sdo(
fsm->state = ec_fsm_slave_state_sdo_request;
ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return 1;
}
return 0;
@ -225,8 +224,7 @@ void ec_fsm_slave_state_sdo_request(
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,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return;
}
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
@ -281,8 +279,7 @@ int ec_fsm_slave_action_process_foe(
fsm->state = ec_fsm_slave_state_foe_request;
ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
ec_fsm_foe_exec(&fsm->fsm_foe);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return 1;
}
return 0;
@ -300,8 +297,7 @@ void ec_fsm_slave_state_foe_request(
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,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return;
}
@ -369,8 +365,7 @@ int ec_fsm_slave_action_process_soe(
fsm->state = ec_fsm_slave_state_soe_request;
ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req);
ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return 1;
}
return 0;
@ -388,8 +383,7 @@ void ec_fsm_slave_state_soe_request(
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,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return;
}

14
master/fsm_slave_config.c
View File

@ -744,7 +744,7 @@ void ec_fsm_slave_config_enter_soe_conf_preop(
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_external_datagram(slave->master,
fsm_soe->datagram, EC_DEVICE_MAIN);
fsm_soe->datagram);
return;
}
}
@ -765,8 +765,7 @@ void ec_fsm_slave_config_state_soe_conf_preop(
ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
if (ec_fsm_soe_exec(fsm_soe)) {
ec_master_queue_external_datagram(slave->master, fsm_soe->datagram,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
return;
}
@ -792,7 +791,7 @@ void ec_fsm_slave_config_state_soe_conf_preop(
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_external_datagram(slave->master,
fsm_soe->datagram, EC_DEVICE_MAIN);
fsm_soe->datagram);
return;
}
}
@ -1474,7 +1473,7 @@ void ec_fsm_slave_config_enter_soe_conf_safeop(
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_external_datagram(slave->master,
fsm_soe->datagram, EC_DEVICE_MAIN);
fsm_soe->datagram);
return;
}
}
@ -1495,8 +1494,7 @@ void ec_fsm_slave_config_state_soe_conf_safeop(
ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
if (ec_fsm_soe_exec(fsm_soe)) {
ec_master_queue_external_datagram(slave->master, fsm_soe->datagram,
EC_DEVICE_MAIN);
ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
return;
}
@ -1522,7 +1520,7 @@ void ec_fsm_slave_config_state_soe_conf_safeop(
ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
ec_fsm_soe_exec(fsm_soe); // execute immediately
ec_master_queue_external_datagram(slave->master,
fsm_soe->datagram, EC_DEVICE_MAIN);
fsm_soe->datagram);
return;
}
}

2
master/globals.h
View File

@ -204,6 +204,8 @@ typedef enum {
EC_NUM_DEVICES /**< Number of devices. */
} ec_device_index_t;
extern const char *ec_device_names[EC_NUM_DEVICES];
/*****************************************************************************/
/** Convenience macro for printing EtherCAT-specific information to syslog.

3
master/ioctl.h
View File

@ -56,7 +56,7 @@
*
* Increment this when changing the ioctl interface!
*/
#define EC_IOCTL_VERSION_MAGIC 16
#define EC_IOCTL_VERSION_MAGIC 17
// Command-line tool
#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t)
@ -195,6 +195,7 @@ typedef struct {
uint16_t position;
// outputs
unsigned int device_index;
uint32_t vendor_id;
uint32_t product_code;
uint32_t revision_number;

70
master/master.c
View File

@ -554,6 +554,7 @@ int ec_master_enter_idle_phase(
)
{
int ret;
ec_device_index_t dev_idx;
EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n");
@ -564,7 +565,9 @@ int ec_master_enter_idle_phase(
master->phase = EC_IDLE;
// reset number of responding slaves to trigger scanning
master->fsm.slaves_responding = 0;
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
master->fsm.slaves_responding[dev_idx] = 0;
}
ret = ec_master_thread_start(master, ec_master_idle_thread,
"EtherCAT-IDLE");
@ -727,8 +730,7 @@ void ec_master_inject_external_datagrams(
datagram->cycles_sent = 0;
#endif
datagram->jiffies_sent = 0;
ec_master_queue_datagram(master, datagram,
datagram->device_index);
ec_master_queue_datagram(master, datagram);
} else {
if (datagram->data_size > master->max_queue_size) {
list_del_init(&datagram->queue);
@ -800,8 +802,7 @@ void ec_master_set_send_interval(
*/
void ec_master_queue_external_datagram(
ec_master_t *master, /**< EtherCAT master */
ec_datagram_t *datagram, /**< datagram */
ec_device_index_t device_index /**< Device index. */
ec_datagram_t *datagram /**< datagram */
)
{
ec_datagram_t *queued_datagram;
@ -824,7 +825,6 @@ void ec_master_queue_external_datagram(
list_add_tail(&datagram->queue, &master->external_datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
datagram->device_index = device_index;
#ifdef EC_HAVE_CYCLES
datagram->cycles_sent = get_cycles();
#endif
@ -840,8 +840,7 @@ void ec_master_queue_external_datagram(
*/
void ec_master_queue_datagram(
ec_master_t *master, /**< EtherCAT master */
ec_datagram_t *datagram, /**< datagram */
ec_device_index_t device_index /**< Device index. */
ec_datagram_t *datagram /**< datagram */
)
{
ec_datagram_t *queued_datagram;
@ -881,7 +880,6 @@ void ec_master_queue_datagram(
list_add_tail(&datagram->queue, &master->datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
datagram->device_index = device_index;
}
/*****************************************************************************/
@ -1388,8 +1386,7 @@ static int ec_master_idle_thread(void *priv_data)
// queue and send
down(&master->io_sem);
if (fsm_exec) {
ec_master_queue_datagram(master, &master->fsm_datagram,
EC_DEVICE_MAIN);
ec_master_queue_datagram(master, &master->fsm_datagram);
}
ecrt_master_send(master);
#ifdef EC_USE_HRTIMER
@ -2231,42 +2228,41 @@ void ecrt_master_deactivate(ec_master_t *master)
void ecrt_master_send(ec_master_t *master)
{
ec_datagram_t *datagram, *n;
unsigned int i;
ec_device_index_t dev_idx;
if (master->injection_seq_rt != master->injection_seq_fsm) {
// inject datagrams produced by master & slave FSMs
ec_master_queue_datagram(master, &master->fsm_datagram,
EC_DEVICE_MAIN);
// inject datagrams produced by master and slave FSMs
ec_master_queue_datagram(master, &master->fsm_datagram);
master->injection_seq_rt = master->injection_seq_fsm;
}
ec_master_inject_external_datagrams(master);
for (i = 0; i < EC_NUM_DEVICES; i++) {
if (unlikely(!master->devices[i].link_state)) {
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
if (unlikely(!master->devices[dev_idx].link_state)) {
// link is down, no datagram can be sent
list_for_each_entry_safe(datagram, n,
&master->datagram_queue, queue) {
if (datagram->device_index == i) {
if (datagram->device_index == dev_idx) {
datagram->state = EC_DATAGRAM_ERROR;
list_del_init(&datagram->queue);
}
}
if (!master->devices[i].dev) {
if (!master->devices[dev_idx].dev) {
continue;
}
// query link state
ec_device_poll(&master->devices[i]);
ec_device_poll(&master->devices[dev_idx]);
// clear frame statistics
ec_device_clear_stats(&master->devices[i]);
ec_device_clear_stats(&master->devices[dev_idx]);
continue;
}
// send frames
ec_master_send_datagrams(master, i);
ec_master_send_datagrams(master, dev_idx);
}
}
@ -2327,7 +2323,7 @@ void ecrt_master_send_ext(ec_master_t *master)
list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue,
queue) {
list_del(&datagram->queue);
ec_master_queue_datagram(master, datagram, EC_DEVICE_MAIN);
ec_master_queue_datagram(master, datagram);
}
ecrt_master_send(master);
@ -2491,9 +2487,22 @@ void ecrt_master_callbacks(ec_master_t *master,
void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
{
state->slaves_responding = master->fsm.slaves_responding;
state->al_states = master->fsm.slave_states;
state->link_up = master->devices[EC_DEVICE_MAIN].link_state;
ec_device_index_t dev_idx;
state->slaves_responding = 0U;
state->al_states = 0;
state->link_up = 0U;
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
/* Announce sum of responding slaves on all links. */
state->slaves_responding += master->fsm.slaves_responding[dev_idx];
/* Binary-or slave states of all links. */
state->al_states |= master->fsm.slave_states[dev_idx];
/* Signal link up if at least one device has link. */
state->link_up |= master->devices[dev_idx].link_state;
}
}
/*****************************************************************************/
@ -2513,8 +2522,7 @@ void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
void ecrt_master_sync_reference_clock(ec_master_t *master)
{
EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time);
ec_master_queue_datagram(master, &master->ref_sync_datagram,
EC_DEVICE_MAIN);
ec_master_queue_datagram(master, &master->ref_sync_datagram);
}
/*****************************************************************************/
@ -2522,8 +2530,7 @@ void ecrt_master_sync_reference_clock(ec_master_t *master)
void ecrt_master_sync_slave_clocks(ec_master_t *master)
{
ec_datagram_zero(&master->sync_datagram);
ec_master_queue_datagram(master, &master->sync_datagram,
EC_DEVICE_MAIN);
ec_master_queue_datagram(master, &master->sync_datagram);
}
/*****************************************************************************/
@ -2531,8 +2538,7 @@ void ecrt_master_sync_slave_clocks(ec_master_t *master)
void ecrt_master_sync_monitor_queue(ec_master_t *master)
{
ec_datagram_zero(&master->sync_mon_datagram);
ec_master_queue_datagram(master, &master->sync_mon_datagram,
EC_DEVICE_MAIN);
ec_master_queue_datagram(master, &master->sync_mon_datagram);
}
/*****************************************************************************/

6
master/master.h
View File

@ -297,11 +297,9 @@ void ec_master_eoe_stop(ec_master_t *);
// datagram IO
void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t);
void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *,
ec_device_index_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 *,
ec_device_index_t);
void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
void ec_master_inject_external_datagrams(ec_master_t *);
// misc.

2
master/module.c
View File

@ -453,7 +453,7 @@ size_t ec_state_string(uint8_t states, /**< slave states */
/** Device names.
*/
static const char *ec_device_names[EC_NUM_DEVICES] = {
const char *ec_device_names[EC_NUM_DEVICES] = {
"main",
"backup"
};

2
master/slave.c
View File

@ -62,6 +62,7 @@ char *ec_slave_sii_string(ec_slave_t *, unsigned int);
void ec_slave_init(
ec_slave_t *slave, /**< EtherCAT slave */
ec_master_t *master, /**< EtherCAT master */
ec_device_index_t dev_idx, /**< Device index. */
uint16_t ring_position, /**< ring position */
uint16_t station_address /**< station address to configure */
)
@ -70,6 +71,7 @@ void ec_slave_init(
int ret;
slave->master = master;
slave->device_index = dev_idx;
slave->ring_position = ring_position;
slave->station_address = station_address;
slave->effective_alias = 0x0000;

5
master/slave.h
View File

@ -174,6 +174,8 @@ typedef struct {
struct ec_slave
{
ec_master_t *master; /**< Master owning the slave. */
ec_device_index_t device_index; /**< Index of device the slave responds
on. */
// addresses
uint16_t ring_position; /**< Ring position. */
@ -238,7 +240,8 @@ struct ec_slave
/*****************************************************************************/
// slave construction/destruction
void ec_slave_init(ec_slave_t *, ec_master_t *, uint16_t, uint16_t);
void ec_slave_init(ec_slave_t *, ec_master_t *, ec_device_index_t,
uint16_t, uint16_t);
void ec_slave_clear(ec_slave_t *);
void ec_slave_clear_sync_managers(ec_slave_t *);

6
master/voe_handler.c
View File

@ -190,9 +190,9 @@ ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
{
if (voe->config->slave) { // FIXME locking?
voe->state(voe);
if (voe->request_state == EC_INT_REQUEST_BUSY)
ec_master_queue_datagram(voe->config->master, &voe->datagram,
EC_DEVICE_MAIN);
if (voe->request_state == EC_INT_REQUEST_BUSY) {
ec_master_queue_datagram(voe->config->master, &voe->datagram);
}
} else {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;

15
tool/CommandSlaves.cpp
View File

@ -144,7 +144,7 @@ void CommandSlaves::listSlaves(
)
{
ec_ioctl_master_t master;
unsigned int i;
unsigned int i, lastDevice;
ec_ioctl_slave_t slave;
uint16_t lastAlias, aliasIndex;
Info info;
@ -184,6 +184,7 @@ void CommandSlaves::listSlaves(
info.state = alStateString(slave.al_state);
info.flag = (slave.error_flag ? 'E' : '+');
info.device = slave.device_index;
if (strlen(slave.name)) {
info.name = slave.name;
@ -215,7 +216,12 @@ void CommandSlaves::listSlaves(
cout << "Master" << dec << m.getIndex() << endl;
}
lastDevice = EC_DEVICE_MAIN;
for (iter = infoList.begin(); iter != infoList.end(); iter++) {
if (iter->device != lastDevice) {
lastDevice = iter->device;
cout << "xxx LINK FAILURE xxx" << endl;
}
cout << indent << setfill(' ') << right
<< setw(maxPosWidth) << iter->pos << " "
<< setw(maxAliasWidth) << iter->alias
@ -245,6 +251,7 @@ void CommandSlaves::showSlaves(
cout << "Alias: " << si->alias << endl;
cout
<< "Device: " << (si->device_index ? "Backup" : "Main") << endl
<< "State: " << alStateString(si->al_state) << endl
<< "Flag: " << (si->error_flag ? 'E' : '+') << endl
<< "Identity:" << endl
@ -332,7 +339,8 @@ void CommandSlaves::showSlaves(
}
cout << " " << setw(10);
if (!si->ports[i].link.loop_closed) {
cout << si->ports[i].receive_time - si->ports[0].receive_time;
cout << si->ports[i].receive_time -
si->ports[0].receive_time;
} else {
cout << "-";
}
@ -406,7 +414,8 @@ void CommandSlaves::showSlaves(
<< " Enable SDO: "
<< (si->coe_details.enable_sdo ? "yes" : "no") << endl
<< " Enable SDO Info: "
<< (si->coe_details.enable_sdo_info ? "yes" : "no") << endl
<< (si->coe_details.enable_sdo_info ? "yes" : "no")
<< endl
<< " Enable PDO Assign: "
<< (si->coe_details.enable_pdo_assign
? "yes" : "no") << endl

1
tool/CommandSlaves.h
View File

@ -51,6 +51,7 @@ class CommandSlaves:
string state;
string flag;
string name;
unsigned int device;
};
void listSlaves(MasterDevice &, const SlaveList &, bool);