intertask_interface.c 34.9 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.0  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

22
#define _GNU_SOURCE
Cedric Roux's avatar
   
Cedric Roux committed
23
24
25
26
27
28
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
29
#include <signal.h>
Cedric Roux's avatar
   
Cedric Roux committed
30

31
32
#include <sys/epoll.h>
#include <sys/eventfd.h>
33

34
35
36
37
#ifdef RTAI
# include <rtai_shm.h>
#endif

gauthier's avatar
gauthier committed
38
39
40
41
#if !defined(TRUE)
#define TRUE 1
#endif

42
43
44
45
46
47
#include "liblfds611.h"

#include "assertions.h"
#include "intertask_interface.h"
#include "intertask_interface_dump.h"

48
#if defined(OAI_EMU) || defined(RTAI)
49
# include "memory_pools.h"
50
51
52
# include "vcd_signal_dumper.h"
#endif

53
54
55
56
#if T_TRACER
#include "T.h"
#endif

57
58
59
/* Includes "intertask_interface_init.h" to check prototype coherence, but
 * disable threads and messages information generation.
 */
Cedric Roux's avatar
   
Cedric Roux committed
60
61
62
63
#define CHECK_PROTOTYPE_ONLY
#include "intertask_interface_init.h"
#undef CHECK_PROTOTYPE_ONLY

64
#include "signals.h"
Cedric Roux's avatar
   
Cedric Roux committed
65
66
#include "timer.h"

67
68
69
70
71
72
73
74
#ifdef RTAI
# include <rtai.h>
# include <rtai_fifos.h>
#    define FIFO_PRINTF_MAX_STRING_SIZE 1000
#    define FIFO_PRINTF_NO              62
#    define FIFO_PRINTF_SIZE            65536
#endif

75
76
77
78
79
80
81
82
83
/* ITTI DEBUG groups */
#define ITTI_DEBUG_POLL             (1<<0)
#define ITTI_DEBUG_SEND             (1<<1)
#define ITTI_DEBUG_EVEN_FD          (1<<2)
#define ITTI_DEBUG_INIT             (1<<3)
#define ITTI_DEBUG_EXIT             (1<<4)
#define ITTI_DEBUG_ISSUES           (1<<5)
#define ITTI_DEBUG_MP_STATISTICS    (1<<6)

84
const int itti_debug = (ITTI_DEBUG_ISSUES | ITTI_DEBUG_MP_STATISTICS);
Cedric Roux's avatar
   
Cedric Roux committed
85

86
87
/* Don't flush if using RTAI */
#ifdef RTAI
88
# define ITTI_DEBUG(m, x, args...)  do { if ((m) & itti_debug) rt_log_debug (x, ##args); } while(0);
89
#else
90
# define ITTI_DEBUG(m, x, args...)  do { if ((m) & itti_debug) {fprintf(stdout, "[ITTI][D]"x, ##args); fflush (stdout);} } while(0);
91
#endif
92
#define ITTI_ERROR(x, args...)      do { fprintf(stdout, "[ITTI][E]"x, ##args); fflush (stdout); } while(0);
Cedric Roux's avatar
   
Cedric Roux committed
93
94
95
96

/* Global message size */
#define MESSAGE_SIZE(mESSAGEiD) (sizeof(MessageHeader) + itti_desc.messages_info[mESSAGEiD].size)

97
98
99
100
101
#ifdef RTAI
# define ITTI_MEM_PAGE_SIZE (1024)
# define ITTI_MEM_SIZE      (16 * 1024 * 1024)
#endif

Cedric Roux's avatar
   
Cedric Roux committed
102
typedef enum task_state_s {
103
  TASK_STATE_NOT_CONFIGURED, TASK_STATE_STARTING, TASK_STATE_READY, TASK_STATE_ENDED, TASK_STATE_MAX,
Cedric Roux's avatar
   
Cedric Roux committed
104
105
106
} task_state_t;

/* This list acts as a FIFO of messages received by tasks (RRC, NAS, ...) */
107
typedef struct message_list_s {
108
  MessageDef *msg; ///< Pointer to the message
Cedric Roux's avatar
   
Cedric Roux committed
109

110
111
  message_number_t message_number; ///< Unique message number
  uint32_t message_priority; ///< Message priority
112
} message_list_t;
Cedric Roux's avatar
   
Cedric Roux committed
113

114
typedef struct thread_desc_s {
115
116
  /* pthread associated with the thread */
  pthread_t task_thread;
117

118
119
  /* State of the thread */
  volatile task_state_t task_state;
120

121
122
  /* This fd is used internally by ITTI. */
  int epoll_fd;
123

124
125
  /* The thread fd */
  int task_event_fd;
126

127
128
  /* Number of events to monitor */
  uint16_t nb_events;
129

130

131
132
133
134
135
136
  /* Array of events monitored by the task.
   * By default only one fd is monitored (the one used to received messages
   * from other tasks).
   * More events can be suscribed later by the task itself.
   */
  struct epoll_event *events;
137

138
  int epoll_nb_events;
139

140
141
142
  //#ifdef RTAI
  /* Flag to mark real time thread */
  unsigned real_time;
143

144
145
146
  /* Counter to indicate from RTAI threads that messages are pending for the thread */
  unsigned messages_pending;
  //#endif
147
148
149
} thread_desc_t;

typedef struct task_desc_s {
150
151
  /* Queue of messages belonging to the task */
  struct lfds611_queue_state *message_queue;
Cedric Roux's avatar
   
Cedric Roux committed
152
153
} task_desc_t;

