git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Os/linux/vp_os_thread.c

   1 /**
   2  * @file thread.c
   3  * @author aurelien.morelle@parrot.fr
   4  * @date 2006/12/15
   5  */
   6
   7 #include <stdarg.h>
   8 #include <signal.h>
   9 #include <sched.h>
  10
  11 #include <VP_Os/vp_os_types.h>
  12 #include <VP_Os/vp_os_thread.h>
  13 #include <VP_Os/vp_os_signal.h>
  14 #include <VP_Os/vp_os_malloc.h>
  15
  16 #define SIGRESUME  SIGUSR1
  17 #define SIGSUSPEND SIGUSR2
  18
  19 typedef struct _pthread_data_t
  20 {
  21   THREAD_HANDLE   handle;
  22   pthread_attr_t  attr;
  23   uint32_t        isSleeping;
  24 } pthread_data_t;
  25
  26 static const THREAD_HANDLE NULL_THREAD_HANDLE = 0;
  27
  28 static uint32_t threadTabSize    = 128;
  29 //static uint32_t numCreatedThread = 0;
  30 static pthread_data_t* threadTab = NULL;
  31
  32 static pthread_once_t once;
  33 static vp_os_mutex_t thread_mutex;
  34
  35
  36 static void suspendSignalHandler(int sig)
  37 {
  38   sigset_t signal_set;
  39
  40   /* Block all signals except PTHREAD_RESUME while suspended. */
  41   sigfillset(&signal_set);
  42   sigdelset(&signal_set, SIGRESUME);
  43   sigsuspend(&signal_set);
  44 }
  45
  46 static void resumeSignalHandler(int sig)
  47 {
  48 }
  49
  50 static void init_thread(void)
  51 {
  52   struct sigaction sigsuspend, sigresume;
  53
  54   vp_os_mutex_init(&thread_mutex);
  55
  56   sigresume.sa_flags = 0;
  57   sigsuspend.sa_flags = 0;
  58
  59   sigemptyset(&sigresume.sa_mask);
  60   sigemptyset(&sigsuspend.sa_mask);
  61   sigaddset(&sigsuspend.sa_mask, SIGSUSPEND);
  62   sigaddset(&sigresume.sa_mask, SIGRESUME);
  63
  64   sigresume.sa_handler  = resumeSignalHandler;
  65   sigsuspend.sa_handler = suspendSignalHandler;
  66
  67   sigaction(SIGRESUME,&sigresume,NULL);
  68   sigaction(SIGSUSPEND,&sigsuspend,NULL);
  69 }
  70
  71 static pthread_data_t* findThread(THREAD_HANDLE handle)
  72 {
  73   uint32_t i = 0;
  74
  75   if(threadTab == NULL)
  76     return NULL;
  77
  78   for(i = 0;i < threadTabSize;i++)
  79   {
  80     if( pthread_equal(handle,threadTab[i].handle) )
  81       return &threadTab[i];
  82   }
  83
  84   return NULL;
  85 }
  86
  87 void
  88 vp_os_thread_create(THREAD_ROUTINE f, THREAD_PARAMS parameters, THREAD_HANDLE *handle, ...)
  89 {
  90   pthread_data_t* freeSlot = NULL;
  91
  92   pthread_once(&once,&init_thread);
  93
  94   vp_os_mutex_lock(&thread_mutex);
  95
  96   freeSlot = findThread( NULL_THREAD_HANDLE );
  97   while(freeSlot == NULL)
  98   {
  99     /* BUG Fix on 2010/07/19 : first half of the array was never initialized. */
 100
 101           if (threadTab!=NULL){
 102                 /* Doubles the size of the array */
 103                         threadTab = ( pthread_data_t* )vp_os_realloc( threadTab, 2 * threadTabSize * sizeof( pthread_data_t ) );
 104                 /* Initializes the newly created second half */
 105                         vp_os_memset( threadTab + threadTabSize, 0, threadTabSize * sizeof( pthread_data_t ) );
 106                         threadTabSize *= 2;
 107
 108         }else{
 109                         threadTab = ( pthread_data_t* )vp_os_malloc( threadTabSize * sizeof( pthread_data_t ) );
 110                         vp_os_memset( threadTab , 0, threadTabSize * sizeof( pthread_data_t ) );
 111         }
 112
 113
 114     freeSlot = findThread( NULL_THREAD_HANDLE );
 115   }
 116
 117   pthread_attr_init( &freeSlot->attr );
 118   pthread_create( &freeSlot->handle, &freeSlot->attr, f, parameters);
 119
 120   *handle = freeSlot->handle;
 121
 122   vp_os_mutex_unlock(&thread_mutex);
 123 }
 124
 125 void
 126 vp_os_thread_join(THREAD_HANDLE handle)
 127 {
 128   vp_os_mutex_lock(&thread_mutex);
 129
 130   pthread_data_t* freeSlot = findThread( handle );
 131
 132   if( freeSlot != NULL )
 133   {
 134     void *res;
 135
 136     vp_os_memset(freeSlot, 0, sizeof(pthread_data_t));
 137     pthread_join( handle, &res);
 138   }
 139
 140   vp_os_mutex_unlock(&thread_mutex);
 141 }
 142
 143 THREAD_HANDLE
 144 vp_os_thread_self(void)
 145 {
 146   return pthread_self();
 147 }
 148
 149 void
 150 vp_os_thread_suspend(THREAD_HANDLE handle)
 151 {
 152   vp_os_mutex_lock(&thread_mutex);
 153
 154   pthread_data_t* freeSlot = findThread( handle );
 155
 156   if( freeSlot != NULL )
 157   {
 158     if(!freeSlot->isSleeping)
 159     {
 160       freeSlot->isSleeping = 1;
 161       pthread_kill(handle,SIGSUSPEND);
 162     }
 163   }
 164
 165   vp_os_mutex_unlock(&thread_mutex);
 166 }
 167
 168 void
 169 vp_os_thread_resume(THREAD_HANDLE handle)
 170 {
 171   vp_os_mutex_lock(&thread_mutex);
 172
 173   pthread_data_t* freeSlot = findThread( handle );
 174
 175   if( freeSlot != NULL )
 176   {
 177     if(freeSlot->isSleeping)
 178     {
 179       pthread_kill(handle,SIGRESUME);
 180       freeSlot->isSleeping = 0;
 181     }
 182   }
 183
 184   vp_os_mutex_unlock(&thread_mutex);
 185 }
 186
 187 void
 188 vp_os_thread_yield(void)
 189 {
 190   sched_yield();
 191 }
 192
 193 void
 194 vp_os_thread_priority(THREAD_HANDLE handle, int32_t priority)
 195 {
 196   vp_os_mutex_lock(&thread_mutex);
 197
 198   pthread_data_t* freeSlot = findThread( handle );
 199
 200   if( freeSlot != NULL )
 201   {
 202     int rc, policy = SCHED_OTHER;
 203     struct sched_param param;
 204
 205     vp_os_memset(&param, 0, sizeof(param));
 206
 207     rc = pthread_getschedparam(handle, &policy, &param);
 208
 209     if( policy == SCHED_OTHER )
 210     {
 211       policy = SCHED_FIFO;
 212     }
 213     param.sched_priority = priority;
 214
 215     rc = pthread_setschedparam(handle, policy, &param);
 216   }
 217
 218   vp_os_mutex_unlock(&thread_mutex);
 219 }