intertask_interface.c 36.4 KB
Newer Older
1 2
/*******************************************************************************

3 4
 Eurecom OpenAirInterface
 Copyright(c) 1999 - 2012 Eurecom
5

6 7 8
 This program is free software; you can redistribute it and/or modify it
 under the terms and conditions of the GNU General Public License,
 version 2, as published by the Free Software Foundation.
9

10 11 12 13
 This program is distributed in the hope 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.
14

15 16 17
 You should have received a copy of the GNU General Public License along with
 this program; if not, write to the Free Software Foundation, Inc.,
 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
18

19 20
 The full GNU General Public License is included in this distribution in
 the file called "COPYING".
21

22 23 24 25 26 27
 Contact Information
 Openair Admin: openair_admin@eurecom.fr
 Openair Tech : openair_tech@eurecom.fr
 Forums       : http://forums.eurecom.fr/openairinterface
 Address      : EURECOM, Campus SophiaTech, 450 Route des Chappes
 06410 Biot FRANCE
28

29
 *******************************************************************************/
30

31
#define _GNU_SOURCE
32 33 34 35 36 37
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
38
#include <signal.h>
39

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

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

47 48 49 50 51 52
#include "liblfds611.h"

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

53
#if defined(OAI_EMU) || defined(RTAI)
54
# include "memory_pools.h"
55 56 57
# include "vcd_signal_dumper.h"
#endif

58 59 60
/* Includes "intertask_interface_init.h" to check prototype coherence, but
 * disable threads and messages information generation.
 */
61 62 63 64
#define CHECK_PROTOTYPE_ONLY
#include "intertask_interface_init.h"
#undef CHECK_PROTOTYPE_ONLY

65
#include "signals.h"
66 67
#include "timer.h"

68 69 70 71 72 73 74 75 76 77
/* 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;
78

79 80
/* Don't flush if using RTAI */
#ifdef RTAI
81
# define ITTI_DEBUG(m, x, args...) do { if ((m) & itti_debug) rt_printk("[ITTI][D]"x, ##args); } \
82 83
    while(0)
#else
84
# define ITTI_DEBUG(m, x, args...) do { if ((m) & itti_debug) fprintf(stdout, "[ITTI][D]"x, ##args); fflush (stdout); } \
85
    while(0)
86
#endif
87 88
#define ITTI_ERROR(x, args...) do { fprintf(stdout, "[ITTI][E]"x, ##args); fflush (stdout); } \
    while(0)
89 90 91 92

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

93
#ifndef EFD_SEMAPHORE
94 95 96
# define KERNEL_VERSION_PRE_2_6_30 1
#endif

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

102
typedef enum task_state_s {
103
    TASK_STATE_NOT_CONFIGURED, TASK_STATE_STARTING, TASK_STATE_READY, TASK_STATE_ENDED, TASK_STATE_MAX,
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
109

110 111
    message_number_t message_number; ///< Unique message number
    uint32_t message_priority; ///< Message priority
112
} message_list_t;
113

114 115 116
typedef struct thread_desc_s {
    /* 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 123

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

124
    /* The thread fd */
125 126 127 128 129
    int task_event_fd;

    /* Number of events to monitor */
    uint16_t nb_events;

130
#if defined(KERNEL_VERSION_PRE_2_6_30)
131
    eventfd_t sem_counter;
132 133
#endif

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

    int epoll_nb_events;
142 143 144 145 146 147 148

#ifdef RTAI
    /* Flag to mark real time thread */
    unsigned real_time;

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

typedef struct task_desc_s {
    /* Queue of messages belonging to the task */
    struct lfds611_queue_state *message_queue;
155 156
} task_desc_t;

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

161
    /* Current message number. Incremented every call to send_msg_to_task */
162
    message_number_t message_number __attribute__((aligned(8)));
163 164

    thread_id_t thread_max;
165
    task_id_t task_max;
166 167
    MessagesIds messages_id_max;

168 169
    pthread_t thread_handling_signals;

170
    const task_info_t *tasks_info;
171 172
    const message_info_t *messages_info;

173
    itti_lte_time_t lte_time;
174 175

    int running;
176 177 178 179

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

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

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

static itti_desc_t itti_desc;
194

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

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

205
        ITTI_ERROR (" Memory pools statistics:\n%s", statistics);
206 207
        free (statistics);
    }
