intertask_interface.c 33.9 KB
Newer Older
Cedric Roux's avatar
 
Cedric Roux committed
1 2
/*******************************************************************************

3 4
 Eurecom OpenAirInterface
 Copyright(c) 1999 - 2012 Eurecom
Cedric Roux's avatar
 
Cedric Roux committed
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.
Cedric Roux's avatar
 
Cedric Roux committed
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.
Cedric Roux's avatar
 
Cedric Roux committed
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.
Cedric Roux's avatar
 
Cedric Roux committed
18

19 20
 The full GNU General Public License is included in this distribution in
 the file called "COPYING".
Cedric Roux's avatar
 
Cedric Roux committed
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
Cedric Roux's avatar
 
Cedric Roux committed
28

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

31
#define _GNU_SOURCE
Cedric Roux's avatar
 
Cedric Roux committed
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>
Cedric Roux's avatar
 
Cedric Roux committed
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.
 */
Cedric Roux's avatar
 
Cedric Roux committed
61 62 63 64
#define CHECK_PROTOTYPE_ONLY
#include "intertask_interface_init.h"
#undef CHECK_PROTOTYPE_ONLY

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

68 69
const int itti_debug = 0;
const int itti_debug_poll = 0;
Cedric Roux's avatar
 
Cedric Roux committed
70

71 72 73 74 75 76 77 78
/* Don't flush if using RTAI */
#ifdef RTAI
# define ITTI_DEBUG(x, args...) do { if (itti_debug) rt_printk("[ITTI][D]"x, ##args); } \
    while(0)
# define ITTI_ERROR(x, args...) do { rt_printk("[ITTI][E]"x, ##args); } \
    while(0)
#else
# define ITTI_DEBUG(x, args...) do { if (itti_debug) fprintf(stdout, "[ITTI][D]"x, ##args); fflush (stdout); } \
Cedric Roux's avatar
 
Cedric Roux committed
79
    while(0)
80
# define ITTI_ERROR(x, args...) do { fprintf(stdout, "[ITTI][E]"x, ##args); fflush (stdout); } \
Cedric Roux's avatar
 
Cedric Roux committed
81
    while(0)
82
#endif
Cedric Roux's avatar
 
Cedric Roux committed
83 84 85 86

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

Cedric Roux's avatar
Cedric Roux committed
87
#ifndef EFD_SEMAPHORE
88 89 90
# define KERNEL_VERSION_PRE_2_6_30 1
#endif

91 92 93 94 95
#ifdef RTAI
# define ITTI_MEM_PAGE_SIZE (1024)
# define ITTI_MEM_SIZE      (16 * 1024 * 1024)
#endif

Cedric Roux's avatar
 
Cedric Roux committed
96
typedef enum task_state_s {
97
    TASK_STATE_NOT_CONFIGURED, TASK_STATE_STARTING, TASK_STATE_READY, TASK_STATE_ENDED, TASK_STATE_MAX,
Cedric Roux's avatar
 
Cedric Roux committed
98 99 100
} task_state_t;

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

104 105
    message_number_t message_number; ///< Unique message number
    uint32_t message_priority; ///< Message priority
106
} message_list_t;
Cedric Roux's avatar
 
Cedric Roux committed
107

