intertask_interface.c 35.7 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
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;
Cedric Roux's avatar
   
Cedric Roux committed
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
84
85
    while(0)
# define ITTI_ERROR(x, args...) do { rt_printk("[ITTI][E]"x, ##args); } \
    while(0)
#else
86
# define ITTI_DEBUG(m, x, args...) do { if ((m) & itti_debug) fprintf(stdout, "[ITTI][D]"x, ##args); fflush (stdout); } \
Cedric Roux's avatar
   
Cedric Roux committed
87
    while(0)
88
# define ITTI_ERROR(x, args...) do { fprintf(stdout, "[ITTI][E]"x, ##args); fflush (stdout); } \
Cedric Roux's avatar
   
Cedric Roux committed
89
    while(0)
90
#endif
Cedric Roux's avatar
   
Cedric Roux committed
91
92
93
94

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

Cedric Roux's avatar
Cedric Roux committed
95
#ifndef EFD_SEMAPHORE
96
97
98
# define KERNEL_VERSION_PRE_2_6_30 1
#endif

99
100
101
102
103
#ifdef RTAI
# define ITTI_MEM_PAGE_SIZE (1024)
# define ITTI_MEM_SIZE      (16 * 1024 * 1024)
#endif

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

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

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

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

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

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

126
    /* The thread fd */
127
128
129
130
131
    int task_event_fd;

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

132
#if defined(KERNEL_VERSION_PRE_2_6_30)
Cedric Roux's avatar
Cedric Roux committed
133
    eventfd_t sem_counter;
134
135
#endif

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

    int epoll_nb_events;
144
145
146
147
148
149
150

#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;
151
#endif
152
153
154
155
156
} 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
157
158
} task_desc_t;

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

Cedric Roux's avatar
   
Cedric Roux committed
163
    /* Current message number. Incremented every call to send_msg_to_task */
164
    message_number_t message_number __attribute__((aligned(8)));
Cedric Roux's avatar
   
Cedric Roux committed
165
166

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

170
171
    pthread_t thread_handling_signals;

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

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
183
184
#ifdef RTAI
    pthread_t rt_relay_thread;
#endif
185
186

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

189
190
191
192
    uint64_t vcd_poll_msg;
    uint64_t vcd_receive_msg;
    uint64_t vcd_send_msg;
#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
200
{
    void *ptr = NULL;

201
202
#if defined(OAI_EMU) || defined(RTAI)
    ptr = memory_pools_allocate (itti_desc.memory_pools_handle, size, origin_task_id, destination_task_id);
203
#else
204
    ptr = malloc (size);
205
206
#endif

207
208
209
210
211
#if defined(OAI_EMU) || defined(RTAI)
    if (ptr == NULL)
    {
        char *statistics = memory_pools_statistics (itti_desc.memory_pools_handle);

212
        ITTI_ERROR (" Memory pools statistics:\n%s", statistics);
213
214
215
        free (statistics);
    }
#endif
winckel's avatar
winckel committed
216
    AssertFatal (ptr != NULL, "Memory allocation of %ld bytes failed (%d -> %d)\n", size, origin_task_id, destination_task_id);
217
218
219
220
221
222

    return ptr;
}

void itti_free(task_id_t task_id, void *ptr)
{
winckel's avatar
winckel committed
223
    AssertFatal (ptr != NULL, "Trying to free a NULL pointer (%d)\n", task_id);
224
225
226

#if defined(OAI_EMU) || defined(RTAI)
    memory_pools_free (itti_desc.memory_pools_handle, ptr, task_id);
227
#else
228
    free (ptr);
229
230
231
#endif
}

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

240
static inline uint32_t itti_get_message_priority(MessagesIds message_id) {
winckel's avatar
winckel committed
241
    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
242
243
244
245

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

246
const char *itti_get_message_name(MessagesIds message_id) {
winckel's avatar
winckel committed
247
    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
250
251

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

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

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

259
static task_id_t itti_get_current_task_id(void)
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
{
    task_id_t task_id;
    thread_id_t thread_id;
    pthread_t thread = pthread_self ();

    for (task_id = TASK_FIRST; task_id < itti_desc.task_max; task_id++)
    {
        thread_id = TASK_GET_THREAD_ID(task_id);
        if (itti_desc.threads[thread_id].task_thread == thread)
        {
            return task_id;
        }
    }

    return TASK_UNKNOWN;
}

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

283
int itti_send_broadcast_message(MessageDef *message_p) {
284
    task_id_t destination_task_id;
285
    task_id_t origin_task_id;
286
    thread_id_t origin_thread_id;
287
    uint32_t thread_id;
Cedric Roux's avatar
   
Cedric Roux committed
288
    int ret = 0;
289
    int result;
Cedric Roux's avatar
   
Cedric Roux committed
290

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

293
294
    origin_task_id = message_p->ittiMsgHeader.originTaskId;
    origin_thread_id = TASK_GET_THREAD_ID(origin_task_id);
Cedric Roux's avatar
   
Cedric Roux committed
295

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

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

                memcpy (new_message_p, message_p, sizeof(MessageDef));
311
                result = itti_send_msg_to_task (destination_task_id, INSTANCE_DEFAULT, new_message_p);
winckel's avatar
winckel committed
312
                AssertFatal (result >= 0, "Failed to send message %d to thread %d (task %d)\n", message_p->ittiMsgHeader.messageId, thread_id, destination_task_id);
313
            }
Cedric Roux's avatar
   
Cedric Roux committed
314
315
        }
    }