winckel's avatar
winckel committed
208 209
#else
    ptr = malloc (size);
210
#endif
winckel's avatar
winckel committed
211

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

    return ptr;
}

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

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

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

    return (result);
231 232
}

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

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

    return (itti_desc.messages_info[message_id].priority);
}

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

    return (itti_desc.messages_info[message_id].name);
}

253
const char *itti_get_task_name(task_id_t task_id)
Cedric Roux's avatar
Cedric Roux committed
254
{
255
    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
256

257
    return (itti_desc.tasks_info[task_id].name);
Cedric Roux's avatar
Cedric Roux committed
258 259
}

260
static task_id_t itti_get_current_task_id(void)
261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277
{
    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;
        }
    }

    return TASK_UNKNOWN;
}

278 279 280 281 282 283
void itti_update_lte_time(uint32_t frame, uint8_t slot)
{
    itti_desc.lte_time.frame = frame;
    itti_desc.lte_time.slot = slot;
}

284
int itti_send_broadcast_message(MessageDef *message_p) {
285
    task_id_t destination_task_id;
286
    task_id_t origin_task_id;
287
    thread_id_t origin_thread_id;
288
    uint32_t thread_id;
289
    int ret = 0;
290
    int result;
291

292
    AssertFatal (message_p != NULL, "Trying to broadcast a NULL message!\n");
293

294 295
    origin_task_id = message_p->ittiMsgHeader.originTaskId;
    origin_thread_id = TASK_GET_THREAD_ID(origin_task_id);
296

297 298
    destination_task_id = TASK_FIRST;
    for (thread_id = THREAD_FIRST; thread_id < itti_desc.thread_max; thread_id++) {
299 300
        MessageDef *new_message_p;

301 302 303 304
        while (thread_id != TASK_GET_THREAD_ID(destination_task_id))
        {
            destination_task_id++;
        }
305
        /* Skip task that broadcast the message */
306
        if (thread_id != origin_thread_id) {
307
            /* Skip tasks which are not running */
308
            if (itti_desc.threads[thread_id].task_state == TASK_STATE_READY) {
309
                new_message_p = itti_malloc (origin_task_id, destination_task_id, sizeof(MessageDef));
310
                AssertFatal (new_message_p != NULL, "New message allocation failed!\n");
311 312

                memcpy (new_message_p, message_p, sizeof(MessageDef));
313
                result = itti_send_msg_to_task (destination_task_id, INSTANCE_DEFAULT, new_message_p);
314
                AssertFatal (result >= 0, "Failed to send message %d to thread %d (task %d)!\n", message_p->ittiMsgHeader.messageId, thread_id, destination_task_id);
315
            }
316 317
        }
    }
318 319
    result = itti_free (ITTI_MSG_ORIGIN_ID(message_p), message_p);
    AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);
320 321 322 323

    return ret;
}

Cedric Roux's avatar
Cedric Roux committed
324 325
inline MessageDef *itti_alloc_new_message_sized(task_id_t origin_task_id, MessagesIds message_id, MessageHeaderSize size)
{
326 327
    MessageDef *temp = NULL;

328
    AssertFatal (message_id < itti_desc.messages_id_max, "Message id (%d) is out of range (%d)!\n", message_id, itti_desc.messages_id_max);
329

330 331 332 333
#if defined(OAI_EMU) || defined(RTAI)
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, size);
#endif

334 335 336 337 338 339
    if (origin_task_id == TASK_UNKNOWN)
    {
        /* Try to identify real origin task ID */
        origin_task_id = itti_get_current_task_id();
    }

340
    temp = itti_malloc (origin_task_id, TASK_UNKNOWN, sizeof(MessageHeader) + size);
