po_hi_semaphore.c 10.6 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37
/*
 * This is a part of PolyORB-HI-C distribution, a minimal
 * middleware written for generated code from AADL models.
 * You should use it with the Ocarina toolsuite.
 *
 * For more informations, please visit http://taste.tuxfamily.org/wiki
 *
 * Copyright (C) 2018 ESA & ISAE.
 */

#include <po_hi_returns.h>
#include <po_hi_config.h>

#include <po_hi_debug.h>
#include <po_hi_task.h>
#include <po_hi_types.h>
#include <po_hi_utils.h>
#include <po_hi_semaphore.h>

#include <deployment.h>
#include <assert.h>

#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
#define __USE_UNIX98 1
#include <pthread.h>

#endif

#ifdef XENO_NATIVE
#include <native/mutex.h>
#include <native/cond.h>
#endif

#ifdef __PO_HI_RTEMS_CLASSIC_API
#include <rtems.h>
#endif

Antonia Francis's avatar
Antonia Francis committed
38
/* TO INITIALIZE THE STRUCTURES */
39 40 41 42 43
int __po_hi_sem_init(__po_hi_sem_t* sem, const __po_hi_mutex_protocol_t protocol, const int priority, int id)
{
   __PO_HI_DEBUG_INFO ("[SEM] Sem Task %d is initialized", id);

#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
Antonia Francis's avatar
Antonia Francis committed
44
   /* Attribute and mutex initialization */
45 46 47 48 49 50
   if (__po_hi_mutex_init (&sem->mutex, protocol, priority) != __PO_HI_SUCCESS)
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when initializing the mutex \n");
      return __PO_HI_ERROR_SEM_CREATE;
   }

Antonia Francis's avatar
Antonia Francis committed
51
   /* Attribute and condvar initialization */
52 53 54 55 56 57 58 59 60 61 62 63
   if (pthread_condattr_init(&sem->posix_condattr) != 0)
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when initializing the cond_var attribute \n");
      /* No return here, such as in the protected API for the mutex attribute */
   }
   if (pthread_cond_init (&sem->posix_condvar, &sem->posix_condattr) != 0)
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error while creating the cond_var\n");
      return __PO_HI_ERROR_SEM_CREATE;
   }

#elif defined (XENO_NATIVE)
Antonia Francis's avatar
Antonia Francis committed
64
    /* Mutex initialization */
65 66 67 68 69 70 71
    /* The protocol and priority are unused here */
    int ret = __po_hi_mutex_init (&sem->mutex, __PO_HI_MUTEX_REGULAR, 0);
    if (ret != __PO_HI_SUCCESS)
    {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when initializing the mutex, code = %d \n, ret");
      return __PO_HI_ERROR_SEM_CREATE;
    }
Antonia Francis's avatar
Antonia Francis committed
72
    /* Conditional Variable initialization */
73 74 75 76 77 78 79 80
    ret = rt_cond_create (&mutex->xeno_condvar, NULL);
    if (ret != 0)
    {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error while creating cond_var, code = %d \n, ret");
      return __PO_HI_ERROR_SEM_CREATE;
    }

#elif defined (__PO_HI_RTEMS_CLASSIC_API)
Antonia Francis's avatar
Antonia Francis committed
81
    /* RTEMS Semaphore initialization */
82 83 84 85 86 87
    rtems_status_code code = rtems_semaphore_create (rtems_build_name ('G', 'S', 'E' , 'A' + (char) id), 1, RTEMS_BINARY_SEMAPHORE, __PO_HI_DEFAULT_PRIORITY, &sem->rtems_sem);
    if (code != RTEMS_SUCCESSFUL)
    {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Cannot create semaphore of task %d, error code=%d\n", id, code);
      return __PO_HI_ERROR_SEM_CREATE;
    }
Antonia Francis's avatar
Antonia Francis committed
88
    /* Barrier initialization */
89 90 91 92 93 94 95 96
    code = rtems_barrier_create (rtems_build_name ('G', 'S', 'I' , 'A' + (char) id),RTEMS_BARRIER_AUTOMATIC_RELEASE , 10, &sem->rtems_barrier);
    if (code != RTEMS_SUCCESSFUL)
    {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Cannot create barrier of task %d, error code=%d\n", id, code);
      return __PO_HI_ERROR_SEM_CREATE;
    }

#elif defined (_WIN32)
Antonia Francis's avatar
Antonia Francis committed
97
    /* Initializing event and critical section*/
98 99 100
    sem->win32_event = CreateEvent (NULL, FALSE, FALSE, NULL);
    if (sem->win32_event == NULL)
    {
Antonia Francis's avatar
Antonia Francis committed
101
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] CreateEvent failed (%d)\n", GetLastError());
102 103 104 105 106 107 108 109 110 111 112
      return __PO_HI_ERROR_SEM_CREATE;
    }
    InitializeCriticalSection(&sem->win32_criticalsection);

#else
#error Unsupported platform
#endif
   return (__PO_HI_SUCCESS);
}


