Marco Mayer / Mbed OS Queue

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

mbed-os/events/equeue/equeue.c@0:6bf0743ece18, 2020-03-28 (annotated)

Committer:
demayer
Date:
Sat Mar 28 15:28:19 2020 +0000
Revision:
0:6bf0743ece18
IMU Thread with an event-queue running parallel to handle tasks like a 5 times blinking LED. Button with interrupt detected.

Who changed what in which revision?

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