AP_HAL: move AP_Semaphore to be part of the HAL

This commit is contained in:
Pat Hickey 2012-11-28 17:53:15 -08:00 committed by Andrew Tridgell
parent 5f48193ea0
commit b2c44d8a81
3 changed files with 121 additions and 0 deletions

View File

@ -0,0 +1,20 @@
#ifndef __AP_HAL_SEMAPHORE_H__
#define __AP_HAL_SEMAPHORE_H__
#include <AP_HAL_Namespace.h>
class AP_HAL::Semaphore {
public:
// get - to claim ownership of the semaphore
virtual bool get(void* caller) = 0;
// release - to give up ownership of the semaphore
virtual bool release(void* caller) = 0;
// call_on_release - returns true if caller successfully added to the
// queue to be called back
virtual bool call_on_release(void* caller, AP_HAL::Proc k) = 0;
};
#endif // __AP_HAL_SEMAPHORE_H__

View File

@ -0,0 +1,69 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
// Constructor
AVRSemaphore::AVRSemaphore() {}
// get - to claim ownership of the semaphore
bool AVRSemaphore::get(void* caller)
{
bool result = false;
hal.scheduler->begin_atomic();
if( !_taken ) {
_taken = true;
_owner = caller;
result = true;
}
hal.scheduler->end_atomic();
return result;
}
// release - to give up ownership of the semaphore
// returns true if successfully released
bool AVRSemaphore::release(void* caller)
{
// check legitimacy of release call
if( caller != _owner ) {
return false;
}
// if another process is waiting immediately call the provided kontinuation
if( _waiting_k != NULL ) {
// give ownership to waiting process
_owner = _waiting_owner;
AP_HAL::Proc k = _waiting_k;
// clear waiting process
_waiting_k = NULL;
_waiting_owner = NULL;
// callback
k();
}
// give up the semaphore
_taken = false;
return true;
}
// call_on_release - returns true if caller successfully added to the queue to
// be called back
bool AVRSemaphore::call_on_release(void* caller, AP_HAL::Proc k)
{
bool result = false;
hal.scheduler->begin_atomic();
if( _waiting_owner == NULL ) {
_waiting_owner = caller;
_waiting_k = k;
result = true;
}
hal.scheduler->end_atomic();
return result;
}

View File

@ -0,0 +1,32 @@
#ifndef __AP_HAL_AVR_SEMAPHORE_H__
#define __AP_HAL_AVR_SEMAPHORE_H__
#include <AP_HAL.h>
#include <AP_HAL_AVR_Namespace.h>
class AP_HAL_AVR::AVRSemaphore : public AP_HAL::Semaphore {
public:
// Constructor
AVRSemaphore();
// get - to claim ownership of the semaphore
bool get(void* caller);
// release - to give up ownership of the semaphore
bool release(void* caller);
// call_on_release - returns true if caller successfully added to the
// queue to be called back
bool call_on_release(void* caller, AP_HAL::Proc k);
protected:
bool _taken;
void* _owner;
void* _waiting_owner;
// procedure of process waiting for sempahore
AP_HAL::Proc _waiting_k;
};
#endif // __AP_HAL_AVR_SEMAPHORE_H__