vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Os/elinux/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 /*#define SUP_THREAD_MINPRIORITY              31
  20 #define SUP_THREAD_INVALID_HANDLE NULL*/
  21
  22 typedef struct _pthread_data_t
  23 {
  24   THREAD_HANDLE   handle;
  25   pthread_attr_t  attr;
  26   uint32_t        isSleeping;
  27 } pthread_data_t;
  28
  29 static const THREAD_HANDLE NULL_THREAD_HANDLE = 0;
  30
  31 static uint32_t threadTabSize    = 0;
  32 static pthread_data_t* threadTab = NULL;
  33
  34 static pthread_once_t once;
  35 static vp_os_mutex_t thread_mutex;
  36
  37
  38 #include <VP_Os/vp_os_print.h>
  39 static void suspendSignalHandler(int sig)
  40 {
  41   sigset_t signal_set;
  42
  43   /* Block all signals except PTHREAD_RESUME while suspended. */
  44   sigfillset(&signal_set);
  45   sigdelset(&signal_set, SIGRESUME);
  46   sigsuspend(&signal_set);
  47 }
  48
  49 static void resumeSignalHandler(int sig)
  50 {
  51 }
  52
  53 static void init_thread(void) {
  54   struct sigaction sigsuspend, sigresume;
  55
  56   vp_os_mutex_init(&thread_mutex);
  57
  58   sigresume.sa_flags = 0;
  59   sigsuspend.sa_flags = 0;
  60
  61   sigemptyset(&sigresume.sa_mask);
  62   sigemptyset(&sigsuspend.sa_mask);
  63   sigaddset(&sigsuspend.sa_mask, SIGSUSPEND);
  64   sigaddset(&sigresume.sa_mask, SIGRESUME);
  65
  66   sigresume.sa_handler  = resumeSignalHandler;
  67   sigsuspend.sa_handler = suspendSignalHandler;
  68
  69   sigaction(SIGRESUME,&sigresume,NULL);
  70   sigaction(SIGSUSPEND,&sigsuspend,NULL);
  71 }
  72
  73 static pthread_data_t* findThread(THREAD_HANDLE handle)
  74 {
  75   uint32_t i = 0;
  76
  77   if(threadTab == NULL)
  78     return NULL;
  79
  80   for(i = 0;i < threadTabSize;i++)
  81   {
  82     if( pthread_equal(handle,threadTab[i].handle) )
  83       return &threadTab[i];
  84   }
  85
  86   return NULL;
  87 }
  88
  89 void
  90 vp_os_thread_create(THREAD_ROUTINE f, THREAD_PARAMS parameters, THREAD_HANDLE *handle, ...)
  91 {
  92   int32_t priority;
  93   char* name;
  94   void* stack_base;
  95   unsigned int stack_size;
  96   va_list va;
  97
  98   pthread_data_t* freeSlot = NULL;
  99
 100   pthread_once(&once,&init_thread);
 101
 102   vp_os_mutex_lock(&thread_mutex);
 103
 104   freeSlot = findThread( NULL_THREAD_HANDLE );
 105   while(freeSlot == NULL)
 106   {
 107     /* BUG Fix on 2010/07/19 : some data were written beyond the end of the array */
 108
 109     int old_size = threadTabSize;
 110     threadTabSize += 128;
 111     threadTab = ( pthread_data_t* )vp_os_realloc( threadTab, threadTabSize * sizeof( pthread_data_t ) );
 112     vp_os_memset( threadTab + old_size, 0, (threadTabSize-old_size) * sizeof( pthread_data_t ) );
 113
 114     freeSlot = findThread( NULL_THREAD_HANDLE );
 115   }
 116
 117   vp_os_mutex_unlock(&thread_mutex);
 118
 119   pthread_attr_init( &freeSlot->attr );
 120
 121   va_start(va, handle);
 122   priority    = va_arg(va, int32_t);
 123   name        = va_arg(va, char *);
 124   stack_base  = va_arg(va, void *);
 125   stack_size  = va_arg(va, unsigned int);
 126 /*  thread = va_arg(va, cyg_thread *);*/
 127   va_end(va);
 128
 129   pthread_create( &freeSlot->handle, &freeSlot->attr, f, parameters);
 130
 131   *handle = freeSlot->handle;
 132
 133   vp_os_thread_priority(freeSlot->handle, priority);
 134 }
 135
 136 void
 137 vp_os_thread_join(THREAD_HANDLE handle)
 138 {
 139   PRINT("vp_os_thread_join\n %d\n", (int)handle);
 140
 141   vp_os_mutex_lock(&thread_mutex);
 142
 143   pthread_data_t* freeSlot = findThread( handle );
 144
 145   vp_os_mutex_unlock(&thread_mutex);
 146
 147   if( freeSlot != NULL )
 148   {
 149     void *res;
 150
 151     vp_os_memset(freeSlot, 0, sizeof(pthread_data_t));
 152     pthread_join( handle, &res);
 153   }
 154 }
 155
 156 THREAD_HANDLE
 157 vp_os_thread_self(void)
 158 {
 159   return pthread_self();
 160 }
 161
 162 void
 163 vp_os_thread_suspend(THREAD_HANDLE handle)
 164 {
 165   vp_os_mutex_lock(&thread_mutex);
 166
 167   pthread_data_t* freeSlot = findThread( handle );
 168
 169   vp_os_mutex_unlock(&thread_mutex);
 170
 171   if( freeSlot != NULL )
 172   {
 173     if(!freeSlot->isSleeping)
 174     {
 175       freeSlot->isSleeping = 1;
 176       pthread_kill(handle,SIGSUSPEND);
 177     }
 178   }
 179 }
 180
 181 void
 182 vp_os_thread_resume(THREAD_HANDLE handle)
 183 {
 184   vp_os_mutex_lock(&thread_mutex);
 185
 186   pthread_data_t* freeSlot = findThread( handle );
 187
 188   vp_os_mutex_unlock(&thread_mutex);
 189
 190   if( freeSlot != NULL )
 191   {
 192     if(freeSlot->isSleeping)
 193     {
 194       pthread_kill(handle,SIGRESUME);
 195       freeSlot->isSleeping = 0;
 196     }
 197   }
 198 }
 199
 200 void
 201 vp_os_thread_yield(void)
 202 {
 203   sched_yield();
 204 }
 205
 206 void
 207 vp_os_thread_priority(THREAD_HANDLE handle, int32_t priority)
 208 {
 209   vp_os_mutex_lock(&thread_mutex);
 210
 211   pthread_data_t* freeSlot = findThread( handle );
 212
 213   vp_os_mutex_unlock(&thread_mutex);
 214
 215   if( freeSlot != NULL )
 216   {
 217     int rc, policy = SCHED_OTHER;
 218     struct sched_param param;
 219
 220     vp_os_memset(&param, 0, sizeof(param));
 221
 222     rc = pthread_getschedparam(handle, &policy, &param);
 223
 224     if( policy == SCHED_OTHER)
 225     {
 226       policy = SCHED_FIFO;
 227     }
 228     param.__sched_priority = 99-priority;
 229     rc = pthread_setschedparam(handle, policy, &param);
 230   }
 231 }