dbdb65ffc7166d3138a684a310cb72a6e7a6845e
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
) {
55 if(pthread_mutex_timedlock(&mq
->lock
, abs_timeout
)) return false;
57 //Wait for queue to be in writable condition
58 while(!pthread_mq_writable(mq
)) {
59 if(abs_timeout
== NULL
) {
60 ret
= pthread_cond_wait(&mq
->cond_writable
, &mq
->lock
);
62 ret
= pthread_cond_timedwait(&mq
->cond_writable
, &mq
->lock
, abs_timeout
);
65 pthread_mutex_unlock(&mq
->lock
);
71 size_t idx
= ( ( mq
->msg_count_max
+ mq
->head_idx
+ (to_front
?-1:mq
->msg_count
) ) % mq
->msg_count_max
);
72 void *ptr
= mq
->data
+ (idx
* mq
->msg_size
);
74 if(to_front
) mq
->head_idx
= (mq
->msg_count_max
+ mq
->head_idx
- 1) % mq
->msg_count_max
;
75 memcpy(ptr
, data
, mq
->msg_size
);
77 //Signal conditions and unlock
79 pthread_mutex_unlock(&mq
->lock
);
83 bool pthread_mq_receive_generic(pthread_mq_t
*mq
, void * data
, bool peek
, const struct timespec
*restrict abs_timeout
) {
87 if(pthread_mutex_timedlock(&mq
->lock
, abs_timeout
)) return false;
89 //Wait for queue to be in readable condition
90 while(!pthread_mq_readable(mq
)) {
91 if(abs_timeout
== NULL
) {
92 ret
= pthread_cond_wait(&mq
->cond_readable
, &mq
->lock
);
94 ret
= pthread_cond_timedwait(&mq
->cond_readable
, &mq
->lock
, abs_timeout
);
97 pthread_mutex_unlock(&mq
->lock
);
102 //Read data from queue
103 void *ptr
= mq
->data
+ (mq
->head_idx
* mq
->msg_size
);
104 memcpy(data
, ptr
, mq
->msg_size
);
106 //Delete data from queue if not peeking
109 mq
->head_idx
= (mq
->head_idx
+1) % mq
->msg_count_max
;
112 //Signal conditions and unlock
114 pthread_mutex_unlock(&mq
->lock
);
This page took 0.41952 seconds and 3 git commands to generate.