Thread.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Peter Soetens Mon May 10 19:10:28 CEST 2004 Thread.cxx
3 
4  Thread.cxx - description
5  -------------------
6  begin : Mon May 10 2004
7  copyright : (C) 2004 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 
39 #include "Thread.hpp"
40 #include "../Time.hpp"
41 #include "threads.hpp"
42 #include "../Logger.hpp"
43 #include "MutexLock.hpp"
44 #include "MainThread.hpp"
45 
46 #include "../rtt-config.h"
47 #include "../internal/CatchConfig.hpp"
48 
49 #ifdef OROPKG_OS_THREAD_SCOPE
50 # include "../extras/dev/DigitalOutInterface.hpp"
51 #define SCOPE_INIT(name)
52 #define SCOPE_ON if ( task->d ) task->d->switchOn( bit );
53 #define SCOPE_OFF if ( task->d ) task->d->switchOff( bit );
54 #elif defined(HAVE_LTTNG_UST) && defined(OROPKG_OS_GNULINUX)
56 #define SCOPE_INIT(name) tracepoint(orocos_rtt, thread_init , name);
57 #define SCOPE_ON tracepoint(orocos_rtt, thread_scope, 1);
58 #define SCOPE_OFF tracepoint(orocos_rtt, thread_scope, 0);
59 #else
60 #define SCOPE_INIT(name)
61 #define SCOPE_ON
62 #define SCOPE_OFF
63 #endif
64 
65 namespace RTT {
66  namespace os
67  {
68  using RTT::Logger;
69 
70  unsigned int Thread::default_stack_size = 0;
71 
73 
75 
76  void Thread::setStackSize(unsigned int ssize) { default_stack_size = ssize; }
77 
78  void Thread::setLockTimeoutNoPeriod(double timeout_in_s) { lock_timeout_no_period_in_s = timeout_in_s; }
79 
81 
82  void *thread_function(void* t)
83  {
87  Thread* task = static_cast<os::Thread*> (t);
88  Logger::In in(task->getName());
89 
90  SCOPE_INIT(task->getName())
91 
92  task->configure();
93 
94  // signal to setup() that we're created.
95  rtos_sem_signal(&(task->sem));
96 
97  // This lock forces setup(), which holds the lock, to continue.
98  { MutexLock lock(task->breaker); }
99 #ifdef OROPKG_OS_THREAD_SCOPE
100  // order thread scope toggle bit on thread number
101  unsigned int bit = task->threadNumber();
102 #endif
103  SCOPE_OFF
104 
105  int overruns = 0, cur_sched = task->msched_type;
106  NANO_TIME cur_period = task->period;
107 
108  while (!task->prepareForExit)
109  {
110  TRY(
114  while (1)
115  {
116  if (!task->active || (task->active && task->period == 0) || !task->running )
117  {
118  // consider this the 'configuration or waiting for command state'
119  if (task->period != 0) {
120  overruns = 0;
121  // drop out of periodic mode:
122  rtos_task_set_period(task->getTask(), 0);
123  }
124  rtos_sem_wait(&(task->sem)); // wait for command.
125  task->configure(); // check for reconfigure
126  if (task->prepareForExit) // check for exit
127  {
128  break; // break while(1) {}
129  }
130  // end of configuration
131  }
132  // This is the running state. Only enter if a task is running
133  if ( task->running )
134  {
135  if (task->period != 0) // periodic
136  {
137  MutexLock lock(task->breaker);
138  while(task->running && !task->prepareForExit )
139  {
140  TRY
141  (
142  SCOPE_ON
143  task->step(); // one cycle
144  SCOPE_OFF
145  )
146  CATCH_ALL
147  (
148  SCOPE_OFF
149  throw;
150  )
151 
152  // Check changes in period
153  if ( cur_period != task->period) {
154  // reconfigure period before going to sleep
155  rtos_task_set_period(task->getTask(), task->period);
156  cur_period = task->period;
157  if (cur_period == 0)
158  break; // break while(task->running) if no longer periodic
159  }
160 
161  // Check changes in scheduler
162  if ( cur_sched != task->msched_type) {
164  cur_sched = task->msched_type;
165  }
166  // rtos_task_wait_period will return immediately if
167  // the task is not periodic (ie period == 0)
168  // return non-zero to indicate overrun.
169  if (rtos_task_wait_period(task->getTask()) != 0)
170  {
171  ++overruns;
172  if (overruns == task->maxOverRun)
173  break; // break while(task->running)
174  }
175  else if (overruns != 0)
176  --overruns;
177  } // while(task->running)
178  if (overruns == task->maxOverRun || task->prepareForExit)
179  break; // break while(1) {}
180  }
181  else // non periodic:
182  TRY
183  (
184  // this mutex guarantees that stop() waits
185  // until loop() returns.
186  MutexLock lock(task->breaker);
187 
188  task->inloop = true;
189  SCOPE_ON
190  task->loop();
191  SCOPE_OFF
192  task->inloop = false;
193  ) CATCH_ALL
194  (
195  SCOPE_OFF
196  task->inloop = false;
197  throw;
198  )
199  }
200  } // while(1)
201  if (overruns == task->maxOverRun)
202  {
203  task->emergencyStop();
206  << " got too many periodic overruns in step() ("
207  << overruns << " times), stopped Thread !"
208  << endlog();
209  log() << " See Thread::setMaxOverrun() for info."
210  << endlog();
211  }
212  )CATCH(std::exception const& e,
213  SCOPE_OFF
214  task->emergencyStop();
217  << " caught a C++ exception, stopped thread !"
218  << endlog();
219  log(Critical) << "exception was: "
220  << e.what() << endlog();
221  ) CATCH_ALL
222  (
223  SCOPE_OFF
224  task->emergencyStop();
227  << " caught an unknown C++ exception, stopped thread !"
228  << endlog();
229  )
230  } // while (!prepareForExit)
231 
232  return 0;
233  }
234 
236  {
237  // set state to not running
238  this->running = false;
239  this->inloop = false;
240  this->active = false;
241  // execute finalize in current mode, even if hard.
242  this->finalize();
243  }
244 
245  Thread::Thread(int scheduler, int _priority,
246  Seconds periods, unsigned cpu_affinity, const std::string & name) :
247  msched_type(scheduler), active(false), prepareForExit(false),
248  inloop(false),running(false),
249  maxOverRun(OROSEM_OS_PERIODIC_THREADS_MAX_OVERRUN),
250  period(Seconds_to_nsecs(periods)) // Do not call setPeriod(), since the semaphores are not yet used !
251 #ifdef OROPKG_OS_THREAD_SCOPE
252  ,d(NULL)
253 #endif
254  , stopTimeout(0)
255  {
256  this->setup(_priority, cpu_affinity, name);
257  }
258 
259  void Thread::setup(int _priority, unsigned cpu_affinity, const std::string& name)
260  {
261  Logger::In in("Thread");
262  int ret;
263 
264  // we do this under lock in order to force the thread to wait until we're done.
265  MutexLock lock(breaker);
266 
267  log(Info) << "Creating Thread for scheduler=" << (msched_type == ORO_SCHED_OTHER ? "ORO_SCHED_OTHER" : "ORO_SCHED_RT")
268  << ", priority=" << _priority
269  << ", CPU affinity=" << cpu_affinity
270  << ", with name='" << name << "'"
271  << endlog();
272  ret = rtos_sem_init(&sem, 0);
273  if (ret != 0)
274  {
275  log(Critical)
276  << "Could not allocate configuration semaphore 'sem' for "
277  << name
278  << ". Throwing std::bad_alloc." << endlog();
280 #ifndef ORO_EMBEDDED
281  throw std::bad_alloc();
282 #else
283  return;
284 #endif
285  }
286 
287 #ifdef OROPKG_OS_THREAD_SCOPE
288  // Check if threadscope device already exists
289 
290  {
291  if ( DigitalOutInterface::nameserver.getObject("ThreadScope") )
292  {
293  d = DigitalOutInterface::nameserver.getObject("ThreadScope");
294  }
295  else
296  {
297  log(Warning) << "Failed to find 'ThreadScope' object in DigitalOutInterface::nameserver." << endlog();
298  }
299  }
300 #endif
301  int rv = rtos_task_create(&rtos_task, _priority, cpu_affinity, name.c_str(),
303  if (rv != 0)
304  {
305  log(Critical) << "Could not create thread "
306  << name << "."
307  << endlog();
309 #ifndef ORO_EMBEDDED
310  throw std::bad_alloc();
311 #else
312  return;
313 #endif
314  }
315 
316  // Wait for creation of thread.
317  rtos_sem_wait( &sem );
318 
319  const char* modname = getName();
320  Logger::In in2(modname);
321  log(Info) << "Thread created with scheduler type '"
322  << (getScheduler() == ORO_SCHED_OTHER ? "ORO_SCHED_OTHER" : "ORO_SCHED_RT") << "', priority " << getPriority()
323  << ", cpu affinity " << getCpuAffinity()
324  << " and period " << getPeriod() << " (PID= " << getPid() << " )." << endlog();
325 #ifdef OROPKG_OS_THREAD_SCOPE
326  if (d)
327  {
328  unsigned int bit = threadNumber();
329  log(Info) << "ThreadScope :"<< modname <<" toggles bit "<< bit << endlog();
330  }
331 #endif
332  }
333 
335  {
336  Logger::In in("~Thread");
337  if (this->isRunning())
338  this->stop();
339 
340  terminate();
342 
343  }
344 
346  {
347  if ( period == 0)
348  {
349  // just signal if already active.
350  if ( isActive() ) {
351 #ifndef OROPKG_OS_MACOSX
352  // This is a 'weak' race condition.
353  // it could be that sem becomes zero
354  // after this check. Technically, this means
355  // loop is being executed (preemption) during start().
356  // For most user code, this is sufficient though, as it
357  // can not know the difference between executing loop()
358  // *in* start or *right after* start (the latter is
359  // guaranteed by the API).
360  // @see ActivityInterface::trigger for how trigger uses this
361  // assumption.
362  if ( rtos_sem_value(&sem) > 0 )
363  return true;
364 #endif
366  return true;
367  }
368 
369  active=true;
370  if ( this->initialize() == false || active == false ) {
371  active = false;
372  return false;
373  }
374 
375  running = true;
377 
378  return true;
379  }
380  else {
381 
382  if ( active )
383  return false;
384  active = true;
385 
386  bool result;
387  result = this->initialize();
388 
389  if (result == false || active == false) // detect call to stop() within initialize()
390  {
391  active = false;
392  return false;
393  }
394 
395  running = true;
396 
397  // signal start :
399  int ret = rtos_sem_signal(&sem);
400  if (ret != 0)
401  log(Critical)
402  << "Thread::start(): sem_signal returns " << ret
403  << endlog();
404  // do not wait, we did our job.
405 
406  return true;
407  }
408  }
409 
411  {
412  stopTimeout = value;
413  }
414 
416  {
417  if (stopTimeout != 0)
418  return stopTimeout;
419  else if (period == 0)
422  }
423 
425  {
426  if (!active)
427  return false;
428 
429  running = false;
430 
431  if ( period == 0)
432  {
433  if ( inloop ) {
434  if ( !this->breakLoop() ) {
435  log(Warning) << "Failed to stop thread " << this->getName() << ": breakLoop() returned false."<<endlog();
436  running = true;
437  return false;
438  }
439  // breakLoop was ok, wait for loop() to return.
440  }
441  // always take this lock, but after breakLoop was called !
443  if ( !lock.isSuccessful() ) {
444  log(Error) << "Failed to stop thread " << this->getName() << ": breakLoop() returned true, but loop() function did not return after " << getStopTimeout() <<" seconds."<<endlog();
445  running = true;
446  return false;
447  }
448  } else {
449  //
451  if ( lock.isSuccessful() ) {
452  // drop out of periodic mode.
454  } else {
455  log(Error) << "Failed to stop thread " << this->getName() << ": step() function did not return after "<< getStopTimeout() <<" seconds."<<endlog();
456  running = true;
457  return false;
458  }
459  }
460 
461  this->finalize();
462  active = false;
463  return true;
464  }
465 
466  bool Thread::isRunning() const
467  {
468  return running;
469  }
470 
471  bool Thread::isActive() const
472  {
473  return active;
474  }
475 
476  bool Thread::setScheduler(int sched_type)
477  {
478  Logger::In in("Thread::setScheduler");
479  if (os::CheckScheduler(sched_type) == false)
480  return false;
481  if (this->getScheduler() == sched_type)
482  {
483  return true;
484  }
485 
486  log(Info) << "Setting scheduler type for Thread '"
487  << rtos_task_get_name(&rtos_task) << "' to "
488  << sched_type << endlog();
489  rtos_task_set_scheduler(&rtos_task, sched_type); // this may be a no-op, in that case, configure() will pick the change up.
490  msched_type = sched_type;
492  return true; // we assume all will go well.
493  }
494 
496  {
498  }
499 
501  {
502  // this function is called from within the thread
503  // when we wake up after start()
504  // It is intended to check our scheduler, priority,..., and do the in-thread
505  // stuff that may be required by the RTOS. For example: RTAI requires that
506  // we set the scheduler within the thread itself.
507 
508  // reconfigure period
510 
511  // reconfigure scheduler.
513  {
516  }
517  }
518 
520  {
521  }
522 
524  {
525  this->step();
526  }
527 
529  {
530  return false;
531  }
532 
533 
535  {
536  return true;
537  }
538 
540  {
541  }
542 
543  bool Thread::setPeriod(double s)
544  {
545  nsecs nsperiod = Seconds_to_nsecs(s);
546  return setPeriod(0, nsperiod);
547  }
548 
550  {
551  nsecs nsperiod = ns + 1000* 1000* 1000* s ;
552  if (nsperiod < 0)
553  return false;
554  // logic to switch from per->nper || nper->per
555  if ( (nsperiod == 0 && period != 0) || (nsperiod != 0 && period == 0)) {
556  // switch between periodic/non-periodic
557  // note for RTAI: the fosi_internal layer must detect if this is called from
558  // within rtos_task or outside the thread.
560  // jump from non periodic into periodic: first sample.
561  if ( period == 0) {
562  period = nsperiod; // avoid race with sem in thread func.
564  }
565  }
566  // update rate:
567  period = nsperiod;
568 
569  return true;
570  }
571 
573  {
574  return this->setPeriod( p.tv_sec, p.tv_nsec );
575  }
576 
577  void Thread::getPeriod(secs& s, nsecs& ns) const
578  {
579  s = secs(period/(1000*1000*1000));
580  ns = period - s*1000*1000*1000;
581  }
582 
584  {
585  return rtos_task_set_priority(&rtos_task, p) == 0;
586  }
587 
588  bool Thread::isPeriodic() const
589  {
590  return period != 0;
591  }
592 
594  {
596  }
597 
598  double Thread::getPeriod() const
599  {
600  return nsecs_to_Seconds(period);
601  }
602 
604  {
605  return period;
606  }
607 
608  bool Thread::setCpuAffinity(unsigned cpu_affinity)
609  {
610  return rtos_task_set_cpu_affinity(&rtos_task, cpu_affinity) == 0;
611  }
612 
613  unsigned Thread::getCpuAffinity() const
614  {
616  }
617 
618  unsigned int Thread::getPid() const
619  {
620  return rtos_task_get_pid(&rtos_task);
621  }
622 
624  {
626  }
627 
629  {
630  // avoid callling twice.
631  if (prepareForExit) return;
632 
633  Logger::In in("Thread");
634  log(Debug) << "Terminating " << this->getName() << endlog();
635 
636  prepareForExit = true;
638 
639  rtos_task_delete(&rtos_task); // this must join the thread.
640  active = false;
641 
642  log(Debug) << " done" << endlog();
643  }
644 
645  const char* Thread::getName() const
646  {
647  return rtos_task_get_name(&rtos_task);
648  }
649 
650  void Thread::setMaxOverrun( int m )
651  {
652  maxOverRun = m;
653  }
654 
656  {
657  return maxOverRun;
658  }
659 
661  {
663  }
664 
665  }
666 }
667 
void setup(int _priority, unsigned cpu_affinity, const std::string &name)
Definition: Thread.cpp:259
long long NANO_TIME
Definition: ecos/fosi.h:67
rt_sem_t sem
Definition: Thread.hpp:336
virtual bool breakLoop()
Definition: Thread.cpp:528
int rtos_task_get_priority(const RTOS_TASK *task)
virtual void setMaxOverrun(int m)
Definition: Thread.cpp:650
static unsigned int default_stack_size
Definition: Thread.hpp:290
INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK *mytask, NANO_TIME nanosecs)
double Seconds
Definition: os/Time.hpp:53
#define TRY(C)
Contains static global configuration variables and cached entries.
Definition: CatchConfig.hpp:56
static void setLockTimeoutNoPeriod(double timeout_in_s)
Definition: Thread.cpp:78
static NameServer< DigitalOutInterface * > nameserver
bool CheckScheduler(int &sched_type)
Definition: threads.cpp:46
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
Seconds nsecs_to_Seconds(const nsecs ns)
Definition: os/Time.hpp:108
void setStopTimeout(Seconds s)
Definition: Thread.cpp:410
int rtos_sem_signal(rt_sem_t *m)
int rtos_task_set_scheduler(RTOS_TASK *t, int sched_type)
friend void * thread_function(void *t)
Definition: Thread.cpp:82
void emergencyStop()
Definition: Thread.cpp:235
MutexRecursive breaker
Definition: Thread.hpp:341
long secs
Definition: os/Time.hpp:57
virtual bool setCpuAffinity(unsigned cpu_affinity)
Definition: Thread.cpp:608
int rtos_task_set_cpu_affinity(RTOS_TASK *task, unsigned cpu_affinity)
virtual bool setScheduler(int sched_type)
Definition: Thread.cpp:476
unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
bool prepareForExit
Definition: Thread.hpp:316
virtual bool setPriority(int priority)
Definition: Thread.cpp:583
virtual void step()
Definition: Thread.cpp:519
virtual RTOS_TASK * getTask()
Definition: Thread.hpp:205
static double lock_timeout_period_factor
Definition: Thread.hpp:300
virtual void loop()
Definition: Thread.cpp:523
virtual int getMaxOverrun() const
Definition: Thread.cpp:655
#define CATCH(T, C)
Definition: CatchConfig.hpp:57
void rtos_task_set_wait_period_policy(RTOS_TASK *task, int policy)
static void setLockTimeoutPeriodFactor(double factor)
Definition: Thread.cpp:80
void rtos_task_delete(RTOS_TASK *mytask)
virtual void finalize()
Definition: Thread.cpp:539
virtual bool isActive() const
Definition: Thread.cpp:471
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK *)
#define SCOPE_OFF
Definition: Thread.cpp:62
int rtos_task_set_priority(RTOS_TASK *task, int priority)
unsigned int threadNumber() const
int rtos_sem_destroy(rt_sem_t *m)
virtual bool initialize()
Definition: Thread.cpp:534
const char * rtos_task_get_name(const RTOS_TASK *task)
A MutexTimedLock locks a Mutex object on construction and if successful, unlocks it on destruction of...
Definition: MutexLock.hpp:146
virtual unsigned getCpuAffinity() const
Definition: Thread.cpp:613
#define CATCH_ALL(C)
Definition: CatchConfig.hpp:58
#define SCOPE_INIT(name)
Definition: Thread.cpp:60
int rtos_task_wait_period(RTOS_TASK *task)
void configure()
Definition: Thread.cpp:500
ValueType getObject(const NameType &s) const
Get the object registered for a name.
Definition: NameServer.hpp:136
void terminate()
Definition: Thread.cpp:628
static double lock_timeout_no_period_in_s
Definition: Thread.hpp:295
virtual int getScheduler() const
Definition: Thread.cpp:495
RTOS_TASK rtos_task
Definition: Thread.hpp:331
bool setPeriod(Seconds s)
Definition: Thread.cpp:543
int rtos_sem_init(rt_sem_t *m, int value)
nsecs Seconds_to_nsecs(const Seconds s)
Definition: os/Time.hpp:107
#define SCOPE_ON
Definition: Thread.cpp:61
virtual ~Thread()
Definition: Thread.cpp:334
static void setStackSize(unsigned int ssize)
Definition: Thread.cpp:76
virtual bool start()
Definition: Thread.cpp:345
Thread(int scheduler, int priority, double period, unsigned cpu_affinity, const std::string &name)
Definition: Thread.cpp:245
int rtos_sem_value(rt_sem_t *m)
Seconds getStopTimeout() const
Definition: Thread.cpp:415
virtual void yield()
Definition: Thread.cpp:623
NANO_TIME period
Definition: Thread.hpp:352
virtual bool stop()
Definition: Thread.cpp:424
virtual void setWaitPeriodPolicy(int p)
Definition: Thread.cpp:660
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:53
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
virtual int getPriority() const
Definition: Thread.cpp:593
virtual Seconds getPeriod() const
Definition: Thread.cpp:598
long long nsecs
Definition: os/Time.hpp:69
struct timespec TIME_SPEC
Definition: ecos/fosi.h:109
virtual unsigned int getPid() const
Definition: Thread.cpp:618
static Logger & log()
Definition: Logger.hpp:350
int rtos_sem_wait(rt_sem_t *m)
#define ORO_SCHED_OTHER
Definition: ecos/fosi.h:62
virtual bool isPeriodic() const
Definition: Thread.cpp:588
static Logger::LogFunction endlog()
Definition: Logger.hpp:362
double stopTimeout
Definition: Thread.hpp:357
virtual nsecs getPeriodNS() const
Definition: Thread.cpp:603
virtual const char * getName() const
Definition: Thread.cpp:645
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)
virtual bool isRunning() const
Definition: Thread.cpp:466
MutexLock is a scope based Monitor, protecting critical sections with a Mutex object through locking ...
Definition: MutexLock.hpp:51
int rtos_task_get_scheduler(const RTOS_TASK *t)


rtt
Author(s): RTT Developers
autogenerated on Fri Oct 25 2019 03:59:44