matrixfurry.com
The open source OpenXR runtime
1// Copyright 2016-2019, Philipp Zabel
2// Copyright 2019, Collabora, Ltd.
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief Vive USB HID reports
7 * @author Christoph Haag <christoph.haag@collabora.com>
8 * @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
9 * @ingroup drv_vive
10 */
11
12#include "math/m_mathinclude.h"
13#include "math/m_api.h"
14
15#include "util/u_debug.h"
16#include "util/u_misc.h"
17#include "util/u_json.h"
18#include "util/u_logging.h"
19#include "util/u_trace_marker.h"
20
21#include "vive_protocol.h"
22
23#include <stdio.h>
24#include <zlib.h>
25
26
27const struct vive_headset_power_report power_on_report = {
28 .id = VIVE_HEADSET_POWER_REPORT_ID,
29 .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
30 .len = 56,
31 .unknown1 =
32 {
33 0x01,
34 0x00,
35 0x00,
36 0x00,
37 0x00,
38 0x00,
39 0x02,
40 0x00,
41 0x01,
42 },
43 .unknown2 = 0x7a,
44};
45
46const struct vive_headset_power_report power_off_report = {
47 .id = VIVE_HEADSET_POWER_REPORT_ID,
48 .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
49 .len = 56,
50 .unknown1 =
51 {
52 0x00,
53 0x00,
54 0x00,
55 0x00,
56 0x00,
57 0x00,
58 0x02,
59 0x00,
60 0x00,
61 },
62 .unknown2 = 0x7c,
63};
64
65#define CONFIG_Z_MAX_SIZE (1024 * 16)
66#define CONFIG_JSON_MAX_SIZE (1024 * 64)
67
68char *
69vive_read_config(struct os_hid_device *hid_dev)
70{
71 XRT_TRACE_MARKER();
72
73 struct vive_config_start_report start_report = {
74 .id = VIVE_CONFIG_START_REPORT_ID,
75 };
76
77 int ret = os_hid_get_feature_timeout(hid_dev, &start_report, sizeof(start_report), 100);
78 if (ret < 0) {
79 // e.g. watchman receiver has no connected device (controller powered off)
80 U_LOG_I("Could not get config start report for device, connected device may be powered off (%d).", ret);
81 return NULL;
82 }
83
84 struct vive_config_read_report report = {
85 .id = VIVE_CONFIG_READ_REPORT_ID,
86 };
87
88 unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, CONFIG_Z_MAX_SIZE);
89
90 uint32_t count = 0;
91 do {
92 ret = os_hid_get_feature_timeout(hid_dev, &report, sizeof(report), 100);
93 if (ret < 0) {
94 U_LOG_E("Read error after %d bytes: %d", count, ret);
95 free(config_z);
96 return NULL;
97 }
98
99 if (report.len > 62) {
100 U_LOG_E("Invalid configuration data at %d", count);
101 free(config_z);
102 return NULL;
103 }
104
105 if (count + report.len > CONFIG_Z_MAX_SIZE) {
106 U_LOG_E("Configuration data too large");
107 free(config_z);
108 return NULL;
109 }
110
111 memcpy(config_z + count, report.payload, report.len);
112 count += report.len;
113 } while (report.len);
114
115 unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, CONFIG_JSON_MAX_SIZE);
116
117 z_stream strm = {
118 .next_in = config_z,
119 .avail_in = count,
120 .next_out = config_json,
121 .avail_out = CONFIG_JSON_MAX_SIZE,
122 .zalloc = Z_NULL,
123 .zfree = Z_NULL,
124 .opaque = Z_NULL,
125 };
126
127 ret = inflateInit(&strm);
128 if (ret != Z_OK) {
129 U_LOG_E("inflate_init failed: %d", ret);
130 free(config_z);
131 free(config_json);
132 return NULL;
133 }
134
135 ret = inflate(&strm, Z_FINISH);
136 free(config_z);
137 if (ret != Z_STREAM_END) {
138 U_LOG_E("Failed to inflate configuration data: %d", ret);
139 free(config_json);
140 return NULL;
141 }
142
143 config_json[strm.total_out] = '\0';
144
145 U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1);
146
147 inflateEnd(&strm);
148
149 return (char *)config_json;
150}
151
152int
153vive_get_imu_range_report(struct os_hid_device *hid_dev, double *gyro_range, double *acc_range)
154{
155 struct vive_imu_range_modes_report report = {.id = VIVE_IMU_RANGE_MODES_REPORT_ID};
156
157 int ret;
158
159 ret = os_hid_get_feature_timeout(hid_dev, &report, sizeof(report), 100);
160 if (ret < 0) {
161 U_LOG_I("Could not get range report, connected device may be powered off (%d)!", ret);
162 return ret;
163 }
164
165 if (!report.gyro_range || !report.accel_range) {
166 U_LOG_W(
167 "Invalid gyroscope and accelerometer data."
168 "Trying to fetch again.");
169 ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report, sizeof(report));
170 if (ret < 0) {
171 U_LOG_E("Could not get feature report %d.", report.id);
172 return ret;
173 }
174
175 if (!report.gyro_range || !report.accel_range) {
176 U_LOG_E("Unexpected range mode report: %02x %02x %02x", report.id, report.gyro_range,
177 report.accel_range);
178 for (int i = 0; i < 61; i++)
179 printf(" %02x", report.unknown[i]);
180 printf("\n");
181 return -1;
182 }
183 }
184
185 if (report.gyro_range > 4 || report.accel_range > 4) {
186 U_LOG_W("Gyroscope or accelerometer range too large.");
187 U_LOG_W("Gyroscope: %d", report.gyro_range);
188 U_LOG_W("Accelerometer: %d", report.accel_range);
189 return -1;
190 }
191
192 /*
193 * Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s,
194 * +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range
195 * (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s².
196 */
197
198 *gyro_range = M_PI / 180.0 * (250 << report.gyro_range);
199 *acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range);
200
201 return 0;
202}
203
204int
205vive_read_firmware(struct os_hid_device *hid_dev,
206 uint32_t *firmware_version,
207 uint8_t *hardware_revision,
208 uint8_t *hardware_version_micro,
209 uint8_t *hardware_version_minor,
210 uint8_t *hardware_version_major)
211{
212 struct vive_firmware_version_report report = {
213 .id = VIVE_FIRMWARE_VERSION_REPORT_ID,
214 };
215
216 int ret;
217 ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report, sizeof(report));
218 if (ret < 0)
219 return ret;
220
221 *firmware_version = __le32_to_cpu(report.firmware_version);
222 *hardware_revision = report.hardware_revision;
223 *hardware_version_major = report.hardware_version_major;
224 *hardware_version_minor = report.hardware_version_minor;
225 *hardware_version_micro = report.hardware_version_micro;
226
227 return 0;
228}