108 109 110
typedef struct thread_desc_s {
    /* pthread associated with the thread */
    pthread_t task_thread;
111

112 113
    /* State of the thread */
    volatile task_state_t task_state;
114 115 116 117

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

118
    /* The thread fd */
119 120 121 122 123
    int task_event_fd;

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

124
#if defined(KERNEL_VERSION_PRE_2_6_30)
Cedric Roux's avatar
Cedric Roux committed
125
    eventfd_t sem_counter;
126 127
#endif

128 129 130 131 132 133
    /* 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;
134 135

    int epoll_nb_events;
136 137 138 139 140 141 142

#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;
143
#endif
144 145 146 147 148
} thread_desc_t;

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

151
typedef struct itti_desc_s {
152
    thread_desc_t *threads;
153
    task_desc_t   *tasks;
154

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

    thread_id_t thread_max;
159
    task_id_t task_max;
Cedric Roux's avatar
 
Cedric Roux committed
160 161
    MessagesIds messages_id_max;

162 163
    pthread_t thread_handling_signals;

164
    const task_info_t *tasks_info;
Cedric Roux's avatar
 
Cedric Roux committed
165 166
    const message_info_t *messages_info;

167
    itti_lte_time_t lte_time;
168 169

    int running;
170 171 172 173

    volatile uint32_t created_tasks;
    volatile uint32_t ready_tasks;
    volatile int      wait_tasks;
174 175 176
#ifdef RTAI
    pthread_t rt_relay_thread;
#endif
177 178

#if defined(OAI_EMU) || defined(RTAI)
179 180
    memory_pools_handle_t memory_pools_handle;

181 182 183 184
    uint64_t vcd_poll_msg;
    uint64_t vcd_receive_msg;
    uint64_t vcd_send_msg;
#endif
185 186 187
} itti_desc_t;

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

189
void *itti_malloc(task_id_t origin_task_id, task_id_t destination_task_id, ssize_t size)
190 191 192
{
    void *ptr = NULL;

193 194
#if defined(OAI_EMU) || defined(RTAI)
    ptr = memory_pools_allocate (itti_desc.memory_pools_handle, size, origin_task_id, destination_task_id);
195
#else
196
    ptr = malloc (size);
197 198
#endif

199 200 201 202 203 204 205 206 207 208
    DevCheck(ptr != NULL, size, origin_task_id, destination_task_id);
#if defined(OAI_EMU) || defined(RTAI)
    if (ptr == NULL)
    {
        char *statistics = memory_pools_statistics (itti_desc.memory_pools_handle);

        ITTI_ERROR ("\n%s", statistics);
        free (statistics);
    }
#endif
209 210 211 212 213 214 215

    return ptr;
}

void itti_free(task_id_t task_id, void *ptr)
{
    DevAssert(ptr != NULL);
216 217 218

#if defined(OAI_EMU) || defined(RTAI)
    memory_pools_free (itti_desc.memory_pools_handle, ptr, task_id);
219
#else
220
    free (ptr);
221 222 223
#endif
}

224
static inline message_number_t itti_increment_message_number(void) {
Cedric Roux's avatar
 
Cedric Roux committed
225 226 227 228
    /* Atomic operation supported by GCC: returns the current message number
     * and then increment it by 1.
     * This can be done without mutex.
     */
229
    return __sync_fetch_and_add (&itti_desc.message_number, 1);
Cedric Roux's avatar
 
Cedric Roux committed
230 231
}

