Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : 
       3             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
       4             : 
       5             : #include "Semaphores.h"
       6             : #include "Scheduler.h"
       7             : 
       8             : extern const AP_HAL::HAL& hal;
       9             : 
      10             : using namespace HALSITL;
      11             : 
      12             : // construct a semaphore
      13      259093 : Semaphore::Semaphore()
      14             : {
      15             :     pthread_mutexattr_t attr;
      16      259093 :     pthread_mutexattr_init(&attr);
      17      259093 :     pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
      18      259093 :     pthread_mutex_init(&_lock, &attr);
      19      259093 : }
      20             : 
      21             : 
      22  5714936779 : bool Semaphore::give()
      23             : {
      24  5714936779 :     take_count--;
      25  5714936779 :     if (take_count == 0) {
      26  5679956696 :         owner = (pthread_t)-1;
      27             :     }
      28  5714936779 :     if (pthread_mutex_unlock(&_lock) != 0) {
      29           0 :         AP_HAL::panic("Bad semaphore usage");
      30             :     }
      31  5715597970 :     return true;
      32             : }
      33             : 
      34     3172223 : void Semaphore::check_owner() const
      35             : {
      36             :     // should probably make sure we're holding the semaphore here....
      37     3172223 :     if (owner != pthread_self()) {
      38           0 :         AP_HAL::panic("Wrong owner");
      39             :     }
      40     3172223 : }
      41             : 
      42  5604187459 : bool Semaphore::take(uint32_t timeout_ms)
      43             : {
      44  5604187459 :     if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
      45  5603170424 :         if (pthread_mutex_lock(&_lock) == 0) {
      46  5603797930 :             owner = pthread_self();
      47  5603797930 :             take_count++;
      48  5603797930 :             return true;
      49             :         }
      50           0 :         return false;
      51             :     }
      52     1017035 :     if (take_nonblocking()) {
      53     1048405 :         owner = pthread_self();
      54     1048405 :         return true;
      55             :     }
      56           8 :     uint64_t start = AP_HAL::micros64();
      57             :     do {
      58           8 :         Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(true);
      59           8 :         hal.scheduler->delay_microseconds(200);
      60           8 :         Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(false);
      61           8 :         if (take_nonblocking()) {
      62           8 :             owner = pthread_self();
      63           8 :             return true;
      64             :         }
      65           0 :     } while ((AP_HAL::micros64() - start) < timeout_ms * 1000);
      66           0 :     return false;
      67             : }
      68             : 
      69   111920291 : bool Semaphore::take_nonblocking()
      70             : {
      71   111920291 :     if (pthread_mutex_trylock(&_lock) == 0) {
      72   111920319 :         owner = pthread_self();
      73   111920319 :         take_count++;
      74   111920319 :         return true;
      75             :     }
      76          27 :     return false;
      77             : }
      78             : 
      79             : 
      80             : /*
      81             :   binary semaphore using pthread condition variables
      82             :  */
      83             : 
      84        4298 : BinarySemaphore::BinarySemaphore(bool initial_state) :
      85        4298 :     AP_HAL::BinarySemaphore(initial_state)
      86             : {
      87        4298 :     pthread_cond_init(&cond, NULL);
      88        4298 :     pending = initial_state;
      89        4298 : }
      90             : 
      91       22743 : bool BinarySemaphore::wait(uint32_t timeout_us)
      92             : {
      93       22743 :     WITH_SEMAPHORE(mtx);
      94       22743 :     if (!pending) {
      95       45343 :         if (hal.scheduler->in_main_thread() ||
      96       22659 :             Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
      97             :             /*
      98             :               when in the main thread we need to do a busy wait to ensure
      99             :               the clock advances
     100             :             */
     101          25 :             uint64_t end_us = AP_HAL::micros64() + timeout_us;
     102          25 :             struct timespec ts {};
     103             :             do {
     104       30025 :                 if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) == 0) {
     105           0 :                     pending = false;
     106           0 :                     return true;
     107             :                 }
     108       30025 :                 hal.scheduler->delay_microseconds(10);
     109       30025 :             } while (AP_HAL::micros64() < end_us);
     110          25 :             return false;
     111             :         }
     112             : 
     113             :         struct timespec ts;
     114       22659 :         if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
     115           0 :             return false;
     116             :         }
     117       22659 :         ts.tv_sec += timeout_us/1000000UL;
     118       22659 :         ts.tv_nsec += (timeout_us % 1000000U) * 1000UL;
     119       22659 :         if (ts.tv_nsec >= 1000000000L) {
     120          18 :             ts.tv_sec++;
     121          18 :             ts.tv_nsec -= 1000000000L;
     122             :         }
     123       22659 :         if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) {
     124       22658 :             return false;
     125             :         }
     126             :     }
     127          59 :     pending = false;
     128          59 :     return true;
     129       22742 : }
     130             : 
     131      287161 : bool BinarySemaphore::wait_blocking(void)
     132             : {
     133      287161 :     WITH_SEMAPHORE(mtx);
     134      287161 :     if (!pending) {
     135      283525 :         if (pthread_cond_wait(&cond, &mtx._lock) != 0) {
     136           0 :             return false;
     137             :         }
     138             :     }
     139      287159 :     pending = false;
     140      287159 :     return true;
     141      287159 : }
     142             : 
     143      288377 : void BinarySemaphore::signal(void)
     144             : {
     145      288377 :     WITH_SEMAPHORE(mtx);
     146      288377 :     if (!pending) {
     147      287219 :         pending = true;
     148      287219 :         pthread_cond_signal(&cond);
     149             :     }
     150      288377 : }
     151             : 
     152             : #endif  // CONFIG_HAL_BOARD