Safely schedule functional ISRs: follow-up to Schedule and FunctionalInterrupt fix

CMakeLists.txt

@@ -29,6 +29,7 @@ set(CORE_SRCS
   cores/esp32/MD5Builder.cpp
   cores/esp32/Print.cpp
   cores/esp32/stdlib_noniso.c
+  cores/esp32/Schedule.cpp
   cores/esp32/Stream.cpp
   cores/esp32/StreamString.cpp
   cores/esp32/wiring_pulse.c

cores/esp32/Arduino.h

@@ -159,6 +159,7 @@ void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val);
 #include "Udp.h"
 #include "HardwareSerial.h"
 #include "Esp.h"
+#include "Schedule.h"
 
 using std::isinf;
 using std::isnan;

cores/esp32/FunctionalInterrupt.cpp

@@ -1,44 +1,52 @@
-/*
- * FunctionalInterrupt.cpp
- *
- *  Created on: 8 jul. 2018
- *      Author: Herman
- */
-
 #include "FunctionalInterrupt.h"
+#include "Schedule.h"
 #include "Arduino.h"
 
-typedef void (*voidFuncPtr)(void);
-typedef void (*voidFuncPtrArg)(void*);
-
-extern "C"
+void ICACHE_RAM_ATTR interruptFunctional(void* arg)
 {
-	extern void __attachInterruptFunctionalArg(uint8_t pin, voidFuncPtrArg userFunc, void * arg, int intr_type, bool functional);
+    ArgStructure* localArg = static_cast<ArgStructure*>(arg);
+    if (localArg->interruptInfo)
+    {
+        localArg->interruptInfo->value = digitalRead(localArg->interruptInfo->pin);
+        localArg->interruptInfo->micro = micros();
+    }
+    if (localArg->scheduledFunction)
+    {
+        schedule_function(
+            [scheduledFunction = localArg->scheduledFunction,
+                               interruptInfo = *localArg->interruptInfo]()
+        {
+            scheduledFunction(interruptInfo);
+        });
+    }
 }
 
-void IRAM_ATTR interruptFunctional(void* arg)
+void cleanupFunctional(void* arg)
 {
-    InterruptArgStructure* localArg = (InterruptArgStructure*)arg;
-	if (localArg->interruptFunction)
-	{
-	  localArg->interruptFunction();
-	}
+    ArgStructure* localArg = static_cast<ArgStructure*>(arg);
+    delete localArg;
 }
 
-void attachInterrupt(uint8_t pin, std::function<void(void)> intRoutine, int mode)
+void attachScheduledInterrupt(uint8_t pin, std::function<void(InterruptInfo)> scheduledIntRoutine, int mode)
 {
-	// use the local interrupt routine which takes the ArgStructure as argument
-	__attachInterruptFunctionalArg (pin, (voidFuncPtrArg)interruptFunctional, new InterruptArgStructure{intRoutine}, mode, true);
+    void* localArg = detachInterruptArg(pin);
+    if (localArg)
+    {
+        cleanupFunctional(localArg);
+    }
+
+    ArgStructure* as = new ArgStructure;
+    as->interruptInfo = new InterruptInfo(pin);
+    as->scheduledFunction = scheduledIntRoutine;
+
+    attachInterruptArg(pin, interruptFunctional, as, mode);
 }
 
-extern "C"
+void detachFunctionalInterrupt(uint8_t pin)
 {
-   void cleanupFunctional(void* arg)
-   {
-	 delete (InterruptArgStructure*)arg;
-   }
+    void* localArg = detachInterruptArg(pin);
+    if (localArg)
+    {
+        cleanupFunctional(localArg);
+    }
 }
-
-
-
-

cores/esp32/FunctionalInterrupt.h

@@ -1,20 +1,29 @@
-/*
- * FunctionalInterrupt.h
- *
- *  Created on: 8 jul. 2018
- *      Author: Herman
- */
-
-#ifndef CORE_CORE_FUNCTIONALINTERRUPT_H_
-#define CORE_CORE_FUNCTIONALINTERRUPT_H_
+#ifndef CORE_FUNCTIONALINTERRUPT_H
+#define CORE_FUNCTIONALINTERRUPT_H
 
 #include <functional>
 
