po_hi_task.c 14.6 KB
Newer Older
1
2
3
4
5
6
7
/*
 * 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.
 *
 * For more informations, please visit http://ocarina.enst.fr
 *
8
 * Copyright (C) 2007-2011, European Space Agency (ESA).
9
10
 */

11
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
12
13
#include <pthread.h>
#include <sched.h>
14
15
#endif

16
17
18
19
20
21
22
23
#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>
24
#include <po_hi_types.h>
25
#include <po_hi_utils.h>
26
27
28
/* Header files in PolyORB-HI */

#include <deployment.h>	
29

30
/* Header files from generated code */
31

32
33
34
35
36
37
38

int nb_tasks; /* number of created tasks */

typedef struct
{
  __po_hi_task_id     id;       /* Identifier of the task in the system */
  __po_hi_time_t      period;
julien.delange's avatar
julien.delange committed
39
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
40
  __po_hi_time_t      timer;
41
42
43
44
  pthread_t           tid;              /* The pthread_t type used by the
                                           POSIX library */
  pthread_mutex_t     mutex;
  pthread_cond_t      cond;
julien.delange's avatar
julien.delange committed
45
#elif defined (RTEMS_PURE)
46
  rtems_id            ratemon_period;
47
  rtems_id            rtems_id;
48
49
#elif defined(XENO_NATIVE)
  RT_TASK             xeno_id;
50
#endif
51
} __po_hi_task_t;
52
53
54
55
56
57
58
59
60
/*
 * Structure of a task, contains platform-dependent members
 */

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

void __po_hi_wait_for_tasks ()
{
julien.delange's avatar
julien.delange committed
61
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
62
63
  int i;

64
65
66
67
  for (i = 0; i < __PO_HI_NB_TASKS; i++)
    {
      pthread_join( tasks[i].tid , NULL );
    }
68
#elif defined (RTEMS_PURE)
69
  rtems_task_suspend(RTEMS_SELF);
70
71
72
73
74
75
76
#elif defined (XENO_NATIVE)
  int ret;
  while (1)
  {
  ret = rt_task_join (&(tasks[0].xeno_id));
  if (ret != 0)
  {
jdelange's avatar
jdelange committed
77
      __PO_HI_DEBUG_DEBUG ("Error while calling rt_task_suspend in __po_hi_wait_for_tasks (ret=%d)\n", ret);
78
79
  }
  }
80
#endif
jdelange's avatar
jdelange committed
81
82
83
84
85
86
87

#ifdef __PO_HI_DEBUG
   while (1)
   {
      __DEBUGMSG ("Should NEVER be called !!!\n");
   }
#endif
88
89
90
91
92
93
94
95
96
}

/*
 * compute next period for a task
 * The argument is the task-id
 * The task must be a periodic task
 */
int __po_hi_compute_next_period (__po_hi_task_id task)
{
97

julien.delange's avatar
julien.delange committed
98
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
99
100
101
102
103
104
  __po_hi_time_t mytime;

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

   __po_hi_add_times(&(tasks[task].timer), &mytime, &tasks[task].period );
107
108
  
  return (__PO_HI_SUCCESS);
109
#elif defined (RTEMS_PURE)
110
111
   rtems_status_code ret;
   rtems_name name;
112

113
114
   if (tasks[task].ratemon_period == RTEMS_INVALID_ID)
   {
115
      name = rtems_build_name ('P', 'R', 'D' + (char)task, ' ');
116

117
118
119
120
121
122
      __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);
      }
123
124
   }
  return (__PO_HI_SUCCESS);
125
126
127
128
129
130
131
132
#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);
133
134
135
#else
   return (__PO_HI_UNAVAILABLE);
#endif
136
137
138
139
}