341

Cedric Roux's avatar
Cedric Roux committed
342 343 344
    temp->ittiMsgHeader.messageId = message_id;
    temp->ittiMsgHeader.originTaskId = origin_task_id;
    temp->ittiMsgHeader.ittiMsgSize = size;
345

346 347 348 349
#if defined(OAI_EMU) || defined(RTAI)
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, 0);
#endif

350 351 352
    return temp;
}

Cedric Roux's avatar
Cedric Roux committed
353 354 355 356 357
inline MessageDef *itti_alloc_new_message(task_id_t origin_task_id, MessagesIds message_id)
{
    return itti_alloc_new_message_sized(origin_task_id, message_id, itti_desc.messages_info[message_id].size);
}

358
int itti_send_msg_to_task(task_id_t destination_task_id, instance_t instance, MessageDef *message)
Cedric Roux's avatar
Cedric Roux committed
359
{
360
    thread_id_t destination_thread_id;
361
    thread_id_t origin_task_id;
362
    message_list_t *new;
363 364 365
    uint32_t priority;
    message_number_t message_number;
    uint32_t message_id;
366

winckel's avatar
winckel committed
367
#if defined(OAI_EMU) || defined(RTAI)
368 369
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_SEND_MSG,
                                            __sync_or_and_fetch (&itti_desc.vcd_send_msg, 1L << destination_task_id));
winckel's avatar
winckel committed
370 371
#endif

372
    AssertFatal (message != NULL, "Message is NULL!\n");
373
    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);
374

375 376
    destination_thread_id = TASK_GET_THREAD_ID(destination_task_id);
    message->ittiMsgHeader.destinationTaskId = destination_task_id;
Cedric Roux's avatar
Cedric Roux committed
377 378 379 380
    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;
381
    AssertFatal (message_id < itti_desc.messages_id_max, "Message id (%d) is out of range (%d)!\n", message_id, itti_desc.messages_id_max);
382

383 384
    origin_task_id = ITTI_MSG_ORIGIN_ID(message);

385
    priority = itti_get_message_priority (message_id);
386

387 388
    /* Increment the global message number */
    message_number = itti_increment_message_number ();
389

390
    itti_dump_queue_message (origin_task_id, message_number, message, itti_desc.messages_info[message_id].name,
391 392 393
                             sizeof(MessageHeader) + message->ittiMsgHeader.ittiMsgSize);

    if (destination_task_id != TASK_UNKNOWN)
394
    {
395
#if defined(OAI_EMU) || defined(RTAI)
winckel's avatar
winckel committed
396
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_IN);
397 398

        memory_pools_set_info (itti_desc.memory_pools_handle, message, 1, destination_task_id);
winckel's avatar
winckel committed
399 400
#endif

401 402
        if (itti_desc.threads[destination_thread_id].task_state == TASK_STATE_ENDED)
        {
403
            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",
404 405 406 407 408 409 410 411 412 413
                       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 */
414
            AssertFatal (itti_desc.threads[destination_thread_id].task_state == TASK_STATE_READY, "Cannot send message %d to thread %d, it is not in ready state (%d)!\n",
415
                         message_id, destination_thread_id, itti_desc.threads[destination_thread_id].task_state);
416

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

420 421 422 423
            /* Fill in members */
            new->msg = message;
            new->message_number = message_number;
            new->message_priority = priority;
424

425 426
            /* Enqueue message in destination task queue */
            lfds611_queue_enqueue(itti_desc.tasks[destination_task_id].message_queue, new);
427

428
#if defined(OAI_EMU) || defined(RTAI)
429
            vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_OUT);
winckel's avatar
winckel committed
430 431
#endif

432
#ifdef RTAI
433 434 435 436 437 438
            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
