po_hi_gqueue.c 17.2 KB
Newer Older
1
2
3
4
5
6
7
/*
 * 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://ocarina.enst.fr
 *
8
 * Copyright (C) 2010, European Space Agency (ESA).
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
 */

#include <po_hi_config.h>
#include <po_hi_types.h>
#include <po_hi_messages.h>
#include <po_hi_returns.h>
#include <po_hi_transport.h>
#include <po_hi_debug.h>
#include <po_hi_gqueue.h>
/* Headers from PolyORB-HI-C */

#include <deployment.h>
#include <activity.h>
#include <request.h>
/* Headers from the generated code */

#include <string.h>
26
27

#if defined (POSIX) || defined (RTEMS_POSIX)
28
#include <pthread.h>
29
30
31
32
33
34
35
36
#elif defined(RTEMS_PURE)
#include <rtems.h>
#include <inttypes.h>
#include <po_hi_time.h>
#define __PO_HI_DEFAULT_PRIORITY RTEMS_NO_PRIORITY
#endif


37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

#define __PO_HI_GQUEUE_OUT_PORT constant_out_identifier 
/* give a default value to the out port */

__po_hi_port_t*        __po_hi_gqueues[__PO_HI_NB_TASKS];
__po_hi_int8_t         __po_hi_gqueues_nb_ports[__PO_HI_NB_TASKS];
__po_hi_int8_t*        __po_hi_gqueues_sizes[__PO_HI_NB_TASKS];
__po_hi_uint8_t*       __po_hi_gqueues_used_size[__PO_HI_NB_TASKS];
__po_hi_uint8_t*       __po_hi_gqueues_offsets[__PO_HI_NB_TASKS];
__po_hi_uint8_t*       __po_hi_gqueues_woffsets[__PO_HI_NB_TASKS];
__po_hi_uint8_t*       __po_hi_gqueues_n_destinations[__PO_HI_NB_TASKS];
__po_hi_port_t**       __po_hi_gqueues_destinations[__PO_HI_NB_TASKS];
__po_hi_uint16_t       __po_hi_gqueues_total_fifo_size[__PO_HI_NB_TASKS];
__po_hi_request_t*     __po_hi_gqueues_most_recent_values[__PO_HI_NB_TASKS];
__po_hi_uint8_t*       __po_hi_gqueues_first[__PO_HI_NB_TASKS];

__po_hi_uint8_t        __po_hi_gqueues_global_size[__PO_HI_NB_TASKS];
__po_hi_local_port_t*  __po_hi_gqueues_global_history[__PO_HI_NB_TASKS];
__po_hi_uint16_t       __po_hi_gqueues_global_history_offset[__PO_HI_NB_TASKS];
__po_hi_uint16_t       __po_hi_gqueues_global_history_woffset[__PO_HI_NB_TASKS];

__po_hi_uint8_t*       __po_hi_gqueues_port_is_empty[__PO_HI_NB_TASKS];
__po_hi_uint8_t        __po_hi_gqueues_queue_is_empty[__PO_HI_NB_TASKS];
__po_hi_uint8_t        __po_hi_gqueues_n_empty[__PO_HI_NB_TASKS];

62
#if defined (RTEMS_POSIX) || defined (POSIX)
63
64
65
66
pthread_mutex_t        __po_hi_gqueues_mutexes[__PO_HI_NB_TASKS];
pthread_cond_t         __po_hi_gqueues_conds[__PO_HI_NB_TASKS];
pthread_mutexattr_t    __po_hi_gqueues_mutexes_attr[__PO_HI_NB_TASKS];
pthread_condattr_t     __po_hi_gqueues_conds_attr[__PO_HI_NB_TASKS];
67
68
69
70
71
72
#endif

#ifdef RTEMS_PURE
rtems_id                __po_hi_gqueues_semaphores[__PO_HI_NB_TASKS];
rtems_id                __po_hi_gqueues_barriers[__PO_HI_NB_TASKS];
#endif
73

74
75
76
77
78
79
80
81
82
83
84
85
86
87
void __po_hi_gqueue_init (__po_hi_task_id       id,
                          __po_hi_uint8_t       nb_ports,
                          __po_hi_port_t        queue[],
                          __po_hi_int8_t        sizes[],
                          __po_hi_uint8_t       first[],
                          __po_hi_uint8_t       offsets[],
                          __po_hi_uint8_t       woffsets[],
                          __po_hi_uint8_t       n_dest[],
                          __po_hi_port_t*       destinations[],
                          __po_hi_uint8_t       used_size[],
                          __po_hi_local_port_t  history[],
                          __po_hi_request_t     recent[],
                          __po_hi_uint8_t       empties[],
                          __po_hi_uint16_t      total_fifo_size)
