Orocos Real-Time Toolkit  2.6.0
fosi_internal.cpp
00001 /***************************************************************************
00002   tag: Peter Soetens  Sat May 21 20:15:50 CEST 2005  fosi_internal.hpp
00003 
00004                         fosi_internal.hpp -  description
00005                            -------------------
00006     begin                : Sat May 21 2005
00007     copyright            : (C) 2005 Peter Soetens
00008     email                : peter.soetens@mech.kuleuven.ac.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 #ifndef OS_FOSI_INTERNAL_HPP
00040 #define OS_FOSI_INTERNAL_HPP
00041 #define OROBLD_OS_LXRT_INTERNAL
00042 
00043 #include <iostream>
00044 #include <sched.h>
00045 #include "os/ThreadInterface.hpp"
00046 #include "fosi.h"
00047 #include "../fosi_internal_interface.hpp"
00048 #include <sys/mman.h>
00049 #include <unistd.h>
00050 #include <sys/types.h>
00051 #include "../../rtt-config.h"
00052 #define INTERNAL_QUAL
00053 
00054 #include <string.h>
00055 
00056 #include "../../Logger.hpp"
00057 
00058 namespace RTT
00059 {
00060     namespace os {
00061 
00062         INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main_task)
00063         {
00064             if ( geteuid() != 0 ) {
00065                 std::cerr << "You are not root. This program requires that you are root." << std::endl;
00066                 exit(1);
00067             }
00068 
00069 #ifdef OROSEM_OS_LOCK_MEMORY
00070             int locktype = MCL_CURRENT;
00071 #ifdef OROSEM_OS_LOCK_MEMORY_FUTURE
00072             locktype |= MCL_FUTURE;
00073 #endif
00074             // locking of all memory for this process
00075             int rv = mlockall(locktype);
00076             if ( rv != 0 ) {
00077                 perror( "rtos_task_create_main: Could not lock memory using mlockall" ); // Logger unavailable.
00078             }
00079 #endif
00080 
00081             /* check to see if rtai_lxrt module is loaded */
00082             //         struct module_info modInfo;
00083             //         size_t retSize;
00084             //         if ( query_module("rtai_lxrt", QM_INFO, &modInfo,
00085             //                           sizeof(modInfo), &retSize) != 0 ) {
00086             //             std::cerr <<"It appears the rtai_lxrt module is not loaded !"<<std::endl;
00087             //             exit();
00088             //         }
00089             unsigned long name = nam2num("main");
00090             while ( rt_get_adr( name ) != 0 ) // check for existing 'MAINTHREAD'
00091                 ++name;
00092 
00093 
00094             const char* tname = "main";
00095             main_task->name = strcpy( (char*)malloc( (strlen(tname) + 1) * sizeof(char)), tname);
00096 
00097             if( !(main_task->rtaitask = rt_task_init(name, 10,0,0)) ) // priority, stack, msg_size
00098                 {
00099                     std::cerr << "Cannot rt_task_init() MainThread." << std::endl;
00100                     exit(1);
00101                 }
00102 
00103             struct sched_param param;
00104 
00105             param.sched_priority = sched_get_priority_max(SCHED_OTHER);
00106             if (param.sched_priority != -1 )
00107                 sched_setscheduler( 0, SCHED_OTHER, &param);
00108 
00109             // Avoid the LXRT CHANGED MODE (TRAP), PID = 4088, VEC = 14, SIGNO = 11. warning
00110             rt_task_use_fpu(main_task->rtaitask, 1);
00111 
00112 #ifdef OROSEM_OS_LXRT_PERIODIC
00113             rt_set_periodic_mode();
00114             start_rt_timer( nano2count( NANO_TIME(ORODAT_OS_LXRT_PERIODIC_TICK*1000*1000*1000) ) );
00115             Logger::log() << Logger::Info << "RTAI Periodic Timer ticks at "<<ORODAT_OS_LXRT_PERIODIC_TICK<<" seconds." << Logger::endl;
00116 #else
00117             // BE SURE TO SET rt_preempt_always(1) when using one shot mode
00118             rt_set_oneshot_mode();
00119             // only call this function for RTAI 3.0 or older
00120 #if defined(CONFIG_RTAI_VERSION_MINOR) && defined(CONFIG_RTAI_VERSION_MAJOR)
00121 #  if CONFIG_RTAI_VERSION_MAJOR == 3 && CONFIG_RTAI_VERSION_MINOR == 0
00122             rt_preempt_always(1);
00123 #  endif
00124 #else
00125             rt_preempt_always(1);
00126 #endif
00127             start_rt_timer(0);
00128             Logger::log() << Logger::Info << "RTAI Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
00129 #endif
00130             Logger::log() << Logger::Debug << "RTAI Task Created" << Logger::endl;
00131             return 0;
00132         }
00133 
00134         INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task)
00135         {
00136             // we don't stop the timer
00137             //stop_rt_timer();
00138             rt_task_delete(main_task->rtaitask);
00139             free(main_task->name);
00140             munlockall();
00141             return 0;
00142         }
00143 
00144         struct RTAI_Thread
00145         {
00146             void *(*wrapper)(void*);
00147             void *data;
00148             RTOS_TASK* task;
00149             int priority;
00150             unsigned int tnum;
00151         };
00152 
00153         INTERNAL_QUAL void* rtai_thread_wrapper( void * arg ) {
00154             RTAI_Thread* d = (RTAI_Thread*)arg;
00155             RTOS_TASK* task = d->task;
00156             void* data =  d->data;
00157             int priority = d->priority;
00158             unsigned int tnum = d->tnum;
00159             void*(*wrapper)(void*) = d->wrapper;
00160             free( d );
00161 
00162             if (!(task->rtaitask = rt_task_init(tnum, priority, 0, 0))) {
00163                 std::cerr << "CANNOT INIT LXRT Thread " << task->name <<std::endl;
00164                 std::cerr << "Exiting this thread." <<std::endl;
00165                 exit(-1);
00166             }
00167 
00168             // Schedule in Linux' SCHED_OTHER
00169             struct sched_param param;
00170             param.sched_priority = sched_get_priority_max(SCHED_OTHER);
00171             if (param.sched_priority != -1 )
00172                 sched_setscheduler( 0, SCHED_OTHER, &param);
00173 
00174             // Avoid the LXRT CHANGED MODE (TRAP), PID = 4088, VEC = 14, SIGNO = 11. warning
00175             rt_task_use_fpu(task->rtaitask, 1);
00176 
00177             // New default: new threads are always hard.
00178             rt_make_hard_real_time();
00179 
00180             data = wrapper( data );
00181 
00182             // Exit in soft mode to avoid RTAI warnings.
00183             rt_make_soft_real_time();
00184             // cleanup here to avoid "LXRT Releases PID" warnings.
00185             rt_task_delete(task->rtaitask);
00186             task->rtaitask = 0;
00187             // See rtos_task_delete for further cleanups.
00188             return data;
00189         }
00190 
00191         INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
00192                                            int priority,
00193                                            unsigned cpu_affinity,
00194                                            const char* name,
00195                                            int sched_type,
00196                                            size_t stack_size,
00197                                            void * (*start_routine)(void *),
00198                                            ThreadInterface* obj)
00199         {
00200             char taskName[7];
00201             if ( strlen(name) == 0 )
00202                 name = "Thread";
00203             strncpy(taskName, name, 7);
00204             unsigned long task_num = nam2num( taskName );
00205             if ( rt_get_adr(nam2num( taskName )) != 0 ) {
00206                 unsigned long nname = nam2num( taskName );
00207                 while ( rt_get_adr( nname ) != 0 ) // check for existing 'NAME'
00208                     ++nname;
00209                 num2nam( nname, taskName); // set taskName to new name
00210                 taskName[6] = 0;
00211                 task_num = nname;
00212             }
00213 
00214             // Set and truncate name
00215             task->name = strcpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name);
00216             // name, priority, stack_size, msg_size, policy, cpus_allowed ( 1111 = 4 first cpus)
00217 
00218             // Set priority
00219             task->priority = priority;
00220 
00221             // Set rtai task struct to zero
00222             task->rtaitask = 0;
00223 
00224             RTAI_Thread* rt = (RTAI_Thread*)malloc( sizeof(RTAI_Thread) );
00225             rt->priority = priority;
00226             rt->data = obj;
00227             rt->wrapper = start_routine;
00228             rt->task = task;
00229             rt->tnum = task_num;
00230             int retv = pthread_create(&(task->thread), 0,
00231                                   rtai_thread_wrapper, rt);
00232             // poll for thread creation to be done.
00233             int timeout = 0;
00234             while ( task->rtaitask == 0 && ++timeout < 20)
00235                 usleep(100000);
00236             return timeout <  20 ? retv : -1;
00237         }
00238 
00239         INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* mytask) {
00240             if (mytask->rtaitask == 0)
00241                 return;
00242 
00243             rt_task_yield();
00244         }
00245 
00246         INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
00247             RT_TASK* self = rt_buddy();
00248             if (self == 0)
00249                 return -1; // non-rtai thread. We could try to compare pthreads like in gnulinux ?
00250             if ( self == task->rtaitask )
00251                 return 1;
00252             return 0;
00253         }
00254 
00255         INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
00256         {
00257             return 0;
00258         }
00259 
00260         INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00261         {
00262             if (*scheduler != SCHED_LXRT_HARD && *scheduler != SCHED_LXRT_SOFT ) {
00263                 log(Error) << "Unknown scheduler type." <<endlog();
00264                 *scheduler = SCHED_LXRT_SOFT;
00265                 return -1;
00266             }
00267             return 0;
00268         }
00269 
00270         INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* t, int s) {
00271             if ( t->rtaitask == 0 || t->rtaitask != rt_buddy() ) {
00272                 return -1;
00273             }
00274             if (rtos_task_check_scheduler(&s) == -1)
00275                 return -1;
00276             if (s == SCHED_LXRT_HARD)
00277                 rt_make_hard_real_time();
00278             else if ( s == SCHED_LXRT_SOFT)
00279                 rt_make_soft_real_time();
00280             return 0;
00281         }
00282 
00283         INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* t) {
00284             if ( rt_is_hard_real_time( t->rtaitask ) )
00285                 return SCHED_LXRT_HARD;
00286             return SCHED_LXRT_SOFT;
00287         }
00288 
00289         INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00290         {
00291             if (mytask->rtaitask == 0)
00292                 return;
00293             if (rt_buddy() == mytask->rtaitask) {
00294                 // do not suspend/resume my own thread
00295                 // do best effort to change period.
00296                 rtos_task_set_period(mytask,nanosecs);
00297                 return;
00298             }
00299             // other thread is instructing us:
00300             if (nanosecs == 0) {
00301                 // in RTAI, to drop from periodic to non periodic, do a
00302                 // suspend/resume cycle.
00303                 rt_task_suspend( mytask->rtaitask );
00304                 rt_task_resume( mytask->rtaitask );
00305             }
00306             else {
00307                 // same for the inverse
00308                 rt_task_suspend( mytask->rtaitask );
00309                 rt_task_make_periodic_relative_ns(mytask->rtaitask, 0, nanosecs);
00310             }
00311         }
00312 
00313         INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00314         {
00315             if (mytask->rtaitask == 0)
00316                 return;
00317             if ( nanosecs == 0 )
00318                 rt_set_period(mytask->rtaitask, 0 );
00319             else
00320                 rt_set_period(mytask->rtaitask, nano2count( nanosecs ));
00321         }
00322 
00323         INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00324         {
00325           // Do nothing
00326         }
00327 
00328         INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* mytask )
00329         {
00330             if (mytask->rtaitask == 0)
00331                 return -1;
00332             // only in RTAI 3.2, this returns overrun or not.
00333             // so do not use retval for compatibility reasons.
00334             rt_task_wait_period();
00335             return 0;
00336         }
00337 
00338         INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00339             if ( pthread_join((mytask->thread),0) != 0 )
00340                 Logger::log() << Logger::Critical << "Failed to join "<< mytask->name <<"."<< Logger::endl;
00341 
00342             free( mytask->name );
00343             // rt_task_delete is done in wrapper !
00344         }
00345 
00346         INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00347         {
00348             int ret = 0;
00349             // check scheduler first.
00350             ret = rtos_task_check_scheduler(scheduler);
00351 
00352             // correct priority
00353             // Hard & Soft:
00354             if (*priority < 0){
00355                 log(Warning) << "Forcing priority ("<<*priority<<") of thread to 0." <<endlog();
00356                 *priority = 0;
00357                 ret = -1;
00358             }
00359             if (*priority > 255){
00360                 log(Warning) << "Forcing priority ("<<*priority<<") of thread to 255." <<endlog();
00361                 *priority = 255;
00362                 ret = -1;
00363             }
00364             return ret;
00365         }
00366 
00367         INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * mytask, int priority)
00368         {
00369             int rv;
00370 
00371             if (mytask->rtaitask == 0)
00372                 return -1;
00373 
00374             // returns the *old* priority.
00375             rv = rt_change_prio( mytask->rtaitask, priority);
00376             if (rv == mytask->priority) {
00377                 mytask->priority = priority;
00378                 return 0;
00379             }
00380             return -1;
00381         }
00382 
00383         INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* t)
00384         {
00385             return t->name;
00386         }
00387 
00388         INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *t)
00389         {
00390             return t->priority;
00391         }
00392 
00393     INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00394     {
00395         return -1;
00396         }
00397 
00398     INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00399     {
00400         return ~0;
00401         }
00402     }
00403 }
00404 #undef INTERNAL_QUAL
00405 #endif