439 440
#endif
            {
441 442 443 444
                /* 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;
445
                    eventfd_t sem_counter = 1;
446

447 448
                    /* 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));
winckel's avatar
winckel committed
449 450
                    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));
451
                }
452
            }
453

454
            ITTI_DEBUG(ITTI_DEBUG_SEND, " Message %s, number %lu with priority %d successfully sent from %s to queue (%u:%s)\n",
455 456 457 458 459 460 461
                       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));
        }
Cedric Roux's avatar
Cedric Roux committed
462 463
    } else {
        /* This is a debug message to TASK_UNKNOWN, we can release safely release it */
464 465
        int result = itti_free(origin_task_id, message);
        AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);
466
    }
467

468
#if defined(OAI_EMU) || defined(RTAI)
469 470
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_SEND_MSG,
                                            __sync_and_and_fetch (&itti_desc.vcd_send_msg, ~(1L << destination_task_id)));
471 472
#endif

473 474 475
    return 0;
}

476 477
void itti_subscribe_event_fd(task_id_t task_id, int fd)
{
478
    thread_id_t thread_id;
479 480
    struct epoll_event event;

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

483 484
    thread_id = TASK_GET_THREAD_ID(task_id);
    itti_desc.threads[thread_id].nb_events++;
485 486

    /* Reallocate the events */
487 488 489
    itti_desc.threads[thread_id].events = realloc(
        itti_desc.threads[thread_id].events,
        itti_desc.threads[thread_id].nb_events * sizeof(struct epoll_event));
490

491
    event.events  = EPOLLIN | EPOLLERR;
Cedric Roux's avatar
Cedric Roux committed
492 493
    event.data.u64 = 0;
    event.data.fd  = fd;
494 495

    /* Add the event fd to the list of monitored events */
496
    if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_ADD, fd,
497 498 499
        &event) != 0)
    {
        /* Always assert on this condition */
500
        AssertFatal (0, "epoll_ctl (EPOLL_CTL_ADD) failed for task %s, fd %d: %s!\n",
501
                     itti_get_task_name(task_id), fd, strerror(errno));
502
    }
503

504
    ITTI_DEBUG(ITTI_DEBUG_EVEN_FD, " Successfully subscribed fd %d for task %s\n", fd, itti_get_task_name(task_id));
505 506 507 508
}

void itti_unsubscribe_event_fd(task_id_t task_id, int fd)
{
509 510
    thread_id_t thread_id;

511 512
    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);
513

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

523 524 525 526
    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));
527 528 529 530
}

int itti_get_events(task_id_t task_id, struct epoll_event **events)
{
531 532
    thread_id_t thread_id;

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

535 536
    thread_id = TASK_GET_THREAD_ID(task_id);
    *events = itti_desc.threads[thread_id].events;
537

538
    return itti_desc.threads[thread_id].epoll_nb_events;
539 540
}

541 542
static inline void itti_receive_msg_internal_event_fd(task_id_t task_id, uint8_t polling, MessageDef **received_msg)
{
543
    thread_id_t thread_id;
544 545
    int epoll_ret = 0;
    int epoll_timeout = 0;
546
    int i;
547

548 549
    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");
550

551
    thread_id = TASK_GET_THREAD_ID(task_id);
552 553 554 555 556 557 558 559
    *received_msg = NULL;

    if (polling) {
        /* In polling mode we set the timeout to 0 causing epoll_wait to return
         * immediately.
         */
        epoll_timeout = 0;
    } else {
560
        /* timeout = -1 causes the epoll_wait to wait indefinitely.
561 562 563 564
         */
        epoll_timeout = -1;
    }

565
    do {
566 567 568
        epoll_ret = epoll_wait(itti_desc.threads[thread_id].epoll_fd,
                               itti_desc.threads[thread_id].events,
                               itti_desc.threads[thread_id].nb_events,
569 570
                               epoll_timeout);
    } while (epoll_ret < 0 && errno == EINTR);
571 572

    if (epoll_ret < 0) {
573
        AssertFatal (0, "epoll_wait failed for task %s: %s!\n", itti_get_task_name(task_id), strerror(errno));
574 575 576 577 578 579
    }
    if (epoll_ret == 0 && polling) {
        /* No data to read -> return */
        return;
    }

580
    itti_desc.threads[thread_id].epoll_nb_events = epoll_ret;
581

582
    for (i = 0; i < epoll_ret; i++) {
583
        /* Check if there is an event for ITTI for the event fd */
584 585
        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))