232
static inline uint32_t itti_get_message_priority(MessagesIds message_id) {
Cedric Roux's avatar
 
Cedric Roux committed
233 234 235 236 237
    DevCheck(message_id < itti_desc.messages_id_max, message_id, itti_desc.messages_id_max, 0);

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

238
const char *itti_get_message_name(MessagesIds message_id) {
Cedric Roux's avatar
 
Cedric Roux committed
239 240 241 242 243
    DevCheck(message_id < itti_desc.messages_id_max, message_id, itti_desc.messages_id_max, 0);

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

244
const char *itti_get_task_name(task_id_t task_id)
Cedric Roux's avatar
Cedric Roux committed
245
{
246
    DevCheck(task_id < itti_desc.task_max, task_id, itti_desc.task_max, 0);
Cedric Roux's avatar
Cedric Roux committed
247

248
    return (itti_desc.tasks_info[task_id].name);
Cedric Roux's avatar
Cedric Roux committed
249 250
}

251
static task_id_t itti_get_current_task_id(void)
252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268
{
    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;
}

269 270 271 272 273 274
void itti_update_lte_time(uint32_t frame, uint8_t slot)
{
    itti_desc.lte_time.frame = frame;
    itti_desc.lte_time.slot = slot;
}

275
int itti_send_broadcast_message(MessageDef *message_p) {
276
    task_id_t destination_task_id;
277
    task_id_t origin_task_id;
278
    thread_id_t origin_thread_id;
279
    uint32_t thread_id;
Cedric Roux's avatar
 
Cedric Roux committed
280
    int ret = 0;
281
    int result;
Cedric Roux's avatar
 
Cedric Roux committed
282

283
    DevAssert(message_p != NULL);
Cedric Roux's avatar
 
Cedric Roux committed
284

285 286
    origin_task_id = message_p->ittiMsgHeader.originTaskId;
    origin_thread_id = TASK_GET_THREAD_ID(origin_task_id);
Cedric Roux's avatar
 
Cedric Roux committed
287

288 289
    destination_task_id = TASK_FIRST;
    for (thread_id = THREAD_FIRST; thread_id < itti_desc.thread_max; thread_id++) {
290 291
        MessageDef *new_message_p;

292 293 294 295
        while (thread_id != TASK_GET_THREAD_ID(destination_task_id))
        {
            destination_task_id++;
        }
296
        /* Skip task that broadcast the message */
297
        if (thread_id != origin_thread_id) {
298
            /* Skip tasks which are not running */
299
            if (itti_desc.threads[thread_id].task_state == TASK_STATE_READY) {
300
                new_message_p = itti_malloc (origin_task_id, destination_task_id, sizeof(MessageDef));
301
                DevAssert(message_p != NULL);
302 303

                memcpy (new_message_p, message_p, sizeof(MessageDef));
304
                result = itti_send_msg_to_task (destination_task_id, INSTANCE_DEFAULT, new_message_p);
Cedric Roux's avatar
Cedric Roux committed
305
                DevCheck(result >= 0, message_p->ittiMsgHeader.messageId, thread_id, destination_task_id);
306
            }
Cedric Roux's avatar
 
Cedric Roux committed
307 308
        }
    }
309
    itti_free (ITTI_MSG_ORIGIN_ID(message_p), message_p);
Cedric Roux's avatar
 
Cedric Roux committed
310 311 312 313

    return ret;
}

Cedric Roux's avatar
Cedric Roux committed
314 315
inline MessageDef *itti_alloc_new_message_sized(task_id_t origin_task_id, MessagesIds message_id, MessageHeaderSize size)
{
Cedric Roux's avatar
 
Cedric Roux committed
316 317
    MessageDef *temp = NULL;

318
    DevCheck(message_id < itti_desc.messages_id_max, message_id, itti_desc.messages_id_max, 0);
Cedric Roux's avatar
 
Cedric Roux committed
319

320 321 322 323
#if defined(OAI_EMU) || defined(RTAI)
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, size);
#endif

324 325 326 327 328 329
    if (origin_task_id == TASK_UNKNOWN)
    {
        /* Try to identify real origin task ID */
        origin_task_id = itti_get_current_task_id();
    }

330
    temp = itti_malloc (origin_task_id, TASK_UNKNOWN, sizeof(MessageHeader) + size);
331
    DevAssert(temp != NULL);
Cedric Roux's avatar
 
Cedric Roux committed
332

Cedric Roux's avatar
Cedric Roux committed
333 334 335
    temp->ittiMsgHeader.messageId = message_id;
    temp->ittiMsgHeader.originTaskId = origin_task_id;
    temp->ittiMsgHeader.ittiMsgSize = size;
Cedric Roux's avatar
 
Cedric Roux committed
336

337 338 339 340
#if defined(OAI_EMU) || defined(RTAI)
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, 0);
#endif

Cedric Roux's avatar
 
Cedric Roux committed
341 342 343
    return temp;
}

