From af63d2c39a8b488a7e2f16d290539d2500a4b9a8 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 21 Jul 2011 18:25:38 +0200
Subject: [PATCH] Fixed bug in ec_master_queue_datagram() introduced in
 c9d32805d984.

---
 master/master.c | 25 ++++++++++++++++++++++---
 1 file changed, 22 insertions(+), 3 deletions(-)

diff --git a/master/master.c b/master/master.c
index c712e6ea..264eff9f 100644
--- a/master/master.c
+++ b/master/master.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- *  $Id: master.c,v b5391b329b5d 2010/11/30 14:24:21 fp $
+ *  $Id$
  *
  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
  *
@@ -821,6 +821,8 @@ void ec_master_queue_datagram(
         ec_datagram_t *datagram /**< datagram */
         )
 {
+    ec_datagram_t *queued_datagram;
+
     switch (datagram->state) {
         case EC_DATAGRAM_QUEUED:
             datagram->skip_count++;
@@ -835,10 +837,27 @@ void ec_master_queue_datagram(
             break;
 
         default:
-            list_add_tail(&datagram->queue, &master->datagram_queue);
-            datagram->state = EC_DATAGRAM_QUEUED;
             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
+     * the datagram is queued to avoid duplicate queuing (which results in an
+     * infinite loop!). Set the state to EC_DATAGRAM_QUEUED again, probably
+     * causing an unmatched 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;
+        }
+    }
+
+    list_add_tail(&datagram->queue, &master->datagram_queue);
+    datagram->state = EC_DATAGRAM_QUEUED;
 }
 
 /*****************************************************************************/