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

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

Cedric Roux's avatar
Cedric Roux committed
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

Cedric Roux's avatar
 
Cedric Roux committed
102
typedef enum task_state_s {
103
    TASK_STATE_NOT_CONFIGURED, TASK_STATE_STARTING, TASK_STATE_READY, TASK_STATE_ENDED, TASK_STATE_MAX,
Cedric Roux's avatar
 
Cedric Roux committed
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
Cedric Roux's avatar
 
Cedric Roux committed
109

110
111
    message_number_t message_number; ///< Unique message number
    uint32_t message_priority; ///< Message priority
112
} message_list_t;
Cedric Roux's avatar
 
Cedric Roux committed
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)
Cedric Roux's avatar
Cedric Roux committed
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;
Cedric Roux's avatar
 
Cedric Roux committed
155
156
} task_desc_t;

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

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

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

168
169
    pthread_t thread_handling_signals;

170
    const task_info_t *tasks_info;
Cedric Roux's avatar
 
Cedric Roux committed
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;
Cedric Roux's avatar
 
Cedric Roux committed
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) {
Cedric Roux's avatar
 
Cedric Roux committed
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);
Cedric Roux's avatar
 
Cedric Roux committed
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);
Cedric Roux's avatar
 
Cedric Roux committed
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);
Cedric Roux's avatar
 
Cedric Roux committed
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;
Cedric Roux's avatar
 
Cedric Roux committed
289
    int ret = 0;
290
    int result;
Cedric Roux's avatar
 
Cedric Roux committed
291

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

294
295
    origin_task_id = message_p->ittiMsgHeader.originTaskId;
    origin_thread_id = TASK_GET_THREAD_ID(origin_task_id);
Cedric Roux's avatar
 
Cedric Roux committed
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
            }
Cedric Roux's avatar
 
Cedric Roux committed
316
317
        }
    }
318
    itti_free (ITTI_MSG_ORIGIN_ID(message_p), message_p);
Cedric Roux's avatar
 
Cedric Roux committed
319
320
321
322

    return ret;
}

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

327
    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
328

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

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

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

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

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

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

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

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

371
    AssertFatal (message != NULL, "Message is NULL!\n");
winckel's avatar
winckel committed
372
    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
373

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

382
383
    origin_task_id = ITTI_MSG_ORIGIN_ID(message);

384
    priority = itti_get_message_priority (message_id);
Cedric Roux's avatar
 
Cedric Roux committed
385

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

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

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

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

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

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

419
420
421
422
            /* Fill in members */
            new->msg = message;
            new->message_number = message_number;
            new->message_priority = priority;
Cedric Roux's avatar
 
Cedric Roux committed
423

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

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

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

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

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

466
#if defined(OAI_EMU) || defined(RTAI)
467
468
    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)));
469
470
#endif

Cedric Roux's avatar
 
Cedric Roux committed
471
472
473
    return 0;
}

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

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

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

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

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

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

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

void itti_unsubscribe_event_fd(task_id_t task_id, int fd)
{
507
508
    thread_id_t thread_id;

509
510
    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);
511

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

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

int itti_get_events(task_id_t task_id, struct epoll_event **events)
{
529
530
    thread_id_t thread_id;

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

533
534
    thread_id = TASK_GET_THREAD_ID(task_id);
    *events = itti_desc.threads[thread_id].events;
535

536
    return itti_desc.threads[thread_id].epoll_nb_events;
537
538
}

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

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

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

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

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

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

578
    itti_desc.threads[thread_id].epoll_nb_events = epoll_ret;
579

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

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

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

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

void itti_receive_msg(task_id_t task_id, MessageDef **received_msg)
{
614
#if defined(OAI_EMU) || defined(RTAI)
615
616
    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)));
617
#endif
618
619
620

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

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

Cedric Roux's avatar
Cedric Roux committed
632
        itti_desc.threads[task_id].sem_counter--;
633
634
    } else
#endif
635
    itti_receive_msg_internal_event_fd(task_id, 0, received_msg);
Cedric Roux's avatar
 
Cedric Roux committed
636

winckel's avatar
winckel committed
637
#if defined(OAI_EMU) || defined(RTAI)
638
639
    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));
640
#endif
Cedric Roux's avatar
 
Cedric Roux committed
641
642
}

643
void itti_poll_msg(task_id_t task_id, MessageDef **received_msg) {
644
    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
645
646
647

    *received_msg = NULL;

648
#if defined(OAI_EMU) || defined(RTAI)
649
650
    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));
651
652
653
654
655
656
657
658
#endif

    {
        struct message_list_s *message;

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

663
664
    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
665
    }
666
667

#if defined(OAI_EMU) || defined(RTAI)
668
669
    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)));
670
#endif
Cedric Roux's avatar
 
Cedric Roux committed
671
672
}

673
int itti_create_task(task_id_t task_id, void *(*start_routine)(void *), void *args_p) {
Cedric Roux's avatar
 
Cedric Roux committed
674
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);
675
    int result;
Cedric Roux's avatar
 
Cedric Roux committed
676

677
678
679
    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",
winckel's avatar
winckel committed
680
                 task_id, thread_id, itti_desc.threads[thread_id].task_state);
Cedric Roux's avatar
 