154
typedef struct itti_desc_s {
155
156
  thread_desc_t *threads;
  task_desc_t   *tasks;
157

158
159
  /* Current message number. Incremented every call to send_msg_to_task */
  message_number_t message_number __attribute__((aligned(8)));
Cedric Roux's avatar
   
Cedric Roux committed
160

161
162
163
  thread_id_t thread_max;
  task_id_t task_max;
  MessagesIds messages_id_max;
Cedric Roux's avatar
   
Cedric Roux committed
164

165
166
  boolean_t thread_handling_signals;
  pthread_t thread_ref;
167

168
169
  const task_info_t *tasks_info;
  const message_info_t *messages_info;
Cedric Roux's avatar
   
Cedric Roux committed
170

171
  itti_lte_time_t lte_time;
172

173
  int running;
174

175
176
177
  volatile uint32_t created_tasks;
  volatile uint32_t ready_tasks;
  volatile int      wait_tasks;
178
#ifdef RTAI
179
  pthread_t rt_relay_thread;
180
#endif
181
182

#if defined(OAI_EMU) || defined(RTAI)
183
  memory_pools_handle_t memory_pools_handle;
184

185
186
187
  uint64_t vcd_poll_msg;
  uint64_t vcd_receive_msg;
  uint64_t vcd_send_msg;
188
#endif
189
190
191
} itti_desc_t;

static itti_desc_t itti_desc;
Cedric Roux's avatar
   
Cedric Roux committed
192

193
void *itti_malloc(task_id_t origin_task_id, task_id_t destination_task_id, ssize_t size)
194
{
195
  void *ptr = NULL;
196

197
#if defined(OAI_EMU) || defined(RTAI)
198
199
200
201
202
203
204
205
  ptr = memory_pools_allocate (itti_desc.memory_pools_handle, size, origin_task_id, destination_task_id);

  if (ptr == NULL) {
    char *statistics = memory_pools_statistics (itti_desc.memory_pools_handle);

    ITTI_ERROR (" Memory pools statistics:\n%s", statistics);
    free (statistics);
  }
206

winckel's avatar
winckel committed
207
#else
208
  ptr = malloc (size);
209
#endif
winckel's avatar
winckel committed
210

211
  AssertFatal (ptr != NULL, "Memory allocation of %d bytes failed (%d -> %d)!\n", (int) size, origin_task_id, destination_task_id);
212

213
  return ptr;
214
215
}

216
int itti_free(task_id_t task_id, void *ptr)
217
{
218
219
  int result = EXIT_SUCCESS;
  AssertFatal (ptr != NULL, "Trying to free a NULL pointer (%d)!\n", task_id);
220
221

#if defined(OAI_EMU) || defined(RTAI)
222
  result = memory_pools_free (itti_desc.memory_pools_handle, ptr, task_id);
223

224
  AssertError (result == EXIT_SUCCESS, {}, "Failed to free memory at %p (%d)!\n", ptr, task_id);
225
#else
226
  free (ptr);
227
#endif
228

229
  return (result);
230
231
}

232
233
234
235
236
237
238
static inline message_number_t itti_increment_message_number(void)
{
  /* Atomic operation supported by GCC: returns the current message number
   * and then increment it by 1.
   * This can be done without mutex.
   */
  return __sync_fetch_and_add (&itti_desc.message_number, 1);
Cedric Roux's avatar
   
Cedric Roux committed
239
240
}

241
242
243
static inline uint32_t itti_get_message_priority(MessagesIds message_id)
{
  AssertFatal (message_id < itti_desc.messages_id_max, "Message id (%d) is out of range (%d)!\n", message_id, itti_desc.messages_id_max);
Cedric Roux's avatar
   
Cedric Roux committed
244

245
  return (itti_desc.messages_info[message_id].priority);
Cedric Roux's avatar
   
Cedric Roux committed
246
247
}

248
249
250
const char *itti_get_message_name(MessagesIds message_id)
{
  AssertFatal (message_id < itti_desc.messages_id_max, "Message id (%d) is out of range (%d)!\n", message_id, itti_desc.messages_id_max);
Cedric Roux's avatar
   
Cedric Roux committed
251

252
  return (itti_desc.messages_info[message_id].name);
Cedric Roux's avatar
   
Cedric Roux committed
253
254
}

255
const char *itti_get_task_name(task_id_t task_id)
Cedric Roux's avatar
Cedric Roux committed
256
{
257
258
259
260
261
  if (itti_desc.task_max > 0) {
    AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)!\n", task_id, itti_desc.task_max);
  } else {
    return ("ITTI NOT INITIALIZED !!!");
  }
Cedric Roux's avatar
Cedric Roux committed
262

263
  return (itti_desc.tasks_info[task_id].name);
Cedric Roux's avatar
Cedric Roux committed
264
265
}

266
static task_id_t itti_get_current_task_id(void)
267
{
268
269
270
271
272
273
274
275
276
  task_id_t task_id;
  thread_id_t thread_id;
  pthread_t thread = pthread_self ();

  for (task_id = TASK_FIRST; task_id < itti_desc.task_max; task_id++) {
    thread_id = TASK_GET_THREAD_ID(task_id);

    if (itti_desc.threads[thread_id].task_thread == thread) {
      return task_id;
277
    }
278
  }
279

280
  return TASK_UNKNOWN;
281
282
}

