Queues now use flags

[mirrors/Programs.git] / c / pthread_extra / pthread_msgqueue.c

diff --git a/c/pthread_extra/pthread_msgqueue.c b/c/pthread_extra/pthread_msgqueue.c
index 0d4e07ea78c4f58cecc6b4f804cd85e9e0c9d9b6..52395d6d3944b5b4c8484357992b6864bd9f844c 100644 (file)
--- a/c/pthread_extra/pthread_msgqueue.c
+++ b/c/pthread_extra/pthread_msgqueue.c
@@ -1,33 +1,20 @@
-/*
- * CFLAGS=-lpthread make pthread_msgqueue
- */
-
 #include <pthread.h>
+#include <pthread_extra.h>
 #include <time.h>
 #include <string.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <stdlib.h>
+#include <assert.h>
 
-typedef struct pthread_mq_t {
-       static pthread_mutex_t lock;
-       static pthread_cond_t cond_readable;
-       static pthread_cond_t cond_writable;
-       void * data;
-       size_t msg_size;
-       size_t msg_count;
-       size_t msg_count_max;
-       size_t head_idx;
-       char * name;
-} pthread_mq_t;
-
-bool pthread_mq_readable(pthread_mq_t *mq) { return (mq->count > 0); }
-bool pthread_mq_writable(pthread_mq_t *mq) { return (mq->count < mq->count_max); }
+bool pthread_mq_readable(pthread_mq_t *mq) { return (mq->msg_count > 0); }
+bool pthread_mq_writable(pthread_mq_t *mq) { return (mq->msg_count < mq->msg_count_max); }
 
 bool pthread_mq_init(pthread_mq_t *mq, size_t msg_size, size_t msg_count_max) {
-       mq->lock = PTHREAD_MUTEX_INITIALIZER;
-       mq->cond_readable = PTHREAD_COND_INITIALIZER;
-       mq->cond_writable = PTHREAD_COND_INITIALIZER;
+       pthread_mutex_init(&mq->lock, NULL);
+       pthread_cond_init(&mq->cond_readable, NULL);
+       pthread_cond_init(&mq->cond_writable, NULL);
+       //pthread_mutexattr_setclock(&mq->lock, CLOCK_MONOTONIC);
        mq->data = malloc(msg_size*msg_count_max);
        mq->msg_size = msg_size;
        mq->msg_count_max = msg_count_max;
@@ -43,51 +30,99 @@ void pthread_mq_free(pthread_mq_t *mq) {
        free(mq->data);
 }
 
-
 void pthread_mq_cond(pthread_mq_t *mq) {
        if(pthread_mq_readable(mq)) pthread_cond_broadcast(&mq->cond_readable);
        if(pthread_mq_writable(mq)) pthread_cond_broadcast(&mq->cond_writable);
 }
 
-bool pthread_mq_send_generic(pthread_mq_t *mq, void * data, bool to_front, bool block, const struct timespec *restrict abs_timeout) {
-       pthread_mutex_timedlock(&mq->lock, abs_timeout);
+size_t pthread_mq_waiting(pthread_mq_t *mq) {
+       return mq->msg_count;
+}
+
+size_t pthread_mq_vacant(pthread_mq_t *mq) {
+       return (mq->msg_count_max - mq->msg_count);
+}
+
+bool pthread_mq_reset(pthread_mq_t *mq) {
+       if(pthread_mutex_lock(&mq->lock)) return false;
+       mq->msg_count = 0;
+       mq->head_idx = 0;
+       pthread_mq_cond(mq);
+       pthread_mutex_unlock(&mq->lock);
+       return true;
+}
+
+bool pthread_mq_send_generic(pthread_mq_t *mq, void * data, pthread_mq_flags_t flags, const struct timespec *restrict abs_timeout) {
+       //printf("S-Timed: %p\n", abs_timeout);
+       int ret;
+
+       //Lock queue
+       if(pthread_mutex_timedlock(&mq->lock, abs_timeout)) return false;
 
+       //Wait for queue to be in writable condition
        while(!pthread_mq_writable(mq)) {
-               pthread_mutex_unlock(&mq->lock);
-               return false;
+               //printf("S+Timed: %p\n", abs_timeout);
+               if(abs_timeout == NULL) {
+                       ret = pthread_cond_wait(&mq->cond_writable, &mq->lock);
+               } else {
+                       //printf("STimed: %p\n", abs_timeout);
+                       //assert(abs_timeout != NULL);
+                       ret = pthread_cond_timedwait(&mq->cond_writable, &mq->lock, abs_timeout);
+               }
+               if(ret) {
+                       pthread_mutex_unlock(&mq->lock);
+                       return false;
+               }
        }
 
-       size_t idx = ( ( mq->count_max + mq->head_idx + (to_front?-1:mq->count) ) % mq->count_max );
+       //Write data to queue
+       bool to_front = (flags & PTHREAD_XMQ_FRONT);
+       size_t idx = ( ( mq->head_idx + (to_front?mq->msg_count_max-1:mq->msg_count) ) % mq->msg_count_max );
        void *ptr = mq->data + (idx * mq->msg_size);
        mq->msg_count++;
+       if(to_front) mq->head_idx = (mq->msg_count_max + mq->head_idx - 1) % mq->msg_count_max;
        memcpy(ptr, data, mq->msg_size);
 
+       //Signal conditions and unlock
        pthread_mq_cond(mq);
        pthread_mutex_unlock(&mq->lock);
        return true;
 }
 
-/*
-              mq_timedreceive(3)   mq_timedreceive(2)
-
-              mq_timedsend(3)      mq_timedsend(2)
-
-void vQueueDelete( QueueHandle_t xQueue );
-BaseType_t xQueueReset( QueueHandle_t xQueue );
-
- BaseType_t xQueueSend(
-                            QueueHandle_t xQueue,
-                            const void * pvItemToQueue,
-                            TickType_t xTicksToWait
-                         );
- BaseType_t xQueueReceive(
-                               QueueHandle_t xQueue,
-                               void *pvBuffer,
-                               TickType_t xTicksToWait
-                            );
+bool pthread_mq_receive_generic(pthread_mq_t *mq, void * data, pthread_mq_flags_t flags, const struct timespec *restrict abs_timeout) {
+       int ret;
+
+       //Lock queue
+       if(pthread_mutex_timedlock(&mq->lock, abs_timeout)) return false;
+
+       //Wait for queue to be in readable condition
+       while(!pthread_mq_readable(mq)) {
+               if(abs_timeout == NULL) {
+                       ret = pthread_cond_wait(&mq->cond_readable, &mq->lock);
+               } else {
+                       //printf("RTimed: %p\n", abs_timeout);
+                       //assert(abs_timeout != NULL);
+                       ret = pthread_cond_timedwait(&mq->cond_readable, &mq->lock, abs_timeout);
+               }
+               if(ret) {
+                       pthread_mutex_unlock(&mq->lock);
+                       return false;
+               }
+       }
 
-*/
+       //Read data from queue
+       void *ptr = mq->data + (mq->head_idx * mq->msg_size);
+       memcpy(data, ptr, mq->msg_size);
 
-int main() {
+       //Delete data from queue if not peeking
+       bool peek = (flags & PTHREAD_XMQ_PEEK);
+       if(!peek) {
+               mq->msg_count--;
+               mq->head_idx = (mq->head_idx+1) % mq->msg_count_max;
+       }
 
+       //Signal conditions and unlock
+       pthread_mq_cond(mq);
+       pthread_mutex_unlock(&mq->lock);
+       return true;
 }