c/pthread_extra/pthread_msgqueue.c

   1 /*
   2  * CFLAGS=-lpthread make pthread_msgqueue
   3  */
   4
   5 #include <pthread.h>
   6 #include <time.h>
   7 #include <string.h>
   8 #include <stdio.h>
   9 #include <stdbool.h>
  10 #include <stdlib.h>
  11 #include <assert.h>
  12
  13 typedef struct pthread_mq_t {
  14         pthread_mutex_t lock;
  15         pthread_cond_t cond_readable;
  16         pthread_cond_t cond_writable;
  17         void * data;
  18         size_t msg_size;
  19         size_t msg_count;
  20         size_t msg_count_max;
  21         size_t head_idx;
  22         char * name;
  23 } pthread_mq_t;
  24
  25 bool pthread_mq_readable(pthread_mq_t *mq) { return (mq->msg_count > 0); }
  26 bool pthread_mq_writable(pthread_mq_t *mq) { return (mq->msg_count < mq->msg_count_max); }
  27
  28 bool pthread_mq_init(pthread_mq_t *mq, size_t msg_size, size_t msg_count_max) {
  29         pthread_mutex_init(&mq->lock, NULL);
  30         pthread_cond_init(&mq->cond_readable, NULL);
  31         pthread_cond_init(&mq->cond_writable, NULL);
  32         mq->data = malloc(msg_size*msg_count_max);
  33         mq->msg_size = msg_size;
  34         mq->msg_count_max = msg_count_max;
  35         mq->msg_count = 0;
  36         mq->head_idx = 0;
  37         mq->name = NULL;
  38
  39         if(((msg_size*msg_count_max) > 0) && mq->data == NULL) return false;
  40         return true;
  41 }
  42
  43 void pthread_mq_free(pthread_mq_t *mq) {
  44         free(mq->data);
  45 }
  46
  47 void pthread_mq_cond(pthread_mq_t *mq) {
  48         if(pthread_mq_readable(mq)) pthread_cond_broadcast(&mq->cond_readable);
  49         if(pthread_mq_writable(mq)) pthread_cond_broadcast(&mq->cond_writable);
  50 }
  51
  52 bool pthread_mq_reset(pthread_mq_t *mq) {
  53         if(pthread_mutex_lock(&mq->lock)) return false;
  54         mq->msg_count = 0;
  55         mq->head_idx = 0;
  56         pthread_mq_cond(mq);
  57         pthread_mutex_unlock(&mq->lock);
  58         return true;
  59 }
  60
  61 bool pthread_mq_send_generic(pthread_mq_t *mq, void * data, bool to_front, bool block, const struct timespec *restrict abs_timeout) {
  62         //Lock queue
  63         if(pthread_mutex_timedlock(&mq->lock, abs_timeout)) return false;
  64
  65         //Wait for queue to be in writable condition
  66         while(!pthread_mq_writable(mq)) {
  67                 if(pthread_cond_timedwait(&mq->cond_writable, &mq->lock, abs_timeout)) {
  68                         pthread_mutex_unlock(&mq->lock);
  69                         return false;
  70                 }
  71         }
  72
  73         //Write data to queue
  74         size_t idx = ( ( mq->msg_count_max + mq->head_idx + (to_front?-1:mq->msg_count) ) % mq->msg_count_max );
  75         void *ptr = mq->data + (idx * mq->msg_size);
  76         mq->msg_count++;
  77         if(to_front) mq->head_idx = (mq->msg_count_max + mq->head_idx - 1) % mq->msg_count_max;
  78         memcpy(ptr, data, mq->msg_size);
  79
  80         //Signal conditions and unlock
  81         pthread_mq_cond(mq);
  82         pthread_mutex_unlock(&mq->lock);
  83         return true;
  84 }
  85
  86 int main() {
  87
  88 }