po_hi_task.c 22.9 KB
Newer Older
1 2 3 4 5
/*
 * This is a part of PolyORB-HI-C distribution, a minimal
 * middleware written for generated code from AADL models.
 * You should use it with the Ocarina toolsuite.
 *
yoogx's avatar
yoogx committed
6
 * For more informations, please visit http://taste.tuxfamily.org/wiki
7
 *
8
 * Copyright (C) 2007-2009 Telecom ParisTech, 2010-2017 ESA & ISAE.
9 10
 */

11
#if defined (__linux__) || defined (RTEMS412)
12
/*  We need GNU extensions to support thread affinify.
13
    These are Linux or RTEMS specific
14 15 16
 */
#ifndef _GNU_SOURCE
#define _GNU_SOURCE 1
yoogx's avatar
yoogx committed
17
#endif /* _GNU_SOURCE */
18 19 20
#endif /* __linux__ || RTEMS412 */

#ifdef __linux__
21
#include <sched.h>
yoogx's avatar
yoogx committed
22
#endif /* __linux__ */
23

24 25 26 27 28
#if defined (SIMULATOR)
#include <um_threads.h>
#undef POSIX
#endif

29
#if defined (RTEMS_POSIX) || defined (__PO_HI_RTEMS_CLASSIC_API)
30
#ifdef RTEMS412
31 32
#include <sys/cpuset.h>
#endif
33
#endif
34

35
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
36
#if defined (__CYGWIN__) || defined (__MINGW32__) || defined (RTEMS_POSIX) || defined (__PO_HI_RTEMS_CLASSIC_API)
yoogx's avatar
yoogx committed
37
#else
38
#include <xlocale.h>
39
#include <unistd.h>
yoogx's avatar
yoogx committed
40 41
#endif

42 43
#include <pthread.h>
#include <sched.h>
44 45
#endif

jdelange's avatar
jdelange committed
46 47 48 49 50
#if defined (_WIN32)
#include <tchar.h>
#include <windows.h>
#endif

51 52 53 54 55 56 57 58
#include <errno.h>
/* Headers from the executive */

#include <po_hi_config.h>
#include <po_hi_time.h>
#include <po_hi_task.h>
#include <po_hi_debug.h>
#include <po_hi_returns.h>
59
#include <po_hi_types.h>
60
#include <po_hi_utils.h>
61 62
/* Header files in PolyORB-HI */

63
#if defined (MONITORING)
Antonia Francis's avatar
Antonia Francis committed
64
#include <trace_manager.h>
65 66 67
#endif
/* Headers from run-time verification */

68
#include <deployment.h>
69
/* Header files from generated code */
70

71 72 73
typedef enum __po_hi_task_category_t { TASK_PERIODIC, TASK_SPORADIC, TASK_BACKGROUND }
    __po_hi_task_category;

74 75 76 77 78 79

int nb_tasks; /* number of created tasks */

typedef struct
{
  __po_hi_task_id     id;       /* Identifier of the task in the system */
80
  __po_hi_task_category task_category;
81
  __po_hi_time_t      period;
yoogx's avatar
yoogx committed
82 83
  __po_hi_time_t      offset;

julien.delange's avatar
julien.delange committed
84
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
85
  __po_hi_time_t      timer;
yoogx's avatar
yoogx committed
86 87 88
#if defined (_WIN32)
  DWORD tid;
#else
89 90
  pthread_t           tid;              /* The pthread_t type used by the
                                           POSIX library */
yoogx's avatar
yoogx committed
91
#endif
92 93
  pthread_mutex_t     mutex;
  pthread_cond_t      cond;
jdelange's avatar
jdelange committed
94 95 96
#elif defined (_WIN32)
  __po_hi_time_t      timer;
  DWORD               tid;
97
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
98
  rtems_id            ratemon_period;
99
  rtems_id            rtems_id;
100 101
#elif defined(XENO_NATIVE)
  RT_TASK             xeno_id;
102 103
#elif defined(SIMULATOR)
  um_thread_id        um_id;
104
#endif
105
} __po_hi_task_t;
106 107 108 109 110 111 112
/*
 * Structure of a task, contains platform-dependent members
 */