-struct InterruptArgStructure {
-	std::function<void(void)> interruptFunction;
+// Structures for communication
+
+struct InterruptInfo
+{
+    InterruptInfo(uint8_t _pin) : pin(_pin) {}
+    const uint8_t pin;
+    uint8_t value = 0;
+    uint32_t micro = 0;
 };
 
-void attachInterrupt(uint8_t pin, std::function<void(void)> intRoutine, int mode);
+struct ArgStructure
+{
+    ~ArgStructure()
+    {
+        delete interruptInfo;
+    }
+    InterruptInfo* interruptInfo = nullptr;
+    std::function<void(InterruptInfo)> scheduledFunction = nullptr;
+};
 
+void attachScheduledInterrupt(uint8_t pin, std::function<void(InterruptInfo)> scheduledIntRoutine, int mode);
+void detachFunctionalInterrupt(uint8_t pin);
 
-#endif /* CORE_CORE_FUNCTIONALINTERRUPT_H_ */
+#endif //CORE_FUNCTIONALINTERRUPT_H

cores/esp32/Schedule.cpp

@@ -0,0 +1,88 @@
+#include "Schedule.h"
+
+struct scheduled_fn_t
+{
+    scheduled_fn_t* mNext;
+    std::function<void(void)> mFunc;
+};
+
+static scheduled_fn_t* sFirst = 0;
+static scheduled_fn_t* sLast = 0;
+
+static scheduled_fn_t* sFirstUnused = 0;
+static scheduled_fn_t* sLastUnused = 0;
+
+static int sCount = 0;
+
+static scheduled_fn_t* get_fn()
+{
+    scheduled_fn_t* result = NULL;
+    // try to get an item from unused items list
+    if (sFirstUnused)
+    {
+        result = sFirstUnused;
+        sFirstUnused = result->mNext;
+        if (sFirstUnused == NULL)
+        {
+            sLastUnused = NULL;
+        }
+    }
+    // if no unused items, and count not too high, allocate a new one
+    else if (sCount != SCHEDULED_FN_MAX_COUNT)
+    {
+        result = new scheduled_fn_t;
+        result->mNext = NULL;
+        ++sCount;
+    }
+    return result;
+}
+
+static void recycle_fn(scheduled_fn_t* fn)
+{
+    if (!sLastUnused)
+    {
+        sFirstUnused = fn;
+    }
+    else
+    {
+        sLastUnused->mNext = fn;
+    }
+    fn->mNext = NULL;
+    sLastUnused = fn;
+}
+
+bool schedule_function(std::function<void(void)> fn)
+{
+    scheduled_fn_t* item = get_fn();
+    if (!item)
+    {
+        return false;
+    }
+    item->mFunc = fn;
+    item->mNext = NULL;
+    if (!sFirst)
+    {
+        sFirst = item;
+    }
+    else
+    {
+        sLast->mNext = item;
+    }
+    sLast = item;
+    return true;
+}
+
+void run_scheduled_functions()
+{
+    scheduled_fn_t* rFirst = sFirst;
+    sFirst = NULL;
+    sLast  = NULL;
+    while (rFirst)
+    {
+        scheduled_fn_t* item = rFirst;
+        rFirst = item->mNext;
+        item->mFunc();
+        item->mFunc = std::function<void(void)>();
+        recycle_fn(item);
+    }
+}

cores/esp32/Schedule.h

@@ -0,0 +1,27 @@
+#ifndef ESP_SCHEDULE_H
+#define ESP_SCHEDULE_H
+
+#include <functional>
+
+#define SCHEDULED_FN_MAX_COUNT 32
+#define SCHEDULED_FN_INITIAL_COUNT 4
+
+// Warning
+// This API is not considered stable.
+// Function signatures will change.
+// You have been warned.
+
+// Run given function next time `loop` function returns,
+// or `run_scheduled_functions` is called.
+// Use std::bind to pass arguments to a function, or call a class member function.
+// Note: there is no mechanism for cancelling scheduled functions.
+// Keep that in mind when binding functions to objects which may have short lifetime.
+// Returns false if the number of scheduled functions exceeds SCHEDULED_FN_MAX_COUNT.
+bool schedule_function(std::function<void(void)> fn);
+
+// Run all scheduled functions.
+// Use this function if your are not using `loop`, or `loop` does not return
+// on a regular basis.
+void run_scheduled_functions();
+
+#endif //ESP_SCHEDULE_H

cores/esp32/esp32-hal-gpio.c

@@ -74,9 +74,8 @@ typedef void (*voidFuncPtrArg)(void*);
 typedef struct {
     voidFuncPtr fn;
     void* arg;
-    bool functional;
 } InterruptHandle_t;
-static InterruptHandle_t __pinInterruptHandlers[GPIO_PIN_COUNT] = {0,};
+static InterruptHandle_t __pinInterruptHandlers[GPIO_PIN_COUNT] = { {0,0}, };
 
 #include "driver/rtc_io.h"
 
@@ -239,9 +238,7 @@ static void IRAM_ATTR __onPinInterrupt()
     }
 }
 
