Marco Zecchini / Mbed OS Example_RTOS

Rtos API example

mbed-os/events/equeue/equeue.c@0:9fca2b23d0ba, 2019-02-23 (annotated)

Committer:
marcozecchini
Date:
Sat Feb 23 12:13:36 2019 +0000
Revision:
0:9fca2b23d0ba
final commit

Who changed what in which revision?

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