po_hi_gqueue.c 32.7 KB
Newer Older
1
2
3
4
5
/*
 * 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.
 *
yoogx's avatar
yoogx committed
6
 * For more informations, please visit http://taste.tuxfamily.org/wiki
7
 *
yoogx's avatar
yoogx committed
8
 * Copyright (C) 2010-2018 ESA & ISAE.
9
10
11
12
13
14
15
16
17
 */

#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>
18
19
20
#include <po_hi_protected.h>
#include <po_hi_semaphore.h>

21
#include <po_hi_utils.h>
22
23
24
25
26
27
28
29
/* Headers from PolyORB-HI-C */

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

#include <string.h>
30
31
#include <assert.h>
#include <stdlib.h>
32

33
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
34
#include <pthread.h>
35
#elif defined(__PO_HI_RTEMS_CLASSIC_API)
36
37
38
39
#include <rtems.h>
#include <inttypes.h>
#include <po_hi_time.h>
#define __PO_HI_DEFAULT_PRIORITY RTEMS_NO_PRIORITY
40
41
42
#elif defined (XENO_NATIVE)
#include <native/cond.h>
#include <native/mutex.h>
43
44
#endif

45
#if defined (MONITORING) /* Headers from run-time verification */
yoogx's avatar
yoogx committed
46
#include <trace_manager.h>
47
#endif
48

yoogx's avatar
yoogx committed
49
#define __PO_HI_GQUEUE_OUT_PORT constant_out_identifier
50
51
/* give a default value to the out port */

52
53
54
55
56
57
58
59
60
61
62
63
64
65
/**
 * The following macro may be defined to add runtime assertions and debug for the gqueue
 */
/* #define __PO_HI_GQUEUE_ASSERTIONS */

/**
 * Pointer Array containing the whole gqueue.
 * The gqueue is divided in multiple gqueues : by task then for each task by ports.
 */
__po_hi_request_t*     __po_hi_gqueues[__PO_HI_NB_TASKS];

/**
 * Array showing the number of ports for each tasks.
 */
66
__po_hi_port_id_t      __po_hi_gqueues_nb_ports[__PO_HI_NB_TASKS];
67
68
69
70
71

/**
 * Array showing the size of the FIFO for each port of each task, or
 * __PO_HI_GQUEUE_FIFO_OUT if this is an out port.
 */
72
__po_hi_port_id_t*     __po_hi_gqueues_sizes[__PO_HI_NB_TASKS];
73
74
75
76

/**
 * Array showing the effective size used for each port (for each task).
 */
77
__po_hi_port_id_t*     __po_hi_gqueues_used_size[__PO_HI_NB_TASKS];
78
79
80
81
82
83
84

/**
 * Array showing the offset necessary to add from the beginning of a
 * port gqueue to access the part allowed to reading.  When adding
 * that offset to the beginning of the PORT gqueue, you enter the part
 * of the port gqueue in which the read value get input.
 */
85
__po_hi_port_id_t*     __po_hi_gqueues_offsets[__PO_HI_NB_TASKS];
86
87
88
89
90
91
92

/**
 * Array showing the offset necessary to add from the beginning of a
 * port gqueue to access the part allowed to writing.  When adding
 * that offset to the beginning of the PORT gqueue, you enter the part
 * of the port gqueue in which the written value get input.
 */
93
__po_hi_port_id_t*     __po_hi_gqueues_woffsets[__PO_HI_NB_TASKS];
94
95
96
97

/**
 * Array showing the number of destinations for each port.
 */
98
__po_hi_port_id_t*     __po_hi_gqueues_n_destinations[__PO_HI_NB_TASKS];
99
100
101
102

/**
 * Array showing the destination for each port.
 */
103
__po_hi_port_t**       __po_hi_gqueues_destinations[__PO_HI_NB_TASKS];
104
105
106
107
108

/**
 * Array showing the size of the FIFO gqueue for each task
 * (subdivision by tasks of the whole gqueue).
 */
109
__po_hi_uint32_t       __po_hi_gqueues_total_fifo_size[__PO_HI_NB_TASKS];
110
111
112
113

/**
 * Array showing the most recent value added (for each port of each task).
 */
114
__po_hi_request_t*     __po_hi_gqueues_most_recent_values[__PO_HI_NB_TASKS];
115
116
117
118
119
120
121

/**
 * Array showing the offset necessary to add from the beginning of a
 * task gqueue to access a specified port gqueue.  When adding that
 * offset to the beginning of the TASK gqueue, you enter the beginning
 * of the port gqueue.
 */
122
__po_hi_port_id_t*     __po_hi_gqueues_first[__PO_HI_NB_TASKS];
123

