queue.cc

#include <errno.h>
#include <fcntl.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
 
#include <linux/memfd.h>
#include <sys/mman.h>
#include <sys/syscall.h>
#include <sys/types.h>
 
#include "queue.hh"
 
// Convenience wrappers for erroring out
static inline void queue_error(const char* fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    fprintf(stderr, "queue error: ");
    vfprintf(stderr, fmt, args);
    fprintf(stderr, "\n");
    va_end(args);
    abort();
}
static inline void queue_error_errno(const char* fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    fprintf(stderr, "queue error: ");
    vfprintf(stderr, fmt, args);
    fprintf(stderr, " (errno %d)\n", errno);
    va_end(args);
    abort();
}
 
// Initialise the queue
void queue::init(size_t s)
{
    // We're going to use a trick where we mmap two adjacent pages (in virtual memory) that point to the
    // same physical memory. This lets us optimize memory access, by virtue of the fact that we don't need
    // to even worry about wrapping our pointers around until we go through the entire buffer.
 
    // Check that the requested size is a multiple of a page. If it isn't, we're in trouble.
    if(s % getpagesize() != 0)
    {
        queue_error("Requested size (%lu) is not a multiple of the page size (%d)",
                    s,
                    getpagesize());
    }
 
    // Create an anonymous file backed by memory
    if((q.fd = fileno(tmpfile())) == -1)
    {
        queue_error_errno("Could not obtain anonymous file");
    }
 
    // Set buffer size
    if(ftruncate(q.fd, s) != 0)
    {
        queue_error_errno("Could not set size of anonymous file");
    }
 
    // Ask mmap for a good address
    if((q.buffer = (uint8_t*)mmap(
            NULL, 2 * s, PROT_NONE, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0)) == MAP_FAILED)
    {
        queue_error_errno("Could not allocate virtual memory");
    }
 
    // Mmap first region
    if(mmap(q.buffer, s, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_FIXED, q.fd, 0) ==
       MAP_FAILED)
    {
        queue_error_errno("Could not map buffer into virtual memory");
    }
 
    // Mmap second region, with exact address
    if(mmap(q.buffer + s, s, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_FIXED, q.fd, 0) ==
       MAP_FAILED)
    {
        queue_error_errno("Could not map buffer into virtual memory");
    }
 
    // Initialize synchronization primitives
    if(pthread_mutex_init(&q.lock, NULL) != 0)
    {
        queue_error_errno("Could not initialize mutex");
    }
    if(pthread_cond_init(&q.readable, NULL) != 0)
    {
        queue_error_errno("Could not initialize condition variable");
    }
    if(pthread_cond_init(&q.writeable, NULL) != 0)
    {
        queue_error_errno("Could not initialize condition variable");
    }
 
    // Initialize remaining members
    q.size = s;
    q.head = 0;
    q.tail = 0;
    // q.head_seq = 0;
    // q.tail_seq = 0;
}
 
// Destroy / unmap the queue
void queue::destroy()
{
    if(munmap(q.buffer + q.size, q.size) != 0)
    {
        queue_error_errno("Could not unmap buffer");
    }
 
    if(munmap(q.buffer, q.size) != 0)
    {
        queue_error_errno("Could not unmap buffer");
    }
 
    if(close(q.fd) != 0)
    {
        queue_error_errno("Could not close anonymous file");
    }
 
    if(pthread_mutex_destroy(&q.lock) != 0)
    {
        queue_error_errno("Could not destroy mutex");
    }
 
    if(pthread_cond_destroy(&q.readable) != 0)
    {
        queue_error_errno("Could not destroy condition variable");
    }
 
    if(pthread_cond_destroy(&q.writeable) != 0)
    {
        queue_error_errno("Could not destroy condition variable");
    }
}
 
// Put data in the queue
void queue::put(int16_t* data, int16_t data_size)
{
    // Lock the queue mutex
    pthread_mutex_lock(&q.lock);
 
    // Wait for space to become available
    while(q.size - (q.tail - q.head) < data_size + sizeof(data_size))
    {
        pthread_cond_wait(&q.writeable, &q.lock);
    }
 
    // Construct header
    // message_t m;
    // m.len = size;
    //  m.seq = q.tail_seq++;
 
    // Write message
    //  memcpy(q.buffer + q.tail, &m,     sizeof(message_t));
    memcpy(q.buffer + q.tail, &data_size, sizeof(data_size));
    //  memcpy(&q.buffer[q.tail + sizeof(data_size)], data, data_size );
    memcpy(q.buffer + q.tail + sizeof(data_size), data, data_size);
 
    // Increment write index in bytes
    q.tail += data_size + sizeof(data_size);
 
    // Signal the queue is readable
    pthread_cond_signal(&q.readable);
 
    // Unlock the mutex
    pthread_mutex_unlock(&q.lock);
}
 
// Get data from the queue
size_t queue::get(int16_t* data, int16_t max_size)
{
    // Lock the queue mutex
    pthread_mutex_lock(&q.lock);
 
    // Wait for a message that we can successfully consume to reach the front of the queue
    //  message_t m;
    int16_t data_size;
    //  for(;;) {
 
    // Wait for a message to arrive
    while((q.tail - q.head) == 0)
    {
        pthread_cond_wait(&q.readable, &q.lock);
    }
 
    // Read message header
    //    memcpy(&m, &q.buffer[q.head], sizeof(message_t));
    memcpy(&data_size, q.buffer + q.head, sizeof(data_size));
 
    // // Message too long, wait for someone else to consume it
    // if(m.len > max){
    //   while(q.head_seq == m.seq) {
    //  pthread_cond_wait(&q.writeable, &q.lock);
    //   }
    //   continue;
    // }
 
    // We successfully consumed the header of a suitable message, so proceed
    //    break;
    //  }
 
    // Read message body
    //  memcpy(data, &q.buffer[q.head + sizeof(message_t)], m.len);
    memcpy(data, q.buffer + q.head + sizeof(data_size), data_size);
 
    // Consume the message by incrementing the read pointer
    q.head += data_size + sizeof(data_size);
    //  q.head_seq++;
 
    // When read buffer moves into 2nd memory region, we can reset to the 1st region
    if(q.head >= q.size)
    {
        q.head -= q.size;
        q.tail -= q.size;
    }
 
    // Signal the queue is writable
    pthread_cond_signal(&q.writeable);
 
    // Unlock the mutex
    pthread_mutex_unlock(&q.lock);
 
    return data_size;
}