File content as of revision 20:2f9d9c53a5af:

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


// 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);
}


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

    if (!event_pointer) {
        _mem = malloc(event_count * event_size);
        _free = (struct event*)_mem;
    } else {
        _mem = 0;
        _free = (struct event*)event_pointer;
    }

    if (_free) {
        for (unsigned i = 0; i < event_count-1; i++) {
            ((struct event*)((char*)_free + i*event_size))->next =
                    (struct event*)((char*)_free + (i+1)*event_size);
        }
        ((struct event*)((char*)_free + (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();
}

bool EventQueue::past_tick(unsigned tick) {
    return static_cast<int>(tick - get_tick()) <= 0;
}

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

void EventQueue::dispatch(int ms) {
    unsigned target = get_tick() + (unsigned)ms;

    while (true) {
        while (_queue) {
            if (!past_tick(_queue->target)) {
                break;
            }

            unsigned primask = irq_disable();
            struct event *volatile e = _queue;
            _queue = _queue->next;
            irq_enable(primask);

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

            if (e->period >= 0) {
                event_trigger(e, e->period);
            } else {
                event_dealloc(e);
            }
        }

        if (ms >= 0 && past_tick(target)) {
            return;
        }

        osStatus status = Thread::yield();
        if (status != osOK) {
            return;
        }
    }
}

void EventQueue::event_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::event_alloc(unsigned size, int ms) {
    if (size > _event_context) {
        return 0;
    }

    unsigned target = get_tick() + (unsigned)ms;
    struct event *e;

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

        if (e || (ms >= 0 && past_tick(target))) {
            return e;
        }

        osStatus status = Thread::yield();
        if (status != osOK) {
            return e;
        }
    }
}

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