283
284
285
#ifdef RTAI
static void rt_log_debug(char *format, ...)
{
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
  task_id_t   task_id;
  va_list     args;
  char        log_buffer[FIFO_PRINTF_MAX_STRING_SIZE];
  int         len;

  task_id = itti_get_current_task_id ();
  len = snprintf(log_buffer, FIFO_PRINTF_MAX_STRING_SIZE-1, "[ITTI][D][%s]", itti_get_task_name(task_id));
  va_start(args, format);
  len += vsnprintf(&log_buffer[len], FIFO_PRINTF_MAX_STRING_SIZE-1-len, format, args);
  va_end (args);

  if (task_id != TASK_UNKNOWN)
    fwrite(log_buffer, len, 1, stdout);
  else
    rtf_put (FIFO_PRINTF_NO, log_buffer, len);
301
302
303
}
#endif

304
305
void itti_update_lte_time(uint32_t frame, uint8_t slot)
{
306
307
  itti_desc.lte_time.frame = frame;
  itti_desc.lte_time.slot = slot;
308
309
}

310
311
312
313
314
315
316
317
318
319
int itti_send_broadcast_message(MessageDef *message_p)
{
  task_id_t destination_task_id;
  task_id_t origin_task_id;
  thread_id_t origin_thread_id;
  uint32_t thread_id;
  int ret = 0;
  int result;

  AssertFatal (message_p != NULL, "Trying to broadcast a NULL message!\n");
Cedric Roux's avatar
   
Cedric Roux committed
320

321
322
  origin_task_id = message_p->ittiMsgHeader.originTaskId;
  origin_thread_id = TASK_GET_THREAD_ID(origin_task_id);
Cedric Roux's avatar
   
Cedric Roux committed
323

324
  destination_task_id = TASK_FIRST;
Cedric Roux's avatar
   
Cedric Roux committed
325

326
327
  for (thread_id = THREAD_FIRST; thread_id < itti_desc.thread_max; thread_id++) {
    MessageDef *new_message_p;
328

329
330
    while (thread_id != TASK_GET_THREAD_ID(destination_task_id)) {
      destination_task_id++;
Cedric Roux's avatar
   
Cedric Roux committed
331
332
    }

333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
    /* Skip task that broadcast the message */
    if (thread_id != origin_thread_id) {
      /* Skip tasks which are not running */
      if (itti_desc.threads[thread_id].task_state == TASK_STATE_READY) {
        size_t size = sizeof(MessageHeader) + message_p->ittiMsgHeader.ittiMsgSize;
        new_message_p = itti_malloc( origin_task_id, destination_task_id, size );
        AssertFatal (new_message_p != NULL, "New message allocation failed!\n");

        memcpy( new_message_p, message_p, size );
        result = itti_send_msg_to_task (destination_task_id, INSTANCE_DEFAULT, new_message_p);
        AssertFatal (result >= 0, "Failed to send message %d to thread %d (task %d)!\n", message_p->ittiMsgHeader.messageId, thread_id, destination_task_id);
      }
    }
  }

  result = itti_free (ITTI_MSG_ORIGIN_ID(message_p), message_p);
  AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);

  return ret;
Cedric Roux's avatar
   
Cedric Roux committed
352
353
}

354
MessageDef *itti_alloc_new_message_sized(task_id_t origin_task_id, MessagesIds message_id, MessageHeaderSize size)
Cedric Roux's avatar
Cedric Roux committed
355
{
356
  MessageDef *temp = NULL;
Cedric Roux's avatar
   
Cedric Roux committed
357

358
  AssertFatal (message_id < itti_desc.messages_id_max, "Message id (%d) is out of range (%d)!\n", message_id, itti_desc.messages_id_max);
Cedric Roux's avatar
   
Cedric Roux committed
359

360
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
361
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, size);
362
363
#endif

364
365
366
367
  if (origin_task_id == TASK_UNKNOWN) {
    /* Try to identify real origin task ID */
    origin_task_id = itti_get_current_task_id();
  }
368

369
  temp = itti_malloc (origin_task_id, TASK_UNKNOWN, sizeof(MessageHeader) + size);
Cedric Roux's avatar
   
Cedric Roux committed
370

371
372
373
  temp->ittiMsgHeader.messageId = message_id;
  temp->ittiMsgHeader.originTaskId = origin_task_id;
  temp->ittiMsgHeader.ittiMsgSize = size;
Cedric Roux's avatar
   
Cedric Roux committed
374

375
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
376
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, 0);
377
378
#endif

379
  return temp;
Cedric Roux's avatar
   
Cedric Roux committed
380
381
}

382
MessageDef *itti_alloc_new_message(task_id_t origin_task_id, MessagesIds message_id)
Cedric Roux's avatar
Cedric Roux committed
383
{
384
  return itti_alloc_new_message_sized(origin_task_id, message_id, itti_desc.messages_info[message_id].size);
Cedric Roux's avatar
Cedric Roux committed
385
386
}

387
int itti_send_msg_to_task(task_id_t destination_task_id, instance_t instance, MessageDef *message)
Cedric Roux's avatar
Cedric Roux committed
388
{
389
390
391
392
393
394
  thread_id_t destination_thread_id;
  task_id_t origin_task_id;
  message_list_t *new;
  uint32_t priority;
  message_number_t message_number;
  uint32_t message_id;
Cedric Roux's avatar
   
Cedric Roux committed
395

winckel's avatar
winckel committed
396
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
397
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_SEND_MSG,
398
                                          __sync_or_and_fetch (&itti_desc.vcd_send_msg, 1L << destination_task_id));