586
        {
587
            struct message_list_s *message = NULL;
588 589 590
            eventfd_t   sem_counter;
            ssize_t     read_ret;
            int         result;
591 592

            /* Read will always return 1 */
593
            read_ret = read (itti_desc.threads[thread_id].task_event_fd, &sem_counter, sizeof(sem_counter));
594
            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));
595

596 597
#if defined(KERNEL_VERSION_PRE_2_6_30)
            /* Store the value of the semaphore counter */
598
            itti_desc.threads[task_id].sem_counter = sem_counter - 1;
599 600
#endif

601
            if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
602
                /* No element in list -> this should not happen */
603
                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);
604
            }
605
            AssertFatal(message != NULL, "Message from message queue is NULL!\n");
606
            *received_msg = message->msg;
607 608 609
            result = itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
            AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);

610 611
            /* Mark that the event has been processed */
            itti_desc.threads[thread_id].events[i].events &= ~EPOLLIN;
612
            return;
613 614 615 616 617 618
        }
    }
}

void itti_receive_msg(task_id_t task_id, MessageDef **received_msg)
{
619
#if defined(OAI_EMU) || defined(RTAI)
620 621
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
                                            __sync_and_and_fetch (&itti_desc.vcd_receive_msg, ~(1L << task_id)));
622
#endif
623 624 625

#if defined(KERNEL_VERSION_PRE_2_6_30)
    /* Store the value of the semaphore counter */
626 627 628
    if (itti_desc.threads[task_id].sem_counter > 0) {
        struct message_list_s *message = NULL;

629 630
        if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
            /* No element in list -> this should not happen */
631
            DevParam(task_id, itti_desc.threads[task_id].sem_counter, 0);
632 633 634
        }
        DevAssert(message != NULL);
        *received_msg = message->msg;
635
        itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
636

637
        itti_desc.threads[task_id].sem_counter--;
638 639
    } else
#endif
640
    itti_receive_msg_internal_event_fd(task_id, 0, received_msg);
641

winckel's avatar
winckel committed
642
#if defined(OAI_EMU) || defined(RTAI)
643 644
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
                                            __sync_or_and_fetch (&itti_desc.vcd_receive_msg, 1L << task_id));
645
#endif
646 647
}

648
void itti_poll_msg(task_id_t task_id, MessageDef **received_msg) {
649
    AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)!\n", task_id, itti_desc.task_max);
650 651 652

    *received_msg = NULL;

653
#if defined(OAI_EMU) || defined(RTAI)
654 655
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_POLL_MSG,
                                            __sync_or_and_fetch (&itti_desc.vcd_poll_msg, 1L << task_id));
656 657 658 659 660 661 662
#endif

    {
        struct message_list_s *message;

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

665
            *received_msg = message->msg;
666 667
            result = itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
            AssertFatal (result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);
668 669
        }
    }
670

671 672
    if (*received_msg == NULL) {
        ITTI_DEBUG(ITTI_DEBUG_POLL, " No message in queue[(%u:%s)]\n", task_id, itti_get_task_name(task_id));
673
    }
674 675

#if defined(OAI_EMU) || defined(RTAI)
676 677
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_POLL_MSG,
                                            __sync_and_and_fetch (&itti_desc.vcd_poll_msg, ~(1L << task_id)));
678
#endif
679 680
}

681
int itti_create_task(task_id_t task_id, void *(*start_routine)(void *), void *args_p) {
682
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
683
    int result;
684

685 686 687
    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",
688
                 task_id, thread_id, itti_desc.threads[thread_id].task_state);
689

690
    itti_desc.threads[thread_id].task_state = TASK_STATE_STARTING;
691

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

694
    result = pthread_create (&itti_desc.threads[thread_id].task_thread, NULL, start_routine, args_p);
