-
Notifications
You must be signed in to change notification settings - Fork 0
/
mq_monotonic.hpp
140 lines (120 loc) · 3.21 KB
/
mq_monotonic.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#include <mqueue.h>
#include <cerrno>
#include <poll.h>
static bool timeCompare(const struct timespec& time_timeout, const struct timespec& time_current)
{
if (time_timeout.tv_sec == time_current.tv_sec)
return time_timeout.tv_nsec < time_current.tv_nsec;
else
return time_timeout.tv_sec < time_current.tv_sec;
}
static ssize_t mq_timedreceive_monotonic(mqd_t mqdes, char *msg_ptr, size_t msg_len, unsigned int *msg_prio, const struct timespec * abs_timeout)
{
ssize_t ret = -1;
errno = ETIMEDOUT;
if (abs_timeout->tv_sec < 0)
{
errno = EINVAL;
return ret;
}
struct timespec tout;
tout.tv_nsec = 0;
tout.tv_sec = 0;
ret = mq_timedreceive(mqdes, msg_ptr, msg_len, msg_prio, &tout);
if (ret < 0) /* FAIL case */
{
struct timespec time_current= {0, 0};
clock_gettime(CLOCK_MONOTONIC, &time_current);
if (timeCompare(*abs_timeout, time_current))
{
errno = ETIMEDOUT;
return ret;
}
else
{
int deltaTimeMs = (abs_timeout->tv_sec - time_current.tv_sec) * 1000 + (abs_timeout->tv_nsec - time_current.tv_nsec) / 1000000;
struct pollfd fdset[1] = {0};
short int pool_res = 0;
int rc = -1;
fdset[0].fd = (int) mqdes;
fdset[0].events = POLLIN;
rc = poll(fdset, 1, deltaTimeMs);
pool_res = fdset[0].revents;
if (rc > 0)
{
errno = 0; /* reset errno */
/* If poll exit receiving an event of POLLIN, the queue can be read */
ret = mq_timedreceive(mqdes, msg_ptr, msg_len, msg_prio, &tout);
}
else if(rc == 0) /* (rc == 0) -> TIMEOUT */
{
errno = ETIMEDOUT;
ret = -1;
}
else /* (rc < 0) -> ERROR */
{
ret = -1;
}
}
}
else /* SUCCES case */
{
errno = 0;
}
return ret;
}
static int mq_timedsend_monotonic(mqd_t mqdes, const char *msg_ptr, size_t msg_len, unsigned msg_prio, const struct timespec *abs_timeout)
{
int ret = -1;
errno = ETIMEDOUT;
if (abs_timeout->tv_sec < 0)
{
errno = EINVAL;
return ret;
}
struct timespec tout;
tout.tv_nsec = 0;
tout.tv_sec = 0;
ret = mq_timedsend(mqdes, msg_ptr, msg_len, msg_prio, &tout);
if (ret < 0) /* FAIL case */
{
struct timespec time_current= {0, 0};
clock_gettime(CLOCK_MONOTONIC, &time_current);
if (timeCompare(*abs_timeout, time_current))
{
errno = ETIMEDOUT;
return ret;
}
else
{
int deltaTimeMs = (abs_timeout->tv_sec - time_current.tv_sec) * 1000 + (abs_timeout->tv_nsec - time_current.tv_nsec) / 1000000;
struct pollfd fdset[1] = {0};
short int pool_res = 0;
int rc = -1;
fdset[0].fd = (int) mqdes;
fdset[0].events = POLLOUT;
rc = poll(fdset, 1, deltaTimeMs);
pool_res = fdset[0].revents;
if (rc > 0)
{
errno = 0; /* reset errno */
/* If poll exit receiving an event of POLLOUT, the queue can be writen */
ret = mq_timedsend(mqdes, msg_ptr, msg_len, msg_prio, &tout);
}
else if(rc == 0) /* (rc == 0) -> TIMEOUT */
{
errno = ETIMEDOUT;
ret = -1;
}
else /* (rc < 0) -> ERROR */
{
ret = -1;
}
}
}
else /* SUCCES case */
{
errno = 0;
}
return ret;
}