4 files changed, 138 insertions, 3 deletions

diff --git a/firmware/export/config-e200.h b/firmware/export/config-e200.h
index acb23cb4d0..63f617e78d 100644
--- a/firmware/export/config-e200.h
+++ b/firmware/export/config-e200.h
@@ -176,4 +176,6 @@
 /* Range for this target: 0xffffff*(0.0-16.000000894069724921567733381255) */
 #define WHEEL_ACCELERATION_FACTOR   (0xffffff*7)
 
+#define INCLUDE_TIMEOUT_API
+
 #endif /* SIMULATOR */
diff --git a/firmware/export/kernel.h b/firmware/export/kernel.h
index b40d602d9f..bf5a9d10c3 100644
--- a/firmware/export/kernel.h
+++ b/firmware/export/kernel.h
@@ -126,6 +126,28 @@ extern void sleep(int ticks);
 int tick_add_task(void (*f)(void));
 int tick_remove_task(void (*f)(void));
 
+struct timeout;
+
+/* timeout callback type
+ * tmo - pointer to struct timeout associated with event
+ */
+typedef bool (* timeout_cb_type)(struct timeout *tmo);
+
+struct timeout
+{
+    /* for use by callback/internal - read/write */
+    timeout_cb_type callback;/* callback - returning false cancels */
+    int             ticks;   /* timeout period in ticks */
+    intptr_t        data;    /* data passed to callback */
+    /* internal use - read-only */
+    const struct timeout * const next; /* next timeout in list */
+    const long expires; /* expiration tick */
+};
+
+void timeout_register(struct timeout *tmo, timeout_cb_type callback,
+                      int ticks, intptr_t data);
+void timeout_cancel(struct timeout *tmo);
+
 extern void queue_init(struct event_queue *q, bool register_queue);
 #if NUM_CORES > 1
 extern void queue_set_irq_safe(struct event_queue *q, bool state);
diff --git a/firmware/kernel.c b/firmware/kernel.c
index b1a4e62a81..bb67ced64d 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -750,6 +750,108 @@ int tick_remove_task(void (*f)(void))
     return -1;
 }
 
+/****************************************************************************
+ * Tick-based interval timers/one-shots - be mindful this is not really
+ * intended for continuous timers but for events that need to run for a short
+ * time and be cancelled without further software intervention.
+ ****************************************************************************/
+#ifdef INCLUDE_TIMEOUT_API
+static struct timeout *tmo_list = NULL; /* list of active timeout events */
+
+/* timeout tick task - calls event handlers when they expire
+ * Event handlers may alter ticks, callback and data during operation.
+ */
+static void timeout_tick(void)
+{
+    unsigned long tick = current_tick;
+    struct timeout *curr, *next;
+
+    for (curr = tmo_list; curr != NULL; curr = next)
+    {
+        next = (struct timeout *)curr->next;
+
+        if (TIME_BEFORE(tick, curr->expires))
+            continue;
+
+        /* this event has expired - call callback */
+        if (curr->callback(curr))
+            *(long *)&curr->expires = tick + curr->ticks; /* reload */
+        else
+            timeout_cancel(curr); /* cancel */
+    }
+}
+
+/* Cancels a timeout callback - can be called from the ISR */
+void timeout_cancel(struct timeout *tmo)
+{
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
+    if (tmo_list != NULL)
+    {
+        struct timeout *curr = tmo_list;
+        struct timeout *prev = NULL;
+
+        while (curr != tmo && curr != NULL)
+        {
+            prev = curr;
+            curr = (struct timeout *)curr->next;
+        }
+
+        if (curr != NULL)
+        {
+            /* in list */
+            if (prev == NULL)
+                tmo_list = (struct timeout *)curr->next;
+            else
+                *(const struct timeout **)&prev->next = curr->next;
+
+            if (tmo_list == NULL)
+                tick_remove_task(timeout_tick); /* last one - remove task */
+        }
+        /* not in list or tmo == NULL */
+    }
+
+    set_irq_level(oldlevel);
+}
+
+/* Adds a timeout callback - calling with an active timeout resets the
+   interval - can be called from the ISR */
+void timeout_register(struct timeout *tmo, timeout_cb_type callback,
+                      int ticks, intptr_t data)
+{
+    int oldlevel;
+    struct timeout *curr;
+
+    if (tmo == NULL)
+        return;
+
+    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
+    /* see if this one is already registered */
+    curr = tmo_list;
+    while (curr != tmo && curr != NULL)
+        curr = (struct timeout *)curr->next;
+
+    if (curr == NULL)
+    {
+        /* not found - add it */
+        if (tmo_list == NULL)
+            tick_add_task(timeout_tick); /* first one - add task */
+
+        *(struct timeout **)&tmo->next = tmo_list;
+        tmo_list = tmo;
+    }
+
+    tmo->callback = callback;
+    tmo->ticks = ticks;
+    tmo->data = data;
+    *(long *)&tmo->expires = current_tick + ticks;
+
+    set_irq_level(oldlevel);
+}
+
+#endif /* INCLUDE_TIMEOUT_API */
+
 #ifndef SIMULATOR
 /*
  * Simulator versions in uisimulator/SIMVER/
diff --git a/firmware/target/arm/sandisk/sansa-e200/ata-e200.c b/firmware/target/arm/sandisk/sansa-e200/ata-e200.c
index 1b67454445..ff277ec8a3 100644
--- a/firmware/target/arm/sandisk/sansa-e200/ata-e200.c
+++ b/firmware/target/arm/sandisk/sansa-e200/ata-e200.c
@@ -1122,15 +1122,24 @@ bool card_detect_target(void)
     return (GPIOA_INPUT_VAL & 0x80) == 0;
 }
 
+static bool sd1_oneshot_callback(struct timeout *tmo)
+{
+    /* Take final state only - insert/remove is bouncy */
+    queue_remove_from_head(&sd_queue, SD_HOTSWAP);
+    queue_post(&sd_queue, SD_HOTSWAP, tmo->data);
+    return false;
+}
+
 /* called on insertion/removal interrupt */
 void microsd_int(void)
 {
+    static struct timeout sd1_oneshot;
+
     int detect = GPIOA_INPUT_VAL & 0x80;
 
     GPIOA_INT_LEV = (GPIOA_INT_LEV & ~0x80) | (detect ^ 0x80);
     GPIOA_INT_CLR = 0x80;
 
-    /* Take final state only - insert/remove is bouncy */
-    queue_remove_from_head(&sd_queue, SD_HOTSWAP);
-    queue_post(&sd_queue, SD_HOTSWAP, detect == 0);
+    timeout_register(&sd1_oneshot, sd1_oneshot_callback,
+                     detect ? 1 : HZ/2, detect == 0);
 }