gnulinux/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  int ret = pthread_join( mytask->thread, 0);
316  if (ret != 0) {
317  log(Error) << "Failed to join thread " << mytask->name << ": "
318  << strerror(ret) << endlog();
319  return;
320  }
321  pthread_attr_destroy( &(mytask->attr) );
322  free(mytask->name);
323  mytask->name = NULL;
324  }
325 
327  {
328 #ifdef ORO_OS_LINUX_CAP_NG
329  if(capng_get_caps_process()) {
330  log(Error) << "Failed to retrieve capabilities (lowering to SCHED_OTHER)." <<endlog();
331  *scheduler = SCHED_OTHER;
332  return -1;
333  }
334 #endif
335 
336  if (*scheduler != SCHED_OTHER && geteuid() != 0
337 #ifdef ORO_OS_LINUX_CAP_NG
338  && capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)==0
339 #endif
340  ) {
341  // they're not root and they want a real-time priority, which _might_
342  // be acceptable if they're using pam_limits and have set the rtprio ulimit
343  // (see "/etc/security/limits.conf" and "ulimit -a")
344  struct rlimit r;
345  if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
346  {
347  log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog();
348  *scheduler = SCHED_OTHER;
349  return -1;
350  }
351  }
352 
353  if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
354  log(Error) << "Unknown scheduler type." <<endlog();
355  *scheduler = SCHED_OTHER;
356  return -1;
357  }
358  return 0;
359  }
360 
361  INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
362  {
363  int ret = 0;
364  // check scheduler first.
365  ret = rtos_task_check_scheduler(scheduler);
366 
367  // correct priority
368  if (*scheduler == SCHED_OTHER) {
369  if ( *priority != 0 ) {
370  if (*priority != LowestPriority)
371  log(Warning) << "Forcing priority ("<<*priority<<") of thread with SCHED_OTHER policy to 0." <<endlog();
372  *priority = 0;
373  ret = -1;
374  }
375  } else {
376  // SCHED_FIFO/SCHED_RR:
377  if (*priority <= 0){
378  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 1." <<endlog();
379  *priority = 1;
380  ret = -1;
381  }
382  if (*priority > 99){
383  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 99." <<endlog();
384  *priority = 99;
385  ret = -1;
386  }
387  // and limit them according to pam_Limits (only if not root)
388  if ( geteuid() != 0
389 #ifdef ORO_OS_LINUX_CAP_NG
390  && !capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)
391 #endif
392  )
393  {
394  struct rlimit r;
395  if (0 == getrlimit(RLIMIT_RTPRIO, &r))
396  {
397  if (*priority > (int)r.rlim_cur)
398  {
399  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to the pam_limit of " << r.rlim_cur <<endlog();
400  *priority = r.rlim_cur;
401  ret = -1;
402  }
403  }
404  else
405  {
406  // this should not be possible, but do something intelligent
407  *priority = 1;
408  ret = -1;
409  }
410  }
411  }
412  return ret;
413  }
414 
416  {
417  int policy = 0;
418  struct sched_param param;
419  // first retrieve thread scheduling parameters:
420  if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0) {
421  if ( rtos_task_check_priority( &policy, &priority ) != 0 )
422  return -1;
423  param.sched_priority = priority;
424  task->priority = priority; // store for set_scheduler
425  // write new policy:
426  return pthread_setschedparam( task->thread, policy, &param);
427  }
428  return -1;
429  }
430 
432  {
433  // if sched_other, return the 'virtual' priority
434  int policy = 0;
435  struct sched_param param;
436  // first retrieve thread scheduling parameters:
437  if ( task == 0 )
438  return -1;
439  if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, &param) != 0)
440  return task->priority;
441  return param.sched_priority;
442  }
443 
444  INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
445  {
446  if ( cpu_affinity == 0 ) // clears the mask.
447  cpu_affinity = ~0;
448  if( task && task->thread != 0 ) {
449  cpu_set_t cs;
450  CPU_ZERO(&cs);
451  for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
452  {
453  if(cpu_affinity & (1 << i)) { CPU_SET(i, &cs); }
454  }
455  return pthread_setaffinity_np(task->thread, sizeof(cs), &cs);
456  }
457  return -1;
458  }
459 
461  {
462  if( task && task->thread != 0) {
463  unsigned cpu_affinity = 0;
464  cpu_set_t cs;
465  pthread_getaffinity_np(task->thread, sizeof(cs), &cs);
466  for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
467  {
468  if(CPU_ISSET(i, &cs)) { cpu_affinity |= (1 << i); }
469  }
470  return cpu_affinity;
471  }
472  return ~0;
473  }
474 
475  INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
476  {
477  return task->name ? task->name : "(destroyed)";
478  }
479 
480  }
481 }
482 #undef INTERNAL_QUAL
int rtos_task_is_self(const RTOS_TASK *task)
long long NANO_TIME
Definition: ecos/fosi.h:67
int rtos_task_get_priority(const RTOS_TASK *task)
INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK *mytask, NANO_TIME nanosecs)
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
#define INTERNAL_QUAL
#define ORO_WAIT_ABS
Definition: ecos/fosi.h:64
NANO_TIME rtos_get_time_ns(void)
Definition: ecos/fosi.h:125
char * name
Definition: ecos/fosi.h:78
INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK *main_task)
int rtos_task_set_scheduler(RTOS_TASK *t, int sched_type)
Definition: mystd.hpp:163
static int rtos_nanosleep(const TIME_SPEC *rqtp, TIME_SPEC *rmtp)
int rtos_task_set_cpu_affinity(RTOS_TASK *task, unsigned cpu_affinity)
unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
cyg_thread thread
Definition: ecos/fosi.h:76
INTERNAL_QUAL int rtos_task_check_priority(int *scheduler, int *priority)
INTERNAL_QUAL void * rtos_posix_thread_wrapper(void *cookie)
NANO_TIME periodMark
Definition: ecos/fosi.h:94
void rtos_task_set_wait_period_policy(RTOS_TASK *task, int policy)
void rtos_task_delete(RTOS_TASK *mytask)
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK *)
int rtos_task_set_priority(RTOS_TASK *task, int priority)
const char * rtos_task_get_name(const RTOS_TASK *task)
int rtos_task_check_scheduler(int *sched_type)
static TIME_SPEC ticks2timespec(TICK_TIME hrt)
int rtos_task_wait_period(RTOS_TASK *task)
TICK_TIME nano2ticks(NANO_TIME nano)
Definition: ecos/fosi.h:112
const int LowestPriority
Definition: ecosthreads.cpp:44
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:53
INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK *main_task)
NANO_TIME period
Definition: ecos/fosi.h:96
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
struct timespec TIME_SPEC
Definition: ecos/fosi.h:109
static Logger & log()
Definition: Logger.hpp:350
static Logger::LogFunction endlog()
Definition: Logger.hpp:362
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)
int rtos_task_get_scheduler(const RTOS_TASK *t)
pthread_attr_t attr
Definition: gnulinux/fosi.h:92


rtt
Author(s): RTT Developers
autogenerated on Tue Jun 25 2019 19:33:24