316
    itti_free (ITTI_MSG_ORIGIN_ID(message_p), message_p);
Cedric Roux's avatar
   
Cedric Roux committed
317
318
319
320

    return ret;
}

Cedric Roux's avatar
Cedric Roux committed
321
322
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
323
324
    MessageDef *temp = NULL;

winckel's avatar
winckel committed
325
    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
326

327
328
329
330
#if defined(OAI_EMU) || defined(RTAI)
    vcd_signal_dumper_dump_variable_by_name(VCD_SIGNAL_DUMPER_VARIABLE_ITTI_ALLOC_MSG, size);
#endif

331
332
333
334
335
336
    if (origin_task_id == TASK_UNKNOWN)
    {
        /* Try to identify real origin task ID */
        origin_task_id = itti_get_current_task_id();
    }

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

Cedric Roux's avatar
Cedric Roux committed
339
340
341
    temp->ittiMsgHeader.messageId = message_id;
    temp->ittiMsgHeader.originTaskId = origin_task_id;
    temp->ittiMsgHeader.ittiMsgSize = size;
Cedric Roux's avatar
   
Cedric Roux committed
342

343
344
345
346
#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
347
348
349
    return temp;
}

Cedric Roux's avatar
Cedric Roux committed
350
351
352
353
354
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);
}

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

winckel's avatar
winckel committed
364
#if defined(OAI_EMU) || defined(RTAI)
365
366
    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
367
368
#endif

winckel's avatar
winckel committed
369
370
    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
371

372
373
    destination_thread_id = TASK_GET_THREAD_ID(destination_task_id);
    message->ittiMsgHeader.destinationTaskId = destination_task_id;
Cedric Roux's avatar
Cedric Roux committed
374
375
376
377
    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;
winckel's avatar
winckel committed
378
    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
379

380
381
    origin_task_id = ITTI_MSG_ORIGIN_ID(message);

382
    priority = itti_get_message_priority (message_id);
Cedric Roux's avatar
   
Cedric Roux committed
383

384
385
    /* Increment the global message number */
    message_number = itti_increment_message_number ();
Cedric Roux's avatar
   
Cedric Roux committed
386

387
    itti_dump_queue_message (origin_task_id, message_number, message, itti_desc.messages_info[message_id].name,
388
389
390
                             sizeof(MessageHeader) + message->ittiMsgHeader.ittiMsgSize);

    if (destination_task_id != TASK_UNKNOWN)
391
    {
392
#if defined(OAI_EMU) || defined(RTAI)
winckel's avatar
winckel committed
393
394
395
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_IN);
#endif

396
397
        if (itti_desc.threads[destination_thread_id].task_state == TASK_STATE_ENDED)
        {
398
            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",
399
400
401
402
403
404
405
406
407
408
                       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 */
winckel's avatar
winckel committed
409
410
            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",
                         message_id, destination_thread_id, itti_desc.threads[destination_thread_id].task_state);
Cedric Roux's avatar
   
Cedric Roux committed
411

412
            /* Allocate new list element */
413
            new = (message_list_t *) itti_malloc (origin_task_id, destination_task_id, sizeof(struct message_list_s));
Cedric Roux's avatar
   
Cedric Roux committed
414

415
416
417
418
            /* Fill in members */
            new->msg = message;
            new->message_number = message_number;
            new->message_priority = priority;
Cedric Roux's avatar
   
Cedric Roux committed
419

420
421
            /* Enqueue message in destination task queue */
            lfds611_queue_enqueue(itti_desc.tasks[destination_task_id].message_queue, new);
422

423
#if defined(OAI_EMU) || defined(RTAI)
424
            vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_ENQUEUE_MESSAGE, VCD_FUNCTION_OUT);
winckel's avatar
winckel committed
425
426
#endif

427
#ifdef RTAI
428
429
430
431
432
433
            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
