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


rtt
Author(s): RTT Developers
autogenerated on Sat Jun 8 2019 18:46:33