Interrupt chaining: preliminary version

libraries/mbed/api/CallChain.h

@@ -0,0 +1,57 @@
+#ifndef MBED_CALLCHAIN_H
+#define MBED_CALLCHAIN_H
+
+#include "FunctionPointer.h"
+#include <string.h>
+
+namespace mbed {
+
+typedef FunctionPointer* pFunctionPointer_t;
+
+class CallChain {
+public:
+    CallChain(int size = 4);
+    ~CallChain();
+
+    pFunctionPointer_t add(void (*function)(void));
+    template<typename T>
+    pFunctionPointer_t add(T *object, void (T::*member)(void)) {
+        return common_add(new FunctionPointer(object, member));
+    }
+
+    pFunctionPointer_t add_front(void (*function)(void));
+    template<typename T>
+    pFunctionPointer_t add_front(T *object, void (T::*member)(void)) {
+        return common_add_front(new FunctionPointer(object, member));
+    }
+
+    int size() const;
+    pFunctionPointer_t get(int i) const;
+    int find(pFunctionPointer_t f) const;
+    void clear();
+    bool remove(pFunctionPointer_t f);
+    void call();
+ 
+#ifdef MBED_OPERATORS
+    void operator ()(void) {
+        call();
+    }
+    pFunctionPointer_t operator [](int i) const {
+        return get(i);
+    }
+#endif
+
+private:
+    void _check_size();
+    pFunctionPointer_t common_add(pFunctionPointer_t pf);
+    pFunctionPointer_t common_add_front(pFunctionPointer_t pf);
+
+    pFunctionPointer_t* _chain;
+    int _size;
+    int _elements;
+};
+
+} // namespace mbed
+
+#endif
+

libraries/mbed/api/FunctionPointer.h

