Implemented an lock free queue based on this paper http://www.cs.rochester.edu/~scott/papers/1996_PODC_queues.pdf, the "lock free queue" we had before wasn't lock free at all.

4 files changed, 284 insertions, 13 deletions

diff --git a/src/rt/sync/interrupt_transparent_queue.cpp b/src/rt/sync/interrupt_transparent_queue.cpp
new file mode 100644
index 00000000000..064b25f17ba
--- /dev/null
+++ b/src/rt/sync/interrupt_transparent_queue.cpp
@@ -0,0 +1,56 @@
+/*
+ * Interrupt transparent queue, Schoen et. al, "On Interrupt-Transparent
+ * Synchronization in an Embedded Object-Oriented Operating System", 2000.
+ * enqueue() is allowed to interrupt enqueue() and dequeue(), however,
+ * dequeue() is not allowed to interrupt itself.
+ */
+
+#include "../globals.h"
+#include "interrupt_transparent_queue.h"
+
+interrupt_transparent_queue_node::interrupt_transparent_queue_node() :
+    next(NULL) {
+
+}
+
+interrupt_transparent_queue::interrupt_transparent_queue() : _tail(this) {
+
+}
+
+void
+interrupt_transparent_queue::enqueue(interrupt_transparent_queue_node *item) {
+    lock.lock();
+    item->next = (interrupt_transparent_queue_node *) NULL;
+    interrupt_transparent_queue_node *last = _tail;
+    _tail = item;
+    while (last->next) {
+        last = last->next;
+    }
+    last->next = item;
+    lock.unlock();
+}
+
+interrupt_transparent_queue_node *
+interrupt_transparent_queue::dequeue() {
+    lock.lock();
+    interrupt_transparent_queue_node *item = next;
+    if (item && !(next = item->next)) {
+        _tail = (interrupt_transparent_queue_node *) this;
+        if (item->next) {
+            interrupt_transparent_queue_node *lost = item->next;
+            interrupt_transparent_queue_node *help;
+            do {
+                help = lost->next;
+                enqueue(lost);
+            } while ((lost = help) !=
+                     (interrupt_transparent_queue_node *) NULL);
+        }
+    }
+    lock.unlock();
+    return item;
+}
+
+bool
+interrupt_transparent_queue::is_empty() {
+    return next == NULL;
+}
diff --git a/src/rt/sync/interrupt_transparent_queue.h b/src/rt/sync/interrupt_transparent_queue.h
new file mode 100644
index 00000000000..7c02d0c8388
--- /dev/null
+++ b/src/rt/sync/interrupt_transparent_queue.h
@@ -0,0 +1,22 @@
+#ifndef INTERRUPT_TRANSPARENT_QUEUE_H
+#define INTERRUPT_TRANSPARENT_QUEUE_H
+
+#include "spin_lock.h"
+
+class interrupt_transparent_queue_node {
+public:
+    interrupt_transparent_queue_node *next;
+    interrupt_transparent_queue_node();
+};
+
+class interrupt_transparent_queue : interrupt_transparent_queue_node {
+    spin_lock lock;
+    interrupt_transparent_queue_node *_tail;
+public:
+    interrupt_transparent_queue();
+    void enqueue(interrupt_transparent_queue_node *item);
+    interrupt_transparent_queue_node *dequeue();
+    bool is_empty();
+};
+
+#endif /* INTERRUPT_TRANSPARENT_QUEUE_H */
diff --git a/src/rt/sync/lock_free_queue.h b/src/rt/sync/lock_free_queue.h
index c1a309b8817..ac0c5b046a0 100644
--- a/src/rt/sync/lock_free_queue.h
+++ b/src/rt/sync/lock_free_queue.h
@@ -1,22 +1,210 @@
 #ifndef LOCK_FREE_QUEUE_H
 #define LOCK_FREE_QUEUE_H
 
-#include "spin_lock.h"
+/**
+ * How and why this lock free queue works:
+ *
+ * Adapted from the paper titled "Simple, Fast, and Practical Non-Blocking
+ * and Blocking Concurrent Queue Algorithms" by Maged M. Michael,
+ * Michael L. Scott.
+ *
+ * Safety Properties:
+ *
+ * 1. The linked list is always connected.
+ * 2. Nodes are only inserted after the last node in the linked list.
+ * 3. Nodes are only deleted from the beginning of the linked list.
+ * 4. Head always points to the first node in the linked list.
+ * 5. Tail always points to a node in the linked list.
+ *
+ *
+ * 1. The linked list is always connected because the next pointer is not set
+ *    to null before the node is freed, and no node is freed until deleted
+ *    from the linked list.
+ *
+ * 2. Nodes are only inserted at the end of the linked list because they are
+ *    linked through the tail pointer which always points to a node in the
+ *    linked list (5) and an inserted node is only linked to a node that has
+ *    a null next pointer, and the only such node is the last one (1).
+ *
+ * 3. Nodes are deleted from the beginning of the list because they are
+ *    deleted only when they are pointed to by head which always points to the
+ *    first node (4).
+ *
+ * 4. Head always points to the first node in the list because it only changes
+ *    its value to the next node atomically. The new value of head cannot be
+ *    null because if there is only one node in the list the dequeue operation
+ *    returns without deleting any nodes.
+ *
+ * 5. Tail always points to a node in the linked list because it never lags
+ *    behind head, so it can never point to a deleted node. Also, when tail
+ *    changes its value it always swings to the next node in the list and it
+ *    never tires to change its value if the next pointer is NULL.
+ */
 