Antonia Francis's avatar
Antonia Francis committed
113
/* TO LOCK/UNLOCK SEMAPHORES AND THEIR MUTEXES */
114 115 116
int __po_hi_sem_wait(__po_hi_sem_t* sem)
{
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
Antonia Francis's avatar
Antonia Francis committed
117
   /* Locking the mutex */
118 119 120 121 122
   if (pthread_mutex_trylock(&sem->mutex.posix_mutex) != 0 )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to trylock mutex\n");
      return __PO_HI_ERROR_SEM_WAIT;
   }
Antonia Francis's avatar
Antonia Francis committed
123
   /* Waiting on a condition and unlocking the mutex while doing so */
124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144
   if (pthread_cond_wait(&sem->posix_condvar, &sem->mutex.posix_mutex) != 0 )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to block the thread\n");
      return __PO_HI_ERROR_SEM_WAIT;
   }

#elif defined (__PO_HI_RTEMS_CLASSIC_API)
   if (rtems_semaphore_release (sem->rtems_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL)
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to release to next obtain the rtems_sem\n");
      return __PO_HI_ERROR_SEM_WAIT;
   }
   rtems_task_wake_after (1);
   //rtems_task_wake_after( RTEMS_YIELD_PROCESSOR );
   if (rtems_semaphore_obtain (sem->rtems_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL)
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to obtain the rtems_sem\n");
      return __PO_HI_ERROR_SEM_WAIT;
   }

#elif defined (XENO_NATIVE)
Antonia Francis's avatar
Antonia Francis committed
145
   /* Locking the mutex */
146 147 148 149 150
   //if (__po_hi_mutex_lock(&sem->mutex) != __PO_HI_SUCCESS )
   //{
   //   __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to acquire/lock mutex\n");
   //   return __PO_HI_ERROR_SEM_WAIT;
   //}
Antonia Francis's avatar
Antonia Francis committed
151
   /* Waiting on a condition and unlocking the mutex while doing so */
152 153 154 155 156 157 158
   if (rt_cond_wait (&sem->xeno_condvar, TM_INFINITE) != 0 )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to block the task\n");
      return __PO_HI_ERROR_SEM_WAIT;
   }

#elif defined (_WIN32)
Antonia Francis's avatar
Antonia Francis committed
159
  /* Letting go of the ownership to wait for it later */
160
  LeaveCriticalSection(&sem->win32_criticalsection);
Antonia Francis's avatar
Antonia Francis committed
161
  /* Event object */
162 163 164
  int ret = WaitForSingleObject (&sem->win32_event,INFINITE);
  if (ret == WAIT_FAILED)
  {
Antonia Francis's avatar
Antonia Francis committed
165
     __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Wait failed\n");
166
  }
Antonia Francis's avatar
Antonia Francis committed
167
  /* Waiting for ownership of the specified critical section object */
168 169 170 171 172 173
  EnterCriticalSection(&sem->win32_criticalsection);

#endif
   return __PO_HI_SUCCESS;
}

Antonia Francis's avatar
Antonia Francis committed
174
/* To work only on a mutex */
175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200
int __po_hi_sem_mutex_wait(__po_hi_sem_t* sem){

#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)|| defined (XENO_NATIVE)
  if (__po_hi_mutex_lock(&sem->mutex) != __PO_HI_SUCCESS )
  {
     __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE MUTEX] Error when trying to acquire/lock mutex\n");
     return __PO_HI_ERROR_SEM_WAIT;
  }

#elif defined (__PO_HI_RTEMS_CLASSIC_API)
  if (rtems_semaphore_obtain (sem->rtems_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL)
   {
     __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE MUTEX] Error when trying to obtain the rtems_sem\n");
     return __PO_HI_ERROR_SEM_WAIT;
   }

#elif defined (_WIN32)
  EnterCriticalSection(&sem->win32_criticalsection);
#endif
  return __PO_HI_SUCCESS;
}

int __po_hi_sem_release(__po_hi_sem_t* sem)
{
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)

Antonia Francis's avatar
Antonia Francis committed
201
   /* Unblocking a thread */
202 203 204 205 206
   if (pthread_cond_signal(&sem->posix_condvar) != 0 )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to unblock the thread\n");
      return __PO_HI_ERROR_SEM_RELEASE;
   }
Antonia Francis's avatar
Antonia Francis committed
207
   /* Unlocking the mutex */
208 209 210 211 212 213 214 215 216 217 218 219 220 221
   if (__po_hi_mutex_unlock(&sem->mutex) != __PO_HI_SUCCESS )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to unlock mutex\n");
      return __PO_HI_ERROR_SEM_RELEASE;
   }

#elif defined(__PO_HI_RTEMS_CLASSIC_API)
   if (rtems_semaphore_release (sem->rtems_sem) != RTEMS_SUCCESSFUL)
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to release the rtems_sem\n");
      return __PO_HI_ERROR_SEM_RELEASE;
   }

#elif defined (XENO_NATIVE)
Antonia Francis's avatar
Antonia Francis committed
222
   /* Unblocking all tasks */
