intertask_interface.c 35 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

Wang Tsu-Han's avatar
Wang Tsu-Han committed
102

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

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

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

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

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

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

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

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

131

132
133
134
135
136
137
  /* 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;
138

139
  int epoll_nb_events;
140

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

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

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

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

159
160
  /* 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
161

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

166
167
  boolean_t thread_handling_signals;
  pthread_t thread_ref;
168

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

172
  itti_lte_time_t lte_time;
173

174
  int running;
175

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

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

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

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

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

198
#if defined(OAI_EMU) || defined(RTAI)
199
200
201
202
203
204
205
206
  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);
  }
207

winckel's avatar
winckel committed
208
#else
209
  ptr = malloc (size);
210
  if (ptr) memset(ptr,0,size);
211
#endif
winckel's avatar
winckel committed
212

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

215
  return ptr;
216
217
}

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

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

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

231
  return (result);
232
233
}

234
235
236
237
238
239
240
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
241
242
}

243
244
245
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
246

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

250
251
252
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
253

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

257
const char *itti_get_task_name(task_id_t task_id)
Cedric Roux's avatar
Cedric Roux committed
258
{
259
260
261
262
263
  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
264

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

268
static task_id_t itti_get_current_task_id(void)
269
{
270
271
272
273
274
275
276
277
278
  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;
279
    }
280
  }
281

282
  return TASK_UNKNOWN;
283
284
}

285
286
287
#ifdef RTAI
static void rt_log_debug(char *format, ...)
{
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
  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);
303
304
305
}
#endif

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

312
313
314
315
316
317
318
319
320
321
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
322

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

326
  destination_task_id = TASK_FIRST;
Cedric Roux's avatar
 
Cedric Roux committed
327

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

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

335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
    /* 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
354
355
}

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

360
  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
361

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

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

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

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

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

381
  return temp;
Cedric Roux's avatar
 
Cedric Roux committed
382
383
}

384
MessageDef *itti_alloc_new_message(task_id_t origin_task_id, MessagesIds message_id)
Cedric Roux's avatar
Cedric Roux committed
385
{
386
  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
387
388
}

389
int itti_send_msg_to_task(task_id_t destination_task_id, instance_t instance, MessageDef *message)
Cedric Roux's avatar
Cedric Roux committed
390
{
391
392
393
394
395
396
  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
397

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

403
404
  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
405

406
407
408
409
410
411
412
  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
413

414
  origin_task_id = ITTI_MSG_ORIGIN_ID(message);
415

416
  priority = itti_get_message_priority (message_id);
Cedric Roux's avatar
 
Cedric Roux committed
417

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

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

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

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

431
432
433
434
435
436
437
438
439
    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 {
Wang Tsu-Han's avatar
Wang Tsu-Han committed
440
441
      #ifdef EMULATE_RF
      #else
442
443
444
445
446
447
448
449
      /* 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);
Wang Tsu-Han's avatar
Wang Tsu-Han committed
450
      #endif
451
452
453
454
455
456
457
458
459
460

      /* 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 */
461
462
463
      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");
      }
464

465
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
466
      VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_OUT);
winckel's avatar
winckel committed
467
468
#endif

469
#ifdef RTAI
470
471
472
473
474

      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
