00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "../ThreadInterface.hpp"
00039 #include "fosi.h"
00040 #include "../fosi_internal_interface.hpp"
00041 #include "../../Logger.hpp"
00042 #include <cassert>
00043 #include <sys/time.h>
00044 #include <sys/resource.h>
00045 #ifdef ORO_OS_LINUX_CAP_NG
00046 #include <cap-ng.h>
00047 #endif
00048 #include <iostream>
00049 #include <cstdlib>
00050 #include <sys/types.h>
00051 #include <unistd.h>
00052 #include <sys/syscall.h>
00053
00054 using namespace std;
00055
00056 #define INTERNAL_QUAL
00057
00058 namespace RTT
00059 { namespace os {
00060
00061 INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main_task)
00062 {
00063 const char* name = "main";
00064 main_task->wait_policy = ORO_WAIT_ABS;
00065 main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00066 main_task->thread = pthread_self();
00067 pthread_attr_init( &(main_task->attr) );
00068 struct sched_param sp;
00069 sp.sched_priority=0;
00070
00071
00072 pthread_attr_setschedparam(&(main_task->attr), &sp);
00073 main_task->priority = sp.sched_priority;
00074 main_task->pid = getpid();
00075 return 0;
00076 }
00077
00078 INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task)
00079 {
00080 pthread_attr_destroy( &(main_task->attr) );
00081 free(main_task->name);
00082 return 0;
00083 }
00084
00085 struct PosixCookie {
00086 void* data;
00087 void* (*wrapper)(void*);
00088 };
00089
00090 INTERNAL_QUAL void* rtos_posix_thread_wrapper( void* cookie )
00091 {
00092
00093 RTOS_TASK* task = ((ThreadInterface*)((PosixCookie*)cookie)->data)->getTask();
00094 task->pid = syscall(SYS_gettid);
00095 assert( task->pid );
00096
00097
00098 ((PosixCookie*)cookie)->wrapper( ((PosixCookie*)cookie)->data );
00099 free(cookie);
00100 return 0;
00101 }
00102
00103
00104
00105 INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
00106 int priority,
00107 unsigned cpu_affinity,
00108 const char * name,
00109 int sched_type,
00110 size_t stack_size,
00111 void * (*start_routine)(void *),
00112 ThreadInterface* obj)
00113 {
00114 int rv;
00115 task->wait_policy = ORO_WAIT_ABS;
00116 rtos_task_check_priority( &sched_type, &priority );
00117
00118
00119 task->priority = priority;
00120
00121 PosixCookie* xcookie = (PosixCookie*)malloc( sizeof(PosixCookie) );
00122 xcookie->data = obj;
00123 xcookie->wrapper = start_routine;
00124
00125
00126 if ( strlen(name) == 0 )
00127 name = "Thread";
00128 task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00129
00130 if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){
00131 return rv;
00132 }
00133
00134 if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
00135 return rv;
00136 }
00137
00138 if (stack_size )
00139 if ( (rv = pthread_attr_setstacksize(&(task->attr), stack_size)) != 0){
00140 return rv;
00141 }
00142 pthread_attr_getschedpolicy(&(task->attr), &rv );
00143 assert( rv == sched_type );
00144
00145
00146
00147 struct sched_param sp;
00148 if (sched_type != SCHED_OTHER){
00149 sp.sched_priority=priority;
00150
00151 if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){
00152 return rv;
00153 }
00154 }
00155 rv = pthread_create(&(task->thread), &(task->attr),
00156 rtos_posix_thread_wrapper, xcookie);
00157
00158 if ( cpu_affinity != ~0 ) {
00159 log(Debug) << "Setting CPU affinity to " << cpu_affinity << endlog();
00160 if (0 != rtos_task_set_cpu_affinity(task, cpu_affinity))
00161 {
00162 log(Error) << "Failed to set CPU affinity to " << cpu_affinity << endlog();
00163 }
00164 }
00165
00166 return rv;
00167 }
00168
00169 INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* t) {
00170 #if 0
00171
00172
00173 NANO_TIME timeRemaining = 1000;
00174 TIME_SPEC ts( ticks2timespec( timeRemaining ) );
00175 rtos_nanosleep( &ts , NULL );
00176 #else
00177 int ret = sched_yield();
00178 if ( ret != 0)
00179 perror("rtos_task_yield");
00180 #endif
00181 }
00182
00183 INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
00184 {
00185 if (task)
00186 return task->pid;
00187 return 0;
00188 }
00189
00190 INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
00191 pthread_t self = pthread_self();
00192 if ( pthread_equal(self, task->thread) == 0 )
00193 return 0;
00194 return 1;
00195 }
00196
00197 INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) {
00198 int policy = -1;
00199 struct sched_param param;
00200
00201 if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
00202 return -1;
00203
00204 if (pthread_getschedparam(task->thread, &policy, ¶m) == 0) {
00205
00206 param.sched_priority = task->priority;
00207 rtos_task_check_priority( &sched_type, ¶m.sched_priority );
00208
00209 return pthread_setschedparam( task->thread, sched_type, ¶m);
00210 }
00211 return -1;
00212 }
00213
00214 INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* task) {
00215 int policy = -1;
00216 struct sched_param param;
00217
00218 if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, ¶m) == 0)
00219 return policy;
00220 return -1;
00221 }
00222
00223 INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00224 {
00225
00226 mytask->period = nanosecs;
00227
00228 mytask->periodMark = ticks2timespec( nano2ticks( rtos_get_time_ns() + nanosecs ) );
00229 }
00230
00231 INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00232 {
00233 rtos_task_make_periodic(mytask, nanosecs);
00234 }
00235
00236 INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00237 {
00238 task->wait_policy = policy;
00239 }
00240
00241 INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
00242 {
00243 if ( task->period == 0 )
00244 return 0;
00245
00246
00247 NANO_TIME now = rtos_get_time_ns();
00248 NANO_TIME wake= task->periodMark.tv_sec * 1000000000LL + task->periodMark.tv_nsec;
00249
00250
00251 while ( clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &(task->periodMark), NULL) != 0 && errno == EINTR ) {
00252 errno = 0;
00253 }
00254
00255 if (task->wait_policy == ORO_WAIT_ABS)
00256 {
00257
00258
00259 TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
00260
00261 NANO_TIME tn = (task->periodMark.tv_nsec + ts.tv_nsec);
00262 task->periodMark.tv_nsec = tn % 1000000000LL;
00263 task->periodMark.tv_sec += ts.tv_sec + tn / 1000000000LL;
00264 }
00265 else
00266 {
00267 TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
00268 TIME_SPEC now = ticks2timespec( rtos_get_time_ns() );
00269 NANO_TIME tn = (now.tv_nsec + ts.tv_nsec);
00270 task->periodMark.tv_nsec = tn % 1000000000LL;
00271 task->periodMark.tv_sec = ts.tv_sec + now.tv_sec + tn / 1000000000LL;
00272 }
00273
00274 return now > wake ? -1 : 0;
00275 }
00276
00277 INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00278 pthread_join( mytask->thread, 0);
00279 pthread_attr_destroy( &(mytask->attr) );
00280 free(mytask->name);
00281 }
00282
00283 INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00284 {
00285 #ifdef ORO_OS_LINUX_CAP_NG
00286 if(capng_get_caps_process()) {
00287 log(Error) << "Failed to retrieve capabilities (lowering to SCHED_OTHER)." <<endlog();
00288 *scheduler = SCHED_OTHER;
00289 return -1;
00290 }
00291 #endif
00292
00293 if (*scheduler != SCHED_OTHER && geteuid() != 0
00294 #ifdef ORO_OS_LINUX_CAP_NG
00295 && capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)==0
00296 #endif
00297 ) {
00298
00299
00300
00301 struct rlimit r;
00302 if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
00303 {
00304 log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog();
00305 *scheduler = SCHED_OTHER;
00306 return -1;
00307 }
00308 }
00309
00310 if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
00311 log(Error) << "Unknown scheduler type." <<endlog();
00312 *scheduler = SCHED_OTHER;
00313 return -1;
00314 }
00315 return 0;
00316 }
00317
00318 INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00319 {
00320 int ret = 0;
00321
00322 ret = rtos_task_check_scheduler(scheduler);
00323
00324
00325 if (*scheduler == SCHED_OTHER) {
00326 if ( *priority != 0 ) {
00327 if (*priority != LowestPriority)
00328 log(Warning) << "Forcing priority ("<<*priority<<") of thread with SCHED_OTHER policy to 0." <<endlog();
00329 *priority = 0;
00330 ret = -1;
00331 }
00332 } else {
00333
00334 if (*priority <= 0){
00335 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 1." <<endlog();
00336 *priority = 1;
00337 ret = -1;
00338 }
00339 if (*priority > 99){
00340 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 99." <<endlog();
00341 *priority = 99;
00342 ret = -1;
00343 }
00344
00345 if ( geteuid() != 0
00346 #ifdef ORO_OS_LINUX_CAP_NG
00347 && !capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)
00348 #endif
00349 )
00350 {
00351 struct rlimit r;
00352 if (0 == getrlimit(RLIMIT_RTPRIO, &r))
00353 {
00354 if (*priority > (int)r.rlim_cur)
00355 {
00356 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to the pam_limit of " << r.rlim_cur <<endlog();
00357 *priority = r.rlim_cur;
00358 ret = -1;
00359 }
00360 }
00361 else
00362 {
00363
00364 *priority = 1;
00365 ret = -1;
00366 }
00367 }
00368 }
00369 return ret;
00370 }
00371
00372 INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
00373 {
00374 int policy = 0;
00375 struct sched_param param;
00376
00377 if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, ¶m) == 0) {
00378 if ( rtos_task_check_priority( &policy, &priority ) != 0 )
00379 return -1;
00380 param.sched_priority = priority;
00381 task->priority = priority;
00382
00383 return pthread_setschedparam( task->thread, policy, ¶m);
00384 }
00385 return -1;
00386 }
00387
00388 INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *task)
00389 {
00390
00391 int policy = 0;
00392 struct sched_param param;
00393
00394 if ( task == 0 )
00395 return -1;
00396 if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, ¶m) != 0)
00397 return task->priority;
00398 return param.sched_priority;
00399 }
00400
00401 INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00402 {
00403 if ( cpu_affinity == 0 )
00404 cpu_affinity = ~0;
00405 if( task && task->thread != 0 ) {
00406 cpu_set_t cs;
00407 CPU_ZERO(&cs);
00408 for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
00409 {
00410 if(cpu_affinity & (1 << i)) { CPU_SET(i, &cs); }
00411 }
00412 return pthread_setaffinity_np(task->thread, sizeof(cs), &cs);
00413 }
00414 return -1;
00415 }
00416
00417 INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00418 {
00419 if( task && task->thread != 0) {
00420 unsigned cpu_affinity = 0;
00421 cpu_set_t cs;
00422 pthread_getaffinity_np(task->thread, sizeof(cs), &cs);
00423 for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
00424 {
00425 if(CPU_ISSET(i, &cs)) { cpu_affinity |= (1 << i); }
00426 }
00427 return cpu_affinity;
00428 }
00429 return ~0;
00430 }
00431
00432 INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
00433 {
00434 return task->name;
00435 }
00436
00437 }
00438 }
00439 #undef INTERNAL_QUAL