int __po_hi_wait_for_next_period (__po_hi_task_id task)
{
140
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
141
  int ret;
142
143
   __PO_HI_INSTRUMENTATION_VCD_WRITE("0t%d\n", task); 
  __po_hi_task_delay_until (&(tasks[task].timer), task);
144
145
146
147
148
  if ( (ret = __po_hi_compute_next_period (task)) != 1)
    {
      return (__PO_HI_ERROR_CLOCK);
    }

149
150
   __PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", task); 

151
  return (__PO_HI_SUCCESS);
152
#elif defined (RTEMS_PURE)
153
   rtems_status_code ret;
154
   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
155
   /*
156
   ret = rtems_rate_monotonic_period (tasks[task].ratemon_period, tasks[task].period / _TOD_Microseconds_per_tick); 
jdelange's avatar
jdelange committed
157
   */
158

159
160
161
162
163
164
   switch (ret)
   {
      case RTEMS_SUCCESSFUL:
         return (__PO_HI_SUCCESS);
         break;
      case RTEMS_TIMEOUT:
165
         __DEBUGMSG ("Error in rtems_rate_monotonic_period (TIMEOUT, task = %d)\n", task);
166
167
168
         return (__PO_HI_ERROR_TASK_PERIOD);
         break;
      default:
169
         __DEBUGMSG ("Error in rtems_rate_monotonic_period (unknown, error code=%d, task=%d)\n", ret, task);
170
         return (__PO_HI_ERROR_UNKNOWN);
171
172
         break;
   }
173

174
   return (__PO_HI_UNAVAILABLE);
175
#elif defined (XENO_NATIVE)
176
177
178
179
   unsigned long overrun;
   int ret;
   ret = rt_task_wait_period (&overrun);
   if ( ret != 0)
180
   {
181
         __DEBUGMSG ("Error in rt_task_period (task=%d;ret=%d,overrun=%lu)\n", task, ret, overrun);
182
183
184
         return (__PO_HI_ERROR_TASK_PERIOD);
   }

185
186
187
188
   if (overrun != 0)
   {
      return (__PO_HI_ERROR_TASK_PERIOD);
   }
189
   return (__PO_HI_SUCCESS);
190
191
192
#else
  return (__PO_HI_UNAVAILABLE);
#endif
193
194
195
196
197
198
199
200
}

int __po_hi_initialize_tasking( )
{
  int i;

  for (i = 0; i < __PO_HI_NB_TASKS; i++)
  {
201
     __po_hi_seconds (&(tasks[i].period), 0);
202
     tasks[i].id     = invalid_task_id; 
203
204
205
#ifdef RTEMS_PURE
      tasks[i].ratemon_period = RTEMS_INVALID_ID;
#endif
206
207
208
209
210
211
212
213
214
215
216
217
218
  }

  nb_tasks = 0;

  return (__PO_HI_SUCCESS);
}

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

219
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
220
221
222
223
224
225
226
pthread_t __po_hi_posix_create_thread (__po_hi_priority_t priority, 
                                       __po_hi_stack_t    stack_size,
				       void*              (*start_routine)(void))
{
  int                policy;
  pthread_t          tid;
  pthread_attr_t     attr;
227
  struct sched_param param;
228
229
230
231
232
233

  if (pthread_attr_init (&attr) != 0)
    {
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }

julien.delange's avatar
julien.delange committed
234
#if defined (POSIX) || defined (XENO_POSIX)
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
  if (pthread_attr_setscope (&attr, PTHREAD_SCOPE_SYSTEM) != 0)
    {
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
    }
  if (stack_size != 0)
    {
      if (pthread_attr_setstacksize (&attr, stack_size) != 0)
	{
	  return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
      }
    }
#elif defined (RTEMS_POSIX)
  if (pthread_attr_setscope (&attr, PTHREAD_SCOPE_PROCESS) != 0)
  {
    return ((pthread_t)__PO_HI_ERROR_PTHREAD_ATTR);
  }
#endif

  if (pthread_create (&tid, &attr, (void* (*)(void*))start_routine, NULL) != 0)
    {
      return ((pthread_t)__PO_HI_ERROR_PTHREAD_CREATE);
    }

julien.delange's avatar
julien.delange committed
258
  policy = SCHED_RR;
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
  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.
   */

  if (pthread_setschedparam (tid, policy, &param)!=0)
    {
#ifdef __PO_HI_DEBUG
      __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");
#endif
    }

  return tid;
}

