Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : 
       3             : #include "Semaphores.h"
       4             : 
       5             : extern const AP_HAL::HAL& hal;
       6             : 
       7             : using namespace Linux;
       8             : 
       9             : // construct a semaphore
      10        1988 : Semaphore::Semaphore()
      11             : {
      12             :     pthread_mutexattr_t attr;
      13        1988 :     pthread_mutexattr_init(&attr);
      14        1988 :     pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
      15        1988 :     pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
      16        1988 :     pthread_mutex_init(&_lock, &attr);
      17        1988 : }
      18             : 
      19         252 : bool Semaphore::give()
      20             : {
      21         252 :     return pthread_mutex_unlock(&_lock) == 0;
      22             : }
      23             : 
      24         252 : bool Semaphore::take(uint32_t timeout_ms)
      25             : {
      26         252 :     if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
      27         252 :         return pthread_mutex_lock(&_lock) == 0;
      28             :     }
      29           0 :     if (take_nonblocking()) {
      30           0 :         return true;
      31             :     }
      32           0 :     uint64_t start = AP_HAL::micros64();
      33             :     do {
      34           0 :         hal.scheduler->delay_microseconds(200);
      35           0 :         if (take_nonblocking()) {
      36           0 :             return true;
      37             :         }
      38           0 :     } while ((AP_HAL::micros64() - start) < timeout_ms*1000);
      39           0 :     return false;
      40             : }
      41             : 
      42           0 : bool Semaphore::take_nonblocking()
      43             : {
      44           0 :     return pthread_mutex_trylock(&_lock) == 0;
      45             : }
      46             : 
      47             : /*
      48             :   binary semaphore using pthread condition variables
      49             :  */
      50             : 
      51           0 : BinarySemaphore::BinarySemaphore(bool initial_state) :
      52           0 :     AP_HAL::BinarySemaphore(initial_state)
      53             : {
      54           0 :     pthread_cond_init(&cond, NULL);
      55           0 :     pending = initial_state;
      56           0 : }
      57             : 
      58           0 : bool BinarySemaphore::wait(uint32_t timeout_us)
      59             : {
      60           0 :     WITH_SEMAPHORE(mtx);
      61           0 :     if (!pending) {
      62             :         struct timespec ts;
      63           0 :         if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
      64           0 :             return false;
      65             :         }
      66           0 :         ts.tv_sec += timeout_us/1000000UL;
      67           0 :         ts.tv_nsec += (timeout_us % 1000000U) * 1000UL;
      68           0 :         if (ts.tv_nsec >= 1000000000L) {
      69           0 :             ts.tv_sec++;
      70           0 :             ts.tv_nsec -= 1000000000L;
      71             :         }
      72           0 :         if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) {
      73           0 :             return false;
      74             :         }
      75             :     }
      76           0 :     pending = false;
      77           0 :     return true;
      78           0 : }
      79             : 
      80           0 : bool BinarySemaphore::wait_blocking(void)
      81             : {
      82           0 :     WITH_SEMAPHORE(mtx);
      83           0 :     if (!pending) {
      84           0 :         if (pthread_cond_wait(&cond, &mtx._lock) != 0) {
      85           0 :             return false;
      86             :         }
      87             :     }
      88           0 :     pending = false;
      89           0 :     return true;
      90           0 : }
      91             : 
      92           0 : void BinarySemaphore::signal(void)
      93             : {
      94           0 :     WITH_SEMAPHORE(mtx);
      95           0 :     if (!pending) {
      96           0 :         pending = true;
      97           0 :         pthread_cond_signal(&cond);
      98             :     }
      99           0 : }