From 924817c2534f5fc6b677b6059115600df03a2999 Mon Sep 17 00:00:00 2001
From: Anders Hoglund <AndWho@gmail.com>
Date: Wed, 15 Aug 2018 10:39:26 +0200
Subject: [PATCH] Fix for issue 6348. Prevent dispatcher deadlock.

---
 src/main/fc/dispatch.c | 10 +++++++++-
 src/main/fc/dispatch.h |  1 +
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/dispatch.c b/src/main/fc/dispatch.c
index db372f121..1e105fb79 100644
--- a/src/main/fc/dispatch.c
+++ b/src/main/fc/dispatch.c
@@ -51,6 +51,7 @@ void dispatchProcess(uint32_t currentTime)
         // unlink entry first, so handler can replan self
         dispatchEntry_t *current = *p;
         *p = (*p)->next;
+        current->inQue = false;
         (*current->dispatch)(current);
     }
 }
@@ -58,10 +59,17 @@ void dispatchProcess(uint32_t currentTime)
 void dispatchAdd(dispatchEntry_t *entry, int delayUs)
 {
     uint32_t delayedUntil = micros() + delayUs;
-    entry->delayedUntil = delayedUntil;
     dispatchEntry_t **p = &head;
+
+    if (entry->inQue) {
+      return;    // Allready in Queue, abort
+    }
+
     while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0)
         p = &(*p)->next;
+
     entry->next = *p;
+    entry->delayedUntil = delayedUntil;
+    entry->inQue = true;
     *p = entry;
 }
diff --git a/src/main/fc/dispatch.h b/src/main/fc/dispatch.h
index ce40918a8..18b2d49dc 100644
--- a/src/main/fc/dispatch.h
+++ b/src/main/fc/dispatch.h
@@ -27,6 +27,7 @@ typedef struct dispatchEntry_s {
     dispatchFunc *dispatch;
     uint32_t delayedUntil;
     struct dispatchEntry_s *next;
+    bool inQue;
 } dispatchEntry_t;
 
 bool dispatchIsEnabled(void);