Orocos Real-Time Toolkit  2.9.0
fosi_internal.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Peter Soetens Sat May 21 20:15:50 CEST 2005 fosi_internal.hpp
3 
4  fosi_internal.hpp - description
5  -------------------
6  begin : Sat May 21 2005
7  copyright : (C) 2005 Peter Soetens
8  email : peter.soetens@mech.kuleuven.ac.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 #ifndef OS_FOSI_INTERNAL_HPP
40 #define OS_FOSI_INTERNAL_HPP
41 #define OROBLD_OS_LXRT_INTERNAL
42 
43 #include <iostream>
44 #include <sched.h>
45 #include "os/ThreadInterface.hpp"
46 #include "fosi.h"
47 #include "../fosi_internal_interface.hpp"
48 #include <sys/mman.h>
49 #include <unistd.h>
50 #include <sys/types.h>
51 #include "../../rtt-config.h"
52 #define INTERNAL_QUAL
53 
54 #include <string.h>
55 
56 #include "../../Logger.hpp"
57 
58 namespace RTT
59 {
60  namespace os {
61 
63  {
64  if ( geteuid() != 0 ) {
65  std::cerr << "You are not root. This program requires that you are root." << std::endl;
66  exit(1);
67  }
68 
69 #ifdef OROSEM_OS_LOCK_MEMORY
70  int locktype = MCL_CURRENT;
71 #ifdef OROSEM_OS_LOCK_MEMORY_FUTURE
72  locktype |= MCL_FUTURE;
73 #endif
74  // locking of all memory for this process
75  int rv = mlockall(locktype);
76  if ( rv != 0 ) {
77  perror( "rtos_task_create_main: Could not lock memory using mlockall" ); // Logger unavailable.
78  }
79 #endif
80 
81  /* check to see if rtai_lxrt module is loaded */
82  // struct module_info modInfo;
83  // size_t retSize;
84  // if ( query_module("rtai_lxrt", QM_INFO, &modInfo,
85  // sizeof(modInfo), &retSize) != 0 ) {
86  // std::cerr <<"It appears the rtai_lxrt module is not loaded !"<<std::endl;
87  // exit();
88  // }
89  unsigned long name = nam2num("main");
90  while ( rt_get_adr( name ) != 0 ) // check for existing 'MAINTHREAD'
91  ++name;
92 
93 
94  const char* tname = "main";
95  main_task->name = strcpy( (char*)malloc( (strlen(tname) + 1) * sizeof(char)), tname);
96 
97  if( !(main_task->rtaitask = rt_task_init(name, 10,0,0)) ) // priority, stack, msg_size
98  {
99  std::cerr << "Cannot rt_task_init() MainThread." << std::endl;
100  exit(1);
101  }
102 
103  struct sched_param param;
104 
105  param.sched_priority = sched_get_priority_max(SCHED_OTHER);
106  if (param.sched_priority != -1 )
107  sched_setscheduler( 0, SCHED_OTHER, &param);
108 
109  // Avoid the LXRT CHANGED MODE (TRAP), PID = 4088, VEC = 14, SIGNO = 11. warning
110  rt_task_use_fpu(main_task->rtaitask, 1);
111 
112 #ifdef OROSEM_OS_LXRT_PERIODIC
113  rt_set_periodic_mode();
114  start_rt_timer( nano2count( NANO_TIME(ORODAT_OS_LXRT_PERIODIC_TICK*1000*1000*1000) ) );
115  Logger::log() << Logger::Info << "RTAI Periodic Timer ticks at "<<ORODAT_OS_LXRT_PERIODIC_TICK<<" seconds." << Logger::endl;
116 #else
117  // BE SURE TO SET rt_preempt_always(1) when using one shot mode
118  rt_set_oneshot_mode();
119  // only call this function for RTAI 3.0 or older
120 #if defined(CONFIG_RTAI_VERSION_MINOR) && defined(CONFIG_RTAI_VERSION_MAJOR)
121 # if CONFIG_RTAI_VERSION_MAJOR == 3 && CONFIG_RTAI_VERSION_MINOR == 0
122  rt_preempt_always(1);
123 # endif
124 #else
125  rt_preempt_always(1);
126 #endif
127  start_rt_timer(0);
128  Logger::log() << Logger::Info << "RTAI Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
129 #endif
130  Logger::log() << Logger::Debug << "RTAI Task Created" << Logger::endl;
131  return 0;
132  }
133 
135  {
136  // we don't stop the timer
137  //stop_rt_timer();
138  rt_task_delete(main_task->rtaitask);
139  free(main_task->name);
140  main_task->name = NULL;
141  munlockall();
142  return 0;
143  }
144 
145  struct RTAI_Thread
146  {
147  void *(*wrapper)(void*);
148  void *data;
150  int priority;
151  unsigned int tnum;
152  };
153 
154  INTERNAL_QUAL void* rtai_thread_wrapper( void * arg ) {
155  RTAI_Thread* d = (RTAI_Thread*)arg;
156  RTOS_TASK* task = d->task;
157  void* data = d->data;
158  int priority = d->priority;
159  unsigned int tnum = d->tnum;
160  void*(*wrapper)(void*) = d->wrapper;
161  free( d );
162 
163  if (!(task->rtaitask = rt_task_init(tnum, priority, 0, 0))) {
164  std::cerr << "CANNOT INIT LXRT Thread " << task->name <<std::endl;
165  std::cerr << "Exiting this thread." <<std::endl;
166  exit(-1);
167  }
168 
169  // Schedule in Linux' SCHED_OTHER
170  struct sched_param param;
171  param.sched_priority = sched_get_priority_max(SCHED_OTHER);
172  if (param.sched_priority != -1 )
173  sched_setscheduler( 0, SCHED_OTHER, &param);
174 
175  // Avoid the LXRT CHANGED MODE (TRAP), PID = 4088, VEC = 14, SIGNO = 11. warning
176  rt_task_use_fpu(task->rtaitask, 1);
177 
178  // New default: new threads are always hard.
179  rt_make_hard_real_time();
180 
181  data = wrapper( data );
182 
183  // Exit in soft mode to avoid RTAI warnings.
184  rt_make_soft_real_time();
185  // cleanup here to avoid "LXRT Releases PID" warnings.
186  rt_task_delete(task->rtaitask);
187  task->rtaitask = 0;
188  // See rtos_task_delete for further cleanups.
189  return data;
190  }
191 
193  int priority,
194  unsigned cpu_affinity,
195  const char* name,
196  int sched_type,
197  size_t stack_size,
198  void * (*start_routine)(void *),
199  ThreadInterface* obj)
200  {
201  char taskName[7];
202  if ( strlen(name) == 0 )
203  name = "Thread";
204  strncpy(taskName, name, 7);
205  unsigned long task_num = nam2num( taskName );
206  if ( rt_get_adr(nam2num( taskName )) != 0 ) {
207  unsigned long nname = nam2num( taskName );
208  while ( rt_get_adr( nname ) != 0 ) // check for existing 'NAME'
209  ++nname;
210  num2nam( nname, taskName); // set taskName to new name
211  taskName[6] = 0;
212  task_num = nname;
213  }
214 
215  // Set and truncate name
216  task->name = strcpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name);
217  // name, priority, stack_size, msg_size, policy, cpus_allowed ( 1111 = 4 first cpus)
218 
219  // Set priority
220  task->priority = priority;
221 
222  // Set rtai task struct to zero
223  task->rtaitask = 0;
224 
225  RTAI_Thread* rt = (RTAI_Thread*)malloc( sizeof(RTAI_Thread) );
226  rt->priority = priority;
227  rt->data = obj;
228  rt->wrapper = start_routine;
229  rt->task = task;
230  rt->tnum = task_num;
231  int retv = pthread_create(&(task->thread), 0,
232  rtai_thread_wrapper, rt);
233  // poll for thread creation to be done.
234  int timeout = 0;
235  while ( task->rtaitask == 0 && ++timeout < 20)
236  usleep(100000);
237  return timeout < 20 ? retv : -1;
238  }
239 
240  INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* mytask) {
241  if (mytask->rtaitask == 0)
242  return;
243 
244  rt_task_yield();
245  }
246 
247  INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
248  RT_TASK* self = rt_buddy();
249  if (self == 0)
250  return -1; // non-rtai thread. We could try to compare pthreads like in gnulinux ?
251  if ( self == task->rtaitask )
252  return 1;
253  return 0;
254  }
255 
256  INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
257  {
258  return 0;
259  }
260 
261  INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
262  {
263  if (*scheduler != SCHED_LXRT_HARD && *scheduler != SCHED_LXRT_SOFT ) {
264  log(Error) << "Unknown scheduler type." <<endlog();
265  *scheduler = SCHED_LXRT_SOFT;
266  return -1;
267  }
268  return 0;
269  }
270 
272  if ( t->rtaitask == 0 || t->rtaitask != rt_buddy() ) {
273  return -1;
274  }
275  if (rtos_task_check_scheduler(&s) == -1)
276  return -1;
277  if (s == SCHED_LXRT_HARD)
278  rt_make_hard_real_time();
279  else if ( s == SCHED_LXRT_SOFT)
280  rt_make_soft_real_time();
281  return 0;
282  }
283 
285  if ( rt_is_hard_real_time( t->rtaitask ) )
286  return SCHED_LXRT_HARD;
287  return SCHED_LXRT_SOFT;
288  }
289 
291  {
292  if (mytask->rtaitask == 0)
293  return;
294  if (rt_buddy() == mytask->rtaitask) {
295  // do not suspend/resume my own thread
296  // do best effort to change period.
297  rtos_task_set_period(mytask,nanosecs);
298  return;
299  }
300  // other thread is instructing us:
301  if (nanosecs == 0) {
302  // in RTAI, to drop from periodic to non periodic, do a
303  // suspend/resume cycle.
304  rt_task_suspend( mytask->rtaitask );
305  rt_task_resume( mytask->rtaitask );
306  }
307  else {
308  // same for the inverse
309  rt_task_suspend( mytask->rtaitask );
310  rt_task_make_periodic_relative_ns(mytask->rtaitask, 0, nanosecs);
311  }
312  }
313 
314  INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
315  {
316  if (mytask->rtaitask == 0)
317  return;
318  if ( nanosecs == 0 )
319  rt_set_period(mytask->rtaitask, 0 );
320  else
321  rt_set_period(mytask->rtaitask, nano2count( nanosecs ));
322  }
323 
325  {
326  // Do nothing
327  }
328 
330  {
331  if (mytask->rtaitask == 0)
332  return -1;
333  // only in RTAI 3.2, this returns overrun or not.
334  // so do not use retval for compatibility reasons.
335  rt_task_wait_period();
336  return 0;
337  }
338 
340  if ( pthread_join((mytask->thread),0) != 0 )
341  Logger::log() << Logger::Critical << "Failed to join "<< mytask->name <<"."<< Logger::endl;
342 
343  free( mytask->name );
344  mytask->name = NULL;
345  // rt_task_delete is done in wrapper !
346  }
347 
348  INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
349  {
350  int ret = 0;
351  // check scheduler first.
352  ret = rtos_task_check_scheduler(scheduler);
353 
354  // correct priority
355  // Hard & Soft:
356  if (*priority < 0){
357  log(Warning) << "Forcing priority ("<<*priority<<") of thread to 0." <<endlog();
358  *priority = 0;
359  ret = -1;
360  }
361  if (*priority > 255){
362  log(Warning) << "Forcing priority ("<<*priority<<") of thread to 255." <<endlog();
363  *priority = 255;
364  ret = -1;
365  }
366  return ret;
367  }
368 
369  INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * mytask, int priority)
370  {
371  int rv;
372 
373  if (mytask->rtaitask == 0)
374  return -1;
375 
376  // returns the *old* priority.
377  rv = rt_change_prio( mytask->rtaitask, priority);
378  if (rv == mytask->priority) {
379  mytask->priority = priority;
380  return 0;
381  }
382  return -1;
383  }
384 
385  INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* t)
386  {
387  return t->name ? t->name : "(destroyed)";
388  }
389 
391  {
392  return t->priority;
393  }
394 
395  INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
396  {
397  return -1;
398  }
399 
401  {
402  return ~0;
403  }
404  }
405 }
406 #undef INTERNAL_QUAL
407 #endif
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.
long long NANO_TIME
Definition: fosi.h:55
int priority
Definition: fosi.h:80
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.
INTERNAL_QUAL void * rtai_thread_wrapper(void *arg)
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.
#define SCHED_LXRT_SOFT
Definition: fosi.h:125
int usleep(unsigned int us)
Definition: fosi.cpp:58
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.
cyg_thread thread
Definition: fosi.h:64
basic_ostreams & endl(basic_ostreams &s)
Flush and newline.
Definition: rtstreams.cpp:110
void *(* wrapper)(void *)
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...
#define SCHED_LXRT_HARD
Definition: fosi.h:124
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.
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...
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK *)
Yields the current thread.
static Logger & log()
As Instance(), but more userfriendly.
Definition: Logger.cpp:117
#define INTERNAL_QUAL
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:52
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
RTOS_RTAI_TASK * rtaitask
Definition: fosi.h:117
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.
int rtos_task_get_scheduler(const RTOS_TASK *t)
Returns the current scheduler set for task t.