695
    AssertFatal (result >= 0, "Thread creation for task %d, thread %d failed (%d)!\n", task_id, thread_id, result);
696

697 698
    itti_desc.created_tasks ++;

699
    /* Wait till the thread is completely ready */
700
    while (itti_desc.threads[thread_id].task_state != TASK_STATE_READY)
701 702
        usleep (1000);

703 704 705
    return 0;
}

706 707 708 709 710 711 712 713 714 715 716
#ifdef RTAI
void itti_set_task_real_time(task_id_t task_id)
{
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);

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

    itti_desc.threads[thread_id].real_time = TRUE;
}
#endif

717 718 719 720
void itti_wait_ready(int wait_tasks)
{
    itti_desc.wait_tasks = wait_tasks;

721
    ITTI_DEBUG(ITTI_DEBUG_INIT, " wait for tasks: %s, created tasks %d, ready tasks %d\n", itti_desc.wait_tasks ? "yes" : "no",
722 723
        itti_desc.created_tasks, itti_desc.ready_tasks);

724
    AssertFatal (itti_desc.created_tasks == itti_desc.ready_tasks, "Number of created tasks (%d) does not match ready tasks (%d), wait task %d!\n",
725
                 itti_desc.created_tasks, itti_desc.ready_tasks, itti_desc.wait_tasks);
726 727
}

728 729
void itti_mark_task_ready(task_id_t task_id)
{
730 731
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);

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

734 735 736 737 738 739
    /* Register the thread in itti dump */
    itti_dump_thread_use_ring_buffer();

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

740 741 742 743 744 745 746 747 748
#ifdef RTAI
    /* 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);
    }
#endif

749
    itti_desc.threads[thread_id].task_state = TASK_STATE_READY;
750 751 752 753 754 755 756
    itti_desc.ready_tasks ++;

    while (itti_desc.wait_tasks != 0)
    {
        usleep (10000);
    }

757
    ITTI_DEBUG(ITTI_DEBUG_INIT, " task %s started\n", itti_get_task_name(task_id));
758 759
}

760
void itti_exit_task(void) {
761 762 763 764 765 766 767 768 769
#if defined(OAI_EMU) || defined(RTAI)
    task_id_t task_id = itti_get_current_task_id();

    if (task_id > TASK_UNKNOWN)
    {
        vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_RECV_MSG,
                                                __sync_and_and_fetch (&itti_desc.vcd_receive_msg, ~(1L << task_id)));
    }
#endif
770 771 772
    pthread_exit (NULL);
}

773
void itti_terminate_tasks(task_id_t task_id) {
Cedric Roux's avatar
Cedric Roux committed
774
    // Sends Terminate signals to all tasks.
775 776 777 778 779 780 781
    itti_send_terminate_message (task_id);

    if (itti_desc.thread_handling_signals >= 0) {
        pthread_kill (itti_desc.thread_handling_signals, SIGUSR1);
    }

    pthread_exit (NULL);
782 783
}

784 785 786 787 788 789 790 791
#ifdef RTAI
static void *itti_rt_relay_thread(void *arg)
{
    thread_id_t thread_id;
    unsigned pending_messages;

    while (itti_desc.running)
    {
792 793 794 795 796
        usleep (200); // Poll for messages a little more than 2 time by slot to get a small latency between RT and other tasks

#if defined(OAI_EMU) || defined(RTAI)
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_IN);
#endif
797 798 799 800 801 802 803 804 805 806 807 808

        /* 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;
809
                    eventfd_t sem_counter = pending_messages;
810 811 812 813 814 815 816

                    /* 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);
                }
            }
        }
817 818 819 820

#if defined(OAI_EMU) || defined(RTAI)
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_OUT);
#endif
821 822 823 824 825
    }
    return NULL;
}
#endif

826
int itti_init(task_id_t task_max, thread_id_t thread_max, MessagesIds messages_id_max, const task_info_t *tasks_info,
827
              const message_info_t *messages_info, const char * const messages_definition_xml, const char * const dump_file_name) {
828 829
    task_id_t task_id;
    thread_id_t thread_id;
830 831
    int ret;

832
    itti_desc.message_number = 1;
833

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

836
    CHECK_INIT_RETURN(signal_mask());
837

838
    /* Saves threads and messages max values */
