From b2c44d8a8183ceca3ad5266a26d7483442d8da47 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Wed, 28 Nov 2012 17:53:15 -0800
Subject: [PATCH] AP_HAL: move AP_Semaphore to be part of the HAL

---
 libraries/AP_HAL/Semaphore.h       | 20 +++++++++
 libraries/AP_HAL_AVR/Semaphore.cpp | 69 ++++++++++++++++++++++++++++++
 libraries/AP_HAL_AVR/Semaphore.h   | 32 ++++++++++++++
 3 files changed, 121 insertions(+)
 create mode 100644 libraries/AP_HAL/Semaphore.h
 create mode 100644 libraries/AP_HAL_AVR/Semaphore.cpp
 create mode 100644 libraries/AP_HAL_AVR/Semaphore.h

diff --git a/libraries/AP_HAL/Semaphore.h b/libraries/AP_HAL/Semaphore.h
new file mode 100644
index 0000000000..2394d58f18
--- /dev/null
+++ b/libraries/AP_HAL/Semaphore.h
@@ -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__
diff --git a/libraries/AP_HAL_AVR/Semaphore.cpp b/libraries/AP_HAL_AVR/Semaphore.cpp
new file mode 100644
index 0000000000..f05ba6b501
--- /dev/null
+++ b/libraries/AP_HAL_AVR/Semaphore.cpp
@@ -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;
+}
diff --git a/libraries/AP_HAL_AVR/Semaphore.h b/libraries/AP_HAL_AVR/Semaphore.h
new file mode 100644
index 0000000000..7a8e095e1c
--- /dev/null
+++ b/libraries/AP_HAL_AVR/Semaphore.h
@@ -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__