88
{
89
90
91
92
93
94
95
   __po_hi_uint8_t      tmp;
   __po_hi_uint16_t     off;
   __po_hi_request_t*   request;

#ifdef RTEMS_PURE
   rtems_status_code    ret;
#endif
96

97
98
   __po_hi_gqueues_global_history_woffset[id] = 0;
   __po_hi_gqueues_global_history_offset[id] = 0;
99

100
101
102
103
104
   __po_hi_gqueues_n_empty[id] = nb_ports;
   __po_hi_gqueues[id] = queue;
   __po_hi_gqueues_most_recent_values[id] = recent;
   __po_hi_gqueues_global_history[id] = history;
   __po_hi_gqueues_woffsets[id] = woffsets;
105

106
   __po_hi_gqueues_port_is_empty[id] = empties;
107

108
109
110
111
   __po_hi_gqueues_nb_ports[id] = nb_ports; 
   __po_hi_gqueues_sizes[id] = sizes;
   __po_hi_gqueues_first[id] = first;
   __po_hi_gqueues_used_size[id] = used_size;
112

113
114
115
116
   __po_hi_gqueues_offsets[id] = offsets;
   __po_hi_gqueues_n_destinations[id] = n_dest;
   __po_hi_gqueues_destinations[id] = destinations;
   __po_hi_gqueues_total_fifo_size[id] = total_fifo_size;
117

118
   __po_hi_gqueues_queue_is_empty[id] = 1;
119

120
#if defined (RTEMS_POSIX) || defined (POSIX)
121
122
   pthread_mutexattr_init (&__po_hi_gqueues_mutexes_attr[id]);
   pthread_condattr_init (&__po_hi_gqueues_conds_attr[id]);
123
#ifdef POSIX
124
   pthread_mutexattr_setpshared(&__po_hi_gqueues_mutexes_attr[id],PTHREAD_PROCESS_SHARED); 
125
#endif
126
127
   pthread_mutex_init (&__po_hi_gqueues_mutexes[id], &__po_hi_gqueues_mutexes_attr[id]);
   pthread_cond_init (&__po_hi_gqueues_conds[id], &__po_hi_gqueues_conds_attr[id]);
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
#endif

#ifdef RTEMS_PURE
   __DEBUGMSG ("[GQUEUE] Create semaphore for queue of task %d\n", id);
   ret = rtems_semaphore_create (rtems_build_name ('G', 'S', 'E' , 'A' + (char) id), 1, RTEMS_BINARY_SEMAPHORE, __PO_HI_DEFAULT_PRIORITY, &(__po_hi_gqueues_semaphores[id]));
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot create semaphore, error code=%d\n", ret);
   }

   __DEBUGMSG ("[GQUEUE] Create barrier for queue of task %d\n", id);
   ret = rtems_barrier_create (rtems_build_name ('G', 'S', 'I' , 'A' + (char) id),RTEMS_BARRIER_AUTOMATIC_RELEASE , 10, &(__po_hi_gqueues_barriers[id]));
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot create barrier, error code=%d\n", ret);
   }
#endif
145

146
   off = 0;
147

148
149
   for (tmp=0;tmp<nb_ports;tmp++)
   {
150
      __po_hi_gqueues_used_size[id][tmp] = 0;
151

152
      if ( (sizes[tmp] != __PO_HI_GQUEUE_FIFO_INDATA) 
153
154
155
156
157
158
159
160
            && (sizes[tmp] != __PO_HI_GQUEUE_FIFO_OUT))
      {
         __po_hi_gqueues_first[id][tmp]=off;
         off += __po_hi_gqueues_sizes[id][tmp];
         __po_hi_gqueues_offsets[id][tmp] = 0;
         __po_hi_gqueues_woffsets[id][tmp] = 0;
         __po_hi_gqueues_port_is_empty[id][tmp] = 1;
      }
161
162
163
164

      /* Set invalid all recent values */
      request = (__po_hi_request_t*)&__po_hi_gqueues_most_recent_values[id][tmp];
      request->port = __PO_HI_GQUEUE_INVALID_PORT;
165
   }
166
167

