matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2021, N Madsen.
2// Copyright 2020-2023, Collabora, Ltd.
3// Copyright 2020-2023, Jan Schmidt
4// SPDX-License-Identifier: BSL-1.0
5/*!
6 * @file
7 * @brief Driver for WMR Controller.
8 * @author Jan Schmidt <jan@centricular.com>
9 * @ingroup drv_wmr
10 */
11
12#include "os/os_time.h"
13#include "os/os_hid.h"
14
15#include "math/m_mathinclude.h"
16#include "math/m_api.h"
17#include "math/m_clock_tracking.h"
18#include "math/m_vec2.h"
19#include "math/m_predict.h"
20
21#include "util/u_file.h"
22#include "util/u_var.h"
23#include "util/u_misc.h"
24#include "util/u_time.h"
25#include "util/u_debug.h"
26#include "util/u_device.h"
27#include "util/u_trace_marker.h"
28
29#include "wmr_common.h"
30#include "wmr_controller_base.h"
31#include "wmr_config_key.h"
32
33#include <ctype.h>
34#include <stdio.h>
35#include <stdlib.h>
36#include <string.h>
37#include <assert.h>
38#include <errno.h>
39
40#define WMR_TRACE(wcb, ...) U_LOG_XDEV_IFL_T(&wcb->base, wcb->log_level, __VA_ARGS__)
41#define WMR_TRACE_HEX(wcb, ...) U_LOG_XDEV_IFL_T_HEX(&wcb->base, wcb->log_level, __VA_ARGS__)
42#define WMR_DEBUG(wcb, ...) U_LOG_XDEV_IFL_D(&wcb->base, wcb->log_level, __VA_ARGS__)
43#define WMR_DEBUG_HEX(wcb, ...) U_LOG_XDEV_IFL_D_HEX(&wcb->base, wcb->log_level, __VA_ARGS__)
44#define WMR_INFO(wcb, ...) U_LOG_XDEV_IFL_I(&wcb->base, wcb->log_level, __VA_ARGS__)
45#define WMR_WARN(wcb, ...) U_LOG_XDEV_IFL_W(&wcb->base, wcb->log_level, __VA_ARGS__)
46#define WMR_ERROR(wcb, ...) U_LOG_XDEV_IFL_E(&wcb->base, wcb->log_level, __VA_ARGS__)
47
48#define wmr_controller_hexdump_buffer(wcb, label, buf, length) \
49 do { \
50 WMR_DEBUG(wcb, "%s", label); \
51 WMR_DEBUG_HEX(wcb, buf, length); \
52 } while (0);
53
54
55static inline struct wmr_controller_base *
56wmr_controller_base(struct xrt_device *p)
57{
58 return (struct wmr_controller_base *)p;
59}
60
61static void
62receive_bytes(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size)
63{
64 if (buf_size < 1) {
65 WMR_ERROR(wcb, "WMR Controller: Error receiving short packet");
66 return;
67 }
68
69 switch (buffer[0]) {
70 case WMR_MOTION_CONTROLLER_STATUS_MSG:
71 os_mutex_lock(&wcb->data_lock);
72 // Note: skipping msg type byte
73 bool b = wcb->handle_input_packet(wcb, time_ns, &buffer[1], (size_t)buf_size - 1);
74 os_mutex_unlock(&wcb->data_lock);
75
76 if (!b) {
77 WMR_ERROR(wcb, "WMR Controller: Failed handling message type: %02x, size: %i", buffer[0],
78 buf_size);
79 wmr_controller_hexdump_buffer(wcb, "Controller Message", buffer, buf_size);
80 return;
81 }
82
83 break;
84 default: WMR_DEBUG(wcb, "WMR Controller: Unknown message type: %02x, size: %i", buffer[0], buf_size); break;
85 }
86
87 return;
88}
89
90static bool
91wmr_controller_send_bytes(struct wmr_controller_base *wcb, const uint8_t *buffer, uint32_t buf_size)
92{
93 bool res = false;
94
95 os_mutex_lock(&wcb->conn_lock);
96 struct wmr_controller_connection *conn = wcb->wcc;
97 if (conn != NULL) {
98 res = wmr_controller_connection_send_bytes(conn, buffer, buf_size);
99 }
100 os_mutex_unlock(&wcb->conn_lock);
101
102 return res;
103}
104
105static int
106wmr_controller_read_sync(struct wmr_controller_base *wcb, uint8_t *buffer, uint32_t buf_size, int timeout_ms)
107{
108 int res = -1;
109 os_mutex_lock(&wcb->conn_lock);
110 struct wmr_controller_connection *conn = wcb->wcc;
111 if (conn != NULL) {
112 res = wmr_controller_connection_read_sync(conn, buffer, buf_size, timeout_ms);
113 }
114 os_mutex_unlock(&wcb->conn_lock);
115
116 return res;
117}
118
119static int
120wmr_controller_send_fw_cmd(struct wmr_controller_base *wcb,
121 const struct wmr_controller_fw_cmd *fw_cmd,
122 unsigned char response_code,
123 struct wmr_controller_fw_cmd_response *response)
124{
125 // comms timeout. Replies are usually in 10ms or so but the first can take longer
126 const int timeout_ms = 250;
127 const int timeout_ns = timeout_ms * U_TIME_1MS_IN_NS;
128 int64_t timeout_start = os_monotonic_get_ns();
129 int64_t timeout_end_ns = timeout_start + timeout_ns;
130
131 if (!wmr_controller_send_bytes(wcb, fw_cmd->buf, sizeof(fw_cmd->buf))) {
132 return -1;
133 }
134
135 do {
136 int size = wmr_controller_read_sync(wcb, response->buf, sizeof(response->buf), timeout_ms);
137 if (size == -1) {
138 return -1;
139 }
140
141 if (size < 1) {
142 // Ignore 0-byte reads (timeout) and try again
143 continue;
144 }
145
146 if (response->buf[0] == response_code) {
147 WMR_TRACE(wcb, "Controller fw read returned %d bytes", size);
148 if (size != sizeof(response->buf) || (response->response.cmd_id_echo != fw_cmd->cmd.cmd_id)) {
149 WMR_DEBUG(
150 wcb, "Unexpected fw response - size %d (expected %zu), cmd_id_echo %u != cmd_id %u",
151 size, sizeof(response->buf), response->response.cmd_id_echo, fw_cmd->cmd.cmd_id);
152 return -1;
153 }
154
155 response->response.blk_remain = __le32_to_cpu(response->response.blk_remain);
156 return size;
157 }
158 } while (os_monotonic_get_ns() < timeout_end_ns);
159
160 WMR_WARN(wcb, "Controller fw read timed out after %u ms",
161 (unsigned int)((os_monotonic_get_ns() - timeout_start) / U_TIME_1MS_IN_NS));
162 return -ETIMEDOUT;
163}
164
165XRT_MAYBE_UNUSED static int
166wmr_read_fw_block(struct wmr_controller_base *d, uint8_t blk_id, uint8_t **out_data, size_t *out_size)
167{
168 struct wmr_controller_fw_cmd_response fw_cmd_response;
169
170 uint8_t *data;
171 uint8_t *data_pos;
172 uint8_t *data_end;
173 uint32_t data_size;
174 uint32_t remain;
175
176 struct wmr_controller_fw_cmd fw_cmd;
177 memset(&fw_cmd, 0, sizeof(fw_cmd));
178
179 fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, 0xffffffff);
180 if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) {
181 WMR_WARN(d, "Failed to read fw - cmd 0x02 failed to read header for block %d", blk_id);
182 return -1;
183 }
184
185 data_size = fw_cmd_response.response.blk_remain + fw_cmd_response.response.len;
186 WMR_DEBUG(d, "FW header %d bytes, %u bytes in block", fw_cmd_response.response.len, data_size);
187 if (data_size == 0)
188 return -1;
189
190 data = calloc(1, data_size + 1);
191 if (!data) {
192 return -1;
193 }
194 data[data_size] = '\0';
195
196 remain = data_size;
197 data_pos = data;
198 data_end = data + data_size;
199
200 uint8_t to_copy = fw_cmd_response.response.len;
201
202 memcpy(data_pos, fw_cmd_response.response.data, to_copy);
203 data_pos += to_copy;
204 remain -= to_copy;
205
206 while (remain > 0) {
207 fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, remain);
208
209 os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms
210 if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) {
211 WMR_WARN(d, "Failed to read fw - cmd 0x02 failed @ offset %zu", data_pos - data);
212 return -1;
213 }
214
215 uint8_t to_copy = fw_cmd_response.response.len;
216 if (data_pos + to_copy > data_end)
217 to_copy = data_end - data_pos;
218
219 WMR_DEBUG(d, "Read %d bytes @ offset %zu / %d", to_copy, data_pos - data, data_size);
220 memcpy(data_pos, fw_cmd_response.response.data, to_copy);
221 data_pos += to_copy;
222 remain -= to_copy;
223 }
224
225 WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id);
226 wmr_controller_hexdump_buffer(d, "Data block", data, data_size);
227
228 *out_data = data;
229 *out_size = data_size;
230
231 return 0;
232}
233
234/*
235 *
236 * Config functions.
237 *
238 */
239static bool
240read_controller_fw_info(struct wmr_controller_base *wcb,
241 uint32_t *fw_revision,
242 uint16_t *calibration_size,
243 char serial_no[16])
244{
245 uint8_t *data = NULL;
246 size_t data_size;
247 int ret;
248
249 /* FW block 0 contains the FW revision (offset 0x14, size 4) and
250 * calibration block size (offset 0x34 size 2) */
251 ret = wmr_read_fw_block(wcb, 0x0, &data, &data_size);
252 if (ret < 0 || data == NULL) {
253 WMR_ERROR(wcb, "Failed to read FW info block 0");
254 return false;
255 }
256 if (data_size < 0x36) {
257 WMR_ERROR(wcb, "Failed to read FW info block 0 - too short");
258 free(data);
259 return false;
260 }
261
262
263 const unsigned char *tmp = data + 0x14;
264 *fw_revision = read32(&tmp);
265 tmp = data + 0x34;
266 *calibration_size = read16(&tmp);
267
268 free(data);
269
270 /* FW block 3 contains the controller serial number at offset
271 * 0x84, size 16 bytes */
272 ret = wmr_read_fw_block(wcb, 0x3, &data, &data_size);
273 if (ret < 0 || data == NULL) {
274 WMR_ERROR(wcb, "Failed to read FW info block 3");
275 return false;
276 }
277 if (data_size < 0x94) {
278 WMR_ERROR(wcb, "Failed to read FW info block 3 - too short");
279 free(data);
280 return false;
281 }
282
283 memcpy(serial_no, data + 0x84, 0x10);
284 serial_no[16] = '\0';
285
286 free(data);
287 return true;
288}
289
290char *
291build_cache_filename(char *serial_no)
292{
293 int outlen = strlen("controller-") + strlen(serial_no) + strlen(".json") + 1;
294 char *out = malloc(outlen);
295 int ret = snprintf(out, outlen, "controller-%s.json", serial_no);
296
297 assert(ret <= outlen);
298 (void)ret;
299
300 // Make sure the filename is valid
301 for (char *cur = out; *cur != '\0'; cur++) {
302 if (!isalnum(*cur) && *cur != '.') {
303 *cur = '_';
304 }
305 }
306
307 return out;
308}
309
310static bool
311read_calibration_cache(struct wmr_controller_base *wcb, char *cache_filename)
312{
313 FILE *f = u_file_open_file_in_config_dir_subpath("wmr", cache_filename, "r");
314 uint8_t *buffer = NULL;
315
316 if (f == NULL) {
317 WMR_DEBUG(wcb, "Failed to open wmr/%s cache file or it doesn't exist.", cache_filename);
318 return false;
319 }
320
321 // Read the file size to allocate a read buffer
322 fseek(f, 0L, SEEK_END);
323 size_t file_size = ftell(f);
324
325 // Reset and read the data
326 fseek(f, 0L, SEEK_SET);
327
328 buffer = calloc(file_size + 1, sizeof(uint8_t));
329 if (buffer == NULL) {
330 goto fail;
331 }
332 buffer[file_size] = '\0';
333
334 size_t ret = fread(buffer, sizeof(char), file_size, f);
335 if (ret != file_size) {
336 WMR_WARN(wcb, "Cache file wmr/%s failed to read %u bytes (got %u)", cache_filename, (int)file_size,
337 (int)ret);
338 goto fail;
339 }
340
341 if (!wmr_controller_config_parse(&wcb->config, (char *)buffer, wcb->log_level)) {
342 WMR_WARN(wcb, "Cache file wmr/%s contains invalid JSON. Ignoring", cache_filename);
343 goto fail;
344 }
345
346 fclose(f);
347 free(buffer);
348
349 return true;
350
351fail:
352 if (buffer) {
353 free(buffer);
354 }
355 fclose(f);
356 return false;
357}
358
359static void
360write_calibration_cache(struct wmr_controller_base *wcb, char *cache_filename, uint8_t *data, size_t data_size)
361{
362 FILE *f = u_file_open_file_in_config_dir_subpath("wmr", cache_filename, "w");
363 if (f == NULL) {
364 return;
365 }
366
367 size_t ret = fwrite(data, sizeof(char), data_size, f);
368 if (ret != data_size) {
369 fclose(f);
370 return;
371 }
372
373 fclose(f);
374}
375
376static bool
377read_controller_config(struct wmr_controller_base *wcb)
378{
379 unsigned char *config_json_block;
380 int ret;
381 uint32_t fw_revision;
382 uint16_t calibration_size;
383 char serial_no[16 + 1];
384
385 if (!read_controller_fw_info(wcb, &fw_revision, &calibration_size, serial_no)) {
386 return false;
387 }
388
389 WMR_INFO(wcb, "Reading configuration for controller serial %s. FW revision %x", serial_no, fw_revision);
390
391#if 0
392 /* WMR also reads block 0x14, which seems to have some FW revision info,
393 * but we don't use it */
394 // Read block 0x14
395 ret = wmr_read_fw_block(wcb, 0x14, &data, &data_size);
396 if (ret < 0 || data == NULL)
397 return false;
398 free(data);
399 data = NULL;
400#endif
401
402 // Read config block
403 WMR_INFO(wcb, "Reading %s controller config",
404 wcb->base.device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER ? "left" : "right");
405
406 // Check if we have it cached already
407 char *cache_filename = build_cache_filename(serial_no);
408
409 if (!read_calibration_cache(wcb, cache_filename)) {
410 unsigned char *data = NULL;
411 size_t data_size;
412
413 ret = wmr_read_fw_block(wcb, 0x02, &data, &data_size);
414 if (ret < 0 || data == NULL || data_size < 2) {
415 free(cache_filename);
416 return false;
417 }
418
419 /* De-obfuscate the JSON config */
420 config_json_block = data + sizeof(uint16_t);
421 for (unsigned int i = 0; i < data_size - sizeof(uint16_t); i++) {
422 config_json_block[i] ^= wmr_config_key[i % sizeof(wmr_config_key)];
423 }
424
425 if (!wmr_controller_config_parse(&wcb->config, (char *)config_json_block, wcb->log_level)) {
426 free(cache_filename);
427 free(data);
428 return false;
429 }
430
431 /* Write to the cache file (if it fails, ignore it, it's just a cache) */
432 write_calibration_cache(wcb, cache_filename, config_json_block, data_size - sizeof(uint16_t));
433 free(data);
434 } else {
435 WMR_DEBUG(wcb, "Read %s controller config from cache %s",
436 wcb->base.device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER ? "left" : "right",
437 cache_filename);
438 }
439 free(cache_filename);
440
441 WMR_DEBUG(wcb, "Parsed %d LED entries from controller calibration", wcb->config.led_count);
442
443 return true;
444}
445
446static xrt_result_t
447wmr_controller_base_get_tracked_pose(struct xrt_device *xdev,
448 enum xrt_input_name name,
449 int64_t at_timestamp_ns,
450 struct xrt_space_relation *out_relation)
451{
452 DRV_TRACE_MARKER();
453
454 struct wmr_controller_base *wcb = wmr_controller_base(xdev);
455
456 // Variables needed for prediction.
457 int64_t last_imu_timestamp_ns = 0;
458 struct xrt_space_relation relation = {0};
459 relation.relation_flags = (enum xrt_space_relation_flags)(
460 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
461 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
462
463
464 struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}};
465 if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
466 pose.position.x = -0.2;
467 } else {
468 pose.position.x = 0.2;
469 }
470 relation.pose = pose;
471
472 // Copy data while holding the lock.
473 os_mutex_lock(&wcb->data_lock);
474 relation.pose.orientation = wcb->fusion.rot;
475 relation.angular_velocity = wcb->last_angular_velocity;
476 last_imu_timestamp_ns = wcb->last_imu_timestamp_ns;
477 os_mutex_unlock(&wcb->data_lock);
478
479 // No prediction needed.
480 if (at_timestamp_ns < last_imu_timestamp_ns) {
481 *out_relation = relation;
482 return XRT_SUCCESS;
483 }
484
485 int64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
486 double prediction_s = time_ns_to_s(prediction_ns);
487
488 m_predict_relation(&relation, prediction_s, out_relation);
489
490 return XRT_SUCCESS;
491}
492
493void
494wmr_controller_base_deinit(struct wmr_controller_base *wcb)
495{
496 DRV_TRACE_MARKER();
497
498 // Remove the variable tracking.
499 u_var_remove_root(wcb);
500
501 // Disconnect from the connection so we don't
502 // receive any more callbacks
503 os_mutex_lock(&wcb->conn_lock);
504 struct wmr_controller_connection *conn = wcb->wcc;
505 wcb->wcc = NULL;
506 os_mutex_unlock(&wcb->conn_lock);
507
508 if (conn != NULL) {
509 wmr_controller_connection_disconnect(conn);
510 }
511
512 os_mutex_destroy(&wcb->conn_lock);
513 os_mutex_destroy(&wcb->data_lock);
514
515 // Destroy the fusion.
516 m_imu_3dof_close(&wcb->fusion);
517}
518
519/*
520 *
521 * 'Exported' functions.
522 *
523 */
524
525bool
526wmr_controller_base_init(struct wmr_controller_base *wcb,
527 struct wmr_controller_connection *conn,
528 enum xrt_device_type controller_type,
529 enum u_logging_level log_level,
530 u_device_destroy_function_t destroy_fn)
531{
532 DRV_TRACE_MARKER();
533
534 wcb->log_level = log_level;
535 wcb->wcc = conn;
536 wcb->receive_bytes = receive_bytes;
537
538 if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
539 snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Left Controller");
540 /* TODO: use proper serial from read_controller_config()? */
541 snprintf(wcb->base.serial, XRT_DEVICE_NAME_LEN, "Left Controller");
542 } else {
543 snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Right Controller");
544 /* TODO: use proper serial from read_controller_config()? */
545 snprintf(wcb->base.serial, XRT_DEVICE_NAME_LEN, "Right Controller");
546 }
547
548 // Set all functions.
549 u_device_populate_function_pointers(&wcb->base, wmr_controller_base_get_tracked_pose, destroy_fn);
550
551 wcb->base.name = XRT_DEVICE_WMR_CONTROLLER;
552 wcb->base.device_type = controller_type;
553 wcb->base.supported.orientation_tracking = true;
554 wcb->base.supported.position_tracking = false;
555 wcb->base.supported.hand_tracking = false;
556
557 m_imu_3dof_init(&wcb->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
558
559 if (os_mutex_init(&wcb->conn_lock) != 0 || os_mutex_init(&wcb->data_lock) != 0) {
560 WMR_ERROR(wcb, "WMR Controller: Failed to init mutex!");
561 return false;
562 }
563
564 u_var_add_root(wcb, wcb->base.str, true);
565
566 /* Send init commands */
567 struct wmr_controller_fw_cmd fw_cmd = {
568 0,
569 };
570 struct wmr_controller_fw_cmd_response fw_cmd_response;
571
572 /* Zero command. Reinits controller internal state */
573 fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x0, 0, 0);
574 if (wmr_controller_send_fw_cmd(wcb, &fw_cmd, 0x06, &fw_cmd_response) < 0) {
575 return false;
576 }
577
578 /* Quiesce/restart controller tasks */
579 fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x04, 0xc1, 0x02);
580 if (wmr_controller_send_fw_cmd(wcb, &fw_cmd, 0x06, &fw_cmd_response) < 0) {
581 return false;
582 }
583
584 // Read config file from controller
585 if (!read_controller_config(wcb)) {
586 return false;
587 }
588
589 wmr_config_precompute_transforms(&wcb->config.sensors, NULL);
590
591 /* Enable the status reports, IMU and control status reports */
592 const unsigned char wmr_controller_status_enable_cmd[64] = {0x06, 0x03, 0x01, 0x00, 0x02};
593 wmr_controller_send_bytes(wcb, wmr_controller_status_enable_cmd, sizeof(wmr_controller_status_enable_cmd));
594 const unsigned char wmr_controller_imu_on_cmd[64] = {0x06, 0x03, 0x02, 0xe1, 0x02};
595 wmr_controller_send_bytes(wcb, wmr_controller_imu_on_cmd, sizeof(wmr_controller_imu_on_cmd));
596
597 return true;
598}