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
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
|
/*
* This Source Code Form is subject to the terms of the Mozilla Public License,
* v. 2.0. If a copy of the MPL was not distributed with this file, You can
* obtain one at https://mozilla.org/MPL/2.0/.
*/
#include <errno.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/net/http/server.h>
#include <zephyr/net/websocket.h>
#include <zephyr/sys_clock.h>
#include <zephyr/zbus/zbus.h>
#include "heart.h"
LOG_MODULE_REGISTER(ws);
#define WS_NUM_HANDLERS 1
#define WS_HANDLER_STACK_SIZE 2048
#define WS_HANDLER_PRIO K_PRIO_PREEMPT(8)
#define WS_CONNECTION_BACKLOG_LEN 5
#define WS_TX_BUFFER_LEN 150
#define WS_CLIENT_HEARTBEAT_GRACE_TIME_MS 10000
K_MSGQ_DEFINE(ws_connection_backlog, sizeof(int), WS_CONNECTION_BACKLOG_LEN, 1);
ZBUS_MSG_SUBSCRIBER_DEFINE(ws_tx_messages);
ZBUS_CHAN_ADD_OBS(heartbeat_channel, ws_tx_messages, 3);
static void ws_tx_thread_function(void *ptr1, void *ptr2, void *ptr3) {
int fd = -1;
struct k_msgq *tx_fds = ptr1;
uint8_t buffer[WS_TX_BUFFER_LEN];
const struct zbus_channel *chan;
struct heartbeat heartbeat;
while (true) {
int ret = zbus_obs_set_enable(&ws_tx_messages, false);
if (ret < 0) {
LOG_ERR("Could not disable zbus observer (%d)", ret);
continue;
}
ret = k_msgq_get(tx_fds, &fd, K_FOREVER);
if (ret < 0) {
LOG_ERR("Error getting new file descriptor (%d)", ret);
continue;
}
LOG_INF("Handling TX of connection with file descriptor %d", fd);
ret = zbus_obs_set_enable(&ws_tx_messages, true);
if (ret < 0) {
LOG_ERR("Could not enable zbus observer (%d)", ret);
continue;
}
while (true) {
ret = zbus_sub_wait_msg(&ws_tx_messages, &chan, &heartbeat, K_FOREVER);
if (ret < 0) {
LOG_ERR("Could not wait for zbus messages (%d)", ret);
break;
}
if (chan != &heartbeat_channel) {
LOG_ERR("Unsupported channel");
continue;
}
ret = snprintf((char *)buffer, sizeof(buffer),
"{\"type\":0,\"ttl_ms\":%d}\n", heartbeat.ttl_ms);
if (ret < 0) {
LOG_ERR("Could not serialize heartbeat message");
continue;
}
size_t len = ret;
int ret = websocket_send_msg(
fd, (const uint8_t *)buffer, len,
WEBSOCKET_OPCODE_DATA_TEXT, false, true, 100
);
if (ret < 0) {
if (ret == -EBADF) {
LOG_DBG("Connection closed, waiting for new one");
} else {
LOG_ERR("Error on websocket_send_msg (%d)", ret);
}
break;
}
}
}
}
static int ws_recv_full_message(int fd, void *buf, const size_t len,
uint32_t *message_type, int64_t deadline) {
uint64_t remaining = 0;
size_t read = 0;
int ret;
while (true) {
int64_t timeout = MIN(50, deadline - k_uptime_get());
if (timeout <= 0) {
ret = -ETIMEDOUT;
LOG_ERR("No time left to read message from %d (%d)", fd, ret);
return ret;
}
if (read >= len) {
ret = -ENOSPC;
LOG_ERR("No buffer space left to store message from %d (%d)", fd, ret);
return ret;
}
ret = websocket_recv_msg(fd, (unsigned char *)buf+read, len-read,
message_type, &remaining, timeout);
if (ret < 0) {
if (ret == -EAGAIN || ret == -EINTR) {
LOG_DBG("Trying again to read full message");
continue;
} else {
LOG_ERR("Unhandled error on message reading (%d)", ret);
return ret;
}
}
read += ret;
if (remaining <= 0) {
return read;
}
}
}
static int ws_handle_data_text(const uint8_t *buf, size_t len, int64_t *deadline) {
LOG_DBG("Got text message");
static const char heartbeat[] = "{\"type\":0,\"ttl_ms\":1100}\n";
if (len + 1 == sizeof(heartbeat)) {
if (memcmp((const void *)buf, heartbeat, len) == 0) {
LOG_DBG("Received heartbeat from client");
*deadline = k_uptime_get() + WS_CLIENT_HEARTBEAT_GRACE_TIME_MS;
return 0;
}
}
LOG_HEXDUMP_DBG(buf, len, "Could not handle message");
return -EIO;
}
static int ws_rx_handle_connection(int fd) {
LOG_INF("Handling RX of connection with file descriptor %d", fd);
uint8_t rx_buf[100];
uint32_t message_type = 0;
int64_t rx_heartbeat_deadline = k_uptime_get() + WS_CLIENT_HEARTBEAT_GRACE_TIME_MS;
while (true) {
memset(rx_buf, 0, sizeof(rx_buf));
int ret = ws_recv_full_message(fd, rx_buf, sizeof(rx_buf),
&message_type, rx_heartbeat_deadline);
if (ret < 0) {
LOG_ERR("Could not receive full message from %d (%d)", fd, ret);
return ret;
}
size_t len = ret;
LOG_DBG("Received message with opcode %d of length %d", message_type, len);
if (message_type == WEBSOCKET_OPCODE_DATA_TEXT ||
message_type == 3) { // FIXME for some reason opcode 1 parses to 3
ret = ws_handle_data_text(rx_buf, len, &rx_heartbeat_deadline);
if (ret < 0) {
LOG_ERR("Failed to handle text data (%d)", ret);
}
} else if (message_type == WEBSOCKET_OPCODE_CLOSE) {
LOG_INF("Client closed connection");
return 0;
} else {
LOG_WRN("Received unhandled message opcode %d", message_type);
LOG_HEXDUMP_WRN(rx_buf, len, "Message content:");
}
if (k_uptime_get() > rx_heartbeat_deadline) {
LOG_INF("Client heartbeat timeout expired on %d - closing", fd);
return -ETIMEDOUT;
}
}
}
static void ws_rx_thread_function(void *ptr1, void *ptr2, void *ptr3) {
struct k_msgq *tx_fds = ptr1;
while (true) {
int fd;
int ret = k_msgq_get(&ws_connection_backlog, &fd, K_FOREVER);
if (ret < 0) {
LOG_ERR("Error getting new file descriptor (%d)", ret);
continue;
}
ret = k_msgq_put(tx_fds, (const void *)&fd, K_FOREVER);
if (ret < 0) {
LOG_ERR("Could not pass file descriptor %d to TX thread (%d)", fd, ret);
goto unregister;
}
ret = ws_rx_handle_connection(fd);
if (ret < 0) {
LOG_ERR("Failed to handle connection %d (%d)", fd, ret);
goto unregister;
}
unregister:
ret = websocket_unregister(fd);
if (ret < 0) {
LOG_ERR("Failed to unregister connection %d (%d)", fd, ret);
}
}
}
K_MSGQ_DEFINE(ws_tx_fd_1, sizeof(int), 1, 1);
K_THREAD_DEFINE(ws_rx_thread_1, WS_HANDLER_STACK_SIZE,
ws_rx_thread_function, &ws_tx_fd_1, NULL, NULL,
WS_HANDLER_PRIO, 0, 0);
K_THREAD_DEFINE(ws_tx_thread_1, WS_HANDLER_STACK_SIZE,
ws_tx_thread_function, &ws_tx_fd_1, NULL, NULL,
WS_HANDLER_PRIO, 0, 0);
int ws_upgrade_handler(
int ws_socket,
struct http_request_ctx *request_ctx,
void *user_data
) {
LOG_DBG("Handling WebSocket upgrade for file descriptor %d", ws_socket);
int ret = k_msgq_put(&ws_connection_backlog, (const void *)&ws_socket,
K_NO_WAIT);
if (ret < 0) {
LOG_ERR("Connection backlog full, dropping file descriptor %d", ws_socket);
return -ENOENT;
}
LOG_INF("Added file descriptor %d to connection backlog", ws_socket);
return 0;
}
|