int __po_hi_posix_initialize_task (__po_hi_task_t* task)
{
        if (pthread_mutex_init (&(task->mutex), NULL) != 0)
        {
                return (__PO_HI_ERROR_PTHREAD_MUTEX);
        }

        if (pthread_cond_init (&(task->cond), NULL) != 0)
        {
                return (__PO_HI_ERROR_PTHREAD_COND);
        }
        return (__PO_HI_SUCCESS);
}
306
307
308
309
310

#endif /* POSIX || RTEMS_POSIX */


#ifdef RTEMS_PURE
311
312
313
rtems_id __po_hi_rtems_create_thread (__po_hi_priority_t priority, 
                                      __po_hi_stack_t    stack_size,
                                      void*              (*start_routine)(void))
314
{
315
316
  rtems_id rid;
   if (rtems_task_create (rtems_build_name( 'T', 'A', nb_tasks, ' ' ), 1, RTEMS_MINIMUM_STACK_SIZE, RTEMS_DEFAULT_MODES, RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &rid) != RTEMS_SUCCESSFUL)
317
318
   {
      __DEBUGMSG ("ERROR when creating the task\n");
319
      return __PO_HI_ERROR_CREATE_TASK;
320
321
   }

322
  if (rtems_task_start (rid, (rtems_task_entry)start_routine, 0 ) != RTEMS_SUCCESSFUL)
323
324
  {
      __DEBUGMSG ("ERROR when starting the task\n");
325
      return __PO_HI_ERROR_CREATE_TASK;
326
327
  }

328
   return rid;
329
330
331
}
#endif

332
333
334
335
336
337
338
#ifdef XENO_NATIVE
RT_TASK __po_hi_xenomai_create_thread (__po_hi_priority_t priority, 
                                     __po_hi_stack_t    stack_size,
                                     void*              (*start_routine)(void))
{
   RT_TASK newtask;

339
340
341
342
343
344
   /*
    * 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))
345
346
347
348
349
350
351
352
   {
      __DEBUGMSG ("ERROR when creating the task\n");
   }
   return newtask;
}
#endif


353
354
355
356
357
int __po_hi_create_generic_task (const __po_hi_task_id      id, 
                                 const __po_hi_time_t*      period, 
                                 const __po_hi_priority_t   priority, 
                                 const __po_hi_stack_t      stack_size,
                                 void*                (*start_routine)(void))
358
359
360
361
{
  __po_hi_task_t* my_task;
  if (id == -1) 
    {
362
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
363
      __po_hi_posix_create_thread (priority, stack_size, start_routine);
364
      return (__PO_HI_SUCCESS);
365
#elif defined (XENO_NATIVE)
366
367
368
369
370
371
      RT_TASK t;
      t = __po_hi_xenomai_create_thread (priority, stack_size, start_routine);
      if (rt_task_start (&(t), (void*)start_routine, NULL))
      {
         __DEBUGMSG ("ERROR when starting the task\n");
      }
372
      return (__PO_HI_SUCCESS);
373
374
375
376
377
378
#elif defined (RTEMS_PURE)
      __po_hi_rtems_create_thread (priority, stack_size, start_routine);
      return (__PO_HI_SUCCESS);
#else
      return (__PO_HI_UNAVAILABLE);
#endif
379
380
381
382
    } 
  else
    {
      my_task         = &(tasks[id]);
383
      __po_hi_time_copy (&(my_task->period), period);
384
      my_task->id     = id;
385
     
386
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
387
388
      my_task->tid    = __po_hi_posix_create_thread (priority, stack_size, start_routine);
      __po_hi_posix_initialize_task (my_task);
389
#elif defined (RTEMS_PURE)
390
      my_task->rtems_id = __po_hi_rtems_create_thread (priority, stack_size, start_routine);
391
392
#elif defined (XENO_NATIVE)
      my_task->xeno_id = __po_hi_xenomai_create_thread (priority, stack_size, start_routine);
393
394
395
#else
      return (__PO_HI_UNAVAILABLE);
#endif
396
397
398
399
400
401
      nb_tasks++;
    }

  return (__PO_HI_SUCCESS);
}

402
403
404
405
406
int __po_hi_create_periodic_task (const __po_hi_task_id     id, 
                                  const __po_hi_time_t*     period, 
                                  const __po_hi_priority_t  priority, 
                                  const __po_hi_stack_t     stack_size,
                                  void*                     (*start_routine)(void))
407
{
408
409
410
   if (__po_hi_create_generic_task( id, period , priority , stack_size, start_routine ) != 1)
   {
      __DEBUGMSG ("ERROR when creating generic task (task id=%d)\n", id);
411
      return (__PO_HI_ERROR_CREATE_TASK);
412
   }
413
414
415
416
417

  /*
   * Compute the next period of the task, using the 
   *__po_hi_time* functions.
   */
418
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
419
420
421
422
  if (__po_hi_compute_next_period (id) != __PO_HI_SUCCESS)
    {
      return (__PO_HI_ERROR_CLOCK);
    }
423
#elif defined (XENO_NATIVE)
424
425
426

   int ret;

427
   ret = rt_task_set_periodic (&(tasks[id].xeno_id), TM_NOW,  (tasks[id].period.sec * 1000000000) + tasks[id].period.nsec);
428
   if (ret != 0)
429
   {
430
      __DEBUGMSG ("ERROR when calling rt_task_set_periodic on task %d, ret=%d, period=%lu\n", id, ret, (unsigned long)tasks[id].period);
431
432
      return (__PO_HI_ERROR_CLOCK);
   }
433
434
435
436
437

   if (rt_task_start (&(tasks[id].xeno_id), (void*)start_routine, NULL))
   {
      __DEBUGMSG ("ERROR when starting the task\n");
   }
438
#endif
439
440
441
442
    
  return (__PO_HI_SUCCESS);
}