475
#endif
476
477
478
479
480
481
482
483
484
485
      {
        /* 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));
486
        }
487
488
489
490
491
492
493
494
495
      }

      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));
496
    }
497
498
499
500
501
  } 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);
  }
502

503
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
504
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_SEND_MSG,
505
                                          __sync_and_and_fetch (&itti_desc.vcd_send_msg, ~(1L << destination_task_id)));
506
507
#endif

508
  return 0;
Cedric Roux's avatar
 
Cedric Roux committed
509
510
}

511
512
void itti_subscribe_event_fd(task_id_t task_id, int fd)
{
513
514
  thread_id_t thread_id;
  struct epoll_event event;
515

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

518
519
  thread_id = TASK_GET_THREAD_ID(task_id);
  itti_desc.threads[thread_id].nb_events++;
520

521
522
523
524
  /* 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));
525

526
527
528
  event.events  = EPOLLIN | EPOLLERR;
  event.data.u64 = 0;
  event.data.fd  = fd;
529

530
531
532
533
534
535
536
  /* 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));
  }
537

538
  ITTI_DEBUG(ITTI_DEBUG_EVEN_FD, " Successfully subscribed fd %d for task %s\n", fd, itti_get_task_name(task_id));
539
540
541
542
}

void itti_unsubscribe_event_fd(task_id_t task_id, int fd)
{
543
  thread_id_t thread_id;
544

545
546
  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);
547

548
549
550
551
552
553
554
555
  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));
  }
556

557
558
559
560
  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));
561
562
563
564
}

int itti_get_events(task_id_t task_id, struct epoll_event **events)
{
565
  thread_id_t thread_id;
566

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

569
570
  thread_id = TASK_GET_THREAD_ID(task_id);
  *events = itti_desc.threads[thread_id].events;
571

572
  return itti_desc.threads[thread_id].epoll_nb_events;
573
574
}

575
576
static inline void itti_receive_msg_internal_event_fd(task_id_t task_id, uint8_t polling, MessageDef **received_msg)
{
577
578
579
580
  thread_id_t thread_id;
  int epoll_ret = 0;
  int epoll_timeout = 0;
  int i;
581

582
583
  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");
584

585
586
  thread_id = TASK_GET_THREAD_ID(task_id);
  *received_msg = NULL;
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
  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;
637
638


639
640
641
      result = itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
      AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);

642

643
644
645
      /* Mark that the event has been processed */
      itti_desc.threads[thread_id].events[i].events &= ~EPOLLIN;
      return;
646
    }
647
  }
648
649
650
651
}

void itti_receive_msg(task_id_t task_id, MessageDef **received_msg)
{
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_and_and_fetch (&itti_desc.vcd_receive_msg, ~(1L << task_id)));
655
#endif
656

657
  itti_receive_msg_internal_event_fd(task_id, 0, received_msg);
Cedric Roux's avatar
 
Cedric Roux committed
658

winckel's avatar
winckel committed
659
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
660
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
661
                                          __sync_or_and_fetch (&itti_desc.vcd_receive_msg, 1L << task_id));
662
#endif
Cedric Roux's avatar
 
Cedric Roux committed
663
664
}

665
666
667
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
668

669
  *received_msg = NULL;
Cedric Roux's avatar
 
Cedric Roux committed
670

671
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
672
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_POLL_MSG,
673
                                          __sync_or_and_fetch (&itti_desc.vcd_poll_msg, 1L << task_id));
674
675
#endif

676
677
  {
    struct message_list_s *message;
678

679
680
    if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 1) {
      int result;
681

682
683
684
      *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);
685
    }
686
  }
Cedric Roux's avatar
 
Cedric Roux committed
687

688
689
690
  if (*received_msg == NULL) {
    ITTI_DEBUG(ITTI_DEBUG_POLL, " No message in queue[(%u:%s)]\n", task_id, itti_get_task_name(task_id));
  }
691
692

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
693
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_POLL_MSG,
694
                                          __sync_and_and_fetch (&itti_desc.vcd_poll_msg, ~(1L << task_id)));
695
#endif
Cedric Roux's avatar
 
Cedric Roux committed
696
697
}

698
699
700
701
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
702

703
704
705
706
  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
707

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

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

712
713
714
715
716
  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
717

718
  itti_desc.created_tasks ++;
719

720
721
722
  /* Wait till the thread is completely ready */
  while (itti_desc.threads[thread_id].task_state != TASK_STATE_READY)
    usleep (1000);
723

724
  return 0;
Cedric Roux's avatar
 
Cedric Roux committed
725
726
}

727
//#ifdef RTAI
728
729
void itti_set_task_real_time(task_id_t task_id)
{
730
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
731

732
  DevCheck(thread_id < itti_desc.thread_max, thread_id, itti_desc.thread_max, 0);
733

734
  itti_desc.threads[thread_id].real_time = TRUE;
735
}
knopp's avatar
   
knopp committed
736
//#endif
737

738
739
void itti_wait_ready(int wait_tasks)
{
740
  itti_desc.wait_tasks = wait_tasks;
741

742
743
744
745
746
  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);
747

748
749
  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);
750
751
}

