Orocos Real-Time Toolkit  2.6.0
fosi.h
00001 /***************************************************************************
00002     copyright            : (C) 2008 Klaas Gadeyne
00003     email                : firstname dot lastname at gmail dot com
00004 
00005 ***************************************************************************
00006 *                                                                         *
00007 *   This program is free software; you can redistribute it and/or modify  *
00008 *   it under the terms of the GNU General Public License as published by  *
00009 *   the Free Software Foundation; either version 2 of the License, or     *
00010 *   (at your option) any later version.                                   *
00011 *                                                                         *
00012 ***************************************************************************/
00013 
00014 
00015 #ifndef __FOSI_H
00016 #define __FOSI_H
00017 
00018 #ifndef _XOPEN_SOURCE
00019 #define _XOPEN_SOURCE 600   // use all Posix features.
00020 #endif
00021 
00022 #define HAVE_FOSI_API
00023 
00024 #ifdef __cplusplus
00025 extern "C"
00026 {
00027 #endif
00028 
00029 
00030 #include <stdio.h>
00031 #include <pthread.h>
00032 #include <mach/mach_init.h>
00033 #include <mach/task.h>
00034 
00035 #include <errno.h>
00036 
00037 #include <limits.h>
00038 #include <float.h>
00039 #include <assert.h>
00040 
00041     typedef long long NANO_TIME;
00042     typedef long long TICK_TIME;
00043 
00044     static const TICK_TIME InfiniteTicks = LLONG_MAX;
00045     static const NANO_TIME InfiniteNSecs = LLONG_MAX;
00046     static const double    InfiniteSeconds = DBL_MAX;
00047 
00048 #define ORO_WAIT_ABS 0 
00050 #define ORO_WAIT_REL 1 
00053     typedef struct {
00054         pthread_t thread;
00055         pthread_attr_t attr;
00056 
00057         NANO_TIME periodMark;
00058         NANO_TIME period;
00059 
00060         char* name;
00061 
00062         int priority;
00063         int wait_policy;
00064     } RTOS_TASK;
00065 
00066 
00067 #define ORO_SCHED_RT    SCHED_FIFO 
00068 #define ORO_SCHED_OTHER SCHED_OTHER 
00070     /*
00071      * Time Related stuff
00072      */
00073 #include <sys/time.h>
00074 #include <time.h>
00075 #include <unistd.h>
00076 
00077     typedef struct timespec TIME_SPEC;
00078 
00079     /* fake clock_gettime for systems like darwin */
00080     #define  CLOCK_REALTIME 0
00081     static inline int clock_gettime(int clk_id /*ignored*/, struct timespec *tp)
00082     {
00083         struct timeval now;
00084         int rv = gettimeofday(&now, NULL);
00085         if (rv != 0){
00086             tp->tv_sec = 0;
00087             tp->tv_nsec = 0;
00088             return rv;
00089         }
00090         tp->tv_sec = now.tv_sec;
00091         tp->tv_nsec = now.tv_usec * 1000;
00092         return 0;
00093     }
00094 
00095     // high-resolution time to timespec
00096     static inline TIME_SPEC ticks2timespec(TICK_TIME hrt)
00097     {
00098         TIME_SPEC timevl;
00099         timevl.tv_sec = hrt / 1000000000LL;
00100         timevl.tv_nsec = hrt % 1000000000LL;
00101         return timevl;
00102     }
00103 
00104     static inline NANO_TIME rtos_get_time_ns( void )
00105     {
00106         TIME_SPEC tv;
00107         clock_gettime(CLOCK_REALTIME, &tv);
00108         // we can not include the C++ Time.hpp header !
00109 #ifdef __cplusplus
00110         return NANO_TIME( tv.tv_sec ) * 1000000000LL + NANO_TIME( tv.tv_nsec );
00111 #else
00112         return ( NANO_TIME ) ( tv.tv_sec * 1000000000LL ) + ( NANO_TIME ) ( tv.tv_nsec );
00113 #endif
00114     }
00115 
00120     static inline NANO_TIME rtos_get_time_ticks()
00121     {
00122         return rtos_get_time_ns();
00123     }
00124 
00125     static inline int rtos_nanosleep( const TIME_SPEC * rqtp, TIME_SPEC * rmtp )
00126     {
00127         //    return usleep(rqtp->tv_nsec/1000L);
00128         return nanosleep( rqtp, rmtp );
00129     }
00130 
00131     static inline long long nano2ticks( long long nano )
00132     {
00133         return nano;
00134     }
00135 
00136     static inline long long ticks2nano( long long count )
00137     {
00138         return count;
00139     }
00140 
00141     /*
00142      * Semaphore functions
00143      * See
00144      * http://developer.apple.com/documentation/Darwin/Conceptual/KernelProgramming/synchronization/chapter_15_section_2.html
00145      */
00146 #include <mach/semaphore.h>
00147     typedef semaphore_t rt_sem_t;
00148 
00149     static inline int rtos_sem_init(rt_sem_t* m, int value )
00150     {
00151         return semaphore_create(mach_task_self(), m, SYNC_POLICY_FIFO, value);
00152     }
00153 
00154     static inline int rtos_sem_destroy(rt_sem_t* m )
00155     {
00156         return semaphore_destroy(mach_task_self(), *m);
00157     }
00158 
00159     static inline int rtos_sem_signal(rt_sem_t* m )
00160     {
00161         return semaphore_signal(*m);
00162     }
00163 
00164     static inline int rtos_sem_wait(rt_sem_t* m )
00165     {
00166         return semaphore_wait(*m);
00167     }
00168 
00169     static inline int rtos_sem_wait_timed(rt_sem_t* m, NANO_TIME delay )
00170     {
00171         TIME_SPEC delayvl = ticks2timespec(delay);
00172         mach_timespec_t mach_delayvl = { delayvl.tv_sec, delayvl.tv_nsec };
00173 
00174         return semaphore_timedwait( *m, mach_delayvl);
00175     }
00176 
00177     static inline int rtos_sem_trywait(rt_sem_t* m )
00178     {
00179         return rtos_sem_wait_timed(m,0);
00180     }
00181 
00182     static inline int rtos_sem_wait_until(rt_sem_t* m, NANO_TIME abs_time )
00183     {
00184         TIME_SPEC timevl, delayvl;
00185         TIME_SPEC arg_time = ticks2timespec( abs_time );
00186         clock_gettime(CLOCK_REALTIME, &timevl);
00187 
00189 
00190         // calculate delay from abs_time
00191         delayvl.tv_sec = arg_time.tv_sec - timevl.tv_sec;
00192         delayvl.tv_nsec = arg_time.tv_nsec - timevl.tv_nsec;
00193         // tv_nsec is signed long in 10.6 (see sys/_structs.h)
00194         if ( delayvl.tv_nsec >= 1000000000) {
00195             ++delayvl.tv_sec;
00196             delayvl.tv_nsec -= 1000000000;
00197         }
00198         if ( delayvl.tv_nsec < 0) {
00199             --delayvl.tv_sec;
00200             delayvl.tv_nsec += 1000000000;
00201         }
00202 
00203         assert( 0 <= delayvl.tv_sec);
00204         assert( 0 <= delayvl.tv_nsec);
00205         assert( delayvl.tv_nsec < 1000000000 );
00206 
00207         mach_timespec_t mach_delayvl = { delayvl.tv_sec, delayvl.tv_nsec };
00208         int rc = semaphore_timedwait( *m, mach_delayvl);
00209         // map to return values from gnulinux, and expected by the calling layer
00210         return (KERN_OPERATION_TIMED_OUT == rc ? -1 : 0);
00211     }
00212 
00213     // semaphore_value is not supported on darwin
00214     /*     static inline int rtos_sem_value(rt_sem_t* m ) */
00215     /*     { */
00216     /*      int val = 0; */
00217     /*         if ( sem_getvalue(m, &val) == 0) */
00218     /*          return val; */
00219     /*      return -1; */
00220     /*     } */
00221 
00222     // Mutex functions - support only those needed by TLSF
00223     // opaque type
00224     typedef struct rt_mutex_impl_t rt_mutex_impl_t;
00225     // type created by tlsf.c (must work in C, not C++ code)
00226     typedef rt_mutex_impl_t* rt_mutex_t;
00227     int rtos_mutex_init(rt_mutex_t* m);
00228     int rtos_mutex_destroy(rt_mutex_t* m);
00229     int rtos_mutex_lock( rt_mutex_t* m);
00230     int rtos_mutex_unlock( rt_mutex_t* m);
00231 
00232     static inline void rtos_enable_rt_warning()
00233     {
00234     }
00235 
00236     static inline void rtos_disable_rt_warning()
00237     {
00238     }
00239 
00240 
00241 #define rtos_printf printf
00242 
00243 #ifdef __cplusplus
00244 }
00245 
00246 #endif
00247 #endif