-class lock_free_queue_node {
-public:
-    lock_free_queue_node *next;
-    lock_free_queue_node();
-};
+#include <assert.h>
+template <class T>
+class lock_free_queue {
+
+    struct node_t;
+    struct pointer_t {
+        node_t *node;
+        uint32_t count;
+        pointer_t() : node(NULL), count(0) {
+            // Nop.
+        }
+        pointer_t(node_t *node, uint32_t count) {
+            this->node = node;
+            this->count = count;
+        }
+        bool equals(pointer_t &other) {
+            return node == other.node && count == other.count;
+        }
+    };
+
+    struct node_t {
+        T value;
+        pointer_t next;
+
+        node_t() {
+            next.node = NULL;
+            next.count = 0;
+        }
+
+        node_t(pointer_t next, T value) {
+            this->next = next;
+            this->value = value;
+        }
+    };
+
+    // Always points to the first node in the list.
+    pointer_t head;
+
+    // Always points to a node in the list, (not necessarily the last).
+    pointer_t tail;
+
+    // Compare and swap counted pointers, we can only do this if pointr_t is
+    // 8 bytes or less since that the maximum size CAS can handle.
+    bool compare_and_swap(pointer_t *address,
+        pointer_t *oldValue,
+        pointer_t newValue) {
+
+        if (sync::compare_and_swap(
+                (uint64_t*) address,
+                *(uint64_t*) oldValue,
+                *(uint64_t*) &newValue)) {
+            return true;
+        }
+        return false;
+    }
 
-class lock_free_queue : lock_free_queue_node {
-    spin_lock lock;
-    lock_free_queue_node *_tail;
 public:
-    lock_free_queue();
-    void enqueue(lock_free_queue_node *item);
-    lock_free_queue_node *dequeue();
-    bool is_empty();
+    lock_free_queue() {
+        // We can only handle 64bit CAS for counted pointers, so this will
+        // not work with 64bit pointers.
+        assert (sizeof(pointer_t) == sizeof(uint64_t));
+
+        // Allocate a dummy node to be used as the first node in the list.
+        node_t *node = new node_t();
+
+        // Head and tail both start out pointing to the dummy node.
+        head.node = node;
+        tail.node = node;
+    }
+
+    virtual ~lock_free_queue() {
+        // Delete dummy node.
+        delete head.node;
+    }
+
+    bool is_empty() {
+        return head.node == tail.node;
+    }
+
+    void enqueue(T value) {
+
+        // Create a new node to be inserted in the linked list, and set the
+        // next node to NULL.
+        node_t *node = new node_t();
+        node->value = value;
+        node->next.node = NULL;
+        pointer_t tail;
+
+        // Keep trying until enqueue is done.
+        while (true) {
+            // Read the current tail which may either point to the last node
+            // or to the second to last node (not sure why second to last,
+            // and not any other node).
+            tail = this->tail;
+
+            // Reads the next node after the tail which will be the last node
+            // if null.
+            pointer_t next;
+            if (tail.node != NULL) {
+                next = tail.node->next;
+            }
+
+            // Loop if another thread changed the tail since we last read it.
+            if (tail.equals(this->tail) == false) {
+                continue;
+            }
+
+            // If next is not pointing to the last node try to swing tail to
+            // the last node and loop.
+            if (next.node != NULL) {
+                compare_and_swap(&this->tail, &tail,
+                    pointer_t(next.node, tail.count + 1));
+                continue;
+            }
+
+            // Try to link node at the end of the linked list.
+            if (compare_and_swap(&tail.node->next, &next,
+                    pointer_t(node, next.count + 1))) {
+                // Enqueueing is done.
+                break;
+            }
+        }
+
+        // Enqueue is done, try to swing tail to the inserted node.
+        compare_and_swap(&this->tail, &tail,
+            pointer_t(node, tail.count + 1));
+    }
+
+    bool dequeue(T *value) {
+        pointer_t head;
+
+        // Keep trying until dequeue is done.
+        while(true) {
+            head = this->head;
+            pointer_t tail = this->tail;
+            pointer_t next = head.node->next;
+
+            if (head.equals(this->head) == false) {
+                continue;
+            }
+
+            // If queue is empty, or if tail is falling behind.
+            if (head.node == tail.node) {
+                // If queue is empty.
+                if (next.node == NULL) {
+                    return false;
+                }
+                // Tail is falling behind, advance it.
+                compare_and_swap(&this->tail,
+                    &tail,
+                    pointer_t(next.node, tail.count + 1));
+            } else {
+                // Read value before CAS, otherwise another
+                // dequeue might advance it.
+                *value = next.node->value;
+                if (compare_and_swap(&this->head, &head,
+                    pointer_t(next.node, head.count + 1))) {
+                    break;
+                }
+            }
+        }
+        delete head.node;
+        return true;
+    }
 };
 
 #endif /* LOCK_FREE_QUEUE_H */
diff --git a/src/rt/sync/sync.h b/src/rt/sync/sync.h
index 902b5661be4..8c9a13f0f4a 100644
--- a/src/rt/sync/sync.h
+++ b/src/rt/sync/sync.h
@@ -4,6 +4,11 @@
 class sync {
 public:
     static void yield();
+    template <class T>
+    static bool compare_and_swap(T *address,
+        T oldValue, T newValue) {
+        return __sync_bool_compare_and_swap(address, oldValue, newValue);
+    }
 };
 
 #endif /* SYNC_H */