ON Semiconductor / mbed-os

5.2.1 - Updated I2C files

Dependents:   mbed-TFT-example-NCS36510 mbed-Accelerometer-example-NCS36510 mbed-Accelerometer-example-NCS36510

events/equeue/equeue.c@0:098463de4c5d, 2017-01-25 (annotated)

Committer:
group-onsemi
Date:
Wed Jan 25 20:34:15 2017 +0000
Revision:
0:098463de4c5d
Initial commit

Who changed what in which revision?

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