Queue

/**
 * \file src/queue.c
 * \brief Thread-safe FIFO queue
 * \author *
 *
 * Implements a thread-safe FIFO buffer that may be used to send data between
 * two threads. The FIFO pre-allocates number of slots when initialized. These
 * slots are then used to store data that has been written by the producer
 * prior to reading by the consumer.
 *
 */

#include "config.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <semaphore.h>
#include <poll.h>

#include "utils.h"
#include "queue.h"

/**
 * \brief Initialize a queue structure
 * \param *queue The queue structure to initialize
 * \param num_slots The number of slots in the queue
 * \param slot_size The size of each slot in the queue
 * \return 0 on success, -1 on failure
 *
 * Initializes the queue structure, and allocates memory for the buffers. The
 * queue is initially empty, and is available for access (not blocked).
 * Failures can result from insufficient memory.
 */
int queue_init(struct queue *queue, uint32_t num_slots, uint32_t slot_size)
{
    queue->buffer = malloc((uint64_t)num_slots * (uint64_t)slot_size);
    queue->slot_size = slot_size;
    if (queue->buffer == NULL) {
        return -1;
    }

    queue->num_slots = num_slots;
    queue->write_pointer = 0;
    queue->read_pointer = 0;

    sem_init(&queue->empty_count, 0, num_slots);
    sem_init(&queue->full_count, 0, 0);

    return 0;
}

/**
 * \brief Destroy a queue
 * \param *queue The queue to destroy
 * \return 0 on success, -1 on failure
 *
 * Destroys a queue structure, and frees all associated memory. The semaphores
 * are released as well.
 * This function will block if another queue is currently using the queue.
 */
int queue_destroy(struct queue *queue)
{
    sem_destroy(&queue->empty_count);
    sem_destroy(&queue->full_count);
    free(queue->buffer);

    return 0;
}

/**
 * \brief Add an entry to the queue
 * \param *queue The queue to modify
 * \param *data Pointer to the data to add to the queue
 * \param data_size Size of the data to add
 * \return 0 on success, -1 on failure
 *
 * Add data to the queue. This will use up one available slot. This function
 * is for internal use by the library, and won't modify necessary parts
 * of the data structure unless called from queue_add or queue_try_add.
 */
inline static int __add(struct queue *queue, void *data, size_t data_size)
{
    void *wp = queue->buffer + ((uint64_t)queue->write_pointer * (uint64_t)queue->slot_size);
    memcpy(wp, data, data_size);
    queue->write_pointer++;
    if (queue->write_pointer >= queue->num_slots) {
        queue->write_pointer = 0;
    }
    return 0;
}

/**
 * \brief Add an entry to the queue
 * \param *queue The queue to modify
 * \param *data Pointer to the data to add to the queue
 * \param data_size Size of the data to add
 * \return 0 on success, -1 on failure
 *
 * Add data to the queue. This will use up one available slot. This function
 * will block until there is at least one empty slot available in the queue.
 */
int queue_add(struct queue *queue, void *data, size_t data_size)
{
    if (data_size > queue->slot_size) {
        return -1;
    }

    sem_wait(&queue->empty_count);

    __add(queue, data, data_size);

    sem_post(&queue->full_count);

    return 0;
}

/**
 * \brief Add an entry to the queue
 * \param *queue The queue to modify
 * \param *data Pointer to the data to add to the queue
 * \param data_size Size of the data to add
 * \return 0 on success, -1 on failure
 *
 * Add data to the queue. This will use up one available slot. This function
 * will return an error if there are no empty slots available in the queue.
 */
int queue_try_add(struct queue *queue, void *data, size_t data_size)
{
    if (data_size > queue->slot_size) {
        return -1;
    }

    if (-1 == sem_trywait(&queue->empty_count)) return -1;

    __add(queue, data, data_size);

    sem_post(&queue->full_count);

    return 0;
}

/**
 * \brief Add an entry to the queue
 * \param *queue The queue to modify
 * \param *data Pointer to the data to add to the queue
 * \param data_size Size of the data to add
 * \param timeout timeout in milliseconds
 * \return 0 on success, -1 on failure
 *
 * Add data to the queue. This will use up one available slot. This function
 * will return an error if there are no empty slots available in the queue.
 */
