Orocos Real-Time Toolkit  2.5.0
fosi_internal.cpp
00001 /***************************************************************************
00002     copyright            : (C) 2008 Klaas Gadeyne
00003     email                : firstname dot lastname at gmail dot com
00004 
00005     ***************************************************************************
00006     *   This library is free software; you can redistribute it and/or         *
00007     *   modify it under the terms of the GNU General Public                   *
00008     *   License as published by the Free Software Foundation;                 *
00009     *   version 2 of the License.                                             *
00010     *                                                                         *
00011     *   As a special exception, you may use this file as part of a free       *
00012     *   software library without restriction.  Specifically, if other files   *
00013     *   instantiate templates or use macros or inline functions from this     *
00014     *   file, or you compile this file and link it with other files to        *
00015     *   produce an executable, this file does not by itself cause the         *
00016     *   resulting executable to be covered by the GNU General Public          *
00017     *   License.  This exception does not however invalidate any other        *
00018     *   reasons why the executable file might be covered by the GNU General   *
00019     *   Public License.                                                       *
00020     *                                                                         *
00021     *   This library is distributed in the hope that it will be useful,       *
00022     *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00023     *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00024     *   Lesser General Public License for more details.                       *
00025     *                                                                         *
00026     *   You should have received a copy of the GNU General Public             *
00027     *   License along with this library; if not, write to the Free Software   *
00028     *   Foundation, Inc., 59 Temple Place,                                    *
00029     *   Suite 330, Boston, MA  02111-1307  USA                                *
00030     *                                                                         *
00031     ***************************************************************************/
00032 
00033 
00034 #include "../ThreadInterface.hpp"
00035 #include "fosi.h"
00036 #include "../fosi_internal_interface.hpp"
00037 #include "../../Logger.hpp"
00038 #include <cassert>
00039 #include "../Mutex.hpp"
00040 
00041 #define INTERNAL_QUAL
00042 
00043 namespace RTT
00044 { namespace os {
00045 
00046     INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main_task)
00047     {
00048         const char* name = "main";
00049         main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00050         main_task->thread = pthread_self();
00051         pthread_attr_init( &(main_task->attr) );
00052         struct sched_param sp;
00053         sp.sched_priority=0;
00054         // Set priority
00055         // fixme check return value and bail out if necessary
00056         pthread_attr_setschedparam(&(main_task->attr), &sp);
00057         main_task->priority = sp.sched_priority;
00058         main_task->wait_policy = ORO_WAIT_ABS;
00059         return 0;
00060     }
00061 
00062     INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task)
00063     {
00064         pthread_attr_destroy( &(main_task->attr) );
00065         free( main_task->name );
00066         return 0;
00067     }
00068 
00069     INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
00070                        int priority,
00071                        unsigned cpu_affinity,
00072                        const char * name,
00073                        int sched_type,
00074                        size_t stack_size,
00075                        void * (*start_routine)(void *),
00076                        ThreadInterface* obj)
00077     {
00078             int rv; // return value
00079             rtos_task_check_priority( &sched_type, &priority );
00080             // Save priority internally, since the pthread_attr* calls are broken !
00081             // we will pick it up later in rtos_task_set_scheduler().
00082             task->priority = priority;
00083             task->wait_policy = ORO_WAIT_ABS;
00084 
00085         // Set name
00086         if ( strlen(name) == 0 )
00087                 name = "Thread";
00088             task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00089 
00090         if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){
00091                 return rv;
00092         }
00093         // Set scheduler type (_before_ assigning priorities!)
00094         if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
00095                 return rv;
00096         }
00097             pthread_attr_getschedpolicy(&(task->attr), &rv );
00098             assert( rv == sched_type );
00099 
00100             struct sched_param sp;
00101             sp.sched_priority=priority;
00102             // Set priority
00103             if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){
00104                 return rv;
00105             }
00106         rv = pthread_create(&(task->thread), &(task->attr), start_routine, obj);
00107             log(Debug) <<"Created Posix thread "<< task->thread <<endlog();
00108             return rv;
00109     }
00110 
00111     INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* t)
00112     {
00113             int ret = sched_yield();
00114             if ( ret != 0)
00115             perror("rtos_task_yield");
00116     }
00117 
00118     INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
00119         pthread_t self = pthread_self();
00120         if ( pthread_equal(self, task->thread) == 0 ) // zero means false.
00121             return 0;
00122         return 1;
00123     }
00124 
00125 
00126         INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type)
00127         {
00128             int policy = -1;
00129             struct sched_param param;
00130             // first check the argument
00131             if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
00132             return -1;
00133             // if sched_type is different, the priority must change as well.
00134             if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
00135                 // now update the priority
00136                 param.sched_priority = task->priority;
00137                 rtos_task_check_priority( &sched_type, &param.sched_priority );
00138                 // write new policy:
00139                 return pthread_setschedparam( task->thread, sched_type, &param);
00140             }
00141             return -1;
00142         }
00143 
00144         INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* task)
00145         {
00146             int policy = -1;
00147             struct sched_param param;
00148             // first retrieve thread scheduling parameters:
00149             if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0)
00150                 return policy;
00151             return -1;
00152         }
00153 
00154     INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00155     {
00156         // set period
00157         mytask->period = nanosecs;
00158         // set next wake-up time.
00159         mytask->periodMark = rtos_get_time_ns() + nanosecs;
00160     }
00161 
00162     INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00163     {
00164         mytask->period = nanosecs;
00165         mytask->periodMark = rtos_get_time_ns() + nanosecs;
00166     }
00167 
00168   INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00169   {
00170     task->wait_policy = policy;
00171   }
00172 
00173     INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
00174     {
00175         if ( task->period == 0 )
00176         return 0;
00177 
00178         //rtos_printf("Time is %lld nsec, Mark is %lld nsec.\n",rtos_get_time_ns(), task->periodMark );
00179         // CALCULATE in nsecs
00180         NANO_TIME timeRemaining = task->periodMark - rtos_get_time_ns();
00181 
00182         // next wake-up time :
00183       if (task->wait_policy == ORO_WAIT_ABS)
00184       {
00185         task->periodMark += task->period;
00186       }
00187       else
00188       {
00189         task->periodMark = rtos_get_time_ns() + task->period;
00190       }
00191 
00192         if ( timeRemaining > 0 ) {
00193             //rtos_printf("Waiting for %lld nsec\n",timeRemaining);
00194             TIME_SPEC ts( ticks2timespec( timeRemaining ) );
00195             rtos_nanosleep( &ts , NULL );
00196             return 0;
00197         }
00198 
00199         return -1;
00200     }
00201 
00202     INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask)
00203     {
00204             pthread_join( mytask->thread, 0);
00205             pthread_attr_destroy( &(mytask->attr) );
00206         free(mytask->name);
00207     }
00208 
00209         INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00210         {
00211             if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
00212                 log(Error) << "Unknown scheduler type." <<endlog();
00213                 *scheduler = SCHED_OTHER;
00214                 return -1;
00215             }
00216             return 0;
00217         }
00218 
00219     INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00220         {
00221             int ret = 0;
00222             // check scheduler first.
00223             ret = rtos_task_check_scheduler(scheduler);
00224 
00225             if (*priority < 0){
00226                 log(Warning) << "Forcing priority ("<<*priority<<") of thread to 0." <<endlog();
00227                 *priority = 0;
00228                 ret = -1;
00229             }
00230             if (*priority > 63){
00231                 log(Warning) << "Forcing priority ("<<*priority<<") of thread to 63." <<endlog();
00232                 *priority = 63;
00233                 ret = -1;
00234             }
00235             return ret;
00236         }
00237 
00238     INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
00239     {
00240             int policy = 0;
00241             struct sched_param param;
00242             // first retrieve thread scheduling parameters:
00243             if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0) {
00244                 if ( rtos_task_check_priority( &policy, &priority ) != 0 )
00245                     return -1;
00246                 param.sched_priority = priority;
00247             task->priority = priority; // store for set_scheduler
00248             // write new policy:
00249             return pthread_setschedparam( task->thread, policy, &param);
00250             }
00251             return -1;
00252         }
00253 
00254     INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *task)
00255     {
00256             // if sched_other, return the 'virtual' priority
00257             int policy = 0;
00258             struct sched_param param;
00259             // first retrieve thread scheduling parameters:
00260             if ( task == 0 )
00261                 return -1;
00262             if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, &param) != 0)
00263                 return task->priority;
00264             return param.sched_priority;
00265     }
00266 
00267     INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00268     {
00269         return -1;
00270         }
00271 
00272     INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00273     {
00274         return ~0;
00275         }
00276 
00277     INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
00278     {
00279         return task->name;
00280     }
00281 
00282     }
00283 }
00284 
00285 // opaque type to hide C object from C code
00286 typedef struct rt_mutex_impl_t
00287 {
00288     RTT::os::Mutex mutex;
00289 };
00290 
00291 int rtos_mutex_init(rt_mutex_t* m)
00292 {
00293     assert(m);
00294     *m = new rt_mutex_impl_t;   // non-realtime
00295     return (0 != (*m));
00296 }
00297 
00298 int rtos_mutex_destroy(rt_mutex_t* m )
00299 {
00300     assert(m);
00301     assert(*m);
00302     delete (*m);               // non-realtime
00303     (*m) = 0;
00304     return 0;
00305 }
00306 
00307 int rtos_mutex_lock( rt_mutex_t* m)
00308 {
00309     assert(m);
00310     assert(*m);
00311     (*m)->mutex.lock();
00312     return 0;
00313 }
00314 
00315 int rtos_mutex_unlock( rt_mutex_t* m)
00316 {
00317     assert(m);
00318     assert(*m);
00319     (*m)->mutex.unlock();
00320     return 0;
00321 }
00322 
00323 #undef INTERNAL_QUAL