# HG changeset patch # User Florian Pose # Date 1326372915 -3600 # Node ID 5e1d3c9430e01711861cc1287d101cb924c6419b # Parent 2d36f36a433c4051174bd8a9a9c1e52a20358542 Datagram device_index member. diff -r 2d36f36a433c -r 5e1d3c9430e0 master/datagram.c --- 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; diff -r 2d36f36a433c -r 5e1d3c9430e0 master/datagram.h --- 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. */ diff -r 2d36f36a433c -r 5e1d3c9430e0 master/domain.c --- 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); } } diff -r 2d36f36a433c -r 5e1d3c9430e0 master/fsm_slave.c --- 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; } diff -r 2d36f36a433c -r 5e1d3c9430e0 master/fsm_slave_config.c --- 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; } } diff -r 2d36f36a433c -r 5e1d3c9430e0 master/globals.h --- 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; /*****************************************************************************/ diff -r 2d36f36a433c -r 5e1d3c9430e0 master/master.c --- 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); } /*****************************************************************************/ diff -r 2d36f36a433c -r 5e1d3c9430e0 master/master.h --- 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. diff -r 2d36f36a433c -r 5e1d3c9430e0 master/voe_handler.c --- 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;