Orocos Real-Time Toolkit  2.5.0
fosi.h
00001 /***************************************************************************
00002  tag: Peter Soetens  Mon Jun 10 14:42:55 CEST 2002  fosi.h
00003 
00004                        fosi.h -  description
00005                           -------------------
00006    begin                : Mon June 10 2002
00007    copyright            : (C) 2002 Peter Soetens
00008    email                : peter.soetens@mech.kuleuven.ac.be
00009 
00010 ***************************************************************************
00011 *                                                                         *
00012 *   This program is free software; you can redistribute it and/or modify  *
00013 *   it under the terms of the GNU General Public License as published by  *
00014 *   the Free Software Foundation; either version 2 of the License, or     *
00015 *   (at your option) any later version.                                   *
00016 *                                                                         *
00017 ***************************************************************************/
00018 
00019 
00025 #ifndef __FOSI_H
00026 #define __FOSI_H
00027 
00028 #define HAVE_FOSI_API
00029 
00030 #ifdef _MSC_VER
00031 #include <cstdio>
00032 #include <cstdlib>
00033 #include <cerrno>
00034 #include <cstring>
00035 #include <climits>
00036 #include <cfloat>
00037 #include <cassert>
00038 #else // MINGW32
00039 #include <stdio.h>
00040 #include <stdlib.h>
00041 #include <errno.h>
00042 #include <string.h>
00043 #include <limits.h>
00044 #include <float.h>
00045 #include <assert.h>
00046 #endif
00047 
00048 #include "../oro_limits.h"
00049 #include "../../rtt-config.h"
00050 
00051     // Time Related
00052 #ifdef _MSC_VER
00053 #include <ctime>
00054 #else // MINGW32
00055 #include <sys/time.h>
00056 #include <time.h>
00057 #include <unistd.h>
00058 #endif
00059 
00060 #ifdef __cplusplus
00061 extern "C"
00062 {
00063 #endif
00064 
00065 #include "dlfcn.h"
00066 
00067     RTT_API unsigned int sleep(unsigned int seconds);
00068     
00069 #if __GNUC__ != 4
00070     RTT_API int usleep(unsigned int us);
00071 #endif
00072 
00073     typedef long long NANO_TIME;
00074     typedef long long TICK_TIME;
00075 
00076     // TODO
00077     const TICK_TIME InfiniteTicks = LLONG_MAX;
00078     const NANO_TIME InfiniteNSecs = LLONG_MAX;
00079     const double    InfiniteSeconds = DBL_MAX;
00080 
00081 #define ORO_WAIT_ABS 0 
00083 #define ORO_WAIT_REL 1 
00086     typedef struct {
00087       HANDLE handle;
00088       DWORD threadId;
00089 
00090       NANO_TIME periodMark;
00091       NANO_TIME period;
00092 
00093       int sched_type; // currently not used
00094       int wait_policy;
00095 
00096       char* name;
00097     } RTOS_TASK;
00098 
00099 #define ORO_SCHED_RT    0 
00100 #define ORO_SCHED_OTHER 1 
00102     //conflicts with another struct under MSVC
00103     struct oro_timespec {
00104         long tv_sec;
00105         long tv_nsec;
00106     };
00107 
00108     typedef struct oro_timespec TIME_SPEC;
00109 
00110     // high-resolution time to timespec
00111     // hrt is in ticks
00112     inline TIME_SPEC ticks2timespec(TICK_TIME hrt)
00113     {
00114         TIME_SPEC timevl;
00115         timevl.tv_sec = (long)(hrt / 1000000000LL);
00116         timevl.tv_nsec = (long)(hrt % 1000000000LL);
00117         return timevl;
00118     }
00119 
00120     inline NANO_TIME rtos_get_time_ns( void )
00121     {
00122         LARGE_INTEGER freq;
00123         LARGE_INTEGER ticks;
00124         QueryPerformanceFrequency(&freq);
00125         QueryPerformanceCounter(&ticks);
00126 
00127         return(NANO_TIME)(((double)ticks.QuadPart * 1000000000LL) / (double)freq.QuadPart);
00128     }
00129 
00134     inline TICK_TIME rtos_get_time_ticks()
00135     {
00136         return rtos_get_time_ns();
00137     }
00138 
00139     inline int win32_nanosleep(long long nano)
00140     {
00141         NANO_TIME start = rtos_get_time_ns();
00142         timeBeginPeriod(1);
00143         if (nano > 3000000L) Sleep((DWORD)(nano/1000000L) - 1);
00144         timeEndPeriod(1);
00145         while(rtos_get_time_ns() - start < nano) Sleep(0);
00146         return 0;
00147     }
00148 
00149     inline int rtos_nanosleep( const TIME_SPEC * rqtp, TIME_SPEC * rmtp )
00150     {
00151         return win32_nanosleep((NANO_TIME)rqtp->tv_sec * 1000000000LL + rqtp->tv_nsec);
00152     }
00153 
00159     inline
00160     long long nano2ticks( long long nano )
00161     {
00162         return nano;
00163     }
00164 
00165     inline
00166     long long ticks2nano( long long count )
00167     {
00168         return count;
00169     }
00170 
00171     // Semaphore functions
00172 
00173     typedef HANDLE rt_sem_t;
00174 
00175     static inline int rtos_sem_init(rt_sem_t* m, int value )
00176     {
00177         *m = CreateSemaphore(NULL, value, 100000, NULL);
00178         return (*m != NULL) ? 0 : -1; 
00179     }
00180 
00181     static inline int rtos_sem_destroy(rt_sem_t* m )
00182     {
00183         CloseHandle(*m);
00184         return 0;
00185     }
00186 
00187     static inline int rtos_sem_signal(rt_sem_t* m )
00188     {
00189         return (ReleaseSemaphore(*m, 1, NULL) == 0) ? -1 : 0;
00190     }
00191 
00192     static inline int rtos_sem_wait(rt_sem_t* m )
00193     {
00194         return (WaitForSingleObject(*m, INFINITE) == WAIT_FAILED) ? -1 : 0;
00195     }
00196 
00197     static inline int rtos_sem_trywait(rt_sem_t* m )
00198     {
00199         return (WaitForSingleObject(*m, 0) == WAIT_TIMEOUT) ? -1 : 0;
00200     }
00201 
00202     static inline int rtos_sem_wait_timed(rt_sem_t* m, NANO_TIME delay )
00203     {
00204         return (WaitForSingleObject(*m, (DWORD)(delay/1000000)) == WAIT_TIMEOUT) ? -1 : 0;
00205     }
00206 
00207     static inline int rtos_sem_wait_until(rt_sem_t* m, NANO_TIME abs_time )
00208     {
00209         if (abs_time < rtos_get_time_ns()) return -1;
00210         NANO_TIME delay = abs_time - rtos_get_time_ns();
00211         return rtos_sem_wait_timed(m, delay);
00212     }
00213 
00214     static inline int rtos_sem_value(rt_sem_t* m )
00215     {
00216       long previous;
00217 
00218       switch (WaitForSingleObject(*m, 0)) 
00219       {
00220         case WAIT_OBJECT_0:
00221           if (!ReleaseSemaphore(*m, 1, &previous)) return -1;
00222           return previous + 1;
00223         case WAIT_TIMEOUT: return 0;
00224         default:
00225           return -1;
00226       }
00227     }
00228 
00229     // Mutex functions
00230 
00231     typedef CRITICAL_SECTION rt_mutex_t;
00232     typedef CRITICAL_SECTION rt_rec_mutex_t;
00233 
00234     static inline int rtos_mutex_init(rt_mutex_t* m)
00235     {
00236         InitializeCriticalSection(m);
00237         return 0;
00238     }
00239 
00240     static inline int rtos_mutex_destroy(rt_mutex_t* m )
00241     {
00242         DeleteCriticalSection(m);
00243         return 0;
00244     }
00245 
00246     static inline int rtos_mutex_rec_init(rt_rec_mutex_t* m)
00247     {
00248         return rtos_mutex_init(m);
00249     }
00250 
00251     static inline int rtos_mutex_rec_destroy(rt_rec_mutex_t* m )
00252     {
00253         return rtos_mutex_destroy(m);
00254     }
00255 
00256     static inline int rtos_mutex_lock( rt_mutex_t* m)
00257     {
00258           EnterCriticalSection(m);
00259         return 0;
00260     }
00261 
00262     static inline int rtos_mutex_unlock( rt_mutex_t* m)
00263     {
00264         LeaveCriticalSection(m);
00265           return 0;
00266     }
00267 
00268     static inline int rtos_mutex_trylock( rt_mutex_t* m)
00269     {
00270         if(TryEnterCriticalSection(m) != 0) return 0;
00271         return -1;
00272     }
00273 
00274     static inline int rtos_mutex_rec_trylock( rt_rec_mutex_t* m)
00275     {
00276         return rtos_mutex_trylock(m);
00277     }
00278 
00279     static inline int rtos_mutex_lock_until( rt_mutex_t* m, NANO_TIME abs_time)
00280     {
00281         while (ticks2nano(rtos_get_time_ticks()) < abs_time) 
00282             if (rtos_mutex_trylock(m) == 0) return 0;
00283         return -1;
00284     }
00285 
00286     static inline int rtos_mutex_rec_lock_until( rt_mutex_t* m, NANO_TIME abs_time)
00287     {
00288         return rtos_mutex_lock_until(m, abs_time);
00289     }
00290 
00291     static inline int rtos_mutex_rec_lock( rt_rec_mutex_t* m)
00292     {
00293         return rtos_mutex_lock(m);
00294     }
00295 
00296     static inline int rtos_mutex_rec_unlock( rt_rec_mutex_t* m)
00297     {
00298         return rtos_mutex_unlock(m);
00299     }
00300 
00301     static inline void rtos_enable_rt_warning()
00302     {
00303     }
00304 
00305     static inline void rtos_disable_rt_warning()
00306     {
00307     }
00308 
00309     // Condition variable implementation (SetEvent solution from ACE framework)
00310 
00311     typedef struct 
00312     {
00313       enum { SIGNAL = 0, BROADCAST = 1, MAX_EVENTS = 2 };
00314 
00315       HANDLE events_[MAX_EVENTS];
00316 
00317       // Count of the number of waiters.
00318       unsigned int waiters_count_;
00319       CRITICAL_SECTION waiters_count_lock_;
00320 
00321     } rt_cond_t;
00322 
00323     static inline int rtos_cond_init(rt_cond_t *cond)
00324     {
00325       // Initialize the count to 0.
00326       cond->waiters_count_ = 0;
00327 
00328       // Create an auto-reset event.
00329       cond->events_[rt_cond_t::SIGNAL] = CreateEvent (NULL,  // no security
00330                                          FALSE, // auto-reset event
00331                                          FALSE, // non-signaled initially
00332                                          NULL); // unnamed
00333 
00334       // Create a manual-reset event.
00335       cond->events_[rt_cond_t::BROADCAST] = CreateEvent (NULL,  // no security
00336                                             TRUE,  // manual-reset
00337                                             FALSE, // non-signaled initially
00338                                             NULL); // unnamed
00339 
00340       InitializeCriticalSection(&cond->waiters_count_lock_);
00341       return 0;
00342     }
00343 
00344     static inline int rtos_cond_destroy(rt_cond_t *cond)
00345     {
00346       CloseHandle(cond->events_[rt_cond_t::SIGNAL]);
00347       CloseHandle(cond->events_[rt_cond_t::BROADCAST]);
00348 
00349       DeleteCriticalSection(&cond->waiters_count_lock_);
00350 
00351       return 0;
00352     }
00353 
00354     static inline int rtos_cond_timedwait_internal(rt_cond_t *cond, rt_mutex_t *external_mutex, DWORD ms)
00355     {
00356       // Avoid race conditions.
00357       EnterCriticalSection (&cond->waiters_count_lock_);
00358       cond->waiters_count_++;
00359       LeaveCriticalSection (&cond->waiters_count_lock_);
00360 
00361       // It's ok to release the <external_mutex> here since Win32
00362       // manual-reset events maintain state when used with
00363       // <SetEvent>.  This avoids the "lost wakeup" bug...
00364       LeaveCriticalSection (external_mutex);
00365 
00366       // Wait for either event to become signaled due to <pthread_cond_signal>
00367       // being called or <pthread_cond_broadcast> being called.
00368       timeBeginPeriod(1);
00369       int result = WaitForMultipleObjects (2, cond->events_, FALSE, ms);
00370       timeEndPeriod(1);
00371 
00372       EnterCriticalSection (&cond->waiters_count_lock_);
00373       cond->waiters_count_--;
00374       bool last_waiter = result == WAIT_OBJECT_0 + rt_cond_t::BROADCAST && cond->waiters_count_ == 0;
00375       LeaveCriticalSection (&cond->waiters_count_lock_);
00376 
00377       // Some thread called <pthread_cond_broadcast>.
00378       if (last_waiter)
00379         // We're the last waiter to be notified or to stop waiting, so
00380         // reset the manual event. 
00381         ResetEvent (cond->events_[rt_cond_t::BROADCAST]); 
00382 
00383       // Reacquire the <external_mutex>.
00384       EnterCriticalSection (external_mutex);
00385 
00386       return 0;
00387     }
00388 
00389     static inline int rtos_cond_timedwait(rt_cond_t *cond, rt_mutex_t *mutex, NANO_TIME abs_time)
00390     {
00391       return rtos_cond_timedwait_internal(cond, mutex, (DWORD)(abs_time / 1000000));
00392     }
00393 
00394     static inline int rtos_cond_wait(rt_cond_t *cond, rt_mutex_t *mutex)
00395     {
00396       return rtos_cond_timedwait_internal(cond, mutex, INFINITE);
00397     }
00398 
00399     static inline int rtos_cond_broadcast(rt_cond_t *cond)
00400     {
00401       // Avoid race conditions.
00402       EnterCriticalSection (&cond->waiters_count_lock_);
00403       bool have_waiters = cond->waiters_count_ > 0;
00404       LeaveCriticalSection (&cond->waiters_count_lock_);
00405 
00406       if (have_waiters) SetEvent (cond->events_[rt_cond_t::BROADCAST]);
00407 
00408       return 0;
00409     }
00410 
00411     static inline int rtos_cond_signal (rt_cond_t *cond)
00412     {
00413       // Avoid race conditions.
00414       EnterCriticalSection (&cond->waiters_count_lock_);
00415       bool have_waiters = cond->waiters_count_ > 0;
00416       LeaveCriticalSection (&cond->waiters_count_lock_);
00417 
00418       if (have_waiters) SetEvent (cond->events_[rt_cond_t::SIGNAL]);
00419 
00420       return 0;
00421     }
00422 
00423 #define rtos_printf printf
00424 
00425 int setenv(const char *name, const char *value, int overwrite);
00426 
00427 #ifdef __cplusplus
00428 }
00429 
00430 #endif
00431 #endif