752
753
void itti_mark_task_ready(task_id_t task_id)
{
754
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
Cedric Roux's avatar
 
Cedric Roux committed
755

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

758
759
  /* Register the thread in itti dump */
  itti_dump_thread_use_ring_buffer();
760

761
762
  /* Mark the thread as using LFDS queue */
  lfds611_queue_use(itti_desc.tasks[task_id].message_queue);
763

764
#ifdef RTAI
765
766
767
768
769
770
  /* 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);
  }
771
772
#endif

773
774
  itti_desc.threads[thread_id].task_state = TASK_STATE_READY;
  itti_desc.ready_tasks ++;
775

776
777
778
  while (itti_desc.wait_tasks != 0) {
    usleep (10000);
  }
779

780
  ITTI_DEBUG(ITTI_DEBUG_INIT, " task %s started\n", itti_get_task_name(task_id));
781
782
}

783
784
void itti_exit_task(void)
{
785
#if defined(OAI_EMU) || defined(RTAI)
786
787
788
  task_id_t task_id = itti_get_current_task_id();

  if (task_id > TASK_UNKNOWN) {
gauthier's avatar
gauthier committed
789
    VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
790
791
                                            __sync_and_and_fetch (&itti_desc.vcd_receive_msg, ~(1L << task_id)));
  }
792
793

#endif
794
  pthread_exit (NULL);
795
796
}

797
798
799
800
void itti_terminate_tasks(task_id_t task_id)
{
  // Sends Terminate signals to all tasks.
  itti_send_terminate_message (task_id);
801

802
803
804
  if (itti_desc.thread_handling_signals) {
    pthread_kill (itti_desc.thread_ref, SIGUSR1);
  }
805

806
  pthread_exit (NULL);
Cedric Roux's avatar
 
Cedric Roux committed
807
808
}

809
810
811
#ifdef RTAI
static void *itti_rt_relay_thread(void *arg)
{
812
813
  thread_id_t thread_id;
  unsigned pending_messages;
814

815
816
  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
817
818

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
819
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_IN);
820
#endif
821

822
823
824
825
826
827
828
829
830
831
832
833
834
    /* 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);
835
        }
836
837
      }
    }
838
839

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
840
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_OUT);
841
#endif
842
843
844
  }

  return NULL;
845
846
847
}
#endif

848
int itti_init(task_id_t task_max, thread_id_t thread_max, MessagesIds messages_id_max, const task_info_t *tasks_info,
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
881
882
883
884
885
886
887
              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);

888
    if (0 == ret) {
889
      AssertFatal (0, "lfds611_queue_new failed for task %s!\n", itti_get_task_name(task_id));
890
    }
891
  }
892

893
894
895
  /* 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;
896

897
    itti_desc.threads[thread_id].epoll_fd = epoll_create1(0);
898

899
900
901
902
    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));
    }
903

904
    itti_desc.threads[thread_id].task_event_fd = eventfd(0, EFD_SEMAPHORE);
905

906
907
908
909
    if (itti_desc.threads[thread_id].task_event_fd == -1) {
      /* Always assert on this condition */
      AssertFatal (0, " eventfd failed: %s!\n", strerror(errno));
    }
910

911
    itti_desc.threads[thread_id].nb_events = 1;
912

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

915
916
917
918
919
920
921
922
923
924
925
926
    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);
927

928
#ifdef RTAI
929
930
    itti_desc.threads[thread_id].real_time = FALSE;
    itti_desc.threads[thread_id].messages_pending = 0;
931
#endif
932
  }
933

934
935
936
937
  itti_desc.running = 1;
  itti_desc.wait_tasks = 0;
  itti_desc.created_tasks = 0;
  itti_desc.ready_tasks = 0;
938
#ifdef RTAI
939
940
  /* Start RT relay thread */
  DevAssert(pthread_create (&itti_desc.rt_relay_thread, NULL, itti_rt_relay_thread, NULL) >= 0);
941

942
  rt_global_heap_open();
943
#endif
944

945
#if defined(OAI_EMU) || defined(RTAI)
946
947
948
949
950
951
952
953
954
955
956
957
958
  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);
  }
959
960
#endif

961
#if defined(OAI_EMU) || defined(RTAI)
962
963
964
  itti_desc.vcd_poll_msg = 0;
  itti_desc.vcd_receive_msg = 0;
  itti_desc.vcd_send_msg = 0;
965
966
#endif

967
  itti_dump_init (messages_definition_xml, dump_file_name);
Cedric Roux's avatar
 
Cedric Roux committed
968

969
  CHECK_INIT_RETURN(timer_init ());
Cedric Roux's avatar
 
Cedric Roux committed
970