443
444
445
446
447
int __po_hi_create_sporadic_task (const __po_hi_task_id     id,
                                  const __po_hi_time_t*     period, 
                                  const __po_hi_priority_t  priority, 
                                  const __po_hi_stack_t     stack_size,
                                  void*                     (*start_routine)(void) )
448
449
450
451
452
453
454
455
456
457
458
{
  /*
   * Create generic task which will execute the routine given in the
   * last parameter. Typically, a sporadic thread will wait on a
   * mutex.
   */
  if (__po_hi_create_generic_task( id, period , priority , stack_size, start_routine ) != 1)
    {
      return (__PO_HI_ERROR_CREATE_TASK);
    }
  
459
460
461
#if defined (XENO_NATIVE)
   int ret;

462
   ret = rt_task_set_periodic (&(tasks[id].xeno_id), TM_NOW,  tasks[id].period.sec * 1000000000 + tasks[id].period.nsec);
463
464
465
466
467
468
469
470
471
472
473
474
   if (ret != 0)
   {
      __DEBUGMSG ("ERROR when calling rt_task_set_periodic on task %d, ret=%d, period=%lu\n", id, ret, (unsigned long)tasks[id].period);
      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
 
475
476
477
  return (__PO_HI_SUCCESS);
}

478
int __po_hi_task_delay_until (__po_hi_time_t* time, __po_hi_task_id task)
479
{
480
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
481
482
483
  struct timespec timer;
  int ret;

484
  timer.tv_sec = time->sec;
485
  
486
  timer.tv_nsec = time->nsec;
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503

  pthread_mutex_lock (&tasks[task].mutex);
  
  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);
504
505
#elif defined (XENO_NATIVE)
  int ret;
506
  ret =  rt_task_sleep_until (time->sec * 1000000000 + time->nsec);
507
508
509
510
511
512
  if (ret)
  {
      __DEBUGMSG ("[TASK] Error in rt_task_sleep_until, ret=%d\n", ret);
      return (__PO_HI_ERROR_PTHREAD_COND);
  }
  return (__PO_HI_SUCCESS);
513
514
#endif
  return (__PO_HI_UNAVAILABLE);
515
}
516
517
518
519
520
521
522
523
524
525

void __po_hi_tasks_killall ()
{
   int i;
   for (i = 0; i < __PO_HI_NB_TASKS; i++)
    {
       __DEBUGMSG ("Kill task %d\n", i);
#ifdef RTEMS_PURE
      rtems_task_delete (tasks[i].rtems_id);
#endif
526
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
527
528
529
530
531
      pthread_cancel (tasks[i].tid);
      __DEBUGMSG ("[TASKS] Cancel thread %d\n", (int) tasks[i].tid);
#endif
    }
}
532
533