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         if (rv != 0) {
00158             log(Error) << "Failed to create thread " << task->name << ": "
00159                        << strerror(rv) << endlog();
00160             return rv;
00161         }
00162 
00163         // Set thread name to match task name, to help with debugging
00164         {
00165             // trim the name to fit 16 bytes restriction (including terminating
00166             // \0 character) of pthread_setname_np
00167             static const int MAX_THREAD_NAME_SIZE = 15;
00168             const char *thread_name = task->name;
00169             std::size_t thread_name_len = strlen(thread_name);
00170             if (thread_name_len > MAX_THREAD_NAME_SIZE) {
00171                 thread_name += thread_name_len - MAX_THREAD_NAME_SIZE;
00172             }
00173             int result = pthread_setname_np(task->thread, thread_name);
00174             if (result != 0) {
00175                 log(Error) << "Failed to set thread name for " << task->name << ": "
00176                            << strerror(result) << endlog();
00177             }
00178         }
00179 
00180         if ( cpu_affinity != (unsigned)~0 ) {
00181             log(Debug) << "Setting CPU affinity to " << cpu_affinity << endlog();
00182             int result = rtos_task_set_cpu_affinity(task, cpu_affinity);
00183             if (result != 0) {
00184                 log(Error) << "Failed to set CPU affinity to " << cpu_affinity << " for " << task->name << ": "
00185                            << strerror(result) << endlog();
00186             }
00187         }
00188 
00189         return rv;
00190         }
00191 
00192         INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* t) {
00193 #if 0
00194         //under plain gnulinux, sched_yield may have little influence, so sleep
00195         // to force rescheduling of other threads.
00196             NANO_TIME timeRemaining = 1000; // 1ms
00197                 TIME_SPEC ts( ticks2timespec( timeRemaining ) );
00198                 rtos_nanosleep( &ts , NULL );
00199 #else
00200         int ret = sched_yield();
00201         if ( ret != 0)
00202             perror("rtos_task_yield");
00203 #endif
00204         }
00205 
00206         INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
00207         {
00208                 if (task)
00209                         return task->pid;
00210                 return 0;
00211         }
00212 
00213         INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
00214             pthread_t self = pthread_self();
00215             if ( pthread_equal(self, task->thread) == 0 ) // zero means false.
00216                 return 0;
00217             return 1;
00218         }
00219 
00220     INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) {
00221         int policy = -1;
00222         struct sched_param param;
00223         // first check the argument
00224         if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
00225             return -1;
00226         // if sched_type is different, the priority must change as well.
00227         if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
00228             // now update the priority
00229             param.sched_priority = task->priority;
00230             rtos_task_check_priority( &sched_type, &param.sched_priority );
00231             // write new policy:
00232             return pthread_setschedparam( task->thread, sched_type, &param);
00233         }
00234         return -1;
00235     }
00236 
00237     INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* task) {
00238         int policy = -1;
00239         struct sched_param param;
00240         // first retrieve thread scheduling parameters:
00241         if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0)
00242             return policy;
00243         return -1;
00244     }
00245 
00246         INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00247         {
00248             // set period
00249             mytask->period = nanosecs;
00250             // set next wake-up time.
00251             mytask->periodMark = ticks2timespec( nano2ticks( rtos_get_time_ns() + nanosecs ) );
00252         }
00253 
00254         INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00255         {
00256         rtos_task_make_periodic(mytask, nanosecs);
00257         }
00258 
00259   INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00260   {
00261     task->wait_policy = policy;
00262   }
00263 
00264         INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
00265         {
00266             if ( task->period == 0 )
00267             return 0;
00268 
00269         // record this to detect overrun.
00270             NANO_TIME now = rtos_get_time_ns();
00271             NANO_TIME wake= task->periodMark.tv_sec * 1000000000LL + task->periodMark.tv_nsec;
00272 
00273         // inspired by nanosleep man page for this construct:
00274         while ( clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &(task->periodMark), NULL) != 0 && errno == EINTR ) {
00275             errno = 0;
00276         }
00277 
00278         if (task->wait_policy == ORO_WAIT_ABS)
00279         {
00280           // in the case of overrun by more than 4 periods,
00281           // skip all the updates before now, with the next update aligned to period
00282           int maxDelayInPeriods = 4;
00283           NANO_TIME period = task->period;
00284           if (now - wake > maxDelayInPeriods*period) {
00285             period = period * ((now - wake) / period);
00286           }
00287           // program next period:
00288           // 1. convert period to timespec
00289           TIME_SPEC ts = ticks2timespec( nano2ticks(period) );
00290           // 2. Add ts to periodMark (danger: tn guards for overflows!)
00291           NANO_TIME tn = (task->periodMark.tv_nsec + ts.tv_nsec);
00292           task->periodMark.tv_nsec = tn % 1000000000LL;
00293           task->periodMark.tv_sec += ts.tv_sec + tn / 1000000000LL;
00294         }
00295         else
00296         {
00297           TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
00298           TIME_SPEC now = ticks2timespec( rtos_get_time_ns() );
00299           NANO_TIME tn = (now.tv_nsec + ts.tv_nsec);
00300           task->periodMark.tv_nsec = tn % 1000000000LL;
00301           task->periodMark.tv_sec = ts.tv_sec + now.tv_sec + tn / 1000000000LL;
00302         }
00303 
00304             return now > wake ? -1 : 0;
00305         }
00306 
00307         INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00308         pthread_join( mytask->thread, 0);
00309         pthread_attr_destroy( &(mytask->attr) );
00310             free(mytask->name);
00311         }
00312 
00313     INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00314     {
00315 #ifdef ORO_OS_LINUX_CAP_NG
00316         if(capng_get_caps_process()) {
00317             log(Error) << "Failed to retrieve capabilities (lowering to SCHED_OTHER)." <<endlog();
00318             *scheduler = SCHED_OTHER;
00319             return -1;
00320         }
00321 #endif
00322 
00323         if (*scheduler != SCHED_OTHER && geteuid() != 0
00324 #ifdef ORO_OS_LINUX_CAP_NG
00325             && capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)==0
00326 #endif
00327             ) {
00328             // they're not root and they want a real-time priority, which _might_
00329             // be acceptable if they're using pam_limits and have set the rtprio ulimit
00330             // (see "/etc/security/limits.conf" and "ulimit -a")
00331             struct rlimit r;
00332             if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
00333             {
00334                 log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog();
00335                 *scheduler = SCHED_OTHER;
00336                 return -1;
00337             }
00338         }
00339 
00340         if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
00341             log(Error) << "Unknown scheduler type." <<endlog();
00342             *scheduler = SCHED_OTHER;
00343             return -1;
00344         }
00345         return 0;
00346     }
00347 
00348         INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00349     {
00350         int ret = 0;
00351         // check scheduler first.
00352         ret = rtos_task_check_scheduler(scheduler);
00353 
00354         // correct priority
00355         if (*scheduler == SCHED_OTHER) {
00356             if ( *priority != 0 ) {
00357                 if (*priority != LowestPriority)
00358                     log(Warning) << "Forcing priority ("<<*priority<<") of thread with SCHED_OTHER policy to 0." <<endlog();
00359                 *priority = 0;
00360                 ret = -1;
00361             }
00362         } else {
00363             // SCHED_FIFO/SCHED_RR:
00364             if (*priority <= 0){
00365                 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 1." <<endlog();
00366                 *priority = 1;
00367                 ret = -1;
00368             }
00369             if (*priority > 99){
00370                 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 99." <<endlog();
00371                 *priority = 99;
00372                 ret = -1;
00373             }
00374             // and limit them according to pam_Limits (only if not root)
00375             if ( geteuid() != 0 
00376 #ifdef ORO_OS_LINUX_CAP_NG
00377                  && !capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)
00378 #endif
00379                  )
00380             {
00381                 struct rlimit   r;
00382                 if (0 == getrlimit(RLIMIT_RTPRIO, &r))
00383                 {
00384                     if (*priority > (int)r.rlim_cur)
00385                     {
00386                         log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to the pam_limit of " << r.rlim_cur <<endlog();
00387                         *priority = r.rlim_cur;
00388                         ret = -1;
00389                     }
00390                 }
00391                 else
00392                 {
00393                     // this should not be possible, but do something intelligent
00394                     *priority = 1;
00395                     ret = -1;
00396                 }
00397             }
00398         }
00399         return ret;
00400     }
00401 
00402         INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
00403         {
00404         int policy = 0;
00405         struct sched_param param;
00406         // first retrieve thread scheduling parameters:
00407         if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0) {
00408             if ( rtos_task_check_priority( &policy, &priority ) != 0 )
00409                 return -1;
00410             param.sched_priority = priority;
00411             task->priority = priority; // store for set_scheduler
00412             // write new policy:
00413             return pthread_setschedparam( task->thread, policy, &param);
00414         }
00415         return -1;
00416     }
00417 
00418         INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *task)
00419         {
00420         // if sched_other, return the 'virtual' priority
00421         int policy = 0;
00422         struct sched_param param;
00423         // first retrieve thread scheduling parameters:
00424         if ( task == 0 )
00425             return -1;
00426         if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, &param) != 0)
00427             return task->priority;
00428         return param.sched_priority;
00429         }
00430 
00431         INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00432         {
00433         if ( cpu_affinity == 0 ) // clears the mask.
00434             cpu_affinity = ~0;
00435         if( task && task->thread != 0 ) {
00436             cpu_set_t cs;
00437             CPU_ZERO(&cs);
00438             for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
00439                 {
00440                     if(cpu_affinity & (1 << i)) { CPU_SET(i, &cs); }
00441                 }
00442             return pthread_setaffinity_np(task->thread, sizeof(cs), &cs);
00443         }
00444         return -1;
00445     }
00446 
00447         INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00448         {
00449         if( task && task->thread != 0) {
00450         unsigned cpu_affinity = 0;
00451         cpu_set_t cs;
00452         pthread_getaffinity_np(task->thread, sizeof(cs), &cs);
00453         for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++)
00454         {
00455           if(CPU_ISSET(i, &cs)) { cpu_affinity |= (1 << i); }
00456         }
00457         return cpu_affinity;
00458          }
00459         return ~0;
00460         }
00461 
00462         INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
00463         {
00464             return task->name;
00465         }
00466 
00467     }
00468 }
00469 #undef INTERNAL_QUAL


rtt
Author(s): RTT Developers
autogenerated on Fri Sep 9 2016 04:01:52