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