mirror of https://github.com/ArduPilot/ardupilot
AP_HAL: moved the WITH_SEMAPHORE() logic into AP_HAL
this is needed to allow us to record the location of a blocking semaphore to track down bugs where we have a semaphore deadlock
This commit is contained in:
parent
e3c80bf08f
commit
8f973da4b5
|
@ -0,0 +1,28 @@
|
||||||
|
#include "AP_HAL.h"
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL &hal;
|
||||||
|
|
||||||
|
/*
|
||||||
|
implement WithSemaphore class for WITH_SEMAPHORE() support
|
||||||
|
*/
|
||||||
|
WithSemaphore::WithSemaphore(AP_HAL::Semaphore *mtx, uint32_t line) :
|
||||||
|
WithSemaphore(*mtx, line)
|
||||||
|
{}
|
||||||
|
|
||||||
|
WithSemaphore::WithSemaphore(AP_HAL::Semaphore &mtx, uint32_t line) :
|
||||||
|
_mtx(mtx)
|
||||||
|
{
|
||||||
|
bool in_main = hal.scheduler->in_main_thread();
|
||||||
|
if (in_main) {
|
||||||
|
hal.util->persistent_data.semaphore_line = line;
|
||||||
|
}
|
||||||
|
_mtx.take_blocking();
|
||||||
|
if (in_main) {
|
||||||
|
hal.util->persistent_data.semaphore_line = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
WithSemaphore::~WithSemaphore()
|
||||||
|
{
|
||||||
|
_mtx.give();
|
||||||
|
}
|
|
@ -20,3 +20,31 @@ public:
|
||||||
virtual bool give() = 0;
|
virtual bool give() = 0;
|
||||||
virtual ~Semaphore(void) {}
|
virtual ~Semaphore(void) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
/*
|
||||||
|
a method to make semaphores less error prone. The WITH_SEMAPHORE()
|
||||||
|
macro will block forever for a semaphore, and will automatically
|
||||||
|
release the semaphore when it goes out of scope
|
||||||
|
|
||||||
|
Note that we have two types of semaphores. A normal semaphore can
|
||||||
|
only be taken once. A recursive semaphore allows for the thread
|
||||||
|
holding the semaphore to take it again. It must be released the same
|
||||||
|
number of times it is taken.
|
||||||
|
|
||||||
|
The WITH_SEMAPHORE() macro can be used with either type of semaphore
|
||||||
|
*/
|
||||||
|
|
||||||
|
class WithSemaphore {
|
||||||
|
public:
|
||||||
|
WithSemaphore(AP_HAL::Semaphore *mtx, uint32_t line);
|
||||||
|
WithSemaphore(AP_HAL::Semaphore &mtx, uint32_t line);
|
||||||
|
|
||||||
|
~WithSemaphore();
|
||||||
|
private:
|
||||||
|
AP_HAL::Semaphore &_mtx;
|
||||||
|
};
|
||||||
|
|
||||||
|
// From: https://stackoverflow.com/questions/19666142/why-is-a-level-of-indirection-needed-for-this-concatenation-macro
|
||||||
|
#define WITH_SEMAPHORE( sem ) JOIN( sem, __LINE__, __COUNTER__ )
|
||||||
|
#define JOIN( sem, line, counter ) _DO_JOIN( sem, line, counter )
|
||||||
|
#define _DO_JOIN( sem, line, counter ) WithSemaphore _getsem ## counter(sem, line)
|
||||||
|
|
Loading…
Reference in New Issue