124
125
126
127
128
129
130
131
132
133
134
135
136
/**
 * Unused.
 */
__po_hi_port_id_t      __po_hi_gqueues_global_size[__PO_HI_NB_TASKS];

/**
 * Array helping in managing the offsets and woffsets in integers.
 */
__po_hi_local_port_t*  __po_hi_gqueues_global_history[__PO_HI_NB_TASKS];

/**
 * Array in managing the offsets in integers.
 */
137
__po_hi_uint32_t        __po_hi_gqueues_global_history_offset[__PO_HI_NB_TASKS];
138

139
140
141
142
/**
 * Array in managing the woffsets in integers.
 */
__po_hi_uint32_t       __po_hi_gqueues_global_history_woffset[__PO_HI_NB_TASKS];
143

144
145
146
147
/**
 * Array showing whether the queue of a specified port of a task is empty (1) or not (0).
 */
__po_hi_port_id_t*     __po_hi_gqueues_port_is_empty[__PO_HI_NB_TASKS];
148

149
150
151
152
/**
 * Array showing whether the global queue of a task is empty (1) or not (0).
 */
__po_hi_port_id_t      __po_hi_gqueues_queue_is_empty[__PO_HI_NB_TASKS];
153

154
155
156
157
158
159
160
/**
 * Array counting how many ports gqueue of a specified task are empty.
 * If the number of empty ports (n_empty) is equal to the number of
 * ports (nb_ports), then the queue is declared empty in the array
 * __po_hi_gqueues_queue_is_empty.
 */
__po_hi_port_id_t      __po_hi_gqueues_n_empty[__PO_HI_NB_TASKS];
161

162
163
164
165
/**
 * Array containing the semaphores for each tasks.
 */
__po_hi_sem_t          __po_hi_gqueues_semaphores[__PO_HI_NB_TASKS];
166

167
/******************************************************************************/
168
void __po_hi_gqueue_init (__po_hi_task_id       id,
169
                          __po_hi_port_id_t     nb_ports,
170
                          __po_hi_request_t     queue[],
171
172
173
174
175
                          __po_hi_port_id_t     sizes[],
                          __po_hi_port_id_t     first[],
                          __po_hi_port_id_t     offsets[],
                          __po_hi_port_id_t     woffsets[],
                          __po_hi_port_id_t     n_dest[],
176
                          __po_hi_port_t*       destinations[],
177
                          __po_hi_port_id_t     used_size[],
178
179
                          __po_hi_local_port_t  history[],
                          __po_hi_request_t     recent[],
180
181
                          __po_hi_port_id_t     empties[],
                          __po_hi_uint32_t      total_fifo_size)
