fosi_internal.cpp
Go to the documentation of this file.
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 unsigned int rtos_task_get_pid(const RTOS_TASK* task)
00268         {
00269                 return 0;
00270         }
00271 
00272         INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00273         {
00274         return -1;
00275         }
00276 
00277         INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00278         {
00279         return ~0;
00280         }
00281 
00282         INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
00283         {
00284             return task->name;
00285         }
00286 
00287     }
00288 }
00289 
00290 // opaque type to hide C object from C code
00291 typedef struct rt_mutex_impl_t
00292 {
00293     RTT::os::Mutex mutex;
00294 };
00295 
00296 int rtos_mutex_init(rt_mutex_t* m)
00297 {
00298     assert(m);
00299     *m = new rt_mutex_impl_t;   // non-realtime
00300     return (0 != (*m));
00301 }
00302 
00303 int rtos_mutex_destroy(rt_mutex_t* m )
00304 {
00305     assert(m);
00306     assert(*m);
00307     delete (*m);               // non-realtime
00308     (*m) = 0;
00309     return 0;
00310 }
00311 
00312 int rtos_mutex_lock( rt_mutex_t* m)
00313 {
00314     assert(m);
00315     assert(*m);
00316     (*m)->mutex.lock();
00317     return 0;
00318 }
00319 
00320 int rtos_mutex_unlock( rt_mutex_t* m)
00321 {
00322     assert(m);
00323     assert(*m);
00324     (*m)->mutex.unlock();
00325     return 0;
00326 }
00327 
00328 #undef INTERNAL_QUAL


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