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

   1 #include <stdarg.h>
   2 #include <malloc.h>
   3
   4 #include <VP_Os/vp_os_thread.h>
   5
   6 typedef struct _pthread_data_t
   7 {
   8   uint32_t                       free;       // bit array representing thread array allocation state (0 means free)
   9   SUP_THREAD    thread[32]; // thread array
  10   struct _pthread_data_t* next;
  11 } pthread_data_t;
  12
  13 static pthread_data_t* threadTab = NULL;
  14
  15 static INLINE uint32_t bittest(uint32_t word, uint32_t pos)
  16 {
  17   return (word & (1 << pos));
  18 }
  19
  20 static INLINE uint32_t bitset(uint32_t word, uint32_t pos)
  21 {
  22   return word |= (1 << pos);
  23 }
  24
  25 static INLINE uint32_t bitreset(uint32_t word, uint32_t pos)
  26 {
  27   return word &= ~(1 << pos);
  28 }
  29
  30 static int32_t findFreeIndex(uint32_t word)
  31 {
  32   int32_t bit;
  33   int32_t index;
  34
  35   index = *(int32_t*)&word;
  36
  37   if( index != -1 )
  38   {
  39     index = 0;
  40     bit = 1;
  41
  42     while( word & bit )
  43     {
  44       index ++;
  45       bit <<= 1;
  46     }
  47   }
  48
  49   return index;
  50 }
  51
  52 static SUP_THREAD* findFreeSlot(void)
  53 {
  54   int32_t index;
  55   pthread_data_t *tab, *prev;
  56
  57   prev = threadTab;
  58   tab  = threadTab;
  59
  60   while( tab != NULL )
  61   {
  62     index = findFreeIndex( tab->free );
  63     if( index >= 0 )
  64     {
  65       tab->free = bitset( tab->free, index );
  66       return &tab->thread[index];
  67     }
  68
  69     prev = tab;
  70     tab = tab->next;
  71   }
  72
  73   // If we are here it means we lack free memory
  74
  75   if(prev)
  76   {
  77           prev->next = (pthread_data_t*)calloc( 1, sizeof(pthread_data_t) );
  78           tab = prev->next;
  79   }
  80   else
  81   {
  82           tab = (pthread_data_t*)calloc( 1, sizeof(pthread_data_t) );
  83           threadTab=tab;
  84   }
  85
  86
  87   tab->free = bitset( tab->free, 0 );
  88   return &tab->thread[0];
  89 }
  90
  91 void vp_os_thread_create(THREAD_ROUTINE entry, THREAD_PARAMS data, THREAD_HANDLE *handle, ...)
  92 {
  93   int32_t priority;
  94   char* name;
  95   void* stack_base;
  96   unsigned int stack_size;
  97   va_list va;
  98
  99
 100   va_start(va, (char*)handle);
 101   priority    = va_arg(va, int32_t);
 102   name        = va_arg(va, char *);
 103   stack_base  = va_arg(va, void *);
 104   stack_size  = va_arg(va, unsigned int);
 105   va_end(va);
 106
 107   SUP_THREAD* thread = findFreeSlot();
 108
 109   sup_thread_create(handle, thread, priority, entry, data, stack_size, name);
 110   sup_thread_resume(*handle);
 111 }
 112
 113 void vp_os_thread_join(THREAD_HANDLE handle)
 114 {
 115 }
 116
 117 THREAD_HANDLE vp_os_thread_self(void)
 118 {
 119   return sup_thread_current();
 120 }
 121
 122 void vp_os_thread_suspend(THREAD_HANDLE handle)
 123 {
 124   sup_thread_suspend(handle);
 125 }
 126
 127 void vp_os_thread_resume(THREAD_HANDLE handle)
 128 {
 129   sup_thread_resume(handle);
 130 }
 131
 132 void vp_os_thread_yield(void)
 133 {
 134   sup_thread_yield();
 135 }
 136
 137 void vp_os_thread_priority(THREAD_HANDLE handle, int32_t priority)
 138 {
 139   sup_thread_setpriority(handle, priority);
 140 }