@@ -20,6 +20,8 @@
 
 namespace mbed {
 
+typedef void (*pvoidf_t)(void);
+
 /** A class for storing and calling a pointer to a static or member void function
  */
 class FunctionPointer {
@@ -64,6 +66,14 @@ public:
      */
     void call();
 
+    pvoidf_t get_function() const {
+        return (pvoidf_t)_function;
+    }
+
+#ifdef MBED_OPERATORS
+    void operator ()(void);
+#endif
+
 private:
     template<typename T>
     static void membercaller(void *object, char *member) {

libraries/mbed/api/InterruptIn.h

@@ -24,6 +24,7 @@
 #include "gpio_irq_api.h"
 
 #include "FunctionPointer.h"
+#include "CallChain.h"
 
 namespace mbed {
 
@@ -72,25 +73,57 @@ public:
     /** Attach a function to call when a rising edge occurs on the input
      *
      *  @param fptr A pointer to a void function, or 0 to set as none
+     *
+     *  @returns
+     *  The function object created for 'fptr'
      */
-    void rise(void (*fptr)(void));
+    pFunctionPointer_t rise(void (*fptr)(void));
+    pFunctionPointer_t rise_add(void (*fptr)(void)) {
+        return rise_add_common(fptr);
+    }
+    pFunctionPointer_t rise_add_front(void (*fptr)(void)) {
+        return rise_add_common(fptr, true);
+    }
 
     /** Attach a member function to call when a rising edge occurs on the input
      *
      *  @param tptr pointer to the object to call the member function on
      *  @param mptr pointer to the member function to be called
+     *
+     *  @returns
+     *  The function object created for 'tptr' and 'mptr'
      */
     template<typename T>
-    void rise(T* tptr, void (T::*mptr)(void)) {
+    pFunctionPointer_t rise(T* tptr, void (T::*mptr)(void)) {
-        _rise.attach(tptr, mptr);
+        _rise.clear();
+        pFunctionPointer_t pf = _rise.add(tptr, mptr);
         gpio_irq_set(&gpio_irq, IRQ_RISE, 1);
+        return pf;
     }
 
+    template<typename T>
+    pFunctionPointer_t rise_add(T* tptr, void (T::*mptr)(void)) {
+        return rise_add_common(tptr, mptr);
+    }
+
+    template<typename T>
+    pFunctionPointer_t rise_add_front(T* tptr, void (T::*mptr)(void)) {
+        return rise_add_common(tptr, mptr, true);
+    }
+
+    bool rise_remove(pFunctionPointer_t pf);
+
     /** Attach a function to call when a falling edge occurs on the input
      *
      *  @param fptr A pointer to a void function, or 0 to set as none
      */
-    void fall(void (*fptr)(void));
+    pFunctionPointer_t fall(void (*fptr)(void));
+    pFunctionPointer_t fall_add(void (*fptr)(void)) {
+        return fall_add_common(fptr);
+    }
+    pFunctionPointer_t fall_add_front(void (*fptr)(void)) {
+        return fall_add_common(fptr, true);
+    }
 
     /** Attach a member function to call when a falling edge occurs on the input
      *
@@ -98,10 +131,22 @@ public:
      *  @param mptr pointer to the member function to be called
      */
     template<typename T>
-    void fall(T* tptr, void (T::*mptr)(void)) {
+    pFunctionPointer_t fall(T* tptr, void (T::*mptr)(void)) {
-        _fall.attach(tptr, mptr);
+        _fall.clear();
+        pFunctionPointer_t pf = _fall.add(tptr, mptr);
         gpio_irq_set(&gpio_irq, IRQ_FALL, 1);
+        return pf;
     }
+    template<typename T>
+    pFunctionPointer_t fall_add(T* tptr, void (T::*mptr)(void)) {
+        return fall_add_common(tptr, mptr);
+    }
+    template<typename T>
+    pFunctionPointer_t fall_add_front(T* tptr, void (T::*mptr)(void)) {
+        return fall_add_common(tptr, mptr, true);
+    }
+
+    bool fall_remove(pFunctionPointer_t pf);
 
     /** Set the input pin mode
      *
@@ -112,11 +157,27 @@ public:
     static void _irq_handler(uint32_t id, gpio_irq_event event);
 
 protected:
+    pFunctionPointer_t rise_add_common(void (*fptr)(void), bool front=false);
+    pFunctionPointer_t fall_add_common(void (*fptr)(void), bool front=false);
+
+    template<typename T>
+    pFunctionPointer_t rise_add_common(T* tptr, void (T::*mptr)(void), bool front=false) {
+        pFunctionPointer_t pf = front ? _rise.add_front(tptr, mptr) : _rise.add(tptr, mptr);
+        gpio_irq_set(&gpio_irq, IRQ_RISE, 1);
+        return pf;
+    }
+    template<typename T>
+    pFunctionPointer_t fall_add_common(T* tptr, void (T::*mptr)(void), bool front=false) {
+        pFunctionPointer_t pf = front ? _fall.add_front(tptr, mptr) : _fall.add(tptr, mptr);
+        gpio_irq_set(&gpio_irq, IRQ_FALL, 1);
+        return pf;
+    }
+
     gpio_t gpio;
     gpio_irq_t gpio_irq;
 
-    FunctionPointer _rise;
+    CallChain _rise;
-    FunctionPointer _fall;
+    CallChain _fall;
 };
 
 } // namespace mbed

libraries/mbed/api/InterruptManager.h

@@ -0,0 +1,68 @@
+#ifndef MBED_INTERRUPTMANAGER_H
+#define MBED_INTERRUPTMANAGER_H
+
+#include "cmsis_nvic.h"
+#include "CallChain.h"
+#include <string.h>
+
+namespace mbed {
+
+class InterruptManager {
+public:
+    static InterruptManager* get();
+    static void destroy();
+
+    pFunctionPointer_t add_handler(void (*function)(void), IRQn_Type irq) {
+        return add_common(function, irq);
+    }
+    pFunctionPointer_t add_handler_front(void (*function)(void), IRQn_Type irq) {
+        return add_common(function, irq, true);
+    }
+
+    template<typename T>
+    pFunctionPointer_t add_handler(T* tptr, void (T::*mptr)(void), IRQn_Type irq) {
+        return add_common(tptr, mptr, irq);
+    }
+    template<typename T>
+    pFunctionPointer_t add_handler_front(T* tptr, void (T::*mptr)(void), IRQn_Type irq) {
+        return add_common(tptr, mptr, irq, true);
+    }
+
+    bool remove_handler(pFunctionPointer_t handler, IRQn_Type irq);
+
+private:
+    InterruptManager();
+    ~InterruptManager();
+
+    // We declare the copy contructor and the assignment operator, but we don't
+    // implement them. This way, if someone tries to copy/assign our instance,
+    // he will get an error at compile time.
+    InterruptManager(const InterruptManager&);
+    InterruptManager& operator =(const InterruptManager&);
+
+    template<typename T>
+    pFunctionPointer_t add_common(T *tptr, void (T::*mptr)(void), IRQn_Type irq, bool front=false) {
+        int irq_pos = get_irq_index(irq);
+        bool change = must_replace_vector(irq);
+
+        pFunctionPointer_t pf = front ? _chains[irq_pos]->add_front(tptr, mptr) : _chains[irq_pos]->add(tptr, mptr);
+        if (change)
+            NVIC_SetVector(irq, (uint32_t)&InterruptManager::static_irq_helper);
+        return pf;
+    }
+
+    pFunctionPointer_t add_common(void (*function)(void), IRQn_Type irq, bool front=false);
+    bool must_replace_vector(IRQn_Type irq);
+    int get_irq_index(IRQn_Type irq);
+    void irq_helper();
+    void add_helper(void (*function)(void), IRQn_Type irq, bool front=false);
+    static void static_irq_helper();
+
+    CallChain* _chains[NVIC_NUM_VECTORS];
+    static InterruptManager* _instance;
+};
+
+} // namespace mbed
+
+#endif
+

libraries/mbed/api/Serial.h

@@ -23,6 +23,7 @@
 #include "Stream.h"
 #include "FunctionPointer.h"
 #include "serial_api.h"
+#include "CallChain.h"
 
 namespace mbed {
 
@@ -82,7 +83,7 @@ public:
      *  @param parity The parity used (Serial::None, Serial::Odd, Serial::Even, Serial::Forced1, Serial::Forced0; default = Serial::None)
      *  @param stop The number of stop bits (1 or 2; default = 1)
      */
-    void format(int bits = 8, Parity parity=Serial::None, int stop_bits=1);
+    void format(int bits=8, Parity parity=Serial::None, int stop_bits=1);
 
     /** Determine if there is a character available to read
      *
@@ -105,7 +106,15 @@ public:
      *  @param fptr A pointer to a void function, or 0 to set as none
      *  @param type Which serial interrupt to attach the member function to (Seriall::RxIrq for receive, TxIrq for transmit buffer empty)
      */
-    void attach(void (*fptr)(void), IrqType type=RxIrq);
+    pFunctionPointer_t attach(void (*fptr)(void), IrqType type=RxIrq);
+
+    pFunctionPointer_t add_handler(void (*fptr)(void), IrqType type=RxIrq) {
+        return add_handler_helper(fptr, type);
+    }
+
+    pFunctionPointer_t add_handler_front(void (*fptr)(void), IrqType type=RxIrq) {
+        return add_handler_helper(fptr, type, true);
+    }
 
     /** Attach a member function to call whenever a serial interrupt is generated
      *
@@ -114,12 +123,27 @@ public:
      *  @param type Which serial interrupt to attach the member function to (Seriall::RxIrq for receive, TxIrq for transmit buffer empty)
      */
     template<typename T>
-    void attach(T* tptr, void (T::*mptr)(void), IrqType type=RxIrq) {
+    pFunctionPointer_t attach(T* tptr, void (T::*mptr)(void), IrqType type=RxIrq) {
         if((mptr != NULL) && (tptr != NULL)) {
-            _irq[type].attach(tptr, mptr);
+            _irq[type].clear();
+            pFunctionPointer_t pf = _irq[type].add(tptr, mptr);
             serial_irq_set(&_serial, (SerialIrq)type, 1);
+            return pf;
         }
+        else
+            return NULL;
     }
+    template<typename T>
+    pFunctionPointer_t add_handler(T* tptr, void (T::*mptr)(void), IrqType type=RxIrq) {
+        return add_handler_helper(tptr, mptr, type);
+    }
+
+    template<typename T>
+    pFunctionPointer_t add_handler_front(T* tptr, void (T::*mptr)(void), IrqType type=RxIrq) {
+        return add_handler_helper(tptr, mptr, type, true);
+    }
+
+    bool remove_handler(pFunctionPointer_t pf, IrqType type=RxIrq);
 
     /** Generate a break condition on the serial line
      */
@@ -130,9 +154,21 @@ public:
 protected:
     virtual int _getc();
     virtual int _putc(int c);
+    pFunctionPointer_t add_handler_helper(void (*function)(void), IrqType type, bool front=false);
+
+    template<typename T>
+    pFunctionPointer_t add_handler_helper(T* tptr, void (T::*mptr)(void), IrqType type, bool front=false) {
+        if ((mptr != NULL) && (tptr != NULL)) {
+            pFunctionPointer_t pf = front ? _irq[type].add_front(tptr, mptr) : _irq[type].add(tptr, mptr);
+            serial_irq_set(&_serial, (SerialIrq)type, 1);
+            return pf;
+        }
+        else
+            return NULL;
+    }
 
     serial_t        _serial;
-    FunctionPointer _irq[2];
+    CallChain       _irq[2];
     int             _baud;
 };
 

libraries/mbed/api/Ticker.h

@@ -18,6 +18,7 @@
 
 #include "TimerEvent.h"
 #include "FunctionPointer.h"
+#include "CallChain.h"
 
 namespace mbed {
 
@@ -63,8 +64,16 @@ public:
      *  @param fptr pointer to the function to be called
      *  @param t the time between calls in seconds
      */
-    void attach(void (*fptr)(void), float t) {
+    pFunctionPointer_t attach(void (*fptr)(void), float t) {
-        attach_us(fptr, t * 1000000.0f);
+        return attach_us(fptr, t * 1000000.0f);
+    }
+
+    pFunctionPointer_t add_function(void (*fptr)(void)) {
+        return add_function_helper(fptr);
+    }
+
+    pFunctionPointer_t add_function_front(void (*fptr)(void)) {
+        return add_function_helper(fptr, true);
     }
 
     /** Attach a member function to be called by the Ticker, specifiying the interval in seconds
@@ -74,8 +83,18 @@ public:
      *  @param t the time between calls in seconds
      */
     template<typename T>
-    void attach(T* tptr, void (T::*mptr)(void), float t) {
+    pFunctionPointer_t attach(T* tptr, void (T::*mptr)(void), float t) {
-        attach_us(tptr, mptr, t * 1000000.0f);
+        return attach_us(tptr, mptr, t * 1000000.0f);         
+    }
+
+    template<typename T>
+    pFunctionPointer_t add_function(T* tptr, void (T::*mptr)(void)) {
+        return add_function_helper(tptr, mptr);
+    }
+
+    template<typename T>
+    pFunctionPointer_t add_function_front(T* tptr, void (T::*mptr)(void)) {
+        return add_function_helper(tptr, mptr, true);
     }
 
     /** Attach a function to be called by the Ticker, specifiying the interval in micro-seconds
@@ -83,9 +102,10 @@ public:
      *  @param fptr pointer to the function to be called
      *  @param t the time between calls in micro-seconds
      */
-    void attach_us(void (*fptr)(void), unsigned int t) {
+    pFunctionPointer_t attach_us(void (*fptr)(void), unsigned int t) {
-        _function.attach(fptr);
+        pFunctionPointer_t pf = _chain.add(fptr);
         setup(t);
+        return pf;
     }
 
     /** Attach a member function to be called by the Ticker, specifiying the interval in micro-seconds
@@ -95,21 +115,37 @@ public:
      *  @param t the time between calls in micro-seconds
      */
     template<typename T>
-    void attach_us(T* tptr, void (T::*mptr)(void), unsigned int t) {
+    pFunctionPointer_t attach_us(T* tptr, void (T::*mptr)(void), unsigned int t) {
-        _function.attach(tptr, mptr);
+        pFunctionPointer_t pf = _chain.add(mptr, tptr);
         setup(t);
+        return pf;
     }
 
     /** Detach the function
      */
     void detach();
 
+    bool remove_function(pFunctionPointer_t pf) {
+        bool res = _chain.remove(pf);
+        if (res && _chain.size() == 0)
+            detach();
+        return res;
+    }
+
 protected:
     void setup(unsigned int t);
+    pFunctionPointer_t add_function_helper(void (*fptr)(void), bool front=false);
     virtual void handler();
 
+    template<typename T>
+    pFunctionPointer_t add_function_helper(T* tptr, void (T::*mptr)(void), bool front=false) {
+        if (_chain.size() == 0)
+            return NULL;
+        return front ? _chain.add_front(tptr, mptr) : _chain.add(tptr, mptr);
+    }
+
     unsigned int _delay;
-    FunctionPointer _function;
+    CallChain _chain;
 };
 
 } // namespace mbed

libraries/mbed/common/CallChain.cpp

@@ -0,0 +1,96 @@
+#include "CallChain.h"
+#include "cmsis.h"
+
+namespace mbed {
+
+CallChain::CallChain(int size) : _size(size), _elements(0) {
+    _chain = new pFunctionPointer_t[size]();
+}
+
+CallChain::~CallChain() {
+    clear();
+    delete _chain;    
+}
+
+pFunctionPointer_t CallChain::add(void (*function)(void)) {
+    return common_add(new FunctionPointer(function));
+}
+
+pFunctionPointer_t CallChain::add_front(void (*function)(void)) {
+    return common_add_front(new FunctionPointer(function));
+}
+
+int CallChain::size() const {
+    return _elements;
+}
+
+pFunctionPointer_t CallChain::get(int i) const {
+    if (i < 0 || i >= _elements)
+        return NULL;
+    return _chain[i];
+}
+
+int CallChain::find(pFunctionPointer_t f) const {
+    for (int i = 0; i < _elements; i++)
+        if (f == _chain[i])
+            return i;
+    return -1;
+}
+
+void CallChain::clear() {
+    __disable_irq();
+    for(int i = 0; i < _elements; i ++) {
+        delete _chain[i];
+        _chain[i] = NULL;
+    }
+    _elements = 0;
+    __enable_irq();
+}
+
+bool CallChain::remove(pFunctionPointer_t f) {
+    int i;
+
+    if ((i = find(f)) == -1)
+        return false;
+    __disable_irq();
+    if (i != _elements - 1)
+        memmove(_chain + i, _chain + i + 1, (_elements - i - 1) * sizeof(pFunctionPointer_t));
+    delete f;
+    _elements --;
+    __enable_irq();
+    return true;
+}
+
+void CallChain::call() {
+    for(int i = 0; i < _elements; i++)
+        _chain[i]->call();
+}
+
+void CallChain::_check_size() {
+    if (_elements < _size)
+        return;
+    __disable_irq();
+    _size = (_size < 4) ? 4 : _size + 4;
+    pFunctionPointer_t* new_chain = new pFunctionPointer_t[_size]();
+    memcpy(new_chain, _chain, _elements * sizeof(pFunctionPointer_t));
+    delete _chain;
+    _chain = new_chain;
+    __enable_irq();
+}
+
+pFunctionPointer_t CallChain::common_add(pFunctionPointer_t pf) {
+    _check_size();
+    _chain[_elements] = pf;
+    _elements ++;
+    return pf;
+}
+
+pFunctionPointer_t CallChain::common_add_front(pFunctionPointer_t pf) {
+    _check_size();
+    memmove(_chain + 1, _chain, _elements * sizeof(pFunctionPointer_t));
+    _chain[0] = pf;
+    _elements ++;
+    return pf;
+}
+
+} // namespace mbed

libraries/mbed/common/FunctionPointer.cpp

@@ -34,4 +34,10 @@ void FunctionPointer::call(void) {
     }
 }
 
+#ifdef MBED_OPERATORS
+void FunctionPointer::operator ()(void) {
+    call();
+}
+#endif
+
 } // namespace mbed

libraries/mbed/common/InterruptIn.cpp

@@ -36,22 +36,58 @@ void InterruptIn::mode(PinMode pull) {
     gpio_mode(&gpio, pull);
 }
 
-void InterruptIn::rise(void (*fptr)(void)) {
+pFunctionPointer_t InterruptIn::rise(void (*fptr)(void)) {
+    pFunctionPointer_t pf = NULL;
+    _rise.clear();
     if (fptr) {
-        _rise.attach(fptr);
+        pf = _rise.add(fptr);
         gpio_irq_set(&gpio_irq, IRQ_RISE, 1);
     } else {
         gpio_irq_set(&gpio_irq, IRQ_RISE, 0);
     }
+    return pf;
 }
 
-void InterruptIn::fall(void (*fptr)(void)) {
+pFunctionPointer_t InterruptIn::rise_add_common(void (*fptr)(void), bool front) {
+    if (NULL == fptr)
+        return NULL;
+    pFunctionPointer_t pf = front ? _rise.add_front(fptr) : _rise.add(fptr);
+    gpio_irq_set(&gpio_irq, IRQ_RISE, 1);
+    return pf;
+}
+
+bool InterruptIn::rise_remove(pFunctionPointer_t pf) {
+    bool res = _rise.remove(pf);
+    if (res && _rise.size() == 0)
+        gpio_irq_set(&gpio_irq, IRQ_RISE, 0);
+    return res;
+}
+
+pFunctionPointer_t InterruptIn::fall(void (*fptr)(void)) {
+    pFunctionPointer_t pf = NULL;
+    _fall.clear();
     if (fptr) {
-        _fall.attach(fptr);
+        pf = _fall.add(fptr);
         gpio_irq_set(&gpio_irq, IRQ_FALL, 1);
     } else {
         gpio_irq_set(&gpio_irq, IRQ_FALL, 0);
     }
+    return pf;
+}
+
+pFunctionPointer_t InterruptIn::fall_add_common(void (*fptr)(void), bool front) {
+    if (NULL == fptr)
+        return NULL;
+    pFunctionPointer_t pf = front ? _fall.add_front(fptr) : _fall.add(fptr);
+    gpio_irq_set(&gpio_irq, IRQ_FALL, 1);
+    return pf;
+}
+
+bool InterruptIn::fall_remove(pFunctionPointer_t pf) {
+    bool res = _fall.remove(pf);
+    if (res && _fall.size() == 0)
+        gpio_irq_set(&gpio_irq, IRQ_FALL, 0);
+    return res;
 }
 
 void InterruptIn::_irq_handler(uint32_t id, gpio_irq_event event) {

libraries/mbed/common/InterruptManager.cpp

@@ -0,0 +1,89 @@
+#include "InterruptManager.h"
+#include <string.h>
+
+#define CHAIN_INITIAL_SIZE    4
+
+namespace mbed {
+
+typedef void (*pvoidf)(void);
+
+InterruptManager* InterruptManager::_instance = NULL;
+
+InterruptManager* InterruptManager::get() {
+    if (NULL == _instance)
+        _instance = new InterruptManager();
+    return _instance;
+}
+
+InterruptManager::InterruptManager() {
+    memset(_chains, 0, NVIC_NUM_VECTORS * sizeof(CallChain*));
+}
+
+void InterruptManager::destroy() {
+    // Not a good idea to call this unless NO interrupt at all
+    // is under the control of the handler; otherwise, a system crash
+    // is very likely to occur
+    if (NULL != _instance) {
+        delete _instance;
+        _instance = NULL;
+    }
+}
+
+InterruptManager::~InterruptManager() {
+    for(int i = 0; i < NVIC_NUM_VECTORS; i++)
+        if (NULL != _chains[i])
+            delete _chains[i];
+}
+
+bool InterruptManager::must_replace_vector(IRQn_Type irq) {
+    int irq_pos = get_irq_index(irq);
+
+    if (NULL == _chains[irq_pos]) {
+        _chains[irq_pos] = new CallChain(CHAIN_INITIAL_SIZE);
+        _chains[irq_pos]->add((pvoidf)NVIC_GetVector(irq));
+        return true;
+    }
+    return false;
+}
+
+pFunctionPointer_t InterruptManager::add_common(void (*function)(void), IRQn_Type irq, bool front) {
+    int irq_pos = get_irq_index(irq);
+    bool change = must_replace_vector(irq);
+
+    pFunctionPointer_t pf = front ? _chains[irq_pos]->add_front(function) : _chains[irq_pos]->add(function);
+    if (change)
+        NVIC_SetVector(irq, (uint32_t)&InterruptManager::static_irq_helper);
+    return pf;
+}
+
+bool InterruptManager::remove_handler(pFunctionPointer_t handler, IRQn_Type irq) {
+    int irq_pos = get_irq_index(irq);
+    
+    if (NULL == _chains[irq_pos])
+        return false;
+    if (!_chains[irq_pos]->remove(handler))
+        return false;
+    // If there's a single function left in the chain, swith the interrupt vector
+    // to call that function directly. This way we save both time and space.
+    if (_chains[irq_pos]->size() == 1 && NULL != _chains[irq_pos]->get(0)->get_function()) {
+        NVIC_SetVector(irq, (uint32_t)_chains[irq_pos]->get(0)->get_function());
+        delete _chains[irq_pos];
+        _chains[irq_pos] = NULL;
+    }
+    return true;
+}
+
+void InterruptManager::irq_helper() {
+    _chains[get_irq_index(NVIC_GetActiveInterrupt())]->call();
+}
+
+int InterruptManager::get_irq_index(IRQn_Type irq) {
+    return (int)irq + NVIC_USER_IRQ_OFFSET;
+}
+
+void InterruptManager::static_irq_helper() {
+    InterruptManager::get()->irq_helper();
+}
+
+} // namespace mbed
+

libraries/mbed/common/Serial.cpp

@@ -44,15 +44,32 @@ int Serial::writeable() {
     return serial_writable(&_serial);
 }
 
-void Serial::attach(void (*fptr)(void), IrqType type) {
+pFunctionPointer_t Serial::attach(void (*fptr)(void), IrqType type) {
+    pFunctionPointer_t pf = NULL;
+    _irq[type].clear();
     if (fptr) {
-        _irq[type].attach(fptr);
+        pf = _irq[type].add(fptr);
         serial_irq_set(&_serial, (SerialIrq)type, 1);
     } else {
         serial_irq_set(&_serial, (SerialIrq)type, 0);
     }
+    return pf;
 }
 
+pFunctionPointer_t Serial::add_handler_helper(void (*fptr)(void), IrqType type, bool front) {
+    if (NULL == fptr)
+        return NULL;
+    pFunctionPointer_t pf = front ? _irq[type].add_front(fptr) : _irq[type].add(fptr);
+    serial_irq_set(&_serial, (SerialIrq)type, 1);
+    return pf;
+}
+
+bool Serial::remove_handler(pFunctionPointer_t pf, IrqType type) {
+    bool res = _irq[type].remove(pf);
+    if (res && _irq[type].size() == 0)
+        serial_irq_set(&_serial, (SerialIrq)type, 0);
+    return res;
+}
 
 void Serial::_irq_handler(uint32_t id, SerialIrq irq_type) {
     Serial *handler = (Serial*)id;

libraries/mbed/common/Ticker.cpp

@@ -22,7 +22,7 @@ namespace mbed {
 
 void Ticker::detach() {
     remove();
-    _function.attach(0);
+    _chain.clear();
 }
 
 void Ticker::setup(unsigned int t) {
@@ -33,7 +33,13 @@ void Ticker::setup(unsigned int t) {
 
 void Ticker::handler() {
     insert(event.timestamp + _delay);
-    _function.call();
+    _chain.call();
+}
+
+pFunctionPointer_t Ticker::add_function_helper(void (*fptr)(void), bool front) {
+    if (_chain.size() == 0)
+        return NULL;
+    return front ? _chain.add_front(fptr) : _chain.add(fptr);
 }
 
 } // namespace mbed

libraries/mbed/common/Timeout.cpp

@@ -18,7 +18,7 @@
 namespace mbed {
 
 void Timeout::handler() {
-    _function.call();
+    _chain.call();
 }
 
 } // namespace mbed

libraries/mbed/targets/cmsis/TARGET_NXP/TARGET_LPC176X/cmsis_nvic.c

@@ -5,7 +5,6 @@
  */ 
 #include "cmsis_nvic.h"
 
-#define NVIC_NUM_VECTORS          (16 + 33)     // CORE + MCU Peripherals
 #define NVIC_RAM_VECTOR_ADDRESS   (0x10000000)  // Location of vectors in RAM
 #define NVIC_FLASH_VECTOR_ADDRESS (0x0)       // Initial vector position in flash
 
@@ -22,11 +21,14 @@ void NVIC_SetVector(IRQn_Type IRQn, uint32_t vector) {
         }
         SCB->VTOR = (uint32_t)NVIC_RAM_VECTOR_ADDRESS;
     }
-    vectors[IRQn + 16] = vector;
+    vectors[IRQn + NVIC_USER_IRQ_OFFSET] = vector;
 }
 
 uint32_t NVIC_GetVector(IRQn_Type IRQn) {
     uint32_t *vectors = (uint32_t*)SCB->VTOR;
-    return vectors[IRQn + 16];
+    return vectors[IRQn + NVIC_USER_IRQ_OFFSET];
 }
 
+IRQn_Type NVIC_GetActiveInterrupt(void) {
+    return (IRQn_Type)((SCB->ICSR & 0x1FF) - NVIC_USER_IRQ_OFFSET);
+}

libraries/mbed/targets/cmsis/TARGET_NXP/TARGET_LPC176X/cmsis_nvic.h

@@ -9,12 +9,16 @@
 
 #include "cmsis.h"
 
+#define NVIC_NUM_VECTORS      (16 + 33)
+#define NVIC_USER_IRQ_OFFSET  16
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 void NVIC_SetVector(IRQn_Type IRQn, uint32_t vector);
 uint32_t NVIC_GetVector(IRQn_Type IRQn);
+IRQn_Type NVIC_GetActiveInterrupt(void);
 
 #ifdef __cplusplus
 }

libraries/tests/mbed/interrupt_chaining/interruptin/main.cpp

@@ -0,0 +1,113 @@
+#include "test_env.h"
+
+DigitalOut myled(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+volatile int checks = 0;
+volatile int total = 0;
+void in_handler() {
+    checks++;
+    led2 = !led2;
+}
+
+#if defined(TARGET_KL25Z)
+#define PIN_OUT     PTC7
+#define PIN_IN      PTA1
+
+#elif defined(TARGET_KL05Z)
+#define PIN_OUT     PTB11
+#define PIN_IN      PTB1
+
+#elif defined(TARGET_LPC812)
+#define PIN_OUT     D10
+#define PIN_IN      D11
+
+#else
+#define PIN_IN      (p5)
+#define PIN_OUT     (p25)
+
+#endif
+
+DigitalOut out(PIN_OUT);
+InterruptIn in(PIN_IN);
+
+void flipper() {
+    for (int i = 0; i < 5; i++) {
+        out = 1; myled = 1; wait(0.2);
+
+        out = 0; myled = 0; wait(0.2);
+    }
+}
+
+void flipper2() {
+    led3 = !led3;
+}
+
+void handler2() {
+    total++;
+}
+
+int object_cnt = 0;
+
+class Incrementer {
+public:
+    Incrementer() : _cnt(0) {}
+    void inc() { _cnt++; }
+    int get() const { return _cnt; }
+private:
+    int _cnt;
+};
+
+int main() {
+    out = 0; myled = 0;
+
+    //Test falling edges first
+    Incrementer i1;
+    in.rise(NULL);
+    in.fall(in_handler);
+    in.fall_add(handler2);
+    in.fall_add(flipper2);
+    in.fall_add_front(&i1, &Incrementer::inc);
+    flipper();
+
+    if(checks != 5 || i1.get() != 5) {
+        printf("falling edges test failed\n");
+        notify_completion(false);
+    }
+
+    //Now test rising edges
+    Incrementer i2;
+    in.rise(in_handler);
+    in.rise_add(handler2);
+    in.rise_add(&i2, &Incrementer::inc);
+    in.rise_add_front(flipper2);
+    in.fall(NULL);
+    flipper();
+
+    if (checks != 10 || i2.get() != 5) {
+        printf("raising edges test failed\n");
+        notify_completion(false);
+    }
+
+    //Finally test both
+    in.rise(in_handler);
+    in.rise_add(handler2);
+    pFunctionPointer_t rise_led = in.rise_add(flipper2);
+    in.fall(in_handler);
+    in.fall_add(handler2);
+    pFunctionPointer_t fall_led = in.fall_add(flipper2);
+    if (!in.rise_remove(rise_led) ||  !in.fall_remove(fall_led)) {
+        printf("remove handler failed\n");
+        notify_completion(false);
+    }
+    flipper();
+
+    if (checks != 20) {
+        printf("Simultaneous rising and falling edges failed! check=%d\n", checks);
+        notify_completion(false);
+    }
+    printf("Total: %d\n", total);
+    notify_completion(true);
+    return 0;
+}

libraries/tests/mbed/interrupt_chaining/serial_interrupt/main.cpp

@@ -0,0 +1,41 @@
+#include "mbed.h"
+ 
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+Serial computer(USBTX, USBRX);
+ 
+// This function is called when a character goes into the TX buffer.
+void txCallback() {
+    led1 = !led1;
+}
+ 
+// This function is called when a character goes into the RX buffer.
+void rxCallback() {
+    led2 = !led2;
+    computer.putc(computer.getc());
+}
+
+class Counter {
+public:
+    Counter(const char* name): _name(name), _cnt(0) {}
+    void inc() { _cnt++; }
+    void show() const { printf("%s: %d\n", _name, _cnt); }
+    int get() const { return _cnt; }
+    ~Counter() { show(); }
+private:
+    const char *_name;
+    volatile int _cnt;
+};
+ 
+int main() {
+    printf("start test\n");
+    Counter rx("RX bytes"), tx("TX bytes");
+
+    computer.attach(&txCallback, Serial::TxIrq);
+    computer.add_handler(&tx, &Counter::inc, Serial::TxIrq);
+    computer.attach(&rxCallback, Serial::RxIrq);
+    computer.add_handler_front(&rx, &Counter::inc, Serial::RxIrq);
+
+    while (rx.get() < 40);
+}

libraries/tests/mbed/interrupt_chaining/ticker/main.cpp

@@ -0,0 +1,101 @@
+#include "mbed.h"
+#include "InterruptManager.h"
+#include "cmsis.h"
+#include "test_env.h"
+
+Serial pc(USBTX, USBRX);
+
+Ticker flipper_1;
+DigitalOut led1(LED1);
+int led1_state = 0;
+void flip_1() {
+    if (led1_state) {
+        led1 = 0; led1_state = 0;
+    } else {
+        led1 = 1; led1_state = 1;
+    }
+}
+
+class Sender {
+public:
+    Sender(Serial&s, char c): _s(s), _c(c) {}
+    void send() { _s.putc(_c); }
+private:
+    Serial& _s;
+    char _c;
+};
+Ticker flipper_2;
+Sender s1(pc, '1');
+Sender s2(pc, '2');
+
+#if defined(TARGET_LPC1768) || defined(TARGET_LPC11U24) || defined(TARGET_LPC4088)
+#   define LED_NAME LED2
+#elif defined(TARGET_KL05Z)
+#   define LED_NAME LED2
+#else
+#   define LED_NAME PTE31
+#endif
+
+DigitalOut led2(LED_NAME);
+int led2_state = 0;
+void flip_2() {
+    if (led2_state) {
+        led2 = 0; led2_state = 0;
+    } else {
+        led2 = 1; led2_state = 1;
+    }
+}
+
+void testme(void) {
+    pc.putc('!');
+}
+
+class Counter {
+public:
+    void inc(void) {
+        count ++;
+    }
+    int get_count(void) const {
+        return count;
+    }
+private:
+    static int count;
+};
+
+int Counter::count = 0;
+
+int main() {
+    led1 = 0;
+    led2 = 0;
+    uint32_t initial_handler, final_handler;
+    Counter c;
+
+    // Test chaining inside Serial class
+    flipper_1.attach(&flip_1, 1.0); // the address of the function to be attached (flip) and the interval (1 second)
+    flipper_1.add_function_front(&s1, &Sender::send);
+    flipper_2.attach(&flip_2, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds)
+    flipper_2.add_function(&s2, &Sender::send);
+
+    // Test global chaining (InterruptManager)
+    printf("Handler initially: %08X\n", initial_handler = NVIC_GetVector(TIMER3_IRQn));
+    InterruptManager *pManager = InterruptManager::get();
+    pFunctionPointer_t ptm = pManager->add_handler(testme, TIMER3_IRQn);
+    pFunctionPointer_t pinc = pManager->add_handler_front(&c, &Counter::inc, TIMER3_IRQn);
+    printf("Handler after calling InterruptManager: %08X\n", NVIC_GetVector(TIMER3_IRQn));
+
+    wait(4.0);
+
+    if (!pManager->remove_handler(ptm, TIMER3_IRQn) || !pManager->remove_handler(pinc, TIMER3_IRQn)) {
+        printf ("remove handler failed.\n");
+        notify_completion(false);
+    }
+    printf("Interrupt handler calls: %d\n", c.get_count());
+    printf("Handler after removing previously added functions: %08X\n", final_handler = NVIC_GetVector(TIMER3_IRQn));
+
+    if (initial_handler != final_handler) {
+        printf( "InteruptManager test failed.\n");
+        notify_completion(false);
+    }
+
+    while(1);
+}