182
{
183
184
  __po_hi_port_id_t    tmp;
  __po_hi_uint32_t     off; /* XXX May overflow for large value .. */
185

186
187
  __po_hi_gqueues_global_history_woffset[id] = 0;
  __po_hi_gqueues_global_history_offset[id] = 0;
188

189
190
191
192
193
  __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;
194

195
  __po_hi_gqueues_port_is_empty[id] = empties;
196

197
198
199
200
  __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;
201

202
203
204
205
  __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;
206

207
  __po_hi_gqueues_queue_is_empty[id] = 1;
208

Antonia Francis's avatar
Antonia Francis committed
209
  /* Using the semaphore API to initialize the semaphore_gqueue array */
210
211
212
  int res = __po_hi_sem_init_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_INIT %d %d\n", id, res);
  assert(res == __PO_HI_SUCCESS);
213

214
215
216
  off = 0;
  for (tmp=0;tmp<nb_ports;tmp++)
    {
217
      __po_hi_gqueues_used_size[id][tmp] = 0;
yoogx's avatar
yoogx committed
218
      if ( (sizes[tmp] != __PO_HI_GQUEUE_FIFO_INDATA)
219
220
221
222
223
224
225
226
           && (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;
        }
227

228
      /* Set invalid all recent values */
229
      __po_hi_request_t* request = (__po_hi_request_t*)&__po_hi_gqueues_most_recent_values[id][tmp];
230
      request->port = __PO_HI_GQUEUE_INVALID_PORT;
231
    }
232
233

#ifdef __PO_HI_DEBUG
234
235
236
  __DEBUGMSG("Initialize global queue for task-id %d ... ", id);
  for (tmp=0;tmp<nb_ports;tmp++)
    {
yoogx's avatar
yoogx committed
237
      __DEBUGMSG("port %d (used_size=%d,first=%d) ",
238
239
240
241
242
                 tmp,
                 __po_hi_gqueues_used_size[id][tmp],
                 __po_hi_gqueues_first[id][tmp]);
    }
  __DEBUGMSG(" ... done\n");
yoogx's avatar
yoogx committed
243
#endif
244

245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
  __PO_HI_DEBUG_DEBUG("Initialize global queue for task %d , first = %d, history_offset = %d, history_woffset = %d, fifo size = %d, gqueue_id adress = %d\n\n", id, __po_hi_gqueues_first[id],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_total_fifo_size[id],__po_hi_gqueues[id] );

#if defined __PO_HI_GQUEUE_ASSERTIONS
  __DEBUGMSG("\nInitialization parameter");
  assert(__po_hi_gqueues_global_history_woffset[id] == 0);
  assert(__po_hi_gqueues_global_history_offset[id] == 0);
  assert(__po_hi_gqueues_n_empty[id] == nb_ports);
  assert(__po_hi_gqueues[id] == queue);
  assert(__po_hi_gqueues_most_recent_values[id] == recent);
  assert(__po_hi_gqueues_global_history[id] == history);
  assert(__po_hi_gqueues_woffsets[id] == woffsets);
  assert(__po_hi_gqueues_port_is_empty[id] == empties);
  assert(__po_hi_gqueues_nb_ports[id] == nb_ports);
  assert(__po_hi_gqueues_sizes[id] == sizes);
  assert(__po_hi_gqueues_first[id] == first);
  assert(__po_hi_gqueues_used_size[id] == used_size);
  assert(__po_hi_gqueues_offsets[id]           == offsets);
  assert(__po_hi_gqueues_n_destinations[id]    == n_dest);
  assert(__po_hi_gqueues_destinations[id]      == destinations);
  assert(__po_hi_gqueues_total_fifo_size[id]   == total_fifo_size);
  assert(__po_hi_gqueues_queue_is_empty[id] = 1);

  for (__po_hi_port_id_t i = 0; i < nb_ports; i++){
    assert(__po_hi_gqueues_used_size[id][i] == 0);
    assert(__po_hi_gqueues_most_recent_values[id][i].port == __PO_HI_GQUEUE_INVALID_PORT);
    if (i > 0){
271
      /* Usually HAS TO be right */
272
273
274
275
276
      //assert(__po_hi_gqueues_first[id][i] >= 0);
    }
  }
#endif
}
277

278
/******************************************************************************/
yoogx's avatar
yoogx committed
279
280
void __po_hi_gqueue_store_out (__po_hi_task_id id,
                               __po_hi_local_port_t port,
281
                               __po_hi_request_t* request)
282
{
283
  __po_hi_request_t* ptr;
284

285
286
287
288
  request->port = __PO_HI_GQUEUE_OUT_PORT;
  ptr = &__po_hi_gqueues_most_recent_values[id][port];
  memcpy (ptr, request, sizeof (__po_hi_request_t));
  __PO_HI_DEBUG_DEBUG ("\n__Po_hi_gqueue_store_out() from task %d on port %d\n", id, port);
yoogx's avatar
yoogx committed
289
290

#if defined (MONITORING)
291
292
  __DEBUGMSG("\nThe last value is the request to be stored");
  record_event(ANY, STORE_OUT, id, invalid_port_t, invalid_port_t, port, invalid_local_port_t, request);
yoogx's avatar
yoogx committed
293
294
#endif

295
296
}

297
/******************************************************************************/
298
__po_hi_port_id_t __po_hi_gqueue_store_in (__po_hi_task_id id,
299
300
                                           __po_hi_local_port_t port,
                                           __po_hi_request_t* request)
301
{
302

303
304
305
306
307
308
309
310
311
312
#ifdef __PO_HI_GQUEUE_ASSERTIONS
  __po_hi_port_id_t init_woffset         = __po_hi_gqueues_woffsets[id][port];
  __po_hi_uint32_t  init_history_woffset = __po_hi_gqueues_global_history_woffset[id];
  __po_hi_port_id_t init_used_size       = __po_hi_gqueues_used_size[id][port];
  __po_hi_port_id_t is_empty             = __po_hi_gqueues_port_is_empty[id][port];
  __po_hi_port_id_t nb_empty             =  __po_hi_gqueues_n_empty[id];
#endif

  __po_hi_request_t* ptr;
  __po_hi_request_t* tmp;
313

314
  ptr = &__po_hi_gqueues_most_recent_values[id][port];
315
#ifdef __PO_HI_DEBUG
316
317
  if (ptr == NULL)
    {
318
      __DEBUGMSG ("__po_hi_gqueue_store_in : NULL POINTER\n");
319
    }
320
#endif
Antonia Francis's avatar
Antonia Francis committed
321
  /* Locking only a mutex */
322
  __PO_HI_DEBUG_DEBUG ("\nWaiting on Store_in on task %d, port = %d, size of port = %d\n", id, port,__po_hi_gqueue_get_port_size(id, port));
323
  int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
Antonia Francis's avatar
Antonia Francis committed
324
  __DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT on task %d result = %d\n", id, result);
325
  assert(result == __PO_HI_SUCCESS);
yoogx's avatar
yoogx committed
326

327
328
329
  if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA)
    {
      memcpy(ptr,request,sizeof(*request));
yoogx's avatar
yoogx committed
330
      __PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE, for a FIFO_INDATA port, the used_size is always at 0 (not augmented in a store_in) task-id=%d, port=%d\n", id, port);
331
332
333
334
    }
  else
    {
      __DEBUGMSG ("[GQUEUE] Received  message for task %d, port %d\n", id, port);
yoogx's avatar
yoogx committed
335

336
      if (__po_hi_gqueue_used_size(id,port) == __po_hi_gqueue_get_port_size(id,port))
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
        {
          /* Releasing only a mutex */
          int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
          __DEBUGMSG("GQUEUE_SEM_MTUEX_RELEASE %d %d\n", id, res);
          assert(res == __PO_HI_SUCCESS);

          __PO_HI_DEBUG_CRITICAL ("[GQUEUE] QUEUE FULL, task-id=%d, port=%d\n", id, port);

          __DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id);
          return __PO_HI_ERROR_QUEUE_FULL;
        }

      __PO_HI_DEBUG_DEBUG("\nBefore store_in for task-id %d , port %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueue id adress = %d,\n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);

      /* The program ensures to write the information at the right place in the buffer.
       *
       * The right first offset has to be applied so that the right
       * port is chosen.  The right woffset (writing_offset) has to be
       * applied not to erase fresh information.
       */
357
      __po_hi_uint32_t   size;
358
      tmp =  __po_hi_gqueues[id];
359
      size = __po_hi_gqueues_woffsets[id][port] + __po_hi_gqueues_first[id][port];
360

jdelange's avatar
jdelange committed
361
      tmp = tmp + size;
362
      __PO_HI_DEBUG_DEBUG(" Store_in first + woffsets = %d, first = %d, gqueue_id adress = %d, tmp (adress + woffset + first)= %d,\n\n", __po_hi_gqueues_first[id][port] + __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_first[id][port],__po_hi_gqueues[id], tmp);
363
364

      memcpy (tmp , request, sizeof (__po_hi_request_t));
jdelange's avatar
jdelange committed
365

366
367
      __po_hi_gqueues_woffsets[id][port] = (__po_hi_gqueues_woffsets[id][port] + 1 ) % __po_hi_gqueues_sizes[id][port];
      __PO_HI_DEBUG_DEBUG ("\nBefore used_size ++, Store_in for task = %d, __po_hi_gqueues_used_size[id][port] = %d\n", id, __po_hi_gqueues_used_size[id][port]);
368
      __po_hi_gqueues_used_size[id][port]++;
369
      __PO_HI_DEBUG_DEBUG ("\nAfter used_size ++ , Store_in for task = %d, __po_hi_gqueues_used_size[id][port] = %d\n",id,  __po_hi_gqueues_used_size[id][port]);
370
      __PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueue_used_size(id,port), id, port);
371

372
      /* The port where information has been written is stored */
373
374
375
376
      __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)
