matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Main hub of the remote driver.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @ingroup drv_remote
8 */
9
10#include "r_internal.h"
11
12#include "util/u_var.h"
13#include "util/u_misc.h"
14#include "util/u_debug.h"
15#include "util/u_space_overseer.h"
16
17#include <stdio.h>
18#include <stdlib.h>
19#include <string.h>
20
21#if defined(XRT_OS_WINDOWS)
22#include <winsock2.h>
23#include <ws2tcpip.h>
24#include "xrt/xrt_windows.h"
25#else
26#include <unistd.h>
27#include <sys/socket.h>
28#include <sys/types.h>
29#include <netdb.h>
30#include <arpa/inet.h>
31#include <netinet/in.h>
32#include <netinet/tcp.h>
33#endif
34
35#ifndef _BSD_SOURCE
36#define _BSD_SOURCE // same, but for musl // NOLINT
37#endif
38
39#ifndef __USE_MISC
40#define __USE_MISC // SOL_TCP on C11
41#endif
42
43// Define the format to use to print a socket descriptor
44#ifdef XRT_OS_WINDOWS
45// On Windows, this is a SOCKET, aka an unsigned long long
46#define R_SOCKET_FMT "%llu"
47#else
48// On non-Windows, this is a file descriptor, aka an int
49#define R_SOCKET_FMT "%i"
50#endif
51
52
53/*
54 *
55 * Small helpers.
56 *
57 */
58
59DEBUG_GET_ONCE_LOG_OPTION(remote_log, "REMOTE_LOG", U_LOGGING_INFO)
60
61#define R_TRACE(R, ...) U_LOG_IFL_T((R)->rc.log_level, __VA_ARGS__)
62#define R_DEBUG(R, ...) U_LOG_IFL_D((R)->rc.log_level, __VA_ARGS__)
63#define R_INFO(R, ...) U_LOG_IFL_I((R)->rc.log_level, __VA_ARGS__)
64#define R_WARN(R, ...) U_LOG_IFL_W((R)->rc.log_level, __VA_ARGS__)
65#define R_ERROR(R, ...) U_LOG_IFL_E((R)->rc.log_level, __VA_ARGS__)
66
67#define RC_TRACE(RC, ...) U_LOG_IFL_T((RC)->log_level, __VA_ARGS__)
68#define RC_DEBUG(RC, ...) U_LOG_IFL_D((RC)->log_level, __VA_ARGS__)
69#define RC_INFO(RC, ...) U_LOG_IFL_I((RC)->log_level, __VA_ARGS__)
70#define RC_WARN(RC, ...) U_LOG_IFL_W((RC)->log_level, __VA_ARGS__)
71#define RC_ERROR(RC, ...) U_LOG_IFL_E((RC)->log_level, __VA_ARGS__)
72
73
74/*
75 *
76 * Platform socket wrapper functions.
77 *
78 */
79
80#if defined(XRT_OS_WINDOWS)
81
82static inline void
83socket_close(r_socket_t id)
84{
85 closesocket(id);
86}
87
88static inline r_socket_t
89socket_create(void)
90{
91 return socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
92}
93
94static inline int
95socket_set_opt(r_socket_t id, int flag)
96{
97 return setsockopt(id, SOL_SOCKET, SO_REUSEADDR, (const char *)&flag, sizeof(flag));
98}
99
100static inline ssize_t
101socket_read(r_socket_t id, void *ptr, size_t size, size_t current)
102{
103 return recv(id, (char *)ptr, (int)(size - current), 0);
104}
105
106static inline ssize_t
107socket_write(r_socket_t id, void *ptr, size_t size, size_t current)
108{
109 return send(id, (const char *)ptr, (int)(size - current), 0);
110}
111
112#elif defined(XRT_OS_UNIX)
113
114static inline void
115socket_close(r_socket_t id)
116{
117 close(id);
118}
119
120static inline r_socket_t
121socket_create(void)
122{
123 return socket(AF_INET, SOCK_STREAM, 0);
124}
125
126static inline int
127socket_set_opt(r_socket_t id, int flag)
128{
129 return setsockopt(id, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(flag));
130}
131
132static inline ssize_t
133socket_read(r_socket_t id, void *ptr, size_t size, size_t current)
134{
135 return read(id, ptr, size - current);
136}
137
138static inline ssize_t
139socket_write(r_socket_t id, void *ptr, size_t size, size_t current)
140{
141 return write(id, ptr, size - current);
142}
143
144#endif // XRT_OS_UNIX
145
146
147/*
148 *
149 * Helper functions.
150 *
151 */
152
153static r_socket_t
154setup_accept_fd(struct r_hub *r)
155{
156 struct sockaddr_in server_address = {0};
157#if defined(XRT_OS_WINDOWS)
158 // Initialize Winsock.
159 WSADATA wsaData;
160 if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
161 int error = WSAGetLastError();
162 R_ERROR(r, "Failed to do WSAStartup %d", error);
163 return error;
164 }
165#endif
166 r_socket_t ret = socket_create();
167
168 if (ret < 0) {
169 R_ERROR(r, "socket: " R_SOCKET_FMT, ret);
170 goto cleanup;
171 }
172
173 r->accept_fd = ret;
174
175 int flag = 1;
176 ret = socket_set_opt(r->accept_fd, flag);
177 if (ret < 0) {
178 R_ERROR(r, "setsockopt: " R_SOCKET_FMT, ret);
179 socket_close(r->accept_fd);
180 r->accept_fd = -1;
181 goto cleanup;
182 }
183
184 server_address.sin_family = AF_INET;
185 server_address.sin_addr.s_addr = htonl(INADDR_ANY);
186 server_address.sin_port = htons(r->port);
187
188 ret = bind(r->accept_fd, (struct sockaddr *)&server_address, sizeof(server_address));
189 if (ret < 0) {
190 R_ERROR(r, "bind: " R_SOCKET_FMT, ret);
191 socket_close(r->accept_fd);
192 r->accept_fd = -1;
193 goto cleanup;
194 }
195
196 R_INFO(r, "Listen address %s on port %d", inet_ntoa(server_address.sin_addr), r->port);
197
198 listen(r->accept_fd, 5);
199
200 return 0;
201cleanup:
202#if defined(XRT_OS_WINDOWS)
203 WSACleanup();
204#endif
205 return ret;
206}
207
208static bool
209wait_for_read_and_to_continue(struct r_hub *r, r_socket_t socket)
210{
211 fd_set set;
212 int ret = 0;
213
214 // To be more roboust
215 if (socket < 0) {
216 return false;
217 }
218
219 while (os_thread_helper_is_running(&r->oth) && ret == 0) {
220 // Select can modify timeout, reset each loop.
221 struct timeval timeout = {.tv_sec = 1, .tv_usec = 0};
222
223 // Reset each loop.
224 FD_ZERO(&set);
225 FD_SET(socket, &set);
226
227 ret = select((int)socket + 1, &set, NULL, NULL, &timeout);
228 }
229
230 if (ret < 0) {
231 R_ERROR(r, "select: %i", ret);
232 return false;
233 } else if (ret > 0) {
234 return true;
235 } else {
236 return false;
237 }
238}
239
240static r_socket_t
241do_accept(struct r_hub *r)
242{
243 struct sockaddr_in addr = {0};
244 r_socket_t ret = 0;
245 if (!wait_for_read_and_to_continue(r, r->accept_fd)) {
246 R_ERROR(r, "Failed to wait for id " R_SOCKET_FMT, r->accept_fd);
247 return -1;
248 }
249
250 socklen_t addr_length = (socklen_t)sizeof(addr);
251 ret = accept(r->accept_fd, (struct sockaddr *)&addr, &addr_length);
252 if (ret < 0) {
253 R_ERROR(r, "accept: " R_SOCKET_FMT, ret);
254 return ret;
255 }
256
257 r_socket_t conn_fd = ret;
258
259 int flags = 1;
260 ret = socket_set_opt(r->accept_fd, flags);
261 if (ret < 0) {
262 R_ERROR(r, "setsockopt: " R_SOCKET_FMT, ret);
263 socket_close(conn_fd);
264 return ret;
265 }
266
267 r->rc.fd = conn_fd;
268
269 R_INFO(r, "Connection received! " R_SOCKET_FMT, r->rc.fd);
270
271 return 0;
272}
273
274static ssize_t
275read_one(struct r_hub *r, struct r_remote_data *data)
276{
277 struct r_remote_connection *rc = &r->rc;
278
279 const size_t size = sizeof(*data);
280 size_t current = 0;
281
282 while (current < size) {
283 void *ptr = (uint8_t *)data + current;
284
285 if (!wait_for_read_and_to_continue(r, rc->fd)) {
286 return -1;
287 }
288
289 ssize_t ret = socket_read(rc->fd, ptr, size, current);
290 if (ret < 0) {
291#if defined(XRT_OS_WINDOWS)
292 RC_ERROR(rc, "recv: %i", WSAGetLastError());
293#else
294 RC_ERROR(rc, "read: %zi", ret);
295#endif
296 return ret;
297 } else if (ret > 0) {
298 current += (size_t)ret;
299 } else {
300 R_INFO(r, "Disconnected!");
301 return -1;
302 }
303 }
304
305 return 0;
306}
307
308static void *
309run_thread(void *ptr)
310{
311 struct r_hub *r = (struct r_hub *)ptr;
312 r_socket_t ret;
313
314 ret = setup_accept_fd(r);
315 if (ret < 0) {
316 R_INFO(r, "Leaving thread");
317 return NULL;
318 }
319
320 while (os_thread_helper_is_running(&r->oth)) {
321 R_INFO(r, "Listening on port '%i'.", r->port);
322
323 ret = do_accept(r);
324 if (ret < 0) {
325 R_INFO(r, "Leaving thread");
326 return NULL;
327 }
328
329 r_remote_connection_write_one(&r->rc, &r->reset);
330 r_remote_connection_write_one(&r->rc, &r->latest);
331
332 while (true) {
333 struct r_remote_data data;
334
335 ret = read_one(r, &data);
336 if (ret < 0) {
337 break;
338 }
339
340 r->latest = data;
341 }
342 }
343
344 R_INFO(r, "Leaving thread");
345
346 return NULL;
347}
348
349static xrt_result_t
350r_hub_system_devices_get_roles(struct xrt_system_devices *xsysd, struct xrt_system_roles *out_roles)
351{
352 struct r_hub *r = (struct r_hub *)xsysd;
353
354 struct xrt_system_roles roles = XRT_SYSTEM_ROLES_INIT;
355 roles.generation_id = 1;
356 roles.left = r->left_index;
357 roles.right = r->right_index;
358
359 *out_roles = roles;
360
361 return XRT_SUCCESS;
362}
363
364static void
365r_hub_system_devices_destroy(struct xrt_system_devices *xsysd)
366{
367 struct r_hub *r = (struct r_hub *)xsysd;
368
369 R_DEBUG(r, "Destroying");
370
371 // Stop the thread first.
372 os_thread_helper_stop_and_wait(&r->oth);
373
374 // Destroy all of the devices now.
375 for (uint32_t i = 0; i < ARRAY_SIZE(r->base.xdevs); i++) {
376 xrt_device_destroy(&r->base.xdevs[i]);
377 }
378
379 // Should be safe to destroy the sockets now.
380 if (r->accept_fd >= 0) {
381 socket_close(r->accept_fd);
382 r->accept_fd = -1;
383 }
384
385 if (r->rc.fd >= 0) {
386 socket_close(r->rc.fd);
387 r->rc.fd = -1;
388 }
389
390 free(r);
391
392#if defined(XRT_OS_WINDOWS)
393 // Clean up Winsock.
394 WSACleanup();
395#endif
396}
397
398
399/*
400 *
401 * 'Exported' create function.
402 *
403 */
404
405xrt_result_t
406r_create_devices(uint16_t port,
407 uint32_t view_count,
408 struct xrt_session_event_sink *broadcast,
409 struct xrt_system_devices **out_xsysd,
410 struct xrt_space_overseer **out_xso)
411{
412 struct r_hub *r = U_TYPED_CALLOC(struct r_hub);
413 int ret;
414
415 r->base.destroy = r_hub_system_devices_destroy;
416 r->base.get_roles = r_hub_system_devices_get_roles;
417 r->origin.type = XRT_TRACKING_TYPE_RGB;
418 r->origin.initial_offset = (struct xrt_pose)XRT_POSE_IDENTITY;
419 r->reset.header = R_HEADER_VALUE;
420 r->reset.head.center = (struct xrt_pose)XRT_POSE_IDENTITY;
421 r->reset.head.center.position.y = 1.6f;
422 r->reset.left.active = true;
423 r->reset.left.hand_tracking_active = true;
424 r->reset.left.pose.position.x = -0.2f;
425 r->reset.left.pose.position.y = 1.3f;
426 r->reset.left.pose.position.z = -0.5f;
427 r->reset.left.pose.orientation.w = 1.0f;
428
429 r->reset.right.active = true;
430 r->reset.right.hand_tracking_active = true;
431 r->reset.right.pose.position.x = 0.2f;
432 r->reset.right.pose.position.y = 1.3f;
433 r->reset.right.pose.position.z = -0.5f;
434 r->reset.right.pose.orientation.w = 1.0f;
435 r->latest = r->reset;
436 r->rc.log_level = debug_get_log_option_remote_log();
437 r->gui.hmd = true;
438 r->gui.left = true;
439 r->gui.right = true;
440 r->port = port;
441 r->view_count = view_count;
442 r->accept_fd = -1;
443 r->rc.fd = -1;
444
445 snprintf(r->origin.name, sizeof(r->origin.name), "Remote Simulator");
446
447 ret = os_thread_helper_init(&r->oth);
448 if (ret != 0) {
449 R_ERROR(r, "Failed to init threading!");
450 r_hub_system_devices_destroy(&r->base);
451 return XRT_ERROR_ALLOCATION;
452 }
453
454 ret = os_thread_helper_start(&r->oth, run_thread, r);
455 if (ret != 0) {
456 R_ERROR(r, "Failed to start thread!");
457 r_hub_system_devices_destroy(&r->base);
458 return XRT_ERROR_ALLOCATION;
459 }
460
461
462 /*
463 * Setup system devices.
464 */
465
466 struct xrt_device *head = r_hmd_create(r);
467 struct xrt_device *left = r_device_create(r, true);
468 struct xrt_device *right = r_device_create(r, false);
469
470 r->base.xdevs[r->base.xdev_count++] = head;
471 r->left_index = (int32_t)r->base.xdev_count;
472 r->base.xdevs[r->base.xdev_count++] = left;
473 r->right_index = (int32_t)r->base.xdev_count;
474 r->base.xdevs[r->base.xdev_count++] = right;
475
476 r->base.static_roles.head = head;
477 r->base.static_roles.hand_tracking.conforming.left = left;
478 r->base.static_roles.hand_tracking.conforming.right = right;
479
480
481 /*
482 * Space overseer.
483 */
484
485 struct u_space_overseer *uso = u_space_overseer_create(broadcast);
486 struct xrt_space_overseer *xso = (struct xrt_space_overseer *)uso;
487 assert(uso != NULL);
488
489 struct xrt_space *root = xso->semantic.root; // Convenience
490 struct xrt_space *offset = NULL;
491 u_space_overseer_create_offset_space(uso, root, &r->origin.initial_offset, &offset);
492
493 for (uint32_t i = 0; i < r->base.xdev_count; i++) {
494 u_space_overseer_link_space_to_device(uso, offset, r->base.xdevs[i]);
495 }
496
497 // Unreference now
498 xrt_space_reference(&offset, NULL);
499
500 // Set root as stage space.
501 xrt_space_reference(&xso->semantic.stage, root);
502
503 // Local 1.6 meters up.
504 struct xrt_pose local_offset = {XRT_QUAT_IDENTITY, {0.0f, 1.6f, 0.0f}};
505 u_space_overseer_create_offset_space(uso, root, &local_offset, &xso->semantic.local);
506
507 // Local floor at the same place as local except at floor height.
508 struct xrt_pose local_floor_offset = local_offset;
509 local_floor_offset.position.y = 0.0f;
510 u_space_overseer_create_offset_space(uso, root, &local_floor_offset, &xso->semantic.local_floor);
511
512 // Make view space be the head pose.
513 u_space_overseer_create_pose_space(uso, head, XRT_INPUT_GENERIC_HEAD_POSE, &xso->semantic.view);
514
515
516 /*
517 * Setup variable tracker.
518 */
519
520 u_var_add_root(r, "Remote Hub", true);
521 // u_var_add_gui_header(r, &r->gui.hmd, "MHD");
522 u_var_add_pose(r, &r->latest.head.center, "head.center");
523 // u_var_add_gui_header(r, &r->gui.left, "Left");
524 u_var_add_bool(r, &r->latest.left.active, "left.active");
525 u_var_add_pose(r, &r->latest.left.pose, "left.pose");
526 // u_var_add_gui_header(r, &r->gui.right, "Right");
527 u_var_add_bool(r, &r->latest.right.active, "right.active");
528 u_var_add_pose(r, &r->latest.right.pose, "right.pose");
529
530 /*
531 * Done now.
532 */
533
534 *out_xsysd = &r->base;
535 *out_xso = xso;
536
537 return XRT_SUCCESS;
538}
539
540
541/*
542 *
543 * 'Exported' connection functions.
544 *
545 */
546
547r_socket_t
548r_remote_connection_init(struct r_remote_connection *rc, const char *ip_addr, uint16_t port)
549{
550 struct sockaddr_in addr = {0};
551 r_socket_t sock_fd;
552 r_socket_t conn_fd;
553 int ret;
554
555 // Set log level.
556 rc->log_level = debug_get_log_option_remote_log();
557
558#if defined(XRT_OS_WINDOWS)
559 // Initialize Winsock.
560 WSADATA wsaData;
561 ret = WSAStartup(MAKEWORD(2, 2), &wsaData);
562 if (ret != 0) {
563 RC_ERROR(rc, "Failed to do WSAStartup %i", WSAGetLastError());
564 return ret;
565 }
566#endif
567
568 // Address
569 addr.sin_family = AF_INET;
570 addr.sin_port = htons(port);
571
572 // inet_pton/InetPton resolves "localhost" as 0.0.0.0 or 255.255.255.255, and it causes connection error. To
573 // avoid this issue, the following logic converts "localhost" to "127.0.0.1" first.
574 if (strcmp("localhost", ip_addr) == 0) {
575 ret = inet_pton(AF_INET, "127.0.0.1", &addr.sin_addr);
576 } else {
577 ret = inet_pton(AF_INET, ip_addr, &addr.sin_addr);
578 }
579 if (ret < 0) {
580 RC_ERROR(rc, "Failed to do inet pton for %s: %i", ip_addr, ret);
581 goto cleanup;
582 }
583
584 sock_fd = socket_create();
585#if defined(XRT_OS_WINDOWS)
586 if (sock_fd == INVALID_SOCKET) {
587 RC_ERROR(rc, "Failed to create socket %i", WSAGetLastError());
588 goto cleanup;
589 }
590#else
591 if (sock_fd < 0) {
592 RC_ERROR(rc, "Failed to create socket: %i", ret);
593 goto cleanup;
594 }
595#endif
596
597 conn_fd = sock_fd;
598
599 ret = connect(conn_fd, (struct sockaddr *)&addr, sizeof(addr));
600 // If connect operation succeed, both Windows and POSIX returns 0.
601 if (ret != 0) {
602#if defined(XRT_OS_WINDOWS)
603 RC_ERROR(rc, "Failed to connect id " R_SOCKET_FMT " and addr %s with failure %d", conn_fd,
604 inet_ntoa(addr.sin_addr), WSAGetLastError());
605#else
606 RC_ERROR(rc, "Failed to connect id " R_SOCKET_FMT " and addr %s with failure %d", conn_fd,
607 inet_ntoa(addr.sin_addr), ret);
608#endif
609 socket_close(conn_fd);
610 goto cleanup;
611 }
612
613 int flags = 1;
614 ret = socket_set_opt(conn_fd, flags);
615 if (ret < 0) {
616 RC_ERROR(rc, "Failed to setsockopt: %i", ret);
617 socket_close(conn_fd);
618 goto cleanup;
619 }
620
621 rc->fd = conn_fd;
622
623 return 0;
624
625cleanup:
626#if defined(XRT_OS_WINDOWS)
627 WSACleanup();
628#endif
629 return ret;
630}
631
632int
633r_remote_connection_read_one(struct r_remote_connection *rc, struct r_remote_data *data)
634{
635 const size_t size = sizeof(*data);
636 size_t current = 0;
637
638 while (current < size) {
639 void *ptr = (uint8_t *)data + current;
640 ssize_t ret = socket_read(rc->fd, ptr, size, current);
641 if (ret < 0) {
642 RC_ERROR(rc, "read: %zi", ret);
643 return ret;
644 }
645 if (ret > 0) {
646 current += (size_t)ret;
647 } else {
648 RC_INFO(rc, "Disconnected!");
649 return -1;
650 }
651 }
652
653 if (data->header != R_HEADER_VALUE) {
654 RC_ERROR(rc, "Protocol version mismatch (expected 0x%08" PRIx64 ", got 0x%08" PRIx64, R_HEADER_VALUE,
655 data->header);
656 return -1;
657 }
658
659 return 0;
660}
661
662int
663r_remote_connection_write_one(struct r_remote_connection *rc, const struct r_remote_data *data)
664{
665 const size_t size = sizeof(*data);
666 size_t current = 0;
667
668 if (data->header != R_HEADER_VALUE) {
669 RC_ERROR(rc, "Protocol version mismatch (expected 0x%08" PRIx64 ", got 0x%08" PRIx64, R_HEADER_VALUE,
670 data->header);
671 return -1;
672 }
673
674 while (current < size) {
675 void *ptr = (uint8_t *)data + current;
676
677 ssize_t ret = socket_write(rc->fd, ptr, size, current);
678 if (ret < 0) {
679 RC_ERROR(rc, "write: %zi", ret);
680 return ret;
681 }
682 if (ret > 0) {
683 current += (size_t)ret;
684 } else {
685 RC_INFO(rc, "Disconnected!");
686 return -1;
687 }
688 }
689
690 return 0;
691}