Orocos Real-Time Toolkit  2.8.3
fosi_internal.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  copyright : (C) 2008 Klaas Gadeyne
3  email : firstname dot lastname at gmail dot com
4 
5  ***************************************************************************
6  * This library is free software; you can redistribute it and/or *
7  * modify it under the terms of the GNU General Public *
8  * License as published by the Free Software Foundation; *
9  * version 2 of the License. *
10  * *
11  * As a special exception, you may use this file as part of a free *
12  * software library without restriction. Specifically, if other files *
13  * instantiate templates or use macros or inline functions from this *
14  * file, or you compile this file and link it with other files to *
15  * produce an executable, this file does not by itself cause the *
16  * resulting executable to be covered by the GNU General Public *
17  * License. This exception does not however invalidate any other *
18  * reasons why the executable file might be covered by the GNU General *
19  * Public License. *
20  * *
21  * This library is distributed in the hope that it will be useful, *
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
24  * Lesser General Public License for more details. *
25  * *
26  * You should have received a copy of the GNU General Public *
27  * License along with this library; if not, write to the Free Software *
28  * Foundation, Inc., 59 Temple Place, *
29  * Suite 330, Boston, MA 02111-1307 USA *
30  * *
31  ***************************************************************************/
32 
33 
34 #include "../ThreadInterface.hpp"
35 #include "fosi.h"
36 #include "../fosi_internal_interface.hpp"
37 #include "../../Logger.hpp"
38 #include <cassert>
39 #include "../Mutex.hpp"
40 
41 #define INTERNAL_QUAL
42 
43 namespace RTT
44 { namespace os {
45 
47  {
48  const char* name = "main";
49  main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
50  main_task->thread = pthread_self();
51  pthread_attr_init( &(main_task->attr) );
52  struct sched_param sp;
53  sp.sched_priority=0;
54  // Set priority
55  // fixme check return value and bail out if necessary
56  pthread_attr_setschedparam(&(main_task->attr), &sp);
57  main_task->priority = sp.sched_priority;
58  main_task->wait_policy = ORO_WAIT_ABS;
59  return 0;
60  }
61 
63  {
64  pthread_attr_destroy( &(main_task->attr) );
65  free( main_task->name );
66  main_task->name = NULL;
67  return 0;
68  }
69 
71  int priority,
72  unsigned cpu_affinity,
73  const char * name,
74  int sched_type,
75  size_t stack_size,
76  void * (*start_routine)(void *),
77  ThreadInterface* obj)
78  {
79  int rv; // return value
80  rtos_task_check_priority( &sched_type, &priority );
81  // Save priority internally, since the pthread_attr* calls are broken !
82  // we will pick it up later in rtos_task_set_scheduler().
83  task->priority = priority;
84  task->wait_policy = ORO_WAIT_ABS;
85 
86  // Set name
87  if ( strlen(name) == 0 )
88  name = "Thread";
89  task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
90 
91  if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){
92  return rv;
93  }
94  // Set scheduler type (_before_ assigning priorities!)
95  if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){
96  return rv;
97  }
98  pthread_attr_getschedpolicy(&(task->attr), &rv );
99  assert( rv == sched_type );
100 
101  struct sched_param sp;
102  sp.sched_priority=priority;
103  // Set priority
104  if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){
105  return rv;
106  }
107  rv = pthread_create(&(task->thread), &(task->attr), start_routine, obj);
108  log(Debug) <<"Created Posix thread "<< task->thread <<endlog();
109  return rv;
110  }
111 
113  {
114  int ret = sched_yield();
115  if ( ret != 0)
116  perror("rtos_task_yield");
117  }
118 
119  INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
120  pthread_t self = pthread_self();
121  if ( pthread_equal(self, task->thread) == 0 ) // zero means false.
122  return 0;
123  return 1;
124  }
125 
126 
127  INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type)
128  {
129  int policy = -1;
130  struct sched_param param;
131  // first check the argument
132  if ( task && task->thread != 0 && rtos_task_check_scheduler( &sched_type) == -1 )
133  return -1;
134  // if sched_type is different, the priority must change as well.
135  if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
136  // now update the priority
137  param.sched_priority = task->priority;
138  rtos_task_check_priority( &sched_type, &param.sched_priority );
139  // write new policy:
140  return pthread_setschedparam( task->thread, sched_type, &param);
141  }
142  return -1;
143  }
144 
146  {
147  int policy = -1;
148  struct sched_param param;
149  // first retrieve thread scheduling parameters:
150  if ( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0)
151  return policy;
152  return -1;
153  }
154 
156  {
157  // set period
158  mytask->period = nanosecs;
159  // set next wake-up time.
160  mytask->periodMark = rtos_get_time_ns() + nanosecs;
161  }
162 
163  INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
164  {
165  mytask->period = nanosecs;
166  mytask->periodMark = rtos_get_time_ns() + nanosecs;
167  }
168 
170  {
171  task->wait_policy = policy;
172  }
173 
175  {
176  if ( task->period == 0 )
177  return 0;
178 
179  //rtos_printf("Time is %lld nsec, Mark is %lld nsec.\n",rtos_get_time_ns(), task->periodMark );
180  // CALCULATE in nsecs
181  NANO_TIME timeRemaining = task->periodMark - rtos_get_time_ns();
182 
183  // next wake-up time :
184  if (task->wait_policy == ORO_WAIT_ABS)
185  {
186  task->periodMark += task->period;
187  }
188  else
189  {
190  task->periodMark = rtos_get_time_ns() + task->period;
191  }
192 
193  if ( timeRemaining > 0 ) {
194  //rtos_printf("Waiting for %lld nsec\n",timeRemaining);
195  TIME_SPEC ts( ticks2timespec( timeRemaining ) );
196  rtos_nanosleep( &ts , NULL );
197  return 0;
198  }
199 
200  return -1;
201  }
202 
204  {
205  pthread_join( mytask->thread, 0);
206  pthread_attr_destroy( &(mytask->attr) );
207  free(mytask->name);
208  mytask->name = NULL;
209  }
210 
211  INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
212  {
213  if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
214  log(Error) << "Unknown scheduler type." <<endlog();
215  *scheduler = SCHED_OTHER;
216  return -1;
217  }
218  return 0;
219  }
220 
221  INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
222  {
223  int ret = 0;
224  // check scheduler first.
225  ret = rtos_task_check_scheduler(scheduler);
226 
227  if (*priority < 0){
228  log(Warning) << "Forcing priority ("<<*priority<<") of thread to 0." <<endlog();
229  *priority = 0;
230  ret = -1;
231  }
232  if (*priority > 63){
233  log(Warning) << "Forcing priority ("<<*priority<<") of thread to 63." <<endlog();
234  *priority = 63;
235  ret = -1;
236  }
237  return ret;
238  }
239 
240  INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
241  {
242  int policy = 0;
243  struct sched_param param;
244  // first retrieve thread scheduling parameters:
245  if( task && task->thread != 0 && pthread_getschedparam(task->thread, &policy, &param) == 0) {
246  if ( rtos_task_check_priority( &policy, &priority ) != 0 )
247  return -1;
248  param.sched_priority = priority;
249  task->priority = priority; // store for set_scheduler
250  // write new policy:
251  return pthread_setschedparam( task->thread, policy, &param);
252  }
253  return -1;
254  }
255 
257  {
258  // if sched_other, return the 'virtual' priority
259  int policy = 0;
260  struct sched_param param;
261  // first retrieve thread scheduling parameters:
262  if ( task == 0 )
263  return -1;
264  if ( task->thread == 0 || pthread_getschedparam(task->thread, &policy, &param) != 0)
265  return task->priority;
266  return param.sched_priority;
267  }
268 
269  INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
270  {
271  return 0;
272  }
273 
274  INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
275  {
276  return -1;
277  }
278 
280  {
281  return ~0;
282  }
283 
284  INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* task)
285  {
286  return task->name ? task->name : "(destroyed)";
287  }
288 
289  }
290 }
291 
292 // opaque type to hide C object from C code
293 typedef struct rt_mutex_impl_t
294 {
296 };
297 
299 {
300  assert(m);
301  *m = new rt_mutex_impl_t; // non-realtime
302  return (0 != (*m));
303 }
304 
306 {
307  assert(m);
308  assert(*m);
309  delete (*m); // non-realtime
310  (*m) = 0;
311  return 0;
312 }
313 
315 {
316  assert(m);
317  assert(*m);
318  (*m)->mutex.lock();
319  return 0;
320 }
321 
323 {
324  assert(m);
325  assert(*m);
326  (*m)->mutex.unlock();
327  return 0;
328 }
329 
330 #undef INTERNAL_QUAL
int rtos_task_is_self(const RTOS_TASK *task)
Returns 1 when task is the task struct of the thread calling this function, 0 otherwise.
long long NANO_TIME
Definition: fosi.h:55
int priority
Definition: fosi.h:80
int rtos_task_get_priority(const RTOS_TASK *task)
Return the priority of a thread.
int wait_policy
Definition: fosi.h:81
INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK *mytask, NANO_TIME nanosecs)
This function is to inform the RTOS that a thread is switching between periodic or non-periodic execu...
INTERNAL_QUAL int rtos_task_check_priority(int *scheduler, int *priority)
This function checks (and corrects) a given priority within a given scheduler type.
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
Returns the process ID the OS gave to the task task.
RTT::os::Mutex mutex
#define ORO_WAIT_ABS
Definition: fosi.h:52
struct rt_mutex_impl_t rt_mutex_impl_t
Definition: fosi.h:240
NANO_TIME rtos_get_time_ns(void)
Get "system" time in nanoseconds.
Definition: fosi.h:113
int rtos_nanosleep(const TIME_SPEC *rqtp, TIME_SPEC *rmtp)
Definition: fosi.h:180
char * name
Definition: fosi.h:66
INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK *main_task)
Cleanup the main thread.
int rtos_task_set_scheduler(RTOS_TASK *t, int sched_type)
Set the scheduler of a given task t to a the type sched_type.
int rtos_mutex_init(rt_mutex_t *m)
int rtos_task_set_cpu_affinity(RTOS_TASK *task, unsigned cpu_affinity)
Set the cpu affinity of a thread.
unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
Return the cpu affinity of a thread.
cyg_thread thread
Definition: fosi.h:64
TIME_SPEC ticks2timespec(TICK_TIME hrt)
Definition: fosi.h:146
NANO_TIME periodMark
Definition: fosi.h:82
int rtos_mutex_destroy(rt_mutex_t *m)
void rtos_task_set_wait_period_policy(RTOS_TASK *task, int policy)
Set the wait policy of a thread.
void rtos_task_delete(RTOS_TASK *mytask)
This function must join the thread created with rtos_task_create and then clean up the RTOS_TASK stru...
int rtos_task_set_priority(RTOS_TASK *task, int priority)
Set the priority of a thread.
const char * rtos_task_get_name(const RTOS_TASK *task)
Returns the name by which a task is known in the RTOS.
int rtos_task_check_scheduler(int *sched_type)
This function checks (and corrects) if the given sched_type is valid for this RTOS.
int rtos_task_wait_period(RTOS_TASK *task)
This function is called by a periodic thread which wants to go to sleep and wake up the next period...
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK *)
Yields the current thread.
int rtos_mutex_lock(rt_mutex_t *m)
An object oriented wrapper around a non recursive mutex.
Definition: Mutex.hpp:82
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:51
INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK *main_task)
Initialise the main thread.
NANO_TIME period
Definition: fosi.h:84
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
Change the period of a periodic RTOS task.
Definition: fosi.h:62
struct timespec TIME_SPEC
Definition: fosi.h:97
int rtos_mutex_unlock(rt_mutex_t *m)
#define INTERNAL_QUAL
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)
Create a thread.
cyg_mutex_t rt_mutex_t
Definition: fosi.h:185
int rtos_task_get_scheduler(const RTOS_TASK *t)
Returns the current scheduler set for task t.
pthread_attr_t attr
Definition: fosi.h:73