839
    itti_desc.task_max = task_max;
840 841
    itti_desc.thread_max = thread_max;
    itti_desc.messages_id_max = messages_id_max;
842
    itti_desc.thread_handling_signals = -1;
843
    itti_desc.tasks_info = tasks_info;
844 845 846
    itti_desc.messages_info = messages_info;

    /* Allocates memory for tasks info */
847 848 849 850
    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));
851 852

    /* Initializing each queue and related stuff */
853
    for (task_id = TASK_FIRST; task_id < itti_desc.task_max; task_id++)
854
    {
855
        ITTI_DEBUG(ITTI_DEBUG_INIT, " Initializing %stask %s%s%s\n",
856 857 858 859 860 861
                   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) : "");

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

        ret = lfds611_queue_new(&itti_desc.tasks[task_id].message_queue, itti_desc.tasks_info[task_id].queue_size);
        if (ret < 0)
866
        {
867
            AssertFatal (0, "lfds611_queue_new failed for task %s!\n", itti_get_task_name(task_id));
868
        }
869 870 871 872 873 874
    }

    /* 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;
875

876 877
        itti_desc.threads[thread_id].epoll_fd = epoll_create1(0);
        if (itti_desc.threads[thread_id].epoll_fd == -1) {
878
            /* Always assert on this condition */
879
            AssertFatal (0, "Failed to create new epoll fd: %s!\n", strerror(errno));
880 881
        }

882 883 884 885 886 887
# if defined(KERNEL_VERSION_PRE_2_6_30)
        /* SR: for kernel versions < 2.6.30 EFD_SEMAPHORE is not defined.
         * A read operation on the event fd will return the 8 byte value.
         */
        itti_desc.threads[thread_id].task_event_fd = eventfd(0, 0);
# else
888
        itti_desc.threads[thread_id].task_event_fd = eventfd(0, EFD_SEMAPHORE);
889
# endif
890
        if (itti_desc.threads[thread_id].task_event_fd == -1)
891
        {
892
            /* Always assert on this condition */
893
            AssertFatal (0, " eventfd failed: %s!\n", strerror(errno));
894 895
        }

896
        itti_desc.threads[thread_id].nb_events = 1;
897

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

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

        /* Add the event fd to the list of monitored events */
904 905
        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)
906 907
        {
            /* Always assert on this condition */
908
            AssertFatal (0, " epoll_ctl (EPOLL_CTL_ADD) failed: %s!\n", strerror(errno));
909
        }
910

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

914 915 916
#ifdef RTAI
        itti_desc.threads[thread_id].real_time = FALSE;
        itti_desc.threads[thread_id].messages_pending = 0;
917
#endif
918
    }
919

920
    itti_desc.running = 1;
921 922 923
    itti_desc.wait_tasks = 0;
    itti_desc.created_tasks = 0;
    itti_desc.ready_tasks = 0;
924 925 926
#ifdef RTAI
    /* Start RT relay thread */
    DevAssert(pthread_create (&itti_desc.rt_relay_thread, NULL, itti_rt_relay_thread, NULL) >= 0);
927 928

    rt_global_heap_open();
929
#endif
930

931 932 933 934
#if defined(OAI_EMU) || defined(RTAI)
    itti_desc.memory_pools_handle = memory_pools_create (4);
    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);
935
    memory_pools_add_pool (itti_desc.memory_pools_handle, 10000,                                1000);
936
    memory_pools_add_pool (itti_desc.memory_pools_handle,  500,                                 20000);
937 938 939 940

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

941
        ITTI_DEBUG(ITTI_DEBUG_MP_STATISTICS, " Memory pools statistics:\n%s", statistics);
942 943 944 945
        free (statistics);
    }