GPIO_intr_hook.diff

diff --git a/app/include/user_config.h b/app/include/user_config.h
index 8060253..9a07daf 100644
--- a/app/include/user_config.h
+++ b/app/include/user_config.h
@@ -42,6 +42,7 @@
 #endif	/* NODE_ERROR */
 
 #define GPIO_INTERRUPT_ENABLE
+#define GPIO_INTERRUPT_HOOK_ENABLE
 // #define GPIO_SAFE_NO_INTR_ENABLE
 
 #define ICACHE_STORE_TYPEDEF_ATTR __attribute__((aligned(4),packed))
diff --git a/app/platform/platform.c b/app/platform/platform.c
index eeb63ce..3583efe 100755
--- a/app/platform/platform.c
+++ b/app/platform/platform.c
@@ -15,13 +15,18 @@
 #include "driver/sigma_delta.h"
 
 #ifdef GPIO_INTERRUPT_ENABLE
-typedef struct _GPIO_HOOK {
-  uint32_t gpio_bits;
-  void (*callback)(uint32_t bits);
-  struct _GPIO_HOOK *next;
-} GPIO_HOOK;
+static task_handle_t gpio_task_handle;
+
+#ifdef GPIO_INTERRUPT_HOOK_ENABLE
+struct gpio_hook {
+  platform_hook_function *func;
+  uint32                 *bits;
+  uint32                  all_bits;
+  uint32                  count;
+};
 
-static GPIO_HOOK* gpio_hooks;
+static struct gpio_hook platform_gpio_hook;
+#endif
 #endif
 
 static void pwms_init();
@@ -162,24 +167,23 @@ int platform_gpio_read( unsigned pin )
 }
 
 #ifdef GPIO_INTERRUPT_ENABLE