winckel's avatar
winckel committed
399
400
#endif

401
402
  AssertFatal (message != NULL, "Message is NULL!\n");
  AssertFatal (destination_task_id < itti_desc.task_max, "Destination task id (%d) is out of range (%d)\n", destination_task_id, itti_desc.task_max);
Cedric Roux's avatar
   
Cedric Roux committed
403

404
405
406
407
408
409
410
  destination_thread_id = TASK_GET_THREAD_ID(destination_task_id);
  message->ittiMsgHeader.destinationTaskId = destination_task_id;
  message->ittiMsgHeader.instance = instance;
  message->ittiMsgHeader.lte_time.frame = itti_desc.lte_time.frame;
  message->ittiMsgHeader.lte_time.slot = itti_desc.lte_time.slot;
  message_id = message->ittiMsgHeader.messageId;
  AssertFatal (message_id < itti_desc.messages_id_max, "Message id (%d) is out of range (%d)!\n", message_id, itti_desc.messages_id_max);
Cedric Roux's avatar
   
Cedric Roux committed
411

412
  origin_task_id = ITTI_MSG_ORIGIN_ID(message);
413

414
  priority = itti_get_message_priority (message_id);
Cedric Roux's avatar
   
Cedric Roux committed
415

416
417
  /* Increment the global message number */
  message_number = itti_increment_message_number ();
Cedric Roux's avatar
   
Cedric Roux committed
418

419
420
  itti_dump_queue_message (origin_task_id, message_number, message, itti_desc.messages_info[message_id].name,
                           sizeof(MessageHeader) + message->ittiMsgHeader.ittiMsgSize);
421

422
  if (destination_task_id != TASK_UNKNOWN) {
423
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
424
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_IN);
winckel's avatar
winckel committed
425

426
    memory_pools_set_info (itti_desc.memory_pools_handle, message, 1, destination_task_id);
winckel's avatar
winckel committed
427
428
#endif

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
    if (itti_desc.threads[destination_thread_id].task_state == TASK_STATE_ENDED) {
      ITTI_DEBUG(ITTI_DEBUG_ISSUES, " Message %s, number %lu with priority %d can not be sent from %s to queue (%u:%s), ended destination task!\n",
                 itti_desc.messages_info[message_id].name,
                 message_number,
                 priority,
                 itti_get_task_name(origin_task_id),
                 destination_task_id,
                 itti_get_task_name(destination_task_id));
    } else {
      /* We cannot send a message if the task is not running */
      AssertFatal (itti_desc.threads[destination_thread_id].task_state == TASK_STATE_READY,
                   "Task %s Cannot send message %s (%d) to thread %d, it is not in ready state (%d)!\n",
                   itti_get_task_name(origin_task_id),
                   itti_desc.messages_info[message_id].name,
                   message_id,
                   destination_thread_id,
                   itti_desc.threads[destination_thread_id].task_state);

      /* Allocate new list element */
      new = (message_list_t *) itti_malloc (origin_task_id, destination_task_id, sizeof(struct message_list_s));

      /* Fill in members */
      new->msg = message;
      new->message_number = message_number;
      new->message_priority = priority;

      /* Enqueue message in destination task queue */
456
457
458
      if (lfds611_queue_enqueue(itti_desc.tasks[destination_task_id].message_queue, new) == 0) {
        AssertFatal(0, "Error: lfds611_queue_enqueue returns 0, queue is full, exiting\n");
      }
459

460
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
461
      VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_OUT);
winckel's avatar
winckel committed
462
463
#endif

464
#ifdef RTAI
465
466
467
468
469

      if (itti_desc.threads[TASK_GET_THREAD_ID(origin_task_id)].real_time) {
        /* This is a RT task, increase destination task messages pending counter */
        __sync_fetch_and_add (&itti_desc.threads[destination_thread_id].messages_pending, 1);
      } else
470
#endif
471
472
473
474
475
476
477
478
479
480
      {
        /* Only use event fd for tasks, subtasks will pool the queue */
        if (TASK_GET_PARENT_TASK_ID(destination_task_id) == TASK_UNKNOWN) {
          ssize_t write_ret;
          eventfd_t sem_counter = 1;

          /* Call to write for an event fd must be of 8 bytes */
          write_ret = write (itti_desc.threads[destination_thread_id].task_event_fd, &sem_counter, sizeof(sem_counter));
          AssertFatal (write_ret == sizeof(sem_counter), "Write to task message FD (%d) failed (%d/%d)\n",
                       destination_thread_id, (int) write_ret, (int) sizeof(sem_counter));
481
        }
482
483
484
485
486
487
488
489
490
      }

      ITTI_DEBUG(ITTI_DEBUG_SEND, " Message %s, number %lu with priority %d successfully sent from %s to queue (%u:%s)\n",
                 itti_desc.messages_info[message_id].name,
                 message_number,
                 priority,
                 itti_get_task_name(origin_task_id),
                 destination_task_id,
                 itti_get_task_name(destination_task_id));
491
    }
492
493
494
495
496
  } else {
    /* This is a debug message to TASK_UNKNOWN, we can release safely release it */
    int result = itti_free(origin_task_id, message);
    AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);
  }
497

498
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
499
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_SEND_MSG,
500
                                          __sync_and_and_fetch (&itti_desc.vcd_send_msg, ~(1L << destination_task_id)));
501
502
#endif

503
  return 0;
Cedric Roux's avatar
   
Cedric Roux committed
504
505
}

