po_hi_semaphore.c 10.5 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
   if (pthread_mutex_trylock(&sem->mutex.posix_mutex) == 0 ) {
     pthread_mutex_lock(&sem->mutex.posix_mutex);
120
   }
121
   
Antonia Francis's avatar
Antonia Francis committed
122
   /* Waiting on a condition and unlocking the mutex while doing so */
123
124
125
126
127
128
129
   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)
130
   if (rtems_semaphore_release (sem->rtems_sem) != RTEMS_SUCCESSFUL)
131
132
133
134
135
136
137
138
139
140
141
142
143
   {
      __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
144
   /* Locking the mutex */
145
146
147
148
149
   //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
150
   /* Waiting on a condition and unlocking the mutex while doing so */
151
152
153
154
155
156
157
   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
158
  /* Letting go of the ownership to wait for it later */
159
  LeaveCriticalSection(&sem->win32_criticalsection);
Antonia Francis's avatar
Antonia Francis committed
160
  /* Event object */
161
162
163
  int ret = WaitForSingleObject (&sem->win32_event,INFINITE);
  if (ret == WAIT_FAILED)
  {
Antonia Francis's avatar
Antonia Francis committed
164
     __PO_HI_DEBUG_CRITICAL ("[SEMAPHORE] Wait failed\n");
165
  }
Antonia Francis's avatar
Antonia Francis committed
166
  /* Waiting for ownership of the specified critical section object */
167
168
169
  EnterCriticalSection(&sem->win32_criticalsection);

#endif
170
  
171
172
173
   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
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)
256
if (rtems_semaphore_release (sem->rtems_sem) != RTEMS_SUCCESSFUL)
257
258
259
260
261
262
263
264
265
266
267
   {
     __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;
}