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

Datagram device_index member.

This commit is contained in:
Florian Pose 2012-01-12 13:55:15 +01:00
parent 6d932e88d5
commit 849f37cd41
9 changed files with 90 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
master/datagram.c
View File

@ -88,6 +88,7 @@ static const char *type_strings[] = {
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;

2
master/datagram.h
View File

@ -88,6 +88,8 @@ typedef struct {
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. */

2
master/domain.c
View File

@ -438,7 +438,7 @@ void ecrt_domain_queue(ec_domain_t *domain)
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);
}
}

24
master/fsm_slave.c
View File

@ -203,7 +203,8 @@ 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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
return 1;
}
return 0;
@ -220,9 +221,9 @@ void ec_fsm_slave_state_sdo_request(
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 @@ 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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
return 1;
}
return 0;
@ -294,9 +296,9 @@ void ec_fsm_slave_state_foe_request(
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 @@ 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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
return 1;
}
return 0;
@ -382,7 +385,8 @@ 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_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
EC_DEVICE_MAIN);
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);
fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
@ -765,7 +765,8 @@ 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_master_queue_external_datagram(slave->master, fsm_soe->datagram,
EC_DEVICE_MAIN);
return;
}
@ -791,7 +792,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);
fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
@ -1473,7 +1474,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);
fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}
@ -1494,7 +1495,8 @@ 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_master_queue_external_datagram(slave->master, fsm_soe->datagram,
EC_DEVICE_MAIN);
return;
}
@ -1520,7 +1522,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);
fsm_soe->datagram, EC_DEVICE_MAIN);
return;
}
}

4
master/globals.h
View File

@ -218,11 +218,11 @@ enum {
/** 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;
/*****************************************************************************/

81
master/master.c
View File

@ -720,9 +720,9 @@ void ec_master_inject_external_datagrams(
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_set_send_interval(
*/
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 @@ 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
@ -831,7 +833,8 @@ void ec_master_queue_external_datagram(
*/
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 @@ void ec_master_queue_datagram(
list_add_tail(&datagram->queue, &master->datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
datagram->device_index = device_index;
}
/*****************************************************************************/
@ -889,14 +893,17 @@ void ec_master_queue_datagram_ext(
/*****************************************************************************/
/** 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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
#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 @@ 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_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_deactivate(ec_master_t *master)
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 @@ void ecrt_master_send(ec_master_t *master)
}
// 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 @@ 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_master_queue_datagram(master, datagram, EC_DEVICE_MAIN);
}
ecrt_master_send(master);
@ -2441,7 +2465,8 @@ 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_master_queue_datagram(master, &master->ref_sync_datagram,
EC_DEVICE_MAIN);
}
/*****************************************************************************/
@ -2449,7 +2474,8 @@ 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_master_queue_datagram(master, &master->sync_datagram,
EC_DEVICE_MAIN);
}
/*****************************************************************************/
@ -2457,7 +2483,8 @@ 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_master_queue_datagram(master, &master->sync_mon_datagram,
EC_DEVICE_MAIN);
}
/*****************************************************************************/

9
master/master.h
View File

@ -242,7 +242,8 @@ struct ec_master {
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 @@ 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 *);
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.

3
master/voe_handler.c
View File

@ -191,7 +191,8 @@ 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_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;