506
507
void itti_subscribe_event_fd(task_id_t task_id, int fd)
{
508
509
  thread_id_t thread_id;
  struct epoll_event event;
510

511
  AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)!\n", task_id, itti_desc.task_max);
512

513
514
  thread_id = TASK_GET_THREAD_ID(task_id);
  itti_desc.threads[thread_id].nb_events++;
515

516
517
518
519
  /* Reallocate the events */
  itti_desc.threads[thread_id].events = realloc(
                                          itti_desc.threads[thread_id].events,
                                          itti_desc.threads[thread_id].nb_events * sizeof(struct epoll_event));
520

521
522
523
  event.events  = EPOLLIN | EPOLLERR;
  event.data.u64 = 0;
  event.data.fd  = fd;
524

525
526
527
528
529
530
531
  /* Add the event fd to the list of monitored events */
  if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_ADD, fd,
                &event) != 0) {
    /* Always assert on this condition */
    AssertFatal (0, "epoll_ctl (EPOLL_CTL_ADD) failed for task %s, fd %d: %s!\n",
                 itti_get_task_name(task_id), fd, strerror(errno));
  }
532

533
  ITTI_DEBUG(ITTI_DEBUG_EVEN_FD, " Successfully subscribed fd %d for task %s\n", fd, itti_get_task_name(task_id));
534
535
536
537
}

void itti_unsubscribe_event_fd(task_id_t task_id, int fd)
{
538
  thread_id_t thread_id;
539

540
541
  AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)!\n", task_id, itti_desc.task_max);
  AssertFatal (fd >= 0, "File descriptor (%d) is invalid!\n", fd);
542

543
544
545
546
547
548
549
550
  thread_id = TASK_GET_THREAD_ID(task_id);

  /* Add the event fd to the list of monitored events */
  if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_DEL, fd, NULL) != 0) {
    /* Always assert on this condition */
    AssertFatal (0, "epoll_ctl (EPOLL_CTL_DEL) failed for task %s, fd %d: %s!\n",
                 itti_get_task_name(task_id), fd, strerror(errno));
  }
551

552
553
554
555
  itti_desc.threads[thread_id].nb_events--;
  itti_desc.threads[thread_id].events = realloc(
                                          itti_desc.threads[thread_id].events,
                                          itti_desc.threads[thread_id].nb_events * sizeof(struct epoll_event));
556
557
558
559
}

int itti_get_events(task_id_t task_id, struct epoll_event **events)
{
560
  thread_id_t thread_id;
561

562
  AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)\n", task_id, itti_desc.task_max);
563

564
565
  thread_id = TASK_GET_THREAD_ID(task_id);
  *events = itti_desc.threads[thread_id].events;
566

567
  return itti_desc.threads[thread_id].epoll_nb_events;
568
569
}

570
571
static inline void itti_receive_msg_internal_event_fd(task_id_t task_id, uint8_t polling, MessageDef **received_msg)
{
572
573
574
575
  thread_id_t thread_id;
  int epoll_ret = 0;
  int epoll_timeout = 0;
  int i;
576

577
578
  AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)!\n", task_id, itti_desc.task_max);
  AssertFatal (received_msg != NULL, "Received message is NULL!\n");
579

580
581
  thread_id = TASK_GET_THREAD_ID(task_id);
  *received_msg = NULL;
582

583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
  if (polling) {
    /* In polling mode we set the timeout to 0 causing epoll_wait to return
     * immediately.
     */
    epoll_timeout = 0;
  } else {
    /* timeout = -1 causes the epoll_wait to wait indefinitely.
     */
    epoll_timeout = -1;
  }

  do {
    epoll_ret = epoll_wait(itti_desc.threads[thread_id].epoll_fd,
                           itti_desc.threads[thread_id].events,
                           itti_desc.threads[thread_id].nb_events,
                           epoll_timeout);
  } while (epoll_ret < 0 && errno == EINTR);

  if (epoll_ret < 0) {
    AssertFatal (0, "epoll_wait failed for task %s: %s!\n", itti_get_task_name(task_id), strerror(errno));
  }

  if (epoll_ret == 0 && polling) {
    /* No data to read -> return */
    return;
  }

  itti_desc.threads[thread_id].epoll_nb_events = epoll_ret;

  for (i = 0; i < epoll_ret; i++) {
    /* Check if there is an event for ITTI for the event fd */
    if ((itti_desc.threads[thread_id].events[i].events & EPOLLIN) &&
        (itti_desc.threads[thread_id].events[i].data.fd == itti_desc.threads[thread_id].task_event_fd)) {
      struct message_list_s *message = NULL;
      eventfd_t   sem_counter;
      ssize_t     read_ret;
      int         result;

      /* Read will always return 1 */
      read_ret = read (itti_desc.threads[thread_id].task_event_fd, &sem_counter, sizeof(sem_counter));
      AssertFatal (read_ret == sizeof(sem_counter), "Read from task message FD (%d) failed (%d/%d)!\n", thread_id, (int) read_ret, (int) sizeof(sem_counter));


      if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
        /* No element in list -> this should not happen */
        AssertFatal (0, "No message in queue for task %d while there are %d events and some for the messages queue!\n", task_id, epoll_ret);
      }

      AssertFatal(message != NULL, "Message from message queue is NULL!\n");
      *received_msg = message->msg;
      result = itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
      AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);

      /* Mark that the event has been processed */
      itti_desc.threads[thread_id].events[i].events &= ~EPOLLIN;
      return;
639
    }
640
  }
641
642
643
644
}

