The open source OpenXR runtime

d/wmr: Infrastructure for tunnelled controller connections

Add a connection object for controllers that are tunnnelled
through the HMD, and calls to allow send/receive of packets
as needed.

authored by

Jan Schmidt and committed by
Jakob Bornecrantz
89407f8d 351dd9b9

+322 -30
+2
src/xrt/drivers/CMakeLists.txt
··· 374 374 wmr/wmr_bt_controller.h 375 375 wmr/wmr_hmd.c 376 376 wmr/wmr_hmd.h 377 + wmr/wmr_hmd_controller.c 378 + wmr/wmr_hmd_controller.h 377 379 wmr/wmr_interface.h 378 380 wmr/wmr_prober.c 379 381 wmr/wmr_protocol.c
-2
src/xrt/drivers/wmr/wmr_bt_controller.h
··· 36 36 struct os_thread_helper controller_thread; 37 37 38 38 struct os_mutex hid_lock; 39 - 40 - struct wmr_controller_base *wcb; 41 39 }; 42 40 43 41 struct xrt_device *
+1 -3
src/xrt/drivers/wmr/wmr_controller_base.c
··· 80 80 } 81 81 82 82 break; 83 - default: 84 - WMR_DEBUG(wcb, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], buf_size); 85 - break; 83 + default: WMR_DEBUG(wcb, "WMR Controller: Unknown message type: %02x, size: %i", buffer[0], buf_size); break; 86 84 } 87 85 88 86 return;
+12 -2
src/xrt/drivers/wmr/wmr_controller_base.h
··· 51 51 struct wmr_controller_base *wcb; 52 52 53 53 bool (*send_bytes)(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size); 54 + void (*receive_bytes)(struct wmr_controller_connection *wcc, 55 + uint64_t time_ns, 56 + uint8_t *buffer, 57 + uint32_t buf_size); 54 58 int (*read_sync)(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms); 55 59 56 60 void (*disconnect)(struct wmr_controller_connection *wcc); ··· 136 140 uint8_t *buffer, 137 141 uint32_t buf_size) 138 142 { 139 - struct wmr_controller_base *wcb = wcc->wcb; 140 143 141 - wcb->receive_bytes(wcb, time_ns, buffer, buf_size); 144 + if (wcc->receive_bytes != NULL) { 145 + wcc->receive_bytes(wcc, time_ns, buffer, buf_size); 146 + } else { 147 + /* Default: deliver directly to the controller instance */ 148 + struct wmr_controller_base *wcb = wcc->wcb; 149 + assert(wcb->receive_bytes != NULL); 150 + wcb->receive_bytes(wcb, time_ns, buffer, buf_size); 151 + } 142 152 } 143 153 144 154 #ifdef __cplusplus
+66 -13
src/xrt/drivers/wmr/wmr_hmd.c
··· 78 78 DEBUG_GET_ONCE_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", NULL) 79 79 #endif 80 80 81 + #define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) 82 + #define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) 83 + #define WMR_DEBUG_HEX(d, data, data_size) U_LOG_XDEV_IFL_D_HEX(&d->base, d->log_level, data, data_size) 84 + #define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) 85 + #define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) 86 + #define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) 87 + 81 88 static int 82 89 wmr_hmd_activate_reverb(struct wmr_hmd *wh); 83 90 static void ··· 254 261 pkt_type = buffer[1]; 255 262 if (pkt_type != WMR_BT_IFACE_MSG_DEBUG) { 256 263 WMR_DEBUG(wh, "Unknown Bluetooth interface packet (%d) type 0x%02x", size, pkt_type); 264 + WMR_DEBUG_HEX(wh, buffer, size); 257 265 return; 258 266 } 259 267 buffer += 2; ··· 431 439 unsigned char buffer[WMR_FEATURE_BUFFER_SIZE]; 432 440 433 441 // Block for 100ms 442 + os_mutex_lock(&wh->hid_lock); 434 443 int size = os_hid_read(wh->hid_hololens_sensors_dev, buffer, sizeof(buffer), 100); 444 + os_mutex_unlock(&wh->hid_lock); 435 445 436 446 if (size < 0) { 437 447 WMR_ERROR(wh, "Error reading from Hololens Sensors device. Call to os_hid_read returned %i", size); ··· 514 524 unsigned char buffer[WMR_FEATURE_BUFFER_SIZE]; 515 525 516 526 // Do not block 527 + os_mutex_lock(&wh->hid_lock); 517 528 int size = os_hid_read(wh->hid_control_dev, buffer, sizeof(buffer), 0); 529 + os_mutex_unlock(&wh->hid_lock); 518 530 519 531 if (size < 0) { 520 532 WMR_ERROR(wh, "Error reading from companion (HMD control) device. Call to os_hid_read returned %i", ··· 621 633 { 622 634 DRV_TRACE_MARKER(); 623 635 636 + os_mutex_lock(&wh->hid_lock); 624 637 int size = os_hid_write(wh->hid_hololens_sensors_dev, hololens_sensors_imu_on, sizeof(hololens_sensors_imu_on)); 638 + os_mutex_unlock(&wh->hid_lock); 639 + 625 640 if (size <= 0) { 626 641 WMR_ERROR(wh, "Error writing to device"); 627 642 return; 628 643 } 629 644 } 630 645 631 - #define HID_SEND(HID, DATA, STR) \ 646 + #define HID_SEND(hmd, HID, DATA, STR) \ 632 647 do { \ 648 + os_mutex_lock(&hmd->hid_lock); \ 633 649 int _ret = os_hid_set_feature(HID, DATA, sizeof(DATA)); \ 650 + os_mutex_unlock(&hmd->hid_lock); \ 634 651 if (_ret < 0) { \ 635 652 WMR_ERROR(wh, "Send (%s): %i", STR, _ret); \ 636 653 } \ 637 654 } while (false); 638 655 639 - #define HID_GET(HID, DATA, STR) \ 656 + #define HID_GET(hmd, HID, DATA, STR) \ 640 657 do { \ 658 + os_mutex_lock(&hmd->hid_lock); \ 641 659 int _ret = os_hid_get_feature(HID, DATA[0], DATA, sizeof(DATA)); \ 660 + os_mutex_unlock(&hmd->hid_lock); \ 642 661 if (_ret < 0) { \ 643 662 WMR_ERROR(wh, "Get (%s): %i", STR, _ret); \ 663 + } else { \ 664 + WMR_DEBUG(wh, "0x%02x HID feature returned", DATA[0]); \ 665 + WMR_DEBUG_HEX(wh, DATA, _ret); \ 644 666 } \ 645 667 } while (false); 646 668 ··· 660 682 661 683 for (int i = 0; i < 4; i++) { 662 684 unsigned char cmd[64] = {0x50, 0x01}; 663 - HID_SEND(hid, cmd, "loop"); 685 + HID_SEND(wh, hid, cmd, "loop"); 664 686 665 687 unsigned char data[64] = {0x50}; 666 - HID_GET(hid, data, "loop"); 688 + HID_GET(wh, hid, data, "loop"); 667 689 668 690 os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms 669 691 } 670 692 671 693 unsigned char data[64] = {0x09}; 672 - HID_GET(hid, data, "data_1"); 694 + HID_GET(wh, hid, data, "data_1"); 673 695 674 696 data[0] = 0x08; 675 - HID_GET(hid, data, "data_2"); 697 + HID_GET(wh, hid, data, "data_2"); 676 698 677 699 data[0] = 0x06; 678 - HID_GET(hid, data, "data_3"); 700 + HID_GET(wh, hid, data, "data_3"); 679 701 680 702 WMR_INFO(wh, "Sent activation report."); 681 703 ··· 730 752 cmd[1] = enable ? 0x01 : 0x00; 731 753 } 732 754 733 - HID_SEND(hid, cmd, (enable ? "screen_on" : "screen_off")); 755 + HID_SEND(wh, hid, cmd, (enable ? "screen_on" : "screen_off")); 734 756 735 757 wh->hmd_screen_enable = enable; 736 758 ··· 750 772 os_nanosleep(U_TIME_1MS_IN_NS * 300); 751 773 752 774 unsigned char data[64] = {0x16}; 753 - HID_GET(hid, data, "data_1"); 775 + HID_GET(wh, hid, data, "data_1"); 754 776 755 777 data[0] = 0x15; 756 - HID_GET(hid, data, "data_2"); 778 + HID_GET(wh, hid, data, "data_2"); 757 779 758 780 data[0] = 0x14; 759 - HID_GET(hid, data, "data_3"); 781 + HID_GET(wh, hid, data, "data_3"); 760 782 761 783 // Enable the HMD screen now, if required. Otherwise, if screen should initially be disabled, then 762 784 // proactively disable it now. Why? Because some cases of irregular termination of Monado will ··· 796 818 cmd[1] = enable ? 0x01 : 0x00; 797 819 } 798 820 799 - HID_SEND(hid, cmd, (enable ? "screen_on" : "screen_off")); 821 + HID_SEND(wh, hid, cmd, (enable ? "screen_on" : "screen_off")); 800 822 801 823 wh->hmd_screen_enable = enable; 802 824 ··· 1152 1174 m_imu_3dof_close(&wh->fusion.i3dof); 1153 1175 1154 1176 os_mutex_destroy(&wh->fusion.mutex); 1177 + os_mutex_destroy(&wh->hid_lock); 1155 1178 1156 1179 u_device_free(&wh->base); 1157 1180 } ··· 1868 1891 // Mutex before thread. 1869 1892 ret = os_mutex_init(&wh->fusion.mutex); 1870 1893 if (ret != 0) { 1871 - WMR_ERROR(wh, "Failed to init mutex!"); 1894 + WMR_ERROR(wh, "Failed to init fusion mutex!"); 1895 + wmr_hmd_destroy(&wh->base); 1896 + wh = NULL; 1897 + return; 1898 + } 1899 + 1900 + ret = os_mutex_init(&wh->hid_lock); 1901 + if (ret != 0) { 1902 + WMR_ERROR(wh, "Failed to init HID mutex!"); 1872 1903 wmr_hmd_destroy(&wh->base); 1873 1904 wh = NULL; 1874 1905 return; ··· 2018 2049 *out_hmd = &wh->base; 2019 2050 *out_handtracker = hand_device; 2020 2051 } 2052 + 2053 + bool 2054 + wmr_hmd_send_controller_packet(struct wmr_hmd *hmd, const uint8_t *buffer, uint32_t buf_size) 2055 + { 2056 + os_mutex_lock(&hmd->hid_lock); 2057 + int ret = os_hid_write(hmd->hid_hololens_sensors_dev, buffer, buf_size); 2058 + os_mutex_unlock(&hmd->hid_lock); 2059 + 2060 + return ret != -1 && (uint32_t)(ret) == buf_size; 2061 + } 2062 + 2063 + /* Called from WMR controller implementation only during fw reads. @todo: Refactor 2064 + * controller firmware reads to happen from a state machine and not require this blocking method */ 2065 + int 2066 + wmr_hmd_read_sync_from_controller(struct wmr_hmd *hmd, uint8_t *buffer, uint32_t buf_size, int timeout_ms) 2067 + { 2068 + os_mutex_lock(&hmd->hid_lock); 2069 + int res = os_hid_read(hmd->hid_hololens_sensors_dev, buffer, buf_size, timeout_ms); 2070 + os_mutex_unlock(&hmd->hid_lock); 2071 + 2072 + return res; 2073 + }
+7 -10
src/xrt/drivers/wmr/wmr_hmd.h
··· 87 87 * IMU data and read the config from. 88 88 * 89 89 * During start it is owned by the thread creating the device, after 90 - * init it is owned by the reading thread, there is no mutex protecting 91 - * this field as it's only used by the reading thread in @p oth. 90 + * init it is owned by the reading thread. Read/write access is 91 + * protected by the hid_lock 92 92 */ 93 93 94 94 struct os_hid_device *hid_hololens_sensors_dev; 95 + struct os_mutex hid_lock; 95 96 96 97 /*! 97 98 * This is the vendor specific companion device of the Hololens Sensors. ··· 205 206 struct xrt_device **out_hmd, 206 207 struct xrt_device **out_handtracker); 207 208 208 - #define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) 209 - #define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) 210 - #define WMR_DEBUG_HEX(d, data, data_size) U_LOG_XDEV_IFL_D_HEX(&d->base, d->log_level, data, data_size) 211 - #define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) 212 - #define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) 213 - #define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) 214 - 215 - 209 + bool 210 + wmr_hmd_send_controller_packet(struct wmr_hmd *hmd, const uint8_t *buffer, uint32_t buf_size); 211 + int 212 + wmr_hmd_read_sync_from_controller(struct wmr_hmd *hmd, uint8_t *buffer, uint32_t buf_size, int timeout_ms); 216 213 #ifdef __cplusplus 217 214 } 218 215 #endif
+180
src/xrt/drivers/wmr/wmr_hmd_controller.c
··· 1 + // Copyright 2023 Jan Schmidt 2 + // SPDX-License-Identifier: BSL-1.0 3 + /*! 4 + * @file 5 + * @brief Implementation of tunnelled controller connection, 6 + * that translates messages passing via an HP G2 or Sasmung Odyssey+ HMD 7 + * @author Jan Schmidt <jan@centricular.com> 8 + * @ingroup drv_wmr 9 + */ 10 + #include "util/u_trace_marker.h" 11 + 12 + #include "wmr_hmd_controller.h" 13 + #include "wmr_controller.h" 14 + #include "wmr_hmd.h" 15 + 16 + #define WMR_TRACE(c, ...) U_LOG_IFL_T(c->log_level, __VA_ARGS__) 17 + #define WMR_DEBUG(c, ...) U_LOG_IFL_D(c->log_level, __VA_ARGS__) 18 + #define WMR_INFO(c, ...) U_LOG_IFL_I(c->log_level, __VA_ARGS__) 19 + #define WMR_WARN(c, ...) U_LOG_IFL_W(c->log_level, __VA_ARGS__) 20 + #define WMR_ERROR(c, ...) U_LOG_IFL_E(c->log_level, __VA_ARGS__) 21 + 22 + /* A note: 23 + * This HMD controller connection object is used for controllers 24 + * where the communication is tunnelled through HMD packets. It 25 + * handles translating the controller packet IDs to raw 26 + * IDs when receiving data from the HMD, and back to HMD packet IDs 27 + * when sending data to the controller. 28 + * 29 + * Both the HMD and the controller hold a reference to this 30 + * connection object. The HMD can pass received packets at 31 + * any time, and will call wmr_controller_connection_disconnect() 32 + * when the HMD xrt_device is freed by the runtime. 33 + * 34 + * The controller may send packets based on calls from the 35 + * runtime (triggering haptics, for example), so can also 36 + * want to send packets at any time. It will also call 37 + * wmr_controller_connection_disconnect() 38 + * when the controller xrt_device is freed by the runtime. 39 + * 40 + * The conn_lock protects access to the HMD and controller 41 + * pointers while making calls to send/receive, to prevent 42 + * from invalid access if _disconnect() is called. 43 + */ 44 + 45 + static bool 46 + send_bytes_to_controller(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size) 47 + { 48 + struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc); 49 + bool res = false; 50 + 51 + assert(buf_size < 64); 52 + 53 + os_mutex_lock(&conn->lock); 54 + if (!conn->disconnected && buf_size > 0) { 55 + uint8_t outbuf[64]; 56 + 57 + memcpy(outbuf, buffer, buf_size); 58 + outbuf[0] += conn->hmd_cmd_base; 59 + res = wmr_hmd_send_controller_packet(conn->hmd, outbuf, buf_size); 60 + } 61 + os_mutex_unlock(&conn->lock); 62 + 63 + return res; 64 + } 65 + 66 + static int 67 + read_sync_from_controller(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms) 68 + { 69 + struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc); 70 + int res = -1; 71 + 72 + os_mutex_lock(&conn->lock); 73 + if (!conn->disconnected && buf_size > 0) { 74 + res = wmr_hmd_read_sync_from_controller(conn->hmd, buffer, buf_size, timeout_ms); 75 + } 76 + os_mutex_unlock(&conn->lock); 77 + 78 + return res; 79 + } 80 + 81 + static void 82 + receive_bytes_from_controller(struct wmr_controller_connection *wcc, 83 + uint64_t time_ns, 84 + uint8_t *buffer, 85 + uint32_t buf_size) 86 + { 87 + struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc); 88 + os_mutex_lock(&conn->lock); 89 + if (!conn->disconnected && buf_size > 0) { 90 + buffer[0] -= conn->hmd_cmd_base; 91 + 92 + struct wmr_controller_base *wcb = wcc->wcb; 93 + assert(wcb->receive_bytes != NULL); 94 + wcb->receive_bytes(wcb, time_ns, buffer, buf_size); 95 + } 96 + os_mutex_unlock(&conn->lock); 97 + } 98 + 99 + static void 100 + wmr_hmd_controller_connection_destroy(struct wmr_hmd_controller_connection *conn) 101 + { 102 + DRV_TRACE_MARKER(); 103 + 104 + os_mutex_destroy(&conn->lock); 105 + free(conn); 106 + } 107 + 108 + static void 109 + wmr_hmd_controller_connection_disconnect(struct wmr_controller_connection *base) 110 + { 111 + struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(base); 112 + 113 + if (xrt_reference_dec(&conn->ref)) { 114 + wmr_hmd_controller_connection_destroy(conn); 115 + } else { 116 + os_mutex_lock(&conn->lock); 117 + conn->disconnected = true; 118 + os_mutex_unlock(&conn->lock); 119 + } 120 + } 121 + 122 + struct wmr_hmd_controller_connection * 123 + wmr_hmd_controller_create(struct wmr_hmd *hmd, 124 + uint8_t hmd_cmd_base, 125 + enum xrt_device_type controller_type, 126 + uint16_t vid, 127 + uint16_t pid, 128 + enum u_logging_level log_level) 129 + { 130 + DRV_TRACE_MARKER(); 131 + 132 + struct wmr_hmd_controller_connection *conn = calloc(1, sizeof(struct wmr_hmd_controller_connection)); 133 + 134 + conn->log_level = log_level; 135 + 136 + conn->hmd = hmd; 137 + conn->hmd_cmd_base = hmd_cmd_base; 138 + 139 + conn->base.receive_bytes = receive_bytes_from_controller; 140 + conn->base.send_bytes = send_bytes_to_controller; 141 + conn->base.read_sync = read_sync_from_controller; 142 + conn->base.disconnect = wmr_hmd_controller_connection_disconnect; 143 + 144 + /* Init 2 references - one for the controller, one for the HMD */ 145 + xrt_reference_inc(&conn->ref); 146 + xrt_reference_inc(&conn->ref); 147 + 148 + int ret = 0; 149 + 150 + ret = os_mutex_init(&conn->lock); 151 + if (ret != 0) { 152 + WMR_ERROR(conn, "WMR Controller (Tunnelled): Failed to init mutex!"); 153 + wmr_hmd_controller_connection_destroy(conn); 154 + return NULL; 155 + } 156 + 157 + // Takes ownership of one reference to the connection, the other will 158 + // belong to the returned pointer 159 + struct wmr_controller_base *wcb = wmr_controller_create(&conn->base, controller_type, vid, pid, log_level); 160 + if (wcb == NULL) { 161 + WMR_ERROR(conn, "WMR Controller (Tunnelled): Failed to create controller"); 162 + wmr_hmd_controller_connection_destroy(conn); 163 + return NULL; 164 + } 165 + 166 + // If the controller device was created, the connection belongs to 167 + // it now and will be cleaned up when it calls disconnect(). 168 + conn->base.wcb = wcb; 169 + 170 + return conn; 171 + } 172 + 173 + struct xrt_device * 174 + wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc) 175 + { 176 + struct wmr_controller_base *wcb = wcc->base.wcb; 177 + struct xrt_device *xdev = &wcb->base; 178 + 179 + return xdev; 180 + }
+54
src/xrt/drivers/wmr/wmr_hmd_controller.h
··· 1 + // Copyright 2023 Jan Schmidt 2 + // SPDX-License-Identifier: BSL-1.0 3 + /*! 4 + * @file 5 + * @brief Implementation of tunnelled controller connection, 6 + * that translates messages passing via an HP G2 or Sasmung Odyssey+ HMD 7 + * @author Jan Schmidt <jan@centricular.com> 8 + * @ingroup drv_wmr 9 + */ 10 + #include <stdint.h> 11 + 12 + #include "os/os_threading.h" 13 + #include "xrt/xrt_device.h" 14 + 15 + #include "wmr_controller_base.h" 16 + 17 + #pragma once 18 + 19 + #ifdef __cplusplus 20 + extern "C" { 21 + #endif 22 + 23 + struct wmr_hmd_controller_connection 24 + { 25 + struct wmr_controller_connection base; 26 + 27 + /* Controller and HMD each hold a reference. It's 28 + * only cleaned up once both release it. */ 29 + struct xrt_reference ref; 30 + enum u_logging_level log_level; 31 + 32 + uint8_t hmd_cmd_base; 33 + 34 + /* Protect access when sending / receiving data */ 35 + struct os_mutex lock; 36 + bool disconnected; /* Set to true once disconnect() is called */ 37 + 38 + struct wmr_hmd *hmd; 39 + }; 40 + 41 + struct wmr_hmd_controller_connection * 42 + wmr_hmd_controller_create(struct wmr_hmd *hmd, 43 + uint8_t hmd_cmd_base, 44 + enum xrt_device_type controller_type, 45 + uint16_t vid, 46 + uint16_t pid, 47 + enum u_logging_level log_level); 48 + 49 + struct xrt_device * 50 + wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc); 51 + 52 + #ifdef __cplusplus 53 + } 54 + #endif