434
435
#endif
            {
436
437
438
439
                /* 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
440
                    eventfd_t sem_counter = 1;
441

442
443
                    /* 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
444
445
                    AssertFatal (write_ret == sizeof(sem_counter), "Write to task message FD (%d) failed (%ld/%ld)\n",
                                 destination_thread_id, write_ret, sizeof(sem_counter));
446
                }
447
            }
Cedric Roux's avatar
   
Cedric Roux committed
448

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

462
#if defined(OAI_EMU) || defined(RTAI)
463
464
    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)));
465
466
#endif

Cedric Roux's avatar
   
Cedric Roux committed
467
468
469
    return 0;
}

470
471
void itti_subscribe_event_fd(task_id_t task_id, int fd)
{
472
    thread_id_t thread_id;
473
474
    struct epoll_event event;

winckel's avatar
winckel committed
475
    AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)\n", task_id, itti_desc.task_max);
476

477
478
    thread_id = TASK_GET_THREAD_ID(task_id);
    itti_desc.threads[thread_id].nb_events++;
479
480

    /* Reallocate the events */
481
482
483
    itti_desc.threads[thread_id].events = realloc(
        itti_desc.threads[thread_id].events,
        itti_desc.threads[thread_id].nb_events * sizeof(struct epoll_event));
484

485
    event.events  = EPOLLIN | EPOLLERR;
Cedric Roux's avatar
Cedric Roux committed
486
487
    event.data.u64 = 0;
    event.data.fd  = fd;
488
489

    /* Add the event fd to the list of monitored events */
490
    if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_ADD, fd,
491
492
493
        &event) != 0)
    {
        /* Always assert on this condition */
winckel's avatar
winckel committed
494
495
        AssertFatal (0, "epoll_ctl (EPOLL_CTL_ADD) failed for task %s, fd %d: %s\n",
                     itti_get_task_name(task_id), fd, strerror(errno));
496
    }
497

498
    ITTI_DEBUG(ITTI_DEBUG_EVEN_FD, " Successfully subscribed fd %d for task %s\n", fd, itti_get_task_name(task_id));
499
500
501
502
}

void itti_unsubscribe_event_fd(task_id_t task_id, int fd)
{
503
504
    thread_id_t thread_id;

winckel's avatar
winckel committed
505
506
    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);
507

508
    thread_id = TASK_GET_THREAD_ID(task_id);
509
    /* Add the event fd to the list of monitored events */
510
    if (epoll_ctl(itti_desc.threads[thread_id].epoll_fd, EPOLL_CTL_DEL, fd, NULL) != 0)
511
512
    {
        /* Always assert on this condition */
winckel's avatar
winckel committed
513
514
        AssertFatal (0, "epoll_ctl (EPOLL_CTL_DEL) failed for task %s, fd %d: %s\n",
                     itti_get_task_name(task_id), fd, strerror(errno));
515
516
    }

517
518
519
520
    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));
521
522
523
524
}

int itti_get_events(task_id_t task_id, struct epoll_event **events)
{
525
526
    thread_id_t thread_id;

winckel's avatar
winckel committed
527
    AssertFatal (task_id < itti_desc.task_max, "Task id (%d) is out of range (%d)\n", task_id, itti_desc.task_max);
528

529
530
    thread_id = TASK_GET_THREAD_ID(task_id);
    *events = itti_desc.threads[thread_id].events;
531

532
    return itti_desc.threads[thread_id].epoll_nb_events;
533
534
}

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

winckel's avatar
winckel committed
542
543
    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");
544

545
    thread_id = TASK_GET_THREAD_ID(task_id);
546
547
548
549
550
551
552
553
    *received_msg = NULL;

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

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

    if (epoll_ret < 0) {
winckel's avatar
winckel committed
567
        AssertFatal (0, "epoll_wait failed for task %s: %s\n", itti_get_task_name(task_id), strerror(errno));
568
569
570
571
572
573
    }
    if (epoll_ret == 0 && polling) {
        /* No data to read -> return */
        return;
    }

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

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

            /* Read will always return 1 */
586
            read_ret = read (itti_desc.threads[thread_id].task_event_fd, &sem_counter, sizeof(sem_counter));
winckel's avatar
winckel committed
587
            AssertFatal (read_ret == sizeof(sem_counter), "Read from task message FD (%d) failed (%ld/%ld)\n", thread_id, read_ret, sizeof(sem_counter));
588

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

594
            if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
595
                /* No element in list -> this should not happen */
winckel's avatar
winckel committed
596
                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);
597
            }
winckel's avatar
winckel committed
598
            AssertFatal(message != NULL, "Message from message queue is NULL\n");
599
            *received_msg = message->msg;
