smq.c

/* 
 shm-arena    shared memory arena
 Copyright (C) 2006-2008 Lance Arsenault (LGPL v3)


    This file is part of shm-arena.

    shm-arena is free software; you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as
    published by the Free Software Foundation; either version 3 of the
    License, or (at your option) any later version.

    shm-arena is distributed in the hope that it will be useful, but
    WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this program.  If not, see
    <http://www.gnu.org/licenses/>.
*/

#include "config.h"
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdint.h>
#include <sys/time.h>
#include <limits.h>
#include <values.h>
#include <pthread.h>

#include "spew.h"
#include "assert.h"
#include "debug_lock.h"
#include "shm_arena.h"
#include "arena.h"
#include "arena_lock.h"
#include "avl.h"


#define MAGIC  (0xC30F53AD)

/* struct smq_header flags */
#define TIMESTAMPED  (01 << 0) /* entry is time-stamped */

/* A time stamp values that shows that the time stamp is
 * uninitialized. */
#define UNTIMESTAMPED   (-(LDBL_MAX))


typedef int16_t q_length_t;

/* 2^15-2 is max q length = 32766 */
#define MAX_Q_LENGTH   (32766)

struct smq_header
{
  /* for signaling blocking reads */
  pthread_cond_t cond;
  /* for blocking reads */
  pthread_mutex_t mutex;

  uint32_t magic;
  /* number of entries that we can read at most this can increase, but
   * not decrease */
  q_length_t q_length;
  /* the index to the entry that will be written to next starting at
   * 0. */
  q_length_t write_index;
  /* wrap_index is so that a new reading object can read all entries
   * that have been written.  Without something like wrap_index is no
   * way to know how much data can be read from the queue since
   * write_index does not tell you if the queue has wrapped around yet
   * or not. wrap_index increases from 0 until it is q_length + 1 and
   * then it never changes unless the queue gets longer.  Any new
   * reader will see if the circular queue is full or if not how much
   * there is to read. */
  q_length_t wrap_index;
  /* element_size is the entry element size. */
  size_t element_size;

  char entries_seg_name[GEN_NAME_SIZE];

  /* Gets set if there is a blocking read happening in some process.
   * You must get the mutex lock from this struct to read or write to
   * this. */
  int is_blocking_read;

  /* flags keeps a record of how the Shared Multi-Queue is
   * configured. flags bits: TIMESTAMPED */
  uint8_t flags;
};

struct smq
{
  shm_arena_t arena;
  /* pointer to struct smq_header Shared memory segment */
  struct smq_header *header;
  /* timestamp is set to the current write_index where the where the
   * timestamp can go.  If it is NULL the timestamp does not need to
   * be set. */
  long double *timestamp;
  /* entry index of the next read */ 
  q_length_t read_index;
  /* We need a local copy of the q_length so that we can fix the
   * read_index and reconnect to the entries shared memory segment if
   * the q_length in shared memory changes. */
  q_length_t q_length;

  /* Used to protect reading and writing of thread_count. */
  pthread_mutex_t thread_count_mutex;
  /* Used to sync the smq_delete() with smq_unblock_rdlock()
   * threads. So that calls to smq_unblock_rdlock() will not signal
   * when threads at done running, and we don't deadlock the shared
   * memory multi-queue. */
  pthread_cond_t thread_count_cond;
  /* The number of threads running that are trying to unblock the a
   * blocking smq_rdlock() call.  There should be one in most use
   * cases, but we do not restrict it, so there can be any number. */
  int thread_count;

  /* This is non-zero if the next smq_rdlock() call is required to not
   * block as set by smq_unblock_rdlock().  We lock the smq_header
   * mutex to read or write to unblock_rdlock. */
  int unblock_rdlock;

  /* pointer to entries shared memory segment */
  uint8_t *entries_seg;
  /* flags keeps a record of how the shared multi-queue is configured.
   * This must be kept in sync with the flags in struct smq_header in
   * shared memory. */
  uint8_t flags;
};

