EventQueue.cpp

00001 #include "EventQueue.h"
00002 #include "Event.h"
00003 #include "mbed.h"
00004 #include "rtos.h"
00005 
00006 
00007 // Platform specific definitions
00008 static inline unsigned irq_disable() {
00009     unsigned primask = __get_PRIMASK();
00010     __disable_irq();
00011     return primask;
00012 }
00013 
00014 static inline void irq_enable(unsigned primask) {
00015     __set_PRIMASK(primask);
00016 }
00017 
00018 
00019 // Event queue definitions
00020 EventQueue::EventQueue(unsigned event_count,
00021                        unsigned event_context, 
00022                        unsigned char *event_pointer) {
00023     _event_context = sizeof(FuncPtr<void()>) + event_context;
00024     unsigned event_size = sizeof(struct event) + _event_context;
00025 
00026     if (!event_pointer) {
00027         _mem = malloc(event_count * event_size);
00028         _free = (struct event*)_mem;
00029     } else {
00030         _mem = 0;
00031         _free = (struct event*)event_pointer;
00032     }
00033 
00034     if (_free) {
00035         for (unsigned i = 0; i < event_count-1; i++) {
00036             ((struct event*)((char*)_free + i*event_size))->next =
00037                     (struct event*)((char*)_free + (i+1)*event_size);
00038         }
00039         ((struct event*)((char*)_free + (event_count-1)*event_size))->next = 0;
00040     }
00041 
00042     _queue = 0;
00043     _tick = 0;
00044     _timer.start();
00045     _ticker.attach_us(this, &EventQueue::tick, (1 << 16) * 1000);
00046 }
00047 
00048 EventQueue::~EventQueue() {
00049     free(_mem);
00050 }
00051 
00052 unsigned EventQueue::get_tick() {
00053     return _tick + (unsigned)_timer.read_ms();
00054 }
00055 
00056 bool EventQueue::past_tick(unsigned tick) {
00057     return static_cast<int>(tick - get_tick()) <= 0;
00058 }
00059 
00060 void EventQueue::tick() {
00061     _timer.reset();
00062     _tick += 1 << 16;
00063 }
00064 
00065 void EventQueue::dispatch(int ms) {
00066     unsigned target = get_tick() + (unsigned)ms;
00067 
00068     while (true) {
00069         while (_queue) {
00070             if (!past_tick(_queue->target)) {
00071                 break;
00072             }
00073 
00074             unsigned primask = irq_disable();
00075             struct event *volatile e = _queue;
00076             _queue = _queue->next;
00077             irq_enable(primask);
00078 
00079             e->dispatch(reinterpret_cast<void *>(e + 1));
00080 
00081             if (e->period >= 0) {
00082                 event_trigger(e, e->period);
00083             } else {
00084                 event_dealloc(e);
00085             }
00086         }
00087 
00088         if (ms >= 0 && past_tick(target)) {
00089             return;
00090         }
00091 
00092         osStatus status = Thread::yield();
00093         if (status != osOK) {
00094             return;
00095         }
00096     }
00097 }
00098 
00099 void EventQueue::event_trigger(struct event *e, int delay) {
00100     e->target = get_tick() + (unsigned)delay;
00101 
00102     unsigned primask = irq_disable();
00103     struct event *volatile *p = &_queue;
00104     while (*p && (*p)->target < e->target) {
00105         p = &(*p)->next;
00106     }
00107 
00108     e->next = *p;
00109     *p = e;
00110     irq_enable(primask);
00111 }
00112 
00113 EventQueue::event *EventQueue::event_alloc(unsigned size, int ms) {
00114     if (size > _event_context) {
00115         return 0;
00116     }
00117 
00118     unsigned target = get_tick() + (unsigned)ms;
00119     struct event *e;
00120 
00121     while (true) {
00122         if (_free) {
00123             unsigned primask = irq_disable();
00124             if (_free) {
00125                 e = _free;
00126                 _free = _free->next;
00127             }
00128             irq_enable(primask);
00129         }
00130 
00131         if (e || (ms >= 0 && past_tick(target))) {
00132             return e;
00133         }
00134 
00135         osStatus status = Thread::yield();
00136         if (status != osOK) {
00137             return e;
00138         }
00139     }
00140 }
00141 
00142 void EventQueue::event_dealloc(struct event *e) {
00143     unsigned primask = irq_disable();
00144     e->next = _free;
00145     _free = e;
00146     irq_enable(primask);
00147 }
00148