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:51 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 #include "../ThreadInterface.hpp"
39 #include "fosi.h"
40 #include "../fosi_internal_interface.hpp"
41 #include "../../Logger.hpp"
42 #include <cassert>
43 #include <sys/time.h>
44 #include <sys/resource.h>
45 #ifdef ORO_OS_LINUX_CAP_NG
46 #include <cap-ng.h>
47 #endif
48 #include <iostream>
49 #include <cstdlib>
50 #include <sys/types.h>
51 #include <unistd.h>
52 #include <sys/syscall.h>
53 
54 using namespace std;
55 
56 #define INTERNAL_QUAL
57 
58 namespace RTT
59 { namespace os {
60 
62  {
63  const char* name = "main";
64  main_task->wait_policy = ORO_WAIT_ABS;
65  main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
66  main_task->thread = pthread_self();
67  pthread_attr_init( &(main_task->attr) );
68  struct sched_param sp;
69  sp.sched_priority=0;
70  // Set priority
71  // fixme check return value and bail out if necessary
72  pthread_attr_setschedparam(&(main_task->attr), &sp);
73  main_task->priority = sp.sched_priority;
74  main_task->pid = getpid();
75  return 0;
76  }
77 
79  {
80  pthread_attr_destroy( &(main_task->attr) );
81  free(main_task->name);
82  main_task->name = NULL;
83  return 0;
84  }
85 
86  struct PosixCookie {
87  void* data;
88  void* (*wrapper)(void*);
89  };
90 
92  {
93  // store 'self'
94  RTOS_TASK* task = ((ThreadInterface*)((PosixCookie*)cookie)->data)->getTask();
95  task->pid = syscall(SYS_gettid);
96  assert( task->pid );
97 
98  // call user function
99  ((PosixCookie*)cookie)->wrapper( ((PosixCookie*)cookie)->data );
100  free(cookie);
101  return 0;
102  }
103 
104 
105 
107  int priority,
108  unsigned cpu_affinity,
109  const char * name,
110  int sched_type,
111  size_t stack_size,
112  void * (*start_routine)(void *),
113  ThreadInterface* obj)
114  {
115  int rv; // return value
116  task->wait_policy = ORO_WAIT_ABS;
117  rtos_task_check_priority( &sched_type, &priority );
118  // Save priority internally, since the pthread_attr* calls are broken !
119  // we will pick it up later in rtos_task_set_scheduler().
120  task->priority = priority;
121 
122  PosixCookie* xcookie = (PosixCookie*)malloc( sizeof(PosixCookie) );
123  xcookie->data = obj;
124  xcookie->wrapper = start_routine;
125 
126  // Set name
127  if ( strlen(name) == 0 )
128  name = "Thread";
129  task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
130 
131  if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){
132  return rv;
133  }
134  // Set scheduler type (_before_ assigning priorities!)
135  if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
136  return rv;
137  }
138  // Set stack size
139  if (stack_size )
140  if ( (rv = pthread_attr_setstacksize(&(task->attr), stack_size)) != 0){
141  return rv;
142  }
143  pthread_attr_getschedpolicy(&(task->attr), &rv );
144  assert( rv == sched_type );
145  /* SCHED_OTHER tasks are always assigned static priority 0, see
146  man sched_setscheduler
147  */
148  struct sched_param sp;
149  if (sched_type != SCHED_OTHER){
150  sp.sched_priority=priority;
151  // Set priority
152  if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){
153  return rv;
154  }
155  }
156  rv = pthread_create(&(task->thread), &(task->attr),
157  rtos_posix_thread_wrapper, xcookie);
158  if (rv != 0) {
159  log(Error) << "Failed to create thread " << task->name << ": "
160  << strerror(rv) << endlog();
161  return rv;
162  }
163 
164 #ifdef ORO_HAVE_PTHREAD_SETNAME_NP
165  // Set thread name to match task name, to help with debugging
166  {
167  // trim the name to fit 16 bytes restriction (including terminating
168  // \0 character) of pthread_setname_np
169  static const int MAX_THREAD_NAME_SIZE = 15;
170  char n[MAX_THREAD_NAME_SIZE + 1];
171  const char *thread_name = task->name;
172  const std::size_t thread_name_len = strlen(thread_name);
173  if (thread_name_len > MAX_THREAD_NAME_SIZE) {
174  // result = first 7 chars + "~" + last 7 chars
175  strncpy(&n[0], thread_name, 7);
176  n[7] = '~';
177  strncpy(&n[8], &thread_name[thread_name_len - 7], 7);
178  // terminate below
179  }
180  else
181  {
182  // result = thread_name
183  strncpy(&n[0], thread_name, MAX_THREAD_NAME_SIZE);
184  }
185  n[MAX_THREAD_NAME_SIZE] = '\0'; // explicitly terminate
186  int result = pthread_setname_np(task->thread, &n[0]);
187  if (result != 0) {
188  log(Warning) << "Failed to set thread name for " << task->name << ": "
189  << strerror(result) << endlog();
190  }
191  }
192 #endif // ORO_HAVE_PTHREAD_SETNAME_NP
193 
194  if ( cpu_affinity != 0 ) {
195  log(Debug) << "Setting CPU affinity to " << cpu_affinity << endlog();
196  int result = rtos_task_set_cpu_affinity(task, cpu_affinity);
197  if (result != 0) {
198  log(Error) << "Failed to set CPU affinity to " << cpu_affinity << " for " << task->name << ": "
199  << strerror(result) << endlog();
200  }
201  }
202 
203  return rv;
204  }
205 
207 #if 0
208  //under plain gnulinux, sched_yield may have little influence, so sleep
209  // to force rescheduling of other threads.
210  NANO_TIME timeRemaining = 1000; // 1ms
211  TIME_SPEC ts( ticks2timespec( timeRemaining ) );
212  rtos_nanosleep( &ts , NULL );
213 #else
214  int ret = sched_yield();
215  if ( ret != 0)
216  perror("rtos_task_yield");
217 #endif
218  }
219 
220  INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
221  {
222  if (task)
223  return task->pid;
224  return 0;
225  }
226 
228  pthread_t self = pthread_self();
229  if ( pthread_equal(self, task->thread) == 0 ) // zero means false.
230  return 0;
231  return 1;
232  }
233 
234  INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) {
235  int policy = -1;
236  struct sched_param param;
237  // first check the argument
238  if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
239  return -1;
240  // if sched_type is different, the priority must change as well.
241  if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
242  // now update the priority
243  param.sched_priority = task->priority;
244  rtos_task_check_priority( &sched_type, &param.sched_priority );
245  // write new policy:
246  return pthread_setschedparam( task->thread, sched_type, &param);
247  }
248  return -1;
249  }
250 
252  int policy = -1;
253  struct sched_param param;
254  // first retrieve thread scheduling parameters:
255  if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0)
256  return policy;
257  return -1;
258  }
259 
261  {
262  // set period
263  mytask->period = nanosecs;
264  // set next wake-up time.
265  mytask->periodMark = ticks2timespec( nano2ticks( rtos_get_time_ns() + nanosecs ) );
266  }
267 
269  {
270  rtos_task_make_periodic(mytask, nanosecs);
271  }
272 
274  {
275  task->wait_policy = policy;
276  }
277 
279  {
280  if ( task->period == 0 )
281  return 0;
282 
283  // record this to detect overrun.
284  NANO_TIME now = rtos_get_time_ns();
285  NANO_TIME wake= task->periodMark.tv_sec * 1000000000LL + task->periodMark.tv_nsec;
286 
287  // inspired by nanosleep man page for this construct:
288  while ( clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &(task->periodMark), NULL) != 0 && errno == EINTR ) {
289  errno = 0;
290  }
291 
292  if (task->wait_policy == ORO_WAIT_ABS)
293  {
294  // program next period:
295  // 1. convert period to timespec
296  TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
297  // 2. Add ts to periodMark (danger: tn guards for overflows!)
298  NANO_TIME tn = (task->periodMark.tv_nsec + ts.tv_nsec);
299  task->periodMark.tv_nsec = tn % 1000000000LL;
300  task->periodMark.tv_sec += ts.tv_sec + tn / 1000000000LL;
301  }
302  else
303  {
304  TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
306  NANO_TIME tn = (now.tv_nsec + ts.tv_nsec);
307  task->periodMark.tv_nsec = tn % 1000000000LL;
308  task->periodMark.tv_sec = ts.tv_sec + now.tv_sec + tn / 1000000000LL;
309  }
310 
311  return now > wake ? -1 : 0;
312  }
313 
315  pthread_join( mytask->thread, 0);
316  pthread_attr_destroy( &(mytask->attr) );
317  free(mytask->name);
318  mytask->name = NULL;
319  }
320 
322  {
323 #ifdef ORO_OS_LINUX_CAP_NG
324  if(capng_get_caps_process()) {
325  log(Error) << "Failed to retrieve capabilities (lowering to SCHED_OTHER)." <<endlog();
326  *scheduler = SCHED_OTHER;
327  return -1;
328  }
329 #endif
330 
331  if (*scheduler != SCHED_OTHER && geteuid() != 0
332 #ifdef ORO_OS_LINUX_CAP_NG
333  && capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)==0
334 #endif
335  ) {
336  // they're not root and they want a real-time priority, which _might_
337  // be acceptable if they're using pam_limits and have set the rtprio ulimit
338  // (see "/etc/security/limits.conf" and "ulimit -a")
339  struct rlimit r;
340  if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
341  {
342  log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog();
343  *scheduler = SCHED_OTHER;
344  return -1;
345  }
346  }
347 
348  if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
349  log(Error) << "Unknown scheduler type." <<endlog();
350  *scheduler = SCHED_OTHER;
351  return -1;
352  }
353  return 0;
354  }
355 
356  INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
357  {
358  int ret = 0;
359  // check scheduler first.
360  ret = rtos_task_check_scheduler(scheduler);
361 
362  // correct priority
363  if (*scheduler == SCHED_OTHER) {
364  if ( *priority != 0 ) {
365  if (*priority != LowestPriority)
366  log(Warning) << "Forcing priority ("<<*priority<<") of thread with SCHED_OTHER policy to 0." <<endlog();
367  *priority = 0;
368  ret = -1;
369  }
370  } else {
371  // SCHED_FIFO/SCHED_RR:
372  if (*priority <= 0){
373  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 1." <<endlog();
374  *priority = 1;
375  ret = -1;
376  }
377  if (*priority > 99){
378  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 99." <<endlog();
379  *priority = 99;
380  ret = -1;
381  }
382  // and limit them according to pam_Limits (only if not root)
383  if ( geteuid() != 0
384 #ifdef ORO_OS_LINUX_CAP_NG
385  && !capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)
386 #endif
387  )
388  {
389  struct rlimit r;
390  if (0 == getrlimit(RLIMIT_RTPRIO, &r))
391  {
392  if (*priority > (int)r.rlim_cur)
393  {
394  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to the pam_limit of " << r.rlim_cur <<endlog();
395  *priority = r.rlim_cur;
396  ret = -1;
397  }
398  }
399  else
400  {
401  // this should not be possible, but do something intelligent
402  *priority = 1;
403  ret = -1;
404  }
405  }
406  }
407  return ret;
408  }
409 
411  {
412  int policy = 0;
413  struct sched_param param;
414  // first retrieve thread scheduling parameters:
415  if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0) {
416  if ( rtos_task_check_priority( &policy, &priority ) != 0 )
417  return -1;
418  param.sched_priority = priority;
419  task->priority = priority; // store for set_scheduler
420  // write new policy:
421  return pthread_setschedparam( task->thread, policy, &param);
422  }
423  return -1;
424  }
425 
427  {
428  // if sched_other, return the 'virtual' priority
429  int policy = 0;
430  struct sched_param param;
431  // first retrieve thread scheduling parameters:
432  if ( task == 0 )
433  return -1;
434  if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, &param) != 0)
435  return task->priority;
436  return param.sched_priority;
437  }
438 
439  INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
440  {
441  if ( cpu_affinity == 0 ) // clears the mask.
442  cpu_affinity = ~0;
443  if( task && task->thread != 0 ) {
444  cpu_set_t cs;
445  CPU_ZERO(&cs);
446  for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
447  {
448  if(cpu_affinity & (1 << i)) { CPU_SET(i, &cs); }
449  }
450  return pthread_setaffinity_np(task->thread, sizeof(cs), &cs);
451  }
452  return -1;
453  }
454 
456  {
457  if( task && task->thread != 0) {
458  unsigned cpu_affinity = 0;
459  cpu_set_t cs;
460  pthread_getaffinity_np(task->thread, sizeof(cs), &cs);
461  for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
462  {
463  if(CPU_ISSET(i, &cs)) { cpu_affinity |= (1 << i); }
464  }
465  return cpu_affinity;
466  }
467  return ~0;
468  }
469 
470  INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
471  {
472  return task->name ? task->name : "(destroyed)";
473  }
474 
475  }
476 }
477 #undef INTERNAL_QUAL
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.
int wait_policy
Definition: fosi.h:81
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.
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
Returns the process ID the OS gave to the task task.
#define INTERNAL_QUAL
#define ORO_WAIT_ABS
Definition: fosi.h:52
pid_t pid
Definition: fosi.h:82
NANO_TIME rtos_get_time_ns(void)
Get "system" time in nanoseconds.
Definition: fosi.h:113
int rtos_nanosleep(const TIME_SPEC *rqtp, TIME_SPEC *rmtp)
Definition: fosi.h:180
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.
STL namespace.
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
INTERNAL_QUAL void * rtos_posix_thread_wrapper(void *cookie)
TIME_SPEC ticks2timespec(TICK_TIME hrt)
Definition: fosi.h:146
void *(* wrapper)(void *)
NANO_TIME periodMark
Definition: fosi.h:82
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.
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.
TICK_TIME nano2ticks(NANO_TIME nano)
Time conversions from nano seconds to system ticks.
Definition: fosi.h:100
const int LowestPriority
An integer denoting the lowest priority of the selected OS.
Definition: ecosthreads.cpp:44
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.
NANO_TIME period
Definition: fosi.h:84
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
Change the period of a periodic RTOS task.
Definition: fosi.h:62
struct timespec TIME_SPEC
Definition: fosi.h:97
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.
pthread_attr_t attr
Definition: fosi.h:73