matrixfurry.com
The open source OpenXR runtime
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
45static bool
46send_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 os_mutex_lock(&conn->lock);
52 if (!conn->disconnected && buf_size > 0) {
53 uint8_t outbuf[64];
54
55 assert(buf_size <= sizeof(outbuf));
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
66static int
67read_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 if (res > 0) {
76 buffer[0] -= conn->hmd_cmd_base;
77 }
78 }
79 os_mutex_unlock(&conn->lock);
80
81 return res;
82}
83
84static void
85receive_bytes_from_controller(struct wmr_controller_connection *wcc,
86 uint64_t time_ns,
87 uint8_t *buffer,
88 uint32_t buf_size)
89{
90 struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc);
91 os_mutex_lock(&conn->lock);
92 if (!conn->disconnected && buf_size > 0) {
93 buffer[0] -= conn->hmd_cmd_base;
94
95 struct wmr_controller_base *wcb = wcc->wcb;
96 assert(wcb->receive_bytes != NULL);
97 wcb->receive_bytes(wcb, time_ns, buffer, buf_size);
98 }
99 os_mutex_unlock(&conn->lock);
100}
101
102static void
103wmr_hmd_controller_connection_destroy(struct wmr_hmd_controller_connection *conn)
104{
105 DRV_TRACE_MARKER();
106
107 os_mutex_destroy(&conn->lock);
108 free(conn);
109}
110
111static void
112wmr_hmd_controller_connection_disconnect(struct wmr_controller_connection *base)
113{
114 struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(base);
115
116 if (xrt_reference_dec_and_is_zero(&conn->ref)) {
117 wmr_hmd_controller_connection_destroy(conn);
118 } else {
119 os_mutex_lock(&conn->lock);
120 conn->disconnected = true;
121 conn->base.wcb = NULL;
122 os_mutex_unlock(&conn->lock);
123 }
124}
125
126struct wmr_hmd_controller_connection *
127wmr_hmd_controller_create(struct wmr_hmd *hmd,
128 uint8_t hmd_cmd_base,
129 enum xrt_device_type controller_type,
130 uint16_t vid,
131 uint16_t pid,
132 enum u_logging_level log_level)
133{
134 DRV_TRACE_MARKER();
135
136 struct wmr_hmd_controller_connection *conn = calloc(1, sizeof(struct wmr_hmd_controller_connection));
137
138 conn->log_level = log_level;
139
140 conn->hmd = hmd;
141 conn->hmd_cmd_base = hmd_cmd_base;
142
143 conn->base.receive_bytes = receive_bytes_from_controller;
144 conn->base.send_bytes = send_bytes_to_controller;
145 conn->base.read_sync = read_sync_from_controller;
146 conn->base.disconnect = wmr_hmd_controller_connection_disconnect;
147
148 /* Init 2 references - one for the controller, one for the HMD */
149 xrt_reference_inc(&conn->ref);
150 xrt_reference_inc(&conn->ref);
151
152 int ret = 0;
153
154 ret = os_mutex_init(&conn->lock);
155 if (ret != 0) {
156 WMR_ERROR(conn, "WMR Controller (Tunnelled): Failed to init mutex!");
157 wmr_hmd_controller_connection_destroy(conn);
158 return NULL;
159 }
160
161 // Takes ownership of one reference to the connection, the other will
162 // belong to the returned pointer
163 struct wmr_controller_base *wcb = wmr_controller_create(&conn->base, controller_type, vid, pid, log_level);
164 if (wcb == NULL) {
165 WMR_ERROR(conn, "WMR Controller (Tunnelled): Failed to create controller");
166 wmr_hmd_controller_connection_destroy(conn);
167 return NULL;
168 }
169
170 // If the controller device was created, the connection belongs to
171 // it now and will be cleaned up when it calls disconnect().
172 conn->base.wcb = wcb;
173
174 return conn;
175}
176
177struct xrt_device *
178wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc)
179{
180 struct wmr_controller_base *wcb = wcc->base.wcb;
181 if (wcb == NULL)
182 return NULL;
183
184 struct xrt_device *xdev = &wcb->base;
185 return xdev;
186}