void itti_receive_msg(task_id_t task_id, MessageDef **received_msg)
{
645
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
646
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
647
                                          __sync_and_and_fetch (&itti_desc.vcd_receive_msg, ~(1L << task_id)));
648
#endif
649

650
  itti_receive_msg_internal_event_fd(task_id, 0, received_msg);
Cedric Roux's avatar
   
Cedric Roux committed
651

winckel's avatar
winckel committed
652
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
653
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
654
                                          __sync_or_and_fetch (&itti_desc.vcd_receive_msg, 1L << task_id));
655
#endif
Cedric Roux's avatar
   
Cedric Roux committed
656
657
}

658
659
660
void itti_poll_msg(task_id_t task_id, MessageDef **received_msg)
{
  AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)!\n", task_id, itti_desc.task_max);
Cedric Roux's avatar
   
Cedric Roux committed
661

662
  *received_msg = NULL;
Cedric Roux's avatar
   
Cedric Roux committed
663

664
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
665
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_POLL_MSG,
666
                                          __sync_or_and_fetch (&itti_desc.vcd_poll_msg, 1L << task_id));
667
668
#endif

669
670
  {
    struct message_list_s *message;
671

672
673
    if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 1) {
      int result;
674

675
676
677
      *received_msg = message->msg;
      result = itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
      AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);
678
    }
679
  }
Cedric Roux's avatar
   
Cedric Roux committed
680

681
682
683
  if (*received_msg == NULL) {
    ITTI_DEBUG(ITTI_DEBUG_POLL, " No message in queue[(%u:%s)]\n", task_id, itti_get_task_name(task_id));
  }
684
685

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
686
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_POLL_MSG,
687
                                          __sync_and_and_fetch (&itti_desc.vcd_poll_msg, ~(1L << task_id)));
688
#endif
Cedric Roux's avatar
   
Cedric Roux committed
689
690
}

691
692
693
694
int itti_create_task(task_id_t task_id, void *(*start_routine)(void *), void *args_p)
{
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
  int result;
Cedric Roux's avatar
   
Cedric Roux committed
695

696
697
698
699
  AssertFatal (start_routine != NULL, "Start routine is NULL!\n");
  AssertFatal (thread_id < itti_desc.thread_max, "Thread id (%d) is out of range (%d)!\n", thread_id, itti_desc.thread_max);
  AssertFatal (itti_desc.threads[thread_id].task_state == TASK_STATE_NOT_CONFIGURED, "Task %d, thread %d state is not correct (%d)!\n",
               task_id, thread_id, itti_desc.threads[thread_id].task_state);
Cedric Roux's avatar
   
Cedric Roux committed
700

701
  itti_desc.threads[thread_id].task_state = TASK_STATE_STARTING;
Cedric Roux's avatar
   
Cedric Roux committed
702

703
  ITTI_DEBUG(ITTI_DEBUG_INIT, " Creating thread for task %s ...\n", itti_get_task_name(task_id));
704

705
706
707
708
709
  result = pthread_create (&itti_desc.threads[thread_id].task_thread, NULL, start_routine, args_p);
  AssertFatal (result >= 0, "Thread creation for task %d, thread %d failed (%d)!\n", task_id, thread_id, result);
  char name[16];
  snprintf( name, sizeof(name), "ITTI %d", thread_id );
  pthread_setname_np( itti_desc.threads[thread_id].task_thread, name );
Cedric Roux's avatar
   
Cedric Roux committed
710

711
  itti_desc.created_tasks ++;
712

713
714
715
  /* Wait till the thread is completely ready */
  while (itti_desc.threads[thread_id].task_state != TASK_STATE_READY)
    usleep (1000);
716

717
  return 0;
Cedric Roux's avatar
   
Cedric Roux committed
718
719
}

720
//#ifdef RTAI
721
722
void itti_set_task_real_time(task_id_t task_id)
{
723
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
724

725
  DevCheck(thread_id < itti_desc.thread_max, thread_id, itti_desc.thread_max, 0);
726

727
  itti_desc.threads[thread_id].real_time = TRUE;
728
}
knopp's avatar
   
knopp committed
729
//#endif
730

731
732
void itti_wait_ready(int wait_tasks)
{
733
  itti_desc.wait_tasks = wait_tasks;
734

735
736
737
738
739
  ITTI_DEBUG(ITTI_DEBUG_INIT,
             " wait for tasks: %s, created tasks %d, ready tasks %d\n",
             itti_desc.wait_tasks ? "yes" : "no",
             itti_desc.created_tasks,
             itti_desc.ready_tasks);
740

741
742
  AssertFatal (itti_desc.created_tasks == itti_desc.ready_tasks, "Number of created tasks (%d) does not match ready tasks (%d), wait task %d!\n",
               itti_desc.created_tasks, itti_desc.ready_tasks, itti_desc.wait_tasks);
743
744
}

745
746
void itti_mark_task_ready(task_id_t task_id)
{
747
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
Cedric Roux's avatar
   
Cedric Roux committed
748

749
  AssertFatal (thread_id < itti_desc.thread_max, "Thread id (%d) is out of range (%d)!\n", thread_id, itti_desc.thread_max);
750

751
752
  /* Register the thread in itti dump */
  itti_dump_thread_use_ring_buffer();
753

754
755
  /* Mark the thread as using LFDS queue */
  lfds611_queue_use(itti_desc.tasks[task_id].message_queue);
756

757
#ifdef RTAI
758
759
760
761
762
763
  /* Assign low priority to created threads */
  {
    struct sched_param sched_param;
    sched_param.sched_priority = sched_get_priority_min(SCHED_FIFO) + 1;
    sched_setscheduler(0, SCHED_FIFO, &sched_param);
  }
764
765
#endif

766
767
  itti_desc.threads[thread_id].task_state = TASK_STATE_READY;
  itti_desc.ready_tasks ++;
768

769
770
771
  while (itti_desc.wait_tasks != 0) {
    usleep (10000);
  }
772

773
  ITTI_DEBUG(ITTI_DEBUG_INIT, " task %s started\n", itti_get_task_name(task_id));
774
775
}