__po_hi_task_t tasks[__PO_HI_NB_TASKS];
/* Array which contains all tasks informations */

jdelange's avatar
jdelange committed
113 114 115 116
#if defined (_WIN32)
HANDLE  __po_hi_tasks_array[__PO_HI_NB_TASKS];
#endif

117 118 119 120
__po_hi_task_id __po_hi_get_task_id (void) {

#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
  pthread_t pthread_id = pthread_self();
yoogx's avatar
yoogx committed
121
  int i;
122

yoogx's avatar
yoogx committed
123
  for (i = 0; i < __PO_HI_NB_TASKS; i++) {
124 125 126 127 128
    if (pthread_id == tasks[i].tid) {
      return tasks[i].id;
    }
  }
#endif
129

130 131 132
  return (__PO_HI_ERROR_UNKNOWN);

}
133

134 135
void __po_hi_wait_for_tasks ()
{
julien.delange's avatar
julien.delange committed
136
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
137 138
  int i;

139 140 141 142
  for (i = 0; i < __PO_HI_NB_TASKS; i++)
    {
      pthread_join( tasks[i].tid , NULL );
    }
143

144
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
145
  rtems_task_suspend(RTEMS_SELF);
146

jdelange's avatar
jdelange committed
147 148
#elif defined (_WIN32)
  WaitForMultipleObjects (__PO_HI_NB_TASKS, __po_hi_tasks_array, TRUE, INFINITE);
149

150 151 152 153
#elif defined (XENO_NATIVE)
  int ret;
  while (1)
  {
154 155 156
    ret = rt_task_join (&(tasks[0].xeno_id));
    if (ret != 0)
      {
157
        __PO_HI_DEBUG_CRITICAL ("Error while calling rt_task_suspend in __po_hi_wait_for_tasks (ret=%d)\n", ret);
158
      }
159
  }
160 161 162 163
#elif defined (SIMULATOR)
  start_scheduler();
  return (__PO_HI_SUCCESS);

164
#endif
jdelange's avatar
jdelange committed
165 166 167 168 169 170 171

#ifdef __PO_HI_DEBUG
   while (1)
   {
      __DEBUGMSG ("Should NEVER be called !!!\n");
   }
#endif
172 173 174 175 176
}

/*
 * compute next period for a task
 * The argument is the task-id
177
 * The task must be a cyclic task (periodic or sporadic)
178 179 180
 */
int __po_hi_compute_next_period (__po_hi_task_id task)
{
jdelange's avatar
jdelange committed
181
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX) || defined (_WIN32)
182

183
    /* If we call this function for the first time, we need to configure
yoogx's avatar
yoogx committed
184
       the initial timer to epoch */
185 186
  if (tasks[task].timer.sec == 0 && tasks[task].timer.nsec == 0) {
    tasks[task].timer = get_epoch();
yoogx's avatar
yoogx committed
187
    __po_hi_add_times(&(tasks[task].timer), &(tasks[task].timer), &tasks[task].offset);
188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208
    return (__PO_HI_SUCCESS);
  }

  /* Otherwise, we increment either using an absoulte timeline
     (periodic), or relative to the latest dispatch time (sporadic task) */

  switch (tasks[task].task_category) {
  case TASK_PERIODIC:
    __po_hi_add_times(&(tasks[task].timer), &(tasks[task].timer), &tasks[task].period );
    break;

  case TASK_SPORADIC: {
      __po_hi_time_t mytime;

      if (__po_hi_get_time (&mytime) != __PO_HI_SUCCESS) {
        return (__PO_HI_ERROR_CLOCK);
      }

      __po_hi_add_times(&(tasks[task].timer), &mytime, &tasks[task].period );
      break;
   }
209 210
  case TASK_BACKGROUND:
    break;
211
  }
212

213
  return (__PO_HI_SUCCESS);
214

215
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
216 217
   rtems_status_code ret;
   rtems_name name;
218

