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