776
777
void itti_exit_task(void)
{
778
#if defined(OAI_EMU) || defined(RTAI)
779
780
781
  task_id_t task_id = itti_get_current_task_id();

  if (task_id > TASK_UNKNOWN) {
gauthier's avatar
gauthier committed
782
    VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
783
784
                                            __sync_and_and_fetch (&itti_desc.vcd_receive_msg, ~(1L << task_id)));
  }
785
786

#endif
787
  pthread_exit (NULL);
788
789
}

790
791
792
793
void itti_terminate_tasks(task_id_t task_id)
{
  // Sends Terminate signals to all tasks.
  itti_send_terminate_message (task_id);
794

795
796
797
  if (itti_desc.thread_handling_signals) {
    pthread_kill (itti_desc.thread_ref, SIGUSR1);
  }
798

799
  pthread_exit (NULL);
Cedric Roux's avatar
   
Cedric Roux committed
800
801
}

802
803
804
#ifdef RTAI
static void *itti_rt_relay_thread(void *arg)
{
805
806
  thread_id_t thread_id;
  unsigned pending_messages;
807

808
809
  while (itti_desc.running) {
    usleep (200); // Poll for messages a little more than 2 time by slot to get a small latency between RT and other tasks
810
811

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
812
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_IN);
813
#endif
814

815
816
817
818
819
820
821
822
823
824
825
826
827
    /* Checks for all non real time tasks if they have pending messages */
    for (thread_id = THREAD_FIRST; thread_id < itti_desc.thread_max; thread_id++) {
      if ((itti_desc.threads[thread_id].task_state == TASK_STATE_READY)
          && (itti_desc.threads[thread_id].real_time == FALSE)) {
        pending_messages = __sync_fetch_and_and (&itti_desc.threads[thread_id].messages_pending, 0);

        if (pending_messages > 0) {
          ssize_t write_ret;
          eventfd_t sem_counter = pending_messages;

          /* Call to write for an event fd must be of 8 bytes */
          write_ret = write (itti_desc.threads[thread_id].task_event_fd, &sem_counter, sizeof(sem_counter));
          DevCheck(write_ret == sizeof(sem_counter), write_ret, sem_counter, thread_id);
828
        }
829
830
      }
    }
831
832

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
833
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_OUT);
834
#endif
835
836
837
  }

  return NULL;
838
839
840
}
#endif

841
int itti_init(task_id_t task_max, thread_id_t thread_max, MessagesIds messages_id_max, const task_info_t *tasks_info,
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
              const message_info_t *messages_info, const char * const messages_definition_xml, const char * const dump_file_name)
{
  task_id_t task_id;
  thread_id_t thread_id;
  int ret;

  itti_desc.message_number = 1;

  ITTI_DEBUG(ITTI_DEBUG_INIT, " Init: %d tasks, %d threads, %d messages\n", task_max, thread_max, messages_id_max);

  CHECK_INIT_RETURN(signal_mask());

  /* Saves threads and messages max values */
  itti_desc.task_max = task_max;
  itti_desc.thread_max = thread_max;
  itti_desc.messages_id_max = messages_id_max;
  itti_desc.thread_handling_signals = FALSE;
  itti_desc.tasks_info = tasks_info;
  itti_desc.messages_info = messages_info;

  /* Allocates memory for tasks info */
  itti_desc.tasks = calloc (itti_desc.task_max, sizeof(task_desc_t));

  /* Allocates memory for threads info */
  itti_desc.threads = calloc (itti_desc.thread_max, sizeof(thread_desc_t));

  /* Initializing each queue and related stuff */
  for (task_id = TASK_FIRST; task_id < itti_desc.task_max; task_id++) {
    ITTI_DEBUG(ITTI_DEBUG_INIT, " Initializing %stask %s%s%s\n",
               itti_desc.tasks_info[task_id].parent_task != TASK_UNKNOWN ? "sub-" : "",
               itti_desc.tasks_info[task_id].name,
               itti_desc.tasks_info[task_id].parent_task != TASK_UNKNOWN ? " with parent " : "",
               itti_desc.tasks_info[task_id].parent_task != TASK_UNKNOWN ?
               itti_get_task_name(itti_desc.tasks_info[task_id].parent_task) : "");

    ITTI_DEBUG(ITTI_DEBUG_INIT, " Creating queue of message of size %u\n", itti_desc.tasks_info[task_id].queue_size);

    ret = lfds611_queue_new(&itti_desc.tasks[task_id].message_queue, itti_desc.tasks_info[task_id].queue_size);

881
    if (0 == ret) {
882
      AssertFatal (0, "lfds611_queue_new failed for task %s!\n", itti_get_task_name(task_id));
883
    }
884
  }
885

886
887
888
  /* Initializing each thread */
  for (thread_id = THREAD_FIRST; thread_id < itti_desc.thread_max; thread_id++) {
    itti_desc.threads[thread_id].task_state = TASK_STATE_NOT_CONFIGURED;
889

890
    itti_desc.threads[thread_id].epoll_fd = epoll_create1(0);
891

892
893
894
895
    if (itti_desc.threads[thread_id].epoll_fd == -1) {
      /* Always assert on this condition */
      AssertFatal (0, "Failed to create new epoll fd: %s!\n", strerror(errno));
    }
896

897
    itti_desc.threads[thread_id].task_event_fd = eventfd(0, EFD_SEMAPHORE);
898

899
900
901
902
    if (itti_desc.threads[thread_id].task_event_fd == -1) {
      /* Always assert on this condition */
      AssertFatal (0, " eventfd failed: %s!\n", strerror(errno));
    }
903

904
    itti_desc.threads[thread_id].nb_events = 1;
905

906
    itti_desc.threads[thread_id].events = calloc(1, sizeof(struct epoll_event));
907

908
909
910
911
912
913
914
915
916
917
918
919
    itti_desc.threads[thread_id].events->events  = EPOLLIN | EPOLLERR;
    itti_desc.threads[thread_id].events->data.fd = itti_desc.threads[thread_id].task_event_fd;

    /* Add the event fd to the list of monitored events */
    if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_ADD,
                  itti_desc.threads[thread_id].task_event_fd, itti_desc.threads[thread_id].events) != 0) {
      /* Always assert on this condition */
      AssertFatal (0, " epoll_ctl (EPOLL_CTL_ADD) failed: %s!\n", strerror(errno));
    }

    ITTI_DEBUG(ITTI_DEBUG_EVEN_FD, " Successfully subscribed fd %d for thread %d\n",
               itti_desc.threads[thread_id].task_event_fd, thread_id);