219 220
   if (tasks[task].ratemon_period == RTEMS_INVALID_ID)
   {
221
      name = rtems_build_name ('P', 'R', 'D' + (char)task, ' ');
222

223 224 225 226 227 228
      __DEBUGMSG ("Create monotonic server for task %d\n", task);
      ret = rtems_rate_monotonic_create (name, &(tasks[task].ratemon_period));
      if (ret != RTEMS_SUCCESSFUL)
      {
         __DEBUGMSG ("Error while creating the monotonic server, task=%d, status=%d\n", task, ret);
      }
229 230
   }
  return (__PO_HI_SUCCESS);
231 232 233 234 235 236 237 238
#elif defined (XENO_NATIVE)

  /*
   * In XENO_NATIVE target, we don't need to recompute the next period
   * since the API provides functionnalities to do it automatically.
   */

  return (__PO_HI_SUCCESS);
239 240 241
#else
   return (__PO_HI_UNAVAILABLE);
#endif
242 243
}

244

245 246
int __po_hi_wait_for_next_period (__po_hi_task_id task)
{
247 248 249 250 251

/*!
 * Entry ports monitoring at dispatch if MONITORING is defined
 */
#if defined (MONITORING)
Antonia Francis's avatar
Antonia Francis committed
252
  record_event(PERIODIC, WAIT_FOR, task, invalid_port_t, invalid_port_t, invalid_local_port_t, invalid_local_port_t, NULL);
253 254
#endif

yoogx's avatar
yoogx committed
255 256 257 258 259
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
  int ret;
   __PO_HI_INSTRUMENTATION_VCD_WRITE("0t%d\n", task);
  __po_hi_task_delay_until (&(tasks[task].timer), task);

260 261 262 263 264
  if ( (ret = __po_hi_compute_next_period (task)) != 1)
    {
      return (__PO_HI_ERROR_CLOCK);
    }

265
   __PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", task);
266

jdelange's avatar
jdelange committed
267
  return (__PO_HI_SUCCESS);
268

jdelange's avatar
jdelange committed
269 270 271 272 273 274 275 276
#elif defined (_WIN32)
  int ret;
  __po_hi_task_delay_until (&(tasks[task].timer), task);
  if ( (ret = __po_hi_compute_next_period (task)) != 1)
    {
      return (__PO_HI_ERROR_CLOCK);
    }

277
  return (__PO_HI_SUCCESS);
278

279
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
280
   rtems_status_code ret;
281
   ret = rtems_rate_monotonic_period (tasks[task].ratemon_period, (rtems_interval)__PO_HI_TIME_TO_US(tasks[task].period) / rtems_configuration_get_microseconds_per_tick());
jdelange's avatar
jdelange committed
282
   /*
283
   ret = rtems_rate_monotonic_period (tasks[task].ratemon_period, tasks[task].period / _TOD_Microseconds_per_tick);
jdelange's avatar
jdelange committed
284
   */
285

286 287 288 289 290 291
   switch (ret)
   {
      case RTEMS_SUCCESSFUL:
         return (__PO_HI_SUCCESS);
         break;
      case RTEMS_TIMEOUT:
292
         __DEBUGMSG ("Error in rtems_rate_monotonic_period (TIMEOUT, task = %d)\n", task);
293 294 295
         return (__PO_HI_ERROR_TASK_PERIOD);
         break;
      default:
296
         __DEBUGMSG ("Error in rtems_rate_monotonic_period (unknown, error code=%d, task=%d)\n", ret, task);
297
         return (__PO_HI_ERROR_UNKNOWN);
298 299
         break;
   }
300

301
   return (__PO_HI_UNAVAILABLE);
302

303
#elif defined (XENO_NATIVE)
304 305 306 307
   unsigned long overrun;
   int ret;
   ret = rt_task_wait_period (&overrun);
   if ( ret != 0)
308
   {
309
         __DEBUGMSG ("Error in rt_task_period (task=%d;ret=%d,overrun=%lu)\n", task, ret, overrun);
310 311 312
         return (__PO_HI_ERROR_TASK_PERIOD);
   }

313 314 315 316
   if (overrun != 0)
   {
      return (__PO_HI_ERROR_TASK_PERIOD);
   }
yoogx's avatar
yoogx committed
317

318
   return (__PO_HI_SUCCESS);
319 320 321
#else
  return (__PO_HI_UNAVAILABLE);
#endif
322 323 324 325 326 327 328 329
}

