38 #include "../ThreadInterface.hpp" 40 #include "../fosi_internal_interface.hpp" 41 #include "../../Logger.hpp" 44 #include <sys/resource.h> 45 #ifdef ORO_OS_LINUX_CAP_NG 50 #include <sys/types.h> 52 #include <sys/syscall.h> 63 const char* name =
"main";
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;
72 pthread_attr_setschedparam(&(main_task->
attr), &sp);
73 main_task->
priority = sp.sched_priority;
74 main_task->
pid = getpid();
80 pthread_attr_destroy( &(main_task->
attr) );
81 free(main_task->
name);
82 main_task->
name = NULL;
88 void* (*wrapper)(
void*);
95 task->
pid = syscall(SYS_gettid);
108 unsigned cpu_affinity,
112 void * (*start_routine)(
void *),
124 xcookie->
wrapper = start_routine;
127 if ( strlen(name) == 0 )
129 task->
name = strcpy( (
char*)malloc( (strlen(name) + 1) *
sizeof(
char)), name);
131 if ( (rv = pthread_attr_init(&(task->
attr))) != 0 ){
135 if ( (rv = pthread_attr_setschedpolicy(&(task->
attr), sched_type)) != 0){
140 if ( (rv = pthread_attr_setstacksize(&(task->
attr), stack_size)) != 0){
143 pthread_attr_getschedpolicy(&(task->
attr), &rv );
144 assert( rv == sched_type );
148 struct sched_param sp;
149 if (sched_type != SCHED_OTHER){
150 sp.sched_priority=priority;
152 if ( (rv = pthread_attr_setschedparam(&(task->
attr), &sp)) != 0 ){
156 rv = pthread_create(&(task->
thread), &(task->
attr),
159 log(
Error) <<
"Failed to create thread " << task->
name <<
": " 160 << strerror(rv) <<
endlog();
164 #ifdef ORO_HAVE_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) {
175 strncpy(&n[0], thread_name, 7);
177 strncpy(&n[8], &thread_name[thread_name_len - 7], 7);
183 strncpy(&n[0], thread_name, MAX_THREAD_NAME_SIZE);
185 n[MAX_THREAD_NAME_SIZE] =
'\0';
186 int result = pthread_setname_np(task->
thread, &n[0]);
188 log(
Warning) <<
"Failed to set thread name for " << task->
name <<
": " 189 << strerror(result) <<
endlog();
192 #endif // ORO_HAVE_PTHREAD_SETNAME_NP 194 if ( cpu_affinity != 0 ) {
195 log(
Debug) <<
"Setting CPU affinity to " << cpu_affinity <<
endlog();
198 log(
Error) <<
"Failed to set CPU affinity to " << cpu_affinity <<
" for " << task->
name <<
": " 199 << strerror(result) <<
endlog();
214 int ret = sched_yield();
216 perror(
"rtos_task_yield");
228 pthread_t
self = pthread_self();
229 if ( pthread_equal(
self, task->
thread) == 0 )
236 struct sched_param param;
241 if (pthread_getschedparam(task->
thread, &policy, ¶m) == 0) {
243 param.sched_priority = task->
priority;
246 return pthread_setschedparam( task->
thread, sched_type, ¶m);
253 struct sched_param param;
255 if ( task && task->
thread != 0 && pthread_getschedparam(task->
thread, &policy, ¶m) == 0)
263 mytask->
period = nanosecs;
288 while ( clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &(task->
periodMark), NULL) != 0 && errno == EINTR ) {
300 task->
periodMark.tv_sec += ts.tv_sec + tn / 1000000000LL;
306 NANO_TIME tn = (now.tv_nsec + ts.tv_nsec);
308 task->
periodMark.tv_sec = ts.tv_sec + now.tv_sec + tn / 1000000000LL;
311 return now > wake ? -1 : 0;
315 int ret = pthread_join( mytask->
thread, 0);
317 log(
Error) <<
"Failed to join thread " << mytask->
name <<
": " 318 << strerror(ret) <<
endlog();
321 pthread_attr_destroy( &(mytask->
attr) );
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;
336 if (*scheduler != SCHED_OTHER && geteuid() != 0
337 #ifdef ORO_OS_LINUX_CAP_NG
338 && capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)==0
345 if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
347 log(
Warning) <<
"Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<
endlog();
348 *scheduler = SCHED_OTHER;
353 if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
355 *scheduler = SCHED_OTHER;
368 if (*scheduler == SCHED_OTHER) {
369 if ( *priority != 0 ) {
371 log(
Warning) <<
"Forcing priority ("<<*priority<<
") of thread with SCHED_OTHER policy to 0." <<
endlog();
378 log(
Warning) <<
"Forcing priority ("<<*priority<<
") of thread with !SCHED_OTHER policy to 1." <<
endlog();
383 log(
Warning) <<
"Forcing priority ("<<*priority<<
") of thread with !SCHED_OTHER policy to 99." <<
endlog();
389 #ifdef ORO_OS_LINUX_CAP_NG
390 && !capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)
395 if (0 == getrlimit(RLIMIT_RTPRIO, &r))
397 if (*priority > (
int)r.rlim_cur)
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;
418 struct sched_param param;
420 if( task && task->
thread != 0 && pthread_getschedparam(task->
thread, &policy, ¶m) == 0) {
423 param.sched_priority = priority;
426 return pthread_setschedparam( task->
thread, policy, ¶m);
435 struct sched_param param;
439 if ( task->
thread == 0 || pthread_getschedparam(task->
thread, &policy, ¶m) != 0)
441 return param.sched_priority;
446 if ( cpu_affinity == 0 )
448 if( task && task->
thread != 0 ) {
451 for(
unsigned i = 0; i < 8*
sizeof(cpu_affinity); i++)
453 if(cpu_affinity & (1 << i)) { CPU_SET(i, &cs); }
455 return pthread_setaffinity_np(task->
thread,
sizeof(cs), &cs);
462 if( task && task->
thread != 0) {
463 unsigned cpu_affinity = 0;
465 pthread_getaffinity_np(task->
thread,
sizeof(cs), &cs);
466 for(
unsigned i = 0; i < 8*
sizeof(cpu_affinity); i++)
468 if(CPU_ISSET(i, &cs)) { cpu_affinity |= (1 << i); }
477 return task->
name ? task->
name :
"(destroyed)";
int rtos_task_is_self(const RTOS_TASK *task)
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)
NANO_TIME rtos_get_time_ns(void)
INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK *main_task)
int rtos_task_set_scheduler(RTOS_TASK *t, int sched_type)
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)
INTERNAL_QUAL int rtos_task_check_priority(int *scheduler, int *priority)
INTERNAL_QUAL void * rtos_posix_thread_wrapper(void *cookie)
void *(* wrapper)(void *)
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)
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK *main_task)
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
struct timespec TIME_SPEC
static Logger::LogFunction endlog()
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)