From 6171cc6e3be620cc629aea8c69c32fe353d2c13e Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Mon, 1 Aug 2011 17:54:56 +0200
Subject: [PATCH] Fixed skipped datagrams output again; added datagram
 information on skip.

---
 master/datagram.c | 50 +++++++++++++++++++++++++++++++++++++++++++++++
 master/datagram.h |  1 +
 master/master.c   | 30 ++++++++--------------------
 3 files changed, 59 insertions(+), 22 deletions(-)

diff --git a/master/datagram.c b/master/datagram.c
index 9ae5e5eb..8367a152 100644
--- a/master/datagram.c
+++ b/master/datagram.c
@@ -594,3 +594,53 @@ const char *ec_datagram_type_string(
 }
 
 /*****************************************************************************/
+
+/** Output datagram information.
+ */
+void ec_datagram_output_info(
+        const ec_datagram_t *datagram /**< EtherCAT datagram. */
+        )
+{
+    printk("state=%s ", ec_datagram_type_string(datagram));
+
+    switch (datagram->type) {
+        case EC_DATAGRAM_APRD:
+        case EC_DATAGRAM_APWR:
+        case EC_DATAGRAM_APRW:
+        case EC_DATAGRAM_ARMW:
+            printk("pos=%i mem=0x%04x",
+                    EC_READ_S16(datagram->address) * (-1),
+                    EC_READ_U16(datagram->address + 2)
+                    );
+            break;
+
+        case EC_DATAGRAM_FPRD:
+        case EC_DATAGRAM_FPWR:
+        case EC_DATAGRAM_FPRW:
+        case EC_DATAGRAM_FRMW:
+            printk("node=%u mem=0x%04x",
+                    EC_READ_U16(datagram->address),
+                    EC_READ_U16(datagram->address + 2));
+            break;
+
+        case EC_DATAGRAM_BRD:
+        case EC_DATAGRAM_BWR:
+        case EC_DATAGRAM_BRW:
+            printk("mem=0x%04x", EC_READ_U16(datagram->address + 2));
+            break;
+
+        case EC_DATAGRAM_LRD:
+        case EC_DATAGRAM_LWR:
+        case EC_DATAGRAM_LRW:
+            printk("log=0x%08x", EC_READ_U32(datagram->address));
+            break;
+
+        default:
+            printk("??");
+            break;
+    }
+
+    printk(" size=%zu", datagram->data_size);
+}
+
+/*****************************************************************************/
diff --git a/master/datagram.h b/master/datagram.h
index ee3416d1..e32c6031 100644
--- a/master/datagram.h
+++ b/master/datagram.h
@@ -141,6 +141,7 @@ void ec_datagram_print_state(const ec_datagram_t *);
 void ec_datagram_print_wc_error(const ec_datagram_t *);
 void ec_datagram_output_stats(ec_datagram_t *);
 const char *ec_datagram_type_string(const ec_datagram_t *);
+void ec_datagram_output_info(const ec_datagram_t *);
 
 /*****************************************************************************/
 
diff --git a/master/master.c b/master/master.c
index e2e951cd..e426cf3c 100644
--- a/master/master.c
+++ b/master/master.c
@@ -846,23 +846,6 @@ void ec_master_queue_datagram(
 {
     ec_datagram_t *queued_datagram;
 
-    switch (datagram->state) {
-        case EC_DATAGRAM_QUEUED:
-            datagram->skip_count++;
-            EC_MASTER_DBG(master, 1, "Skipping already queued datagram %p.\n",
-                    datagram);
-            break;
-
-        case EC_DATAGRAM_SENT:
-            datagram->skip_count++;
-            EC_MASTER_DBG(master, 1, "Skipping already sent datagram %p.\n",
-                    datagram);
-            break;
-
-        default:
-            break;
-    }
-
     /* It is possible, that a datagram in the queue is re-initialized with the
      * ec_datagram_<type>() methods and then shall be queued with this method.
      * In that case, the state is already reset to EC_DATAGRAM_INIT. Check if
@@ -872,14 +855,17 @@ void ec_master_queue_datagram(
     list_for_each_entry(queued_datagram, &master->datagram_queue, queue) {
         if (queued_datagram == datagram) {
             datagram->skip_count++;
-            EC_MASTER_DBG(master, 1, "Skipping re-initialized datagram %p.\n",
-                    datagram);
-            datagram->state = EC_DATAGRAM_QUEUED;
-            return;
+            if (master->debug_level) {
+                EC_MASTER_DBG(master, 1, "Skipping datagram %p (", datagram);
+                ec_datagram_output_info(datagram);
+                printk(")\n");
+            }
+            goto queued;
         }
     }
 
     list_add_tail(&datagram->queue, &master->datagram_queue);
+queued:
     datagram->state = EC_DATAGRAM_QUEUED;
 }
 
@@ -933,7 +919,7 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
             datagram->index = master->datagram_index++;
 
             EC_MASTER_DBG(master, 2, "Adding datagram %p i=0x%02X size=%zu\n",
-                    datagram, datagram->index,datagram_size);
+                    datagram, datagram->index, datagram_size);
 
             // set "datagram following" flag in previous frame
             if (follows_word) {