/********************************************************************
          Multi-Queue Shared Memory Segments Structure
*********************************************************************

     It uses two segments.  The smq_header segment never moves, but
     the entries segment may move if the multi-queue is re-sized making
     the queue longer.  It's very important for the multi-queue to be
     able to increase the q_length as a slower reader program needs to
     read.


              Header Segment

         ----------------------------
         | struct smq_header        |
         ----------------------------


              Entries Segment

         ----------------------------  0
         | entry element_size       |
         | padding                  |
         | long double              |  if(flags&TIMESTAMPED)
         | padding                  |
         |--------------------------|  1
         | entry element_size       |
         | padding                  |
         | long double              |  if(flags&TIMESTAMPED)
         | padding                  |
         |--------------------------|  2
         | entry element_size       |
         | padding                  |
         | long double              |  if(flags&TIMESTAMPED)
         | padding                  |
         |--------------------------|
         |          .               |
                    .
                    .
                    .
         |          .               |
         |--------------------------|  q_length
         | entry element_size       |
         | padding                  |
         | long double              |  if(flags&TIMESTAMPED)
         | padding                  |
         ----------------------------


    Note that there is q_length + 1 entries.  That's because this is a
    circular queue which requires there is one more than the number
    that can be filled, so that it knows when it is wrapped and full.
    The padding puts the data boundary at 8 byte word, using CHUNK
    from arena.h.  The long double and its padding is present if
    flags has the TIMESTAMPED bit set, else it's not present.


**********************************************************************/


