Orocos Real-Time Toolkit  2.6.0
fosi_internal.cpp
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 
00039 #include "../ThreadInterface.hpp"
00040 #include "fosi.h"
00041 #include "../fosi_internal_interface.hpp"
00042 #include "../../Logger.hpp"
00043 #include <cassert>
00044 #include <iostream>
00045 #include <cstdlib>
00046 using namespace std;
00047 
00048 
00049 
00050 
00051 #define INTERNAL_QUAL
00052 
00053 namespace RTT
00054 { namespace os {
00055 
00056 void ErrorHandler(LPTSTR lpszFunction)
00057 {
00058     // Retrieve the system error message for the last-error code.
00059 
00060     LPVOID lpMsgBuf;
00061     LPVOID lpDisplayBuf;
00062     DWORD dw = GetLastError();
00063 
00064     FormatMessage(
00065         FORMAT_MESSAGE_ALLOCATE_BUFFER |
00066         FORMAT_MESSAGE_FROM_SYSTEM |
00067         FORMAT_MESSAGE_IGNORE_INSERTS,
00068         NULL,
00069         dw,
00070         MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
00071         (LPTSTR) &lpMsgBuf,
00072         0, NULL );
00073 
00074     // Display the error message.
00075 
00076     lpDisplayBuf = (LPVOID)LocalAlloc(LMEM_ZEROINIT,
00077         (lstrlen((LPCTSTR) lpMsgBuf) + lstrlen((LPCTSTR) lpszFunction) + 40) * sizeof(TCHAR));
00078     /*StringCchPrintf((LPTSTR)lpDisplayBuf,
00079         LocalSize(lpDisplayBuf) / sizeof(TCHAR),
00080         TEXT("%s failed with error %d: %s"),
00081         lpszFunction, dw, lpMsgBuf);
00082         */
00083     MessageBox(NULL, (LPCTSTR) lpDisplayBuf, TEXT("Error"), MB_OK);
00084 
00085     // Free error-handling buffer allocations.
00086 
00087     LocalFree(lpMsgBuf);
00088     LocalFree(lpDisplayBuf);
00089 }
00090 
00091 
00092     struct ThreadWrapperData {
00093           void* (*realThread)(void*);
00094           void* realData;
00095     };
00096 
00097     DWORD WINAPI ThreadWrapper(void* threadData)
00098     {
00099         ThreadWrapperData* threadWrapperData =
00100                 (ThreadWrapperData*) threadData;
00101 
00102         void* (*realThread)(void*) = threadWrapperData->realThread;
00103         void* realData = threadWrapperData->realData;
00104         delete threadWrapperData;
00105 
00106         return (DWORD) realThread(realData);
00107     }
00108 
00109 
00110     //TODO
00111     INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main_task)
00112     {
00113         const char* name = "main";
00114         main_task->wait_policy = ORO_WAIT_ABS;
00115         main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00116         main_task->threadId = GetCurrentThreadId();
00117         main_task->handle = 0;
00118         //pthread_attr_init( &(main_task->attr) );
00119         //struct sched_param sp;
00120         //sp.sched_priority=0;
00121         // Set priority
00122         // fixme check return value and bail out if necessary
00123         //pthread_attr_setschedparam(&(main_task->attr), &sp);
00124         //main_task->priority = sp.sched_priority;
00125         return 0;
00126     }
00127 
00128     INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task)
00129     {
00130         //pthread_attr_destroy( &(main_task->attr) );
00131         // printf("rtos_task_delete_main");
00132         free(main_task->name);
00133         return 0;
00134     }
00135 
00136 
00137     INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
00138                        int priority,
00139                        unsigned cpu_affinity,
00140                        const char * name,
00141                        int sched_type,
00142                        size_t stack_size,
00143                        void * (*start_routine)(void *),
00144                        ThreadInterface* obj)
00145     {
00146         //int rv; // return value
00147         // TODO implement scheduler by using CreateProcess
00148         // Set name
00149         task->wait_policy = ORO_WAIT_ABS;
00150         if (name == 0 || strlen(name) == 0)
00151             name = "Thread";
00152         task->name = strncpy((char*) malloc((strlen(name) + 1)
00153                                            * sizeof(char)), name, strlen(name) + 1);
00154 
00155         ThreadWrapperData* data = new ThreadWrapperData;
00156         data->realThread = start_routine;
00157         data->realData = obj;
00158 
00159         task->handle = CreateThread(NULL, 2*1000*1000, ThreadWrapper, data, 0,
00160                                     &task->threadId);
00161 
00162         if (task->handle == NULL)
00163         {
00164            ErrorHandler(TEXT("CreateThread"));
00165            ExitProcess(3);
00166         }
00167 
00168         SetThreadPriority(task->handle, priority);
00169 
00170         return 0;
00171     }
00172 
00173     INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* mytask) {
00174         // printf("T:%u -> ", (unsigned int) mytask);
00175         // printf(" yield \n ");
00176         Sleep(0);
00177     }
00178 
00179     INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
00180         DWORD self = GetCurrentThreadId();
00181         if ( self == task->threadId )
00182             return 1;
00183         return 0;
00184     }
00185 
00186     INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) {
00187 
00188         //int policy = -1;
00189         //struct sched_param param;
00190         // first check the argument
00191         if ( task && task->handle != NULL && rtos_task_check_scheduler( &sched_type) == -1 )
00192             return -1;
00193         // if sched_type is different, the priority must change as well.
00194         //if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
00195             // now update the priority
00196         //    param.sched_priority = task->priority;
00197         //    rtos_task_check_priority( &sched_type, &param.sched_priority );
00198             // write new policy:
00199         //    return pthread_setschedparam( task->thread, sched_type, &param);
00200         //}
00201         // TODO: change the scheduling of the thread
00202         task->sched_type = sched_type;
00203         return 0;
00204     }
00205 
00206     INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* task) {
00207         return task->sched_type;
00208     }
00209 
00210     INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
00211     {
00212         return 0;
00213     }
00214 
00215     INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00216     {
00217       // set period
00218       mytask->period = nanosecs;
00219       // set next wake-up time.
00220       mytask->periodMark = rtos_get_time_ns() + nanosecs;
00221     }
00222 
00223     INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00224     {
00225       mytask->period = nanosecs;
00226       mytask->periodMark = rtos_get_time_ns() + nanosecs;
00227     }
00228 
00229     INTERNAL_QUAL NANO_TIME rtos_task_get_period(const RTOS_TASK* t) {
00230       return t->period;
00231     }
00232 
00233     INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00234     {
00235       task->wait_policy = policy;
00236     }
00237 
00238     INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
00239     {
00240       if ( task->period == 0 )
00241           return 0;
00242 
00243       // rtos_printf("Time is %lld nsec, Mark is %lld nsec.\n",rtos_get_time_ns(), task->periodMark );
00244       // CALCULATE in nsecs
00245       NANO_TIME timeRemaining = task->periodMark - rtos_get_time_ns();
00246 
00247       if ( timeRemaining > 0 ) {
00248         TIME_SPEC ts( ticks2timespec( timeRemaining ) );
00249         rtos_nanosleep( &ts , NULL );
00250       }
00251       //             else
00252       //                 rtos_printf( "GNULinux task did not get deadline !\n" );
00253 
00254       // next wake-up time :
00255       if (task->wait_policy == ORO_WAIT_ABS)
00256         task->periodMark += task->period;
00257       else
00258         task->periodMark = rtos_get_time_ns() + task->period;
00259 
00260       return 0;
00261     }
00262 
00263     INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00264       // printf("T:%u -> ", (unsigned int) mytask);
00265       //printf(" rtos_task_delete ");
00266       //DWORD exitCode;
00267       //TerminateThread(mytask->handle, exitCode);
00268       WaitForSingleObject( mytask->handle, INFINITE );
00269 
00270       CloseHandle(mytask->handle);
00271       free(mytask->name);
00272       mytask->name = NULL;
00273       mytask->handle = NULL;
00274 
00275       // printf(" success \n ");
00276     };
00277 
00278     INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00279     {
00280 
00281         /*if (*scheduler != SCHED_OTHER && geteuid() != 0 ) {
00282             // they're not root and they want a real-time priority, which _might_
00283             // be acceptable if they're using pam_limits and have set the rtprio ulimit
00284             // (see "/etc/security/limits.conf" and "ulimit -a")
00285             struct rlimit   r;
00286             if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
00287             {
00288                 log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog();
00289                 *scheduler = SCHED_OTHER;
00290                 return -1;
00291             }
00292         }
00293         if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
00294             log(Error) << "Unknown scheduler type." <<endlog();
00295             *scheduler = SCHED_OTHER;
00296             return -1;
00297         }*/
00298         return 0;
00299     }
00300 
00301     INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00302     {
00303         int ret = 0;
00304         // TODO: check scheduler first.
00305         //ret = rtos_task_check_scheduler(scheduler);
00306 
00307         if (*priority <= -15){
00308             log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to -15." <<endlog();
00309             *priority = -15;
00310             ret = -1;
00311         }
00312         if (*priority > 15){
00313             log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 15." <<endlog();
00314             *priority = 15;
00315             ret = -1;
00316         }
00317         return ret;
00318     }
00319 
00320     INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
00321     {
00322       // FIXME, only works on the current task
00323       // init the scheduler. The rt_task_initschmod code is broken, so we do it ourselves.
00324         SetThreadPriority(task->handle, priority);
00325         return 0;
00326     }
00327 
00328     INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *t)
00329     {
00330         return GetThreadPriority(t->handle);
00331     }
00332 
00333     INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00334     {
00335     return -1;
00336     }
00337 
00338     INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00339     {
00340     return ~0;
00341     }
00342 
00343     INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* t)
00344     {
00345         /* printf("Get Name: ");
00346         if( t->name!=NULL ){
00347             printf("%s",t->name);
00348         } else {
00349             printf("ERRROR TASK HAS ALLREADY BEEN DELETED");
00350         }
00351         */
00352 
00353         return t->name;
00354     }
00355 
00356     }
00357 }
00358 #undef INTERNAL_QUAL