Add a lock-free queue to core-util

CHANGELOG.md

@@ -3,6 +3,9 @@ All notable changes to this project will be documented in this file.
 This project adheres to [Semantic Versioning](http://semver.org/).
 
 ## [Unreleased]
+### Fixed
+- A race condition in `PoolAllocator::alloc()`
+
 
 ## [1.6.0] 2016-03-07
 ### Fixed

core-util/atomic-queue.h

@@ -0,0 +1,76 @@
+/*
+ * SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CORE_UTIL_ATOMIC_QUEUE_H
+#define CORE_UTIL_ATOMIC_QUEUE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct lockfree_queue_element {
+    struct lockfree_queue_element * volatile next;
+    void * data;
+};
+
+struct lockfree_queue {
+    struct lockfree_queue_element * volatile tail;
+};
+
+/**
+ * \brief Add an element to the tail of the queue
+ *
+ * Since the queue only maintains a tail pointer, this simply inserts the new element before the tail pointer
+ *
+ * @param[in,out] q the queue structure to operate on
+ * @param[in] e The element to add to the queue
+ */
+void lfq_push_tail(struct lockfree_queue * q, struct lockfree_queue_element * e);
+/**
+ * \brief Get an element from the head of the queue
+ *
+ * This function iterates over the queue and removes an element from the head when it finds the head. This is slower
+ * than maintaining a head pointer, but it is necessary to ensure that a pop is completely atomic.
+ *
+ * @param[in,out] q The queue to pop from
+ * @return The popped element or NULL if the queue was empty
+ */
+struct lockfree_queue_element * lfq_pop_head(struct lockfree_queue * q);
+/**
+ * Check if there are any elements in the queue
+ *
+ * Note that there is no guarantee that a queue which is not empty when this API is called will not be become empty
+ * before lfq_pop_head is called
+ *
+ * @retval non-zero when the queue is empty
+ * @retval 0 when the queue is not empty
+ */
+int lfq_empty(struct lockfree_queue * q);
+/**
+ * Iterates over the queue and counts the elements in the queue
+ *
+ * The value returned by this function may be invalid by the time it returns. Do not depend on this value except in
+ * a critical section.
+ *
+ * @return the number of elements in the queue
+ */
+unsigned lfq_count(struct lockfree_queue * q);
+#ifdef __cplusplus
+}
+#endif
+
+#endif // CORE_UTIL_ATOMIC_QUEUE_H

source/PoolAllocator.cpp

@@ -33,9 +33,9 @@ PoolAllocator::PoolAllocator(void *start, size_t elements, size_t element_size,
 
 void* PoolAllocator::alloc() {
     uintptr_t prev_free = reinterpret_cast<uintptr_t>(_free_block);
-    if (0 == prev_free)
-        return NULL;
     while (true) {
+        if (0 == prev_free)
+            return NULL;
         void **const new_free = (void **)(*((void **)prev_free));
         if (atomic_cas((uintptr_t*)&_free_block, &prev_free, (uintptr_t)new_free)) {
             return (void*)prev_free;

source/atomic-queue.c

@@ -0,0 +1,96 @@
+/*
+ * SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "core-util/assert.h"
+#include "core-util/atomic-queue.h"
+#include <stddef.h>
+#include "cmsis.h"
+
+int atomic_cas_deref_uint32( uint32_t * volatile *ptr, uint32_t ** currentValue, uint32_t expectedValue, uint32_t *newValue, uintptr_t offset) {
+    uint32_t *current;
+    current = (uint32_t *)__LDREXW((volatile uint32_t *)ptr);
+    if (currentValue != NULL) {
+        *currentValue = current;
+    }
+    if (current == NULL) {
+        return -1;
+    } else if ( *(uint32_t *)((uintptr_t)current + offset) != expectedValue) {
+        return 1;
+    } else if(!__STREXW((uint32_t)newValue, (volatile uint32_t *)ptr)) {
+        return 0;
+    } else {
+        return -1;
+    }
+}
+
+void lfq_push_tail(struct lockfree_queue * q, struct lockfree_queue_element * e)
+{
+    CORE_UTIL_ASSERT_MSG(q != NULL, "null queue used");
+    if (q == NULL) {
+        return;
+    }
+    do {
+        e->next = q->tail;
+    } while (!__sync_bool_compare_and_swap(&q->tail, e->next, e));
+}
+
+struct lockfree_queue_element * lfq_pop_head(struct lockfree_queue * q)
+{
+    CORE_UTIL_ASSERT_MSG(q != NULL, "null queue used");
+    if (q == NULL) {
+        return NULL;
+    }
+    struct lockfree_queue_element * current;
+    int fail = 1;
+    while (fail) {
+        // Set the element reference pointer to the tail pointer
+        struct lockfree_queue_element * volatile * px = &q->tail;
+        if (*px == NULL) {
+            return NULL;
+        }
+        fail = 1;
+        while (fail > 0) {
+            fail = atomic_cas_deref_uint32((uint32_t * volatile *)px,
+                            (uint32_t **)&current,
+                            (uint32_t) NULL,
+                            NULL,
+                            offsetof(struct lockfree_queue_element, next));
+            if (fail == 1) {
+                px = &current->next;
+            }
+        }
+    }
+    return current;
+}
+
+
+int lfq_empty(struct lockfree_queue * q)
+{
+    return q->tail == NULL;
+}
+
+unsigned lfq_count(struct lockfree_queue *q)
+{
+    unsigned x;
+    struct lockfree_queue_element * volatile e;
+    if (lfq_empty(q)) {
+        return 0;
+    }
+    e = q->tail;
+    for (x = 1; e->next != NULL; x++, e = e->next);
+    return x;
+}