-/*
- * 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;
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 );
+ //Handle overwrite
+ assert(!(flags & PTHREAD_XMQ_OVERW) && "FIXME: Overwrite not implemented yet!");
+
+ //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;
}