-extern void cleanupFunctional(void* arg);
-
-extern void __attachInterruptFunctionalArg(uint8_t pin, voidFuncPtrArg userFunc, void * arg, int intr_type, bool functional)
+extern void __attachInterruptArg(uint8_t pin, voidFuncPtrArg userFunc, void* arg, int intr_type)
 {
     static bool interrupt_initialized = false;
 
@@ -250,14 +247,8 @@ extern void __attachInterruptFunctionalArg(uint8_t pin, voidFuncPtrArg userFunc,
         esp_intr_alloc(ETS_GPIO_INTR_SOURCE, (int)ESP_INTR_FLAG_IRAM, __onPinInterrupt, NULL, &gpio_intr_handle);
     }
 
-    // if new attach without detach remove old info
-    if (__pinInterruptHandlers[pin].functional && __pinInterruptHandlers[pin].arg)
-    {
-    	cleanupFunctional(__pinInterruptHandlers[pin].arg);
-    }
     __pinInterruptHandlers[pin].fn = (voidFuncPtr)userFunc;
     __pinInterruptHandlers[pin].arg = arg;
-    __pinInterruptHandlers[pin].functional = functional;
 
     esp_intr_disable(gpio_intr_handle);
     if(esp_intr_get_cpu(gpio_intr_handle)) { //APP_CPU
@@ -269,36 +260,31 @@ extern void __attachInterruptFunctionalArg(uint8_t pin, voidFuncPtrArg userFunc,
     esp_intr_enable(gpio_intr_handle);
 }
 
-extern void __attachInterruptArg(uint8_t pin, voidFuncPtrArg userFunc, void * arg, int intr_type)
-{
-	__attachInterruptFunctionalArg(pin, userFunc, arg, intr_type, false);
-}
-
 extern void __attachInterrupt(uint8_t pin, voidFuncPtr userFunc, int intr_type) {
-    __attachInterruptFunctionalArg(pin, (voidFuncPtrArg)userFunc, NULL, intr_type, false);
+    __attachInterruptArg(pin, (voidFuncPtrArg)userFunc, NULL, intr_type);
 }
 
 extern void __detachInterrupt(uint8_t pin)
 {
     esp_intr_disable(gpio_intr_handle);
-    if (__pinInterruptHandlers[pin].functional && __pinInterruptHandlers[pin].arg)
-    {
-    	cleanupFunctional(__pinInterruptHandlers[pin].arg);
-    }
     __pinInterruptHandlers[pin].fn = NULL;
     __pinInterruptHandlers[pin].arg = NULL;
-    __pinInterruptHandlers[pin].functional = false;
 
     GPIO.pin[pin].int_ena = 0;
     GPIO.pin[pin].int_type = 0;
     esp_intr_enable(gpio_intr_handle);
 }
 
+extern void* __detachInterruptArg(uint8_t pin) {
+	void* arg = (pin < GPIO_PIN_COUNT) ? __pinInterruptHandlers[pin].arg : NULL;
+	__detachInterrupt(pin);
+	return arg;
+}
 
 extern void pinMode(uint8_t pin, uint8_t mode) __attribute__ ((weak, alias("__pinMode")));
 extern void digitalWrite(uint8_t pin, uint8_t val) __attribute__ ((weak, alias("__digitalWrite")));
 extern int digitalRead(uint8_t pin) __attribute__ ((weak, alias("__digitalRead")));
 extern void attachInterrupt(uint8_t pin, voidFuncPtr handler, int mode) __attribute__ ((weak, alias("__attachInterrupt")));
+extern void detachInterrupt(uint8_t pin) __attribute__((weak, alias("__detachInterrupt")));
 extern void attachInterruptArg(uint8_t pin, voidFuncPtrArg handler, void * arg, int mode) __attribute__ ((weak, alias("__attachInterruptArg")));
-extern void detachInterrupt(uint8_t pin) __attribute__ ((weak, alias("__detachInterrupt")));
-
+extern void* detachInterruptArg(uint8_t pin) __attribute__((weak, alias("__detachInterruptArg")));

cores/esp32/esp32-hal-gpio.h

@@ -79,8 +79,9 @@ void digitalWrite(uint8_t pin, uint8_t val);
 int digitalRead(uint8_t pin);
 
 void attachInterrupt(uint8_t pin, void (*)(void), int mode);
-void attachInterruptArg(uint8_t pin, void (*)(void*), void * arg, int mode);
 void detachInterrupt(uint8_t pin);
+void attachInterruptArg(uint8_t pin, void (*)(void*), void* arg, int mode);
+void* detachInterruptArg(uint8_t pin);
 
 #ifdef __cplusplus
 }

cores/esp32/main.cpp

@@ -17,6 +17,7 @@ void loopTask(void *pvParameters)
             esp_task_wdt_reset();
         }
         loop();
+        run_scheduled_functions();
     }
 }