int __po_hi_initialize_tasking( )
{
  int i;

  for (i = 0; i < __PO_HI_NB_TASKS; i++)
  {
330
     __po_hi_seconds (&(tasks[i].period), 0);
331
     tasks[i].id     = invalid_task_id;
332
#ifdef __PO_HI_RTEMS_CLASSIC_API
333 334
      tasks[i].ratemon_period = RTEMS_INVALID_ID;
#endif
335 336 337 338 339 340 341
  }

  nb_tasks = 0;

  return (__PO_HI_SUCCESS);
}

342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361
/*
 * For multi-core aware systems, this function returns the number of
 * core available
 */

#if defined (POSIX) || defined (RTEMS_POSIX)
int __po_hi_number_of_cpus (void)
{
  int cores = 1;

#if defined (__linux__)  || defined (__APPLE__)

  cores = (int) sysconf (_SC_NPROCESSORS_ONLN);
#endif

  return cores;
}

#endif

362 363 364 365 366 367
/*
 * For each kind of system, we declare a generic function that
 * create a thread. For POSIX-compliant systems, the function
 * is called __po_hi_posix_create_thread
 */

368
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
369
pthread_t __po_hi_posix_create_thread (__po_hi_priority_t priority,
370
                                       __po_hi_stack_t    stack_size,
371 372
                                       const __po_hi_int8_t       core_id,
                                       void*              (*start_routine)(void),
jdelange's avatar
jdelange committed
373
                                       void*              arg)
374 375 376 377
{
  int                policy;
  pthread_t          tid;
  pthread_attr_t     attr;
378
  struct sched_param param;
379
  int err;
380

381 382
  /* Create attributes to store all configuration parameters */

383 384 385 386 387
  if (pthread_attr_init (&attr) != 0)
    {
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }

388 389 390 391 392
#if (defined (POSIX) && defined (__linux__))

  /* The following is disabled pending further investigation on affinity support in 4.11.99 */
  /*|| (defined (RTEMS_POSIX) && defined (RTEMS412))) */

393
#ifndef __COMPCERT__
394 395 396
  /* Thread affinity */
  cpu_set_t cpuset;

397 398 399 400 401 402
  if ( core_id > __po_hi_number_of_cpus() )
    {
      __PO_HI_DEBUG_CRITICAL("Invalid CPU core id\n");
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }

403 404 405 406 407 408 409 410
  CPU_ZERO(&cpuset);
  CPU_SET(core_id, &cpuset);

  if (pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset) != 0)
    {
      __DEBUGMSG("CANNOT SET AFFINTY\n");
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }
411 412 413
#else
#warning pthread_affinity managmeent disabled for Compcert
#endif
414 415 416 417 418 419 420 421

#else
  if ( core_id != 0 )
    {
      __PO_HI_DEBUG_CRITICAL("This platform does not support CPU affinity setting\n");
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }

422 423
#endif

julien.delange's avatar
julien.delange committed
424
#if defined (POSIX) || defined (XENO_POSIX)
425 426 427 428
  if (pthread_attr_setscope (&attr, PTHREAD_SCOPE_SYSTEM) != 0)
    {
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }
429 430 431 432 433 434 435
#elif defined (RTEMS_POSIX)
  if (pthread_attr_setscope (&attr, PTHREAD_SCOPE_PROCESS) != 0)
  {
    return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
  }
#endif

436 437 438 439 440
  if (stack_size != 0)
    {
      if (pthread_attr_setstacksize (&attr, stack_size) != 0)
	{
	  return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
441
        }
442 443
    }

444 445
    err = pthread_create (&tid, &attr, (void* (*)(void*))start_routine, arg);
    if (err != 0)
446
    {
447
      __PO_HI_DEBUG_CRITICAL ("Thread creation failed - pthread_create returned %d\n", err);
448 449 450
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_CREATE);
    }

451
  policy = SCHED_FIFO;
452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472
  param.sched_priority = priority;

