Greg Steiert / pegasus_dev

Development mbed library for MAX32630FTHR

Dependents:   blinky_max32630fthr

events/equeue/equeue.c@0:5c4d7b2438d3, 2016-11-11 (annotated)

Committer:
switches
Date:
Fri Nov 11 20:59:50 2016 +0000
Revision:
0:5c4d7b2438d3
Initial commit

Who changed what in which revision?

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