LocklessPriorityQueue.hpp

Go to the documentation of this file.

// ======================================================================
// \title Os/Generic/LocklessPriorityQueue.hpp
// \brief lockless ISR-safe priority queue implementation for Os::Queue
// ======================================================================
#ifndef OS_GENERIC_LOCKLESSPRIORITYQUEUE_HPP
#define OS_GENERIC_LOCKLESSPRIORITYQUEUE_HPP

#include <atomic>
#include <limits>
#include <type_traits>
#include "Fw/FPrimeBasicTypes.hpp"
#include "Os/Queue.hpp"
#include "config/LocklessQueueCfg.hpp"

namespace Os {
namespace Generic {

static_assert(std::is_integral<LocklessStateTagType>::value && std::is_unsigned<LocklessStateTagType>::value,
              "LocklessStateTagType must be an unsigned integral type");

template <FwSizeType WIDTH>
struct LocklessAtomicLockFree {
    static constexpr bool value = ((sizeof(unsigned char) == WIDTH) && (ATOMIC_CHAR_LOCK_FREE != 0)) ||
                                  ((sizeof(unsigned short) == WIDTH) && (ATOMIC_SHORT_LOCK_FREE != 0)) ||
                                  ((sizeof(unsigned int) == WIDTH) && (ATOMIC_INT_LOCK_FREE != 0)) ||
                                  ((sizeof(unsigned long) == WIDTH) && (ATOMIC_LONG_LOCK_FREE != 0)) ||
                                  ((sizeof(unsigned long long) == WIDTH) && (ATOMIC_LLONG_LOCK_FREE != 0));
};

static_assert(LocklessAtomicLockFree<sizeof(LocklessStateTagType)>::value,
              "std::atomic<LocklessStateTagType> is never lock-free on this platform; "
              "configure a narrower type in config/LocklessQueueCfg.hpp");
static_assert(LocklessAtomicLockFree<sizeof(U32)>::value, "std::atomic<U32> is never lock-free on this platform");
static_assert(LocklessAtomicLockFree<sizeof(FwQueuePriorityType)>::value,
              "std::atomic<FwQueuePriorityType> is never lock-free on this platform");

static_assert(LOCKLESS_QUEUE_MAX_RETRY_PASSES >= 1, "LOCKLESS_QUEUE_MAX_RETRY_PASSES must be at least 1");

// A zero backoff could livelock a high-priority blocking caller against a lower-priority
// thread on a strict-priority scheduler.
static_assert(LOCKLESS_QUEUE_BLOCKING_BACKOFF_US > 0, "LOCKLESS_QUEUE_BLOCKING_BACKOFF_US must be greater than 0");

static_assert((LOCKLESS_QUEUE_SLOT_ALIGNMENT & (LOCKLESS_QUEUE_SLOT_ALIGNMENT - 1)) == 0,
              "LOCKLESS_QUEUE_SLOT_ALIGNMENT must be a power of two");

enum LocklessSlotState : LocklessStateTagType {
    LOCKLESS_SLOT_FREE = 0,     
    LOCKLESS_SLOT_WRITING = 1,  
    LOCKLESS_SLOT_READY = 2,    
    LOCKLESS_SLOT_READING = 3   
};

struct alignas(LOCKLESS_QUEUE_SLOT_ALIGNMENT) LocklessSlot {
    static constexpr U32 STATE_BITS = 2;
    static constexpr LocklessStateTagType STATE_MASK =
        (static_cast<LocklessStateTagType>(1) << STATE_BITS) - static_cast<LocklessStateTagType>(1);
    static constexpr U32 TAG_BITS = static_cast<U32>(std::numeric_limits<LocklessStateTagType>::digits) - STATE_BITS;

    std::atomic<LocklessStateTagType> m_stateTag;
    std::atomic<U32> m_sequence;
    FwSizeType m_size;
    std::atomic<FwQueuePriorityType> m_priority;

    LocklessSlot();
};

struct LocklessPriorityQueueHandle : public QueueHandle {
    LocklessSlot* m_slots;
    void* m_slotsAllocation;
    U8* m_data;
    FwSizeType m_depth;
    FwSizeType m_messageSize;
    std::atomic<U32> m_sequence;
    std::atomic<U32> m_count;
    std::atomic<U32> m_available;
    std::atomic<U32> m_highMark;
    FwEnumStoreType m_id;

    LocklessPriorityQueueHandle();
};

class LocklessPriorityQueue final : public Os::QueueInterface {
  public:
    static constexpr FwSizeType MAX_RETRY_PASSES = LOCKLESS_QUEUE_MAX_RETRY_PASSES;

    static bool isCandidatePreferred(FwQueuePriorityType candidatePriority,
                                     U32 candidateSequence,
                                     FwQueuePriorityType bestPriority,
                                     U32 bestSequence);

    LocklessPriorityQueue() = default;

    ~LocklessPriorityQueue() override;

    LocklessPriorityQueue(const QueueInterface& other) = delete;

    LocklessPriorityQueue(const QueueInterface* other) = delete;

    LocklessPriorityQueue& operator=(const QueueInterface& other) override = delete;

    Status create(FwEnumStoreType id,
                  const Fw::ConstStringBase& name,
                  FwSizeType depth,
                  FwSizeType messageSize) override;

    void teardown() override;

    Status send(const U8* buffer, FwSizeType size, FwQueuePriorityType priority, BlockingType blockType) override;

    Status receive(U8* destination,
                   FwSizeType capacity,
                   BlockingType blockType,
                   FwSizeType& actualSize,
                   FwQueuePriorityType& priority) override;

    FwSizeType getMessagesAvailable() const override;

    FwSizeType getMessageHighWaterMark() const override;

    QueueHandle* getHandle() override;

    LocklessPriorityQueueHandle m_handle;
};

}  // namespace Generic
}  // namespace Os

#endif  // OS_GENERIC_LOCKLESSPRIORITYQUEUE_HPP