Cedric Roux committed
681

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

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

686
    result = pthread_create (&itti_desc.threads[thread_id].task_thread, NULL, start_routine, args_p);
687
    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
688

689
690
    itti_desc.created_tasks ++;

Cedric Roux's avatar
 
Cedric Roux committed
691
    /* Wait till the thread is completely ready */
692
    while (itti_desc.threads[thread_id].task_state != TASK_STATE_READY)
693
694
        usleep (1000);

Cedric Roux's avatar
 
Cedric Roux committed
695
696
697
    return 0;
}

698
699
700
701
702
703
704
705
706
707
708
#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

709
710
711
712
void itti_wait_ready(int wait_tasks)
{
    itti_desc.wait_tasks = wait_tasks;

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

716
    AssertFatal (itti_desc.created_tasks == itti_desc.ready_tasks, "Number of created tasks (%d) does not match ready tasks (%d), wait task %d!\n",
winckel's avatar
winckel committed
717
                 itti_desc.created_tasks, itti_desc.ready_tasks, itti_desc.wait_tasks);
718
719
}

720
721
void itti_mark_task_ready(task_id_t task_id)
{
Cedric Roux's avatar
 
Cedric Roux committed
722
723
    thread_id_t thread_id = TASK_GET_THREAD_ID(task_id);

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

726
727
728
729
730
731
    /* 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);

732
733
734
735
736
737
738
739
740
#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

741
    itti_desc.threads[thread_id].task_state = TASK_STATE_READY;
742
743
744
745
746
747
748
    itti_desc.ready_tasks ++;

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

749
    ITTI_DEBUG(ITTI_DEBUG_INIT, " task %s started\n", itti_get_task_name(task_id));
750
751
}

752
void itti_exit_task(void) {
753
754
755
756
757
758
759
760
761
#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
762
763
764
    pthread_exit (NULL);
}

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

776
777
778
779
780
781
782
783
#ifdef RTAI
static void *itti_rt_relay_thread(void *arg)
{
    thread_id_t thread_id;
    unsigned pending_messages;

    while (itti_desc.running)
    {
784
785
786
787
788
        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
789
790
791
792
793
794
795
796
797
798
799
800

        /* 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
801
                    eventfd_t sem_counter = pending_messages;
802
803
804
805
806
807
808

                    /* 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);
                }
            }
        }
809
810
811
812

#if defined(OAI_EMU) || defined(RTAI)
        vcd_signal_dumper_dump_function_by_name(VCD_SIGNAL_DUMPER_FUNCTIONS_ITTI_RELAY_THREAD, VCD_FUNCTION_OUT);
#endif
813
814
815
816
817
    }
    return NULL;
}
#endif

818
int itti_init(task_id_t task_max, thread_id_t thread_max, MessagesIds messages_id_max, const task_info_t *tasks_info,
819
              const message_info_t *messages_info, const char * const messages_definition_xml, const char * const dump_file_name) {
820
821
    task_id_t task_id;
    thread_id_t thread_id;
822
823
    int ret;

824
    itti_desc.message_number = 1;
Cedric Roux's avatar
 
Cedric Roux committed
825

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

828
    CHECK_INIT_RETURN(signal_mask());
829

Cedric Roux's avatar
 
Cedric Roux committed
830
    /* Saves threads and messages max values */
831
    itti_desc.task_max = task_max;
Cedric Roux's avatar
 
Cedric Roux committed
832
833
    itti_desc.thread_max = thread_max;
    itti_desc.messages_id_max = messages_id_max;
834
    itti_desc.thread_handling_signals = -1;
835
    itti_desc.tasks_info = tasks_info;
Cedric Roux's avatar
 
Cedric Roux committed
836
837
838
    itti_desc.messages_info = messages_info;

    /* Allocates memory for tasks info */
839
840
841
842
    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
843
844

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

854
        ITTI_DEBUG(ITTI_DEBUG_INIT, " Creating queue of message of size %u\n", itti_desc.tasks_info[task_id].queue_size);
855
856
857

        ret = lfds611_queue_new(&itti_desc.tasks[task_id].message_queue, itti_desc.tasks_info[task_id].queue_size);
        if (ret < 0)
858
        {
859
            AssertFatal (0, "lfds611_queue_new failed for task %s!\n", itti_get_task_name(task_id));
860
        }
861
862
863
864
865
866
    }

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

868
869
        itti_desc.threads[thread_id].epoll_fd = epoll_create1(0);
        if (itti_desc.threads[thread_id].epoll_fd == -1) {
870
            /* Always assert on this condition */
871
            AssertFatal (0, "Failed to create new epoll fd: %s!\n", strerror(errno));
872
873
        }

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

888
        itti_desc.threads[thread_id].nb_events = 1;
889

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

892
893
        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;
894
895

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

903
        ITTI_DEBUG(ITTI_DEBUG_EVEN_FD, " Successfully subscribed fd %d for thread %d\n",
904
                   itti_desc.threads[thread_id].task_event_fd, thread_id);
905

906
907
908
#ifdef RTAI
        itti_desc.threads[thread_id].real_time = FALSE;
        itti_desc.threads[thread_id].messages_pending = 0;
909
#endif
910
    }
911

912
    itti_desc.running = 1;