Cedric Roux's avatar
Cedric Roux committed
344 345 346 347 348
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);
}

349
int itti_send_msg_to_task(task_id_t destination_task_id, instance_t instance, MessageDef *message)
Cedric Roux's avatar
Cedric Roux committed
350
{
351
    thread_id_t destination_thread_id;
352
    thread_id_t origin_task_id;
353
    message_list_t *new;
354 355 356
    uint32_t priority;
    message_number_t message_number;
    uint32_t message_id;
Cedric Roux's avatar
 
Cedric Roux committed
357

winckel's avatar
winckel committed
358
#if defined(OAI_EMU) || defined(RTAI)
359 360
    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
361 362
#endif

363
    DevAssert(message != NULL);
364
    DevCheck(destination_task_id < itti_desc.task_max, destination_task_id, itti_desc.task_max, 0);
Cedric Roux's avatar
 
Cedric Roux committed
365

366 367
    destination_thread_id = TASK_GET_THREAD_ID(destination_task_id);
    message->ittiMsgHeader.destinationTaskId = destination_task_id;
Cedric Roux's avatar
Cedric Roux committed
368 369 370 371
    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;
Cedric Roux's avatar
 
Cedric Roux committed
372 373
    DevCheck(message_id < itti_desc.messages_id_max, itti_desc.messages_id_max, message_id, 0);

374 375
    origin_task_id = ITTI_MSG_ORIGIN_ID(message);

376
    priority = itti_get_message_priority (message_id);
Cedric Roux's avatar
 
Cedric Roux committed
377

378 379
    /* Increment the global message number */
    message_number = itti_increment_message_number ();
Cedric Roux's avatar
 
Cedric Roux committed
380

381
    itti_dump_queue_message (origin_task_id, message_number, message, itti_desc.messages_info[message_id].name,
382 383 384
                             sizeof(MessageHeader) + message->ittiMsgHeader.ittiMsgSize);

    if (destination_task_id != TASK_UNKNOWN)
385
    {
386
#if defined(OAI_EMU) || defined(RTAI)
winckel's avatar
winckel committed
387 388 389
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_IN);
#endif

390 391
        if (itti_desc.threads[destination_thread_id].task_state == TASK_STATE_ENDED)
        {
392
            ITTI_DEBUG(" Message %s, number %lu with priority %d can not be sent from %s to queue (%u:%s), ended destination task!\n",
393 394 395 396 397 398 399 400 401 402 403 404
                       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 */
            DevCheck(itti_desc.threads[destination_thread_id].task_state == TASK_STATE_READY, destination_thread_id,
                     itti_desc.threads[destination_thread_id].task_state, message_id);
Cedric Roux's avatar
 
Cedric Roux committed
405

406
            /* Allocate new list element */
407
            new = (message_list_t *) itti_malloc (origin_task_id, destination_task_id, sizeof(struct message_list_s));
408
            DevAssert(new != NULL);
Cedric Roux's avatar
 
Cedric Roux committed
409

410 411 412 413
            /* Fill in members */
            new->msg = message;
            new->message_number = message_number;
            new->message_priority = priority;
Cedric Roux's avatar
 
Cedric Roux committed
414

415 416
            /* Enqueue message in destination task queue */
            lfds611_queue_enqueue(itti_desc.tasks[destination_task_id].message_queue, new);
417

418
#if defined(OAI_EMU) || defined(RTAI)
419
            vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_OUT);
winckel's avatar
winckel committed
420 421
#endif

422
#ifdef RTAI
423 424 425 426 427 428
            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
429 430
#endif
            {
431 432 433 434
                /* 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;
Cedric Roux's avatar
Cedric Roux committed
435
                    eventfd_t sem_counter = 1;
436

437 438 439 440
                    /* 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));
                    DevCheck(write_ret == sizeof(sem_counter), write_ret, sem_counter, destination_thread_id);
                }
441
            }
Cedric Roux's avatar
 
Cedric Roux committed
442

443
            ITTI_DEBUG(" Message %s, number %lu with priority %d successfully sent from %s to queue (%u:%s)\n",
444 445 446 447 448 449 450
                       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
451 452 453
    } else {
        /* This is a debug message to TASK_UNKNOWN, we can release safely release it */
        itti_free(origin_task_id, message);
454
    }
455

456
#if defined(OAI_EMU) || defined(RTAI)
457 458
    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)));
