INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
{
int policy = 0;
struct sched_param param;
// first retrieve thread scheduling parameters:
if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, ¶m) == 0) {
if ( rtos_task_check_priority( &policy, &priority ) != 0 )
return -1;
param.sched_priority = priority;
task->priority = priority; // store for set_scheduler
// write new policy:
return pthread_setschedparam( task->thread, policy, ¶m);
}
return -1;
}
INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) {
int policy = -1;
struct sched_param param;
// first check the argument
if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
return -1;
// if sched_type is different, the priority must change as well.
if (pthread_getschedparam(task->thread, &policy, ¶m) == 0) {
// now update the priority
param.sched_priority = task->priority;
rtos_task_check_priority( &sched_type, ¶m.sched_priority );
// write new policy:
return pthread_setschedparam( task->thread, sched_type, ¶m);
}
return -1;
}
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
int priority,
const char * name,
int sched_type,
size_t stack_size,
void * (*start_routine)(void *),
ThreadInterface* obj)
{
int rv; // return value
rtos_task_check_priority( &sched_type, &priority );
// Save priority internally, since the pthread_attr* calls are broken !
// we will pick it up later in rtos_task_set_scheduler().
task->priority = priority;
// Set name
if ( strlen(name) == 0 )
name = "Thread";
task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){
return rv;
}
// Set scheduler type (_before_ assigning priorities!)
if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
return rv;
}
pthread_attr_getschedpolicy(&(task->attr), &rv );
assert( rv == sched_type );
struct sched_param sp;
sp.sched_priority=priority;
// Set priority
if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){
return rv;
}
rv = pthread_create(&(task->thread), &(task->attr), start_routine, obj);
log(Debug) <<"Created Posix thread "<< task->thread <<endlog();
return rv;
}
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
int priority,
unsigned cpu_affinity,
const char* name,
int sched_type,
size_t stack_size,
void * (*start_routine)(void *),
ThreadInterface* obj)
{
rtos_task_check_priority(&sched_type, &priority);
XenoCookie* xcookie = (XenoCookie*)malloc( sizeof(XenoCookie) );
xcookie->data = obj;
xcookie->wrapper = start_routine;
if ( name == 0 || strlen(name) == 0)
name = "XenoThread";
task->name = strncpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name, strlen(name)+1 );
task->sched_type = sched_type; // User requested scheduler.
int rv;
unsigned int aff = 0;
if ( cpu_affinity != 0 ) {
// calculate affinity:
for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) {
if(cpu_affinity & (1 << i)) {
// RTHAL_NR_CPUS is defined in the kernel, not in user space. So we just limit up to 7, until Xenomai allows us to get the maximum.
if ( i > 7 ) {
const unsigned int all_cpus = ~0;
if ( cpu_affinity != all_cpus ) // suppress this warning when ~0 is provided
log(Warning) << "rtos_task_create: ignoring cpu_affinity for "<< name << " on CPU " << i << " since it's larger than RTHAL_NR_CPUS - 1 (="<< 7 <<")"<<endlog();
} else {
aff |= T_CPU(i);
}
}
}
}
if (stack_size == 0) {
log(Debug) << "Raizing default stack size to 128kb for Xenomai threads in Orocos." <<endlog();
stack_size = 128000;
}
// task, name, stack, priority, mode, fun, arg
// UGLY, how can I check in Xenomai that a name is in use before calling rt_task_spawn ???
rv = rt_task_spawn(&(task->xenotask), name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
if ( rv == -EEXIST ) {
free( task->name );
task->name = strncpy( (char*)malloc( (strlen(name)+2)*sizeof(char) ), name, strlen(name)+1 );
task->name[ strlen(name) ] = '0';
task->name[ strlen(name)+1 ] = 0;
while ( rv == -EEXIST && task->name[ strlen(name) ] != '9') {
task->name[ strlen(name) ] += 1;
rv = rt_task_spawn(&(task->xenotask), task->name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
}
}
if ( rv == -EEXIST ) {
log(Warning) << name << ": an object with that name is already existing in Xenomai." << endlog();
rv = rt_task_spawn(&(task->xenotask), 0, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
}
if ( rv != 0) {
log(Error) << name << " : CANNOT INIT Xeno TASK " << task->name <<" error code: "<< rv << endlog();
return rv;
}
rt_task_yield();
return 0;
}
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
int priority,
unsigned cpu_affinity,
const char * name,
int sched_type,
size_t stack_size,
void * (*start_routine)(void *),
ThreadInterface* obj)
{
int rv; // return value
task->wait_policy = ORO_WAIT_ABS;
rtos_task_check_priority( &sched_type, &priority );
// Save priority internally, since the pthread_attr* calls are broken !
// we will pick it up later in rtos_task_set_scheduler().
task->priority = priority;
PosixCookie* xcookie = (PosixCookie*)malloc( sizeof(PosixCookie) );
xcookie->data = obj;
xcookie->wrapper = start_routine;
// Set name
if ( strlen(name) == 0 )
name = "Thread";
task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){
return rv;
}
// Set scheduler type (_before_ assigning priorities!)
if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
return rv;
}
// Set stack size
if (stack_size )
if ( (rv = pthread_attr_setstacksize(&(task->attr), stack_size)) != 0){
return rv;
}
pthread_attr_getschedpolicy(&(task->attr), &rv );
assert( rv == sched_type );
/* SCHED_OTHER tasks are always assigned static priority 0, see
man sched_setscheduler
*/
struct sched_param sp;
if (sched_type != SCHED_OTHER){
sp.sched_priority=priority;
// Set priority
if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){
return rv;
}
}
rv = pthread_create(&(task->thread), &(task->attr),
rtos_posix_thread_wrapper, xcookie);
if (rv != 0) {
log(Error) << "Failed to create thread " << task->name << ": "
<< strerror(rv) << endlog();
return rv;
}
// Set thread name to match task name, to help with debugging
{
// trim the name to fit 16 bytes restriction (including terminating
// \0 character) of pthread_setname_np
static const int MAX_THREAD_NAME_SIZE = 15;
const char *thread_name = task->name;
std::size_t thread_name_len = strlen(thread_name);
if (thread_name_len > MAX_THREAD_NAME_SIZE) {
thread_name += thread_name_len - MAX_THREAD_NAME_SIZE;
}
int result = pthread_setname_np(task->thread, thread_name);
if (result != 0) {
log(Error) << "Failed to set thread name for " << task->name << ": "
<< strerror(result) << endlog();
}
}
if ( cpu_affinity != (unsigned)~0 ) {
log(Debug) << "Setting CPU affinity to " << cpu_affinity << endlog();
int result = rtos_task_set_cpu_affinity(task, cpu_affinity);
if (result != 0) {
log(Error) << "Failed to set CPU affinity to " << cpu_affinity << " for " << task->name << ": "
<< strerror(result) << endlog();
}
}
return rv;
}
