Orocos Real-Time Toolkit  2.8.3
fosi_internal.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Peter Soetens Wed Jan 18 14:11:39 CET 2006 fosi_internal.hpp
3 
4  fosi_internal.hpp - description
5  -------------------
6  begin : Wed January 18 2006
7  copyright : (C) 2006 Peter Soetens
8  email : peter.soetens@mech.kuleuven.be
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU General Public *
13  * License as published by the Free Software Foundation; *
14  * version 2 of the License. *
15  * *
16  * As a special exception, you may use this file as part of a free *
17  * software library without restriction. Specifically, if other files *
18  * instantiate templates or use macros or inline functions from this *
19  * file, or you compile this file and link it with other files to *
20  * produce an executable, this file does not by itself cause the *
21  * resulting executable to be covered by the GNU General Public *
22  * License. This exception does not however invalidate any other *
23  * reasons why the executable file might be covered by the GNU General *
24  * Public License. *
25  * *
26  * This library is distributed in the hope that it will be useful, *
27  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
28  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
29  * Lesser General Public License for more details. *
30  * *
31  * You should have received a copy of the GNU General Public *
32  * License along with this library; if not, write to the Free Software *
33  * Foundation, Inc., 59 Temple Place, *
34  * Suite 330, Boston, MA 02111-1307 USA *
35  * *
36  ***************************************************************************/
37 
38 
39 
40 #define OROBLD_OS_INTERNAL
41 #include "os/ThreadInterface.hpp"
42 #include "fosi.h"
43 #include "../fosi_internal_interface.hpp"
44 #include <cassert>
45 #define INTERNAL_QUAL
46 
47 #include "../../Logger.hpp"
48 #include <signal.h>
49 #include <execinfo.h>
50 
51 extern "C"
52 void warn_upon_switch(int sig __attribute__((unused)))
53 {
54  void *bt[32];
55  int nentries;
56 
57  /* Dump a backtrace to standard error of the frame which caused the switch to
58  secondary mode: */
59  nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
60  backtrace_symbols_fd(bt,nentries,fileno(stderr));
61 }
62 
63 
64 namespace RTT
65 {
66  namespace os {
67 
69  {
70  // first check if root (or if have sufficient privileges)
71  if ( geteuid() != 0 ) {
72 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2302
73  printf( "WARNING: You are not root. This program *may* require that you are root.\n");
74  // \todo verify have sufficient privileges
75 #else
76  printf( "You are not root. This program requires that you are root.\n");
77  exit(1);
78 #endif
79  }
80 
81  // locking of all memory for this process
82  int rv = mlockall(MCL_CURRENT | MCL_FUTURE);
83  if ( rv != 0 ) {
84  perror( "rtos_task_create_main: Could not lock memory using mlockall" ); // Logger unavailable.
85  exit(1);
86  }
87 
88  struct sched_param param;
89  // we set the MT to the highest sched priority to allow the console
90  // to interrupt a loose running thread.
91  param.sched_priority = sched_get_priority_max(ORO_SCHED_OTHER);
92  if (param.sched_priority != -1 )
93  sched_setscheduler( 0, ORO_SCHED_OTHER, &param);
94 
95  const char* mt_name = "MainThread";
96  main->sched_type = SCHED_XENOMAI_SOFT; // default for MainThread
97  main->name = strncpy( (char*)malloc( (strlen(mt_name)+1)*sizeof(char) ), mt_name, strlen(mt_name)+1 );
98 
99  int ret = -1;
100  while( ret != 0) {
101  // name, priority, mode
102  if ( (ret = rt_task_shadow( &(main->xenotask),mt_name, 0, 0)) != 0 ) {
103  if ( ret == -ENOMEM ) {
104  // fail: abort
105  printf( "Cannot rt_task_create() MainThread: Out of memory.\n");
106  exit(1);
107  }
108  if ( ret == -EBUSY ) {
109  // ok: we are a xeno thread (may log() ):
110  log(Info) << "MainThread already a Xenomai task." <<endlog();
111  break;
112  }
113  if ( ret == -EEXIST ) {
114  // fail: retry without using a name.
115  mt_name = 0; // do not register
116  continue;
117  }
118  if ( ret == -EPERM ) {
119  // fail: abort
120  printf( "Can not rt_task_create() MainThread: No permission.\n");
121  exit(1);
122  }
123  // uncaught error: abort
124  printf( "Can not rt_task_create() MainThread: Error %d.\n",ret);
125  exit(1);
126  }
127  }
128  // We are a xeno thread now:
129  // Only use Logger after this point (i.e. when rt_task_shadow was succesful).
130  if ( mt_name == 0) {
131  log(Warning) << "'MainThread' name was already in use. Registered empty name with Xenomai.\n" <<endlog();
132  }
133 
134  // main is created in main thread.
135  main->xenoptr = rt_task_self();
136 
137 #ifdef OROSEM_OS_XENO_PERIODIC
138 # if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
139  // time in nanoseconds
140  rt_timer_start( ORODAT_OS_XENO_PERIODIC_TICK*1000*1000*1000 );
141  Logger::In in("Scheduler");
142  Logger::log() << Logger::Info << "Xenomai Periodic Timer started using "<<ORODAT_OS_XENO_PERIODIC_TICK<<" seconds." << Logger::endl;
143 # else
144  Logger::In in("Scheduler");
145  Logger::log() << Logger::Error << "Set Xenomai Periodic Timer using the Linux kernel configuration." << Logger::endl;
146 # endif
147 #else
148 # if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
149  rt_timer_start( TM_ONESHOT );
150  Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
151 # else
152 # if CONFIG_XENO_OPT_TIMING_PERIODIC
153  Logger::log() << Logger::Info << "Xenomai Periodic Timer configured in 'periodic' mode." << Logger::endl;
154 # else
155  Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
156 # endif
157 # endif
158 #endif
159  log(Info) << "Installing SIGXCPU handler." <<endlog();
160  //signal(SIGXCPU, warn_upon_switch);
161  struct sigaction sa;
162  sa.sa_handler = warn_upon_switch;
163  sigemptyset( &sa.sa_mask );
164  sa.sa_flags = 0;
165  sigaction(SIGXCPU, &sa, 0);
166 
167  Logger::log() << Logger::Debug << "Xenomai Timer and Main Task Created" << Logger::endl;
168  return 0;
169  }
170 
172  {
173  //rt_task_delete( &(main_task->xenotask) );
174  free (main_task->name);
175  main_task->name = NULL;
176  munlockall();
177  return 0;
178  }
179 
180 
181  struct XenoCookie {
182  void* data;
183  void* (*wrapper)(void*);
184  };
185 
187  {
188  // store 'self'
189  RTOS_TASK* task = ((ThreadInterface*)((XenoCookie*)cookie)->data)->getTask();
190  task->xenoptr = rt_task_self();
191  assert( task->xenoptr );
192 
193  // call user function
194  ((XenoCookie*)cookie)->wrapper( ((XenoCookie*)cookie)->data );
195  free(cookie);
196  }
197 
199  int priority,
200  unsigned cpu_affinity,
201  const char* name,
202  int sched_type,
203  size_t stack_size,
204  void * (*start_routine)(void *),
205  ThreadInterface* obj)
206  {
207  rtos_task_check_priority(&sched_type, &priority);
208  XenoCookie* xcookie = (XenoCookie*)malloc( sizeof(XenoCookie) );
209  xcookie->data = obj;
210  xcookie->wrapper = start_routine;
211  if ( name == 0 || strlen(name) == 0)
212  name = "XenoThread";
213  task->name = strncpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name, strlen(name)+1 );
214  task->sched_type = sched_type; // User requested scheduler.
215  int rv;
216 
217  unsigned int aff = 0;
218  if ( cpu_affinity != 0 ) {
219  // calculate affinity:
220  for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) {
221  if(cpu_affinity & (1 << i)) {
222  // RTHAL_NR_CPUS is defined in the kernel, not in user space. So we just limit up to 7, until Xenomai allows us to get the maximum.
223  if ( i > 7 ) {
224  const unsigned int all_cpus = ~0;
225  if ( cpu_affinity != all_cpus ) // suppress this warning when ~0 is provided
226  log(Warning) << "rtos_task_create: ignoring cpu_affinity for "<< name << " on CPU " << i << " since it's larger than RTHAL_NR_CPUS - 1 (="<< 7 <<")"<<endlog();
227  } else {
228  aff |= T_CPU(i);
229  }
230  }
231  }
232  }
233 
234  if (stack_size == 0) {
235  log(Debug) << "Raizing default stack size to 128kb for Xenomai threads in Orocos." <<endlog();
236  stack_size = 128000;
237  }
238 
239  // task, name, stack, priority, mode, fun, arg
240  // UGLY, how can I check in Xenomai that a name is in use before calling rt_task_spawn ???
241  rv = rt_task_spawn(&(task->xenotask), name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
242  if ( rv == -EEXIST ) {
243  free( task->name );
244  task->name = strncpy( (char*)malloc( (strlen(name)+2)*sizeof(char) ), name, strlen(name)+1 );
245  task->name[ strlen(name) ] = '0';
246  task->name[ strlen(name)+1 ] = 0;
247  while ( rv == -EEXIST && task->name[ strlen(name) ] != '9') {
248  task->name[ strlen(name) ] += 1;
249  rv = rt_task_spawn(&(task->xenotask), task->name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
250  }
251  }
252  if ( rv == -EEXIST ) {
253  log(Warning) << name << ": an object with that name is already existing in Xenomai." << endlog();
254  rv = rt_task_spawn(&(task->xenotask), 0, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
255  }
256  if ( rv != 0) {
257  log(Error) << name << " : CANNOT INIT Xeno TASK " << task->name <<" error code: "<< rv << endlog();
258  return rv;
259  }
260 
261  rt_task_yield();
262  return 0;
263  }
264 
266  rt_task_yield();
267  }
268 
269  INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
270  RT_TASK* self = rt_task_self();
271  if (self == 0 || task == 0)
272  return -1; // non-xeno thread. We could try to compare pthreads like in gnulinux ?
273 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2500
274  if ( rt_task_same( self, task->xenoptr ) != 0 )
275  return 1;
276 #else
277  // older versions:
278  if ( self == task->xenoptr ) // xenoptr is also set by rt_task_self() during construction.
279  return 1;
280 #endif
281  return 0;
282  }
283 
284 
285  INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
286  {
287  if (*scheduler != SCHED_XENOMAI_HARD && *scheduler != SCHED_XENOMAI_SOFT ) {
288  log(Error) << "Unknown scheduler type." <<endlog();
289  *scheduler = SCHED_XENOMAI_SOFT;
290  return -1;
291  }
292  return 0;
293  }
294 
295  INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
296  {
297  int ret = 0;
298  // check scheduler first.
299  ret = rtos_task_check_scheduler(scheduler);
300 
301  // correct priority
302  // Hard & Soft:
303 #if ((CONFIG_XENO_VERSION_MAJOR*10000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 20500
304  const int minprio = 0;
305 #else
306  const int minprio = 1;
307 #endif
308  if (*priority < minprio){
309  log(Warning) << "Forcing priority ("<<*priority<<") of thread to " << minprio <<"." <<endlog();
310  *priority = minprio;
311  ret = -1;
312  }
313  if (*priority > 99){
314  log(Warning) << "Forcing priority ("<<*priority<<") of thread to 99." <<endlog();
315  *priority = 99;
316  ret = -1;
317  }
318  return ret;
319  }
320 
321 
322  // we could implement here the interrupt shield logic.
323  INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* t, int sched_type) {
324  // xenoptr was initialised from the thread wrapper.
325  if (t->xenoptr != rt_task_self() ) {
326  return -1;
327  }
328 
329  if ( rtos_task_check_scheduler( &sched_type ) == -1)
330  return -1;
331 
332 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2600
333  if (sched_type == SCHED_XENOMAI_HARD) {
334  if ( rt_task_set_mode( 0, T_PRIMARY, 0 ) == 0 ) {
336  return 0;
337  } else {
338  return -1;
339  }
340  } else {
341  if ( sched_type == SCHED_XENOMAI_SOFT) {
342  // This mode setting is only temporary. See rtos_task_wait_period() as well !
343  if (rt_task_set_mode( T_PRIMARY, 0, 0 ) == 0 ) {
345  return 0;
346  } else {
347  return -1;
348  }
349  }
350  }
351  assert(false);
352  return -1;
353 #else
354  t->sched_type = sched_type;
355  return 0;
356 #endif
357  }
358 
359  INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* mytask) {
360 #if 0
361  // WORK AROUND constness: (need non-const mytask)
362  RT_TASK* tt = mytask->xenoptr;
363  RT_TASK_INFO info;
364  if ( tt )
365  if ( rt_task_inquire( tt, &info) == 0 )
366  if ( info.status & XNRELAX )
367  return SCHED_XENOMAI_SOFT;
368  else
369  return SCHED_XENOMAI_HARD;
370  return -1;
371 #else
372  return mytask->sched_type;
373 #endif
374  }
375 
377  {
378  if (nanosecs == 0) {
379  rt_task_set_periodic( &(mytask->xenotask), TM_NOW, TM_INFINITE);
380  }
381  else {
382  rt_task_set_periodic( &(mytask->xenotask), TM_NOW, rt_timer_ns2ticks(nanosecs) );
383  }
384  }
385 
386  INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
387  {
388  rtos_task_make_periodic( mytask, nanosecs);
389  //rt_task_set_period(&(mytask->xenotask), rt_timer_ns2ticks( nanosecs ));
390  }
391 
393  {
394  // Do nothing
395  }
396 
398  {
399  // detect overrun.
400 #if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
401  if ( rt_task_wait_period() == -ETIMEDOUT)
402  return 1;
403 #else // 2.1, 2.2, 2.3, 2.4,...
404  long unsigned int overrun = 0;
405  rt_task_wait_period(&overrun);
406 
407 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2600
408  // When running soft, switch to secondary mode:
409  if ( mytask->sched_type == SCHED_XENOMAI_SOFT )
410  rt_task_set_mode(T_PRIMARY, 0, 0 );
411 #endif
412 
413  if ( overrun != 0)
414  return 1;
415 #endif
416  return 0;
417  }
418 
419  INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
420  {
421  log(Error) << "rtos_task_set_cpu_affinity: Xenomai tasks don't allow to migrate to another CPU once created." << endlog();
422  return -1;
423  }
424 
426  {
427  return ~0;
428  }
429 
430  INTERNAL_QUAL const char* rtos_task_get_name(const RTOS_TASK* mytask) {
431  return mytask->name ? mytask->name : "(destroyed)";
432  }
433 
434  INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
435  {
436  return 0;
437  }
438 
439  INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK* mytask) {
440  RT_TASK_INFO info;
441  // WORK AROUND constness: (need non-const mytask)
442  RT_TASK* tt = mytask->xenoptr;
443  if ( tt )
444  if ( rt_task_inquire ( tt, &info) == 0 )
445  return info.bprio;
446  return -1;
447  }
448 
450  if ( rt_task_join(&(mytask->xenotask)) != 0 ) {
451  log(Error) << "Failed to join with thread " << mytask->name << endlog();
452  }
453  rt_task_delete(&(mytask->xenotask));
454  free(mytask->name);
455  mytask->name = NULL;
456  }
457 
458  INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * mytask, int priority)
459  {
460  // ignorint 'type'
461  RT_TASK* tt = mytask->xenoptr;
462  if ( tt )
463  return rt_task_set_priority( tt, priority);
464  return -1;
465  }
466  }
467 }
468 #undef INTERNAL_QUAL
469 
int rtos_task_is_self(const RTOS_TASK *task)
Returns 1 when task is the task struct of the thread calling this function, 0 otherwise.
#define INTERNAL_QUAL
long long NANO_TIME
Definition: fosi.h:55
int rtos_task_get_priority(const RTOS_TASK *task)
Return the priority of a thread.
INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK *mytask, NANO_TIME nanosecs)
This function is to inform the RTOS that a thread is switching between periodic or non-periodic execu...
INTERNAL_QUAL int rtos_task_check_priority(int *scheduler, int *priority)
This function checks (and corrects) a given priority within a given scheduler type.
#define SCHED_XENOMAI_HARD
Definition: fosi.h:117
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
Returns the process ID the OS gave to the task task.
char * name
Definition: fosi.h:66
INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK *main_task)
Cleanup the main thread.
int rtos_task_set_scheduler(RTOS_TASK *t, int sched_type)
Set the scheduler of a given task t to a the type sched_type.
int sched_type
Definition: fosi.h:93
int rtos_task_set_cpu_affinity(RTOS_TASK *task, unsigned cpu_affinity)
Set the cpu affinity of a thread.
unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
Return the cpu affinity of a thread.
RTOS_XENO_TASK xenotask
Definition: fosi.h:108
RTOS_XENO_TASK * xenoptr
Definition: fosi.h:109
A thread which is being run.
void rtos_task_set_wait_period_policy(RTOS_TASK *task, int policy)
Set the wait policy of a thread.
void rtos_task_delete(RTOS_TASK *mytask)
This function must join the thread created with rtos_task_create and then clean up the RTOS_TASK stru...
int rtos_task_set_priority(RTOS_TASK *task, int priority)
Set the priority of a thread.
static std::ostream & endl(std::ostream &__os)
Definition: Logger.cpp:383
const char * rtos_task_get_name(const RTOS_TASK *task)
Returns the name by which a task is known in the RTOS.
int rtos_task_check_scheduler(int *sched_type)
This function checks (and corrects) if the given sched_type is valid for this RTOS.
void *(* wrapper)(void *)
int rtos_task_wait_period(RTOS_TASK *task)
This function is called by a periodic thread which wants to go to sleep and wake up the next period...
#define SCHED_XENOMAI_SOFT
Definition: fosi.h:118
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK *)
Yields the current thread.
INTERNAL_QUAL void rtos_xeno_thread_wrapper(void *cookie)
static Logger & log()
As Instance(), but more userfriendly.
Definition: Logger.cpp:117
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:51
INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK *main_task)
Initialise the main thread.
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
Change the period of a periodic RTOS task.
Definition: fosi.h:62
#define ORO_SCHED_OTHER
Definition: fosi.h:50
INTERNAL_QUAL int rtos_task_create(RTOS_TASK *task, int priority, unsigned cpu_affinity, const char *name, int sched_type, size_t stack_size, void *(*start_routine)(void *), ThreadInterface *obj)
Create a thread.
void warn_upon_switch(int sig __attribute__((unused)))
int rtos_task_get_scheduler(const RTOS_TASK *t)
Returns the current scheduler set for task t.