File content as of revision 15:92d7c0b8a0f5:

#include "EventQueue.h"
#include "Event.h"
#include "mbed.h"

#ifndef EVENTS_NO_RTOS
#include "rtos.h"
#endif


// Platform specific definitions
static inline unsigned irq_disable() {
    unsigned primask = __get_PRIMASK();
    __disable_irq();
    return primask;
}

static inline void irq_enable(unsigned primask) {
    __set_PRIMASK(primask);
}

static inline void irq_wfi() {
#ifndef EVENTS_NO_RTOS
    Thread::yield();
#else
    __WFI();
#endif
}


// Event queue definitions
EventQueue::EventQueue(unsigned event_count, unsigned event_context) {
    _event_context = sizeof(FuncPtr<void()>) + event_context;
    unsigned event_size = sizeof(struct event) + _event_context;
    _mem = malloc(event_count * event_size);
    _free = (struct event*)_mem;

    if (_mem) {
        for (unsigned i = 0; i < event_count-1; i++) {
            ((struct event*)((char*)_mem + i*event_size))->next =
                    (struct event*)((char*)_mem + (i+1)*event_size);
        }
        ((struct event*)((char*)_mem + (event_count-1)*event_size))->next = 0;
    }

    _queue = 0;
    _tick = 0;
    _timer.start();
    _ticker.attach_us(this, &EventQueue::tick, (1 << 16) * 1000);
}

EventQueue::~EventQueue() {
    free(_mem);
}

unsigned EventQueue::get_tick() {
    return _tick + (unsigned)_timer.read_ms();
}

void EventQueue::tick() {
    _timer.reset();
    _tick += 1 << 16;
}

void EventQueue::wakeup() {

}

void EventQueue::exit(volatile bool *exit) {
    *exit = true;
}

void EventQueue::dispatch(int ms) {
    Timeout timeout;
    volatile bool exit = false;
    
    if (ms >= 0) {
        Event<void(volatile bool*)> e(this, this, &EventQueue::exit);
        e.delay(ms);
        e.trigger(&exit);
    }

    while (true) {
        while (_queue) {
            int diff = (int)(_queue->target - get_tick());
            if (diff > 0) {
                timeout.attach_us(this, &EventQueue::wakeup, diff * 1000);
                break;
            }

            struct event *volatile e = _queue;
            _queue = _queue->next;

            e->dispatch(reinterpret_cast<void *>(e + 1));

            if (e->period >= 0) {
                trigger(e, e->period);
            } else {
                dealloc(e);
            }
        }

        if (exit) {
            break;
        }

        irq_wfi();
    }
}

void EventQueue::trigger(struct event *e, int delay) {
    e->target = get_tick() + (unsigned)delay;

    unsigned primask = irq_disable();
    struct event *volatile *p = &_queue;
    while (*p && (*p)->target < e->target) {
        p = &(*p)->next;
    }

    e->next = *p;
    *p = e;
    irq_enable(primask);
}

EventQueue::event *EventQueue::alloc(unsigned size, int ms) {
    if (size > _event_context) {
        return 0;
    }

    struct event *e;

    unsigned primask = irq_disable();
    e = _free;
    if (e) {
        _free = e->next;
    }
    irq_enable(primask);

    return e;
}

void EventQueue::dealloc(struct event *e) {
    unsigned primask = irq_disable();
    e->next = _free;
    _free = e;
    irq_enable(primask);
}