Orocos Real-Time Toolkit  2.6.0
Thread.cpp
00001 /***************************************************************************
00002   tag: Peter Soetens  Mon May 10 19:10:28 CEST 2004  Thread.cxx
00003 
00004                         Thread.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 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 #include "fosi_internal_interface.hpp"
00039 #include "Thread.hpp"
00040 #include "../Time.hpp"
00041 #include "threads.hpp"
00042 #include "../Logger.hpp"
00043 #include "MutexLock.hpp"
00044 
00045 #include "../rtt-config.h"
00046 
00047 #ifdef OROPKG_OS_THREAD_SCOPE
00048 # include "../extras/dev/DigitalOutInterface.hpp"
00049 #define SCOPE_ON   if ( task->d ) task->d->switchOn( bit );
00050 #define SCOPE_OFF  if ( task->d ) task->d->switchOff( bit );
00051 #else
00052 #define SCOPE_ON
00053 #define SCOPE_OFF
00054 #endif
00055 
00056 #ifndef ORO_EMBEDDED
00057 #define TRY try
00058 #define CATCH(a) catch(a)
00059 #define CATCH_ALL catch(...)
00060 #else
00061 #define TRY
00062 #define CATCH(a) if (false)
00063 #define CATCH_ALL if (false)
00064 #endif
00065 
00066 namespace RTT {
00067     namespace os
00068     {
00069         using RTT::Logger;
00070 
00071         unsigned int Thread::default_stack_size = 0;
00072 
00073         void Thread::setStackSize(unsigned int ssize) { default_stack_size = ssize; }
00074 
00075         void *thread_function(void* t)
00076         {
00080             Thread* task = static_cast<os::Thread*> (t);
00081             Logger::In in(task->getName());
00082 
00083             task->configure();
00084 
00085             // signal to setup() that we're created.
00086             rtos_sem_signal(&(task->sem));
00087 
00088             // This lock forces setup(), which holds the lock, to continue.
00089             { MutexLock lock(task->breaker); }
00090 #ifdef OROPKG_OS_THREAD_SCOPE
00091             // order thread scope toggle bit on thread number
00092             unsigned int bit = task->threadNumber();
00093 #endif
00094             SCOPE_OFF
00095 
00096             int overruns = 0, cur_sched = task->msched_type;
00097             NANO_TIME cur_period = task->period;
00098 
00099             while (!task->prepareForExit)
00100             {
00101                 TRY
00102                 {
00106                     while (1)
00107                     {
00108                         if (!task->active || (task->active && task->period == 0) || !task->running )
00109                         {
00110                             // consider this the 'configuration or waiting for command state'
00111                             if (task->period != 0) {
00112                                 overruns = 0;
00113                                 // drop out of periodic mode:
00114                                 rtos_task_set_period(task->getTask(), 0);
00115                             }
00116                             rtos_sem_wait(&(task->sem)); // wait for command.
00117                             task->configure();           // check for reconfigure
00118                             if (task->prepareForExit)    // check for exit
00119                             {
00120                                 break; // break while(1) {}
00121                             }
00122                             // end of configuration
00123                         }
00124                         // This is the running state. Only enter if a task is running
00125                         if ( task->running )
00126                         {
00127                             if (task->period != 0) // periodic
00128                             {
00129                                 MutexLock lock(task->breaker);
00130                                 while(task->running && !task->prepareForExit )
00131                                 {
00132                                     try
00133                                     {
00134                                         SCOPE_ON
00135                                         task->step(); // one cycle
00136                                         SCOPE_OFF
00137                                     }
00138                                     catch(...)
00139                                     {
00140                                         SCOPE_OFF
00141                                         throw;
00142                                     }
00143 
00144                                     // Check changes in period
00145                                     if ( cur_period != task->period) {
00146                                         // reconfigure period before going to sleep
00147                                         rtos_task_set_period(task->getTask(), task->period);
00148                                         cur_period = task->period;
00149                                         if (cur_period == 0)
00150                                             break; // break while(task->running) if no longer periodic
00151                                     }
00152 
00153                                     // Check changes in scheduler
00154                                     if ( cur_sched != task->msched_type) {
00155                                         rtos_task_set_scheduler(task->getTask(), task->msched_type);
00156                                         cur_sched = task->msched_type;
00157                                     }
00158                                     // rtos_task_wait_period will return immediately if
00159                                     // the task is not periodic (ie period == 0)
00160                                     // return non-zero to indicate overrun.
00161                                     if (rtos_task_wait_period(task->getTask()) != 0)
00162                                     {
00163                                         ++overruns;
00164                                         if (overruns == task->maxOverRun)
00165                                             break; // break while(task->running)
00166                                     }
00167                                     else if (overruns != 0)
00168                                         --overruns;
00169                                 } // while(task->running)
00170                                 if (overruns == task->maxOverRun || task->prepareForExit)
00171                                     break; // break while(1) {}
00172                             }
00173                             else // non periodic:
00174                             try
00175                             {
00176                                 // this mutex guarantees that stop() waits
00177                                 // until loop() returns.
00178                                 MutexLock lock(task->breaker);
00179 
00180                                 task->inloop = true;
00181                                 SCOPE_ON
00182                                 task->loop();
00183                                 SCOPE_OFF
00184                                 task->inloop = false;
00185                             }
00186                             catch(...) {
00187                                 SCOPE_OFF
00188                                 task->inloop = false;
00189                                 throw;
00190                             }
00191                         }
00192                     } // while(1)
00193                     if (overruns == task->maxOverRun)
00194                     {
00195                         task->emergencyStop();
00196                         Logger::In in(rtos_task_get_name(task->getTask()));
00197                         log(Critical) << rtos_task_get_name(task->getTask())
00198                                 << " got too many periodic overruns in step() ("
00199                                 << overruns << " times), stopped Thread !"
00200                                 << endlog();
00201                         log()   << " See Thread::setMaxOverrun() for info."
00202                                 << endlog();
00203                     }
00204                 } CATCH(std::exception const& e)
00205                 {
00206                     SCOPE_OFF
00207                     task->emergencyStop();
00208                     Logger::In in(rtos_task_get_name(task->getTask()));
00209                     log(Critical) << rtos_task_get_name(task->getTask())
00210                             << " caught a C++ exception, stopped thread !"
00211                             << endlog();
00212                     log(Critical) << "exception was: "
00213                                << e.what() << endlog();
00214                 } CATCH_ALL
00215                 {
00216                     SCOPE_OFF
00217                     task->emergencyStop();
00218                     Logger::In in(rtos_task_get_name(task->getTask()));
00219                     log(Critical) << rtos_task_get_name(task->getTask())
00220                             << " caught an unknown C++ exception, stopped thread !"
00221                             << endlog();
00222                 }
00223             } // while (!prepareForExit)
00224 
00225             return 0;
00226         }
00227 
00228         void Thread::emergencyStop()
00229         {
00230             // set state to not running
00231             this->running = false;
00232             this->inloop  = false;
00233             this->active  = false;
00234             // execute finalize in current mode, even if hard.
00235             this->finalize();
00236         }
00237 
00238         Thread::Thread(int scheduler, int _priority,
00239                 Seconds periods, unsigned cpu_affinity, const std::string & name) :
00240                     msched_type(scheduler), active(false), prepareForExit(false),
00241                     inloop(false),running(false),
00242                     maxOverRun(OROSEM_OS_PERIODIC_THREADS_MAX_OVERRUN),
00243                     period(Seconds_to_nsecs(periods)) // Do not call setPeriod(), since the semaphores are not yet used !
00244 #ifdef OROPKG_OS_THREAD_SCOPE
00245         ,d(NULL)
00246 #endif
00247         {
00248             this->setup(_priority, cpu_affinity, name);
00249         }
00250 
00251         void Thread::setup(int _priority, unsigned cpu_affinity, const std::string& name)
00252         {
00253             Logger::In in("Thread");
00254             int ret;
00255 
00256             // we do this under lock in order to force the thread to wait until we're done.
00257             MutexLock lock(breaker);
00258 
00259             log(Info) << "Creating Thread for scheduler=" << (msched_type == ORO_SCHED_OTHER ? "ORO_SCHED_OTHER" : "ORO_SCHED_RT")
00260                       << ", priority=" << _priority
00261                       << ", CPU affinity=" << cpu_affinity
00262                       << ", with name='" << name << "'"
00263                       << endlog();
00264             ret = rtos_sem_init(&sem, 0);
00265             if (ret != 0)
00266             {
00267                 log(Critical)
00268                         << "Could not allocate configuration semaphore 'sem' for "
00269                         << name
00270                         << ". Throwing std::bad_alloc." << endlog();
00271                 rtos_sem_destroy(&sem);
00272 #ifndef ORO_EMBEDDED
00273                 throw std::bad_alloc();
00274 #else
00275                 return;
00276 #endif
00277             }
00278 
00279 #ifdef OROPKG_OS_THREAD_SCOPE
00280             // Check if threadscope device already exists
00281 
00282             {
00283                 if ( DigitalOutInterface::nameserver.getObject("ThreadScope") )
00284                 {
00285                     d = DigitalOutInterface::nameserver.getObject("ThreadScope");
00286                 }
00287                 else
00288                 {
00289                     log(Warning) << "Failed to find 'ThreadScope' object in DigitalOutInterface::nameserver." << endlog();
00290                 }
00291             }
00292 #endif
00293             int rv = rtos_task_create(&rtos_task, _priority, cpu_affinity, name.c_str(),
00294                     msched_type, default_stack_size, thread_function, this);
00295             if (rv != 0)
00296             {
00297                 log(Critical) << "Could not create thread "
00298                         << name << "."
00299                         << endlog();
00300                 rtos_sem_destroy(&sem);
00301 #ifndef ORO_EMBEDDED
00302                 throw std::bad_alloc();
00303 #else
00304                 return;
00305 #endif
00306             }
00307 
00308             // Wait for creation of thread.
00309             rtos_sem_wait( &sem );
00310 
00311             const char* modname = getName();
00312             Logger::In in2(modname);
00313             log(Info) << "Thread created with scheduler type '"
00314                     << (getScheduler() == ORO_SCHED_OTHER ? "ORO_SCHED_OTHER" : "ORO_SCHED_RT") << "', priority " << getPriority()
00315                     << ", cpu affinity " << getCpuAffinity()
00316                     << " and period " << getPeriod() << " (PID= " << getPid() << " )." << endlog();
00317 #ifdef OROPKG_OS_THREAD_SCOPE
00318             if (d)
00319             {
00320                 unsigned int bit = threadNumber();
00321                 log(Info) << "ThreadScope :"<< modname <<" toggles bit "<< bit << endlog();
00322             }
00323 #endif
00324         }
00325 
00326         Thread::~Thread()
00327         {
00328             Logger::In in("~Thread");
00329             if (this->isRunning())
00330                 this->stop();
00331 
00332             log(Debug) << "Terminating " << this->getName() << endlog();
00333             terminate();
00334             log(Debug) << " done" << endlog();
00335             rtos_sem_destroy(&sem);
00336 
00337         }
00338 
00339         bool Thread::start()
00340         {
00341             if ( period == 0)
00342             {
00343                 // just signal if already active.
00344                 if ( isActive() ) {
00345 #ifndef OROPKG_OS_MACOSX
00346                     // This is a 'weak' race condition.
00347                     // it could be that sem becomes zero
00348                     // after this check. Technically, this means
00349                     // loop is being executed (preemption) during start().
00350                     // For most user code, this is sufficient though, as it
00351                     // can not know the difference between executing loop()
00352                     // *in* start or *right after* start (the latter is
00353                     // guaranteed by the API).
00354                     // @see ActivityInterface::trigger for how trigger uses this
00355                     // assumption.
00356                     if ( rtos_sem_value(&sem) > 0 )
00357                         return true;
00358 #endif
00359                     rtos_sem_signal(&sem);
00360                     return true;
00361                 }
00362 
00363                 active=true;
00364                 if ( this->initialize() == false || active == false ) {
00365                     active = false;
00366                     return false;
00367                 }
00368 
00369                 running = true;
00370                 rtos_sem_signal(&sem);
00371 
00372                 return true;
00373             }
00374             else {
00375 
00376                 if ( active )
00377                     return false;
00378                 active = true;
00379 
00380                 bool result;
00381                 result = this->initialize();
00382 
00383                 if (result == false || active == false) // detect call to stop() within initialize()
00384                 {
00385                     active = false;
00386                     return false;
00387                 }
00388 
00389                 running = true;
00390 
00391                 // signal start :
00392                 rtos_task_make_periodic(&rtos_task, period);
00393                 int ret = rtos_sem_signal(&sem);
00394                 if (ret != 0)
00395                     log(Critical)
00396                     << "Thread::start(): sem_signal returns " << ret
00397                     << endlog();
00398                 // do not wait, we did our job.
00399 
00400                 return true;
00401             }
00402         }
00403 
00404         bool Thread::stop()
00405         {
00406             if (!active)
00407                 return false;
00408 
00409             running = false;
00410 
00411             if ( period == 0)
00412             {
00413                 if ( inloop ) {
00414                     if ( !this->breakLoop() ) {
00415                         log(Warning) << "Failed to stop thread " << this->getName() << ": breakLoop() returned false."<<endlog();
00416                         running = true;
00417                         return false;
00418                     }
00419                     // breakLoop was ok, wait for loop() to return.
00420                 }
00421                 // always take this lock, but after breakLoop was called !
00422                 MutexTimedLock lock(breaker, 1.0); // hard-coded: wait 1 second.
00423                 if ( !lock.isSuccessful() ) {
00424                     log(Error) << "Failed to stop thread " << this->getName() << ": breakLoop() returned true, but loop() function did not return after 1 second."<<endlog();
00425                     running = true;
00426                     return false;
00427                 }
00428             } else {
00429                 //
00430                 MutexTimedLock lock(breaker, 10*getPeriod() ); // hard-coded: wait 5 times the period
00431                 if ( lock.isSuccessful() ) {
00432                     // drop out of periodic mode.
00433                     rtos_task_make_periodic(&rtos_task, 0);
00434                 } else {
00435                     log(Error) << "Failed to stop thread " << this->getName() << ": step() function did not return after "<< 5*getPeriod() <<" seconds."<<endlog();
00436                     running = true;
00437                     return false;
00438                 }
00439             }
00440 
00441             this->finalize();
00442             active = false;
00443             return true;
00444         }
00445 
00446         bool Thread::isRunning() const
00447         {
00448             return period == 0 ? inloop : running;
00449         }
00450 
00451         bool Thread::isActive() const
00452         {
00453             return active;
00454         }
00455 
00456         bool Thread::setScheduler(int sched_type)
00457         {
00458             Logger::In in("Thread::setScheduler");
00459             if (os::CheckScheduler(sched_type) == false)
00460                 return false;
00461             if (this->getScheduler() == sched_type)
00462             {
00463                 return true;
00464             }
00465 
00466             log(Info) << "Setting scheduler type for Thread '"
00467                       << rtos_task_get_name(&rtos_task) << "' to "
00468                       << sched_type << endlog();
00469             rtos_task_set_scheduler(&rtos_task, sched_type); // this may be a no-op, in that case, configure() will pick the change up.
00470             msched_type = sched_type;
00471             rtos_sem_signal(&sem);
00472             return true; // we assume all will go well.
00473         }
00474 
00475         int Thread::getScheduler() const
00476         {
00477             return rtos_task_get_scheduler(&rtos_task);
00478         }
00479 
00480         void Thread::configure()
00481         {
00482             // this function is called from within the thread
00483             // when we wake up after start()
00484             // It is intended to check our scheduler, priority,..., and do the in-thread
00485             // stuff that may be required by the RTOS. For example: RTAI requires that
00486             // we set the scheduler within the thread itself.
00487 
00488             // reconfigure period
00489             rtos_task_set_period(&rtos_task, period);
00490 
00491             // reconfigure scheduler.
00492             if (msched_type != rtos_task_get_scheduler(&rtos_task))
00493             {
00494                 rtos_task_set_scheduler(&rtos_task, msched_type);
00495                 msched_type = rtos_task_get_scheduler(&rtos_task);
00496             }
00497         }
00498 
00499         void Thread::step()
00500         {
00501         }
00502 
00503         void Thread::loop()
00504         {
00505             this->step();
00506         }
00507 
00508         bool Thread::breakLoop()
00509         {
00510             return false;
00511         }
00512 
00513 
00514         bool Thread::initialize()
00515         {
00516             return true;
00517         }
00518 
00519         void Thread::finalize()
00520         {
00521         }
00522 
00523         bool Thread::setPeriod(double s)
00524         {
00525             nsecs nsperiod = Seconds_to_nsecs(s);
00526             return setPeriod(0, nsperiod);
00527         }
00528 
00529         bool Thread::setPeriod(secs s, nsecs ns)
00530         {
00531             nsecs nsperiod = ns + 1000* 1000* 1000* s ;
00532             if (nsperiod < 0)
00533                 return false;
00534             // logic to switch from per->nper || nper->per
00535             if ( (nsperiod == 0 && period != 0) || (nsperiod != 0 && period == 0)) {
00536                 // switch between periodic/non-periodic
00537                 // note for RTAI: the fosi_internal layer must detect if this is called from
00538                 // within rtos_task or outside the thread.
00539                 rtos_task_make_periodic(&rtos_task, nsperiod);
00540                 // jump from non periodic into periodic: first sample.
00541                 if ( period == 0) {
00542                     period = nsperiod; // avoid race with sem in thread func.
00543                     rtos_sem_signal(&sem);
00544                 }
00545             }
00546             // update rate:
00547             period = nsperiod;
00548 
00549             return true;
00550         }
00551 
00552         bool Thread::setPeriod( TIME_SPEC p)
00553         {
00554             return this->setPeriod( p.tv_sec, p.tv_nsec );
00555         }
00556 
00557         void Thread::getPeriod(secs& s, nsecs& ns) const
00558         {
00559             s = secs(period/(1000*1000*1000));
00560             ns = period - s*1000*1000*1000;
00561         }
00562 
00563         bool Thread::setPriority(int p)
00564         {
00565             return rtos_task_set_priority(&rtos_task, p) == 0;
00566         }
00567 
00568         bool Thread::isPeriodic() const
00569         {
00570             return period != 0;
00571         }
00572 
00573         int Thread::getPriority() const
00574         {
00575             return rtos_task_get_priority(&rtos_task);
00576         }
00577 
00578         double Thread::getPeriod() const
00579         {
00580             return nsecs_to_Seconds(period);
00581         }
00582 
00583         nsecs Thread::getPeriodNS() const
00584         {
00585             return period;
00586         }
00587 
00588         bool Thread::setCpuAffinity(unsigned cpu_affinity)
00589         {
00590             return rtos_task_set_cpu_affinity(&rtos_task, cpu_affinity) == 0;
00591         }
00592 
00593         unsigned Thread::getCpuAffinity() const
00594         {
00595             return rtos_task_get_cpu_affinity(&rtos_task);
00596         }
00597 
00598         unsigned int Thread::getPid() const
00599         {
00600             return rtos_task_get_pid(&rtos_task);
00601         }
00602 
00603         void Thread::yield()
00604         {
00605             rtos_task_yield( &rtos_task );
00606         }
00607 
00608         void Thread::terminate()
00609         {
00610             // avoid callling twice.
00611             if (prepareForExit) return;
00612 
00613             prepareForExit = true;
00614             rtos_sem_signal(&sem);
00615 
00616             rtos_task_delete(&rtos_task); // this must join the thread.
00617         }
00618 
00619         const char* Thread::getName() const
00620         {
00621             return rtos_task_get_name(&rtos_task);
00622         }
00623 
00624         void Thread::setMaxOverrun( int m )
00625         {
00626             maxOverRun = m;
00627         }
00628 
00629         int Thread::getMaxOverrun() const
00630         {
00631             return maxOverRun;
00632         }
00633 
00634         void Thread::setWaitPeriodPolicy(int p)
00635         {
00636             rtos_task_set_wait_period_policy(&rtos_task, p);  
00637         }
00638     }
00639 }
00640