Orocos Real-Time Toolkit  2.6.0
fosi.h
00001 /***************************************************************************
00002   tag: Peter Soetens  Wed Jan 18 14:11:39 CET 2006  fosi.h
00003 
00004                         fosi.h -  description
00005                            -------------------
00006     begin                : Wed January 18 2006
00007     copyright            : (C) 2006 Peter Soetens
00008     email                : peter.soetens@mech.kuleuven.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU General Public                   *
00013  *   License as published by the Free Software Foundation;                 *
00014  *   version 2 of the License.                                             *
00015  *                                                                         *
00016  *   As a special exception, you may use this file as part of a free       *
00017  *   software library without restriction.  Specifically, if other files   *
00018  *   instantiate templates or use macros or inline functions from this     *
00019  *   file, or you compile this file and link it with other files to        *
00020  *   produce an executable, this file does not by itself cause the         *
00021  *   resulting executable to be covered by the GNU General Public          *
00022  *   License.  This exception does not however invalidate any other        *
00023  *   reasons why the executable file might be covered by the GNU General   *
00024  *   Public License.                                                       *
00025  *                                                                         *
00026  *   This library is distributed in the hope that it will be useful,       *
00027  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00028  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00029  *   Lesser General Public License for more details.                       *
00030  *                                                                         *
00031  *   You should have received a copy of the GNU General Public             *
00032  *   License along with this library; if not, write to the Free Software   *
00033  *   Foundation, Inc., 59 Temple Place,                                    *
00034  *   Suite 330, Boston, MA  02111-1307  USA                                *
00035  *                                                                         *
00036  ***************************************************************************/
00037 
00038 
00039 #ifndef _XENO_FOSI_H
00040 #define _XENO_FOSI_H
00041 
00042 #ifndef _XOPEN_SOURCE
00043 #define _XOPEN_SOURCE 600   // use all Posix features.
00044 #endif
00045 
00046 #define HAVE_FOSI_API
00047 
00048 #ifdef __cplusplus
00049 extern "C" {
00050 #endif
00051 
00052 #include "../../rtt-config.h"
00053 
00054     //Xenomai headers
00055     //#include <linux/types.h>
00056     // xenomai assumes presence of u_long
00057 #ifndef _GNU_SOURCE
00058 #define _GNU_SOURCE
00059 #endif
00060 #include <sys/mman.h>
00061 #include <sys/time.h>
00062 #include <unistd.h>
00063 #include <stdlib.h>
00064 #include <math.h>
00065 #include <stdio.h>
00066 #include <stdarg.h>
00067 #include <string.h>
00068 #include <signal.h>
00069 #include <getopt.h>
00070 #include <time.h>
00071 #include <limits.h>
00072 #include <float.h>
00073 #include "../oro_limits.h"
00074 
00075 #include <xeno_config.h> // version number
00076 #include <native/task.h>
00077 #include <native/timer.h>
00078 #include <native/mutex.h>
00079 #include <native/sem.h>
00080 #include <native/cond.h>
00081 
00082 // BC: support Xenomai < 2.3.0
00083 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2300
00084 #define rt_mutex_acquire rt_mutex_lock
00085 #define rt_mutex_release rt_mutex_unlock
00086 #endif
00087 // BC: support Xenomai < 2.5.0
00088 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2500
00089 #define ORO_XENO_HAS_ACQUIRE_UNTIL
00090 #endif
00091 
00092 
00093     typedef RT_MUTEX rt_mutex_t;
00094     typedef RT_MUTEX rt_rec_mutex_t;
00095     typedef RT_SEM rt_sem_t;
00096     typedef RT_COND rt_cond_t;
00097 
00098     // Time Related
00099     // 'S' -> Signed long long
00100     typedef SRTIME          NANO_TIME;
00101     typedef SRTIME          TICK_TIME;
00102     typedef struct timespec TIME_SPEC;
00103     typedef RT_TASK         RTOS_XENO_TASK;
00104 
00105     // Thread/Task related.
00106     typedef struct {
00107         char * name;
00108         RTOS_XENO_TASK xenotask;
00109         RTOS_XENO_TASK* xenoptr;
00110         int sched_type;
00111     } RTOS_TASK;
00112 
00113     static const TICK_TIME InfiniteTicks = LONG_LONG_MAX;
00114     static const NANO_TIME InfiniteNSecs = LONG_LONG_MAX;
00115     static const double    InfiniteSeconds = DBL_MAX;
00116 
00117 #define SCHED_XENOMAI_HARD 0 
00118 #define SCHED_XENOMAI_SOFT 1 
00119 #define ORO_SCHED_RT    0 
00120 #define ORO_SCHED_OTHER 1 
00122     // hrt is in ticks
00123 static inline TIME_SPEC ticks2timespec(TICK_TIME hrt)
00124 {
00125     TIME_SPEC timevl;
00126     timevl.tv_sec = rt_timer_tsc2ns(hrt) / 1000000000LL;
00127     timevl.tv_nsec = rt_timer_tsc2ns(hrt) % 1000000000LL;
00128     return timevl;
00129 }
00130 
00131     // hrt is in ticks
00132 static inline TICK_TIME timespec2ticks(const TIME_SPEC* ts)
00133 {
00134     return  rt_timer_ns2tsc(ts->tv_nsec + ts->tv_sec*1000000000LL);
00135 }
00136 
00137 // turn this on to have maximum detection of valid system calls.
00138 #ifdef OROSEM_OS_XENO_CHECK
00139 #define CHK_XENO_CALL() do { if(rt_task_self() == 0) { \
00140         printf("RTT: XENO NOT INITIALISED IN THIS THREAD pid=%d,\n\
00141     BUT TRIES TO INVOKE XENO FUNCTION >>%s<< ANYWAY\n", getpid(), __FUNCTION__ );\
00142         assert( rt_task_self() != 0 ); }\
00143         } while(0)
00144 #define CHK_XENO_PTR(ptr) do { if(ptr == 0) { \
00145         printf("RTT: TRIED TO PASS NULL POINTER TO XENO IN THREAD pid=%d,\n\
00146     IN TRYING TO INVOKE XENO FUNCTION >>%s<<\n", getpid(), __FUNCTION__ );\
00147         assert( ptr != 0 ); }\
00148         } while(0)
00149 #else
00150 #define CHK_XENO_CALL()
00151 #define CHK_XENO_PTR( a )
00152 #endif
00153 
00154 static inline NANO_TIME rtos_get_time_ns(void) { return rt_timer_ticks2ns(rt_timer_read()); }
00155 
00156 static inline TICK_TIME rtos_get_time_ticks(void) { return rt_timer_tsc(); }
00157 
00158 static inline TICK_TIME ticksPerSec(void) { return rt_timer_ns2tsc( 1000 * 1000 * 1000 ); }
00159 
00160 // WARNING: Orocos 'ticks' are 'Xenomai tsc' and Xenomai `ticks' are not
00161 // used in the Orocos API. Thus Orocos uses `Xenomai tsc' and `Xenomai ns',
00162 // yet Xenomai requires `Xenomai ticks' at the interface
00163 // ==> do not use nano2ticks to convert to `Xenomai ticks' because it
00164 // converts to `Xenomai tsc'.
00165 static inline TICK_TIME nano2ticks(NANO_TIME t) { return rt_timer_ns2tsc(t); }
00166 static inline NANO_TIME ticks2nano(TICK_TIME t) { return rt_timer_tsc2ns(t); }
00167 
00168 static inline int rtos_nanosleep(const TIME_SPEC *rqtp, TIME_SPEC *rmtp)
00169     {
00170         CHK_XENO_CALL();
00171         RTIME ticks = rqtp->tv_sec * 1000000000LL + rqtp->tv_nsec;
00172         rt_task_sleep( rt_timer_ns2ticks(ticks) );
00173         return 0;
00174     }
00175 
00176     static inline int rtos_sem_init(rt_sem_t* m, int value )
00177     {
00178         CHK_XENO_CALL();
00179         return rt_sem_create( m, 0, value, S_PRIO);
00180     }
00181 
00182     static inline int rtos_sem_destroy(rt_sem_t* m )
00183     {
00184         CHK_XENO_CALL();
00185         return rt_sem_delete( m );
00186     }
00187 
00188     static inline int rtos_sem_signal(rt_sem_t* m )
00189     {
00190         CHK_XENO_CALL();
00191         return rt_sem_v( m );
00192     }
00193 
00194     static inline int rtos_sem_wait(rt_sem_t* m )
00195     {
00196         CHK_XENO_CALL();
00197         return rt_sem_p( m, TM_INFINITE );
00198     }
00199 
00200     static inline int rtos_sem_trywait(rt_sem_t* m )
00201     {
00202         CHK_XENO_CALL();
00203         return rt_sem_p( m, TM_NONBLOCK);
00204     }
00205 
00206     static inline int rtos_sem_value(rt_sem_t* m )
00207     {
00208         CHK_XENO_CALL();
00209     RT_SEM_INFO sinfo;
00210         if (rt_sem_inquire(m, &sinfo) == 0 ) {
00211       return sinfo.count;
00212     }
00213     return -1;
00214     }
00215 
00216     static inline int rtos_sem_wait_timed(rt_sem_t* m, NANO_TIME delay )
00217     {
00218         CHK_XENO_CALL();
00219         return rt_sem_p(m, rt_timer_ns2ticks(delay) ) == 0 ? 0 : -1;
00220     }
00221 
00222     static inline int rtos_sem_wait_until(rt_sem_t* m, NANO_TIME when )
00223     {
00224         CHK_XENO_CALL();
00225         return rt_sem_p(m, rt_timer_ns2ticks(when) - rt_timer_read() ) == 0 ? 0 : -1;
00226     }
00227 
00228     static inline int rtos_mutex_init(rt_mutex_t* m)
00229     {
00230         CHK_XENO_CALL();
00231         // a Xenomai mutex is always recursive
00232         return rt_mutex_create(m, 0);
00233     }
00234 
00235     static inline int rtos_mutex_destroy(rt_mutex_t* m )
00236     {
00237         CHK_XENO_CALL();
00238         return rt_mutex_delete(m);
00239     }
00240 
00241     static inline int rtos_mutex_rec_init(rt_mutex_t* m)
00242     {
00243         CHK_XENO_CALL();
00244         // a Xenomai mutex is always recursive
00245         return rt_mutex_create(m, 0);
00246     }
00247 
00248     static inline int rtos_mutex_rec_destroy(rt_mutex_t* m )
00249     {
00250         CHK_XENO_CALL();
00251         return rt_mutex_delete(m);
00252     }
00253 
00254     static inline int rtos_mutex_lock( rt_mutex_t* m)
00255     {
00256         CHK_XENO_CALL();
00257         return rt_mutex_acquire(m, TM_INFINITE );
00258     }
00259 
00260     static inline int rtos_mutex_trylock( rt_mutex_t* m)
00261     {
00262         CHK_XENO_CALL();
00263         struct rt_mutex_info info;
00264         rt_mutex_inquire(m, &info );
00265 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2500
00266         if (info.locked)
00267             return 0;
00268 #else
00269         if (info.lockcnt)
00270             return 0;
00271 #endif
00272         // from here on: we're sure our thread didn't lock it
00273         // now check if any other thread locked it:
00274         return rt_mutex_acquire(m, TM_NONBLOCK);
00275     }
00276 
00277     static inline int rtos_mutex_lock_until( rt_mutex_t* m, NANO_TIME abs_time)
00278     {
00279         CHK_XENO_CALL();
00280 #if !defined(ORO_XENO_HAS_ACQUIRE_UNTIL) // see top of this file
00281         // calling the old style API
00282         return rt_mutex_acquire(m, rt_timer_ns2ticks(abs_time) - rt_timer_read()  );
00283 #else
00284         // new style API > 2.5.0
00285         return rt_mutex_acquire_until(m, rt_timer_ns2ticks(abs_time) );
00286 #endif
00287     }
00288 
00289     static inline int rtos_mutex_unlock( rt_mutex_t* m)
00290     {
00291         CHK_XENO_CALL();
00292         return rt_mutex_release(m);
00293     }
00294 
00295     static inline int rtos_mutex_rec_lock( rt_rec_mutex_t* m)
00296     {
00297         CHK_XENO_CALL();
00298         return rt_mutex_acquire(m, TM_INFINITE );
00299     }
00300 
00301     static inline int rtos_mutex_rec_trylock( rt_rec_mutex_t* m)
00302     {
00303         CHK_XENO_CALL();
00304         return rtos_mutex_trylock(m);
00305     }
00306 
00307     static inline int rtos_mutex_rec_lock_until( rt_rec_mutex_t* m, NANO_TIME abs_time)
00308     {
00309         CHK_XENO_CALL();
00310         return rtos_mutex_lock_until(m, abs_time);
00311     }
00312 
00313     static inline int rtos_mutex_rec_unlock( rt_rec_mutex_t* m)
00314     {
00315         CHK_XENO_CALL();
00316         return rt_mutex_release(m);
00317     }
00318 
00319     static inline void rtos_enable_rt_warning()
00320     {
00321         CHK_XENO_CALL();
00322         rt_task_set_mode(0, T_WARNSW, NULL);
00323     }
00324 
00325     static inline void rtos_disable_rt_warning()
00326     {
00327         CHK_XENO_CALL();
00328         rt_task_set_mode(T_WARNSW, 0, NULL);
00329     }
00330 
00331     static inline int rtos_cond_init(rt_cond_t *cond)
00332     {
00333         CHK_XENO_CALL();
00334         return rt_cond_create(cond, 0);
00335     }
00336 
00337     static inline int rtos_cond_destroy(rt_cond_t *cond)
00338     {
00339         CHK_XENO_CALL();
00340         return rt_cond_delete(cond);
00341     }
00342 
00343     static inline int rtos_cond_wait(rt_cond_t *cond, rt_mutex_t *mutex)
00344     {
00345         CHK_XENO_CALL();
00346         int ret = rt_cond_wait(cond, mutex, TM_INFINITE);
00347         if (ret == 0)
00348             return 0;
00349         return -1;
00350     }
00351 
00352     static inline int rtos_cond_timedwait(rt_cond_t *cond, rt_mutex_t *mutex, NANO_TIME abs_time)
00353     {
00354         CHK_XENO_CALL();
00355         // BC: support Xenomai < 2.5.0
00356 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2500
00357         return rt_cond_wait(cond, mutex, rt_timer_ns2ticks(abs_time) - rt_timer_read() );
00358 #else
00359         return rt_cond_wait_until(cond, mutex, rt_timer_ns2ticks(abs_time) );
00360 #endif
00361     }
00362 
00363     static inline int rtos_cond_broadcast(rt_cond_t *cond)
00364     {
00365         CHK_XENO_CALL();
00366         return rt_cond_broadcast(cond);
00367     }
00368 
00369 
00370 #define rtos_printf printf
00371 
00372 #ifdef __cplusplus
00373 }
00374 #endif
00375 
00376 #endif