vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Multiplatform/Protocol/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     threadTab = ( pthread_data_t* )vp_os_realloc( threadTab, 2 * threadTabSize * sizeof( pthread_data_t ) );
 100     vp_os_memset( threadTab + threadTabSize, 0, threadTabSize * sizeof( pthread_data_t ) );
 101
 102     threadTabSize *= 2;
 103
 104     freeSlot = findThread( NULL_THREAD_HANDLE );
 105   }
 106
 107   pthread_attr_init( &freeSlot->attr );
 108   pthread_create( &freeSlot->handle, &freeSlot->attr, f, parameters);
 109
 110   *handle = freeSlot->handle;
 111
 112   vp_os_mutex_unlock(&thread_mutex);
 113 }
 114
 115 void
 116 vp_os_thread_join(THREAD_HANDLE handle)
 117 {
 118   vp_os_mutex_lock(&thread_mutex);
 119
 120   pthread_data_t* freeSlot = findThread( handle );
 121
 122   if( freeSlot != NULL )
 123   {
 124     void *res;
 125
 126     vp_os_memset(freeSlot, 0, sizeof(pthread_data_t));
 127     pthread_join( handle, &res);
 128   }
 129
 130   vp_os_mutex_unlock(&thread_mutex);
 131 }
 132
 133 THREAD_HANDLE
 134 vp_os_thread_self(void)
 135 {
 136   return pthread_self();
 137 }
 138
 139 void
 140 vp_os_thread_suspend(THREAD_HANDLE handle)
 141 {
 142   vp_os_mutex_lock(&thread_mutex);
 143
 144   pthread_data_t* freeSlot = findThread( handle );
 145
 146   if( freeSlot != NULL )
 147   {
 148     if(!freeSlot->isSleeping)
 149     {
 150       freeSlot->isSleeping = 1;
 151       pthread_kill(handle,SIGSUSPEND);
 152     }
 153   }
 154
 155   vp_os_mutex_unlock(&thread_mutex);
 156 }
 157
 158 void
 159 vp_os_thread_resume(THREAD_HANDLE handle)
 160 {
 161   vp_os_mutex_lock(&thread_mutex);
 162
 163   pthread_data_t* freeSlot = findThread( handle );
 164
 165   if( freeSlot != NULL )
 166   {
 167     if(freeSlot->isSleeping)
 168     {
 169       pthread_kill(handle,SIGRESUME);
 170       freeSlot->isSleeping = 0;
 171     }
 172   }
 173
 174   vp_os_mutex_unlock(&thread_mutex);
 175 }
 176
 177 void
 178 vp_os_thread_yield(void)
 179 {
 180   sched_yield();
 181 }
 182
 183 void
 184 vp_os_thread_priority(THREAD_HANDLE handle, int32_t priority)
 185 {
 186   vp_os_mutex_lock(&thread_mutex);
 187
 188   pthread_data_t* freeSlot = findThread( handle );
 189
 190   if( freeSlot != NULL )
 191   {
 192     int rc, policy = SCHED_OTHER;
 193     struct sched_param param;
 194
 195     vp_os_memset(&param, 0, sizeof(param));
 196
 197     rc = pthread_getschedparam(handle, &policy, &param);
 198
 199     if( policy == SCHED_OTHER )
 200     {
 201       policy = SCHED_FIFO;
 202     }
 203     param.__sched_priority = priority;
 204     rc = pthread_setschedparam(handle, policy, &param);
 205   }
 206
 207   vp_os_mutex_unlock(&thread_mutex);
 208 }