intertask_interface.c 35.1 KB
Newer Older
Cedric Roux's avatar
 
Cedric Roux committed
1
/*******************************************************************************
2
    OpenAirInterface
ghaddab's avatar
ghaddab committed
3 4 5 6 7 8 9 10 11 12 13 14 15 16
    Copyright(c) 1999 - 2014 Eurecom

    OpenAirInterface is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.


    OpenAirInterface is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
17 18
    along with OpenAirInterface.The full GNU General Public License is
   included in this distribution in the file called "COPYING". If not,
ghaddab's avatar
ghaddab committed
19 20 21 22 23 24
   see <http://www.gnu.org/licenses/>.

  Contact Information
  OpenAirInterface Admin: openair_admin@eurecom.fr
  OpenAirInterface Tech : openair_tech@eurecom.fr
  OpenAirInterface Dev  : openair4g-devel@eurecom.fr
25

ghaddab's avatar
ghaddab committed
26
  Address      : Eurecom, Campus SophiaTech, 450 Route des Chappes, CS 50193 - 06904 Biot Sophia Antipolis cedex, FRANCE
Cedric Roux's avatar
 
Cedric Roux committed
27

28
 *******************************************************************************/
Cedric Roux's avatar
 
Cedric Roux committed
29

30
#define _GNU_SOURCE
Cedric Roux's avatar
 
Cedric Roux committed
31 32 33 34 35 36
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
37
#include <signal.h>
Cedric Roux's avatar
 
Cedric Roux committed
38

39 40
#include <sys/epoll.h>
#include <sys/eventfd.h>
41

42 43 44 45
#ifdef RTAI
# include <rtai_shm.h>
#endif

gauthier's avatar
gauthier committed
46 47 48 49
#if !defined(TRUE)
#define TRUE 1
#endif

50 51 52 53 54 55
#include "liblfds611.h"

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

56
#if defined(OAI_EMU) || defined(RTAI)
57
# include "memory_pools.h"
58 59 60
# include "vcd_signal_dumper.h"
#endif

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

68
#include "signals.h"
Cedric Roux's avatar
 
Cedric Roux committed
69 70
#include "timer.h"

71 72 73 74 75 76 77 78
#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

79 80 81 82 83 84 85 86 87 88
/* 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)

const int itti_debug = ITTI_DEBUG_ISSUES | ITTI_DEBUG_MP_STATISTICS;
Cedric Roux's avatar
 
Cedric Roux committed
89

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

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

101 102 103 104 105
#ifdef RTAI
# define ITTI_MEM_PAGE_SIZE (1024)
# define ITTI_MEM_SIZE      (16 * 1024 * 1024)
#endif

Cedric Roux's avatar
 
Cedric Roux committed
106
typedef enum task_state_s {
107
  TASK_STATE_NOT_CONFIGURED, TASK_STATE_STARTING, TASK_STATE_READY, TASK_STATE_ENDED, TASK_STATE_MAX,
Cedric Roux's avatar
 
Cedric Roux committed
108 109 110
} task_state_t;

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

114 115
  message_number_t message_number; ///< Unique message number
  uint32_t message_priority; ///< Message priority
116
} message_list_t;
Cedric Roux's avatar
 
Cedric Roux committed
117

118
typedef struct thread_desc_s {
119 120
  /* pthread associated with the thread */
  pthread_t task_thread;
121

122 123
  /* State of the thread */
  volatile task_state_t task_state;
124

125 126
  /* This fd is used internally by ITTI. */
  int epoll_fd;
127

128 129
  /* The thread fd */
  int task_event_fd;
130

131 132
  /* Number of events to monitor */
  uint16_t nb_events;
133

134

135 136 137 138 139 140
  /* 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;
141

142
  int epoll_nb_events;
143

144 145 146
  //#ifdef RTAI
  /* Flag to mark real time thread */
  unsigned real_time;
147

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

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