600
            itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
601
602
            /* Mark that the event has been processed */
            itti_desc.threads[thread_id].events[i].events &= ~EPOLLIN;
603
            return;
604
605
606
607
608
609
        }
    }
}

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

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

620
621
        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
622
            DevParam(task_id, itti_desc.threads[task_id].sem_counter, 0);
623
624
625
        }
        DevAssert(message != NULL);
        *received_msg = message->msg;
626
        itti_free (ITTI_MSG_ORIGIN_ID(*received_msg), message);
627

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

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

639
void itti_poll_msg(task_id_t task_id, MessageDef **received_msg) {
winckel's avatar
winckel committed
640
    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
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
660
    if (*received_msg == NULL) {
        ITTI_DEBUG(ITTI_DEBUG_POLL, " 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

winckel's avatar
winckel committed
673
674
675
676
    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
677

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

680
    ITTI_DEBUG(ITTI_DEBUG_INIT, " 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);
winckel's avatar
winckel committed
683
    AssertFatal (result >= 0, "Thread creation for task %d, thread %d failed (%d)\n", 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
void itti_wait_ready(int wait_tasks)
{
    itti_desc.wait_tasks = wait_tasks;

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

winckel's avatar
winckel committed
712
713
    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);
714
715
}

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

winckel's avatar
winckel committed
720
    AssertFatal (thread_id < itti_desc.thread_max, "Thread id (%d) is out of range (%d)\n", thread_id, itti_desc.thread_max);
721

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

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

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

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

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

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

761
void itti_terminate_tasks(task_id_t task_id) {
Cedric Roux's avatar
Cedric Roux committed
762
    // Sends Terminate signals to all tasks.
763
764
765
766
767
768
769
    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
770
771
}

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

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

        /* 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
797
                    eventfd_t sem_counter = pending_messages;
798
799
800
801
802
803
804

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

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

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

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

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

824
    CHECK_INIT_RETURN(signal_mask());
825

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

    /* Allocates memory for tasks info */
835
836
837
838
    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
839
840

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

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

        ret = lfds611_queue_new(&itti_desc.tasks[task_id].message_queue, itti_desc.tasks_info[task_id].queue_size);
        if (ret < 0)
854
        {
winckel's avatar
winckel committed
855
            AssertFatal (0, "lfds611_queue_new failed for task %s\n", itti_get_task_name(task_id));
856
        }
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
            /* Always assert on this condition */
winckel's avatar
winckel committed
867
            AssertFatal (0, "Failed to create new epoll fd: %s\n", strerror(errno));
868
869
        }

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

884
        itti_desc.threads[thread_id].nb_events = 1;
885

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

888
889
        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;
890
891

        /* Add the event fd to the list of monitored events */
892
893
        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)
894
895
        {
            /* Always assert on this condition */
winckel's avatar
winckel committed
896
            AssertFatal (0, " epoll_ctl (EPOLL_CTL_ADD) failed: %s\n", strerror(errno));
897
        }
898

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

902
903
904
#ifdef RTAI
        itti_desc.threads[thread_id].real_time = FALSE;
        itti_desc.threads[thread_id].messages_pending = 0;
905
#endif
906
    }
907

908
    itti_desc.running = 1;
909
910
911
    itti_desc.wait_tasks = 0;
    itti_desc.created_tasks = 0;
    itti_desc.ready_tasks = 0;
912
913
914
#ifdef RTAI
    /* Start RT relay thread */
    DevAssert(pthread_create (&itti_desc.rt_relay_thread, NULL, itti_rt_relay_thread, NULL) >= 0);
915
916

    rt_global_heap_open();
917
#endif
918

919
920
921
922
923
#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);
    memory_pools_add_pool (itti_desc.memory_pools_handle, 1000,                                 1000);
winckel's avatar
winckel committed
924
    memory_pools_add_pool (itti_desc.memory_pools_handle,  500,                                 20000);
925
926
927
928

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

929
        ITTI_DEBUG(ITTI_DEBUG_MP_STATISTICS, " Memory pools statistics:\n%s", statistics);
930
931
932
933
        free (statistics);
    }
#endif

934
935
936
937
938
939
#if defined(OAI_EMU) || defined(RTAI)
    itti_desc.vcd_poll_msg = 0;
    itti_desc.vcd_receive_msg = 0;
    itti_desc.vcd_send_msg = 0;
#endif

940
    itti_dump_init (messages_definition_xml, dump_file_name);
Cedric Roux's avatar
   
Cedric Roux committed
941

942
    CHECK_INIT_RETURN(timer_init ());
Cedric Roux's avatar
   
Cedric Roux committed
943
944
945
946

    return 0;
}