2 #include <pthread_extra.h>
10 bool pthread_mq_readable(pthread_mq_t
*mq
) { return (mq
->msg_count
> 0); }
11 bool pthread_mq_writable(pthread_mq_t
*mq
) { return (mq
->msg_count
< mq
->msg_count_max
); }
13 bool pthread_mq_init(pthread_mq_t
*mq
, size_t msg_size
, size_t msg_count_max
) {
14 pthread_mutex_init(&mq
->lock
, NULL
);
15 pthread_cond_init(&mq
->cond_readable
, NULL
);
16 pthread_cond_init(&mq
->cond_writable
, NULL
);
17 //pthread_mutexattr_setclock(&mq->lock, CLOCK_MONOTONIC);
18 mq
->data
= malloc(msg_size
*msg_count_max
);
19 mq
->msg_size
= msg_size
;
20 mq
->msg_count_max
= msg_count_max
;
25 if(((msg_size
*msg_count_max
) > 0) && mq
->data
== NULL
) return false;
29 void pthread_mq_free(pthread_mq_t
*mq
) {
33 void pthread_mq_cond(pthread_mq_t
*mq
) {
34 if(pthread_mq_readable(mq
)) pthread_cond_broadcast(&mq
->cond_readable
);
35 if(pthread_mq_writable(mq
)) pthread_cond_broadcast(&mq
->cond_writable
);
38 size_t pthread_mq_waiting(pthread_mq_t
*mq
) {
42 bool pthread_mq_reset(pthread_mq_t
*mq
) {
43 if(pthread_mutex_lock(&mq
->lock
)) return false;
47 pthread_mutex_unlock(&mq
->lock
);
51 bool pthread_mq_send_generic(pthread_mq_t
*mq
, void * data
, bool to_front
, const struct timespec
*restrict abs_timeout
) {
52 //printf("S-Timed: %p\n", abs_timeout);
56 if(pthread_mutex_timedlock(&mq
->lock
, abs_timeout
)) return false;
58 //Wait for queue to be in writable condition
59 while(!pthread_mq_writable(mq
)) {
60 //printf("S+Timed: %p\n", abs_timeout);
61 if(abs_timeout
== NULL
) {
62 ret
= pthread_cond_wait(&mq
->cond_writable
, &mq
->lock
);
64 //printf("STimed: %p\n", abs_timeout);
65 //assert(abs_timeout != NULL);
66 ret
= pthread_cond_timedwait(&mq
->cond_writable
, &mq
->lock
, abs_timeout
);
69 pthread_mutex_unlock(&mq
->lock
);
75 size_t idx
= ( ( mq
->head_idx
+ (to_front
?mq
->msg_count_max
-1:mq
->msg_count
) ) % mq
->msg_count_max
);
76 void *ptr
= mq
->data
+ (idx
* mq
->msg_size
);
78 if(to_front
) mq
->head_idx
= (mq
->msg_count_max
+ mq
->head_idx
- 1) % mq
->msg_count_max
;
79 memcpy(ptr
, data
, mq
->msg_size
);
81 //Signal conditions and unlock
83 pthread_mutex_unlock(&mq
->lock
);
87 bool pthread_mq_receive_generic(pthread_mq_t
*mq
, void * data
, bool peek
, const struct timespec
*restrict abs_timeout
) {
91 if(pthread_mutex_timedlock(&mq
->lock
, abs_timeout
)) return false;
93 //Wait for queue to be in readable condition
94 while(!pthread_mq_readable(mq
)) {
95 if(abs_timeout
== NULL
) {
96 ret
= pthread_cond_wait(&mq
->cond_readable
, &mq
->lock
);
98 //printf("RTimed: %p\n", abs_timeout);
99 //assert(abs_timeout != NULL);
100 ret
= pthread_cond_timedwait(&mq
->cond_readable
, &mq
->lock
, abs_timeout
);
103 pthread_mutex_unlock(&mq
->lock
);
108 //Read data from queue
109 void *ptr
= mq
->data
+ (mq
->head_idx
* mq
->msg_size
);
110 memcpy(data
, ptr
, mq
->msg_size
);
112 //Delete data from queue if not peeking
115 mq
->head_idx
= (mq
->head_idx
+1) % mq
->msg_count_max
;
118 //Signal conditions and unlock
120 pthread_mutex_unlock(&mq
->lock
);
This page took 0.680984 seconds and 4 git commands to generate.