1/* 2 * Copyright 2017, Data61 3 * Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 * ABN 41 687 119 230. 5 * 6 * This software may be distributed and modified according to the terms of 7 * the BSD 2-Clause license. Note that NO WARRANTY is provided. 8 * See "LICENSE_BSD2.txt" for details. 9 * 10 * @TAG(DATA61_BSD) 11 */ 12 13#include <utils/sglib.h> 14#include <platsupport/tqueue.h> 15 16static int cmp(uint64_t a, uint64_t b) 17{ 18 if (a > b) { 19 return 1; 20 } else if (a < b) { 21 return -1; 22 } else { 23 return 0; 24 } 25} 26 27#define TIMEOUT_CMP(t1, t2) (cmp(t1->timeout.abs_time, t2->timeout.abs_time)) 28 29#pragma GCC diagnostic push 30#pragma GCC diagnostic ignored "-Wunused-variable" 31SGLIB_DEFINE_SORTED_LIST_PROTOTYPES(tqueue_node_t, TIMEOUT_CMP, next) 32SGLIB_DEFINE_SORTED_LIST_FUNCTIONS(tqueue_node_t, TIMEOUT_CMP, next) 33#pragma GCC diagnostic pop 34 35static tqueue_node_t *head(tqueue_node_t *list) 36{ 37 struct sglib_tqueue_node_t_iterator it; 38 return sglib_tqueue_node_t_it_init(&it, list); 39} 40 41int tqueue_alloc_id(tqueue_t *tq, unsigned int *id) 42{ 43 if (!tq || !id) { 44 return EINVAL; 45 } 46 47 for (int i = 0; i < tq->n; i++) { 48 if (!tq->array[i].allocated) { 49 tq->array[i].allocated = true; 50 *id = i; 51 return 0; 52 } 53 } 54 55 ZF_LOGE("Out of timer client ids\n"); 56 return ENOMEM; 57} 58 59int tqueue_alloc_id_at(tqueue_t *tq, unsigned int id) 60{ 61 if (!tq || id >= tq->n) { 62 return EINVAL; 63 } 64 65 if (tq->array[id].allocated) { 66 return EADDRINUSE; 67 } 68 69 tq->array[id].allocated = true; 70 return 0; 71} 72 73int tqueue_free_id(tqueue_t *tq, unsigned int id) 74{ 75 if (!tq) { 76 return EINVAL; 77 } 78 79 if (id < 0 || id >= tq->n) { 80 ZF_LOGE("Invalid id"); 81 return EINVAL; 82 } 83 84 if (!tq->array[id].allocated) { 85 ZF_LOGW("Freeing unallocated id"); 86 return EINVAL; 87 } 88 89 /* remove from queue */ 90 if (tq->array[id].active) { 91 sglib_tqueue_node_t_delete(&tq->queue, &tq->array[id]); 92 tq->array[id].active = false; 93 } 94 95 tq->array[id].allocated = false; 96 return 0; 97} 98 99int tqueue_register(tqueue_t *tq, unsigned int id, timeout_t *timeout) 100{ 101 if (!tq) { 102 return EINVAL; 103 } 104 105 if (id < 0 || id > tq->n || !tq->array[id].allocated) { 106 ZF_LOGE("invalid id"); 107 return EINVAL; 108 } 109 110 /* delete the callback from the queue if its present */ 111 if (tq->array[id].active) { 112 sglib_tqueue_node_t_delete(&tq->queue, &tq->array[id]); 113 } 114 115 /* update node */ 116 tq->array[id].active = true; 117 tq->array[id].timeout = *timeout; 118 119 /* add to data structure */ 120 sglib_tqueue_node_t_add(&tq->queue, &tq->array[id]); 121 return 0; 122} 123 124int tqueue_cancel(tqueue_t *tq, unsigned int id) { 125 126 if (!tq) { 127 return EINVAL; 128 } 129 130 /* iterate through the list until we find that id */ 131 if (id < 0 || id > tq->n) { 132 ZF_LOGE("Invalid id"); 133 return EINVAL; 134 } 135 136 /* delete the callback from the queue if its present */ 137 if (tq->array[id].active) { 138 sglib_tqueue_node_t_delete(&tq->queue, &tq->array[id]); 139 } 140 141 tq->array[id].active = false; 142 return 0; 143} 144 145int tqueue_update(tqueue_t *tq, uint64_t curr_time, uint64_t *next_time) { 146 if (!tq) { 147 return EINVAL; 148 } 149 150 /* keep checking the head of this queue */ 151 tqueue_node_t *t = head(tq->queue); 152 while (t != NULL && t->timeout.abs_time <= curr_time) { 153 if (t->active) { 154 t->timeout.callback(t->timeout.token); 155 } 156 157 /* check if it is active again, as callback may have deactivated the timeout */ 158 if (t->active) { 159 sglib_tqueue_node_t_delete(&tq->queue, t); 160 if (t->timeout.period > 0) { 161 t->timeout.abs_time += t->timeout.period; 162 sglib_tqueue_node_t_add(&tq->queue, t); 163 } else { 164 t->active = false; 165 } 166 } 167 t = head(tq->queue); 168 } 169 170 if (next_time) { 171 if (t) { 172 *next_time = t->timeout.abs_time; 173 } else { 174 *next_time = 0; 175 } 176 } 177 return 0; 178} 179 180int tqueue_init_static(tqueue_t *tq, ps_malloc_ops_t *mops, int size) 181{ 182 if (!tq || !mops) { 183 return EINVAL; 184 } 185 186 if (size <= 0) { 187 return EINVAL; 188 } 189 190 /* initialise the list */ 191 tq->n = size; 192 int error = ps_calloc(mops, size, sizeof(tqueue_node_t), (void **) &tq->array); 193 if (error) { 194 return ENOMEM; 195 } 196 197 assert(tq->array != NULL); 198 199 /* noone currently in the queue */ 200 tq->queue = NULL; 201 202 return 0; 203} 204