-static task_handle_t gpio_task_handle;
-
 static void ICACHE_RAM_ATTR platform_gpio_intr_dispatcher (void *dummy){
   uint32 j=0;
   uint32 gpio_status = GPIO_REG_READ(GPIO_STATUS_ADDRESS);
   UNUSED(dummy);
 
-  GPIO_HOOK *hooks = gpio_hooks;
-  for (; hooks; hooks = hooks->next) {
-    if (gpio_status & hooks->gpio_bits) {
-      hooks->callback(gpio_status & hooks->gpio_bits);
+#ifdef GPIO_INTERRUPT_HOOK_ENABLE
+  if (gpio_status & platform_gpio_hook.all_bits) {
+    for (j = 0; j < platform_gpio_hook.count; j++) {
+       if (gpio_status & platform_gpio_hook.bits[j])
+         gpio_status = (platform_gpio_hook.func[j])(gpio_status);
     }
   }
-
+#endif
   /*
    * gpio_status is a bit map where bit 0 is set if unmapped gpio pin 0 (pin3) has
    * triggered the ISR. bit 1 if unmapped gpio pin 1 (pin10=U0TXD), etc.  Since this
-   * in the ISR, it makes sense to optimize this by doing a fast scan of the status
+   * is the ISR, it makes sense to optimize this by doing a fast scan of the status
    * and reverse mapping any set bits.
    */
    for (j = 0; gpio_status>0; j++, gpio_status >>= 1) {
@@ -207,34 +211,62 @@ void platform_gpio_init( task_handle_t gpio_task )
   ETS_GPIO_INTR_ATTACH(platform_gpio_intr_dispatcher, NULL);
 }
 
+#ifdef GPIO_INTERRUPT_HOOK_ENABLE
 /*
- * Register a callback to happen at interrupt level for some set of
- * gpio_bits
+ * Register an ISR hook to be called from the GPIO ISR for a given GPIO bitmask.
+ * This routine is only called a few times so has been optimised for size and
+ * the unregister is a special case when the hook is NULL.
  */
-int platform_gpio_register_callback(uint32_t gpio_bits, void (*callback)(uint32_t)) 
+int platform_gpio_register_intr_hook(uint32_t bits, platform_hook_function hook)
 {
-  GPIO_HOOK *hook;
+  struct gpio_hook nh, oh = platform_gpio_hook;
+  int i, j;
 
-  for (hook = gpio_hooks; hook; hook = hook->next) {
-    if (hook->callback == callback) {
-      hook->gpio_bits = gpio_bits;
-      return 1;
+  nh.count = (unsigned) -1;
+
+  if (!hook) {
+    for (i=0; i<oh.count; i++) if (oh.bits[i] == bits) {
+      nh.count = oh.count - 1;  // unregister an existing hook
+      break;
     }
+  } else if (!(oh.all_bits & bits)) {
+    nh.count = oh.count + 1;    // register a new hook
   }
 
-  hook = (GPIO_HOOK *) c_zalloc(sizeof(GPIO_HOOK));
+  if (nh.count == (unsigned) -1)
+    return 0;  //This is an invalid request
 
-  if (!hook) {
-    return 0;
+  // These return NULL if the count = 0 so only error check if > 0)
+  nh.bits = c_malloc( nh.count * sizeof(*(nh.bits)) );
+  nh.func = c_malloc( nh.count * sizeof(*(nh.func)) );
+  if (nh.count && !(nh.bits && nh.func)) {
+    c_free(nh.bits); c_free(nh.func);
+    return 0;  // Allocation failure
+  }
+
+  for (i=0, j=0; i<oh.count; i++) {
+    if (oh.bits[i] == bits) continue; // matches once on unregister
+    nh.bits[j]   = oh.bits[i];
+    nh.func[j++] = oh.func[i];
   }
 
-  hook->gpio_bits = gpio_bits;
-  hook->callback = callback;
-  hook->next = gpio_hooks;
-  gpio_hooks = hook;
+  if (hook) { // for a register add the hook to the tail and set the all bits
+    nh.bits[j]  = bits;
+    nh.func[j]  = hook;
+    nh.all_bits = oh.all_bits | bits;
+  } else {    // for an register clear the matching all bits
+    nh.all_bits = oh.all_bits & (~bits);
+  }
+
+  ETS_GPIO_INTR_DISABLE();
+  platform_gpio_hook = nh;
+  ETS_GPIO_INTR_ENABLE();
 
+  c_free(nh.bits);
+  c_free(nh.func);
   return 1;
 }
+#endif // GPIO_INTERRUPT_HOOK_ENABLE
 
 /*
  * Initialise GPIO interrupt mode. Optionally in RAM because interrupts are dsabled
diff --git a/app/platform/platform.h b/app/platform/platform.h
index 30d22e5..70d1d1e 100644
--- a/app/platform/platform.h
+++ b/app/platform/platform.h
@@ -37,14 +37,22 @@ uint8_t platform_key_led( uint8_t level);
 #define PLATFORM_GPIO_HIGH 1
 #define PLATFORM_GPIO_LOW 0
 
+typedef uint32 (* platform_hook_function)(uint32 bitmask);
+
 static inline int platform_gpio_exists( unsigned pin ) { return pin < NUM_GPIO; }
 int platform_gpio_mode( unsigned pin, unsigned mode, unsigned pull );
 int platform_gpio_write( unsigned pin, unsigned level );
 int platform_gpio_read( unsigned pin );
 
-int platform_gpio_register_callback(uint32_t gpio_bits, void (*callback)(uint32_t));
-void platform_gpio_init( task_handle_t gpio_task );
+#ifdef GPIO_INTERRUPT_ENABLE
+#ifdef GPIO_INTERRUPT_HOOK_ENABLE
+int platform_gpio_register_intr_hook(uint32_t gpio_bits, platform_hook_function hook);
+#define platform_gpio_unregister_intr_hook(b) \
+  platform_gpio_register_intr_hook((b), (platform_hook_function) (NULL));
 void platform_gpio_intr_init( unsigned pin, GPIO_INT_TYPE type );
+#endif
+#endif
+void platform_gpio_init( task_handle_t gpio_task );
 // *****************************************************************************
 // Timer subsection