Orocos Real-Time Toolkit
2.5.0
|
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 <sys/time.h> 00045 #include <sys/resource.h> 00046 #ifdef ORO_OS_LINUX_CAP_NG 00047 #include <cap-ng.h> 00048 #endif 00049 #include <iostream> 00050 #include <cstdlib> 00051 using namespace std; 00052 00053 #define INTERNAL_QUAL 00054 00055 namespace RTT 00056 { namespace os { 00057 00058 INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main_task) 00059 { 00060 const char* name = "main"; 00061 main_task->wait_policy = ORO_WAIT_ABS; 00062 main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name); 00063 main_task->thread = pthread_self(); 00064 pthread_attr_init( &(main_task->attr) ); 00065 struct sched_param sp; 00066 sp.sched_priority=0; 00067 // Set priority 00068 // fixme check return value and bail out if necessary 00069 pthread_attr_setschedparam(&(main_task->attr), &sp); 00070 main_task->priority = sp.sched_priority; 00071 return 0; 00072 } 00073 00074 INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task) 00075 { 00076 pthread_attr_destroy( &(main_task->attr) ); 00077 free(main_task->name); 00078 return 0; 00079 } 00080 00081 INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task, 00082 int priority, 00083 unsigned cpu_affinity, 00084 const char * name, 00085 int sched_type, 00086 size_t stack_size, 00087 void * (*start_routine)(void *), 00088 ThreadInterface* obj) 00089 { 00090 int rv; // return value 00091 task->wait_policy = ORO_WAIT_ABS; 00092 rtos_task_check_priority( &sched_type, &priority ); 00093 // Save priority internally, since the pthread_attr* calls are broken ! 00094 // we will pick it up later in rtos_task_set_scheduler(). 00095 task->priority = priority; 00096 00097 // Set name 00098 if ( strlen(name) == 0 ) 00099 name = "Thread"; 00100 task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name); 00101 00102 if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){ 00103 return rv; 00104 } 00105 // Set scheduler type (_before_ assigning priorities!) 00106 if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){ 00107 return rv; 00108 } 00109 // Set stack size 00110 if (stack_size ) 00111 if ( (rv = pthread_attr_setstacksize(&(task->attr), stack_size)) != 0){ 00112 return rv; 00113 } 00114 pthread_attr_getschedpolicy(&(task->attr), &rv ); 00115 assert( rv == sched_type ); 00116 /* SCHED_OTHER tasks are always assigned static priority 0, see 00117 man sched_setscheduler 00118 */ 00119 struct sched_param sp; 00120 if (sched_type != SCHED_OTHER){ 00121 sp.sched_priority=priority; 00122 // Set priority 00123 if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){ 00124 return rv; 00125 } 00126 } 00127 rv = pthread_create(&(task->thread), &(task->attr), 00128 start_routine, obj); 00129 log(Debug) <<"Created Posix thread "<< task->thread <<endlog(); 00130 00131 if ( cpu_affinity != ~0 ) { 00132 log(Debug) << "Setting CPU affinity to " << cpu_affinity << endlog(); 00133 if (0 != rtos_task_set_cpu_affinity(task, cpu_affinity)) 00134 { 00135 log(Error) << "Failed to set CPU affinity to " << cpu_affinity << endlog(); 00136 } 00137 } 00138 00139 return rv; 00140 } 00141 00142 INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* t) { 00143 #if 0 00144 //under plain gnulinux, sched_yield may have little influence, so sleep 00145 // to force rescheduling of other threads. 00146 NANO_TIME timeRemaining = 1000; // 1ms 00147 TIME_SPEC ts( ticks2timespec( timeRemaining ) ); 00148 rtos_nanosleep( &ts , NULL ); 00149 #else 00150 int ret = sched_yield(); 00151 if ( ret != 0) 00152 perror("rtos_task_yield"); 00153 #endif 00154 } 00155 00156 INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) { 00157 pthread_t self = pthread_self(); 00158 if ( pthread_equal(self, task->thread) == 0 ) // zero means false. 00159 return 0; 00160 return 1; 00161 } 00162 00163 INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) { 00164 int policy = -1; 00165 struct sched_param param; 00166 // first check the argument 00167 if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 ) 00168 return -1; 00169 // if sched_type is different, the priority must change as well. 00170 if (pthread_getschedparam(task->thread, &policy, ¶m) == 0) { 00171 // now update the priority 00172 param.sched_priority = task->priority; 00173 rtos_task_check_priority( &sched_type, ¶m.sched_priority ); 00174 // write new policy: 00175 return pthread_setschedparam( task->thread, sched_type, ¶m); 00176 } 00177 return -1; 00178 } 00179 00180 INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* task) { 00181 int policy = -1; 00182 struct sched_param param; 00183 // first retrieve thread scheduling parameters: 00184 if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, ¶m) == 0) 00185 return policy; 00186 return -1; 00187 } 00188 00189 INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs ) 00190 { 00191 // set period 00192 mytask->period = nanosecs; 00193 // set next wake-up time. 00194 mytask->periodMark = ticks2timespec( nano2ticks( rtos_get_time_ns() + nanosecs ) ); 00195 } 00196 00197 INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs ) 00198 { 00199 rtos_task_make_periodic(mytask, nanosecs); 00200 } 00201 00202 INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy ) 00203 { 00204 task->wait_policy = policy; 00205 } 00206 00207 INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task ) 00208 { 00209 if ( task->period == 0 ) 00210 return 0; 00211 00212 // record this to detect overrun. 00213 NANO_TIME now = rtos_get_time_ns(); 00214 NANO_TIME wake= task->periodMark.tv_sec * 1000000000LL + task->periodMark.tv_nsec; 00215 00216 // inspired by nanosleep man page for this construct: 00217 while ( clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &(task->periodMark), NULL) != 0 && errno == EINTR ) { 00218 errno = 0; 00219 } 00220 00221 if (task->wait_policy == ORO_WAIT_ABS) 00222 { 00223 // program next period: 00224 // 1. convert period to timespec 00225 TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) ); 00226 // 2. Add ts to periodMark (danger: tn guards for overflows!) 00227 NANO_TIME tn = (task->periodMark.tv_nsec + ts.tv_nsec); 00228 task->periodMark.tv_nsec = tn % 1000000000LL; 00229 task->periodMark.tv_sec += ts.tv_sec + tn / 1000000000LL; 00230 } 00231 else 00232 { 00233 TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) ); 00234 TIME_SPEC now = ticks2timespec( rtos_get_time_ns() ); 00235 NANO_TIME tn = (now.tv_nsec + ts.tv_nsec); 00236 task->periodMark.tv_nsec = tn % 1000000000LL; 00237 task->periodMark.tv_sec = ts.tv_sec + now.tv_sec + tn / 1000000000LL; 00238 } 00239 00240 return now > wake ? -1 : 0; 00241 } 00242 00243 INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) { 00244 pthread_join( mytask->thread, 0); 00245 pthread_attr_destroy( &(mytask->attr) ); 00246 free(mytask->name); 00247 } 00248 00249 INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler) 00250 { 00251 #ifdef ORO_OS_LINUX_CAP_NG 00252 if(capng_get_caps_process()) { 00253 log(Error) << "Failed to retrieve capabilities (lowering to SCHED_OTHER)." <<endlog(); 00254 *scheduler = SCHED_OTHER; 00255 return -1; 00256 } 00257 #endif 00258 00259 if (*scheduler != SCHED_OTHER && geteuid() != 0 00260 #ifdef ORO_OS_LINUX_CAP_NG 00261 && capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE)==0 00262 #endif 00263 ) { 00264 // they're not root and they want a real-time priority, which _might_ 00265 // be acceptable if they're using pam_limits and have set the rtprio ulimit 00266 // (see "/etc/security/limits.conf" and "ulimit -a") 00267 struct rlimit r; 00268 if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur)) 00269 { 00270 log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog(); 00271 *scheduler = SCHED_OTHER; 00272 return -1; 00273 } 00274 } 00275 00276 if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) { 00277 log(Error) << "Unknown scheduler type." <<endlog(); 00278 *scheduler = SCHED_OTHER; 00279 return -1; 00280 } 00281 return 0; 00282 } 00283 00284 INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority) 00285 { 00286 int ret = 0; 00287 // check scheduler first. 00288 ret = rtos_task_check_scheduler(scheduler); 00289 00290 // correct priority 00291 if (*scheduler == SCHED_OTHER) { 00292 if ( *priority != 0 ) { 00293 if (*priority != LowestPriority) 00294 log(Warning) << "Forcing priority ("<<*priority<<") of thread with SCHED_OTHER policy to 0." <<endlog(); 00295 *priority = 0; 00296 ret = -1; 00297 } 00298 } else { 00299 // SCHED_FIFO/SCHED_RR: 00300 if (*priority <= 0){ 00301 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 1." <<endlog(); 00302 *priority = 1; 00303 ret = -1; 00304 } 00305 if (*priority > 99){ 00306 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 99." <<endlog(); 00307 *priority = 99; 00308 ret = -1; 00309 } 00310 // and limit them according to pam_Limits (only if not root) 00311 if ( geteuid() != 0 00312 #ifdef ORO_OS_LINUX_CAP_NG 00313 && !capng_have_capability(CAPNG_EFFECTIVE, CAP_SYS_NICE) 00314 #endif 00315 ) 00316 { 00317 struct rlimit r; 00318 if (0 == getrlimit(RLIMIT_RTPRIO, &r)) 00319 { 00320 if (*priority > (int)r.rlim_cur) 00321 { 00322 log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to the pam_limit of " << r.rlim_cur <<endlog(); 00323 *priority = r.rlim_cur; 00324 ret = -1; 00325 } 00326 } 00327 else 00328 { 00329 // this should not be possible, but do something intelligent 00330 *priority = 1; 00331 ret = -1; 00332 } 00333 } 00334 } 00335 return ret; 00336 } 00337 00338 INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority) 00339 { 00340 int policy = 0; 00341 struct sched_param param; 00342 // first retrieve thread scheduling parameters: 00343 if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, ¶m) == 0) { 00344 if ( rtos_task_check_priority( &policy, &priority ) != 0 ) 00345 return -1; 00346 param.sched_priority = priority; 00347 task->priority = priority; // store for set_scheduler 00348 // write new policy: 00349 return pthread_setschedparam( task->thread, policy, ¶m); 00350 } 00351 return -1; 00352 } 00353 00354 INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *task) 00355 { 00356 // if sched_other, return the 'virtual' priority 00357 int policy = 0; 00358 struct sched_param param; 00359 // first retrieve thread scheduling parameters: 00360 if ( task == 0 ) 00361 return -1; 00362 if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, ¶m) != 0) 00363 return task->priority; 00364 return param.sched_priority; 00365 } 00366 00367 INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity) 00368 { 00369 if ( cpu_affinity == 0 ) // clears the mask. 00370 cpu_affinity = ~0; 00371 if( task && task->thread != 0 ) { 00372 cpu_set_t cs; 00373 CPU_ZERO(&cs); 00374 for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) 00375 { 00376 if(cpu_affinity & (1 << i)) { CPU_SET(i, &cs); } 00377 } 00378 return pthread_setaffinity_np(task->thread, sizeof(cs), &cs); 00379 } 00380 return -1; 00381 } 00382 00383 INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task) 00384 { 00385 if( task && task->thread != 0) { 00386 unsigned cpu_affinity = 0; 00387 cpu_set_t cs; 00388 pthread_getaffinity_np(task->thread, sizeof(cs), &cs); 00389 for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) 00390 { 00391 if(CPU_ISSET(i, &cs)) { cpu_affinity |= (1 << i); } 00392 } 00393 return cpu_affinity; 00394 } 00395 return ~0; 00396 } 00397 00398 INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task) 00399 { 00400 return task->name; 00401 } 00402 00403 } 00404 } 00405 #undef INTERNAL_QUAL