#ifdef __PO_HI_DEBUG
  if (priority < sched_get_priority_min (policy))
  {
      __DEBUGMSG("PRIORITY IS TOO LOW\n");
  }

  if (priority > sched_get_priority_max (policy))
  {
      __DEBUGMSG("PRIORITY IS TOO HIGH\n");
  }
#endif

  /*
   * We print a message that the user has to be root on
   * its computer. In fact, most of the time, the
   * function pthread_setschedparam fails because
   * the user is not root. On many systems, only root
   * can change the priority of the threads.
   */
473
  __DEBUGMSG("ABOUT TO SET PRIORITY FOR TASK %d\n" , nb_tasks );
474 475 476 477 478 479 480 481 482 483
  if (pthread_setschedparam (tid, policy, &param)!=0)
    {
      __DEBUGMSG("CANNOT SET PRIORITY FOR TASK %d\n" , nb_tasks );
      __DEBUGMSG("IF YOU ARE USING POSIX IMPLEMENTATION\n");
      __DEBUGMSG("BE SURE TO BE LOGGED AS ROOT\n");
    }

  return tid;
}

484

485 486
int __po_hi_posix_initialize_task (__po_hi_task_t* task)
{
487 488 489 490
  if (pthread_mutex_init (&(task->mutex), NULL) != 0)
    {
      return (__PO_HI_ERROR_PTHREAD_MUTEX);
    }
491

492 493 494 495 496 497
  if (pthread_cond_init (&(task->cond), NULL) != 0)
    {
      return (__PO_HI_ERROR_PTHREAD_COND);
    }

  return (__PO_HI_SUCCESS);
498
}
499 500 501

#endif /* POSIX || RTEMS_POSIX */

jdelange's avatar
jdelange committed
502 503
#if defined (_WIN32)
DWORD __po_hi_win32_create_thread (__po_hi_task_id    id,
504
                                   __po_hi_priority_t priority,
jdelange's avatar
jdelange committed
505 506 507 508 509 510
                                   __po_hi_stack_t    stack_size,
                                   void*              (*start_routine)(void),
                                   void*              arg)
{
   DWORD tid;
   HANDLE h;
yoogx's avatar
yoogx committed
511
   h = CreateThread (NULL, 0, (LPTHREAD_START_ROUTINE) start_routine, NULL, 0, &tid);
jdelange's avatar
jdelange committed
512 513 514 515 516
   __po_hi_tasks_array[id] = h;
   return tid;
}
#endif

517
#ifdef __PO_HI_RTEMS_CLASSIC_API
518
rtems_id __po_hi_rtems_create_thread (__po_hi_priority_t priority,
519
                                      __po_hi_stack_t    stack_size,
520
                                      __po_hi_int8_t       core_id,
jdelange's avatar
jdelange committed
521 522
                                      void*              (*start_routine)(void),
                                      void*              arg)
523
{
524
  rtems_id rid;
525 526 527
  if (rtems_task_create (rtems_build_name( 'T', 'A', '0' + nb_tasks, ' '),
                         priority,
                         stack_size,
528 529 530 531
                         RTEMS_DEFAULT_MODES,
                         RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &rid)
      != RTEMS_SUCCESSFUL)
    {
532
      __DEBUGMSG ("ERROR when creating the task\n");
533
      return __PO_HI_ERROR_CREATE_TASK;
534 535
    }

536
#ifdef RTEMS412
537
  /* Thread affinity API for SMP systems appeared in RTEMS 4.11,
538 539
     section 25 of RTEMS Applications C User's Guide .
  */
540 541 542 543

  cpu_set_t         cpuset;

  CPU_ZERO(&cpuset);
544
  CPU_SET(core_id, &cpuset);
545

546
  if (rtems_task_set_affinity(rid, sizeof(cpuset), &cpuset) != RTEMS_SUCCESSFUL)
547 548 549 550 551
    {
      __DEBUGMSG ("ERROR setting thread affinity\n");
      return __PO_HI_ERROR_CREATE_TASK;
    }
#endif
552

553
  if (rtems_task_start (rid, (rtems_task_entry)start_routine, arg ) != RTEMS_SUCCESSFUL)
554
    {
555
      __DEBUGMSG ("ERROR when starting the task\n");
556
      return __PO_HI_ERROR_CREATE_TASK;
557
    }
558

559
   return rid;
560 561 562
}
#endif

