#include "lamp_tx_queue.h"

#if defined(__GNUC__) || defined(__clang__)
#define LAMP_COMPILER_BARRIER() __asm__ volatile ("" ::: "memory")
#else
#error "TX queue requires a compiler barrier implementation"
#endif

void lamp_tx_queue_init(lamp_tx_queue *queue)
{
    if (queue == NULL) {
        return;
    }
    queue->head = 0u;
    queue->tail = 0u;
}

bool lamp_tx_queue_push(lamp_tx_queue *queue, const uint8_t *bytes, size_t length)
{
    uint8_t head;
    uint8_t tail;
    size_t used;

    if (queue == NULL || bytes == NULL || length == 0u ||
        length > LAMP_TX_QUEUE_USABLE) {
        return false;
    }
    head = queue->head;
    LAMP_COMPILER_BARRIER();
    tail = queue->tail;
    used = (size_t)((head - tail) & LAMP_TX_QUEUE_MASK);
    if (length > LAMP_TX_QUEUE_USABLE - used) {
        return false;
    }
    for (size_t i = 0u; i < length; ++i) {
        queue->bytes[head] = bytes[i];
        head = (uint8_t)((head + 1u) & LAMP_TX_QUEUE_MASK);
    }
    LAMP_COMPILER_BARRIER();
    queue->head = head;
    return true;
}

bool lamp_tx_queue_pop(lamp_tx_queue *queue, uint8_t *byte)
{
    uint8_t tail;
    uint8_t head;

    if (queue == NULL || byte == NULL) {
        return false;
    }
    tail = queue->tail;
    LAMP_COMPILER_BARRIER();
    head = queue->head;
    if (tail == head) {
        return false;
    }
    *byte = queue->bytes[tail];
    tail = (uint8_t)((tail + 1u) & LAMP_TX_QUEUE_MASK);
    LAMP_COMPILER_BARRIER();
    queue->tail = tail;
    return true;
}

size_t lamp_tx_queue_count(const lamp_tx_queue *queue)
{
    uint8_t head;
    uint8_t tail;

    if (queue == NULL) {
        return 0u;
    }
    head = queue->head;
    LAMP_COMPILER_BARRIER();
    tail = queue->tail;
    return (size_t)((head - tail) & LAMP_TX_QUEUE_MASK);
}
