Orocos Real-Time Toolkit  2.8.3
fosi_internal.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Peter Soetens Sat May 21 20:15:51 CEST 2005 fosi_internal.hpp
3 
4  fosi_internal.hpp - description
5  -------------------
6  begin : Sat May 21 2005
7  copyright : (C) 2005 Peter Soetens
8  email : peter.soetens@mech.kuleuven.ac.be
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU General Public *
13  * License as published by the Free Software Foundation; *
14  * version 2 of the License. *
15  * *
16  * As a special exception, you may use this file as part of a free *
17  * software library without restriction. Specifically, if other files *
18  * instantiate templates or use macros or inline functions from this *
19  * file, or you compile this file and link it with other files to *
20  * produce an executable, this file does not by itself cause the *
21  * resulting executable to be covered by the GNU General Public *
22  * License. This exception does not however invalidate any other *
23  * reasons why the executable file might be covered by the GNU General *
24  * Public License. *
25  * *
26  * This library is distributed in the hope that it will be useful, *
27  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
28  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
29  * Lesser General Public License for more details. *
30  * *
31  * You should have received a copy of the GNU General Public *
32  * License along with this library; if not, write to the Free Software *
33  * Foundation, Inc., 59 Temple Place, *
34  * Suite 330, Boston, MA 02111-1307 USA *
35  * *
36  ***************************************************************************/
37 
38 
39 #include "../ThreadInterface.hpp"
40 #include "fosi.h"
41 #include "../fosi_internal_interface.hpp"
42 #include "../../Logger.hpp"
43 #include <cassert>
44 #include <iostream>
45 #include <cstdlib>
46 using namespace std;
47 
48 
49 
50 
51 #define INTERNAL_QUAL
52 
53 namespace RTT
54 { namespace os {
55 
56 void ErrorHandler(LPTSTR lpszFunction)
57 {
58  // Retrieve the system error message for the last-error code.
59 
60  LPVOID lpMsgBuf;
61  LPVOID lpDisplayBuf;
62  DWORD dw = GetLastError();
63 
64  FormatMessage(
65  FORMAT_MESSAGE_ALLOCATE_BUFFER |
66  FORMAT_MESSAGE_FROM_SYSTEM |
67  FORMAT_MESSAGE_IGNORE_INSERTS,
68  NULL,
69  dw,
70  MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
71  (LPTSTR) &lpMsgBuf,
72  0, NULL );
73 
74  // Display the error message.
75 
76  lpDisplayBuf = (LPVOID)LocalAlloc(LMEM_ZEROINIT,
77  (lstrlen((LPCTSTR) lpMsgBuf) + lstrlen((LPCTSTR) lpszFunction) + 40) * sizeof(TCHAR));
78  /*StringCchPrintf((LPTSTR)lpDisplayBuf,
79  LocalSize(lpDisplayBuf) / sizeof(TCHAR),
80  TEXT("%s failed with error %d: %s"),
81  lpszFunction, dw, lpMsgBuf);
82  */
83  MessageBox(NULL, (LPCTSTR) lpDisplayBuf, TEXT("Error"), MB_OK);
84 
85  // Free error-handling buffer allocations.
86 
87  LocalFree(lpMsgBuf);
88  LocalFree(lpDisplayBuf);
89 }
90 
91 
93  void* (*realThread)(void*);
94  void* realData;
95  };
96 
97  DWORD WINAPI ThreadWrapper(void* threadData)
98  {
99  ThreadWrapperData* threadWrapperData =
100  (ThreadWrapperData*) threadData;
101 
102  void* (*realThread)(void*) = threadWrapperData->realThread;
103  void* realData = threadWrapperData->realData;
104  delete threadWrapperData;
105 
106  return (DWORD) realThread(realData);
107  }
108 
109 
110  //TODO
112  {
113  const char* name = "main";
114  main_task->wait_policy = ORO_WAIT_ABS;
115  main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
116  main_task->threadId = GetCurrentThreadId();
117  main_task->handle = 0;
118  //pthread_attr_init( &(main_task->attr) );
119  //struct sched_param sp;
120  //sp.sched_priority=0;
121  // Set priority
122  // fixme check return value and bail out if necessary
123  //pthread_attr_setschedparam(&(main_task->attr), &sp);
124  //main_task->priority = sp.sched_priority;
125  return 0;
126  }
127 
129  {
130  //pthread_attr_destroy( &(main_task->attr) );
131  // printf("rtos_task_delete_main");
132  free(main_task->name);
133  main_task->name = NULL;
134  return 0;
135  }
136 
137 
139  int priority,
140  unsigned cpu_affinity,
141  const char * name,
142  int sched_type,
143  size_t stack_size,
144  void * (*start_routine)(void *),
145  ThreadInterface* obj)
146  {
147  //int rv; // return value
148  // TODO implement scheduler by using CreateProcess
149  // Set name
150  task->wait_policy = ORO_WAIT_ABS;
151  if (name == 0 || strlen(name) == 0)
152  name = "Thread";
153  task->name = strncpy((char*) malloc((strlen(name) + 1)
154  * sizeof(char)), name, strlen(name) + 1);
155 
157  data->realThread = start_routine;
158  data->realData = obj;
159 
160  task->handle = CreateThread(NULL, 2*1000*1000, ThreadWrapper, data, 0,
161  &task->threadId);
162 
163  if (task->handle == NULL)
164  {
165  ErrorHandler(TEXT("CreateThread"));
166  ExitProcess(3);
167  }
168 
169  SetThreadPriority(task->handle, priority);
170 
171  return 0;
172  }
173 
174  INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* mytask) {
175  // printf("T:%u -> ", (unsigned int) mytask);
176  // printf(" yield \n ");
177  Sleep(0);
178  }
179 
180  INTERNAL_QUAL int rtos_task_is_self(const RTOS_TASK* task) {
181  DWORD self = GetCurrentThreadId();
182  if ( self == task->threadId )
183  return 1;
184  return 0;
185  }
186 
187  INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* task, int sched_type) {
188 
189  //int policy = -1;
190  //struct sched_param param;
191  // first check the argument
192  if ( task && task->handle != NULL && rtos_task_check_scheduler( &sched_type) == -1 )
193  return -1;
194  // if sched_type is different, the priority must change as well.
195  //if (pthread_getschedparam(task->thread, &policy, &param) == 0) {
196  // now update the priority
197  // param.sched_priority = task->priority;
198  // rtos_task_check_priority( &sched_type, &param.sched_priority );
199  // write new policy:
200  // return pthread_setschedparam( task->thread, sched_type, &param);
201  //}
202  // TODO: change the scheduling of the thread
203  task->sched_type = sched_type;
204  return 0;
205  }
206 
208  return task->sched_type;
209  }
210 
211  INTERNAL_QUAL unsigned int rtos_task_get_pid(const RTOS_TASK* task)
212  {
213  return 0;
214  }
215 
217  {
218  // set period
219  mytask->period = nanosecs;
220  // set next wake-up time.
221  mytask->periodMark = rtos_get_time_ns() + nanosecs;
222  }
223 
224  INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
225  {
226  mytask->period = nanosecs;
227  mytask->periodMark = rtos_get_time_ns() + nanosecs;
228  }
229 
231  return t->period;
232  }
233 
235  {
236  task->wait_policy = policy;
237  }
238 
240  {
241  if ( task->period == 0 )
242  return 0;
243 
244  // rtos_printf("Time is %lld nsec, Mark is %lld nsec.\n",rtos_get_time_ns(), task->periodMark );
245  // CALCULATE in nsecs
246  NANO_TIME timeRemaining = task->periodMark - rtos_get_time_ns();
247 
248  if ( timeRemaining > 0 ) {
249  TIME_SPEC ts( ticks2timespec( timeRemaining ) );
250  rtos_nanosleep( &ts , NULL );
251  }
252  // else
253  // rtos_printf( "GNULinux task did not get deadline !\n" );
254 
255  // next wake-up time :
256  if (task->wait_policy == ORO_WAIT_ABS)
257  task->periodMark += task->period;
258  else
259  task->periodMark = rtos_get_time_ns() + task->period;
260 
261  return 0;
262  }
263 
265  // printf("T:%u -> ", (unsigned int) mytask);
266  //printf(" rtos_task_delete ");
267  //DWORD exitCode;
268  //TerminateThread(mytask->handle, exitCode);
269  WaitForSingleObject( mytask->handle, INFINITE );
270 
271  CloseHandle(mytask->handle);
272  free(mytask->name);
273  mytask->name = NULL;
274  mytask->handle = NULL;
275 
276  // printf(" success \n ");
277  };
278 
279  INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
280  {
281 
282  /*if (*scheduler != SCHED_OTHER && geteuid() != 0 ) {
283  // they're not root and they want a real-time priority, which _might_
284  // be acceptable if they're using pam_limits and have set the rtprio ulimit
285  // (see "/etc/security/limits.conf" and "ulimit -a")
286  struct rlimit r;
287  if ((0 != getrlimit(RLIMIT_RTPRIO, &r)) || (0 == r.rlim_cur))
288  {
289  log(Warning) << "Lowering scheduler type to SCHED_OTHER for non-privileged users.." <<endlog();
290  *scheduler = SCHED_OTHER;
291  return -1;
292  }
293  }
294  if (*scheduler != SCHED_OTHER && *scheduler != SCHED_FIFO && *scheduler != SCHED_RR ) {
295  log(Error) << "Unknown scheduler type." <<endlog();
296  *scheduler = SCHED_OTHER;
297  return -1;
298  }*/
299  return 0;
300  }
301 
302  INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
303  {
304  int ret = 0;
305  // TODO: check scheduler first.
306  //ret = rtos_task_check_scheduler(scheduler);
307 
308  if (*priority <= -15){
309  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to -15." <<endlog();
310  *priority = -15;
311  ret = -1;
312  }
313  if (*priority > 15){
314  log(Warning) << "Forcing priority ("<<*priority<<") of thread with !SCHED_OTHER policy to 15." <<endlog();
315  *priority = 15;
316  ret = -1;
317  }
318  return ret;
319  }
320 
321  INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK * task, int priority)
322  {
323  // FIXME, only works on the current task
324  // init the scheduler. The rt_task_initschmod code is broken, so we do it ourselves.
325  SetThreadPriority(task->handle, priority);
326  return 0;
327  }
328 
330  {
331  return GetThreadPriority(t->handle);
332  }
333 
334  INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
335  {
336  return -1;
337  }
338 
340  {
341  return ~0;
342  }
343 
344  INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* t)
345  {
346  /* printf("Get Name: ");
347  if( t->name!=NULL ){
348  printf("%s",t->name);
349  } else {
350  printf("ERRROR TASK HAS ALLREADY BEEN DELETED");
351  }
352  */
353 
354  return t->name ? t->name : "(destroyed)";
355  }
356 
357  }
358 }
359 #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
DWORD WINAPI ThreadWrapper(void *threadData)
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.
cyg_handle_t handle
Definition: fosi.h:69
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
Returns the process ID the OS gave to the task task.
#define ORO_WAIT_ABS
Definition: fosi.h:52
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 sched_type
Definition: fosi.h:93
STL namespace.
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.
TIME_SPEC ticks2timespec(TICK_TIME hrt)
Definition: fosi.h:146
NANO_TIME periodMark
Definition: fosi.h:82
A thread which is being run.
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.
void *(* realThread)(void *)
const char * rtos_task_get_name(const RTOS_TASK *task)
Returns the name by which a task is known in the RTOS.
void ErrorHandler(LPTSTR lpszFunction)
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.
DWORD threadId
Definition: fosi.h:88
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.
#define INTERNAL_QUAL
Definition: fosi.h:62
struct timespec TIME_SPEC
Definition: fosi.h:97
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.
INTERNAL_QUAL NANO_TIME rtos_task_get_period(const RTOS_TASK *t)
int rtos_task_get_scheduler(const RTOS_TASK *t)
Returns the current scheduler set for task t.