AP_HAL: utility: add thread safe ring buffer duplicate

libraries/AP_HAL/utility/RingBuffer.h

@@ -2,6 +2,7 @@
 
 #include <atomic>
 #include <stdint.h>
+#include <AP_HAL/AP_HAL.h>
 
 /*
  * Circular buffer of bytes.
@@ -93,6 +94,7 @@ private:
 
 /*
   ring buffer class for objects of fixed size
+  !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
  */
 template <class T>
 class ObjectBuffer {
@@ -109,27 +111,32 @@ public:
     }
 
     // Discards the buffer content, emptying it.
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     void clear(void)
     {
         buffer->clear();
     }
 
     // return number of objects available to be read from the front of the queue
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     uint32_t available(void) const {
         return buffer->available() / sizeof(T);
     }
 
     // return number of objects that could be written to the back of the queue
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     uint32_t space(void) const {
         return buffer->space() / sizeof(T);
     }
 
     // true is available() == 0
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool empty(void) const {
         return buffer->empty();
     }
 
     // push one object onto the back of the queue
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool push(const T &object) {
         if (buffer->space() < sizeof(T)) {
             return false;
@@ -138,6 +145,7 @@ public:
     }
 
     // push N objects onto the back of the queue
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool push(const T *object, uint32_t n) {
         if (buffer->space() < n*sizeof(T)) {
             return false;
@@ -148,6 +156,7 @@ public:
     /*
       throw away an object from the front of the queue
      */
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool pop(void) {
         return buffer->advance(sizeof(T));
     }
@@ -155,6 +164,7 @@ public:
     /*
       pop earliest object off the front of the queue
      */
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool pop(T &object) {
         if (buffer->available() < sizeof(T)) {
             return false;
@@ -167,6 +177,7 @@ public:
      * push_force() is semantically equivalent to:
      *   if (!push(t)) { pop(); push(t); }
      */
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool push_force(const T &object) {
         if (buffer->space() < sizeof(T)) {
             buffer->advance(sizeof(T));
@@ -177,6 +188,7 @@ public:
     /*
      * push_force() N objects
      */
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool push_force(const T *object, uint32_t n) {
         uint32_t _space = buffer->space();
         if (_space < sizeof(T)*n) {
@@ -188,6 +200,7 @@ public:
     /*
       peek copies an object out from the front of the queue without advancing the read pointer
      */
+    // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
     bool peek(T &object) {
         return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T);
     }
@@ -196,6 +209,7 @@ public:
       return a pointer to first contiguous array of available
       objects. Return nullptr if none available
      */
+    // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!!
     const T *readptr(uint32_t &n) {
         uint32_t avail_bytes = 0;
         const T *ret = (const T *)buffer->readptr(avail_bytes);
@@ -207,12 +221,14 @@ public:
     }
 
     // advance the read pointer (discarding objects)
+    // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!!
     bool advance(uint32_t n) {
         return buffer->advance(n * sizeof(T));
     }
     
     /* update the object at the front of the queue (the one that would
        be fetched by pop()) */
+    // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!!
     bool update(const T &object) {
         return buffer->update((uint8_t*)&object, sizeof(T));
     }
@@ -221,7 +237,164 @@ private:
     ByteBuffer *buffer = nullptr;
 };
 
+/*
+  Thread safe ring buffer class for objects of fixed size
+  !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+ */
+template <class T>
+class ObjectBuffer_TS {
+public:
+    ObjectBuffer_TS(uint32_t _size) {
+        // we set size to 1 more than requested as the byte buffer
+        // gives one less byte than requested. We round up to a full
+        // multiple of the object size so that we always get aligned
+        // elements, which makes the readptr() method possible
+        buffer = new ByteBuffer(((_size+1) * sizeof(T)));
+    }
+    ~ObjectBuffer_TS(void) {
+        delete buffer;
+    }
+
+    // Discards the buffer content, emptying it.
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    void clear(void)
+    {
+        WITH_SEMAPHORE(sem);
+        buffer->clear();
+    }
+
+    // return number of objects available to be read from the front of the queue
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    uint32_t available(void) {
+        WITH_SEMAPHORE(sem);
+        return buffer->available() / sizeof(T);
+    }
+
+    // return number of objects that could be written to the back of the queue
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    uint32_t space(void) {
+        WITH_SEMAPHORE(sem);
+        return buffer->space() / sizeof(T);
+    }
+
+    // true is available() == 0
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool empty(void) {
+        WITH_SEMAPHORE(sem);
+        return buffer->empty();
+    }
+
+    // push one object onto the back of the queue
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool push(const T &object) {
+        WITH_SEMAPHORE(sem);
+        if (buffer->space() < sizeof(T)) {
+            return false;
+        }
+        return buffer->write((uint8_t*)&object, sizeof(T)) == sizeof(T);
+    }
+
+    // push N objects onto the back of the queue
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool push(const T *object, uint32_t n) {
+        WITH_SEMAPHORE(sem);
+        if (buffer->space() < n*sizeof(T)) {
+            return false;
+        }
+        return buffer->write((uint8_t*)object, n*sizeof(T)) == n*sizeof(T);
+    }
+
+    /*
+      throw away an object from the front of the queue
+     */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool pop(void) {
+        WITH_SEMAPHORE(sem);
+        return buffer->advance(sizeof(T));
+    }
+
+    /*
+      pop earliest object off the front of the queue
+     */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool pop(T &object) {
+        WITH_SEMAPHORE(sem);
+        if (buffer->available() < sizeof(T)) {
+            return false;
+        }
+        return buffer->read((uint8_t*)&object, sizeof(T)) == sizeof(T);
+    }
+
+    /*
+     * push_force() is semantically equivalent to:
+     *   if (!push(t)) { pop(); push(t); }
+     */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool push_force(const T &object) {
+        WITH_SEMAPHORE(sem);
+        if (buffer->space() < sizeof(T)) {
+            buffer->advance(sizeof(T));
+        }
+        return push(object);
+    }
+
+    /*
+     * push_force() N objects
+     */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool push_force(const T *object, uint32_t n) {
+        WITH_SEMAPHORE(sem);
+        uint32_t _space = buffer->space();
+        if (_space < sizeof(T)*n) {
+            buffer->advance(sizeof(T)*(n-_space));
+        }
+        return push(object, n);
+    }
+
+    /*
+      peek copies an object out from the front of the queue without advancing the read pointer
+     */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool peek(T &object) {
+        WITH_SEMAPHORE(sem);
+        return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T);
+    }
+
+    /*
+      return a pointer to first contiguous array of available
+      objects. Return nullptr if none available
+     */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    const T *readptr(uint32_t &n) {
+        WITH_SEMAPHORE(sem);
+        uint32_t avail_bytes = 0;
+        const T *ret = (const T *)buffer->readptr(avail_bytes);
+        if (!ret || avail_bytes < sizeof(T)) {
+            return nullptr;
+        }
+        n = avail_bytes / sizeof(T);
+        return ret;
+    }
+
+    // advance the read pointer (discarding objects)
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool advance(uint32_t n) {
+        WITH_SEMAPHORE(sem);
+        return buffer->advance(n * sizeof(T));
+    }
 
+    /* update the object at the front of the queue (the one that would
+       be fetched by pop()) */
+    // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
+    bool update(const T &object) {
+        WITH_SEMAPHORE(sem);
+        return buffer->update((uint8_t*)&object, sizeof(T));
+    }
+
+private:
+    ByteBuffer *buffer = nullptr;
+    HAL_Semaphore sem;
+};
 
 /*
   ring buffer class for objects of fixed size with pointer