static inline
int connect(struct smq *q)
/* We have q->header, and connect to the multi-queue.  The reason that
 * we reconnect is when the q_length is increased or flags changes. */
{
  size_t entry_size;

  if(q->q_length < 0)
    {
      /* This is a new object connection. Make is so that we can read
       * as much of the entries as possible */
      if(q->header->wrap_index > q->header->q_length)
    {
      /* The queue is full and wrapping */
      ASSERT(q->header->wrap_index == q->header->q_length + 1);

      q->read_index = q->header->write_index + 1;
      if(q->read_index > q->header->q_length)
        {
          ASSERT(q->read_index == q->header->q_length + 1);
          q->read_index = 0;
        }
    }
      else
    {
      q->read_index = q->header->write_index - q->header->wrap_index;
      if(q->read_index < 0)
        q->read_index += q->header->q_length + 1;
    }
    }
  else if(q->q_length != q->header->q_length)
    {
      ASSERT(q->q_length < q->header->q_length);

      /* this is a reconnect to a longer queue */
      if(q->read_index > q->header->write_index)
    q->read_index += q->header->q_length - q->q_length;
    }

  entry_size = CHUNKS(q->header->element_size) +
    ((q->header->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0);

  q->entries_seg = (uint8_t *)
    shm_get(q->arena, (q->header->q_length + 1)*entry_size,
        q->header->entries_seg_name, SHM_WITHOUT_RWLOCK);

  if(!q->entries_seg)
    return -1; /* error */

  if((q->flags & TIMESTAMPED) &&
     (q->header->flags & TIMESTAMPED) && q->q_length < 0)
    {
      /* If this a new connection looking for time stamped data we
       * must make sure that any entries in the queue are time
       * stamped, if not advance the read_index until it points to
       * time stamped data or to the current write_index. Un-time
       * stamped data has value UNTIMESTAMPED */
      while(( ( *((long double *)
          (q->entries_seg +
           (q->read_index*entry_size +
            CHUNKS(q->header->element_size)))))
          == UNTIMESTAMPED) &&
        q->read_index != q->header->write_index)
    {
      /* Advance the read_index until read_index == write_index or
       * we have a good time stamp (tv_usec != -1). */
      q->read_index++;
      if(q->read_index > q->header->q_length)
        q->read_index = 0;
    }
    }


  SPEW(_INFO,
       "%sonnected to Shared multi-queue "
       "with queue length %d with entry segment \"%s\" "
       "%s time stamping",
       (q->q_length < 0)?"C":"Rec", q->header->q_length,
       q->header->entries_seg_name,
       ((q->header->flags & TIMESTAMPED)?"with":"without"));

  if(q->q_length >= 0)
    /* If this is the first connection (not a reconnection) to the
     * Shared Multi-Queue don't change the q->flags so that we can add
     * time stamping in smq_get() if it is set in the flags passed to
     * smq_get().*/
     q->flags = q->header->flags;

  q->q_length = q->header->q_length;

  return 0;
}

smq_t smq_get(shm_arena_t arena, size_t element_size,
          int q_length, const char *name, int flags)
{
  struct smq *q = NULL;
  int errno_save;

  SPEW(_INFO, "%s(arena=%p, element_size=%zu, queue_length=%d,"
       "name=\"%s\", flags=%d )", __func__, arena, element_size,
       q_length, name, flags);

  if(q_length > MAX_Q_LENGTH)
    return SPEW_SYS_ERR_RET(NULL, _WARN, EINVAL,
                "%s(,,q_length=%d,,) failed: "
                "the max q_length is %d",
                __func__, q_length, MAX_Q_LENGTH);

  if(element_size == SHM_UNKNOWN_SIZE && (flags & O_CREAT))
    return SPEW_SYS_ERR_RET(NULL, _WARN, EINVAL,
                "%s(,element_size=SHM_UNKNOWN_SIZE,"
                ",,) failed: flag O_CREAT should not "
                "be set to connect to unknow size "
                "element", __func__);

  /* We don't know if this is the entries seg size, but if we create
   * it or it is smaller now it will be this size when we are done. */
  if(q_length < 1) q_length = 1;

  q = (struct smq *) malloc(sizeof(struct smq));
  if(!q) return NULL;

  /* We get the lock first to make this all atomic with respect to the
   * arena file.  If the user has an arena write lock already, it's
   * okay they are recursive. */
  if(shm_arena_wrlock(arena))
    {
      free(q);
      return NULL;
    }

  /* after shm_arena_wrlock() we have an arena or we have a default
   * arena */
  if(!arena)
    {
      pthread_mutex_lock(&_shm_create_arena_mutex);
      arena =  _shm_default_arena;
      pthread_mutex_unlock(&_shm_create_arena_mutex);
    }
  q->arena = arena;
  q->read_index = 0;
  q->q_length = -1; /* mark a new connection */
  q->header = NULL;
  q->entries_seg = NULL;
  q->timestamp = NULL;
  q->flags = ((flags & SHM_TIMESTAMP)?TIMESTAMPED:0);
  q->unblock_rdlock = 0;
  q->thread_count = 0;
  pthread_mutex_init(&q->thread_count_mutex, NULL);
  pthread_cond_init(&q->thread_count_cond, NULL);

  /* We need the segment read-write lock for the Multi-Queue header
   * segment so we will make sure we request them. */

  errno_save = errno;
  errno = 0;
  q->header = (struct smq_header *)
    shm_get(arena, sizeof(struct smq_header), name, 0);
 
  if(q->header)
    {
      errno = errno_save;
      /* the header segment exists already */
      if(q->header->magic != MAGIC)
    {
      SPEW_SYS_ERR(_WARN, EINVAL, "Segment \"%s\" is not a Multi-Queue "
               "bad magic number", name);
      goto fail;
    }
      if(sizeof(struct smq_header) != shm_size(arena, q->header))
    {
      SPEW_SYS_ERR(_WARN, EINVAL, "Segment \"%s\" is wrong size",
               name);
      goto fail;
    }
      if(element_size != q->header->element_size &&
     element_size != SHM_UNKNOWN_SIZE)
    {
      SPEW_SYS_ERR(_WARN, EINVAL,
               "Segment \"%s\" has element_size=%zu not %zu",
               name, q->header->element_size, element_size);
      goto fail;
    }
      if(connect(q)) goto fail;
    }
  else if(element_size == SHM_UNKNOWN_SIZE)
    {
      SPEW_SYS(_INFO, "Shared Multi-Queue \"%s\" does not exist", name);
      goto fail;
    }
  else if(!errno && flags & O_CREAT)
    {
      errno = errno_save;
      /* the header segment does not exist already */
      /* make a new Shared Multi-Queue */

      q->header = (struct smq_header *)
    shm_get(arena, sizeof(struct smq_header), name,
        flags & (~SHM_WITHOUT_RWLOCK) & (~SHM_TIMESTAMP));
      if(!q->header) goto fail;

      q->entries_seg = (uint8_t *)
    shm_get(arena, (q_length + 1)*
        (CHUNKS(element_size) +
         ((q->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0)),
        NULL, flags|O_EXCL|SHM_WITHOUT_RWLOCK);
      if(!q->entries_seg ||
     !shm_name(arena, q->entries_seg, q->header->entries_seg_name,
          GEN_NAME_SIZE))
    {
      shm_remove(arena, q->entries_seg);
      goto fail;
    }

      {
    pthread_condattr_t c_attr;
    int err;

    if((err = pthread_condattr_init(&c_attr)))
      {
        errno = err;
        SPEW(_WARN, "pthread_condattr_init() failed");
        goto fail;
      }
    if((err = pthread_condattr_setpshared(&c_attr,
                          PTHREAD_PROCESS_SHARED)))
      {
        errno = err;
        SPEW(_WARN, "pthread_condattr_setpshared() failed");
        goto fail;
      }
    if((err = pthread_cond_init(&q->header->cond, &c_attr)))
      {
        errno = err;
        SPEW(_WARN, "pthread_cond_init() failed");
        goto fail;
      }
    if((err = pthread_condattr_destroy(&c_attr)))
      {
        errno = err;
        SPEW(_WARN, "pthread_condattr_destroy() failed");
        goto fail;
      }
    
      }
      {
    pthread_mutexattr_t m_attr;
    int err;


    if((err = pthread_mutexattr_init(&m_attr)))
      {
        errno = err;
        SPEW(_WARN, "pthread_mutexattr_init() failed");
        goto fail;
      }
    if((err = pthread_mutexattr_setpshared(&m_attr,
                           PTHREAD_PROCESS_SHARED)))
      {
        errno = err;
        SPEW(_WARN, "pthread_mutexattr_setpshared() failed");
        goto fail;
      }
    if((err = pthread_mutex_init(&q->header->mutex, &m_attr)))
      {
        errno = err;
        SPEW(_WARN, "pthread_mutex_init() failed");
        goto fail;
      }
    if((err = pthread_mutexattr_destroy(&m_attr)))
      {
        errno = err;
        SPEW(_WARN, "pthread_mutexattr_destroy() failed");
        goto fail;
      }
      }

      /* We are creating the Shared Multi-Queue now. */
      q->header->magic = MAGIC;
      q->header->q_length = q_length;
      q->header->write_index = 0;
      q->header->wrap_index = 0;
      q->header->element_size = element_size;
      q->header->is_blocking_read = 0;
      q->header->flags = q->flags;

      q->read_index = 0;
      q->q_length = q_length;
    }
  else if(!errno && !(flags & O_CREAT))
    {
      /* some other failure and errno is set */
      SPEW(_WARN, "Shared Multi-Queue does not exist"
       " O_CREAT must be set to create it");
      errno = EINVAL; /* "Invalid argument" */
      goto fail;
    }
  else /* errno && flags & O_CREAT */
    goto fail;

  if(q->header->q_length > q_length)
    /* don't let the queue length get smaller */
    q_length = q->header->q_length;

  if(q->header->q_length < q_length ||
     (q->flags & TIMESTAMPED && !(q->header->flags & TIMESTAMPED)))
    {
      /* Increase q_length and/or add a time-stamp */
      uint8_t *new_entries_seg;
      size_t new_entry_size, old_entry_size;

      new_entry_size = CHUNKS(q->header->element_size) +
    ((q->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0);
      old_entry_size = CHUNKS(q->header->element_size) +
    ((q->header->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0);

      /* make the new entries segment */
      new_entries_seg = (uint8_t *)
    shm_get(arena, (q_length + 1)*new_entry_size,
        NULL, O_CREAT|O_EXCL|SHM_WITHOUT_RWLOCK);

#ifdef SHM_DEBUG
      /* this should not fail because we have an arena write lock and
       * we just got this segment */
      ASSERT(shm_name(arena, new_entries_seg,
               q->header->entries_seg_name, GEN_NAME_SIZE));
#else
      shm_name(arena, new_entries_seg,
           q->header->entries_seg_name, GEN_NAME_SIZE);
#endif

      /* copy the old entries to the new entries_seg */
      if(new_entry_size == old_entry_size)
    {
      /* Just the queue length changed. */

      if(q->header->write_index)
        /* copy the top of the queue */
        memcpy(new_entries_seg, q->entries_seg,
           (q->header->write_index*new_entry_size));

      ASSERT(q_length > q->header->q_length);
      /* zero middle of the queue */
      memset(new_entries_seg +
         (q->header->write_index*new_entry_size),
         0, (q_length - q->header->q_length)*new_entry_size);

      if(q->header->q_length > q->header->write_index)
        /* copy the bottom of the queue */
        memcpy(new_entries_seg +
           (q->header->write_index +
            (q_length - q->header->q_length)) *
           new_entry_size,
           q->entries_seg + q->header->write_index *
           new_entry_size,
           ((q->header->q_length + 1) - q->header->write_index) *
           new_entry_size);
    }
      else /* new_entry_size > old_entry_size */
    {
      /* We added a time-stamp and the queue length may or may not
       * be longer. */
      q_length_t i, max;
      /* Start with the whole queue zero-ed.  The old entries
       * time-stamps will all be zero. */
      memset(new_entries_seg, 0, (q_length + 1)*new_entry_size);

      /* mark the unset time stamps with UNTIMESTAMPED so that we
       * may see that it is not used yet. */
      max = q_length+1;
      for(i=0;i<max;i++)
        {
          long double *tv;
          tv = (long double *)
        (new_entries_seg +
         (i*new_entry_size + CHUNKS(q->header->element_size)));
          *tv = UNTIMESTAMPED;
        }

      if(q->header->write_index)
        {
          /* copy the top of the queue */
          max = q->header->q_length - q->header->write_index;
          for(i=0;i<max;i++)
        memcpy(new_entries_seg + i*new_entry_size,
               q->entries_seg + i*old_entry_size,
               q->header->element_size);
        }

      if(q->header->q_length > q->header->write_index)
        {
          /* copy the bottom of the queue */
          max = (q->header->q_length + 1) - q->header->write_index;
          for(i=0;i<max;i++)
        memcpy(new_entries_seg +
               (q->header->write_index +
            (q_length - q->header->q_length) +
            i) * new_entry_size,
               q->entries_seg +
               (q->header->write_index + i) * new_entry_size,
               q->header->element_size);
        }
      SPEW(_INFO, "Added time stamping to Shared Multi-Queue"); 
    }

      /* remove the old entries segment */
      if(shm_remove(arena, q->entries_seg))
    goto fail;

      /* Set the read_index */
      if(q->read_index > q->header->write_index &&
     !(q->flags & TIMESTAMPED && !(q->header->flags & TIMESTAMPED)))
    q->read_index += q_length - q->header->q_length - 1;
      else if(q->flags & TIMESTAMPED && !(q->header->flags & TIMESTAMPED))
    /* If we just added time-stamping we set the read_index so
     * that the queue looks like all the current entries have been
     * read, so that we can't read any entries that are not
     * time-stamped. */
    q->read_index = q->header->write_index;

      q->header->q_length = q->q_length = q_length;
      q->header->flags = q->flags;
      q->entries_seg = new_entries_seg;
    }

  if(shm_arena_unlock(arena))
    {
      free(q);
      q = NULL;
    }

  return q;
  
 fail:

  shm_arena_unlock(arena);
  free(q);
  return NULL;
}

int smq_remove(smq_t q)
{
  int ret;
  int err = 0;
  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  if(!q || !q->arena)
    {
      SPEW(_WARN, "Invalid Shared Multi-Queue object");
      errno = EINVAL; /* "Invalid argument" */
      return -1;
    }

  /* We need an exclusive arena write lock to remove shared memory
   * segments.  We use the auto write locker in case the user has the
   * arena write lock already. */
  if(!get_arena_and_autolock(q->arena, 2, &err IF_SPEW(, __func__)))
    {
      errno = err;
      SPEW(_WARN, "Invalid Shared Multi-Queue object");
      return -1;
    }

  ret = smq_wrlock(q, 0);
  if(ret) goto unlock;

  if(q->header->magic != MAGIC)
    {
      shm_unlock(q->arena, q->header);
      SPEW(_WARN, "Bad magic number in Shared Multi-Queue segment header");
      errno = EINVAL; /* "Invalid argument" */
      ret = -1;
      goto unlock;
    }

  shm_unlock(q->arena, q->header);

  q->header->magic = 0;

  ret = shm_remove(q->arena, q->header);

  if(!ret)
    ret = shm_remove(q->arena, q->entries_seg);
  else
    shm_remove(q->arena, q->entries_seg);


 unlock:

  if(!ret)
    err = arena_autounlock(q->arena IF_SPEW(, __func__));
  else
    arena_autounlock(q->arena IF_SPEW(, __func__));

  if(err)
    {
      errno = err;
      ret = -1;
    }

  free(q);

  return ret;
}

size_t smq_element_size(smq_t q)
{
  return q->header->element_size;
}

int smq_delete(smq_t q)
{
  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  /* Wait until all unblock_rdlock_thread() threads are finished.
   * This could keep the inter-process shared memory from getting
   * deadlocked due to the thread getting killed by a process exit
   * when the thread has a lock. */
  pthread_mutex_lock(&q->thread_count_mutex);
  while(q->thread_count)
    {
      pthread_cond_wait(&q->thread_count_cond, &q->thread_count_mutex);
    }
  pthread_mutex_unlock(&q->thread_count_mutex);

  pthread_mutex_destroy(&q->thread_count_mutex);
  pthread_cond_destroy(&q->thread_count_cond);

  /* Lets make the object be really unusable for really stupid
   * users. */
  memset(q, 0, sizeof(struct smq));

  free(q);

  return 0;
}

static inline int check_connect(smq_t q)
// We need a shm_rdlock(q->arena, q->header)
// to call this.
{
  if(q->q_length != q->header->q_length ||
     q->flags != q->header->flags)
    return connect(q);
  return 0;
}

static inline int get_num_entries(smq_t q)
// We need a shm_rdlock(q->arena, q->header)
// to call this.
{
  if(q->header->write_index >= q->read_index)
    return q->header->write_index - q->read_index;
  else
    return 1 + q->q_length +
      q->header->write_index - q->read_index;
}


int smq_rdlock(smq_t q, int num)
{
  int ret, err;

  SPEW(_DEBUG, "%s(q=%p, num=%d)", __func__, q, num);

  if(num < 1) /* nonblocking read */
    {
      int ret;

      ret = shm_rdlock(q->arena, q->header);
      if(ret) return -1;

      ret = check_connect(q);
      
      if(ret)
    shm_unlock(q->arena, q->header);
      else
    return get_num_entries(q);

      return -1;
    }


  if(num > q->q_length)
    num = q->q_length;

  /* blocking read */
  if(shm_rdlock(q->arena, q->header)) return -1;
  
  if(check_connect(q))
    {
      shm_unlock(q->arena, q->header);
      return -1;
    }
  
  ret = get_num_entries(q);

  while(ret < num)
    {
      if((err = pthread_mutex_lock(&q->header->mutex)))
    {
      shm_unlock(q->arena, q->header);
      errno = err;
      SPEW(_WARN, "%s(): pthread_mutex_lock() failed",
           __func__); /* We're screwed */
      return -1;
    }

      if(q->unblock_rdlock)
    {
      q->unblock_rdlock = 0;
      pthread_mutex_unlock(&q->header->mutex);
      return ret;
    }

      q->header->is_blocking_read = 1;

      // TODO: We need to keep the arena read lock
      // but release the segment lock; but we can't
      // until we fix how we create shared
      // memory, it would hang all shm_arena_wrlock()
      // calls.
      if(shm_unlock(q->arena, q->header)) return -1; /* screwed */

      ret = pthread_cond_wait(&q->header->cond, &q->header->mutex);
      if(ret)
    {
      SPEW(_WARN, "%s(): pthread_cond_wait() failed", __func__);
      pthread_mutex_unlock(&q->header->mutex);
          shm_arena_unlock(q->arena);
      return -1;
    }
      
      ret = shm_rdlock(q->arena, q->header);

      pthread_mutex_unlock(&q->header->mutex);

      if(check_connect(q))
    {
      shm_unlock(q->arena, q->header);
      return -1;
    }

      ret = get_num_entries(q);
    }

  return ret;
}

/* This is the worker thread that smq_unblock_rdlock() spawns. */
static void *unblock_rdlock_thread(void *d)
{
  smq_t q;
  int ret;

  q = (smq_t) d;

  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  ret = pthread_mutex_lock(&q->header->mutex);
  if(ret) goto thread_exit; /* We're screwed */

  /* We'll keep the next smq_rdlock() from calling pthread_cond_wait()
   * or we'll call pthread_cond_broadcast().  Either way we will keep
   * the current and the next smq_rdlock() call from blocking. */
  q->unblock_rdlock = 1;

  if(q->header->is_blocking_read)
    {
      q->header->is_blocking_read = 0;
      pthread_cond_broadcast(&q->header->cond);
    }

  pthread_mutex_unlock(&q->header->mutex);

 thread_exit:

  /* Let others know we are done. */
  pthread_mutex_lock(&q->thread_count_mutex);
  q->thread_count--;
  if(q->thread_count == 0)
    pthread_cond_broadcast(&q->thread_count_cond);
  pthread_mutex_unlock(&q->thread_count_mutex);

  return NULL;
}


int smq_unblock_rdlock(smq_t q)
{
  int ret;
  pthread_t thread;

  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  pthread_mutex_lock(&q->thread_count_mutex);
  q->thread_count++;
  pthread_mutex_unlock(&q->thread_count_mutex);

  /* By running the shm_wrlock(), sem_post(), and shm_wrlock() in a
   * different thread we don't run the risk of dead-lock do to calling
   * back-to-back calls of shm_wrlock() in the same thread. */
  ret = pthread_create(&thread, NULL, unblock_rdlock_thread, q);
  if(!ret)
    pthread_detach(thread);
  else
    {
      /* The thread was not created */
      pthread_mutex_lock(&q->thread_count_mutex);
      q->thread_count--;
      pthread_mutex_unlock(&q->thread_count_mutex);
    }

  return ret;
}

int smq_wrlock(smq_t q, int num)
{
  int ret;
  SPEW(_DEBUG, "%s(q=%p, num=%d)", __func__, q, num);
  ret = shm_wrlock(q->arena, q->header);
  if(ret) return ret;

  ret = check_connect(q);
  if(ret)
    shm_unlock(q->arena, q->header);

  return ret;
}

int smq_unlock(smq_t q)
{
  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  /* If we are time-stamping and we haven't time-stamped the last
   * entry we need to do it now. */
  if(q->timestamp)
    {
      struct timeval t;
      gettimeofday(&t, NULL);
      *(q->timestamp) = t.tv_sec + t.tv_usec * 1.0e-6L;
    }

  return shm_unlock(q->arena, q->header);
}

long double *smq_timestamp(smq_t q, const void *ptr)
{
  if(q->flags & TIMESTAMPED)
    {
      /* If this is a writer calling this, we will assume that they
       * will write a time-stamp, so we will mark it as done. */
      if(q->timestamp) q->timestamp = NULL;

      return (long double *)
    (((uint8_t *) ptr) + CHUNKS(q->header->element_size));
    }
  return NULL;
}

void *smq_write(smq_t q)
{
  void *ret;
  int err = 0;

  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  /* If we are time-stamping and we haven't time-stamped the last
   * entry we need to do it now. */
  if(q->timestamp)
    {
      struct timeval tv;
      gettimeofday(&tv, NULL);
      *(q->timestamp) = tv.tv_sec + tv.tv_usec * 1.0e-6L;
    }

  ret = (void *)
    (q->entries_seg +
     ((q->header->write_index)*
      (CHUNKS(q->header->element_size) +
       ((q->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0))));

  q->header->write_index++;
  if(q->header->write_index > q->q_length)
    q->header->write_index = 0;
  if(q->header->wrap_index <= q->q_length)
    q->header->wrap_index++;


  /* Unblock a smq_rdlock() if it is needed. */
  if((err = pthread_mutex_lock(&q->header->mutex)))
    return
      SPEW_SYS_ERR_RET(NULL, _WARN, err,
               "pthread_mutex_lock() failed"); /* We're screwed */
  if(q->header->is_blocking_read)
    {
      q->header->is_blocking_read = 0;
      pthread_cond_broadcast(&q->header->cond);
    }
  pthread_mutex_unlock(&q->header->mutex);



  if(ret && (q->flags & TIMESTAMPED))
    /* This is the only place that q->timestamp is ever set. */
    q->timestamp = (long double *)
      (((uint8_t *) ret) + CHUNKS(q->header->element_size));

  return ret;
}

void *smq_poll(smq_t q)
{
  q_length_t index;

  SPEW(_DEBUG, "%s(q=%p)", __func__, q);

  index = q->header->write_index - 1;
  if(index < 0 && q->header->wrap_index)
    index = q->q_length;
  else if(q->header->wrap_index == 0)
    return NULL; /* there is no data written yet */
  
  /* flush the the queue */
  q->read_index = q->header->write_index;

  return (void *)
    (q->entries_seg +
     index*(
        CHUNKS(q->header->element_size) +
        ((q->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0))
     );
}

void *smq_read(smq_t q)
{
  SPEW(_DEBUG, "%s(q=%p)", __func__, q);
  void *ret;

  /* read queue */
  if(q->read_index == q->header->write_index)
    return NULL;

  ret = (void *)
    (q->entries_seg +
     (q->read_index)*
     (CHUNKS(q->header->element_size) +
      ((q->flags & TIMESTAMPED)?CHUNKS(sizeof(long double)):0)));

  q->read_index++;
  if(q->read_index > q->q_length)
    q->read_index = 0;

  return ret;
}