fosi_internal.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Peter Soetens  Sat May 21 20:15:51 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 #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             // Set priority
00071             // fixme check return value and bail out if necessary
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         // store 'self'
00093         RTOS_TASK* task = ((ThreadInterface*)((PosixCookie*)cookie)->data)->getTask();
00094         task->pid = syscall(SYS_gettid);
00095         assert( task->pid );
00096 
00097         // call user function
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; // return value
00115         task->wait_policy = ORO_WAIT_ABS;
00116         rtos_task_check_priority( &sched_type, &priority );
00117         // Save priority internally, since the pthread_attr* calls are broken !
00118         // we will pick it up later in rtos_task_set_scheduler().
00119         task->priority = priority;
00120 
00121         PosixCookie* xcookie = (PosixCookie*)malloc( sizeof(PosixCookie) );
00122         xcookie->data = obj;
00123         xcookie->wrapper = start_routine;
00124 
00125             // Set name
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             // Set scheduler type (_before_ assigning priorities!)
00134             if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
00135             return rv;
00136             }
00137         // Set stack size
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             /* SCHED_OTHER tasks are always assigned static priority 0, see
00145                man sched_setscheduler
00146             */
00147         struct sched_param sp;
00148             if (sched_type != SCHED_OTHER){
00149             sp.sched_priority=priority;
00150             // Set priority
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 != (unsigned)~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         //under plain gnulinux, sched_yield may have little influence, so sleep
00172         // to force rescheduling of other threads.
00173             NANO_TIME timeRemaining = 1000; // 1ms
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 ) // zero means false.
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         // first check the argument
00201         if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
00202             return -1;
00203         // if sched_type is different, the priority must change as well.
00204         if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
00205             // now update the priority
00206             param.sched_priority = task->priority;
00207             rtos_task_check_priority( &sched_type, &param.sched_priority );
00208             // write new policy:
00209             return pthread_setschedparam( task->thread, sched_type, &param);
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         // first retrieve thread scheduling parameters:
00218         if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 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             // set period
00226             mytask->period = nanosecs;
00227             // set next wake-up time.
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         // record this to detect overrun.
00247             NANO_TIME now = rtos_get_time_ns();
00248             NANO_TIME wake= task->periodMark.tv_sec * 1000000000LL + task->periodMark.tv_nsec;
00249 
00250         // inspired by nanosleep man page for this construct:
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           // program next period:
00258           // 1. convert period to timespec
00259           TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
00260           // 2. Add ts to periodMark (danger: tn guards for overflows!)
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             // they're not root and they want a real-time priority, which _might_
00299             // be acceptable if they're using pam_limits and have set the rtprio ulimit
00300             // (see "/etc/security/limits.conf" and "ulimit -a")
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         // check scheduler first.
00322         ret = rtos_task_check_scheduler(scheduler);
00323 
00324         // correct priority
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             // SCHED_FIFO/SCHED_RR:
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             // and limit them according to pam_Limits (only if not root)
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                     // this should not be possible, but do something intelligent
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         // first retrieve thread scheduling parameters:
00377         if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0) {
00378             if ( rtos_task_check_priority( &policy, &priority ) != 0 )
00379                 return -1;
00380             param.sched_priority = priority;
00381             task->priority = priority; // store for set_scheduler
00382             // write new policy:
00383             return pthread_setschedparam( task->thread, policy, &param);
00384         }
00385         return -1;
00386     }
00387 
00388         INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *task)
00389         {
00390         // if sched_other, return the 'virtual' priority
00391         int policy = 0;
00392         struct sched_param param;
00393         // first retrieve thread scheduling parameters:
00394         if ( task == 0 )
00395             return -1;
00396         if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, &param) != 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 ) // clears the mask.
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


rtt
Author(s): RTT Developers
autogenerated on Wed Aug 26 2015 16:15:48