459 460
#endif

Cedric Roux's avatar
 
Cedric Roux committed
461 462 463
    return 0;
}

464 465
void itti_subscribe_event_fd(task_id_t task_id, int fd)
{
466
    thread_id_t thread_id;
467 468
    struct epoll_event event;

469
    DevCheck(task_id < itti_desc.task_max, task_id, itti_desc.task_max, 0);
470 471
    DevCheck(fd >= 0, fd, 0, 0);

472 473
    thread_id = TASK_GET_THREAD_ID(task_id);
    itti_desc.threads[thread_id].nb_events++;
474 475

    /* Reallocate the events */
476 477 478
    itti_desc.threads[thread_id].events = realloc(
        itti_desc.threads[thread_id].events,
        itti_desc.threads[thread_id].nb_events * sizeof(struct epoll_event));
479

480
    event.events  = EPOLLIN | EPOLLERR;
Cedric Roux's avatar
Cedric Roux committed
481 482
    event.data.u64 = 0;
    event.data.fd  = fd;
483 484

    /* Add the event fd to the list of monitored events */
485
    if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_ADD, fd,
486 487
        &event) != 0)
    {
488
        ITTI_ERROR(" epoll_ctl (EPOLL_CTL_ADD) failed for task %s, fd %d: %s\n",
489 490 491 492
                   itti_get_task_name(task_id), fd, strerror(errno));
        /* Always assert on this condition */
        DevAssert(0 == 1);
    }
493

494
    ITTI_DEBUG(" Successfully subscribed fd %d for task %s\n", fd, itti_get_task_name(task_id));
495 496 497 498
}

void itti_unsubscribe_event_fd(task_id_t task_id, int fd)
{
499 500
    thread_id_t thread_id;

501
    DevCheck(task_id < itti_desc.task_max, task_id, itti_desc.task_max, 0);
502 503
    DevCheck(fd >= 0, fd, 0, 0);

504
    thread_id = TASK_GET_THREAD_ID(task_id);
505
    /* Add the event fd to the list of monitored events */
506
    if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_DEL, fd, NULL) != 0)
507
    {
508
        ITTI_ERROR(" epoll_ctl (EPOLL_CTL_DEL) failed for task %s and fd %d: %s\n",
509 510 511 512 513
                   itti_get_task_name(task_id), fd, strerror(errno));
        /* Always assert on this condition */
        DevAssert(0 == 1);
    }

514 515 516 517
    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));
518 519 520 521
}

int itti_get_events(task_id_t task_id, struct epoll_event **events)
{
522 523
    thread_id_t thread_id;

524
    DevCheck(task_id < itti_desc.task_max, task_id, itti_desc.task_max, 0);
525

526 527
    thread_id = TASK_GET_THREAD_ID(task_id);
    *events = itti_desc.threads[thread_id].events;
528

529
    return itti_desc.threads[thread_id].epoll_nb_events;
530 531
}