563
#ifdef XENO_NATIVE
564
RT_TASK __po_hi_xenomai_create_thread (__po_hi_priority_t priority,
jdelange's avatar
jdelange committed
565 566 567
                                       __po_hi_stack_t    stack_size,
                                       void*              (*start_routine)(void),
                                       void*              arg)
568 569 570
{
   RT_TASK newtask;

571 572 573 574 575 576
   /*
    * Uses T_JOINABLE at this time, should avoid that later and put 0 instead
    * to be able to use kernel-based threads.
    */

   if (rt_task_create (&newtask, NULL, stack_size, priority, T_JOINABLE))
577 578 579 580 581 582 583
   {
      __DEBUGMSG ("ERROR when creating the task\n");
   }
   return newtask;
}
#endif

584 585 586
int __po_hi_create_generic_task (const __po_hi_task_id      id,
                                 const __po_hi_time_t*      period,
                                 const __po_hi_priority_t   priority,
587
                                 const __po_hi_stack_t      stack_size,
588
                                 const __po_hi_int8_t       core_id,
jdelange's avatar
jdelange committed
589 590
                                 void*                      (*start_routine)(void),
                                 void*                      arg)
591
{
592

593
  if (id == -1)
594
    {
595
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
596
      __po_hi_posix_create_thread (priority, stack_size, core_id, start_routine, arg);
597
      return (__PO_HI_SUCCESS);
jdelange's avatar
jdelange committed
598 599 600
#elif defined (_WIN32)
      __po_hi_win32_create_thread (id, priority, stack_size, start_routine, arg);
      return (__PO_HI_SUCCESS);
601
#elif defined (XENO_NATIVE)
602
      RT_TASK t;
jdelange's avatar
jdelange committed
603
      (void) arg;
jdelange's avatar
jdelange committed
604
      t = __po_hi_xenomai_create_thread (priority, stack_size, start_routine, arg);
605 606 607 608
      if (rt_task_start (&(t), (void*)start_routine, NULL))
      {
         __DEBUGMSG ("ERROR when starting the task\n");
      }
609
      return (__PO_HI_SUCCESS);
610
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
jdelange's avatar
jdelange committed
611
      (void) arg;
612
      __po_hi_rtems_create_thread (priority, stack_size, core_id, start_routine, arg);
613 614 615 616
      return (__PO_HI_SUCCESS);
#else
      return (__PO_HI_UNAVAILABLE);
#endif
617
    }
618 619
  else
    {
620
      __po_hi_task_t* my_task;
621
      my_task         = &(tasks[id]);
622
      __po_hi_time_copy (&(my_task->period), period);
623
      my_task->id     = id;
624

625
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
626 627
      my_task->tid    = __po_hi_posix_create_thread
        (priority, stack_size, core_id, start_routine, arg);
628
      my_task->timer = ORIGIN_OF_TIME;
629
      __po_hi_posix_initialize_task (my_task);
630
#elif defined (__PO_HI_RTEMS_CLASSIC_API)
631 632
      my_task->rtems_id = __po_hi_rtems_create_thread
        (priority, stack_size, core_id, start_routine, arg);
jdelange's avatar
jdelange committed
633
#elif defined (_WIN32)
634 635
      my_task->tid = __po_hi_win32_create_thread
        (id, priority, stack_size, start_routine, arg);
636
#elif defined (XENO_NATIVE)
637 638 639 640
      my_task->xeno_id = __po_hi_xenomai_create_thread
        (priority, stack_size, start_routine, arg);
#elif defined (SIMULATOR)
      my_task->um_id = um_thread_create (start_routine, stack_size, priority);
641 642 643
#else
      return (__PO_HI_UNAVAILABLE);
#endif
644 645 646 647 648 649
      nb_tasks++;
    }

  return (__PO_HI_SUCCESS);
}

650

