1 /*
2 Copyright (c) 2012, Broadcom Europe Ltd
3 All rights reserved.
4
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7 * Redistributions of source code must retain the above copyright
8 notice, this list of conditions and the following disclaimer.
9 * Redistributions in binary form must reproduce the above copyright
10 notice, this list of conditions and the following disclaimer in the
11 documentation and/or other materials provided with the distribution.
12 * Neither the name of the copyright holder nor the
13 names of its contributors may be used to endorse or promote products
14 derived from this software without specific prior written permission.
15
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28 /*#define VCOS_INLINE_BODIES */
29 #include "interface/vcos/vcos.h"
30 #include "interface/vcos/vcos_msgqueue.h"
31 #include <string.h>
32 #include <stdarg.h>
33 #include <stdlib.h>
34 #include <stdio.h>
35 #include <sys/time.h>
36 #include <linux/param.h>
37
38 /* Cygwin doesn't always have prctl.h and it doesn't have PR_SET_NAME */
39 #if defined( __linux__ )
40 # if !defined(HAVE_PRCTL)
41 # define HAVE_PRCTL
42 # endif
43 #include <sys/prctl.h>
44 #endif
45
46 #ifdef HAVE_CMAKE_CONFIG
47 #include "cmake_config.h"
48 #endif
49
50 #ifdef HAVE_MTRACE
51 #include <mcheck.h>
52 #endif
53
54 #if defined(ANDROID)
55 #include <android/log.h>
56 #endif
57
58 #ifndef VCOS_DEFAULT_STACK_SIZE
59 #define VCOS_DEFAULT_STACK_SIZE 4096
60 #endif
61
62 static int vcos_argc;
63 static const char **vcos_argv;
64
65 typedef void (*LEGACY_ENTRY_FN_T)(int, void *);
66
67 static VCOS_THREAD_ATTR_T default_attrs = {
68 .ta_stacksz = VCOS_DEFAULT_STACK_SIZE,
69 };
70
71 /** Singleton global lock used for vcos_global_lock/unlock(). */
72 static pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
73
74 #ifdef ANDROID
75 static VCOS_MUTEX_T printf_lock;
76 #endif
77
78 /* Create a per-thread key for faking up vcos access
79 * on non-vcos threads.
80 */
81 pthread_key_t _vcos_thread_current_key;
82
83 static VCOS_UNSIGNED _vcos_thread_current_key_created = 0;
84 static VCOS_ONCE_T current_thread_key_once; /* init just once */
85
vcos_thread_cleanup(VCOS_THREAD_T * thread)86 static void vcos_thread_cleanup(VCOS_THREAD_T *thread)
87 {
88 vcos_semaphore_delete(&thread->suspend);
89 if (thread->task_timer_created)
90 {
91 vcos_timer_delete(&thread->task_timer);
92 }
93 }
94
vcos_dummy_thread_cleanup(void * cxt)95 static void vcos_dummy_thread_cleanup(void *cxt)
96 {
97 VCOS_THREAD_T *thread = cxt;
98 if (thread->dummy)
99 {
100 int i;
101 /* call termination functions */
102 for (i=0; thread->at_exit[i].pfn != NULL; i++)
103 {
104 thread->at_exit[i].pfn(thread->at_exit[i].cxt);
105 }
106 vcos_thread_cleanup(thread);
107 vcos_free(thread);
108 }
109 }
110
current_thread_key_init(void)111 static void current_thread_key_init(void)
112 {
113 vcos_assert(!_vcos_thread_current_key_created);
114 pthread_key_create (&_vcos_thread_current_key, vcos_dummy_thread_cleanup);
115 _vcos_thread_current_key_created = 1;
116 }
117
118 /* A VCOS wrapper for the thread which called vcos_init. */
119 static VCOS_THREAD_T vcos_thread_main;
120
vcos_thread_entry(void * arg)121 static void *vcos_thread_entry(void *arg)
122 {
123 int i;
124 void *ret;
125 VCOS_THREAD_T *thread = (VCOS_THREAD_T *)arg;
126
127 vcos_assert(thread != NULL);
128 thread->dummy = 0;
129
130 pthread_setspecific(_vcos_thread_current_key, thread);
131 #if defined( HAVE_PRCTL ) && defined( PR_SET_NAME )
132 /* cygwin doesn't have PR_SET_NAME */
133 prctl( PR_SET_NAME, (unsigned long)thread->name, 0, 0, 0 );
134 #endif
135 if (thread->legacy)
136 {
137 LEGACY_ENTRY_FN_T fn = (LEGACY_ENTRY_FN_T)thread->entry;
138 (*fn)(0, thread->arg);
139 ret = 0;
140 }
141 else
142 {
143 ret = (*thread->entry)(thread->arg);
144 }
145
146 /* call termination functions */
147 for (i=0; thread->at_exit[i].pfn != NULL; i++)
148 {
149 thread->at_exit[i].pfn(thread->at_exit[i].cxt);
150 }
151
152 return ret;
153 }
154
_task_timer_expiration_routine(void * cxt)155 static void _task_timer_expiration_routine(void *cxt)
156 {
157 VCOS_THREAD_T *thread = (VCOS_THREAD_T *)cxt;
158
159 vcos_assert(thread->orig_task_timer_expiration_routine);
160 thread->orig_task_timer_expiration_routine(thread->orig_task_timer_context);
161 thread->orig_task_timer_expiration_routine = NULL;
162 }
163
vcos_thread_create(VCOS_THREAD_T * thread,const char * name,VCOS_THREAD_ATTR_T * attrs,VCOS_THREAD_ENTRY_FN_T entry,void * arg)164 VCOS_STATUS_T vcos_thread_create(VCOS_THREAD_T *thread,
165 const char *name,
166 VCOS_THREAD_ATTR_T *attrs,
167 VCOS_THREAD_ENTRY_FN_T entry,
168 void *arg)
169 {
170 VCOS_STATUS_T st;
171 pthread_attr_t pt_attrs;
172 VCOS_THREAD_ATTR_T *local_attrs = attrs ? attrs : &default_attrs;
173 int rc;
174
175 vcos_assert(thread);
176 memset(thread, 0, sizeof(VCOS_THREAD_T));
177
178 rc = pthread_attr_init(&pt_attrs);
179 if (rc < 0)
180 return VCOS_ENOMEM;
181
182 st = vcos_semaphore_create(&thread->suspend, NULL, 0);
183 if (st != VCOS_SUCCESS)
184 {
185 pthread_attr_destroy(&pt_attrs);
186 return st;
187 }
188
189 pthread_attr_setstacksize(&pt_attrs, local_attrs->ta_stacksz);
190 #if VCOS_CAN_SET_STACK_ADDR
191 if (local_attrs->ta_stackaddr)
192 {
193 pthread_attr_setstackaddr(&pt_attrs, local_attrs->ta_stackaddr);
194 }
195 #else
196 vcos_demand(local_attrs->ta_stackaddr == 0);
197 #endif
198
199 /* pthread_attr_setpriority(&pt_attrs, local_attrs->ta_priority); */
200
201 vcos_assert(local_attrs->ta_stackaddr == 0); /* Not possible */
202
203 thread->entry = entry;
204 thread->arg = arg;
205 thread->legacy = local_attrs->legacy;
206
207 strncpy(thread->name, name, sizeof(thread->name));
208 thread->name[sizeof(thread->name)-1] = '\0';
209 memset(thread->at_exit, 0, sizeof(thread->at_exit));
210
211 rc = pthread_create(&thread->thread, &pt_attrs, vcos_thread_entry, thread);
212
213 pthread_attr_destroy(&pt_attrs);
214
215 if (rc < 0)
216 {
217 vcos_semaphore_delete(&thread->suspend);
218 return VCOS_ENOMEM;
219 }
220 else
221 {
222 return VCOS_SUCCESS;
223 }
224 }
225
vcos_thread_join(VCOS_THREAD_T * thread,void ** pData)226 void vcos_thread_join(VCOS_THREAD_T *thread,
227 void **pData)
228 {
229 pthread_join(thread->thread, pData);
230 vcos_thread_cleanup(thread);
231 }
232
vcos_thread_create_classic(VCOS_THREAD_T * thread,const char * name,void * (* entry)(void * arg),void * arg,void * stack,VCOS_UNSIGNED stacksz,VCOS_UNSIGNED priaff,VCOS_UNSIGNED timeslice,VCOS_UNSIGNED autostart)233 VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_thread_create_classic(VCOS_THREAD_T *thread,
234 const char *name,
235 void *(*entry)(void *arg),
236 void *arg,
237 void *stack,
238 VCOS_UNSIGNED stacksz,
239 VCOS_UNSIGNED priaff,
240 VCOS_UNSIGNED timeslice,
241 VCOS_UNSIGNED autostart)
242 {
243 VCOS_THREAD_ATTR_T attrs;
244 vcos_thread_attr_init(&attrs);
245 vcos_thread_attr_setstacksize(&attrs, stacksz);
246 vcos_thread_attr_setpriority(&attrs, priaff & ~_VCOS_AFFINITY_MASK);
247 vcos_thread_attr_setaffinity(&attrs, priaff & _VCOS_AFFINITY_MASK);
248 (void)timeslice;
249 (void)autostart;
250
251 if (VCOS_CAN_SET_STACK_ADDR)
252 {
253 vcos_thread_attr_setstack(&attrs, stack, stacksz);
254 }
255
256 return vcos_thread_create(thread, name, &attrs, entry, arg);
257 }
258
vcos_getmicrosecs64_internal(void)259 uint64_t vcos_getmicrosecs64_internal(void)
260 {
261 struct timeval tv;
262 uint64_t tm = 0;
263
264 if (!gettimeofday(&tv, NULL))
265 {
266 tm = (tv.tv_sec * 1000000LL) + tv.tv_usec;
267 }
268
269 return tm;
270 }
271
272 #ifdef ANDROID
273
274 static int log_prio[] =
275 {
276 ANDROID_LOG_INFO, /* VCOS_LOG_UNINITIALIZED */
277 ANDROID_LOG_INFO, /* VCOS_LOG_NEVER */
278 ANDROID_LOG_ERROR, /* VCOS_LOG_ERROR */
279 ANDROID_LOG_WARN, /* VCOS_LOG_WARN */
280 ANDROID_LOG_INFO, /* VCOS_LOG_INFO */
281 ANDROID_LOG_DEBUG /* VCOS_LOG_TRACE */
282 };
283
284 int vcos_use_android_log = 1;
285 int vcos_log_to_file = 0;
286 #else
287 int vcos_use_android_log = 0;
288 int vcos_log_to_file = 0;
289 #endif
290
291 static FILE * log_fhandle = NULL;
292
vcos_vlog_default_impl(const VCOS_LOG_CAT_T * cat,VCOS_LOG_LEVEL_T _level,const char * fmt,va_list args)293 void vcos_vlog_default_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, va_list args)
294 {
295 (void)_level;
296
297 #ifdef ANDROID
298 if ( vcos_use_android_log )
299 {
300 __android_log_vprint(log_prio[_level], cat->name, fmt, args);
301 }
302 else
303 {
304 vcos_mutex_lock(&printf_lock);
305 #endif
306 if(NULL != log_fhandle)
307 {
308 if (cat->flags.want_prefix)
309 fprintf( log_fhandle, "%s: ", cat->name );
310 vfprintf(log_fhandle, fmt, args);
311 fputs("\n", log_fhandle);
312 fflush(log_fhandle);
313 }
314 #ifdef ANDROID
315 vcos_mutex_unlock(&printf_lock);
316 }
317 #endif
318 }
319
_vcos_log_platform_init(void)320 void _vcos_log_platform_init(void)
321 {
322 if(vcos_log_to_file)
323 {
324 char log_fname[100];
325 #ifdef ANDROID
326 snprintf(log_fname, 100, "/data/log/vcos_log%u.txt", vcos_process_id_current());
327 #else
328 snprintf(log_fname, 100, "/var/log/vcos_log%u.txt", vcos_process_id_current());
329 #endif
330 log_fhandle = fopen(log_fname, "w");
331 }
332 else
333 log_fhandle = stderr;
334 }
335
336 /* Flags for init/deinit components */
337 enum
338 {
339 VCOS_INIT_NAMED_SEM = (1 << 0),
340 VCOS_INIT_PRINTF_LOCK = (1 << 1),
341 VCOS_INIT_MAIN_SEM = (1 << 2),
342 VCOS_INIT_MSGQ = (1 << 3),
343 VCOS_INIT_ALL = 0xffffffff
344 };
345
vcos_term(uint32_t flags)346 static void vcos_term(uint32_t flags)
347 {
348 if (flags & VCOS_INIT_MSGQ)
349 vcos_msgq_deinit();
350
351 if (flags & VCOS_INIT_MAIN_SEM)
352 vcos_semaphore_delete(&vcos_thread_main.suspend);
353
354 #ifdef ANDROID
355 if (flags & VCOS_INIT_PRINTF_LOCK)
356 vcos_mutex_delete(&printf_lock);
357 #endif
358
359 if (flags & VCOS_INIT_NAMED_SEM)
360 _vcos_named_semaphore_deinit();
361 }
362
vcos_platform_init(void)363 VCOS_STATUS_T vcos_platform_init(void)
364 {
365 VCOS_STATUS_T st;
366 uint32_t flags = 0;
367 int pst;
368
369 st = _vcos_named_semaphore_init();
370 if (!vcos_verify(st == VCOS_SUCCESS))
371 goto end;
372
373 flags |= VCOS_INIT_NAMED_SEM;
374
375 #ifdef HAVE_MTRACE
376 /* enable glibc memory debugging, if the environment
377 * variable MALLOC_TRACE names a valid file.
378 */
379 mtrace();
380 #endif
381
382 #ifdef ANDROID
383 st = vcos_mutex_create(&printf_lock, "printf");
384 if (!vcos_verify(st == VCOS_SUCCESS))
385 goto end;
386
387 flags |= VCOS_INIT_PRINTF_LOCK;
388 #endif
389
390 st = vcos_once(¤t_thread_key_once, current_thread_key_init);
391 if (!vcos_verify(st == VCOS_SUCCESS))
392 goto end;
393
394 /* Initialise a VCOS wrapper for the thread which called vcos_init. */
395 st = vcos_semaphore_create(&vcos_thread_main.suspend, NULL, 0);
396 if (!vcos_verify(st == VCOS_SUCCESS))
397 goto end;
398
399 flags |= VCOS_INIT_MAIN_SEM;
400
401 vcos_thread_main.thread = pthread_self();
402
403 pst = pthread_setspecific(_vcos_thread_current_key, &vcos_thread_main);
404 if (!vcos_verify(pst == 0))
405 {
406 st = VCOS_EINVAL;
407 goto end;
408 }
409
410 st = vcos_msgq_init();
411 if (!vcos_verify(st == VCOS_SUCCESS))
412 goto end;
413
414 flags |= VCOS_INIT_MSGQ;
415
416 vcos_logging_init();
417
418 end:
419 if (st != VCOS_SUCCESS)
420 vcos_term(flags);
421
422 return st;
423 }
424
vcos_platform_deinit(void)425 void vcos_platform_deinit(void)
426 {
427 vcos_term(VCOS_INIT_ALL);
428 }
429
vcos_global_lock(void)430 void vcos_global_lock(void)
431 {
432 pthread_mutex_lock(&lock);
433 }
434
vcos_global_unlock(void)435 void vcos_global_unlock(void)
436 {
437 pthread_mutex_unlock(&lock);
438 }
439
vcos_thread_exit(void * arg)440 void vcos_thread_exit(void *arg)
441 {
442 VCOS_THREAD_T *thread = vcos_thread_current();
443
444 if ( thread && thread->dummy )
445 {
446 vcos_free ( (void*) thread );
447 thread = NULL;
448 }
449
450 pthread_exit(arg);
451 }
452
vcos_thread_attr_init(VCOS_THREAD_ATTR_T * attrs)453 void vcos_thread_attr_init(VCOS_THREAD_ATTR_T *attrs)
454 {
455 *attrs = default_attrs;
456 }
457
vcos_pthreads_map_error(int error)458 VCOS_STATUS_T vcos_pthreads_map_error(int error)
459 {
460 switch (error)
461 {
462 case ENOMEM:
463 return VCOS_ENOMEM;
464 case ENXIO:
465 return VCOS_ENXIO;
466 case EAGAIN:
467 return VCOS_EAGAIN;
468 case ENOSPC:
469 return VCOS_ENOSPC;
470 default:
471 return VCOS_EINVAL;
472 }
473 }
474
vcos_pthreads_map_errno(void)475 VCOS_STATUS_T vcos_pthreads_map_errno(void)
476 {
477 return vcos_pthreads_map_error(errno);
478 }
479
_vcos_task_timer_set(void (* pfn)(void *),void * cxt,VCOS_UNSIGNED ms)480 void _vcos_task_timer_set(void (*pfn)(void*), void *cxt, VCOS_UNSIGNED ms)
481 {
482 VCOS_THREAD_T *thread = vcos_thread_current();
483
484 if (thread == NULL)
485 return;
486
487 vcos_assert(thread->orig_task_timer_expiration_routine == NULL);
488
489 if (!thread->task_timer_created)
490 {
491 VCOS_STATUS_T st = vcos_timer_create(&thread->task_timer, NULL,
492 _task_timer_expiration_routine, thread);
493 (void)st;
494 vcos_assert(st == VCOS_SUCCESS);
495 thread->task_timer_created = 1;
496 }
497
498 thread->orig_task_timer_expiration_routine = pfn;
499 thread->orig_task_timer_context = cxt;
500
501 vcos_timer_set(&thread->task_timer, ms);
502 }
503
_vcos_task_timer_cancel(void)504 void _vcos_task_timer_cancel(void)
505 {
506 VCOS_THREAD_T *thread = vcos_thread_current();
507
508 if (thread == NULL || !thread->task_timer_created)
509 return;
510
511 vcos_timer_cancel(&thread->task_timer);
512 thread->orig_task_timer_expiration_routine = NULL;
513 }
514
vcos_vsnprintf(char * buf,size_t buflen,const char * fmt,va_list ap)515 int vcos_vsnprintf( char *buf, size_t buflen, const char *fmt, va_list ap )
516 {
517 return vsnprintf( buf, buflen, fmt, ap );
518 }
519
vcos_snprintf(char * buf,size_t buflen,const char * fmt,...)520 int vcos_snprintf(char *buf, size_t buflen, const char *fmt, ...)
521 {
522 int ret;
523 va_list ap;
524 va_start(ap,fmt);
525 ret = vsnprintf(buf, buflen, fmt, ap);
526 va_end(ap);
527 return ret;
528 }
529
vcos_have_rtos(void)530 int vcos_have_rtos(void)
531 {
532 return 1;
533 }
534
vcos_thread_get_name(const VCOS_THREAD_T * thread)535 const char * vcos_thread_get_name(const VCOS_THREAD_T *thread)
536 {
537 return thread->name;
538 }
539
540 #ifdef VCOS_HAVE_BACKTRACK
541 void __attribute__((weak)) vcos_backtrace_self(void);
542 #endif
543
vcos_pthreads_logging_assert(const char * file,const char * func,unsigned int line,const char * fmt,...)544 void vcos_pthreads_logging_assert(const char *file, const char *func, unsigned int line, const char *fmt, ...)
545 {
546 va_list ap;
547 va_start(ap, fmt);
548 fprintf(stderr, "assertion failure:%s:%d:%s():",
549 file, line, func);
550 vfprintf(stderr, fmt, ap);
551 va_end(ap);
552 fprintf(stderr, "\n");
553
554 #ifdef VCOS_HAVE_BACKTRACK
555 if (vcos_backtrace_self)
556 vcos_backtrace_self();
557 #endif
558 abort();
559 }
560
vcos_thread_at_exit(void (* pfn)(void *),void * cxt)561 extern VCOS_STATUS_T vcos_thread_at_exit(void (*pfn)(void*), void *cxt)
562 {
563 int i;
564 VCOS_THREAD_T *self = vcos_thread_current();
565 if (!self)
566 {
567 vcos_assert(0);
568 return VCOS_EINVAL;
569 }
570 for (i=0; i<VCOS_MAX_EXIT_HANDLERS; i++)
571 {
572 if (self->at_exit[i].pfn == NULL)
573 {
574 self->at_exit[i].pfn = pfn;
575 self->at_exit[i].cxt = cxt;
576 return VCOS_SUCCESS;
577 }
578 }
579 return VCOS_ENOSPC;
580 }
581
vcos_set_args(int argc,const char ** argv)582 void vcos_set_args(int argc, const char **argv)
583 {
584 vcos_argc = argc;
585 vcos_argv = argv;
586 }
587
vcos_get_argc(void)588 int vcos_get_argc(void)
589 {
590 return vcos_argc;
591 }
592
vcos_get_argv(void)593 const char ** vcos_get_argv(void)
594 {
595 return vcos_argv;
596 }
597
598 /* we can't inline this, because HZ comes from sys/param.h which
599 * dumps all sorts of junk into the global namespace, notable MIN and
600 * MAX.
601 */
_vcos_get_ticks_per_second(void)602 uint32_t _vcos_get_ticks_per_second(void)
603 {
604 return HZ;
605 }
606
vcos_once(VCOS_ONCE_T * once_control,void (* init_routine)(void))607 VCOS_STATUS_T vcos_once(VCOS_ONCE_T *once_control,
608 void (*init_routine)(void))
609 {
610 int rc = pthread_once(once_control, init_routine);
611 if (rc != 0)
612 {
613 switch (errno)
614 {
615 case EINVAL:
616 return VCOS_EINVAL;
617 default:
618 vcos_assert(0);
619 return VCOS_EACCESS;
620 }
621 }
622 else
623 {
624 return VCOS_SUCCESS;
625 }
626 }
627
vcos_dummy_thread_create(void)628 VCOS_THREAD_T *vcos_dummy_thread_create(void)
629 {
630 VCOS_STATUS_T st;
631 VCOS_THREAD_T *thread_hndl = NULL;
632 int rc;
633
634 thread_hndl = (VCOS_THREAD_T *)vcos_malloc(sizeof(VCOS_THREAD_T), NULL);
635 vcos_assert(thread_hndl != NULL);
636
637 memset(thread_hndl, 0, sizeof(VCOS_THREAD_T));
638
639 thread_hndl->dummy = 1;
640 thread_hndl->thread = pthread_self();
641
642 st = vcos_semaphore_create(&thread_hndl->suspend, NULL, 0);
643 if (st != VCOS_SUCCESS)
644 {
645 vcos_free(thread_hndl);
646 return( thread_hndl );
647 }
648
649 vcos_once(¤t_thread_key_once, current_thread_key_init);
650
651 rc = pthread_setspecific(_vcos_thread_current_key,
652 thread_hndl);
653 (void)rc;
654
655 return( thread_hndl );
656 }
657
658 /***********************************************************
659 *
660 * Timers
661 *
662 ***********************************************************/
663
664 /* On Linux we could use POSIX timers with a bit of synchronization.
665 * Unfortunately POSIX timers on Bionic are NOT POSIX compliant
666 * what makes that option not viable.
667 * That's why we ended up with our own implementation of timers.
668 * NOTE: That condition variables on Bionic are also buggy and
669 * they work incorrectly with CLOCK_MONOTONIC, so we have to
670 * use CLOCK_REALTIME (and hope that no one will change the time
671 * significantly after the timer has been set up
672 */
673 #define NSEC_IN_SEC (1000*1000*1000)
674 #define MSEC_IN_SEC (1000)
675 #define NSEC_IN_MSEC (1000*1000)
676
_timespec_is_zero(struct timespec * ts)677 static int _timespec_is_zero(struct timespec *ts)
678 {
679 return ((ts->tv_sec == 0) && (ts->tv_nsec == 0));
680 }
681
_timespec_set_zero(struct timespec * ts)682 static void _timespec_set_zero(struct timespec *ts)
683 {
684 ts->tv_sec = ts->tv_nsec = 0;
685 }
686
687 /* Adds left to right and stores the result in left */
_timespec_add(struct timespec * left,struct timespec * right)688 static void _timespec_add(struct timespec *left, struct timespec *right)
689 {
690 left->tv_sec += right->tv_sec;
691 left->tv_nsec += right->tv_nsec;
692 if (left->tv_nsec >= (NSEC_IN_SEC))
693 {
694 left->tv_nsec -= NSEC_IN_SEC;
695 left->tv_sec++;
696 }
697 }
698
_timespec_is_larger(struct timespec * left,struct timespec * right)699 static int _timespec_is_larger(struct timespec *left, struct timespec *right)
700 {
701 if (left->tv_sec != right->tv_sec)
702 return left->tv_sec > right->tv_sec;
703 else
704 return left->tv_nsec > right->tv_nsec;
705 }
706
_timer_thread(void * arg)707 static void* _timer_thread(void *arg)
708 {
709 VCOS_TIMER_T *timer = (VCOS_TIMER_T*)arg;
710
711 pthread_mutex_lock(&timer->lock);
712 while (!timer->quit)
713 {
714 struct timespec now;
715
716 /* Wait until next expiry time, or until timer's settings are changed */
717 if (_timespec_is_zero(&timer->expires))
718 pthread_cond_wait(&timer->settings_changed, &timer->lock);
719 else
720 pthread_cond_timedwait(&timer->settings_changed, &timer->lock, &timer->expires);
721
722 /* See if the timer has expired - reloop if it didn't */
723 clock_gettime(CLOCK_REALTIME, &now);
724 if (_timespec_is_zero(&timer->expires) || _timespec_is_larger(&timer->expires, &now))
725 continue;
726
727 /* The timer has expired. Clear the expiry time and call the
728 * expiration routine
729 */
730 _timespec_set_zero(&timer->expires);
731 timer->orig_expiration_routine(timer->orig_context);
732 }
733 pthread_mutex_unlock(&timer->lock);
734
735 return NULL;
736 }
737
vcos_timer_init(void)738 VCOS_STATUS_T vcos_timer_init(void)
739 {
740 return VCOS_SUCCESS;
741 }
742
vcos_pthreads_timer_create(VCOS_TIMER_T * timer,const char * name,void (* expiration_routine)(void * context),void * context)743 VCOS_STATUS_T vcos_pthreads_timer_create(VCOS_TIMER_T *timer,
744 const char *name,
745 void (*expiration_routine)(void *context),
746 void *context)
747 {
748 pthread_mutexattr_t lock_attr;
749 VCOS_STATUS_T result = VCOS_SUCCESS;
750 int settings_changed_initialized = 0;
751 int lock_attr_initialized = 0;
752 int lock_initialized = 0;
753
754 (void)name;
755
756 vcos_assert(timer);
757 vcos_assert(expiration_routine);
758
759 memset(timer, 0, sizeof(VCOS_TIMER_T));
760
761 timer->orig_expiration_routine = expiration_routine;
762 timer->orig_context = context;
763
764 /* Create conditional variable for notifying the timer's thread
765 * when settings change.
766 */
767 if (result == VCOS_SUCCESS)
768 {
769 int rc = pthread_cond_init(&timer->settings_changed, NULL);
770 if (rc == 0)
771 settings_changed_initialized = 1;
772 else
773 result = vcos_pthreads_map_error(rc);
774 }
775
776 /* Create attributes for the lock (we want it to be recursive) */
777 if (result == VCOS_SUCCESS)
778 {
779 int rc = pthread_mutexattr_init(&lock_attr);
780 if (rc == 0)
781 {
782 pthread_mutexattr_settype(&lock_attr, PTHREAD_MUTEX_RECURSIVE);
783 lock_attr_initialized = 1;
784 }
785 else
786 {
787 result = vcos_pthreads_map_error(rc);
788 }
789 }
790
791 /* Create lock for the timer structure */
792 if (result == VCOS_SUCCESS)
793 {
794 int rc = pthread_mutex_init(&timer->lock, &lock_attr);
795 if (rc == 0)
796 lock_initialized = 1;
797 else
798 result = vcos_pthreads_map_error(rc);
799 }
800
801 /* Lock attributes are no longer needed */
802 if (lock_attr_initialized)
803 pthread_mutexattr_destroy(&lock_attr);
804
805 /* Create the underlying thread */
806 if (result == VCOS_SUCCESS)
807 {
808 int rc = pthread_create(&timer->thread, NULL, _timer_thread, timer);
809 if (rc != 0)
810 result = vcos_pthreads_map_error(rc);
811 }
812
813 /* Clean up if anything went wrong */
814 if (result != VCOS_SUCCESS)
815 {
816 if (lock_initialized)
817 pthread_mutex_destroy(&timer->lock);
818
819 if (settings_changed_initialized)
820 pthread_cond_destroy(&timer->settings_changed);
821 }
822
823 return result;
824 }
825
vcos_pthreads_timer_set(VCOS_TIMER_T * timer,VCOS_UNSIGNED delay_ms)826 void vcos_pthreads_timer_set(VCOS_TIMER_T *timer, VCOS_UNSIGNED delay_ms)
827 {
828 struct timespec now;
829
830 vcos_assert(timer);
831
832 /* Other implementations of this function do undefined things
833 * when delay_ms is 0. This implementation will simply assert and return
834 */
835 vcos_assert(delay_ms != 0);
836 if (delay_ms == 0)
837 return;
838
839 pthread_mutex_lock(&timer->lock);
840
841 /* Calculate the new absolute expiry time */
842 clock_gettime(CLOCK_REALTIME, &now);
843 timer->expires.tv_sec = delay_ms / MSEC_IN_SEC;
844 timer->expires.tv_nsec = (delay_ms % MSEC_IN_SEC) * NSEC_IN_MSEC;
845 _timespec_add(&timer->expires, &now);
846
847 /* Notify the timer's thread about the change */
848 pthread_cond_signal(&timer->settings_changed);
849
850 pthread_mutex_unlock(&timer->lock);
851 }
852
vcos_pthreads_timer_cancel(VCOS_TIMER_T * timer)853 void vcos_pthreads_timer_cancel(VCOS_TIMER_T *timer)
854 {
855 vcos_assert(timer);
856
857 pthread_mutex_lock(&timer->lock);
858
859 _timespec_set_zero(&timer->expires);
860 pthread_cond_signal(&timer->settings_changed);
861
862 pthread_mutex_unlock(&timer->lock);
863 }
864
vcos_pthreads_timer_delete(VCOS_TIMER_T * timer)865 void vcos_pthreads_timer_delete(VCOS_TIMER_T *timer)
866 {
867 vcos_assert(timer);
868
869 pthread_mutex_lock(&timer->lock);
870
871 /* Other implementation of this function (e.g. ThreadX)
872 * disallow it being called from the expiration routine
873 */
874 vcos_assert(pthread_self() != timer->thread);
875
876 /* Stop the timer and set flag telling the timer thread to quit */
877 _timespec_set_zero(&timer->expires);
878 timer->quit = 1;
879
880 /* Notify the timer's thread about the change */
881 pthread_cond_signal(&timer->settings_changed);
882
883 /* Release the lock, so that the timer's thread can quit */
884 pthread_mutex_unlock(&timer->lock);
885
886 /* Wait for the timer thread to finish */
887 pthread_join(timer->thread, NULL);
888
889 /* Free resources used by the timer */
890 pthread_mutex_destroy(&timer->lock);
891 pthread_cond_destroy(&timer->settings_changed);
892 }
893