223 224 225 226 227
   if (rt_cond_broadcast (&sem->xeno_condvar) != 0 )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to unblock the task\n");
      return __PO_HI_ERROR_SEM_RELEASE;
   }
Antonia Francis's avatar
Antonia Francis committed
228
   /* Unlocking the mutex */
229 230 231 232 233 234 235
   if (__po_hi_mutex_unlock(&sem->x_mutex) != __PO_HI_SUCCESS )
   {
      __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Error when trying to unlock mutex\n");
      return __PO_HI_ERROR_SEM_RELEASE;
   }

#elif defined (_WIN32)
Antonia Francis's avatar
Antonia Francis committed
236
  /* Waiting for ownership of the specified critical section object */
237 238 239 240 241 242 243 244 245 246
  LeaveCriticalSection(&sem->win32_criticalsection);
  if (! SetEvent(&sem->win32_event)
  {
      __DEBUGMSG("SetEvent failed (%d)\n", GetLastError());
      return __PO_HI_ERROR_SEM_RELEASE;
  }
#endif
  return __PO_HI_SUCCESS;
}

Antonia Francis's avatar
Antonia Francis committed
247
/* To work only on a mutex */
248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267
int __po_hi_sem_mutex_release(__po_hi_sem_t* sem){
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)|| defined (XENO_NATIVE)
  if (__po_hi_mutex_unlock(&sem->mutex) != __PO_HI_SUCCESS )
  {
     __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE MUTEX] Error when trying to unlock the mutex\n");
     return __PO_HI_ERROR_SEM_RELEASE;
  }
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
if (rtems_semaphore_release (sem->rtems_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL)
   {
     __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE MUTEX] Error when trying to release the rtems_sem\n");
     return __PO_HI_ERROR_SEM_RELEASE;
   }

#elif defined (_WIN32)
  EnterCriticalSection(&sem->win32_criticalsection);
#endif
  return __PO_HI_SUCCESS;
}

Antonia Francis's avatar
Antonia Francis committed
268
/* TO INITIALIZE THE SEM_GQUEUE_ARRAY */
269 270 271 272
int __po_hi_sem_init_gqueue(__po_hi_sem_t array[__PO_HI_NB_TASKS], __po_hi_task_id id)
{
  int result = __po_hi_sem_init(&array[id], __PO_HI_MUTEX_REGULAR, 0, id);
  __DEBUGMSG("SEM_INIT task number : %d, result : %d\n", id, result);
Antonia Francis's avatar
Antonia Francis committed
273

274 275 276 277 278 279 280 281 282 283 284 285 286 287

#if defined (POSIX) || defined (XENO_POSIX)
   // XXX disabled for OS X
#ifndef __MACH__ // OS X bugs on this attribute
   if (pthread_mutexattr_setpshared(&array[id].mutex.posix_mutexattr,PTHREAD_PROCESS_SHARED) !=0){
     __DEBUGMSG("MACH, setpshared task : %d, result : %d\n", id, result);
     return __PO_HI_ERROR_PTHREAD_MUTEX;
   }
#endif
#endif

  return result;
}

Antonia Francis's avatar
Antonia Francis committed
288
/* TO ACCESS TO THE SEM_GQUEUE_ARRAY */
289 290 291 292
int __po_hi_sem_wait_gqueue(__po_hi_sem_t array[__PO_HI_NB_TASKS], __po_hi_task_id id)
{
  int result = __PO_HI_SUCCESS;
  result = __po_hi_sem_wait(&array[id]);
Antonia Francis's avatar
Antonia Francis committed
293
  __DEBUGMSG("SEM_WAIT_GQUEUE task number : %d, result : %d\n", id, result);
294 295 296 297 298 299 300
  return result;
}

int __po_hi_sem_mutex_wait_gqueue(__po_hi_sem_t array[__PO_HI_NB_TASKS], __po_hi_task_id id)
{
  int result = __PO_HI_SUCCESS;
  result = __po_hi_sem_mutex_wait(&array[id]);
Antonia Francis's avatar
Antonia Francis committed
301
  __DEBUGMSG("SEM_MUTEX_WAIT_GQUEUE task number : %d, result : %d\n", id, result);
302 303 304 305 306 307 308 309
  return result;

}

int __po_hi_sem_release_gqueue(__po_hi_sem_t array[__PO_HI_NB_TASKS], __po_hi_task_id id)
{
  int result = __PO_HI_SUCCESS;
  result = __po_hi_sem_release(&array[id]);
Antonia Francis's avatar
Antonia Francis committed
310
  __DEBUGMSG("SEM_RELEASE_GQUEUE task number : %d, result : %d\n", id, result);
311 312 313 314 315 316 317
  return result;
}

int __po_hi_sem_mutex_release_gqueue(__po_hi_sem_t array[__PO_HI_NB_TASKS], __po_hi_task_id id)
{
  int result = __PO_HI_SUCCESS;
  result = __po_hi_sem_mutex_release(&array[id]);
Antonia Francis's avatar
Antonia Francis committed
318
  __DEBUGMSG("SEM_MUTEX_RELEASE_GQUEUE task number : %d, result : %d\n", id, result);
319 320
  return result;
}