# HG changeset patch # User Martin Troxler # Date 1292489357 -3600 # Node ID 6c05411fee9b66774d154ba68152464d0a512f27 # Parent 4b16e2dce5fb4d42fb50ff3af6fe98f8c3acb615 replaced injection_seq mechanism with fsm datagram queue diff -r 4b16e2dce5fb -r 6c05411fee9b master/fsm_slave.c --- a/master/fsm_slave.c Tue Dec 14 14:00:19 2010 +0100 +++ b/master/fsm_slave.c Thu Dec 16 09:49:17 2010 +0100 @@ -206,7 +206,7 @@ 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_request_fsm_datagram(fsm->slave->master,fsm->datagram); return 1; } return 0; @@ -225,7 +225,7 @@ if (ec_fsm_coe_exec(&fsm->fsm_coe)) { - ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); + ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); return; } if (!ec_fsm_coe_success(&fsm->fsm_coe)) { @@ -280,7 +280,7 @@ 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_request_fsm_datagram(fsm->slave->master,fsm->datagram); return 1; } return 0; @@ -299,7 +299,7 @@ if (ec_fsm_foe_exec(&fsm->fsm_foe)) { - ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); + ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); return; } @@ -367,7 +367,7 @@ 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_request_fsm_datagram(fsm->slave->master, fsm->datagram); return 1; } return 0; @@ -385,7 +385,7 @@ 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_request_fsm_datagram(fsm->slave->master, fsm->datagram); return; } diff -r 4b16e2dce5fb -r 6c05411fee9b master/fsm_slave_config.c --- a/master/fsm_slave_config.c Tue Dec 14 14:00:19 2010 +0100 +++ b/master/fsm_slave_config.c Thu Dec 16 09:49:17 2010 +0100 @@ -738,7 +738,7 @@ ec_soe_request_write(&fsm->soe_request_copy); 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, + ec_master_queue_request_fsm_datagram(slave->master, fsm_soe->datagram); return; } @@ -760,7 +760,7 @@ 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_request_fsm_datagram(slave->master, fsm_soe->datagram); return; } @@ -785,7 +785,7 @@ ec_soe_request_write(&fsm->soe_request_copy); 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, + ec_master_queue_request_fsm_datagram(slave->master, fsm_soe->datagram); return; } @@ -1454,7 +1454,7 @@ ec_soe_request_write(&fsm->soe_request_copy); 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, + ec_master_queue_request_fsm_datagram(slave->master, fsm_soe->datagram); return; } @@ -1476,7 +1476,7 @@ 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_request_fsm_datagram(slave->master, fsm_soe->datagram); return; } @@ -1501,7 +1501,7 @@ ec_soe_request_write(&fsm->soe_request_copy); 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, + ec_master_queue_request_fsm_datagram(slave->master, fsm_soe->datagram); return; } diff -r 4b16e2dce5fb -r 6c05411fee9b master/master.c --- a/master/master.c Tue Dec 14 14:00:19 2010 +0100 +++ b/master/master.c Thu Dec 16 09:49:17 2010 +0100 @@ -172,7 +172,7 @@ INIT_LIST_HEAD(&master->ext_datagram_queue); sema_init(&master->ext_queue_sem, 1); - INIT_LIST_HEAD(&master->external_datagram_queue); + INIT_LIST_HEAD(&master->fsm_datagram_queue); // send interval in IDLE phase ec_master_set_send_interval(master, 1000000 / HZ); @@ -684,27 +684,30 @@ /*****************************************************************************/ -/** Injects external datagrams that fit into the datagram queue. - */ -void ec_master_inject_external_datagrams( +/** Injects fsm datagrams that fit into the datagram queue. + */ +void ec_master_inject_fsm_datagrams( ec_master_t *master /**< EtherCAT master */ ) { ec_datagram_t *datagram, *n; size_t queue_size = 0; + if (list_empty(&master->fsm_datagram_queue)) { + return; + } list_for_each_entry(datagram, &master->datagram_queue, queue) { queue_size += datagram->data_size; } - list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, + list_for_each_entry_safe(datagram, n, &master->fsm_datagram_queue, queue) { queue_size += datagram->data_size; if (queue_size <= master->max_queue_size) { list_del_init(&datagram->queue); #if DEBUG_INJECT - EC_MASTER_DBG(master, 0, "Injecting external datagram %08x" - " size=%u, queue_size=%u\n", (unsigned int) datagram, + EC_MASTER_DBG(master, 0, "Injecting fsm datagram %p" + " size=%zu, queue_size=%zu\n", datagram, datagram->data_size, queue_size); #endif #ifdef EC_HAVE_CYCLES @@ -717,7 +720,7 @@ if (datagram->data_size > master->max_queue_size) { list_del_init(&datagram->queue); datagram->state = EC_DATAGRAM_ERROR; - EC_MASTER_ERR(master, "External datagram %p is too large," + EC_MASTER_ERR(master, "Fsm datagram %p is too large," " size=%zu, max_queue_size=%zu\n", datagram, datagram->data_size, master->max_queue_size); @@ -745,15 +748,15 @@ ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); #endif EC_MASTER_ERR(master, "Timeout %u us: Injecting" - " external datagram %p size=%zu," + " fsm datagram %p size=%zu," " max_queue_size=%zu\n", time_us, datagram, datagram->data_size, master->max_queue_size); } #if DEBUG_INJECT else { EC_MASTER_DBG(master, 0, "Deferred injecting" - " of external datagram %p" - " size=%u, queue_size=%u\n", + " of fsm datagram %p" + " size=%zu, queue_size=%zu\n", datagram, datagram->data_size, queue_size); } #endif @@ -780,19 +783,32 @@ /*****************************************************************************/ -/** Places an external datagram in the sdo datagram queue. - */ -void ec_master_queue_external_datagram( +/** Places an request (SDO/FoE/SoE/EoE) fsm datagram in the sdo datagram queue. + */ +void ec_master_queue_request_fsm_datagram( ec_master_t *master, /**< EtherCAT master */ ec_datagram_t *datagram /**< datagram */ ) { + ec_master_queue_fsm_datagram(master,datagram); + master->fsm.idle = 0; // pump the bus as fast as possible +} + +/*****************************************************************************/ + +/** Places an fsm datagram in the sdo datagram queue. + */ +void ec_master_queue_fsm_datagram( + ec_master_t *master, /**< EtherCAT master */ + ec_datagram_t *datagram /**< datagram */ + ) +{ ec_datagram_t *queued_datagram; down(&master->io_sem); // check, if the datagram is already queued - list_for_each_entry(queued_datagram, &master->external_datagram_queue, + list_for_each_entry(queued_datagram, &master->fsm_datagram_queue, queue) { if (queued_datagram == datagram) { datagram->state = EC_DATAGRAM_QUEUED; @@ -802,18 +818,17 @@ } #if DEBUG_INJECT - EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n", + EC_MASTER_DBG(master, 0, "Requesting fsm datagram %p size=%zu\n", datagram, datagram->data_size); #endif - list_add_tail(&datagram->queue, &master->external_datagram_queue); + list_add_tail(&datagram->queue, &master->fsm_datagram_queue); datagram->state = EC_DATAGRAM_QUEUED; #ifdef EC_HAVE_CYCLES datagram->cycles_sent = get_cycles(); #endif datagram->jiffies_sent = jiffies; - master->fsm.idle = 0; up(&master->io_sem); } @@ -1257,7 +1272,6 @@ ec_master_t *master = (ec_master_t *) priv_data; ec_slave_t *slave = NULL; size_t sent_bytes; - // send interval in IDLE phase ec_master_set_send_interval(master, 1000000 / HZ); @@ -1273,20 +1287,19 @@ ecrt_master_receive(master); up(&master->io_sem); - if (master->injection_seq_rt == master->injection_seq_fsm) { - // execute master & slave state machines - if (down_interruptible(&master->master_sem)) - break; - if (ec_fsm_master_exec(&master->fsm)) - master->injection_seq_fsm++; - for (slave = master->slaves; - slave < master->slaves + master->slave_count; - slave++) { - if (ec_fsm_slave_exec(&slave->fsm)) - master->injection_seq_fsm++; - } - up(&master->master_sem); - } + // execute master & slave state machines + if (down_interruptible(&master->master_sem)) + break; + if (ec_fsm_master_exec(&master->fsm)) { + ec_master_queue_fsm_datagram(master, &master->fsm_datagram); + } + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + ec_fsm_slave_exec(&slave->fsm); // may queue datagram in external queue + } + up(&master->master_sem); + // queue and send down(&master->io_sem); ecrt_master_send(master); @@ -1331,23 +1344,20 @@ while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); - if (master->injection_seq_rt == master->injection_seq_fsm) { - // output statistics - ec_master_output_stats(master); - - // execute master & slave state machines - if (down_interruptible(&master->master_sem)) - break; - if (ec_fsm_master_exec(&master->fsm)) - master->injection_seq_fsm++; - for (slave = master->slaves; - slave < master->slaves + master->slave_count; - slave++) { - if (ec_fsm_slave_exec(&slave->fsm)) - master->injection_seq_fsm++; - } - up(&master->master_sem); - } + // output statistics + ec_master_output_stats(master); + + // execute master & slave state machines + if (down_interruptible(&master->master_sem)) + break; + if (ec_fsm_master_exec(&master->fsm)) + ec_master_queue_fsm_datagram(master, &master->fsm_datagram); + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + ec_fsm_slave_exec(&slave->fsm); // may queue datagram in external queue + } + up(&master->master_sem); #ifdef EC_USE_HRTIMER // the op thread should not work faster than the sending RT thread @@ -2119,12 +2129,7 @@ { ec_datagram_t *datagram, *n; - if (master->injection_seq_rt != master->injection_seq_fsm) { - // inject datagrams produced by master & slave FSMs - ec_master_queue_datagram(master, &master->fsm_datagram); - master->injection_seq_rt = master->injection_seq_fsm; - } - ec_master_inject_external_datagrams(master); + ec_master_inject_fsm_datagrams(master); if (unlikely(!master->main_device.link_state)) { // link is down, no datagram can be sent diff -r 4b16e2dce5fb -r 6c05411fee9b master/master.h --- a/master/master.h Tue Dec 14 14:00:19 2010 +0100 +++ b/master/master.h Thu Dec 16 09:49:17 2010 +0100 @@ -215,7 +215,7 @@ struct semaphore ext_queue_sem; /**< Semaphore protecting the \a ext_datagram_queue. */ - struct list_head external_datagram_queue; /**< External Datagram queue. */ + struct list_head fsm_datagram_queue; /**< External Datagram queue. */ unsigned int send_interval; /**< Interval between calls to ecrt_master_send */ size_t max_queue_size; /**< Maximum size of datagram queue */ @@ -274,8 +274,9 @@ 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_ext(ec_master_t *, ec_datagram_t *); -void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *); -void ec_master_inject_external_datagrams(ec_master_t *); +void ec_master_queue_request_fsm_datagram(ec_master_t *, ec_datagram_t *); +void ec_master_queue_fsm_datagram(ec_master_t *, ec_datagram_t *); +void ec_master_inject_fsm_datagrams(ec_master_t *); // misc. void ec_master_set_send_interval(ec_master_t *, unsigned int);