matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2021, N Madsen.
2// Copyright 2020-2021, Collabora, Ltd.
3// Copyright 2020-2023, Jan Schmidt
4// SPDX-License-Identifier: BSL-1.0
5/*!
6 * @file
7 * @brief Driver for Bluetooth based WMR Controller.
8 * @author Nis Madsen <nima_zero_one@protonmail.com>
9 * @ingroup drv_wmr
10 */
11
12#include "os/os_time.h"
13#include "os/os_hid.h"
14
15#include "util/u_trace_marker.h"
16
17#include "wmr_common.h"
18#include "wmr_bt_controller.h"
19#include "wmr_controller.h"
20#include "wmr_config_key.h"
21
22#include <stdio.h>
23#include <stdlib.h>
24#include <string.h>
25#include <assert.h>
26#include <errno.h>
27
28#define WMR_TRACE(c, ...) U_LOG_IFL_T(c->log_level, __VA_ARGS__)
29#define WMR_DEBUG(c, ...) U_LOG_IFL_D(c->log_level, __VA_ARGS__)
30#define WMR_INFO(c, ...) U_LOG_IFL_I(c->log_level, __VA_ARGS__)
31#define WMR_WARN(c, ...) U_LOG_IFL_W(c->log_level, __VA_ARGS__)
32#define WMR_ERROR(c, ...) U_LOG_IFL_E(c->log_level, __VA_ARGS__)
33
34static inline struct wmr_bt_connection *
35wmr_bt_connection(struct wmr_controller_connection *p)
36{
37 return (struct wmr_bt_connection *)p;
38}
39
40static bool
41read_packets(struct wmr_bt_connection *conn)
42{
43 DRV_TRACE_MARKER();
44
45 unsigned char buffer[WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE];
46
47 // Better cpu efficiency with blocking reads instead of multiple reads.
48 os_mutex_lock(&conn->hid_lock);
49 int size = os_hid_read(conn->controller_hid, buffer, sizeof(buffer), 500);
50
51 // Get the timing as close to reading packet as possible.
52 uint64_t now_ns = os_monotonic_get_ns();
53 os_mutex_unlock(&conn->hid_lock);
54
55 DRV_TRACE_IDENT(read_packets_got);
56
57 if (size < 0) {
58 WMR_ERROR(conn, "WMR Controller (Bluetooth): Error reading from device");
59 return false;
60 }
61 if (size == 0) {
62 WMR_TRACE(conn, "WMR Controller (Bluetooth): No data to read from device");
63 return true; // No more messages, return.
64 }
65
66 WMR_TRACE(conn, "WMR Controller (Bluetooth): Read %u bytes from device", size);
67
68 struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)conn;
69 wmr_controller_connection_receive_bytes(wcc, now_ns, buffer, size);
70
71 return true;
72}
73
74static bool
75send_bytes(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size)
76{
77 struct wmr_bt_connection *conn = (struct wmr_bt_connection *)(wcc);
78
79 os_mutex_lock(&conn->hid_lock);
80 int ret = os_hid_write(conn->controller_hid, buffer, buf_size);
81 os_mutex_unlock(&conn->hid_lock);
82
83 return ret != -1 && (uint32_t)(ret) == buf_size;
84}
85
86/* Synchronously read a buffer from the HID connection.
87 * This is only used for reading firmware during startup,
88 * before the hid reading loop is running */
89static int
90read_sync(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms)
91{
92 struct wmr_bt_connection *conn = (struct wmr_bt_connection *)(wcc);
93
94 os_mutex_lock(&conn->hid_lock);
95 int res = os_hid_read(conn->controller_hid, buffer, buf_size, timeout_ms);
96 os_mutex_unlock(&conn->hid_lock);
97
98 return res;
99}
100
101static void *
102wmr_bt_connection_run_thread(void *ptr)
103{
104 U_TRACE_SET_THREAD_NAME("WMR: BT-Controller");
105
106 struct wmr_bt_connection *conn = wmr_bt_connection(ptr);
107
108 os_thread_helper_lock(&conn->controller_thread);
109 while (os_thread_helper_is_running_locked(&conn->controller_thread)) {
110 os_thread_helper_unlock(&conn->controller_thread);
111
112 // Does not block.
113 if (!read_packets(conn)) {
114 break;
115 }
116 }
117
118 WMR_DEBUG(conn, "WMR Controller (Bluetooth): Exiting reading thread.");
119
120 return NULL;
121}
122
123static void
124wmr_bt_connection_destroy(struct wmr_controller_connection *base)
125{
126 struct wmr_bt_connection *conn = (struct wmr_bt_connection *)base;
127
128 DRV_TRACE_MARKER();
129
130 // Destroy the thread object.
131 os_thread_helper_destroy(&conn->controller_thread);
132
133 if (conn->controller_hid != NULL) {
134 os_hid_destroy(conn->controller_hid);
135 conn->controller_hid = NULL;
136 }
137
138 os_mutex_destroy(&conn->hid_lock);
139
140 free(conn);
141}
142
143/*
144 *
145 * 'Exported' functions.
146 *
147 */
148
149struct xrt_device *
150wmr_bt_controller_create(struct os_hid_device *controller_hid,
151 enum xrt_device_type controller_type,
152 uint16_t vid,
153 uint16_t pid,
154 enum u_logging_level log_level)
155{
156 DRV_TRACE_MARKER();
157
158 struct wmr_bt_connection *conn = calloc(1, sizeof(struct wmr_bt_connection));
159
160 conn->log_level = log_level;
161 conn->controller_hid = controller_hid;
162
163 conn->base.send_bytes = send_bytes;
164 conn->base.read_sync = read_sync;
165 conn->base.disconnect = wmr_bt_connection_destroy;
166
167 int ret = 0;
168
169 ret = os_mutex_init(&conn->hid_lock);
170 if (ret != 0) {
171 WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to init mutex!");
172 wmr_bt_connection_destroy(&conn->base);
173 return NULL;
174 }
175
176 // Thread and other state.
177 ret = os_thread_helper_init(&conn->controller_thread);
178 if (ret != 0) {
179 WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to init controller threading!");
180 wmr_bt_connection_destroy(&conn->base);
181 return NULL;
182 }
183
184 // Takes ownership of the connection
185 struct wmr_controller_base *wcb = wmr_controller_create(&conn->base, controller_type, vid, pid, log_level);
186 if (wcb == NULL) {
187 WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to create controller");
188 return NULL;
189 }
190
191 // If the controller device was created, the connection belongs to
192 // it now and will be cleaned up when it calls disconnect().
193 conn->base.wcb = wcb;
194
195 struct xrt_device *xdev = &wcb->base;
196
197 // Hand over controller device to reading thread.
198 ret = os_thread_helper_start(&conn->controller_thread, wmr_bt_connection_run_thread, conn);
199 if (ret != 0) {
200 WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to start controller thread!");
201 xrt_device_destroy(&xdev);
202 return NULL;
203 }
204
205 return xdev;
206}