EE192 Team 4 / telemetry

telemetry

Dependents:   Everything Sequential_Timing FixedPWM FixedPWMWill

queue.h@0:aca5a32d2759, 2016-03-18 (annotated)

Committer:
vsutardja
Date:
Fri Mar 18 22:33:32 2016 +0000
Revision:
0:aca5a32d2759
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsutardja 0:aca5a32d2759 1 #include <stddef.h>
vsutardja 0:aca5a32d2759 2 #include <stdint.h>
vsutardja 0:aca5a32d2759 3
vsutardja 0:aca5a32d2759 4 #ifndef _QUEUE_H_
vsutardja 0:aca5a32d2759 5 #define _QUEUE_H_
vsutardja 0:aca5a32d2759 6
vsutardja 0:aca5a32d2759 7 namespace telemetry {
vsutardja 0:aca5a32d2759 8 /**
vsutardja 0:aca5a32d2759 9 * Statically allocated, lock-free queue.
vsutardja 0:aca5a32d2759 10 * Thread-safe if used in single-producer single-consumer mode.
vsutardja 0:aca5a32d2759 11 */
vsutardja 0:aca5a32d2759 12 template <typename T, size_t N> class Queue {
vsutardja 0:aca5a32d2759 13 public:
vsutardja 0:aca5a32d2759 14 Queue() : read_ptr(values), write_ptr(values), begin(values), last(values + N) {}
vsutardja 0:aca5a32d2759 15
vsutardja 0:aca5a32d2759 16 // Return true if the queue is full (enqueue will return false).
vsutardja 0:aca5a32d2759 17 bool full() const {
vsutardja 0:aca5a32d2759 18 if (read_ptr == begin) {
vsutardja 0:aca5a32d2759 19 // Read pointer at beginning, need to wrap around check.
vsutardja 0:aca5a32d2759 20 if (write_ptr == last) {
vsutardja 0:aca5a32d2759 21 return true;
vsutardja 0:aca5a32d2759 22 } else {
vsutardja 0:aca5a32d2759 23 return false;
vsutardja 0:aca5a32d2759 24 }
vsutardja 0:aca5a32d2759 25 } else {
vsutardja 0:aca5a32d2759 26 if (write_ptr == (read_ptr - 1)) {
vsutardja 0:aca5a32d2759 27 return true;
vsutardja 0:aca5a32d2759 28 } else {
vsutardja 0:aca5a32d2759 29 return false;
vsutardja 0:aca5a32d2759 30 }
vsutardja 0:aca5a32d2759 31 }
vsutardja 0:aca5a32d2759 32 }
vsutardja 0:aca5a32d2759 33
vsutardja 0:aca5a32d2759 34 // Return true if the queue is empty (dequeue will return false).
vsutardja 0:aca5a32d2759 35 bool empty() const {
vsutardja 0:aca5a32d2759 36 return (read_ptr == write_ptr);
vsutardja 0:aca5a32d2759 37 }
vsutardja 0:aca5a32d2759 38
vsutardja 0:aca5a32d2759 39 /**
vsutardja 0:aca5a32d2759 40 * Puts a new value to the tail of the queue. Returns true if successful,
vsutardja 0:aca5a32d2759 41 * false if not.
vsutardja 0:aca5a32d2759 42 */
vsutardja 0:aca5a32d2759 43 bool enqueue(const T& value) {
vsutardja 0:aca5a32d2759 44 if (full()) {
vsutardja 0:aca5a32d2759 45 return false;
vsutardja 0:aca5a32d2759 46 }
vsutardja 0:aca5a32d2759 47
vsutardja 0:aca5a32d2759 48 //memcpy((void*)write_ptr, &value, sizeof(T)); // Make it array-compatible.
vsutardja 0:aca5a32d2759 49 *write_ptr = value;
vsutardja 0:aca5a32d2759 50
vsutardja 0:aca5a32d2759 51 if (write_ptr == last) {
vsutardja 0:aca5a32d2759 52 write_ptr = begin;
vsutardja 0:aca5a32d2759 53 } else {
vsutardja 0:aca5a32d2759 54 write_ptr++;
vsutardja 0:aca5a32d2759 55 }
vsutardja 0:aca5a32d2759 56
vsutardja 0:aca5a32d2759 57 return true;
vsutardja 0:aca5a32d2759 58 }
vsutardja 0:aca5a32d2759 59
vsutardja 0:aca5a32d2759 60 /**
vsutardja 0:aca5a32d2759 61 * Assigns output to the last element in the queue.
vsutardja 0:aca5a32d2759 62 */
vsutardja 0:aca5a32d2759 63 bool dequeue(T* output) {
vsutardja 0:aca5a32d2759 64 if (empty()) {
vsutardja 0:aca5a32d2759 65 return false;
vsutardja 0:aca5a32d2759 66 }
vsutardja 0:aca5a32d2759 67
vsutardja 0:aca5a32d2759 68 //memcpy(output, (void*)read_ptr, sizeof(T)); // Make it array-compatible.
vsutardja 0:aca5a32d2759 69 *output = *read_ptr;
vsutardja 0:aca5a32d2759 70
vsutardja 0:aca5a32d2759 71 if (read_ptr == last) {
vsutardja 0:aca5a32d2759 72 read_ptr = begin;
vsutardja 0:aca5a32d2759 73 } else {
vsutardja 0:aca5a32d2759 74 read_ptr++;
vsutardja 0:aca5a32d2759 75 }
vsutardja 0:aca5a32d2759 76
vsutardja 0:aca5a32d2759 77 return true;
vsutardja 0:aca5a32d2759 78 }
vsutardja 0:aca5a32d2759 79
vsutardja 0:aca5a32d2759 80 protected:
vsutardja 0:aca5a32d2759 81 // Lots of volatiles to prevent compiler reordering which could corrupt data
vsutardja 0:aca5a32d2759 82 // when accessed by multiple threads. Yes, it's completely overkill, but
vsutardja 0:aca5a32d2759 83 // memory fences aren't in earlier C++ versions.
vsutardja 0:aca5a32d2759 84 volatile T values[N+1];
vsutardja 0:aca5a32d2759 85
vsutardja 0:aca5a32d2759 86 // Read pointer, points to next element to be returned by dequeue.
vsutardja 0:aca5a32d2759 87 // Queue is empty if this equals write_ptr. Must never be incremented
vsutardja 0:aca5a32d2759 88 // past write_ptr.
vsutardja 0:aca5a32d2759 89 volatile T* volatile read_ptr;
vsutardja 0:aca5a32d2759 90 // Write pointer, points to next location to be written by enqueue.
vsutardja 0:aca5a32d2759 91 // Must never be incremented to read_ptr. Queue is full when this is one
vsutardja 0:aca5a32d2759 92 // less than read_ptr.
vsutardja 0:aca5a32d2759 93 volatile T* volatile write_ptr;
vsutardja 0:aca5a32d2759 94
vsutardja 0:aca5a32d2759 95 // Pointer to beginning of array, cleaner than using values directly.
vsutardja 0:aca5a32d2759 96 volatile T* const begin;
vsutardja 0:aca5a32d2759 97 // Pointer to one past the last element of the array.
vsutardja 0:aca5a32d2759 98 volatile T* const last;
vsutardja 0:aca5a32d2759 99 };
vsutardja 0:aca5a32d2759 100
vsutardja 0:aca5a32d2759 101 }
vsutardja 0:aca5a32d2759 102
vsutardja 0:aca5a32d2759 103 #endif