532 533
static inline void itti_receive_msg_internal_event_fd(task_id_t task_id, uint8_t polling, MessageDef **received_msg)
{
534
    thread_id_t thread_id;
535 536
    int epoll_ret = 0;
    int epoll_timeout = 0;
537
    int i;
538

539
    DevCheck(task_id < itti_desc.task_max, task_id, itti_desc.task_max, 0);
540 541
    DevAssert(received_msg != NULL);

542
    thread_id = TASK_GET_THREAD_ID(task_id);
543 544 545 546 547 548 549 550
    *received_msg = NULL;

    if (polling) {
        /* In polling mode we set the timeout to 0 causing epoll_wait to return
         * immediately.
         */
        epoll_timeout = 0;
    } else {
551
        /* timeout = -1 causes the epoll_wait to wait indefinitely.
552 553 554 555
         */
        epoll_timeout = -1;
    }

556
    do {
557 558 559
        epoll_ret = epoll_wait(itti_desc.threads[thread_id].epoll_fd,
                               itti_desc.threads[thread_id].events,
                               itti_desc.threads[thread_id].nb_events,
560 561
                               epoll_timeout);
    } while (epoll_ret < 0 && errno == EINTR);
562 563

    if (epoll_ret < 0) {
564
        ITTI_ERROR(" epoll_wait failed for task %s: %s\n",
565
                   itti_get_task_name(task_id), strerror(errno));
566 567 568 569 570 571 572
        DevAssert(0 == 1);
    }
    if (epoll_ret == 0 && polling) {
        /* No data to read -> return */
        return;
    }

573
    itti_desc.threads[thread_id].epoll_nb_events = epoll_ret;
574

575
    for (i = 0; i < epoll_ret; i++) {
576
        /* Check if there is an event for ITTI for the event fd */
577 578
        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))
579
        {
580
            struct message_list_s *message = NULL;
Cedric Roux's avatar
Cedric Roux committed
581 582
            eventfd_t sem_counter;
            ssize_t   read_ret;
583 584

            /* Read will always return 1 */
585
            read_ret = read (itti_desc.threads[thread_id].task_event_fd, &sem_counter, sizeof(sem_counter));
586
            DevCheck(read_ret == sizeof(sem_counter), read_ret, sizeof(sem_counter), 0);
587

588 589
#if defined(KERNEL_VERSION_PRE_2_6_30)
            /* Store the value of the semaphore counter */
Cedric Roux's avatar
Cedric Roux committed
590
            itti_desc.threads[task_id].sem_counter = sem_counter - 1;
591 592
#endif

593
            if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
594
                /* No element in list -> this should not happen */
595
                DevParam(task_id, epoll_ret, 0);
596
            }
597
            DevAssert(message != NULL);
598
            *received_msg = message->msg;
599
            itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
600 601
            /* Mark that the event has been processed */
            itti_desc.threads[thread_id].events[i].events &= ~EPOLLIN;
602
            return;
603 604 605 606 607 608
        }
    }
}

void itti_receive_msg(task_id_t task_id, MessageDef **received_msg)
{
609
#if defined(OAI_EMU) || defined(RTAI)
610 611
    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)));
612
#endif
613 614 615

#if defined(KERNEL_VERSION_PRE_2_6_30)
    /* Store the value of the semaphore counter */
Cedric Roux's avatar
Cedric Roux committed
616 617 618
    if (itti_desc.threads[task_id].sem_counter > 0) {
        struct message_list_s *message = NULL;

619 620
        if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
            /* No element in list -> this should not happen */
Cedric Roux's avatar
Cedric Roux committed
621
            DevParam(task_id, itti_desc.threads[task_id].sem_counter, 0);
622 623 624
        }
        DevAssert(message != NULL);
        *received_msg = message->msg;
625
        itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
626

Cedric Roux's avatar
Cedric Roux committed
627
        itti_desc.threads[task_id].sem_counter--;
628 629
    } else
#endif
630
    itti_receive_msg_internal_event_fd(task_id, 0, received_msg);
Cedric Roux's avatar
 
Cedric Roux committed
631

winckel's avatar
winckel committed
632
#if defined(OAI_EMU) || defined(RTAI)
633 634
    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));
635
#endif
Cedric Roux's avatar
 
Cedric Roux committed
636 637
}

