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
00039
00040 #define OROBLD_OS_INTERNAL
00041 #include "os/ThreadInterface.hpp"
00042 #include "fosi.h"
00043 #include "../fosi_internal_interface.hpp"
00044 #include <cassert>
00045 #define INTERNAL_QUAL
00046
00047 #include "../../Logger.hpp"
00048 #include <signal.h>
00049 #include <execinfo.h>
00050
00051 extern "C"
00052 void warn_upon_switch(int sig __attribute__((unused)))
00053 {
00054 void *bt[32];
00055 int nentries;
00056
00057
00058
00059 nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
00060 backtrace_symbols_fd(bt,nentries,fileno(stderr));
00061 }
00062
00063
00064 namespace RTT
00065 {
00066 namespace os {
00067
00068 INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main)
00069 {
00070
00071 if ( geteuid() != 0 ) {
00072 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2302
00073 printf( "WARNING: You are not root. This program *may* require that you are root.\n");
00074
00075 #else
00076 printf( "You are not root. This program requires that you are root.\n");
00077 exit(1);
00078 #endif
00079 }
00080
00081
00082 int rv = mlockall(MCL_CURRENT | MCL_FUTURE);
00083 if ( rv != 0 ) {
00084 perror( "rtos_task_create_main: Could not lock memory using mlockall" );
00085 exit(1);
00086 }
00087
00088 struct sched_param param;
00089
00090
00091 param.sched_priority = sched_get_priority_max(ORO_SCHED_OTHER);
00092 if (param.sched_priority != -1 )
00093 sched_setscheduler( 0, ORO_SCHED_OTHER, ¶m);
00094
00095 const char* mt_name = "MainThread";
00096 main->sched_type = SCHED_XENOMAI_SOFT;
00097 main->name = strncpy( (char*)malloc( (strlen(mt_name)+1)*sizeof(char) ), mt_name, strlen(mt_name)+1 );
00098
00099 int ret = -1;
00100 while( ret != 0) {
00101
00102 if ( (ret = rt_task_shadow( &(main->xenotask),mt_name, 0, 0)) != 0 ) {
00103 if ( ret == -ENOMEM ) {
00104
00105 printf( "Cannot rt_task_create() MainThread: Out of memory.\n");
00106 exit(1);
00107 }
00108 if ( ret == -EBUSY ) {
00109
00110 log(Info) << "MainThread already a Xenomai task." <<endlog();
00111 break;
00112 }
00113 if ( ret == -EEXIST ) {
00114
00115 mt_name = 0;
00116 continue;
00117 }
00118 if ( ret == -EPERM ) {
00119
00120 printf( "Can not rt_task_create() MainThread: No permission.\n");
00121 exit(1);
00122 }
00123
00124 printf( "Can not rt_task_create() MainThread: Error %d.\n",ret);
00125 exit(1);
00126 }
00127 }
00128
00129
00130 if ( mt_name == 0) {
00131 log(Warning) << "'MainThread' name was already in use. Registered empty name with Xenomai.\n" <<endlog();
00132 }
00133
00134
00135 main->xenoptr = rt_task_self();
00136
00137 #ifdef OROSEM_OS_XENO_PERIODIC
00138 # if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
00139
00140 rt_timer_start( ORODAT_OS_XENO_PERIODIC_TICK*1000*1000*1000 );
00141 Logger::In in("Scheduler");
00142 Logger::log() << Logger::Info << "Xenomai Periodic Timer started using "<<ORODAT_OS_XENO_PERIODIC_TICK<<" seconds." << Logger::endl;
00143 # else
00144 Logger::In in("Scheduler");
00145 Logger::log() << Logger::Error << "Set Xenomai Periodic Timer using the Linux kernel configuration." << Logger::endl;
00146 # endif
00147 #else
00148 # if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
00149 rt_timer_start( TM_ONESHOT );
00150 Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
00151 # else
00152 # if CONFIG_XENO_OPT_TIMING_PERIODIC
00153 Logger::log() << Logger::Info << "Xenomai Periodic Timer configured in 'periodic' mode." << Logger::endl;
00154 # else
00155 Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
00156 # endif
00157 # endif
00158 #endif
00159 log(Info) << "Installing SIGXCPU handler." <<endlog();
00160
00161 struct sigaction sa;
00162 sa.sa_handler = warn_upon_switch;
00163 sigemptyset( &sa.sa_mask );
00164 sa.sa_flags = 0;
00165 sigaction(SIGXCPU, &sa, 0);
00166
00167 Logger::log() << Logger::Debug << "Xenomai Timer and Main Task Created" << Logger::endl;
00168 return 0;
00169 }
00170
00171 INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task)
00172 {
00173
00174 free (main_task->name);
00175 munlockall();
00176 return 0;
00177 }
00178
00179
00180 struct XenoCookie {
00181 void* data;
00182 void* (*wrapper)(void*);
00183 };
00184
00185 INTERNAL_QUAL void rtos_xeno_thread_wrapper( void* cookie )
00186 {
00187
00188 RTOS_TASK* task = ((ThreadInterface*)((XenoCookie*)cookie)->data)->getTask();
00189 task->xenoptr = rt_task_self();
00190 assert( task->xenoptr );
00191
00192
00193 ((XenoCookie*)cookie)->wrapper( ((XenoCookie*)cookie)->data );
00194 free(cookie);
00195 }
00196
00197 INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
00198 int priority,
00199 unsigned cpu_affinity,
00200 const char* name,
00201 int sched_type,
00202 size_t stack_size,
00203 void * (*start_routine)(void *),
00204 ThreadInterface* obj)
00205 {
00206 rtos_task_check_priority(&sched_type, &priority);
00207 XenoCookie* xcookie = (XenoCookie*)malloc( sizeof(XenoCookie) );
00208 xcookie->data = obj;
00209 xcookie->wrapper = start_routine;
00210 if ( name == 0 || strlen(name) == 0)
00211 name = "XenoThread";
00212 task->name = strncpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name, strlen(name)+1 );
00213 task->sched_type = sched_type;
00214 int rv;
00215
00216 unsigned int aff = 0;
00217 if ( cpu_affinity != 0 ) {
00218
00219 for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) {
00220 if(cpu_affinity & (1 << i)) {
00221
00222 if ( i > 7 ) {
00223 const unsigned int all_cpus = ~0;
00224 if ( cpu_affinity != all_cpus )
00225 log(Warning) << "rtos_task_create: ignoring cpu_affinity for "<< name << " on CPU " << i << " since it's larger than RTHAL_NR_CPUS - 1 (="<< 7 <<")"<<endlog();
00226 } else {
00227 aff |= T_CPU(i);
00228 }
00229 }
00230 }
00231 }
00232
00233 if (stack_size == 0) {
00234 log(Debug) << "Raizing default stack size to 128kb for Xenomai threads in Orocos." <<endlog();
00235 stack_size = 128000;
00236 }
00237
00238
00239
00240 rv = rt_task_spawn(&(task->xenotask), name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
00241 if ( rv == -EEXIST ) {
00242 free( task->name );
00243 task->name = strncpy( (char*)malloc( (strlen(name)+2)*sizeof(char) ), name, strlen(name)+1 );
00244 task->name[ strlen(name) ] = '0';
00245 task->name[ strlen(name)+1 ] = 0;
00246 while ( rv == -EEXIST && task->name[ strlen(name) ] != '9') {
00247 task->name[ strlen(name) ] += 1;
00248 rv = rt_task_spawn(&(task->xenotask), task->name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
00249 }
00250 }
00251 if ( rv == -EEXIST ) {
00252 log(Warning) << name << ": an object with that name is already existing in Xenomai." << endlog();
00253 rv = rt_task_spawn(&(task->xenotask), 0, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
00254 }
00255 if ( rv != 0) {
00256 log(Error) << name << " : CANNOT INIT Xeno TASK " << task->name <<" error code: "<< rv << endlog();
00257 return rv;
00258 }
00259
00260 rt_task_yield();
00261 return 0;
00262 }
00263
00264 INTERNAL_QUAL void rtos_task_yield(RTOS_TASK*) {
00265 rt_task_yield();
00266 }
00267
00268 INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
00269 RT_TASK* self = rt_task_self();
00270 if (self == 0 || task == 0)
00271 return -1;
00272 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2500
00273 if ( rt_task_same( self, task->xenoptr ) != 0 )
00274 return 1;
00275 #else
00276
00277 if ( self == task->xenoptr )
00278 return 1;
00279 #endif
00280 return 0;
00281 }
00282
00283
00284 INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00285 {
00286 if (*scheduler != SCHED_XENOMAI_HARD && *scheduler != SCHED_XENOMAI_SOFT ) {
00287 log(Error) << "Unknown scheduler type." <<endlog();
00288 *scheduler = SCHED_XENOMAI_SOFT;
00289 return -1;
00290 }
00291 return 0;
00292 }
00293
00294 INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00295 {
00296 int ret = 0;
00297
00298 ret = rtos_task_check_scheduler(scheduler);
00299
00300
00301
00302 #if ((CONFIG_XENO_VERSION_MAJOR*10000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 20500
00303 const int minprio = 0;
00304 #else
00305 const int minprio = 1;
00306 #endif
00307 if (*priority < minprio){
00308 log(Warning) << "Forcing priority ("<<*priority<<") of thread to " << minprio <<"." <<endlog();
00309 *priority = minprio;
00310 ret = -1;
00311 }
00312 if (*priority > 99){
00313 log(Warning) << "Forcing priority ("<<*priority<<") of thread to 99." <<endlog();
00314 *priority = 99;
00315 ret = -1;
00316 }
00317 return ret;
00318 }
00319
00320
00321
00322 INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* t, int sched_type) {
00323
00324 if (t->xenoptr != rt_task_self() ) {
00325 return -1;
00326 }
00327
00328 if ( rtos_task_check_scheduler( &sched_type ) == -1)
00329 return -1;
00330
00331 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2600
00332 if (sched_type == SCHED_XENOMAI_HARD) {
00333 if ( rt_task_set_mode( 0, T_PRIMARY, 0 ) == 0 ) {
00334 t->sched_type = SCHED_XENOMAI_HARD;
00335 return 0;
00336 } else {
00337 return -1;
00338 }
00339 } else {
00340 if ( sched_type == SCHED_XENOMAI_SOFT) {
00341
00342 if (rt_task_set_mode( T_PRIMARY, 0, 0 ) == 0 ) {
00343 t->sched_type = SCHED_XENOMAI_SOFT;
00344 return 0;
00345 } else {
00346 return -1;
00347 }
00348 }
00349 }
00350 assert(false);
00351 return -1;
00352 #else
00353 t->sched_type = sched_type;
00354 return 0;
00355 #endif
00356 }
00357
00358 INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* mytask) {
00359 #if 0
00360
00361 RT_TASK* tt = mytask->xenoptr;
00362 RT_TASK_INFO info;
00363 if ( tt )
00364 if ( rt_task_inquire( tt, &info) == 0 )
00365 if ( info.status & XNRELAX )
00366 return SCHED_XENOMAI_SOFT;
00367 else
00368 return SCHED_XENOMAI_HARD;
00369 return -1;
00370 #else
00371 return mytask->sched_type;
00372 #endif
00373 }
00374
00375 INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00376 {
00377 if (nanosecs == 0) {
00378 rt_task_set_periodic( &(mytask->xenotask), TM_NOW, TM_INFINITE);
00379 }
00380 else {
00381 rt_task_set_periodic( &(mytask->xenotask), TM_NOW, rt_timer_ns2ticks(nanosecs) );
00382 }
00383 }
00384
00385 INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00386 {
00387 rtos_task_make_periodic( mytask, nanosecs);
00388
00389 }
00390
00391 INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00392 {
00393
00394 }
00395
00396 INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* mytask )
00397 {
00398
00399 #if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
00400 if ( rt_task_wait_period() == -ETIMEDOUT)
00401 return 1;
00402 #else // 2.1, 2.2, 2.3, 2.4,...
00403 long unsigned int overrun = 0;
00404 rt_task_wait_period(&overrun);
00405
00406 #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2600
00407
00408 if ( mytask->sched_type == SCHED_XENOMAI_SOFT )
00409 rt_task_set_mode(T_PRIMARY, 0, 0 );
00410 #endif
00411
00412 if ( overrun != 0)
00413 return 1;
00414 #endif
00415 return 0;
00416 }
00417
00418 INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00419 {
00420 log(Error) << "rtos_task_set_cpu_affinity: Xenomai tasks don't allow to migrate to another CPU once created." << endlog();
00421 return -1;
00422 }
00423
00424 INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00425 {
00426 return 0;
00427 }
00428
00429 INTERNAL_QUAL const char* rtos_task_get_name(const RTOS_TASK* mytask) {
00430 return mytask->name;
00431 }
00432
00433 INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
00434 {
00435 return 0;
00436 }
00437
00438 INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK* mytask) {
00439 RT_TASK_INFO info;
00440
00441 RT_TASK* tt = mytask->xenoptr;
00442 if ( tt )
00443 if ( rt_task_inquire ( tt, &info) == 0 )
00444 return info.bprio;
00445 return -1;
00446 }
00447
00448 INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00449 if ( rt_task_join(&(mytask->xenotask)) != 0 ) {
00450 log(Error) << "Failed to join with thread " << mytask->name << endlog();
00451 }
00452 rt_task_delete(&(mytask->xenotask));
00453 }
00454
00455 INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * mytask, int priority)
00456 {
00457
00458 RT_TASK* tt = mytask->xenoptr;
00459 if ( tt )
00460 return rt_task_set_priority( tt, priority);
00461 return -1;
00462 }
00463 }
00464 }
00465 #undef INTERNAL_QUAL
00466