158
typedef struct itti_desc_s {
159 160
  thread_desc_t *threads;
  task_desc_t   *tasks;
161

162 163
  /* 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
164

165 166 167
  thread_id_t thread_max;
  task_id_t task_max;
  MessagesIds messages_id_max;
Cedric Roux's avatar
 
Cedric Roux committed
168

169 170
  boolean_t thread_handling_signals;
  pthread_t thread_ref;
171

172 173
  const task_info_t *tasks_info;
  const message_info_t *messages_info;
Cedric Roux's avatar
 
Cedric Roux committed
174

175
  itti_lte_time_t lte_time;
176

177
  int running;
178

179 180 181
  volatile uint32_t created_tasks;
  volatile uint32_t ready_tasks;
  volatile int      wait_tasks;
182
#ifdef RTAI
183
  pthread_t rt_relay_thread;
184
#endif
185 186

#if defined(OAI_EMU) || defined(RTAI)
187
  memory_pools_handle_t memory_pools_handle;
188

189 190 191
  uint64_t vcd_poll_msg;
  uint64_t vcd_receive_msg;
  uint64_t vcd_send_msg;
192
#endif
193 194 195
} itti_desc_t;

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

197
void *itti_malloc(task_id_t origin_task_id, task_id_t destination_task_id, ssize_t size)
198
{
199
  void *ptr = NULL;
200

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

winckel's avatar
winckel committed
211
#else
212
  ptr = malloc (size);
213
#endif
winckel's avatar
winckel committed
214

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

217
  return ptr;
218 219
}

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

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

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

233
  return (result);
234 235
}

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

245 246 247
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
248

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

252 253 254
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
255

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

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

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

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

284
  return TASK_UNKNOWN;
285 286
}

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

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

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

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

328
  destination_task_id = TASK_FIRST;
Cedric Roux's avatar
 
Cedric Roux committed
329

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

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

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

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

362
  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
363

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

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

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

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

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

383
  return temp;
Cedric Roux's avatar
 
Cedric Roux committed
384 385
}

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

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

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

405 406
  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
407

408 409 410 411 412 413 414
  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
415

416
  origin_task_id = ITTI_MSG_ORIGIN_ID(message);
417

418
  priority = itti_get_message_priority (message_id);
Cedric Roux's avatar
 
Cedric Roux committed
419

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

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

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

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

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

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

468
#ifdef RTAI
469 470 471 472 473

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

584 585
  thread_id = TASK_GET_THREAD_ID(task_id);
  *received_msg = NULL;
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 639 640 641 642
  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;
643
    }
644
  }
645 646 647 648
}

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

654
  itti_receive_msg_internal_event_fd(task_id, 0, received_msg);
Cedric Roux's avatar
 
Cedric Roux committed
655

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

662 663 664
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
665

666
  *received_msg = NULL;
Cedric Roux's avatar
 
Cedric Roux committed
667

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

673 674
  {
    struct message_list_s *message;
675

676 677
    if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 1) {
      int result;
678

679 680 681
      *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);
682
    }
683
  }
Cedric Roux's avatar
 
Cedric Roux committed
684

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

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

695 696 697 698
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
699

700 701 702 703
  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
704

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

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

709 710 711 712 713
  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
714

715
  itti_desc.created_tasks ++;
716

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

721
  return 0;
Cedric Roux's avatar
 
Cedric Roux committed
722 723
}

724
//#ifdef RTAI
725 726
void itti_set_task_real_time(task_id_t task_id)
{
727
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
728

729
  DevCheck(thread_id < itti_desc.thread_max, thread_id, itti_desc.thread_max, 0);
730

731
  itti_desc.threads[thread_id].real_time = TRUE;
732
}
knopp's avatar
 
knopp committed
733
//#endif
734

735 736
void itti_wait_ready(int wait_tasks)
{
737
  itti_desc.wait_tasks = wait_tasks;
738

739 740 741 742 743
  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);
744

745 746
  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);
747 748
}

749 750
void itti_mark_task_ready(task_id_t task_id)
{
751
  thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
Cedric Roux's avatar
 
Cedric Roux committed
752

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

755 756
  /* Register the thread in itti dump */
  itti_dump_thread_use_ring_buffer();
757

758 759
  /* Mark the thread as using LFDS queue */
  lfds611_queue_use(itti_desc.tasks[task_id].message_queue);
760

761
#ifdef RTAI
762 763 764 765 766 767
  /* 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);
  }
768 769
#endif

770 771
  itti_desc.threads[thread_id].task_state = TASK_STATE_READY;
  itti_desc.ready_tasks ++;
772

773 774 775
  while (itti_desc.wait_tasks != 0) {
    usleep (10000);
  }
776

777
  ITTI_DEBUG(ITTI_DEBUG_INIT, " task %s started\n", itti_get_task_name(task_id));
778 779
}

780 781
void itti_exit_task(void)
{
782
#if defined(OAI_EMU) || defined(RTAI)
783 784 785
  task_id_t task_id = itti_get_current_task_id();

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

#endif
791
  pthread_exit (NULL);
792 793
}

794 795 796 797
void itti_terminate_tasks(task_id_t task_id)
{
  // Sends Terminate signals to all tasks.
  itti_send_terminate_message (task_id);
798

799 800 801
  if (itti_desc.thread_handling_signals) {
    pthread_kill (itti_desc.thread_ref, SIGUSR1);
  }
802

803
  pthread_exit (NULL);
Cedric Roux's avatar
 
Cedric Roux committed
804 805
}

806 807 808
#ifdef RTAI
static void *itti_rt_relay_thread(void *arg)
{
809 810
  thread_id_t thread_id;
  unsigned pending_messages;
811

812 813
  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
814 815

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
816
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_IN);
817
#endif
818

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

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
837
    VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_OUT);
838
#endif
839 840 841
  }

  return NULL;
842 843 844
}
#endif

845
int itti_init(task_id_t task_max, thread_id_t thread_max, MessagesIds messages_id_max, const task_info_t *tasks_info,