Dispatch handling

Slightly refactored code from @blckmn; modified dispatch handling so
that dispatchEntry can be easily included in struct

src/main/fc/fc_dispatch.c

@@ -26,57 +26,26 @@
 #include "drivers/system.h"
 #include "fc/fc_dispatch.h"
 
-static dispatchTask_t *head = NULL;
+static dispatchEntry_t *head = NULL;
 
 void dispatchProcess(uint32_t currentTime)
 {
-    if (!head || currentTime < head->delayedUntil) {
-        return;
-    }
-
-    dispatchTask_t *current = head;
-    dispatchTask_t *previous = NULL;
-    while (current && current->delayedUntil < currentTime) {
-        if (current->ptr) {
-            (*current->ptr)();
-        }
-
-        /* remove item from list */
-        if (previous) {
-            previous->next = current->next;
-        } else {
-            head = current->next;
-        }     
-        current->delayedUntil = 0;
-        current = current->next;
+    for(dispatchEntry_t **p = &head; *p; ) {
+        if(cmp32(currentTime, (*p)->delayedUntil) < 0)
+            break;
+        // unlink entry first, so handler can replan self
+        dispatchEntry_t *current = *p;
+        *p = (*p)->next;
+        (*current->dispatch)(current);
     }
 }
 
-void dispatchAdd(dispatchTask_t *task)
+void dispatchAdd(dispatchEntry_t *entry, int delayUs)
 {
-    if (!task || task->delayedUntil) {
-        /* invalid or already in the list */
-        return;
-    }
-
-    task->next = NULL;
-    task->delayedUntil = micros() + task->minimumDelayUs;
-
-    if (!head) {
-        head = task;
-        return;
-    }
-
-    if (task->delayedUntil < head->delayedUntil) {
-        task->next = head;
-        head = task;
-        return;
-    }
-
-    dispatchTask_t *pos = head;
-    while (pos->next && pos->next->delayedUntil < task->delayedUntil) {
-        pos = pos->next;
-    }
-    task->next = pos->next;
-    pos->next = task;
+    uint32_t delayedUntil = micros() + delayUs;
+    dispatchEntry_t **p = &head;
+    while(*p && cmp32((*p)->delayedUntil, delayedUntil) < 0)
+        p = &(*p)->next;
+    entry->next = *p;
+    *p = entry;
 }

src/main/fc/fc_dispatch.h

@@ -17,15 +17,14 @@
 
 #pragma once
 
-typedef void (*dispatchFuncPtr)(void);
-
-typedef struct dispatchTask_s {
-    dispatchFuncPtr ptr;
-    uint16_t minimumDelayUs;
+struct dispatchEntry_s;
+typedef void dispatchFunc(struct dispatchEntry_s* self);
 
+typedef struct dispatchEntry_s {
+    dispatchFunc *dispatch;
     uint32_t delayedUntil;
-    struct dispatchTask_s *next;
-} dispatchTask_t;
+    struct dispatchEntry_s *next;
+} dispatchEntry_t;
 
 void dispatchProcess(uint32_t currentTime);
-void dispatchAdd(dispatchTask_t *task);
+void dispatchAdd(dispatchEntry_t *entry, int delayUs);

src/main/rx/spektrum.c

@@ -85,7 +85,7 @@ static IO_t BindPlug = DEFIO_IO(NONE);
 static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
 static uint8_t telemetryBufLen = 0;
 
-void srxlRxSendTelemetryData(void);
+void srxlRxSendTelemetryDataDispatch(dispatchEntry_t *self);
 
 // Receive ISR callback
 static void spektrumDataReceive(uint16_t c)
@@ -113,7 +113,7 @@ static void spektrumDataReceive(uint16_t c)
 }
 
 static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
-static dispatchTask_t srxlTelemetryTask = { .ptr = srxlRxSendTelemetryData, .minimumDelayUs = 100 };
+static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch};
 
 static uint8_t spektrumFrameStatus(void)
 {
@@ -157,7 +157,7 @@ static uint8_t spektrumFrameStatus(void)
 
     /* only process if 2048, some data in buffer AND servos in phase 0 */
     if (spekHiRes && telemetryBufLen && (spekFrame[2] & 0x80)) {
-        dispatchAdd(&srxlTelemetryTask);
+        dispatchAdd(&srxlTelemetryDispatch, 100);
     }
     return RX_FRAME_COMPLETE;
 }
@@ -322,8 +322,9 @@ void srxlRxWriteTelemetryData(const void *data, int len)
     telemetryBufLen = len;
 }
 
-void srxlRxSendTelemetryData(void)
+void srxlRxSendTelemetryDataDispatch(dispatchEntry_t* self)
 {
+    UNUSED(self);
     // if there is telemetry data to write
     if (telemetryBufLen > 0) {
         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);