···8787 * IMU data and read the config from.
8888 *
8989 * During start it is owned by the thread creating the device, after
9090- * init it is owned by the reading thread, there is no mutex protecting
9191- * this field as it's only used by the reading thread in @p oth.
9090+ * init it is owned by the reading thread. Read/write access is
9191+ * protected by the hid_lock
9292 */
93939494 struct os_hid_device *hid_hololens_sensors_dev;
9595+ struct os_mutex hid_lock;
95969697 /*!
9798 * This is the vendor specific companion device of the Hololens Sensors.
···205206 struct xrt_device **out_hmd,
206207 struct xrt_device **out_handtracker);
207208208208-#define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
209209-#define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)
210210-#define WMR_DEBUG_HEX(d, data, data_size) U_LOG_XDEV_IFL_D_HEX(&d->base, d->log_level, data, data_size)
211211-#define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)
212212-#define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)
213213-#define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)
214214-215215-209209+bool
210210+wmr_hmd_send_controller_packet(struct wmr_hmd *hmd, const uint8_t *buffer, uint32_t buf_size);
211211+int
212212+wmr_hmd_read_sync_from_controller(struct wmr_hmd *hmd, uint8_t *buffer, uint32_t buf_size, int timeout_ms);
216213#ifdef __cplusplus
217214}
218215#endif
+180
src/xrt/drivers/wmr/wmr_hmd_controller.c
···11+// Copyright 2023 Jan Schmidt
22+// SPDX-License-Identifier: BSL-1.0
33+/*!
44+ * @file
55+ * @brief Implementation of tunnelled controller connection,
66+ * that translates messages passing via an HP G2 or Sasmung Odyssey+ HMD
77+ * @author Jan Schmidt <jan@centricular.com>
88+ * @ingroup drv_wmr
99+ */
1010+#include "util/u_trace_marker.h"
1111+1212+#include "wmr_hmd_controller.h"
1313+#include "wmr_controller.h"
1414+#include "wmr_hmd.h"
1515+1616+#define WMR_TRACE(c, ...) U_LOG_IFL_T(c->log_level, __VA_ARGS__)
1717+#define WMR_DEBUG(c, ...) U_LOG_IFL_D(c->log_level, __VA_ARGS__)
1818+#define WMR_INFO(c, ...) U_LOG_IFL_I(c->log_level, __VA_ARGS__)
1919+#define WMR_WARN(c, ...) U_LOG_IFL_W(c->log_level, __VA_ARGS__)
2020+#define WMR_ERROR(c, ...) U_LOG_IFL_E(c->log_level, __VA_ARGS__)
2121+2222+/* A note:
2323+ * This HMD controller connection object is used for controllers
2424+ * where the communication is tunnelled through HMD packets. It
2525+ * handles translating the controller packet IDs to raw
2626+ * IDs when receiving data from the HMD, and back to HMD packet IDs
2727+ * when sending data to the controller.
2828+ *
2929+ * Both the HMD and the controller hold a reference to this
3030+ * connection object. The HMD can pass received packets at
3131+ * any time, and will call wmr_controller_connection_disconnect()
3232+ * when the HMD xrt_device is freed by the runtime.
3333+ *
3434+ * The controller may send packets based on calls from the
3535+ * runtime (triggering haptics, for example), so can also
3636+ * want to send packets at any time. It will also call
3737+ * wmr_controller_connection_disconnect()
3838+ * when the controller xrt_device is freed by the runtime.
3939+ *
4040+ * The conn_lock protects access to the HMD and controller
4141+ * pointers while making calls to send/receive, to prevent
4242+ * from invalid access if _disconnect() is called.
4343+ */
4444+4545+static bool
4646+send_bytes_to_controller(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size)
4747+{
4848+ struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc);
4949+ bool res = false;
5050+5151+ assert(buf_size < 64);
5252+5353+ os_mutex_lock(&conn->lock);
5454+ if (!conn->disconnected && buf_size > 0) {
5555+ uint8_t outbuf[64];
5656+5757+ memcpy(outbuf, buffer, buf_size);
5858+ outbuf[0] += conn->hmd_cmd_base;
5959+ res = wmr_hmd_send_controller_packet(conn->hmd, outbuf, buf_size);
6060+ }
6161+ os_mutex_unlock(&conn->lock);
6262+6363+ return res;
6464+}
6565+6666+static int
6767+read_sync_from_controller(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms)
6868+{
6969+ struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc);
7070+ int res = -1;
7171+7272+ os_mutex_lock(&conn->lock);
7373+ if (!conn->disconnected && buf_size > 0) {
7474+ res = wmr_hmd_read_sync_from_controller(conn->hmd, buffer, buf_size, timeout_ms);
7575+ }
7676+ os_mutex_unlock(&conn->lock);
7777+7878+ return res;
7979+}
8080+8181+static void
8282+receive_bytes_from_controller(struct wmr_controller_connection *wcc,
8383+ uint64_t time_ns,
8484+ uint8_t *buffer,
8585+ uint32_t buf_size)
8686+{
8787+ struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc);
8888+ os_mutex_lock(&conn->lock);
8989+ if (!conn->disconnected && buf_size > 0) {
9090+ buffer[0] -= conn->hmd_cmd_base;
9191+9292+ struct wmr_controller_base *wcb = wcc->wcb;
9393+ assert(wcb->receive_bytes != NULL);
9494+ wcb->receive_bytes(wcb, time_ns, buffer, buf_size);
9595+ }
9696+ os_mutex_unlock(&conn->lock);
9797+}
9898+9999+static void
100100+wmr_hmd_controller_connection_destroy(struct wmr_hmd_controller_connection *conn)
101101+{
102102+ DRV_TRACE_MARKER();
103103+104104+ os_mutex_destroy(&conn->lock);
105105+ free(conn);
106106+}
107107+108108+static void
109109+wmr_hmd_controller_connection_disconnect(struct wmr_controller_connection *base)
110110+{
111111+ struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(base);
112112+113113+ if (xrt_reference_dec(&conn->ref)) {
114114+ wmr_hmd_controller_connection_destroy(conn);
115115+ } else {
116116+ os_mutex_lock(&conn->lock);
117117+ conn->disconnected = true;
118118+ os_mutex_unlock(&conn->lock);
119119+ }
120120+}
121121+122122+struct wmr_hmd_controller_connection *
123123+wmr_hmd_controller_create(struct wmr_hmd *hmd,
124124+ uint8_t hmd_cmd_base,
125125+ enum xrt_device_type controller_type,
126126+ uint16_t vid,
127127+ uint16_t pid,
128128+ enum u_logging_level log_level)
129129+{
130130+ DRV_TRACE_MARKER();
131131+132132+ struct wmr_hmd_controller_connection *conn = calloc(1, sizeof(struct wmr_hmd_controller_connection));
133133+134134+ conn->log_level = log_level;
135135+136136+ conn->hmd = hmd;
137137+ conn->hmd_cmd_base = hmd_cmd_base;
138138+139139+ conn->base.receive_bytes = receive_bytes_from_controller;
140140+ conn->base.send_bytes = send_bytes_to_controller;
141141+ conn->base.read_sync = read_sync_from_controller;
142142+ conn->base.disconnect = wmr_hmd_controller_connection_disconnect;
143143+144144+ /* Init 2 references - one for the controller, one for the HMD */
145145+ xrt_reference_inc(&conn->ref);
146146+ xrt_reference_inc(&conn->ref);
147147+148148+ int ret = 0;
149149+150150+ ret = os_mutex_init(&conn->lock);
151151+ if (ret != 0) {
152152+ WMR_ERROR(conn, "WMR Controller (Tunnelled): Failed to init mutex!");
153153+ wmr_hmd_controller_connection_destroy(conn);
154154+ return NULL;
155155+ }
156156+157157+ // Takes ownership of one reference to the connection, the other will
158158+ // belong to the returned pointer
159159+ struct wmr_controller_base *wcb = wmr_controller_create(&conn->base, controller_type, vid, pid, log_level);
160160+ if (wcb == NULL) {
161161+ WMR_ERROR(conn, "WMR Controller (Tunnelled): Failed to create controller");
162162+ wmr_hmd_controller_connection_destroy(conn);
163163+ return NULL;
164164+ }
165165+166166+ // If the controller device was created, the connection belongs to
167167+ // it now and will be cleaned up when it calls disconnect().
168168+ conn->base.wcb = wcb;
169169+170170+ return conn;
171171+}
172172+173173+struct xrt_device *
174174+wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc)
175175+{
176176+ struct wmr_controller_base *wcb = wcc->base.wcb;
177177+ struct xrt_device *xdev = &wcb->base;
178178+179179+ return xdev;
180180+}
+54
src/xrt/drivers/wmr/wmr_hmd_controller.h
···11+// Copyright 2023 Jan Schmidt
22+// SPDX-License-Identifier: BSL-1.0
33+/*!
44+ * @file
55+ * @brief Implementation of tunnelled controller connection,
66+ * that translates messages passing via an HP G2 or Sasmung Odyssey+ HMD
77+ * @author Jan Schmidt <jan@centricular.com>
88+ * @ingroup drv_wmr
99+ */
1010+#include <stdint.h>
1111+1212+#include "os/os_threading.h"
1313+#include "xrt/xrt_device.h"
1414+1515+#include "wmr_controller_base.h"
1616+1717+#pragma once
1818+1919+#ifdef __cplusplus
2020+extern "C" {
2121+#endif
2222+2323+struct wmr_hmd_controller_connection
2424+{
2525+ struct wmr_controller_connection base;
2626+2727+ /* Controller and HMD each hold a reference. It's
2828+ * only cleaned up once both release it. */
2929+ struct xrt_reference ref;
3030+ enum u_logging_level log_level;
3131+3232+ uint8_t hmd_cmd_base;
3333+3434+ /* Protect access when sending / receiving data */
3535+ struct os_mutex lock;
3636+ bool disconnected; /* Set to true once disconnect() is called */
3737+3838+ struct wmr_hmd *hmd;
3939+};
4040+4141+struct wmr_hmd_controller_connection *
4242+wmr_hmd_controller_create(struct wmr_hmd *hmd,
4343+ uint8_t hmd_cmd_base,
4444+ enum xrt_device_type controller_type,
4545+ uint16_t vid,
4646+ uint16_t pid,
4747+ enum u_logging_level log_level);
4848+4949+struct xrt_device *
5050+wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc);
5151+5252+#ifdef __cplusplus
5353+}
5454+#endif