Diff: USBSerial/CircBuffer.h

Branch:

feature_WebUSB

Revision:

77:a98f786d05d4

Parent:

56:151ba33713ff

--- a/USBSerial/CircBuffer.h	Thu Jul 19 12:57:27 2018 +0200
+++ b/USBSerial/CircBuffer.h	Tue Jul 24 13:08:29 2018 +0200
@@ -53,6 +53,79 @@
         return(!empty);
     };
 
+    void flush()
+    {
+        write = 0;
+        read = 0;
+    }
+
+    // Queue a block of data of blockSize items
+    void queue(const T *block, uint16_t blockSize)
+    {
+        if (blockSize >= size)
+        {
+            // Block is too big to fit in buffer, take the last size-1 items
+            block = &block[blockSize - (size-1)];
+            blockSize = size-1;
+        }
+
+        if (write + blockSize > size)
+        {
+            // Need to wrap around
+            std::memcpy(&buf[write], block, sizeof(T)*(size-write));
+            std::memcpy(buf, &block[size-write], sizeof(T)*(blockSize - (size-write)));
+        }
+        else
+        {
+            std::memcpy(&buf[write], block, sizeof(T)*blockSize);
+        }
+
+        // Update write position
+        uint16_t wasFree = available() - size - 1;
+        write = write + blockSize;
+        write %= size;
+        if (wasFree < blockSize)
+        {
+            // Update read position as well
+            read = write + 1;
+            read %= size;
+        }
+    }
+
+    // Dequeue a block of data of at most blockSize items, writing them into block
+    // Returns the number of items dequeued
+    uint16_t dequeue(T *block, uint16_t blockSize)
+    {
+        if (isEmpty())
+        {
+            return 0;
+        }
+
+        uint16_t isAvailable = available();
+        if (isAvailable < blockSize)
+        {
+            // Only return what we have
+            blockSize = isAvailable;
+        }
+
+        if (read + blockSize > size)
+        {
+            // Need to wrap around
+            std::memcpy(block, &buf[read], sizeof(T)*(size-read));
+            std::memcpy(&block[size-read], buf, sizeof(T)*(blockSize - (size-read)));
+        }
+        else
+        {
+            std::memcpy(block, &buf[read], sizeof(T)*blockSize);
+        }
+
+        // Update read position
+        read = read + blockSize;
+        read %= size;
+
+        return blockSize;
+    }
+
 private:
     volatile uint16_t write;
     volatile uint16_t read;