377
378
379
380
        {
          __po_hi_gqueues_port_is_empty[id][port] = 0;
          __po_hi_gqueues_n_empty[id]--;
        }
381
      __po_hi_gqueues_queue_is_empty[id] = 0;
382
383
384
385
    }

  __PO_HI_DEBUG_DEBUG("\nAfter store_in for task-id %d , port %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueue_id adress= %d, \n\n", id, port,  __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);

Antonia Francis's avatar
Antonia Francis committed
386
  /* Releasing a complete semaphore */
387
388
389
390
  int rel = __po_hi_sem_release_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_RELEASE %d %d\n", id, rel);
  assert(rel == __PO_HI_SUCCESS);
  __DEBUGMSG ("[GQUEUE] store_in completed\n");
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416

#ifdef __PO_HI_GQUEUE_ASSERTIONS
  /* The port length is superior to 1 */
  if ((__po_hi_gqueue_get_port_size(id,port) != __PO_HI_GQUEUE_FIFO_INDATA)&&(init_used_size != __po_hi_gqueue_get_port_size(id,port))){
    __DEBUGMSG("\nThe woffset should be incremented by one and stay inferior to the port size");
    assert(__po_hi_gqueues_woffsets[id][port] == (init_woffset + 1)% __po_hi_gqueues_sizes[id][port]);
    assert(__po_hi_gqueues_woffsets[id][port] < __po_hi_gqueues_sizes[id][port]);
    __DEBUGMSG("\nThe effective port size used should be incremented by one");
    assert (__po_hi_gqueues_used_size[id][port] == init_used_size +1);
    __DEBUGMSG("\nThe port array is filled by the right port so that the reading is done at the right port");
    assert (__po_hi_gqueues_global_history[id][init_history_woffset] == port);
    __DEBUGMSG("The woffset_index should then be incremented by one and stay inferior to the fifo size");
    assert(__po_hi_gqueues_global_history_woffset[id] == (init_history_woffset + 1)% __po_hi_gqueues_total_fifo_size[id]);
    assert(__po_hi_gqueues_global_history_woffset[id] < __po_hi_gqueues_total_fifo_size[id]);
    __DEBUGMSG("\nIf this port queue was empty, the number of empty port is reduced by 1");
    /* The port was empty */
    if (is_empty == 1){
      assert(__po_hi_gqueues_n_empty[id] == nb_empty - 1);
    }
    __DEBUGMSG("\nThis port queue must be considered not empty ");
    assert (__po_hi_gqueues_port_is_empty[id][port] == 0);
    __DEBUGMSG("\nThe task queue must be considered not empty ");
    assert (__po_hi_gqueues_queue_is_empty[id] == 0);
  }
