001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019 #include <sos/assert.h>
020 #include <sos/klibc.h>
021 #include <hwcore/irq.h>
022 #include <sos/list.h>
023
024 #include "time.h"
025
026
027
028
029
030 #define NS_IN_SEC 1000000000UL
031
032
033
034
035
036
037
038
039
040
041 static struct sos_timeout_action *timeout_action_list;
042
043
044
045
046
047 static struct sos_time tick_resolution;
048
049
050
051
052
053
054
055
056
057
058
059 static struct sos_time last_tick_time;
060
061
062 sos_ret_t sos_time_inc(struct sos_time *dest,
063 const struct sos_time *to_add)
064 {
065
066
067 sos_ui32_t sigma_ns = dest->nanosec + to_add->nanosec;
068
069 dest->sec += to_add->sec;
070 dest->sec += sigma_ns / NS_IN_SEC;
071 dest->nanosec = sigma_ns % NS_IN_SEC;
072 return SOS_OK;
073 }
074
075
076 sos_ret_t sos_time_dec(struct sos_time *dest,
077 const struct sos_time *to_dec)
078 {
079
080
081
082 sos_si32_t diff_ns = ((sos_si32_t)dest->nanosec)
083 - ((sos_si32_t)to_dec->nanosec);
084
085
086 SOS_ASSERT_FATAL(dest->sec >= to_dec->sec);
087 if (dest->sec == to_dec->sec)
088 SOS_ASSERT_FATAL(dest->nanosec >= to_dec->nanosec);
089
090 dest->sec -= to_dec->sec;
091 if (diff_ns > 0)
092 dest->sec += diff_ns / NS_IN_SEC;
093 else
094 dest->sec -= ((-diff_ns) / NS_IN_SEC);
095 dest->nanosec = (NS_IN_SEC + diff_ns) % NS_IN_SEC;
096 if (diff_ns < 0)
097 dest->sec --;
098 return SOS_OK;
099 }
100
101
102 int sos_time_cmp(const struct sos_time *t1,
103 const struct sos_time *t2)
104 {
105
106 if (t1->sec < t2->sec)
107 return -1;
108 else if (t1->sec > t2->sec)
109 return 1;
110
111
112 else if (t1->nanosec < t2->nanosec)
113 return -1;
114 else if (t1->nanosec > t2->nanosec)
115 return 1;
116
117
118 return 0;
119 }
120
121
122 sos_bool_t sos_time_is_zero(const struct sos_time *tm)
123 {
124 return ( (0 == tm->sec) && (0 == tm->nanosec) );
125 }
126
127
128 sos_ret_t sos_time_subsysem_setup(const struct sos_time *initial_resolution)
129 {
130 timeout_action_list = NULL;
131 last_tick_time = (struct sos_time) { .sec = 0, .nanosec = 0 };
132 memcpy(& tick_resolution, initial_resolution, sizeof(struct sos_time));
133
134 return SOS_OK;
135 }
136
137
138 sos_ret_t sos_time_get_tick_resolution(struct sos_time *resolution)
139 {
140 sos_ui32_t flags;
141 sos_disable_IRQs(flags);
142
143 memcpy(resolution, & tick_resolution, sizeof(struct sos_time));
144
145 sos_restore_IRQs(flags);
146 return SOS_OK;
147 }
148
149
150 sos_ret_t sos_time_set_tick_resolution(const struct sos_time *resolution)
151 {
152 sos_ui32_t flags;
153
154 sos_disable_IRQs(flags);
155 memcpy(& tick_resolution, resolution, sizeof(struct sos_time));
156 sos_restore_IRQs(flags);
157
158 return SOS_OK;
159 }
160
161
162 sos_ret_t sos_time_get_now(struct sos_time *now)
163 {
164 sos_ui32_t flags;
165 sos_disable_IRQs(flags);
166
167 memcpy(now, & last_tick_time, sizeof(struct sos_time));
168
169 sos_restore_IRQs(flags);
170 return SOS_OK;
171 }
172
173
174
175
176
177
178 static sos_ret_t _add_action(struct sos_timeout_action *act,
179 const struct sos_time *due_date,
180 sos_bool_t is_relative_due_date,
181 sos_timeout_routine_t *routine,
182 void *routine_data)
183 {
184 struct sos_timeout_action *other, *insert_before;
185 int nb_act;
186
187
188 if (due_date == NULL)
189 return -SOS_EINVAL;
190
191
192 if (act == NULL)
193 return -SOS_EINVAL;
194
195
196 if (NULL == routine)
197 return -SOS_EINVAL;
198
199
200 if (NULL != act->tmo_next)
201 return -SOS_EBUSY;
202
203
204 if (is_relative_due_date)
205 {
206
207 memcpy(& act->timeout, & last_tick_time, sizeof(struct sos_time));
208 sos_time_inc(& act->timeout, due_date);
209 }
210 else
211 {
212
213
214
215 if (sos_time_cmp(due_date, & last_tick_time) < 0)
216
217 return -SOS_EINVAL;
218
219 memcpy(& act->timeout, due_date, sizeof(struct sos_time));
220 }
221
222
223 act->routine = routine;
224 act->routine_data = routine_data;
225
226
227 insert_before = NULL;
228 list_foreach_forward_named(timeout_action_list,
229 other, nb_act,
230 tmo_prev, tmo_next)
231 {
232 if (sos_time_cmp(& act->timeout, & other->timeout) < 0)
233 {
234 insert_before = other;
235 break;
236 }
237
238
239 }
240
241
242 if (insert_before != NULL)
243 list_insert_before_named(timeout_action_list, insert_before, act,
244 tmo_prev, tmo_next);
245 else
246 list_add_tail_named(timeout_action_list, act,
247 tmo_prev, tmo_next);
248
249 return SOS_OK;
250 }
251
252
253 sos_ret_t
254 sos_time_register_action_relative(struct sos_timeout_action *act,
255 const struct sos_time *delay,
256 sos_timeout_routine_t *routine,
257 void *routine_data)
258 {
259 sos_ui32_t flags;
260 sos_ret_t retval;
261
262 sos_disable_IRQs(flags);
263 retval = _add_action(act, delay, TRUE, routine, routine_data);
264 sos_restore_IRQs(flags);
265
266 return retval;
267 }
268
269
270 sos_ret_t
271 sos_time_register_action_absolute(struct sos_timeout_action *act,
272 const struct sos_time *date,
273 sos_timeout_routine_t *routine,
274 void *routine_data)
275 {
276 sos_ui32_t flags;
277 sos_ret_t retval;
278
279 sos_disable_IRQs(flags);
280 retval = _add_action(act, date, FALSE, routine, routine_data);
281 sos_restore_IRQs(flags);
282
283 return retval;
284 }
285
286
287
288
289
290
291 static sos_ret_t _remove_action(struct sos_timeout_action *act)
292 {
293
294 if (NULL == act->tmo_next)
295 return -SOS_EINVAL;
296
297
298 if (sos_time_cmp(& act->timeout, & last_tick_time) <= 0)
299 act->timeout = (struct sos_time){ .sec=0, .nanosec=0 };
300 else
301 sos_time_dec(& act->timeout, & last_tick_time);
302
303
304 list_delete_named(timeout_action_list, act,
305 tmo_prev, tmo_next);
306 act->tmo_prev = act->tmo_next = NULL;
307
308 return SOS_OK;
309 }
310
311
312 sos_ret_t sos_time_unregister_action(struct sos_timeout_action *act)
313 {
314 sos_ret_t retval;
315 sos_ui32_t flags;
316
317 sos_disable_IRQs(flags);
318 retval = _remove_action(act);
319 sos_restore_IRQs(flags);
320
321 return SOS_OK;
322 }
323
324
325 sos_ret_t sos_time_do_tick()
326 {
327 sos_ui32_t flags;
328
329 sos_disable_IRQs(flags);
330
331
332 sos_time_inc(& last_tick_time, & tick_resolution);
333
334 while (! list_is_empty_named(timeout_action_list, tmo_prev, tmo_next))
335 {
336 struct sos_timeout_action *act;
337 act = list_get_head_named(timeout_action_list, tmo_prev, tmo_next);
338
339
340 if (sos_time_cmp(& last_tick_time, & act->timeout) < 0)
341 {
342
343 break;
344 }
345
346
347 _remove_action(act);
348
349
350 act->routine(act);
351 }
352
353 sos_restore_IRQs(flags);
354 return SOS_OK;
355 }