920

921
#ifdef RTAI
922
923
    itti_desc.threads[thread_id].real_time = FALSE;
    itti_desc.threads[thread_id].messages_pending = 0;
924
#endif
925
  }
926

927
928
929
930
  itti_desc.running = 1;
  itti_desc.wait_tasks = 0;
  itti_desc.created_tasks = 0;
  itti_desc.ready_tasks = 0;
931
#ifdef RTAI
932
933
  /* Start RT relay thread */
  DevAssert(pthread_create (&itti_desc.rt_relay_thread, NULL, itti_rt_relay_thread, NULL) >= 0);
934

935
  rt_global_heap_open();
936
#endif
937

938
#if defined(OAI_EMU) || defined(RTAI)
939
940
941
942
943
944
945
946
947
948
949
950
951
  itti_desc.memory_pools_handle = memory_pools_create (5);
  memory_pools_add_pool (itti_desc.memory_pools_handle, 1000 + ITTI_QUEUE_MAX_ELEMENTS,       50);
  memory_pools_add_pool (itti_desc.memory_pools_handle, 1000 + (2 * ITTI_QUEUE_MAX_ELEMENTS), 100);
  memory_pools_add_pool (itti_desc.memory_pools_handle, 10000,                                1000);
  memory_pools_add_pool (itti_desc.memory_pools_handle,  400,                                 20050);
  memory_pools_add_pool (itti_desc.memory_pools_handle,  100,                                 30050);

  {
    char *statistics = memory_pools_statistics (itti_desc.memory_pools_handle);

    ITTI_DEBUG(ITTI_DEBUG_MP_STATISTICS, " Memory pools statistics:\n%s", statistics);
    free (statistics);
  }
952
953
#endif

954
#if defined(OAI_EMU) || defined(RTAI)
955
956
957
  itti_desc.vcd_poll_msg = 0;
  itti_desc.vcd_receive_msg = 0;
  itti_desc.vcd_send_msg = 0;
958
959
#endif

960
  itti_dump_init (messages_definition_xml, dump_file_name);
Cedric Roux's avatar
   
Cedric Roux committed
961

962
  CHECK_INIT_RETURN(timer_init ());
Cedric Roux's avatar
   
Cedric Roux committed
963

964
  return 0;
Cedric Roux's avatar
   
Cedric Roux committed
965
966
}

967
968
969
970
971
972
973
974
void itti_wait_tasks_end(void)
{
  int end = 0;
  int thread_id;
  task_id_t task_id;
  int ready_tasks;
  int result;
  int retries = 10;
975

976
977
  itti_desc.thread_handling_signals = TRUE;
  itti_desc.thread_ref=pthread_self ();
978

979
980
981
982
983
984
  /* Handle signals here */
  while (end == 0) {
    signal_handle (&end);
  }

  printf("closing all tasks\n");
985
  sleep(1);
986

987
988
989
990
991
992
993
994
995
996
  do {
    ready_tasks = 0;

    task_id = TASK_FIRST;

    for (thread_id = THREAD_FIRST; thread_id < itti_desc.thread_max; thread_id++) {
      /* Skip tasks which are not running */
      if (itti_desc.threads[thread_id].task_state == TASK_STATE_READY) {
        while (thread_id != TASK_GET_THREAD_ID(task_id)) {
          task_id++;
997
        }
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008

        result = pthread_tryjoin_np (itti_desc.threads[thread_id].task_thread, NULL);

        ITTI_DEBUG(ITTI_DEBUG_EXIT, " Thread %s join status %d\n", itti_get_task_name(task_id), result);

        if (result == 0) {
          /* Thread has terminated */
          itti_desc.threads[thread_id].task_state = TASK_STATE_ENDED;
        } else {
          /* Thread is still running, count it */
          ready_tasks++;
1009
<