ST / ST_Events-old

This is a fork of the `events` subdirectory of https://github.com/ARMmbed/mbed-os

Dependents:   HelloWorld_CCA01M1 HelloWorld_CCA02M1 CI-data-logger-server HelloWorld_CCA02M1 ... more

This is a fork of the events subdirectory of https://github.com/ARMmbed/mbed-os.

Note, you must import this library with import name: events!!!

equeue/equeue.c@2:a60d8117d0e0, 2016-10-01 (annotated)

Committer:
Sam Grove
Date:
Sat Oct 01 02:11:36 2016 -0500
Revision:
2:a60d8117d0e0
Parent:
0:a792d4bf36c2
Child:
3:3d3560169945
For drivers, events, hal, platform, rtos and mbed.h add one level of path to make sure specific and unique includes files are found.

Who changed what in which revision?

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