#endif

417
  return __PO_HI_SUCCESS;
418
419
}

420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
/******************************************************************************/
__po_hi_bool_t __po_hi_gqueue_compute_index_transition_to_execute (__po_hi_task_id id,
                                                                   __po_hi_ba_automata_state_t* next_complete_state,
                                                                   int* initial_sizes_of_dispatch_triggers_of_all_transitions,
                                                                   __po_hi_int32_t* index_transition_to_execute)
{
	__po_hi_int32_t i = 0;
	__po_hi_bool_t dispatch_condition_of_any_transition_is_verified = 0;
	__po_hi_int32_t tmp = 0;
	
	__po_hi_int32_t j = 0;
	__po_hi_bool_t dispatch_condition;

	while (i < next_complete_state->nb_transitions && ! dispatch_condition_of_any_transition_is_verified)
	{
		dispatch_condition = 1;
		while (j < (tmp + next_complete_state->nb_dispatch_triggers_of_each_transition[i]) && dispatch_condition)
		{
			dispatch_condition = (initial_sizes_of_dispatch_triggers_of_all_transitions[j] < __po_hi_gqueue_get_count (id, next_complete_state->dispatch_triggers_of_all_transitions[j]));
			j++;
		}
		
		if (dispatch_condition)
		{ 
			*index_transition_to_execute = i + 1;
		}
			

		tmp = tmp + next_complete_state->nb_dispatch_triggers_of_each_transition[i];
		j = tmp;

		dispatch_condition_of_any_transition_is_verified = dispatch_condition;
		i++;
    }

	return dispatch_condition;
}

/******************************************************************************/
void __po_hi_gqueue_wait_for_specific_incoming_events (__po_hi_task_id id,
                                                       __po_hi_ba_automata_state_t* next_complete_state,
                                                       __po_hi_int32_t* index_transition_to_execute)
{
  /* Locking only the mutex of the semaphore */
  int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT %d %d\n", id, result);
  assert(result == __PO_HI_SUCCESS);
  
  int initial_sizes_of_dispatch_triggers_of_all_transitions[next_complete_state->nb_of_all_dispatch_events];

  for(int i=0;i<(next_complete_state->nb_of_all_dispatch_events);i++)
  {
	  initial_sizes_of_dispatch_triggers_of_all_transitions[i] = __po_hi_gqueue_get_count (id, next_complete_state->dispatch_triggers_of_all_transitions[i]);
  }
  
  *index_transition_to_execute = -1;
  
  while (! __po_hi_gqueue_compute_index_transition_to_execute(id, next_complete_state, initial_sizes_of_dispatch_triggers_of_all_transitions, index_transition_to_execute))
    {
      __PO_HI_INSTRUMENTATION_VCD_WRITE("0t%d\n", id);

      /* Telling the semaphore to wait with putting its condvar on wait mode */
      int res_sem =  __po_hi_sem_wait_gqueue(__po_hi_gqueues_semaphores,id);
      __DEBUGMSG("GQUEUE_SEM_WAIT %d %d\n", id, res_sem);
      assert(res_sem == __PO_HI_SUCCESS);
      __PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", id);
    }

#if defined (MONITORING)
  record_event(SPORADIC, WAIT_FOR, id, invalid_port_t, invalid_port_t, *port, invalid_local_port_t, NULL);
#endif

  /** Releasing only the mutex of the semaphore*/

  int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MTUEX_RELEASE %d %d\n", id, res);
  assert(res == __PO_HI_SUCCESS);

#ifdef __PO_HI_GQUEUE_ASSERTIONS
  __DEBUGMSG("\nThe task queue must be considered not empty ");
#endif
}