638 639
void itti_poll_msg(task_id_t task_id, MessageDef **received_msg) {
    DevCheck(task_id < itti_desc.task_max, task_id, itti_desc.task_max, 0);
640
    DevAssert(received_msg != NULL);
Cedric Roux's avatar
 
Cedric Roux committed
641 642 643

    *received_msg = NULL;

644
#if defined(OAI_EMU) || defined(RTAI)
645 646
    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));
647 648 649 650 651 652 653 654
#endif

    {
        struct message_list_s *message;

        if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 1)
        {
            *received_msg = message->msg;
655
            itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
656 657
        }
    }
Cedric Roux's avatar
 
Cedric Roux committed
658

659
    if ((itti_debug_poll) && (*received_msg == NULL)) {
660
        ITTI_DEBUG(" No message in queue[(%u:%s)]\n", task_id, itti_get_task_name(task_id));
Cedric Roux's avatar
 
Cedric Roux committed
661
    }
662 663

#if defined(OAI_EMU) || defined(RTAI)
664 665
    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)));
666
#endif
Cedric Roux's avatar
 
Cedric Roux committed
667 668
}

669
int itti_create_task(task_id_t task_id, void *(*start_routine)(void *), void *args_p) {
Cedric Roux's avatar
 
Cedric Roux committed
670
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
671
    int result;
Cedric Roux's avatar
 
Cedric Roux committed
672 673

    DevAssert(start_routine != NULL);
674
    DevCheck(thread_id < itti_desc.thread_max, thread_id, itti_desc.thread_max, 0);
675 676
    DevCheck(itti_desc.threads[thread_id].task_state == TASK_STATE_NOT_CONFIGURED, task_id, thread_id,
             itti_desc.threads[thread_id].task_state);
Cedric Roux's avatar
 
Cedric Roux committed
677

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

680
    ITTI_DEBUG(" Creating thread for task %s ...\n", itti_get_task_name(task_id));
681

682
    result = pthread_create (&itti_desc.threads[thread_id].task_thread, NULL, start_routine, args_p);
683
    DevCheck(result >= 0, task_id, thread_id, result);
Cedric Roux's avatar
 
Cedric Roux committed
684

685 686
    itti_desc.created_tasks ++;

Cedric Roux's avatar
 
Cedric Roux committed
687
    /* Wait till the thread is completely ready */
688
    while (itti_desc.threads[thread_id].task_state != TASK_STATE_READY)
689 690
        usleep (1000);

Cedric Roux's avatar
 
Cedric Roux committed
691 692 693
    return 0;
}

694 695 696 697 698 699 700 701 702 703 704
#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