#ifdef __PO_HI_DEBUG
168
169
170
   __DEBUGMSG("Initialize global queue for task-id %d ... ", id);
   for (tmp=0;tmp<nb_ports;tmp++)
   {
171
      __DEBUGMSG("port %d (used_size=%d,first=%d) ", 
172
173
174
175
176
            tmp, 
            __po_hi_gqueues_used_size[id][tmp],
            __po_hi_gqueues_first[id][tmp]);
   }
   __DEBUGMSG(" ... done\n");
177
178
179
180
181
#endif 
}


void __po_hi_gqueue_store_out (__po_hi_task_id id, 
182
183
                               __po_hi_local_port_t port, 
                               __po_hi_request_t* request)
184
{
185
   __po_hi_request_t* ptr;
186

187
188
189
   request->port = __PO_HI_GQUEUE_OUT_PORT;
   ptr = &__po_hi_gqueues_most_recent_values[id][port];
   memcpy (ptr, request, sizeof (*request));
190
   __DEBUGMSG ("__po_hi_gqueue_store_out() from task %d on port %d\n", id, port);
191
192
}

193
194


195
__po_hi_uint8_t __po_hi_gqueue_store_in (__po_hi_task_id id, 
196
197
                                         __po_hi_local_port_t port, 
                                         __po_hi_request_t* request)
198
{
199
   __po_hi_request_t* ptr;
200
201
202
#ifdef RTEMS_PURE
   rtems_status_code ret;
#endif
203
   ptr = &__po_hi_gqueues_most_recent_values[id][port];
204
#ifdef __PO_HI_DEBUG
205
206
   if (ptr == NULL)
   {
207
      __DEBUGMSG ("__po_hi_gqueue_store_in : NULL POINTER\n");
208
   }
209
210
#endif

211
#if defined (POSIX) || defined (RTEMS_POSIX)
212
   pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
#elif defined (RTEMS_PURE)
   /*
  ret = rtems_barrier_wait (__po_hi_gqueues_barriers[id], RTEMS_WAIT);

rtems_id                __po_hi_gqueues_semaphores[__PO_HI_NB_TASKS];
rtems_id                __po_hi_gqueues_barriers[__PO_HI_NB_TASKS];
  */
   __DEBUGMSG ("[GQUEUE] Try to obtain semaphore for queue of task %d\n", id);
   ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
   }

   __DEBUGMSG ("[GQUEUE] Semaphore got\n");
#endif
229
230
   if (__po_hi_gqueues_sizes[id][port] == __PO_HI_GQUEUE_FIFO_INDATA)
   {
231
      memcpy(ptr,request,sizeof(*request));
232
233
234
   }
   else
   {
235
#ifdef __PO_HI_DEBUG
236
      __DEBUGMSG ("[GQUEUE] Received  message for task %d, port %d\n", id, port);
237
238
#endif
      if (__po_hi_gqueues_used_size[id][port] == __po_hi_gqueues_sizes[id][port])
239
      {
240
#if defined (POSIX) || defined (RTEMS_POSIX)
241
         pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
242
243
244
245
246
247
248
249
#elif defined (RTEMS_PURE)
         ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
         if (ret != RTEMS_SUCCESSFUL)
         {
            __DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
         }
#endif
         __DEBUGMSG ("[GQUEUE] QUEUE FULL, task-id=%d, port=%d", id, port);
250
251
         return __PO_HI_ERROR_QUEUE_FULL;
      }
252
253
254
255
256
257
258
259
260
261

      memcpy ((void *)&__po_hi_gqueues[id][port] + ( (__po_hi_gqueues_woffsets[id][port] + __po_hi_gqueues_first[id][port])  * sizeof (*request) ) , request, sizeof (*request));
      __po_hi_gqueues_woffsets[id][port] =  (__po_hi_gqueues_woffsets[id][port] + 1 ) % __po_hi_gqueues_sizes[id][port];

      __po_hi_gqueues_used_size[id][port]++;

      __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_woffset[id]] = port;
      __po_hi_gqueues_global_history_woffset[id] = (__po_hi_gqueues_global_history_woffset[id] + 1 ) % __po_hi_gqueues_total_fifo_size[id];

      if (__po_hi_gqueues_port_is_empty[id][port] == 1)
262
263
264
265
      {
         __po_hi_gqueues_port_is_empty[id][port] = 0;
         __po_hi_gqueues_n_empty[id]--;
      }
266
      __po_hi_gqueues_queue_is_empty[id] = 0;