503
/******************************************************************************/
yoogx's avatar
yoogx committed
504
void __po_hi_gqueue_wait_for_incoming_event (__po_hi_task_id id,
505
                                             __po_hi_local_port_t* port)
506
{
Antonia Francis's avatar
Antonia Francis committed
507
  /* Locking only the mutex of the semaphore */
508
509
510
  int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT %d %d\n", id, result);
  assert(result == __PO_HI_SUCCESS);
511

512
  while(po_hi_gqueues_queue_is_empty(id) == 1)
yoogx's avatar
yoogx committed
513
514
    {
      __PO_HI_INSTRUMENTATION_VCD_WRITE("0t%d\n", id);
515

516
517
518
519
520
      /* Telling the semaphore to wait with putting its condvar on wait mode */
      int res_sem =  __po_hi_sem_wait_gqueue(__po_hi_gqueues_semaphores,id);
      __DEBUGMSG("GQUEUE_SEM_WAIT %d %d\n", id, res_sem);
      assert(res_sem == __PO_HI_SUCCESS);
      __PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", id);
yoogx's avatar
yoogx committed
521
    }
jdelange's avatar
jdelange committed
522

yoogx's avatar
yoogx committed
523
  *port = __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]];
jdelange's avatar
jdelange committed
524

yoogx's avatar
yoogx committed
525
526
#if defined (MONITORING)
  record_event(SPORADIC, WAIT_FOR, id, invalid_port_t, invalid_port_t, *port, invalid_local_port_t, NULL);
Antonia Francis's avatar
Antonia Francis committed
527
#endif
528

yoogx's avatar
yoogx committed
529
  /** Releasing only the mutex of the semaphore*/
530

531
532
533
  int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MTUEX_RELEASE %d %d\n", id, res);
  assert(res == __PO_HI_SUCCESS);
534

535
536
537
538
#ifdef __PO_HI_GQUEUE_ASSERTIONS
  __DEBUGMSG("\nThe task queue must be considered not empty ");
  assert (*port == __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]]);
#endif
539
540
}

541
/******************************************************************************/
542
543
int __po_hi_gqueue_get_count( __po_hi_task_id id, __po_hi_local_port_t port)
{
544
545
  if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA)
    {
yoogx's avatar
yoogx committed
546
      __PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE a FIFO_INDATA port will always have a get_count of 1, even if empty, task-id=%d, port=%d\n", id, port);
547
      return 1; /* data port are always of size 1 */
548
549
550
    }
  else
    {
551
      return (__po_hi_gqueue_used_size(id,port));
552
    }
553
554
}

555
/******************************************************************************/
yoogx's avatar
yoogx committed
556
557
int __po_hi_gqueue_get_value (__po_hi_task_id      id,
                              __po_hi_local_port_t port,
558
                              __po_hi_request_t*   request)