651 652 653
int __po_hi_create_periodic_task (const __po_hi_task_id     id,
                                  const __po_hi_time_t*     period,
                                  const __po_hi_priority_t  priority,
654
                                  const __po_hi_stack_t     stack_size,
655
                                  const __po_hi_int8_t      core_id,
656
                                  void*                     (*start_routine)(void))
657
{
658

659
  if (__po_hi_create_generic_task( id, period , priority , stack_size, core_id, start_routine, NULL) != 1)
660 661
   {
      __DEBUGMSG ("ERROR when creating generic task (task id=%d)\n", id);
662
      return (__PO_HI_ERROR_CREATE_TASK);
663
   }
664 665

  /*
666
   * Compute the next period of the task, using the
667 668
   *__po_hi_time* functions.
   */
669
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
670 671 672 673 674 675

  // XXX The following is (a priori) not necessary, it should be
  // reviewed for all runtimes: the body of a periodic task already
  // call __po_hi_compute_next_period() as part of its skeleton

  /*if (__po_hi_compute_next_period (id) != __PO_HI_SUCCESS)
676 677 678
    {
      return (__PO_HI_ERROR_CLOCK);
    }
679
  */
680

681
#elif defined (XENO_NATIVE)
682 683
   int ret;

684
   ret = rt_task_set_periodic (&(tasks[id].xeno_id), TM_NOW,  (tasks[id].period.sec * 1000000000) + tasks[id].period.nsec);
685
   if (ret != 0)
686
   {
jdelange's avatar
jdelange committed
687
      __DEBUGMSG ("ERROR when calling rt_task_set_periodic on task %d, ret=%d\n", id, ret);
688 689
      return (__PO_HI_ERROR_CLOCK);
   }
690 691 692 693 694

   if (rt_task_start (&(tasks[id].xeno_id), (void*)start_routine, NULL))
   {
      __DEBUGMSG ("ERROR when starting the task\n");
   }
695
#endif
696
   tasks[id].task_category = TASK_PERIODIC;
Antonia Francis's avatar
Antonia Francis committed
697 698 699 700 701 702 703 704 705 706 707


  /*
   * Send Task type to trace manager, if there is monitoring.
   */

#if defined (MONITORING)
  //__DEBUGMSG ("Periodic_task_creation\n");
  record_event(PERIODIC, CREATION, id, invalid_port_t, invalid_port_t, invalid_local_port_t, invalid_local_port_t, NULL);
#endif

708
   return (__PO_HI_SUCCESS);
709 710
}

jdelange's avatar
jdelange committed
711 712
void __po_hi_task_wait_offset (const __po_hi_time_t* time)
{
713

yoogx's avatar
yoogx committed
714 715
  if (time->sec == 0 && time->nsec == 0)
    return; // No need to wait, exit immediately
716

yoogx's avatar
yoogx committed
717
  tasks[__po_hi_get_task_id()].offset = *time;
jdelange's avatar
jdelange committed
718 719
}

720
int __po_hi_create_sporadic_task (const __po_hi_task_id     id,
721 722
                                  const __po_hi_time_t*     period,
                                  const __po_hi_priority_t  priority,
723
                                  const __po_hi_stack_t     stack_size,
724
                                  const __po_hi_int8_t      core_id,
725
                                  void*                     (*start_routine)(void) )