267
   }
268

269
#if defined (POSIX) || defined (RTEMS_POSIX)
270
271
   pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
   pthread_cond_broadcast (&__po_hi_gqueues_conds[id]);
272
273
274
275
276
277
278
#elif defined (RTEMS_PURE)
   ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
   }
#endif
279

280
   return __PO_HI_SUCCESS;
281
282
}

283
284
void __po_hi_gqueue_wait_for_incoming_event (__po_hi_task_id id, 
                                             __po_hi_local_port_t* port)
285
{
286
287
288
289
290
291
#ifdef RTEMS_PURE
   rtems_status_code ret;
#endif


#if defined (POSIX) || defined (RTEMS_POSIX)
292
   pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
293
294
295
296
297
298
299
300
301
302
303
304
305
306
#elif defined (RTEMS_PURE)
   /*
  ret = rtems_barrier_wait (__po_hi_gqueues_barriers[id], RTEMS_WAIT);

rtems_id                __po_hi_gqueues_semaphores[__PO_HI_NB_TASKS];
rtems_id                __po_hi_gqueues_barriers[__PO_HI_NB_TASKS];
  */
   ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
  if (ret != RTEMS_SUCCESSFUL)
  {
     __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
  }
#endif

307
308
   while(__po_hi_gqueues_queue_is_empty[id] == 1)
   {
309
#if defined (POSIX) || defined (RTEMS_POSIX)
310
      pthread_cond_wait (&__po_hi_gqueues_conds[id],
311
            &__po_hi_gqueues_mutexes[id]);
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
#elif defined (RTEMS_PURE)
      ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
      if (ret != RTEMS_SUCCESSFUL)
      {
         __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
      }
      rtems_task_wake_after( RTEMS_YIELD_PROCESSOR );
      ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
      if (ret != RTEMS_SUCCESSFUL)
      {
         __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
      }

#endif

327
328
   }
   *port = __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]];
329
#if defined (POSIX) || defined (RTEMS_POSIX)
330
   pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
331
332
333
334
335
336
337
338
#elif defined (RTEMS_PURE)
   ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
   }
#endif

339
340
341
342
}

int __po_hi_gqueue_get_count( __po_hi_task_id id, __po_hi_local_port_t port)
{
343
344
   if (__po_hi_gqueues_sizes[id][port] == __PO_HI_GQUEUE_FIFO_INDATA)
   {
345
      return 1; /* data port are always of size 1 */
346
347
348
   }
   else
   {
349
      return (__po_hi_gqueues_used_size[id][port]);
350
   }
351
352
353
}

int __po_hi_gqueue_get_value( __po_hi_task_id id, 
354
355
      __po_hi_local_port_t port, 
      __po_hi_request_t* request)
356
{
357
   __po_hi_request_t* ptr;
358
359
360
361
362
#ifdef RTEMS_PURE
   rtems_status_code ret;
#endif


363
   ptr = &__po_hi_gqueues_most_recent_values[id][port];
364
#if defined (POSIX) || defined (RTEMS_POSIX)
365
   pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
366
367
368
369
370
371
372
373
374
375
376
377
378
379
#elif defined (RTEMS_PURE)
   /*
  ret = rtems_barrier_wait (__po_hi_gqueues_barriers[id], RTEMS_WAIT);

rtems_id                __po_hi_gqueues_semaphores[__PO_HI_NB_TASKS];
rtems_id                __po_hi_gqueues_barriers[__PO_HI_NB_TASKS];
  */
   ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
  if (ret != RTEMS_SUCCESSFUL)
  {
     __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
  }
#endif

380
381
382
383
384
385
386

   /*
    * If the port is an event port, with no value queued, then we block
    * the thread.
    */
   if (__po_hi_gqueues_sizes[id][port] != __PO_HI_GQUEUE_FIFO_INDATA)
   {
387
      while (__po_hi_gqueues_port_is_empty[id][port] == 1)
388
      {
389
#if defined (POSIX) || defined (RTEMS_POSIX)
390
391
         pthread_cond_wait (&__po_hi_gqueues_conds[id],
               &__po_hi_gqueues_mutexes[id]);
392
393
394
#elif defined (RTEMS_PURE)
         rtems_task_wake_after( RTEMS_YIELD_PROCESSOR );
#endif
395
396
397
398
399
      }
   }

   if (__po_hi_gqueues_used_size[id][port] == 0)
   {
400
      memcpy (request, ptr, sizeof (__po_hi_request_t));
401
402
403
   }
   else
   {
404
      memcpy (request, 
405
             (void *)&__po_hi_gqueues[id][port] + ( __po_hi_gqueues_first[id][port] + __po_hi_gqueues_offsets[id][port] )* sizeof (__po_hi_request_t), 
406
407
            sizeof (__po_hi_request_t));
   }
408
409
410
    
   
   __DEBUGMSG ("Task %d get a value on port %d\n", id, port);
411

412
413
414
   /*
    * As this part of the code is now considered as stable, we don't print debug output
    *
415

416
   __DEBUGMSG ("RECEIVED vars in gqueue: |");
417
418
419
   {
         int s;
         int i;
420
         uint8_t* tmp;
421
422
         tmp = (unsigned int*) &request->vars;
         s = sizeof (request->vars);
423
         for (i = 0 ; i < s ; i++)
424
425
426
427
428
429
430
         {
            printf("%x", *tmp);
            tmp++;
            fflush (stdout);
         }
   }
   __DEBUGMSG ("|\n");
431
#endif
432
*/
433

434
#if defined (POSIX) || defined (RTEMS_POSIX)
435
   pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
436
437
438
439
440
441
442
443
#elif defined (RTEMS_PURE)
   ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
   }