559
{
560
  __po_hi_request_t* ptr;
561

562
  __PO_HI_DEBUG_DEBUG("before get_value for task-id %d , port = %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d , fifo size = %d, gqueues_id adress = %d, \n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
jdelange's avatar
jdelange committed
563

564
  ptr = &__po_hi_gqueues_most_recent_values[id][port];
Antonia Francis's avatar
Antonia Francis committed
565

566
  /* Locking only the mutex of the semaphore */
567
568
569
  int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT %d %d\n", id, result);
  assert(result == __PO_HI_SUCCESS);
570

571
572
573
574
  /*
   * If the port is an OUTPUT, with no value queued, the function returns
   * nothing.
   */
575
  if (__po_hi_gqueue_get_port_size(id,port) == -2)
576
    {
577
      __PO_HI_DEBUG_CRITICAL ("[GQUEUE] OUTPUT PORT, REQUEST NOT SET UP, task-id=%d, port=%d\n", id, port);
578
      __DEBUGMSG("THE PORT IS AN OUTPUT, REQUEST NOT SET UP");
579
580
581
582
583
        /* Releasing only the mutex of the semaphore*/
       int rel = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
       __DEBUGMSG("GQUEUE_SEM_MUTEX_RELEASE %d %d\n", id, rel);
       assert(rel == __PO_HI_SUCCESS);
      return __PO_HI_INVALID;
584
585
586
587
588
    }
  /*
   * If the port is an event port, with no value queued, then we block
   * the thread.
   */
589
  /* Empty port case 1 : NO FIFO INDATA */
590
591
  if (__po_hi_gqueue_get_port_size(id,port) != __PO_HI_GQUEUE_FIFO_INDATA)
    {
592
      while (__po_hi_gqueues_port_is_empty[id][port] == 1)
593
594
595
596
597
598
599
        {
          /* Telling the semaphore to wait with putting its condvar on wait mode */
          int res_sem =  __po_hi_sem_wait_gqueue(__po_hi_gqueues_semaphores,id);
          __DEBUGMSG("GQUEUE_SEM_WAIT %d %d\n", id, result);
          assert(res_sem == __PO_HI_SUCCESS);
        }
    }
600
601
  /* Empty port case 2 : FIFO INDATA */
  if ((__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA) && (__po_hi_gqueue_used_size(id,port) == 0))
602
    {
603
      memcpy (request, ptr, sizeof (__po_hi_request_t));
604
      //update_runtime (id, port, ptr);
605
606
607
608
609
610
611
612
613
614
    }
  else
    {
      /* The program ensures to read the information at the right place in the buffer.
       * The right first offset has to be applied so that the right port is chosen.
       * The right offset (read_offset) has to be applied not to erase fresh information.
       */

      ptr = (__po_hi_gqueues[id]) +  __po_hi_gqueues_first[id][port] + __po_hi_gqueues_offsets[id][port];
      __PO_HI_DEBUG_DEBUG("Get_value if port not empty first + offsets = %d, gqueue_id adress =  %d, first = %d, ptr (adress + first +offset) = %d, \n\n",  __po_hi_gqueues_first[id][port] + __po_hi_gqueues_offsets[id][port],__po_hi_gqueues[id], __po_hi_gqueues_first[id][port], ptr);
615
      memcpy (request, ptr, sizeof (__po_hi_request_t));
616
    }
yoogx's avatar
yoogx committed
617

618
#if defined (MONITORING)
619
  record_event(ANY, GET_VALUE, id, invalid_port_t, invalid_port_t, port, invalid_local_port_t , request);
620
621
#endif

622
  __PO_HI_DEBUG_INFO ("[GQUEUE] Task %d get a value on port %d\n", id, port);
623

624
  /* Releasing only the mutex of the semaphore*/
625
626
627
  int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MUTEX_RELEASE %d %d\n", id, res);
  assert(res == __PO_HI_SUCCESS);
628

629
  __PO_HI_DEBUG_DEBUG("After get_value for task-id %d , port = %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueues adress = %d \n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
630
  return __PO_HI_SUCCESS;
631
632
}

633
/******************************************************************************/
634
int __po_hi_gqueue_next_value (__po_hi_task_id id, __po_hi_local_port_t port)
635
{
636

637
638
639
640
641
642
#ifdef __PO_HI_GQUEUE_ASSERTIONS
  __po_hi_port_id_t init_offset = __po_hi_gqueues_offsets[id][port];
  __po_hi_uint32_t init_history_offset = __po_hi_gqueues_global_history_offset[id];
  __po_hi_port_id_t init_used_size = __po_hi_gqueues_used_size[id][port];
  __po_hi_port_id_t nb_empty =  __po_hi_gqueues_n_empty[id];
#endif
643

644
  /* Incomplete semantics, Should discriminate and report whether
645
646
647
     there is a next value or not */
  if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA)
    {
yoogx's avatar
yoogx committed
648
      __PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE, for a FIFO_INDATA port, the used_size is always at 0 (not reduced in a next_value) task-id=%d, port=%d\n", id, port);
649
      return 1;
650
    }
651

652
653
  /* Locking a mutex */
  __PO_HI_DEBUG_DEBUG ("\nWaiting on next_value on task %d, port = %d, size of port = %d\n", id, port,__po_hi_gqueue_get_port_size(id, port));
654
655
656
  int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT %d %d\n", id, result);
  assert(result == __PO_HI_SUCCESS);
657

658
  __PO_HI_DEBUG_DEBUG("\nBefore next_value for task-id %d , offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port_size = %d, fifo size = %d, gqueues adress = %d, \n\n", id, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
659

660
661
662
663
664
665
666
  __po_hi_gqueues_offsets[id][port] =
    (__po_hi_gqueues_offsets[id][port] + 1)
    % __po_hi_gqueues_sizes[id][port];
  __PO_HI_DEBUG_DEBUG ("\nBefore -- on size, Next_value for task id = %d, __po_hi_gqueues_used_size[id][port] = %d\n",id, __po_hi_gqueues_used_size[id][port]);
  __po_hi_gqueues_used_size[id][port]--;
  __PO_HI_DEBUG_DEBUG ("\nAfter -- on size , Next_value for task id = %d, __po_hi_gqueues_used_size[id][port] = %d\n",id, __po_hi_gqueues_used_size[id][port]);
  __PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueue_used_size(id,port), id, port);
667

668
669
  if (__po_hi_gqueue_used_size(id,port) == 0)
    {
670
671
      __po_hi_gqueues_n_empty[id]++;
      __po_hi_gqueues_port_is_empty[id][port] = 1;
672
    }
673

674
675
  if (__po_hi_gqueues_n_empty[id] == __po_hi_gqueues_nb_ports[id])
    {
676
      __po_hi_gqueues_queue_is_empty[id] = 1;
677
    }
678

679
680
681
  __po_hi_gqueues_global_history_offset[id] =
    (__po_hi_gqueues_global_history_offset[id] + 1)
    % __po_hi_gqueues_total_fifo_size[id];
682

683
  __PO_HI_DEBUG_DEBUG("\nAfter next_value for task-id %d , offset = %d, woffset = %d, history_offset = %d, history_woffset = %d , port size = %d, fifo size = %d, gqueue = %d \n\n", id, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
684

685
  /* Releasing a mutex*/
686
687
688
689
  int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
  __DEBUGMSG("GQUEUE_SEM_MUTEX_RELEASE %d %d\n", id, res);
  assert(res == __PO_HI_SUCCESS);

690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
#ifdef __PO_HI_GQUEUE_ASSERTIONS
  /* The port length is superior to 1 */
  if ((__po_hi_gqueue_get_port_size(id,port) != __PO_HI_GQUEUE_FIFO_INDATA)){
    __DEBUGMSG("\nThe woffset should be incremented by one");
    assert(__po_hi_gqueues_offsets[id][port] == (init_offset + 1)% __po_hi_gqueues_sizes[id][port]);
    assert(__po_hi_gqueues_offsets[id][port] < __po_hi_gqueues_sizes[id][port]);
    __DEBUGMSG("\nThe effective port size used should be decremented by one");
    assert (__po_hi_gqueues_used_size[id][port] == init_used_size -1);
    __DEBUGMSG("The offset_index should then be incremented by one");
    assert(__po_hi_gqueues_global_history_offset[id] == (init_history_offset + 1)% __po_hi_gqueues_total_fifo_size[id]);
    assert(__po_hi_gqueues_global_history_offset[id] < __po_hi_gqueues_total_fifo_size[id]);
    __DEBUGMSG("\nIf this port queue was empty, the number of empty port is reduced by 1");
    /* If the port is now empty */
    if (__po_hi_gqueue_used_size(id,port) == 0){
      assert(__po_hi_gqueues_n_empty[id] == nb_empty + 1);
      __DEBUGMSG("\nThis port queue must be considered empty ");
      assert(__po_hi_gqueues_port_is_empty[id][port] == 1);
    }
    if (__po_hi_gqueues_n_empty[id] == __po_hi_gqueues_nb_ports[id])
      {
        assert(__po_hi_gqueues_queue_is_empty[id] == 1);
      }
  }
#endif
714
  return __PO_HI_SUCCESS;
715
}
716

717
718
719
/******************************************************************************/
__po_hi_request_t*  __po_hi_gqueue_get_most_recent_value (__po_hi_task_id task_id,
                                                          __po_hi_local_port_t local_port)
720
{
721
  return (&__po_hi_gqueues_most_recent_values[task_id][local_port]);
722
723
}

724
725
726
/******************************************************************************/
__po_hi_port_id_t __po_hi_gqueue_get_destinations_number (__po_hi_task_id task_id,
                                                          __po_hi_local_port_t local_port)
727
{
728
  return (__po_hi_gqueues_n_destinations[task_id][local_port]);
729
730
}

731
732
733
734
/******************************************************************************/
__po_hi_port_t __po_hi_gqueue_get_destination (__po_hi_task_id task_id,
                                               __po_hi_local_port_t local_port,
                                               uint8_t destination_number)
735
{
736
  return (__po_hi_gqueues_destinations[task_id][local_port][destination_number]);
737
}
738

739
740
741
/******************************************************************************/
__po_hi_port_id_t __po_hi_gqueue_get_port_size(__po_hi_task_id id,
                                               __po_hi_local_port_t port)
742
{
743
  return __po_hi_gqueues_sizes[id][port];
744
745
}

746
747
748
/******************************************************************************/
__po_hi_port_id_t __po_hi_gqueue_used_size(__po_hi_task_id id,
                                           __po_hi_local_port_t port)
749
{
750
  return __po_hi_gqueues_used_size[id][port];
751
752
}

753
/******************************************************************************/
754
__po_hi_port_id_t po_hi_gqueues_queue_is_empty( __po_hi_task_id id)
755
{
756
  return __po_hi_gqueues_queue_is_empty[id];
757
}