Orocos Real-Time Toolkit  2.6.0
fosi_internal.cpp
00001 /***************************************************************************
00002   tag: Peter Soetens  Wed Jan 18 14:11:39 CET 2006  fosi_internal.hpp
00003 
00004                         fosi_internal.hpp -  description
00005                            -------------------
00006     begin                : Wed January 18 2006
00007     copyright            : (C) 2006 Peter Soetens
00008     email                : peter.soetens@mech.kuleuven.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU General Public                   *
00013  *   License as published by the Free Software Foundation;                 *
00014  *   version 2 of the License.                                             *
00015  *                                                                         *
00016  *   As a special exception, you may use this file as part of a free       *
00017  *   software library without restriction.  Specifically, if other files   *
00018  *   instantiate templates or use macros or inline functions from this     *
00019  *   file, or you compile this file and link it with other files to        *
00020  *   produce an executable, this file does not by itself cause the         *
00021  *   resulting executable to be covered by the GNU General Public          *
00022  *   License.  This exception does not however invalidate any other        *
00023  *   reasons why the executable file might be covered by the GNU General   *
00024  *   Public License.                                                       *
00025  *                                                                         *
00026  *   This library is distributed in the hope that it will be useful,       *
00027  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00028  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00029  *   Lesser General Public License for more details.                       *
00030  *                                                                         *
00031  *   You should have received a copy of the GNU General Public             *
00032  *   License along with this library; if not, write to the Free Software   *
00033  *   Foundation, Inc., 59 Temple Place,                                    *
00034  *   Suite 330, Boston, MA  02111-1307  USA                                *
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     /* Dump a backtrace to standard error of the frame which caused the switch to
00058        secondary mode: */
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             // first check if root (or if have sufficient privileges)
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                 // \todo verify have sufficient privileges
00075 #else
00076                 printf( "You are not root. This program requires that you are root.\n");
00077                 exit(1);
00078 #endif
00079             }
00080 
00081             // locking of all memory for this process
00082             int rv = mlockall(MCL_CURRENT | MCL_FUTURE);
00083             if ( rv != 0 ) {
00084                 perror( "rtos_task_create_main: Could not lock memory using mlockall" ); // Logger unavailable.
00085                 exit(1);
00086             }
00087 
00088             struct sched_param param;
00089             // we set the MT to the highest sched priority to allow the console
00090             // to interrupt a loose running thread.
00091             param.sched_priority = sched_get_priority_max(ORO_SCHED_OTHER);
00092             if (param.sched_priority != -1 )
00093                 sched_setscheduler( 0, ORO_SCHED_OTHER, &param);
00094 
00095             const char* mt_name = "MainThread";
00096             main->sched_type = SCHED_XENOMAI_SOFT; // default for MainThread
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                 // name, priority, mode
00102                 if ( (ret = rt_task_shadow( &(main->xenotask),mt_name, 0, 0)) != 0 ) {
00103                     if ( ret == -ENOMEM ) {
00104                         // fail: abort
00105                         printf( "Cannot rt_task_create() MainThread: Out of memory.\n");
00106                         exit(1);
00107                     }
00108                     if ( ret == -EBUSY ) {
00109                         // ok: we are a xeno thread (may log() ):
00110                         log(Info) << "MainThread already a Xenomai task." <<endlog();
00111                         break;
00112                     }
00113                     if ( ret == -EEXIST ) {
00114                         // fail: retry without using a name.
00115                         mt_name = 0; // do not register
00116                         continue;
00117                     }
00118                     if ( ret == -EPERM ) {
00119                         // fail: abort
00120                         printf( "Can not rt_task_create() MainThread: No permission.\n");
00121                         exit(1);
00122                     }
00123                     // uncaught error: abort
00124                     printf( "Can not rt_task_create() MainThread: Error %d.\n",ret);
00125                     exit(1);
00126                 }
00127             }
00128             // We are a xeno thread now:
00129             // Only use Logger after this point (i.e. when rt_task_shadow was succesful).
00130             if ( mt_name == 0) {
00131                 log(Warning) << "'MainThread' name was already in use. Registered empty name with Xenomai.\n" <<endlog();
00132             }
00133 
00134             // main is created in main thread.
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             // time in nanoseconds
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             //signal(SIGXCPU, warn_upon_switch);
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             //rt_task_delete( &(main_task->xenotask) );
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             // store 'self'
00188             RTOS_TASK* task = ((ThreadInterface*)((XenoCookie*)cookie)->data)->getTask();
00189             task->xenoptr = rt_task_self();
00190             assert( task->xenoptr );
00191 
00192             // call user function
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; // User requested scheduler.
00214             int rv;
00215 
00216             unsigned int aff = 0;
00217             if ( cpu_affinity != 0 ) {
00218                 // calculate affinity:
00219                 for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) {
00220                     if(cpu_affinity & (1 << i)) { 
00221                         // RTHAL_NR_CPUS is defined in the kernel, not in user space. So we just limit up to 7, until Xenomai allows us to get the maximum.
00222                         if ( i > 7 ) {
00223                             const unsigned int all_cpus = ~0;
00224                             if ( cpu_affinity != all_cpus ) // suppress this warning when ~0 is provided
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             // task, name, stack, priority, mode, fun, arg
00239             // UGLY, how can I check in Xenomai that a name is in use before calling rt_task_spawn ???
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; // non-xeno thread. We could try to compare pthreads like in gnulinux ?
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             // older versions:
00277             if ( self == task->xenoptr ) // xenoptr is also set by rt_task_self() during construction.
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         // check scheduler first.
00298         ret = rtos_task_check_scheduler(scheduler);
00299 
00300         // correct priority
00301         // Hard & Soft:
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         // we could implement here the interrupt shield logic.
00322         INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* t, int sched_type) {
00323             // xenoptr was initialised from the thread wrapper.
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                     // This mode setting is only temporary. See rtos_task_wait_period() as well !
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             // WORK AROUND constness: (need non-const mytask)
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             //rt_task_set_period(&(mytask->xenotask), rt_timer_ns2ticks( nanosecs ));
00389         }
00390 
00391         INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00392         {
00393           // Do nothing
00394         }
00395 
00396         INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* mytask )
00397         {
00398             // detect overrun.
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             // When running soft, switch to secondary mode:
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             // WORK AROUND constness: (need non-const mytask)
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             // ignorint 'type'
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