#endif

444
   return 0;
445
446
}

447
int __po_hi_gqueue_next_value (__po_hi_task_id id, __po_hi_local_port_t port)
448
{
449
450
451
452
453
#ifdef RTEMS_PURE
   rtems_status_code ret;
#endif


454
455
   /* incomplete semantics, should discriminate and report whether
      there is a next value or not */
456

457
458
459
   /* XXX change and use assert ? */
   if (__po_hi_gqueues_sizes[id][port] == __PO_HI_GQUEUE_FIFO_INDATA)
   {
460
      return 1;
461
   }
462

463
#if defined (POSIX) || defined (RTEMS_POSIX)
464
   pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
#elif defined (RTEMS_PURE)
   /*
  ret = rtems_barrier_wait (__po_hi_gqueues_barriers[id], RTEMS_WAIT);

rtems_id                __po_hi_gqueues_semaphores[__PO_HI_NB_TASKS];
rtems_id                __po_hi_gqueues_barriers[__PO_HI_NB_TASKS];
  */
   ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
  if (ret != RTEMS_SUCCESSFUL)
  {
     __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
  }
#endif


480
481
482
   __po_hi_gqueues_offsets[id][port] = 
      (__po_hi_gqueues_offsets[id][port] + 1) 
      % __po_hi_gqueues_sizes[id][port];
483

484
   __po_hi_gqueues_used_size[id][port]--;
485

486
487
   if (__po_hi_gqueues_used_size[id][port] == 0)
   {
488
489
      __po_hi_gqueues_n_empty[id]++;
      __po_hi_gqueues_port_is_empty[id][port] = 1;
490
491
492
493
   }

   if (__po_hi_gqueues_n_empty[id] == __po_hi_gqueues_nb_ports[id])
   {
494
      __po_hi_gqueues_queue_is_empty[id] = 1;
495
   }
496

497
498
499
   __po_hi_gqueues_global_history_offset[id] = 
      (__po_hi_gqueues_global_history_offset[id] + 1) 
      % __po_hi_gqueues_total_fifo_size[id];
500

501
#if defined (POSIX) || defined (RTEMS_POSIX)
502
   pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
503
504
505
506
507
508
509
510
#elif defined (RTEMS_PURE)
   ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
   if (ret != RTEMS_SUCCESSFUL)
   {
      __DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
   }
#endif

511
   return __PO_HI_SUCCESS;
512
}
513

514
__po_hi_request_t*  __po_hi_gqueue_get_most_recent_value (const __po_hi_task_id task_id, const __po_hi_local_port_t local_port)
515
516
517
518
519
520
521
522
523
524
525
526
527
{
   return (&__po_hi_gqueues_most_recent_values[task_id][local_port]);
}

uint8_t __po_hi_gqueue_get_destinations_number (const __po_hi_task_id task_id, const __po_hi_local_port_t local_port)
{
      return (__po_hi_gqueues_n_destinations[task_id][local_port]);
}

__po_hi_port_t __po_hi_gqueue_get_destination (const __po_hi_task_id task_id, const __po_hi_local_port_t local_port, const uint8_t destination_number)
{
      return (__po_hi_gqueues_destinations[task_id][local_port][destination_number]);
}