726 727 728 729 730 731
{
  /*
   * Create generic task which will execute the routine given in the
   * last parameter. Typically, a sporadic thread will wait on a
   * mutex.
   */
732
  if (__po_hi_create_generic_task( id, period , priority , stack_size, core_id, start_routine, NULL) != 1)
733 734 735
    {
      return (__PO_HI_ERROR_CREATE_TASK);
    }
736

737 738 739
#if defined (XENO_NATIVE)
   int ret;

740
   ret = rt_task_set_periodic (&(tasks[id].xeno_id), TM_NOW,  tasks[id].period.sec * 1000000000 + tasks[id].period.nsec);
741 742
   if (ret != 0)
   {
jdelange's avatar
jdelange committed
743
      __DEBUGMSG ("ERROR when calling rt_task_set_periodic on task %d, ret=%d\n", id, ret);
744 745 746 747 748 749 750 751
      return (__PO_HI_ERROR_CLOCK);
   }

   if (rt_task_start (&(tasks[id].xeno_id), (void*)start_routine, NULL))
   {
      __DEBUGMSG ("ERROR when starting the task\n");
   }
#endif
752
   tasks[id].task_category = TASK_SPORADIC;
Antonia Francis's avatar
Antonia Francis committed
753 754 755 756 757 758 759 760 761 762

  /*
   * Send Task type to trace manager, if there is monitoring.
   */

#if defined (MONITORING)
//__DEBUGMSG ("Sporadic_task_creation\n");
  record_event(SPORADIC, CREATION, id, invalid_port_t, invalid_port_t, invalid_local_port_t, invalid_local_port_t, NULL);
#endif

763
   return (__PO_HI_SUCCESS);
764 765
}

766
int __po_hi_task_delay_until (__po_hi_time_t* time, __po_hi_task_id task)
767
{
768
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
769 770 771
  struct timespec timer;
  int ret;

772
  timer.tv_sec = time->sec;
773

774
  timer.tv_nsec = time->nsec;
775 776

  pthread_mutex_lock (&tasks[task].mutex);
777

778 779 780 781 782 783 784 785 786 787 788 789 790 791
  ret = pthread_cond_timedwait (&tasks[task].cond, &tasks[task].mutex, &timer);

  if ( (ret != 0) && (ret != ETIMEDOUT))
    {
      ret = __PO_HI_ERROR_PTHREAD_COND;
    }
  else
    {
      ret = __PO_HI_SUCCESS;
    }

  pthread_mutex_unlock (&tasks[task].mutex);

  return (ret);
jdelange's avatar
jdelange committed
792 793 794 795 796 797 798 799 800
#elif defined (_WIN32)
   HANDLE hTimer = NULL;
   LARGE_INTEGER ularge;

   hTimer = CreateWaitableTimer(NULL, TRUE, NULL);
   ularge = __po_hi_unix_seconds_to_windows_tick (time->sec, time->nsec);

    if (!SetWaitableTimer(hTimer, &ularge, 0, NULL, NULL, 0))
    {
801
        __PO_HI_DEBUG_CRITICAL("[DELAY UNTIL] SetWaitableTimer failed (%d)\n", GetLastError());
jdelange's avatar
jdelange committed
802 803 804 805 806
        return 2;
    }

    if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0)
    {
807
        __PO_HI_DEBUG_CRITICAL("[DELAY UNTIL] WaitForSingleObject failed (%d)\n", GetLastError());
jdelange's avatar
jdelange committed
808 809 810 811
    }

    if (CloseHandle(hTimer) != TRUE)
    {
812
        __PO_HI_DEBUG_CRITICAL("[DELAY UNTIL] CloseHandle failed (%d)\n", GetLastError());
jdelange's avatar
jdelange committed
813 814 815 816
    }


  return __PO_HI_SUCCESS;
817 818
#elif defined (XENO_NATIVE)
  int ret;
819
  ret =  rt_task_sleep_until (time->sec * 1000000000 + time->nsec);
820 821 822 823 824 825
  if (ret)
  {
      __DEBUGMSG ("[TASK] Error in rt_task_sleep_until, ret=%d\n", ret);
      return (__PO_HI_ERROR_PTHREAD_COND);
  }
  return (__PO_HI_SUCCESS);
826 827
#endif
  return (__PO_HI_UNAVAILABLE);
828
}
829 830 831 832 833 834 835

void __po_hi_tasks_killall ()
{
   int i;
   for (i = 0; i < __PO_HI_NB_TASKS; i++)
    {
       __DEBUGMSG ("Kill task %d\n", i);
836
#ifdef __PO_HI_RTEMS_CLASSIC_API
837
      rtems_task_delete (tasks[i].rtems_id);
838
#elif defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
839
      pthread_cancel (tasks[i].tid);
yoogx's avatar
yoogx committed
840
      __DEBUGMSG ("[TASKS] Cancel thread %p\n", tasks[i].tid);
841 842 843
#endif
    }
}