Datagram device_index member.
--- a/master/datagram.c Thu Jan 12 12:14:33 2012 +0100
+++ b/master/datagram.c Thu Jan 12 13:55:15 2012 +0100
@@ -88,6 +88,7 @@
void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */)
{
INIT_LIST_HEAD(&datagram->queue); // mark as unqueued
+ datagram->device_index = EC_DEVICE_MAIN;
datagram->type = EC_DATAGRAM_NONE;
memset(datagram->address, 0x00, EC_ADDR_LEN);
datagram->data = NULL;
--- a/master/datagram.h Thu Jan 12 12:14:33 2012 +0100
+++ b/master/datagram.h Thu Jan 12 13:55:15 2012 +0100
@@ -88,6 +88,8 @@
struct list_head list; /**< Needed by domain datagram lists. */
struct list_head queue; /**< Master datagram queue item. */
struct list_head sent; /**< Master list item for sent datagrams. */
+ ec_device_index_t device_index; /**< Device via which the datagram shall
+ be / was sent. */
ec_datagram_type_t type; /**< Datagram type (APRD, BWR, etc.). */
uint8_t address[EC_ADDR_LEN]; /**< Recipient address. */
uint8_t *data; /**< Datagram payload. */
--- a/master/domain.c Thu Jan 12 12:14:33 2012 +0100
+++ b/master/domain.c Thu Jan 12 13:55:15 2012 +0100
@@ -438,7 +438,7 @@
ec_datagram_t *datagram;
list_for_each_entry(datagram, &domain->datagrams, list) {
- ec_master_queue_datagram(domain->master, datagram);
+ ec_master_queue_datagram(domain->master, datagram, EC_DEVICE_MAIN);
}
}
--- a/master/fsm_slave.c Thu Jan 12 12:14:33 2012 +0100
+++ b/master/fsm_slave.c Thu Jan 12 13:55:15 2012 +0100
@@ -203,7 +203,8 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+ EC_DEVICE_MAIN);
return 1;
}
return 0;
@@ -220,9 +221,9 @@
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)) {
+ ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+ EC_DEVICE_MAIN);
return;
}
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
@@ -277,7 +278,8 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+ EC_DEVICE_MAIN);
return 1;
}
return 0;
@@ -294,9 +296,9 @@
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)) {
+ ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+ EC_DEVICE_MAIN);
return;
}
@@ -364,7 +366,8 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+ EC_DEVICE_MAIN);
return 1;
}
return 0;
@@ -382,7 +385,8 @@
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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+ EC_DEVICE_MAIN);
return;
}
--- a/master/fsm_slave_config.c Thu Jan 12 12:14:33 2012 +0100
+++ b/master/fsm_slave_config.c Thu Jan 12 13:55:15 2012 +0100
@@ -744,7 +744,7 @@
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);
+ fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
@@ -765,7 +765,8 @@
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_master_queue_external_datagram(slave->master, fsm_soe->datagram,
+ EC_DEVICE_MAIN);
return;
}
@@ -791,7 +792,7 @@
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);
+ fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
@@ -1473,7 +1474,7 @@
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);
+ fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
@@ -1494,7 +1495,8 @@
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_master_queue_external_datagram(slave->master, fsm_soe->datagram,
+ EC_DEVICE_MAIN);
return;
}
@@ -1520,7 +1522,7 @@
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);
+ fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
--- a/master/globals.h Thu Jan 12 12:14:33 2012 +0100
+++ b/master/globals.h Thu Jan 12 13:55:15 2012 +0100
@@ -218,11 +218,11 @@
/** Master devices.
*/
-enum {
+typedef enum {
EC_DEVICE_MAIN, /**< Main device. */
EC_DEVICE_BACKUP, /**< Backup device */
EC_NUM_DEVICES /**< Number of devices. */
-};
+} ec_device_index_t;
/*****************************************************************************/
--- a/master/master.c Thu Jan 12 12:14:33 2012 +0100
+++ b/master/master.c Thu Jan 12 13:55:15 2012 +0100
@@ -720,9 +720,9 @@
datagram->cycles_sent = 0;
#endif
datagram->jiffies_sent = 0;
- ec_master_queue_datagram(master, datagram);
- }
- else {
+ ec_master_queue_datagram(master, datagram,
+ datagram->device_index);
+ } else {
if (datagram->data_size > master->max_queue_size) {
list_del_init(&datagram->queue);
datagram->state = EC_DATAGRAM_ERROR;
@@ -793,7 +793,8 @@
*/
void ec_master_queue_external_datagram(
ec_master_t *master, /**< EtherCAT master */
- ec_datagram_t *datagram /**< datagram */
+ ec_datagram_t *datagram, /**< datagram */
+ ec_device_index_t device_index /**< Device index. */
)
{
ec_datagram_t *queued_datagram;
@@ -816,6 +817,7 @@
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
@@ -831,7 +833,8 @@
*/
void ec_master_queue_datagram(
ec_master_t *master, /**< EtherCAT master */
- ec_datagram_t *datagram /**< datagram */
+ ec_datagram_t *datagram, /**< datagram */
+ ec_device_index_t device_index /**< Device index. */
)
{
ec_datagram_t *queued_datagram;
@@ -871,6 +874,7 @@
list_add_tail(&datagram->queue, &master->datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
+ datagram->device_index = device_index;
}
/*****************************************************************************/
@@ -889,14 +893,17 @@
/*****************************************************************************/
-/** Sends the datagrams in the queue.
+/** Sends the datagrams in the queue for a certain device.
*
*/
-void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
+void ec_master_send_datagrams(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_device_index_t device_index /**< Device index. */
+ )
{
ec_datagram_t *datagram, *next;
size_t datagram_size;
- uint8_t *frame_data, *cur_data;
+ uint8_t *frame_data, *cur_data = NULL;
void *follows_word;
#ifdef EC_HAVE_CYCLES
cycles_t cycles_start, cycles_sent, cycles_end;
@@ -911,18 +918,27 @@
frame_count = 0;
INIT_LIST_HEAD(&sent_datagrams);
- EC_MASTER_DBG(master, 2, "ec_master_send_datagrams\n");
+ EC_MASTER_DBG(master, 2, "%s(device_index = %u)\n",
+ __func__, device_index);
do {
- // fetch pointer to transmit socket buffer
- frame_data = ec_device_tx_data(&master->devices[EC_DEVICE_MAIN]);
- cur_data = frame_data + EC_FRAME_HEADER_SIZE;
+ frame_data = NULL;
follows_word = NULL;
more_datagrams_waiting = 0;
// fill current frame with datagrams
list_for_each_entry(datagram, &master->datagram_queue, queue) {
- if (datagram->state != EC_DATAGRAM_QUEUED) continue;
+ if (datagram->state != EC_DATAGRAM_QUEUED ||
+ datagram->device_index != device_index) {
+ continue;
+ }
+
+ if (!frame_data) {
+ // fetch pointer to transmit socket buffer
+ frame_data =
+ ec_device_tx_data(&master->devices[device_index]);
+ cur_data = frame_data + EC_FRAME_HEADER_SIZE;
+ }
// does the current datagram fit in the frame?
datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
@@ -935,16 +951,17 @@
list_add_tail(&datagram->sent, &sent_datagrams);
datagram->index = master->datagram_index++;
- EC_MASTER_DBG(master, 2, "adding datagram 0x%02X\n",
+ EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n",
datagram->index);
- // set "datagram following" flag in previous frame
- if (follows_word)
+ // set "datagram following" flag in previous datagram
+ if (follows_word) {
EC_WRITE_U16(follows_word,
EC_READ_U16(follows_word) | 0x8000);
+ }
// EtherCAT datagram header
- EC_WRITE_U8 (cur_data, datagram->type);
+ EC_WRITE_U8 (cur_data, datagram->type);
EC_WRITE_U8 (cur_data + 1, datagram->index);
memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN);
EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
@@ -977,7 +994,7 @@
EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data);
// send frame
- ec_device_send(&master->devices[EC_DEVICE_MAIN],
+ ec_device_send(&master->devices[device_index],
cur_data - frame_data);
#ifdef EC_HAVE_CYCLES
cycles_sent = get_cycles();
@@ -1001,8 +1018,8 @@
#ifdef EC_HAVE_CYCLES
if (unlikely(master->debug_level > 1)) {
cycles_end = get_cycles();
- EC_MASTER_DBG(master, 0, "ec_master_send_datagrams"
- " sent %u frames in %uus.\n", frame_count,
+ EC_MASTER_DBG(master, 0, "%s()"
+ " sent %u frames in %uus.\n", __func__, frame_count,
(unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz);
}
#endif
@@ -1363,7 +1380,8 @@
// queue and send
down(&master->io_sem);
if (fsm_exec) {
- ec_master_queue_datagram(master, &master->fsm_datagram);
+ ec_master_queue_datagram(master, &master->fsm_datagram,
+ EC_DEVICE_MAIN);
}
ec_master_inject_external_datagrams(master);
ecrt_master_send(master);
@@ -2190,10 +2208,12 @@
void ecrt_master_send(ec_master_t *master)
{
ec_datagram_t *datagram, *n;
+ unsigned int i;
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_master_queue_datagram(master, &master->fsm_datagram,
+ EC_DEVICE_MAIN);
master->injection_seq_rt = master->injection_seq_fsm;
}
ec_master_inject_external_datagrams(master);
@@ -2215,7 +2235,11 @@
}
// send frames
- ec_master_send_datagrams(master);
+ for (i = 0; i < EC_NUM_DEVICES; i++) {
+ if (master->devices[i].dev) {
+ ec_master_send_datagrams(master, i);
+ }
+ }
}
/*****************************************************************************/
@@ -2275,7 +2299,7 @@
list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue,
queue) {
list_del(&datagram->queue);
- ec_master_queue_datagram(master, datagram);
+ ec_master_queue_datagram(master, datagram, EC_DEVICE_MAIN);
}
ecrt_master_send(master);
@@ -2441,7 +2465,8 @@
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_master_queue_datagram(master, &master->ref_sync_datagram,
+ EC_DEVICE_MAIN);
}
/*****************************************************************************/
@@ -2449,7 +2474,8 @@
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_master_queue_datagram(master, &master->sync_datagram,
+ EC_DEVICE_MAIN);
}
/*****************************************************************************/
@@ -2457,7 +2483,8 @@
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_master_queue_datagram(master, &master->sync_mon_datagram,
+ EC_DEVICE_MAIN);
}
/*****************************************************************************/
--- a/master/master.h Thu Jan 12 12:14:33 2012 +0100
+++ b/master/master.h Thu Jan 12 13:55:15 2012 +0100
@@ -242,7 +242,8 @@
ext_datagram_queue. */
struct list_head external_datagram_queue; /**< External Datagram queue. */
- unsigned int send_interval; /**< Interval between calls to ecrt_master_send */
+ unsigned int send_interval; /**< Interval between calls to
+ ecrt_master_send */
size_t max_queue_size; /**< Maximum size of datagram queue */
unsigned int debug_level; /**< Master debug level. */
@@ -298,9 +299,11 @@
// 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 *);
+void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *,
+ ec_device_index_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_queue_external_datagram(ec_master_t *, ec_datagram_t *,
+ ec_device_index_t);
void ec_master_inject_external_datagrams(ec_master_t *);
// misc.
--- a/master/voe_handler.c Thu Jan 12 12:14:33 2012 +0100
+++ b/master/voe_handler.c Thu Jan 12 13:55:15 2012 +0100
@@ -191,7 +191,8 @@
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_master_queue_datagram(voe->config->master, &voe->datagram,
+ EC_DEVICE_MAIN);
} else {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;