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@9832:b95afde9ef7e, 2017-09-05 (annotated)

Committer:
Wolfgang Betz
Date:
Tue Sep 05 09:09:24 2017 +0200
Revision:
9832:b95afde9ef7e
Parent:
11:1bc395c82a41 
Merge branch 'master' of hg::http://developer.mbed.org/teams/ST/code/ST_Events into events-split

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) {
Christopher Haster 3:3d3560169945 27 return (int)(unsigned)(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) {
Christopher Haster 11:1bc395c82a41 561 if (!target) {
Christopher Haster 11:1bc395c82a41 562 equeue_background(q, 0, 0);
Christopher Haster 11:1bc395c82a41 563 return;
Christopher Haster 11:1bc395c82a41 564 }
Christopher Haster 11:1bc395c82a41 565
Bogdan Marinescu 0:a792d4bf36c2 566 struct equeue_chain_context *c = equeue_alloc(q,
Bogdan Marinescu 0:a792d4bf36c2 567 sizeof(struct equeue_chain_context));
Bogdan Marinescu 0:a792d4bf36c2 568
Bogdan Marinescu 0:a792d4bf36c2 569 c->q = q;
Bogdan Marinescu 0:a792d4bf36c2 570 c->target = target;
Bogdan Marinescu 0:a792d4bf36c2 571 c->id = 0;
Bogdan Marinescu 0:a792d4bf36c2 572
Bogdan Marinescu 0:a792d4bf36c2 573 equeue_background(q, equeue_chain_update, c);
Bogdan Marinescu 0:a792d4bf36c2 574 }