--- a/master/master.c Mon Dec 14 13:11:36 2009 +0100
+++ b/master/master.c Mon Dec 14 13:25:50 2009 +0100
@@ -59,13 +59,14 @@
/** Frame timeout in cycles.
*/
static cycles_t timeout_cycles;
-
+static cycles_t sdo_injection_timeout_cycles;
#else
/** Frame timeout in jiffies.
*/
static unsigned long timeout_jiffies;
-
+static unsigned long sdo_injection_timeout_jiffies;
+
#endif
/*****************************************************************************/
@@ -87,9 +88,11 @@
{
#ifdef EC_HAVE_CYCLES
timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
+ sdo_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
#else
// one jiffy may always elapse between time measurement
timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
+ sdo_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
#endif
}
@@ -151,6 +154,9 @@
INIT_LIST_HEAD(&master->ext_datagram_queue);
sema_init(&master->ext_queue_sem, 1);
+ INIT_LIST_HEAD(&master->sdo_datagram_queue);
+ master->max_queue_size = EC_MAX_DATA_SIZE;
+
INIT_LIST_HEAD(&master->domains);
master->debug_level = debug_level;
@@ -684,6 +690,91 @@
master->phase = EC_IDLE;
}
+
+/*****************************************************************************/
+
+/** Injects sdo datagrams that fit into the datagram queue
+ */
+void ec_master_inject_sdo_datagrams(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ ec_datagram_t *datagram, *n;
+ size_t queue_size = 0;
+ list_for_each_entry(datagram, &master->datagram_queue, queue) {
+ queue_size += datagram->data_size;
+ }
+ list_for_each_entry_safe(datagram, n, &master->sdo_datagram_queue, queue) {
+ queue_size += datagram->data_size;
+ if (queue_size <= master->max_queue_size) {
+ list_del_init(&datagram->queue);
+ if (master->debug_level) {
+ EC_DBG("Injecting SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
+ }
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_sent = 0;
+#endif
+ datagram->jiffies_sent = 0;
+ ec_master_queue_datagram(master, datagram);
+ }
+ else {
+ if (datagram->data_size > master->max_queue_size) {
+ list_del_init(&datagram->queue);
+ datagram->state = EC_DATAGRAM_ERROR;
+ EC_ERR("SDO datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size);
+ }
+ else {
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_now = get_cycles();
+ if (cycles_now - datagram->cycles_sent
+ > sdo_injection_timeout_cycles) {
+#else
+ if (jiffies - datagram->jiffies_sent
+ > sdo_injection_timeout_jiffies) {
+#endif
+ unsigned int time_us;
+ list_del_init(&datagram->queue);
+ datagram->state = EC_DATAGRAM_ERROR;
+#ifdef EC_HAVE_CYCLES
+ time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz;
+#else
+ time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
+#endif
+ EC_ERR("Timeout %u us: injecting SDO datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size);
+ }
+ else {
+ if (master->debug_level) {
+ EC_DBG("Deferred injecting of SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
+ }
+ }
+ }
+ }
+ }
+}
+
+/*****************************************************************************/
+
+/** Places a sdo datagram in the sdo datagram queue.
+ */
+void ec_master_queue_sdo_datagram(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_datagram_t *datagram /**< datagram */
+ )
+{
+ if (master->debug_level) {
+ EC_DBG("Requesting SDO datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size);
+ }
+ datagram->state = EC_DATAGRAM_QUEUED;
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_sent = get_cycles();
+#endif
+ datagram->jiffies_sent = jiffies;
+
+ down(&master->io_sem);
+ list_add_tail(&datagram->queue, &master->sdo_datagram_queue);
+ up(&master->io_sem);
+}
+
/*****************************************************************************/
/** Places a datagram in the datagram queue.
@@ -695,6 +786,8 @@
{
ec_datagram_t *queued_datagram;
+ if (datagram->state == EC_DATAGRAM_SENT)
+ return;
// check, if the datagram is already queued
list_for_each_entry(queued_datagram, &master->datagram_queue, queue) {
if (queued_datagram == datagram) {
@@ -1001,7 +1094,8 @@
static int ec_master_idle_thread(void *priv_data)
{
ec_master_t *master = (ec_master_t *) priv_data;
-
+ ec_slave_t *slave = NULL;
+ int fsm_exec;
if (master->debug_level)
EC_DBG("Idle thread running.\n");
@@ -1013,22 +1107,27 @@
ecrt_master_receive(master);
up(&master->io_sem);
- if (master->fsm_datagram.state == EC_DATAGRAM_SENT)
- goto schedule;
-
- // execute master state machine
+ fsm_exec = 0;
+ // execute master & slave state machines
if (down_interruptible(&master->master_sem))
break;
- ec_fsm_master_exec(&master->fsm);
+ fsm_exec = ec_fsm_master_exec(&master->fsm);
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_fsm_slave_exec(&slave->fsm);
+ }
up(&master->master_sem);
// queue and send
down(&master->io_sem);
- ec_master_queue_datagram(master, &master->fsm_datagram);
+ if (fsm_exec) {
+ ec_master_queue_datagram(master, &master->fsm_datagram);
+ }
+ ec_master_inject_sdo_datagrams(master);
ecrt_master_send(master);
up(&master->io_sem);
-
-schedule:
+
if (ec_fsm_master_idle(&master->fsm)) {
set_current_state(TASK_INTERRUPTIBLE);
schedule_timeout(1);
@@ -1045,35 +1144,38 @@
/*****************************************************************************/
-/** Master kernel thread function for IDLE phase.
+/** Master kernel thread function for OPERATION phase.
*/
static int ec_master_operation_thread(void *priv_data)
{
ec_master_t *master = (ec_master_t *) priv_data;
-
+ ec_slave_t *slave = NULL;
+ int fsm_exec;
if (master->debug_level)
EC_DBG("Operation thread running.\n");
while (!kthread_should_stop()) {
ec_datagram_output_stats(&master->fsm_datagram);
- if (master->injection_seq_rt != master->injection_seq_fsm ||
- master->fsm_datagram.state == EC_DATAGRAM_SENT ||
- master->fsm_datagram.state == EC_DATAGRAM_QUEUED)
- goto schedule;
-
- // output statistics
- ec_master_output_stats(master);
-
- // execute master state machine
- if (down_interruptible(&master->master_sem))
- break;
- ec_fsm_master_exec(&master->fsm);
- up(&master->master_sem);
-
- // inject datagram
- master->injection_seq_fsm++;
-
-schedule:
+ if (master->injection_seq_rt == master->injection_seq_fsm) {
+ // output statistics
+ ec_master_output_stats(master);
+
+ fsm_exec = 0;
+ // execute master & slave state machines
+ if (down_interruptible(&master->master_sem))
+ break;
+ fsm_exec += ec_fsm_master_exec(&master->fsm);
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_fsm_slave_exec(&slave->fsm);
+ }
+ up(&master->master_sem);
+
+ // inject datagrams (let the rt thread queue them, see ecrt_master_send)
+ if (fsm_exec)
+ master->injection_seq_fsm++;
+ }
if (ec_fsm_master_idle(&master->fsm)) {
set_current_state(TASK_INTERRUPTIBLE);
schedule_timeout(1);
@@ -1797,10 +1899,11 @@
ec_datagram_t *datagram, *n;
if (master->injection_seq_rt != master->injection_seq_fsm) {
- // inject datagram produced by master 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_sdo_datagrams(master);
if (unlikely(!master->main_device.link_state)) {
// link is down, no datagram can be sent