int queue_try_add_timeout(struct queue *queue, void *data, size_t data_size, int timeout)
{
    if (data_size > queue->slot_size) {
        return -1;
    }

    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    add_ms_to_timespec(&ts, timeout);

    if (-1 == sem_timedwait(&queue->empty_count, &ts)) return -1;

    __add(queue, data, data_size);

    sem_post(&queue->full_count);

    return 0;
}

inline static void __remove(struct queue *queue, void *data, size_t data_size)
{
    if (data_size > queue->slot_size) {
        data_size = queue->slot_size;
    }
    void *rp = queue->buffer + ((uint64_t)queue->read_pointer * (uint64_t)queue->slot_size);
    memcpy(data, rp, data_size);
    queue->read_pointer++;
    if (queue->read_pointer >= queue->num_slots) {
        queue->read_pointer = 0;
    }
}

/**
 * \brief Remove a value from the queue
 * \param *queue Pointer to the queue to remove a value from
 * \param *data Pointer to the buffer where data is copied
 * \param *data_size Size of the data buffer
 * \return 0 if a value is removed, -1 otherwise
 *
 * This function will remove an item from the queue. If the queue is empty and
 * there are no more items to remove, it will return an error.
 */
int queue_try_remove(struct queue *queue, void *data, size_t data_size)
{
    if (-1 == sem_trywait(&queue->full_count)) return -1;

    __remove(queue, data, data_size);

    sem_post(&queue->empty_count);

    return 0;
}

/**
 * \brief Remove a value from the queue with timeout
 * \param *queue Pointer to the queue to remove a value from
 * \param *data Pointer to the buffer where data is copied
 * \param *data_size Size of the data buffer
 * \param timeout timeout in milliseconds
 * \return 0 if a value is removed, -1 otherwise
 *
 * This function will remove an item from the queue. If the queue is empty and
 * there are no more items to remove, it will return an error.
 */
int queue_try_remove_timeout(struct queue *queue, void *data, size_t data_size, int timeout)
{
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    add_ms_to_timespec(&ts, timeout);

    if (-1 == sem_timedwait(&queue->full_count, &ts)) return -1;

    __remove(queue, data, data_size);

    sem_post(&queue->empty_count);

    return 0;
}

/**
 * \brief Remove a value from the queue
 * \param *queue Pointer to the queue to remove a value from
 * \param *data Pointer to the buffer where data is copied
 * \param *data_size Size of the data buffer
 * \return Always returns 0
 *
 * This function will remove an item from the queue. If the queue is empty and
 * there are no more items to remove, it will block.
 */
int queue_remove(struct queue *queue, void *data, size_t data_size)
{
    sem_wait(&queue->full_count);

    __remove(queue, data, data_size);

    sem_post(&queue->empty_count);

    return 0;
}

/**
 * \brief Queries if the queue is empty
 * \param *queue The queue to query
 * \return 1 if the queue is empty, 0 if the queue is not empty
 *
 * This function tests if the queue is empty. The queue empty-status guaranteed
 * to be the return value if this is the only thread reading from the queue
 */
int queue_get_empty(struct queue *queue)
{
    return (queue_get_empties(queue) == queue->num_slots) ? 1 : 0;
}

/**
 * \brief Queries if the queue is full
 * \param *queue The queue to query
 * \return 1 if the queue is full, 0 if the queue is not full
 *
 * This function tests if the queue is full. The queue full-status guaranteed
 * to be the return value if this is the only thread writing to the queue
 */
int queue_get_full(struct queue *queue)
{
    return (queue_get_size(queue) == queue->num_slots) ? 1 : 0;
}

/**
 * \brief Returns number of elements in the queue
 * \param *queue The queue to query
 * \return Number of elements in queue
 *
 * This function returns the number of queue entries that are used. This is
 * guaranteed to be the correct value only if this function is called from the
 * thread that is writing to the queue
 */
int queue_get_size(struct queue *queue)
{
    int cnt;

    sem_getvalue(&queue->full_count, &cnt);
    return cnt;
}

/**
 * \brief Returns number of empty elements in queue
 * \param *queue The queue to query
 * \return Number of empty elements in queue
 *
 * This function returns the number of queue entries that are free. This is
 * guaranteed to be the correct value only if this function is called from the
 * thread that is reading from the queue
 */
int queue_get_empties(struct queue *queue)
{
    int cnt;

    sem_getvalue(&queue->empty_count, &cnt);
    return cnt;
}