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
  12 typedef struct pthread_mq_t {
  13         static pthread_mutex_t lock;
  14         static pthread_cond_t cond_readable;
  15         static pthread_cond_t cond_writable;
  16         void * data;
  17         size_t msg_size;
  18         size_t msg_count;
  19         size_t msg_count_max;
  20         size_t head_idx;
  21         char * name;
  22 } pthread_mq_t;
  23
  24 bool pthread_mq_readable(pthread_mq_t *mq) { return (mq->count > 0); }
  25 bool pthread_mq_writable(pthread_mq_t *mq) { return (mq->count < mq->count_max); }
  26
  27 bool pthread_mq_init(pthread_mq_t *mq, size_t msg_size, size_t msg_count_max) {
  28         mq->lock = PTHREAD_MUTEX_INITIALIZER;
  29         mq->cond_readable = PTHREAD_COND_INITIALIZER;
  30         mq->cond_writable = PTHREAD_COND_INITIALIZER;
  31         mq->data = malloc(msg_size*msg_count_max);
  32         mq->msg_size = msg_size;
  33         mq->msg_count_max = msg_count_max;
  34         mq->msg_count = 0;
  35         mq->head_idx = 0;
  36         mq->name = NULL;
  37
  38         if(((msg_size*msg_count_max) > 0) && mq->data == NULL) return false;
  39         return true;
  40 }
  41
  42 void pthread_mq_free(pthread_mq_t *mq) {
  43         free(mq->data);
  44 }
  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_send_generic(pthread_mq_t *mq, void * data, bool to_front, bool block, const struct timespec *restrict abs_timeout) {
  53         pthread_mutex_timedlock(&mq->lock, abs_timeout);
  54
  55         while(!pthread_mq_writable(mq)) {
  56                 pthread_mutex_unlock(&mq->lock);
  57                 return false;
  58         }
  59
  60         size_t idx = ( ( mq->count_max + mq->head_idx + (to_front?-1:mq->count) ) % mq->count_max );
  61         void *ptr = mq->data + (idx * mq->msg_size);
  62         mq->msg_count++;
  63         memcpy(ptr, data, mq->msg_size);
  64
  65         pthread_mq_cond(mq);
  66         pthread_mutex_unlock(&mq->lock);
  67         return true;
  68 }
  69
  70 /*
  71               mq_timedreceive(3)   mq_timedreceive(2)
  72
  73               mq_timedsend(3)      mq_timedsend(2)
  74
  75 void vQueueDelete( QueueHandle_t xQueue );
  76 BaseType_t xQueueReset( QueueHandle_t xQueue );
  77
  78  BaseType_t xQueueSend(
  79                             QueueHandle_t xQueue,
  80                             const void * pvItemToQueue,
  81                             TickType_t xTicksToWait
  82                          );
  83  BaseType_t xQueueReceive(
  84                                QueueHandle_t xQueue,
  85                                void *pvBuffer,
  86                                TickType_t xTicksToWait
  87                             );
  88
  89 */
  90
  91 int main() {
  92
  93 }