Rizky Ardi Maulana / mbed-os

mbed os rtos

events/equeue/equeue.c@0:f269e3021894, 2016-10-23 (annotated)

Committer:
elessair
Date:
Sun Oct 23 15:10:02 2016 +0000
Revision:
0:f269e3021894
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elessair 0:f269e3021894 1 /*
elessair 0:f269e3021894 2 * Flexible event queue for dispatching events
elessair 0:f269e3021894 3 *
elessair 0:f269e3021894 4 * Copyright (c) 2016 Christopher Haster
elessair 0:f269e3021894 5 *
elessair 0:f269e3021894 6 * Licensed under the Apache License, Version 2.0 (the "License");
elessair 0:f269e3021894 7 * you may not use this file except in compliance with the License.
elessair 0:f269e3021894 8 * You may obtain a copy of the License at
elessair 0:f269e3021894 9 *
elessair 0:f269e3021894 10 * http://www.apache.org/licenses/LICENSE-2.0
elessair 0:f269e3021894 11 *
elessair 0:f269e3021894 12 * Unless required by applicable law or agreed to in writing, software
elessair 0:f269e3021894 13 * distributed under the License is distributed on an "AS IS" BASIS,
elessair 0:f269e3021894 14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
elessair 0:f269e3021894 15 * See the License for the specific language governing permissions and
elessair 0:f269e3021894 16 * limitations under the License.
elessair 0:f269e3021894 17 */
elessair 0:f269e3021894 18 #include "equeue/equeue.h"
elessair 0:f269e3021894 19
elessair 0:f269e3021894 20 #include <stdlib.h>
elessair 0:f269e3021894 21 #include <string.h>
elessair 0:f269e3021894 22
elessair 0:f269e3021894 23
elessair 0:f269e3021894 24 // calculate the relative-difference between absolute times while
elessair 0:f269e3021894 25 // correctly handling overflow conditions
elessair 0:f269e3021894 26 static inline int equeue_tickdiff(unsigned a, unsigned b) {
elessair 0:f269e3021894 27 return (int)(unsigned)(a - b);
elessair 0:f269e3021894 28 }
elessair 0:f269e3021894 29
elessair 0:f269e3021894 30 // calculate the relative-difference between absolute times, but
elessair 0:f269e3021894 31 // also clamp to zero, resulting in only non-zero values.
elessair 0:f269e3021894 32 static inline int equeue_clampdiff(unsigned a, unsigned b) {
elessair 0:f269e3021894 33 int diff = equeue_tickdiff(a, b);
elessair 0:f269e3021894 34 return ~(diff >> (8*sizeof(int)-1)) & diff;
elessair 0:f269e3021894 35 }
elessair 0:f269e3021894 36
elessair 0:f269e3021894 37 // Increment the unique id in an event, hiding the event from cancel
elessair 0:f269e3021894 38 static inline void equeue_incid(equeue_t *q, struct equeue_event *e) {
elessair 0:f269e3021894 39 e->id += 1;
elessair 0:f269e3021894 40 if (!(e->id << q->npw2)) {
elessair 0:f269e3021894 41 e->id = 1;
elessair 0:f269e3021894 42 }
elessair 0:f269e3021894 43 }
elessair 0:f269e3021894 44
elessair 0:f269e3021894 45
elessair 0:f269e3021894 46 // equeue lifetime management
elessair 0:f269e3021894 47 int equeue_create(equeue_t *q, size_t size) {
elessair 0:f269e3021894 48 // dynamically allocate the specified buffer
elessair 0:f269e3021894 49 void *buffer = malloc(size);
elessair 0:f269e3021894 50 if (!buffer) {
elessair 0:f269e3021894 51 return -1;
elessair 0:f269e3021894 52 }
elessair 0:f269e3021894 53
elessair 0:f269e3021894 54 int err = equeue_create_inplace(q, size, buffer);
elessair 0:f269e3021894 55 q->allocated = buffer;
elessair 0:f269e3021894 56 return err;
elessair 0:f269e3021894 57 }
elessair 0:f269e3021894 58
elessair 0:f269e3021894 59 int equeue_create_inplace(equeue_t *q, size_t size, void *buffer) {
elessair 0:f269e3021894 60 // setup queue around provided buffer
elessair 0:f269e3021894 61 q->buffer = buffer;
elessair 0:f269e3021894 62 q->allocated = 0;
elessair 0:f269e3021894 63
elessair 0:f269e3021894 64 q->npw2 = 0;
elessair 0:f269e3021894 65 for (unsigned s = size; s; s >>= 1) {
elessair 0:f269e3021894 66 q->npw2++;
elessair 0:f269e3021894 67 }
elessair 0:f269e3021894 68
elessair 0:f269e3021894 69 q->chunks = 0;
elessair 0:f269e3021894 70 q->slab.size = size;
elessair 0:f269e3021894 71 q->slab.data = buffer;
elessair 0:f269e3021894 72
elessair 0:f269e3021894 73 q->queue = 0;
elessair 0:f269e3021894 74 q->tick = equeue_tick();
elessair 0:f269e3021894 75 q->generation = 0;
elessair 0:f269e3021894 76 q->breaks = 0;
elessair 0:f269e3021894 77
elessair 0:f269e3021894 78 q->background.active = false;
elessair 0:f269e3021894 79 q->background.update = 0;
elessair 0:f269e3021894 80 q->background.timer = 0;
elessair 0:f269e3021894 81
elessair 0:f269e3021894 82 // initialize platform resources
elessair 0:f269e3021894 83 int err;
elessair 0:f269e3021894 84 err = equeue_sema_create(&q->eventsema);
elessair 0:f269e3021894 85 if (err < 0) {
elessair 0:f269e3021894 86 return err;
elessair 0:f269e3021894 87 }
elessair 0:f269e3021894 88
elessair 0:f269e3021894 89 err = equeue_mutex_create(&q->queuelock);
elessair 0:f269e3021894 90 if (err < 0) {
elessair 0:f269e3021894 91 return err;
elessair 0:f269e3021894 92 }
elessair 0:f269e3021894 93
elessair 0:f269e3021894 94 err = equeue_mutex_create(&q->memlock);
elessair 0:f269e3021894 95 if (err < 0) {
elessair 0:f269e3021894 96 return err;
elessair 0:f269e3021894 97 }
elessair 0:f269e3021894 98
elessair 0:f269e3021894 99 return 0;
elessair 0:f269e3021894 100 }
elessair 0:f269e3021894 101
elessair 0:f269e3021894 102 void equeue_destroy(equeue_t *q) {
elessair 0:f269e3021894 103 // call destructors on pending events
elessair 0:f269e3021894 104 for (struct equeue_event *es = q->queue; es; es = es->next) {
elessair 0:f269e3021894 105 for (struct equeue_event *e = q->queue; e; e = e->sibling) {
elessair 0:f269e3021894 106 if (e->dtor) {
elessair 0:f269e3021894 107 e->dtor(e + 1);
elessair 0:f269e3021894 108 }
elessair 0:f269e3021894 109 }
elessair 0:f269e3021894 110 }
elessair 0:f269e3021894 111
elessair 0:f269e3021894 112 // notify background timer
elessair 0:f269e3021894 113 if (q->background.update) {
elessair 0:f269e3021894 114 q->background.update(q->background.timer, -1);
elessair 0:f269e3021894 115 }
elessair 0:f269e3021894 116
elessair 0:f269e3021894 117 // clean up platform resources + memory
elessair 0:f269e3021894 118 equeue_mutex_destroy(&q->memlock);
elessair 0:f269e3021894 119 equeue_mutex_destroy(&q->queuelock);
elessair 0:f269e3021894 120 equeue_sema_destroy(&q->eventsema);
elessair 0:f269e3021894 121 free(q->allocated);
elessair 0:f269e3021894 122 }
elessair 0:f269e3021894 123
elessair 0:f269e3021894 124
elessair 0:f269e3021894 125 // equeue chunk allocation functions
elessair 0:f269e3021894 126 static struct equeue_event *equeue_mem_alloc(equeue_t *q, size_t size) {
elessair 0:f269e3021894 127 // add event overhead
elessair 0:f269e3021894 128 size += sizeof(struct equeue_event);
elessair 0:f269e3021894 129 size = (size + sizeof(void*)-1) & ~(sizeof(void*)-1);
elessair 0:f269e3021894 130
elessair 0:f269e3021894 131 equeue_mutex_lock(&q->memlock);
elessair 0:f269e3021894 132
elessair 0:f269e3021894 133 // check if a good chunk is available
elessair 0:f269e3021894 134 for (struct equeue_event **p = &q->chunks; *p; p = &(*p)->next) {
elessair 0:f269e3021894 135 if ((*p)->size >= size) {
elessair 0:f269e3021894 136 struct equeue_event *e = *p;
elessair 0:f269e3021894 137 if (e->sibling) {
elessair 0:f269e3021894 138 *p = e->sibling;
elessair 0:f269e3021894 139 (*p)->next = e->next;
elessair 0:f269e3021894 140 } else {
elessair 0:f269e3021894 141 *p = e->next;
elessair 0:f269e3021894 142 }
elessair 0:f269e3021894 143
elessair 0:f269e3021894 144 equeue_mutex_unlock(&q->memlock);
elessair 0:f269e3021894 145 return e;
elessair 0:f269e3021894 146 }
elessair 0:f269e3021894 147 }
elessair 0:f269e3021894 148
elessair 0:f269e3021894 149 // otherwise allocate a new chunk out of the slab
elessair 0:f269e3021894 150 if (q->slab.size >= size) {
elessair 0:f269e3021894 151 struct equeue_event *e = (struct equeue_event *)q->slab.data;
elessair 0:f269e3021894 152 q->slab.data += size;
elessair 0:f269e3021894 153 q->slab.size -= size;
elessair 0:f269e3021894 154 e->size = size;
elessair 0:f269e3021894 155 e->id = 1;
elessair 0:f269e3021894 156
elessair 0:f269e3021894 157 equeue_mutex_unlock(&q->memlock);
elessair 0:f269e3021894 158 return e;
elessair 0:f269e3021894 159 }
elessair 0:f269e3021894 160
elessair 0:f269e3021894 161 equeue_mutex_unlock(&q->memlock);
elessair 0:f269e3021894 162 return 0;
elessair 0:f269e3021894 163 }
elessair 0:f269e3021894 164
elessair 0:f269e3021894 165 static void equeue_mem_dealloc(equeue_t *q, struct equeue_event *e) {
elessair 0:f269e3021894 166 equeue_mutex_lock(&q->memlock);
elessair 0:f269e3021894 167
elessair 0:f269e3021894 168 // stick chunk into list of chunks
elessair 0:f269e3021894 169 struct equeue_event **p = &q->chunks;
elessair 0:f269e3021894 170 while (*p && (*p)->size < e->size) {
elessair 0:f269e3021894 171 p = &(*p)->next;
elessair 0:f269e3021894 172 }
elessair 0:f269e3021894 173
elessair 0:f269e3021894 174 if (*p && (*p)->size == e->size) {
elessair 0:f269e3021894 175 e->sibling = *p;
elessair 0:f269e3021894 176 e->next = (*p)->next;
elessair 0:f269e3021894 177 } else {
elessair 0:f269e3021894 178 e->sibling = 0;
elessair 0:f269e3021894 179 e->next = *p;
elessair 0:f269e3021894 180 }
elessair 0:f269e3021894 181 *p = e;
elessair 0:f269e3021894 182
elessair 0:f269e3021894 183 equeue_mutex_unlock(&q->memlock);
elessair 0:f269e3021894 184 }
elessair 0:f269e3021894 185
elessair 0:f269e3021894 186 void *equeue_alloc(equeue_t *q, size_t size) {
elessair 0:f269e3021894 187 struct equeue_event *e = equeue_mem_alloc(q, size);
elessair 0:f269e3021894 188 if (!e) {
elessair 0:f269e3021894 189 return 0;
elessair 0:f269e3021894 190 }
elessair 0:f269e3021894 191
elessair 0:f269e3021894 192 e->target = 0;
elessair 0:f269e3021894 193 e->period = -1;
elessair 0:f269e3021894 194 e->dtor = 0;
elessair 0:f269e3021894 195
elessair 0:f269e3021894 196 return e + 1;
elessair 0:f269e3021894 197 }
elessair 0:f269e3021894 198
elessair 0:f269e3021894 199 void equeue_dealloc(equeue_t *q, void *p) {
elessair 0:f269e3021894 200 struct equeue_event *e = (struct equeue_event*)p - 1;
elessair 0:f269e3021894 201
elessair 0:f269e3021894 202 if (e->dtor) {
elessair 0:f269e3021894 203 e->dtor(e+1);
elessair 0:f269e3021894 204 }
elessair 0:f269e3021894 205
elessair 0:f269e3021894 206 equeue_mem_dealloc(q, e);
elessair 0:f269e3021894 207 }
elessair 0:f269e3021894 208
elessair 0:f269e3021894 209
elessair 0:f269e3021894 210 // equeue scheduling functions
elessair 0:f269e3021894 211 static int equeue_enqueue(equeue_t *q, struct equeue_event *e, unsigned tick) {
elessair 0:f269e3021894 212 // setup event and hash local id with buffer offset for unique id
elessair 0:f269e3021894 213 int id = (e->id << q->npw2) | ((unsigned char *)e - q->buffer);
elessair 0:f269e3021894 214 e->target = tick + equeue_clampdiff(e->target, tick);
elessair 0:f269e3021894 215 e->generation = q->generation;
elessair 0:f269e3021894 216
elessair 0:f269e3021894 217 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 218
elessair 0:f269e3021894 219 // find the event slot
elessair 0:f269e3021894 220 struct equeue_event **p = &q->queue;
elessair 0:f269e3021894 221 while (*p && equeue_tickdiff((*p)->target, e->target) < 0) {
elessair 0:f269e3021894 222 p = &(*p)->next;
elessair 0:f269e3021894 223 }
elessair 0:f269e3021894 224
elessair 0:f269e3021894 225 // insert at head in slot
elessair 0:f269e3021894 226 if (*p && (*p)->target == e->target) {
elessair 0:f269e3021894 227 e->next = (*p)->next;
elessair 0:f269e3021894 228 if (e->next) {
elessair 0:f269e3021894 229 e->next->ref = &e->next;
elessair 0:f269e3021894 230 }
elessair 0:f269e3021894 231
elessair 0:f269e3021894 232 e->sibling = *p;
elessair 0:f269e3021894 233 e->sibling->ref = &e->sibling;
elessair 0:f269e3021894 234 } else {
elessair 0:f269e3021894 235 e->next = *p;
elessair 0:f269e3021894 236 if (e->next) {
elessair 0:f269e3021894 237 e->next->ref = &e->next;
elessair 0:f269e3021894 238 }
elessair 0:f269e3021894 239
elessair 0:f269e3021894 240 e->sibling = 0;
elessair 0:f269e3021894 241 }
elessair 0:f269e3021894 242
elessair 0:f269e3021894 243 *p = e;
elessair 0:f269e3021894 244 e->ref = p;
elessair 0:f269e3021894 245
elessair 0:f269e3021894 246 // notify background timer
elessair 0:f269e3021894 247 if ((q->background.update && q->background.active) &&
elessair 0:f269e3021894 248 (q->queue == e && !e->sibling)) {
elessair 0:f269e3021894 249 q->background.update(q->background.timer,
elessair 0:f269e3021894 250 equeue_clampdiff(e->target, tick));
elessair 0:f269e3021894 251 }
elessair 0:f269e3021894 252
elessair 0:f269e3021894 253 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 254
elessair 0:f269e3021894 255 return id;
elessair 0:f269e3021894 256 }
elessair 0:f269e3021894 257
elessair 0:f269e3021894 258 static struct equeue_event *equeue_unqueue(equeue_t *q, int id) {
elessair 0:f269e3021894 259 // decode event from unique id and check that the local id matches
elessair 0:f269e3021894 260 struct equeue_event *e = (struct equeue_event *)
elessair 0:f269e3021894 261 &q->buffer[id & ((1 << q->npw2)-1)];
elessair 0:f269e3021894 262
elessair 0:f269e3021894 263 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 264 if (e->id != id >> q->npw2) {
elessair 0:f269e3021894 265 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 266 return 0;
elessair 0:f269e3021894 267 }
elessair 0:f269e3021894 268
elessair 0:f269e3021894 269 // clear the event and check if already in-flight
elessair 0:f269e3021894 270 e->cb = 0;
elessair 0:f269e3021894 271 e->period = -1;
elessair 0:f269e3021894 272
elessair 0:f269e3021894 273 int diff = equeue_tickdiff(e->target, q->tick);
elessair 0:f269e3021894 274 if (diff < 0 || (diff == 0 && e->generation != q->generation)) {
elessair 0:f269e3021894 275 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 276 return 0;
elessair 0:f269e3021894 277 }
elessair 0:f269e3021894 278
elessair 0:f269e3021894 279 // disentangle from queue
elessair 0:f269e3021894 280 if (e->sibling) {
elessair 0:f269e3021894 281 e->sibling->next = e->next;
elessair 0:f269e3021894 282 if (e->sibling->next) {
elessair 0:f269e3021894 283 e->sibling->next->ref = &e->sibling->next;
elessair 0:f269e3021894 284 }
elessair 0:f269e3021894 285
elessair 0:f269e3021894 286 *e->ref = e->sibling;
elessair 0:f269e3021894 287 e->sibling->ref = e->ref;
elessair 0:f269e3021894 288 } else {
elessair 0:f269e3021894 289 *e->ref = e->next;
elessair 0:f269e3021894 290 if (e->next) {
elessair 0:f269e3021894 291 e->next->ref = e->ref;
elessair 0:f269e3021894 292 }
elessair 0:f269e3021894 293 }
elessair 0:f269e3021894 294
elessair 0:f269e3021894 295 equeue_incid(q, e);
elessair 0:f269e3021894 296 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 297
elessair 0:f269e3021894 298 return e;
elessair 0:f269e3021894 299 }
elessair 0:f269e3021894 300
elessair 0:f269e3021894 301 static struct equeue_event *equeue_dequeue(equeue_t *q, unsigned target) {
elessair 0:f269e3021894 302 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 303
elessair 0:f269e3021894 304 // find all expired events and mark a new generation
elessair 0:f269e3021894 305 q->generation += 1;
elessair 0:f269e3021894 306 if (equeue_tickdiff(q->tick, target) <= 0) {
elessair 0:f269e3021894 307 q->tick = target;
elessair 0:f269e3021894 308 }
elessair 0:f269e3021894 309
elessair 0:f269e3021894 310 struct equeue_event *head = q->queue;
elessair 0:f269e3021894 311 struct equeue_event **p = &head;
elessair 0:f269e3021894 312 while (*p && equeue_tickdiff((*p)->target, target) <= 0) {
elessair 0:f269e3021894 313 p = &(*p)->next;
elessair 0:f269e3021894 314 }
elessair 0:f269e3021894 315
elessair 0:f269e3021894 316 q->queue = *p;
elessair 0:f269e3021894 317 if (q->queue) {
elessair 0:f269e3021894 318 q->queue->ref = &q->queue;
elessair 0:f269e3021894 319 }
elessair 0:f269e3021894 320
elessair 0:f269e3021894 321 *p = 0;
elessair 0:f269e3021894 322
elessair 0:f269e3021894 323 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 324
elessair 0:f269e3021894 325 // reverse and flatten each slot to match insertion order
elessair 0:f269e3021894 326 struct equeue_event **tail = &head;
elessair 0:f269e3021894 327 struct equeue_event *ess = head;
elessair 0:f269e3021894 328 while (ess) {
elessair 0:f269e3021894 329 struct equeue_event *es = ess;
elessair 0:f269e3021894 330 ess = es->next;
elessair 0:f269e3021894 331
elessair 0:f269e3021894 332 struct equeue_event *prev = 0;
elessair 0:f269e3021894 333 for (struct equeue_event *e = es; e; e = e->sibling) {
elessair 0:f269e3021894 334 e->next = prev;
elessair 0:f269e3021894 335 prev = e;
elessair 0:f269e3021894 336 }
elessair 0:f269e3021894 337
elessair 0:f269e3021894 338 *tail = prev;
elessair 0:f269e3021894 339 tail = &es->next;
elessair 0:f269e3021894 340 }
elessair 0:f269e3021894 341
elessair 0:f269e3021894 342 return head;
elessair 0:f269e3021894 343 }
elessair 0:f269e3021894 344
elessair 0:f269e3021894 345 int equeue_post(equeue_t *q, void (*cb)(void*), void *p) {
elessair 0:f269e3021894 346 struct equeue_event *e = (struct equeue_event*)p - 1;
elessair 0:f269e3021894 347 unsigned tick = equeue_tick();
elessair 0:f269e3021894 348 e->cb = cb;
elessair 0:f269e3021894 349 e->target = tick + e->target;
elessair 0:f269e3021894 350
elessair 0:f269e3021894 351 int id = equeue_enqueue(q, e, tick);
elessair 0:f269e3021894 352 equeue_sema_signal(&q->eventsema);
elessair 0:f269e3021894 353 return id;
elessair 0:f269e3021894 354 }
elessair 0:f269e3021894 355
elessair 0:f269e3021894 356 void equeue_cancel(equeue_t *q, int id) {
elessair 0:f269e3021894 357 if (!id) {
elessair 0:f269e3021894 358 return;
elessair 0:f269e3021894 359 }
elessair 0:f269e3021894 360
elessair 0:f269e3021894 361 struct equeue_event *e = equeue_unqueue(q, id);
elessair 0:f269e3021894 362 if (e) {
elessair 0:f269e3021894 363 equeue_dealloc(q, e + 1);
elessair 0:f269e3021894 364 }
elessair 0:f269e3021894 365 }
elessair 0:f269e3021894 366
elessair 0:f269e3021894 367 void equeue_break(equeue_t *q) {
elessair 0:f269e3021894 368 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 369 q->breaks++;
elessair 0:f269e3021894 370 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 371 equeue_sema_signal(&q->eventsema);
elessair 0:f269e3021894 372 }
elessair 0:f269e3021894 373
elessair 0:f269e3021894 374 void equeue_dispatch(equeue_t *q, int ms) {
elessair 0:f269e3021894 375 unsigned tick = equeue_tick();
elessair 0:f269e3021894 376 unsigned timeout = tick + ms;
elessair 0:f269e3021894 377 q->background.active = false;
elessair 0:f269e3021894 378
elessair 0:f269e3021894 379 while (1) {
elessair 0:f269e3021894 380 // collect all the available events and next deadline
elessair 0:f269e3021894 381 struct equeue_event *es = equeue_dequeue(q, tick);
elessair 0:f269e3021894 382
elessair 0:f269e3021894 383 // dispatch events
elessair 0:f269e3021894 384 while (es) {
elessair 0:f269e3021894 385 struct equeue_event *e = es;
elessair 0:f269e3021894 386 es = e->next;
elessair 0:f269e3021894 387
elessair 0:f269e3021894 388 // actually dispatch the callbacks
elessair 0:f269e3021894 389 void (*cb)(void *) = e->cb;
elessair 0:f269e3021894 390 if (cb) {
elessair 0:f269e3021894 391 cb(e + 1);
elessair 0:f269e3021894 392 }
elessair 0:f269e3021894 393
elessair 0:f269e3021894 394 // reenqueue periodic events or deallocate
elessair 0:f269e3021894 395 if (e->period >= 0) {
elessair 0:f269e3021894 396 e->target += e->period;
elessair 0:f269e3021894 397 equeue_enqueue(q, e, equeue_tick());
elessair 0:f269e3021894 398 } else {
elessair 0:f269e3021894 399 equeue_incid(q, e);
elessair 0:f269e3021894 400 equeue_dealloc(q, e+1);
elessair 0:f269e3021894 401 }
elessair 0:f269e3021894 402 }
elessair 0:f269e3021894 403
elessair 0:f269e3021894 404 int deadline = -1;
elessair 0:f269e3021894 405 tick = equeue_tick();
elessair 0:f269e3021894 406
elessair 0:f269e3021894 407 // check if we should stop dispatching soon
elessair 0:f269e3021894 408 if (ms >= 0) {
elessair 0:f269e3021894 409 deadline = equeue_tickdiff(timeout, tick);
elessair 0:f269e3021894 410 if (deadline <= 0) {
elessair 0:f269e3021894 411 // update background timer if necessary
elessair 0:f269e3021894 412 if (q->background.update) {
elessair 0:f269e3021894 413 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 414 if (q->background.update && q->queue) {
elessair 0:f269e3021894 415 q->background.update(q->background.timer,
elessair 0:f269e3021894 416 equeue_clampdiff(q->queue->target, tick));
elessair 0:f269e3021894 417 }
elessair 0:f269e3021894 418 q->background.active = true;
elessair 0:f269e3021894 419 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 420 }
elessair 0:f269e3021894 421 return;
elessair 0:f269e3021894 422 }
elessair 0:f269e3021894 423 }
elessair 0:f269e3021894 424
elessair 0:f269e3021894 425 // find closest deadline
elessair 0:f269e3021894 426 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 427 if (q->queue) {
elessair 0:f269e3021894 428 int diff = equeue_clampdiff(q->queue->target, tick);
elessair 0:f269e3021894 429 if ((unsigned)diff < (unsigned)deadline) {
elessair 0:f269e3021894 430 deadline = diff;
elessair 0:f269e3021894 431 }
elessair 0:f269e3021894 432 }
elessair 0:f269e3021894 433 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 434
elessair 0:f269e3021894 435 // wait for events
elessair 0:f269e3021894 436 equeue_sema_wait(&q->eventsema, deadline);
elessair 0:f269e3021894 437
elessair 0:f269e3021894 438 // check if we were notified to break out of dispatch
elessair 0:f269e3021894 439 if (q->breaks) {
elessair 0:f269e3021894 440 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 441 if (q->breaks > 0) {
elessair 0:f269e3021894 442 q->breaks--;
elessair 0:f269e3021894 443 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 444 return;
elessair 0:f269e3021894 445 }
elessair 0:f269e3021894 446 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 447 }
elessair 0:f269e3021894 448
elessair 0:f269e3021894 449 // update tick for next iteration
elessair 0:f269e3021894 450 tick = equeue_tick();
elessair 0:f269e3021894 451 }
elessair 0:f269e3021894 452 }
elessair 0:f269e3021894 453
elessair 0:f269e3021894 454
elessair 0:f269e3021894 455 // event functions
elessair 0:f269e3021894 456 void equeue_event_delay(void *p, int ms) {
elessair 0:f269e3021894 457 struct equeue_event *e = (struct equeue_event*)p - 1;
elessair 0:f269e3021894 458 e->target = ms;
elessair 0:f269e3021894 459 }
elessair 0:f269e3021894 460
elessair 0:f269e3021894 461 void equeue_event_period(void *p, int ms) {
elessair 0:f269e3021894 462 struct equeue_event *e = (struct equeue_event*)p - 1;
elessair 0:f269e3021894 463 e->period = ms;
elessair 0:f269e3021894 464 }
elessair 0:f269e3021894 465
elessair 0:f269e3021894 466 void equeue_event_dtor(void *p, void (*dtor)(void *)) {
elessair 0:f269e3021894 467 struct equeue_event *e = (struct equeue_event*)p - 1;
elessair 0:f269e3021894 468 e->dtor = dtor;
elessair 0:f269e3021894 469 }
elessair 0:f269e3021894 470
elessair 0:f269e3021894 471
elessair 0:f269e3021894 472 // simple callbacks
elessair 0:f269e3021894 473 struct ecallback {
elessair 0:f269e3021894 474 void (*cb)(void*);
elessair 0:f269e3021894 475 void *data;
elessair 0:f269e3021894 476 };
elessair 0:f269e3021894 477
elessair 0:f269e3021894 478 static void ecallback_dispatch(void *p) {
elessair 0:f269e3021894 479 struct ecallback *e = (struct ecallback*)p;
elessair 0:f269e3021894 480 e->cb(e->data);
elessair 0:f269e3021894 481 }
elessair 0:f269e3021894 482
elessair 0:f269e3021894 483 int equeue_call(equeue_t *q, void (*cb)(void*), void *data) {
elessair 0:f269e3021894 484 struct ecallback *e = equeue_alloc(q, sizeof(struct ecallback));
elessair 0:f269e3021894 485 if (!e) {
elessair 0:f269e3021894 486 return 0;
elessair 0:f269e3021894 487 }
elessair 0:f269e3021894 488
elessair 0:f269e3021894 489 e->cb = cb;
elessair 0:f269e3021894 490 e->data = data;
elessair 0:f269e3021894 491 return equeue_post(q, ecallback_dispatch, e);
elessair 0:f269e3021894 492 }
elessair 0:f269e3021894 493
elessair 0:f269e3021894 494 int equeue_call_in(equeue_t *q, int ms, void (*cb)(void*), void *data) {
elessair 0:f269e3021894 495 struct ecallback *e = equeue_alloc(q, sizeof(struct ecallback));
elessair 0:f269e3021894 496 if (!e) {
elessair 0:f269e3021894 497 return 0;
elessair 0:f269e3021894 498 }
elessair 0:f269e3021894 499
elessair 0:f269e3021894 500 equeue_event_delay(e, ms);
elessair 0:f269e3021894 501 e->cb = cb;
elessair 0:f269e3021894 502 e->data = data;
elessair 0:f269e3021894 503 return equeue_post(q, ecallback_dispatch, e);
elessair 0:f269e3021894 504 }
elessair 0:f269e3021894 505
elessair 0:f269e3021894 506 int equeue_call_every(equeue_t *q, int ms, void (*cb)(void*), void *data) {
elessair 0:f269e3021894 507 struct ecallback *e = equeue_alloc(q, sizeof(struct ecallback));
elessair 0:f269e3021894 508 if (!e) {
elessair 0:f269e3021894 509 return 0;
elessair 0:f269e3021894 510 }
elessair 0:f269e3021894 511
elessair 0:f269e3021894 512 equeue_event_delay(e, ms);
elessair 0:f269e3021894 513 equeue_event_period(e, ms);
elessair 0:f269e3021894 514 e->cb = cb;
elessair 0:f269e3021894 515 e->data = data;
elessair 0:f269e3021894 516 return equeue_post(q, ecallback_dispatch, e);
elessair 0:f269e3021894 517 }
elessair 0:f269e3021894 518
elessair 0:f269e3021894 519
elessair 0:f269e3021894 520 // backgrounding
elessair 0:f269e3021894 521 void equeue_background(equeue_t *q,
elessair 0:f269e3021894 522 void (*update)(void *timer, int ms), void *timer) {
elessair 0:f269e3021894 523 equeue_mutex_lock(&q->queuelock);
elessair 0:f269e3021894 524 if (q->background.update) {
elessair 0:f269e3021894 525 q->background.update(q->background.timer, -1);
elessair 0:f269e3021894 526 }
elessair 0:f269e3021894 527
elessair 0:f269e3021894 528 q->background.update = update;
elessair 0:f269e3021894 529 q->background.timer = timer;
elessair 0:f269e3021894 530
elessair 0:f269e3021894 531 if (q->background.update && q->queue) {
elessair 0:f269e3021894 532 q->background.update(q->background.timer,
elessair 0:f269e3021894 533 equeue_clampdiff(q->queue->target, equeue_tick()));
elessair 0:f269e3021894 534 }
elessair 0:f269e3021894 535 q->background.active = true;
elessair 0:f269e3021894 536 equeue_mutex_unlock(&q->queuelock);
elessair 0:f269e3021894 537 }
elessair 0:f269e3021894 538
elessair 0:f269e3021894 539 struct equeue_chain_context {
elessair 0:f269e3021894 540 equeue_t *q;
elessair 0:f269e3021894 541 equeue_t *target;
elessair 0:f269e3021894 542 int id;
elessair 0:f269e3021894 543 };
elessair 0:f269e3021894 544
elessair 0:f269e3021894 545 static void equeue_chain_dispatch(void *p) {
elessair 0:f269e3021894 546 equeue_dispatch((equeue_t *)p, 0);
elessair 0:f269e3021894 547 }
elessair 0:f269e3021894 548
elessair 0:f269e3021894 549 static void equeue_chain_update(void *p, int ms) {
elessair 0:f269e3021894 550 struct equeue_chain_context *c = (struct equeue_chain_context *)p;
elessair 0:f269e3021894 551 equeue_cancel(c->target, c->id);
elessair 0:f269e3021894 552
elessair 0:f269e3021894 553 if (ms >= 0) {
elessair 0:f269e3021894 554 c->id = equeue_call_in(c->target, ms, equeue_chain_dispatch, c->q);
elessair 0:f269e3021894 555 } else {
elessair 0:f269e3021894 556 equeue_dealloc(c->target, c);
elessair 0:f269e3021894 557 }
elessair 0:f269e3021894 558 }
elessair 0:f269e3021894 559
elessair 0:f269e3021894 560 void equeue_chain(equeue_t *q, equeue_t *target) {
elessair 0:f269e3021894 561 struct equeue_chain_context *c = equeue_alloc(q,
elessair 0:f269e3021894 562 sizeof(struct equeue_chain_context));
elessair 0:f269e3021894 563
elessair 0:f269e3021894 564 c->q = q;
elessair 0:f269e3021894 565 c->target = target;
elessair 0:f269e3021894 566 c->id = 0;
elessair 0:f269e3021894 567
elessair 0:f269e3021894 568 equeue_background(q, equeue_chain_update, c);
elessair 0:f269e3021894 569 }