705 706 707 708 709 710 711 712 713 714
void itti_wait_ready(int wait_tasks)
{
    itti_desc.wait_tasks = wait_tasks;

    ITTI_DEBUG(" 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);

    DevCheck(itti_desc.created_tasks == itti_desc.ready_tasks, itti_desc.created_tasks, itti_desc.ready_tasks, itti_desc.wait_tasks);
}

715 716
void itti_mark_task_ready(task_id_t task_id)
{
Cedric Roux's avatar
 
Cedric Roux committed
717 718
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);

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

721 722 723 724 725 726
    /* 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);

727 728 729 730 731 732 733 734 735
#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

736
    itti_desc.threads[thread_id].task_state = TASK_STATE_READY;
737 738 739 740 741 742 743 744
    itti_desc.ready_tasks ++;

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

    ITTI_DEBUG(" task %s started\n", itti_get_task_name(task_id));
745 746
}

747
void itti_exit_task(void) {
748 749 750 751 752 753 754 755 756
#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
757 758 759
    pthread_exit (NULL);
}

760
void itti_terminate_tasks(task_id_t task_id) {
Cedric Roux's avatar
Cedric Roux committed
761
    // Sends Terminate signals to all tasks.
762 763 764 765 766 767 768
    itti_send_terminate_message (task_id);

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

    pthread_exit (NULL);
Cedric Roux's avatar
 
Cedric Roux committed
769 770
}

771 772 773 774 775 776 777 778
#ifdef RTAI
static void *itti_rt_relay_thread(void *arg)
{
    thread_id_t thread_id;
    unsigned pending_messages;

    while (itti_desc.running)
    {
779 780 781 782 783
        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
784 785 786 787 788 789 790 791 792 793 794 795

        /* 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;
Cedric Roux's avatar
Cedric Roux committed
796
                    eventfd_t sem_counter = pending_messages;
797 798 799 800 801 802 803

                    /* 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);
                }
            }
        }
804 805 806 807

#if defined(OAI_EMU) || defined(RTAI)
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_OUT);
#endif
808 809 810 811 812
    }
    return NULL;
}
#endif

813
int itti_init(task_id_t task_max, thread_id_t thread_max, MessagesIds messages_id_max, const task_info_t *tasks_info,
814
              const message_info_t *messages_info, const char * const messages_definition_xml, const char * const dump_file_name) {
815 816
    task_id_t task_id;
    thread_id_t thread_id;
817 818
    int ret;

819
    itti_desc.message_number = 1;
Cedric Roux's avatar
 
Cedric Roux committed
820

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

823
    CHECK_INIT_RETURN(signal_mask());
824

Cedric Roux's avatar
 
Cedric Roux committed
825
    /* Saves threads and messages max values */
826
    itti_desc.task_max = task_max;
Cedric Roux's avatar
 
Cedric Roux committed
827 828
    itti_desc.thread_max = thread_max;
    itti_desc.messages_id_max = messages_id_max;
829
    itti_desc.thread_handling_signals = -1;
830
    itti_desc.tasks_info = tasks_info;
Cedric Roux's avatar
 
Cedric Roux committed
831 832 833
    itti_desc.messages_info = messages_info;

    /* Allocates memory for tasks info */
834 835 836 837
    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));
Cedric Roux's avatar
 
Cedric Roux committed
838 839

    /* Initializing each queue and related stuff */
840
    for (task_id = TASK_FIRST; task_id < itti_desc.task_max; task_id++)
841
    {
842
        ITTI_DEBUG(" Initializing %stask %s%s%s\n",
843 844 845 846 847 848
                   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) : "");

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

        ret = lfds611_queue_new(&itti_desc.tasks[task_id].message_queue, itti_desc.tasks_info[task_id].queue_size);
        if (ret < 0)
853
        {
854
            ITTI_ERROR(" lfds611_queue_new failed for task %u\n", task_id);
855 856
            DevAssert(0 == 1);
        }
857 858 859 860 861 862
    }

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

864 865
        itti_desc.threads[thread_id].epoll_fd = epoll_create1(0);
        if (itti_desc.threads[thread_id].epoll_fd == -1) {
866
            ITTI_ERROR(" Failed to create new epoll fd: %s\n", strerror(errno));
867 868 869 870
            /* Always assert on this condition */
            DevAssert(0 == 1);
        }

871 872 873 874 875 876
# 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
877
        itti_desc.threads[thread_id].task_event_fd = eventfd(0, EFD_SEMAPHORE);
878
# endif
879
        if (itti_desc.threads[thread_id].task_event_fd == -1)
880
        {
881
            ITTI_ERROR(" eventfd failed: %s\n", strerror(errno));
882 883 884 885
            /* Always assert on this condition */
            DevAssert(0 == 1);
        }

886
        itti_desc.threads[thread_id].nb_events = 1;
887

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

890 891
        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;
892 893

        /* Add the event fd to the list of monitored events */
894 895
        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)
896
        {
897
            ITTI_ERROR(" epoll_ctl (EPOLL_CTL_ADD) failed: %s\n", strerror(errno));
898 899 900
            /* Always assert on this condition */
            DevAssert(0 == 1);
        }
901