d/vive: Make common functions available through vive_protocol.h

Move common code to vive_protocol.c

src/xrt/drivers/CMakeLists.txt

@@ -139,6 +139,7 @@ if(BUILD_DRIVER_VIVE)
 		vive/vive_device.c
 		vive/vive_prober.h
 		vive/vive_prober.c
+		vive/vive_protocol.c
 		vive/vive_protocol.h
 		)
 

src/xrt/drivers/meson.build

@@ -119,6 +119,7 @@ lib_drv_vive = static_library(
 	files(
 		'vive/vive_device.c',
 		'vive/vive_device.h',
+		'vive/vive_protocol.c',
 		'vive/vive_protocol.h',
 		'vive/vive_prober.h',
 		'vive/vive_prober.c',

src/xrt/drivers/vive/vive_device.c

@@ -518,95 +518,7 @@ vive_sensors_run_thread(void *ptr)
 	return NULL;
 }
 
-
+static void
-int
-vive_sensors_read_firmware(struct vive_device *d)
-{
-	struct vive_firmware_version_report report = {
-	    .id = VIVE_FIRMWARE_VERSION_REPORT_ID,
-	};
-
-	int ret;
-	ret = os_hid_get_feature(d->sensors_dev, report.id, (uint8_t *)&report,
-	                         sizeof(report));
-	if (ret < 0)
-		return ret;
-
-	d->firmware.firmware_version = __le32_to_cpu(report.firmware_version);
-	d->firmware.hardware_revision = report.hardware_revision;
-
-	VIVE_DEBUG(d, "Firmware version %u %s@%s FPGA %u.%u",
-	           d->firmware.firmware_version, report.string1, report.string2,
-	           report.fpga_version_major, report.fpga_version_minor);
-	VIVE_DEBUG(d, "Hardware revision: %d rev %d.%d.%d",
-	           d->firmware.hardware_revision, report.hardware_version_major,
-	           report.hardware_version_minor,
-	           report.hardware_version_micro);
-
-	return 0;
-}
-
-int
-vive_sensors_get_imu_range_report(struct vive_device *d)
-{
-	struct vive_imu_range_modes_report report = {
-	    .id = VIVE_IMU_RANGE_MODES_REPORT_ID};
-
-	int ret;
-	ret = os_hid_get_feature(d->sensors_dev, report.id, (uint8_t *)&report,
-	                         sizeof(report));
-	if (ret < 0) {
-		printf("Could not get range report!\n");
-		return ret;
-	}
-
-	if (!report.gyro_range || !report.accel_range) {
-		VIVE_ERROR(
-		    "Invalid gyroscope and accelerometer data. Trying to fetch "
-		    "again.");
-		ret = os_hid_get_feature(d->sensors_dev, report.id,
-		                         (uint8_t *)&report, sizeof(report));
-		if (ret < 0) {
-			VIVE_ERROR("Could not get feature report %d.",
-			           report.id);
-			return ret;
-		}
-
-		if (!report.gyro_range || !report.accel_range) {
-			VIVE_ERROR(
-			    "Unexpected range mode report: %02x %02x %02x",
-			    report.id, report.gyro_range, report.accel_range);
-			for (int i = 0; i < 61; i++)
-				printf(" %02x", report.unknown[i]);
-			printf("\n");
-			return -1;
-		}
-	}
-
-	if (report.gyro_range > 4 || report.accel_range > 4) {
-		VIVE_ERROR("Gyroscope or accelerometer range too large.");
-		VIVE_ERROR("Gyroscope: %d", report.gyro_range);
-		VIVE_ERROR("Accelerometer: %d", report.accel_range);
-		return -1;
-	}
-
-	/*
-	 * Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s,
-	 * +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range
-	 * (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s².
-	 */
-
-	d->imu.gyro_range = M_PI / 180.0 * (250 << report.gyro_range);
-	VIVE_DEBUG(d, "Vive gyroscope range     %f", d->imu.gyro_range);
-
-	d->imu.acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range);
-	VIVE_DEBUG(d, "Vive accelerometer range %f", d->imu.acc_range);
-
-	return 0;
-}
-
-
-void
 print_vec3(const char *title, struct xrt_vec3 *vec)
 {
 	printf("%s = %f %f %f\n", title, (double)vec->x, (double)vec->y,
@@ -1048,86 +960,6 @@ vive_parse_config(struct vive_device *d, char *json_string)
 	return true;
 }
 
-char *
-vive_sensors_read_config(struct vive_device *d)
-{
-	struct vive_config_start_report start_report = {
-	    .id = VIVE_CONFIG_START_REPORT_ID,
-	};
-
-	int ret = os_hid_get_feature_timeout(d->sensors_dev, &start_report,
-	                                     sizeof(start_report), 100);
-	if (ret < 0) {
-		VIVE_ERROR("Could not get config start report.");
-		return NULL;
-	}
-
-	struct vive_config_read_report report = {
-	    .id = VIVE_CONFIG_READ_REPORT_ID,
-	};
-
-	unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, 4096);
-
-	uint32_t count = 0;
-	do {
-		ret = os_hid_get_feature_timeout(d->sensors_dev, &report,
-		                                 sizeof(report), 100);
-		if (ret < 0) {
-			VIVE_ERROR("Read error after %d bytes: %d", count, ret);
-			free(config_z);
-			return NULL;
-		}
-
-		if (report.len > 62) {
-			VIVE_ERROR("Invalid configuration data at %d", count);
-			free(config_z);
-			return NULL;
-		}
-
-		if (count + report.len > 4096) {
-			VIVE_ERROR("Configuration data too large");
-			free(config_z);
-			return NULL;
-		}
-
-		memcpy(config_z + count, report.payload, report.len);
-		count += report.len;
-	} while (report.len);
-
-	unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, 32768);
-
-	z_stream strm = {
-	    .next_in = config_z,
-	    .avail_in = count,
-	    .next_out = config_json,
-	    .avail_out = 32768,
-	    .zalloc = Z_NULL,
-	    .zfree = Z_NULL,
-	    .opaque = Z_NULL,
-	};
-
-	ret = inflateInit(&strm);
-	if (ret != Z_OK) {
-		VIVE_ERROR("inflate_init failed: %d", ret);
-		free(config_z);
-		free(config_json);
-		return NULL;
-	}
-
-	ret = inflate(&strm, Z_FINISH);
-	free(config_z);
-	if (ret != Z_STREAM_END) {
-		VIVE_ERROR("Failed to inflate configuration data: %d", ret);
-		free(config_json);
-		return NULL;
-	}
-
-	config_json[strm.total_out] = '\0';
-
-	U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1);
-	return (char *)config_json;
-}
-
 struct vive_device *
 vive_device_create(struct os_hid_device *mainboard_dev,
                    struct os_hid_device *sensors_dev,
@@ -1172,11 +1004,31 @@ vive_device_create(struct os_hid_device *mainboard_dev,
 		vive_mainboard_power_on(d);
 		vive_mainboard_get_device_info(d);
 	}
-	vive_sensors_read_firmware(d);
+	vive_read_firmware(d->sensors_dev, &d->firmware.firmware_version,
+	                   &d->firmware.hardware_revision,
+	                   &d->firmware.hardware_version_micro,
+	                   &d->firmware.hardware_version_minor,
+	                   &d->firmware.hardware_version_major);
 
-	vive_sensors_get_imu_range_report(d);
+	/*
+	VIVE_DEBUG(d, "Firmware version %u %s@%s FPGA %u.%u",
+	           d->firmware.firmware_version, report.string1, report.string2,
+	           report.fpga_version_major, report.fpga_version_minor);
+	*/
 
-	char *config = vive_sensors_read_config(d);
+	VIVE_DEBUG(d, "Firmware version %u", d->firmware.firmware_version);
+	VIVE_DEBUG(d, "Hardware revision: %d rev %d.%d.%d",
+	           d->firmware.hardware_revision,
+	           d->firmware.hardware_version_major,
+	           d->firmware.hardware_version_minor,
+	           d->firmware.hardware_version_micro);
+
+	vive_get_imu_range_report(d->sensors_dev, &d->imu.gyro_range,
+	                          &d->imu.acc_range);
+	VIVE_DEBUG(d, "Vive gyroscope range     %f", d->imu.gyro_range);
+	VIVE_DEBUG(d, "Vive accelerometer range %f", d->imu.acc_range);
+
+	char *config = vive_read_config(d->sensors_dev);
 	if (config != NULL) {
 		vive_parse_config(d, config);
 		free(config);

src/xrt/drivers/vive/vive_device.h

@@ -112,6 +112,9 @@ struct vive_device
 		uint32_t display_firmware_version;
 		uint32_t firmware_version;
 		uint8_t hardware_revision;
+		uint8_t hardware_version_micro;
+		uint8_t hardware_version_minor;
+		uint8_t hardware_version_major;
 		char *mb_serial_number;
 		char *model_number;
 		char *device_serial_number;

src/xrt/drivers/vive/vive_protocol.c

@@ -0,0 +1,231 @@
+// Copyright 2016-2019, Philipp Zabel
+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Vive USB HID reports
+ * @author Christoph Haag <christoph.haag@collabora.com>
+ * @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
+ * @ingroup drv_vive
+ */
+
+#include <stdio.h>
+#include <zlib.h>
+#include <math.h>
+#include "math/m_api.h"
+
+#include "vive_protocol.h"
+
+#include "util/u_debug.h"
+#include "util/u_misc.h"
+
+#define VIVE_ERROR(...)                                                        \
+	do {                                                                   \
+		fprintf(stderr, "%s - ", __func__);                            \
+		fprintf(stderr, __VA_ARGS__);                                  \
+		fprintf(stderr, "\n");                                         \
+	} while (false)
+
+const struct vive_headset_power_report power_on_report = {
+    .id = VIVE_HEADSET_POWER_REPORT_ID,
+    .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
+    .len = 56,
+    .unknown1 =
+        {
+            0x01,
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+            0x02,
+            0x00,
+            0x01,
+        },
+    .unknown2 = 0x7a,
+};
+
+const struct vive_headset_power_report power_off_report = {
+    .id = VIVE_HEADSET_POWER_REPORT_ID,
+    .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
+    .len = 56,
+    .unknown1 =
+        {
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+            0x02,
+            0x00,
+            0x00,
+        },
+    .unknown2 = 0x7c,
+};
+
+
+char *
+vive_read_config(struct os_hid_device *hid_dev)
+{
+	struct vive_config_start_report start_report = {
+	    .id = VIVE_CONFIG_START_REPORT_ID,
+	};
+
+	int ret = os_hid_get_feature_timeout(hid_dev, &start_report,
+	                                     sizeof(start_report), 100);
+	if (ret < 0) {
+		VIVE_ERROR("Could not get config start report.");
+		return NULL;
+	}
+
+	struct vive_config_read_report report = {
+	    .id = VIVE_CONFIG_READ_REPORT_ID,
+	};
+
+	unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, 4096);
+
+	uint32_t count = 0;
+	do {
+		ret = os_hid_get_feature_timeout(hid_dev, &report,
+		                                 sizeof(report), 100);
+		if (ret < 0) {
+			VIVE_ERROR("Read error after %d bytes: %d", count, ret);
+			free(config_z);
+			return NULL;
+		}
+
+		if (report.len > 62) {
+			VIVE_ERROR("Invalid configuration data at %d", count);
+			free(config_z);
+			return NULL;
+		}
+
+		if (count + report.len > 4096) {
+			VIVE_ERROR("Configuration data too large");
+			free(config_z);
+			return NULL;
+		}
+
+		memcpy(config_z + count, report.payload, report.len);
+		count += report.len;
+	} while (report.len);
+
+	unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, 32768);
+
+	z_stream strm = {
+	    .next_in = config_z,
+	    .avail_in = count,
+	    .next_out = config_json,
+	    .avail_out = 32768,
+	    .zalloc = Z_NULL,
+	    .zfree = Z_NULL,
+	    .opaque = Z_NULL,
+	};
+
+	ret = inflateInit(&strm);
+	if (ret != Z_OK) {
+		VIVE_ERROR("inflate_init failed: %d", ret);
+		free(config_z);
+		free(config_json);
+		return NULL;
+	}
+
+	ret = inflate(&strm, Z_FINISH);
+	free(config_z);
+	if (ret != Z_STREAM_END) {
+		VIVE_ERROR("Failed to inflate configuration data: %d", ret);
+		free(config_json);
+		return NULL;
+	}
+
+	config_json[strm.total_out] = '\0';
+
+	U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1);
+	return (char *)config_json;
+}
+
+int
+vive_get_imu_range_report(struct os_hid_device *hid_dev,
+                          double *gyro_range,
+                          double *acc_range)
+{
+	struct vive_imu_range_modes_report report = {
+	    .id = VIVE_IMU_RANGE_MODES_REPORT_ID};
+
+	int ret;
+
+	ret = os_hid_get_feature_timeout(hid_dev, &report, sizeof(report), 100);
+	if (ret < 0) {
+		printf("Could not get range report!\n");
+		return ret;
+	}
+
+	if (!report.gyro_range || !report.accel_range) {
+		VIVE_ERROR(
+		    "Invalid gyroscope and accelerometer data. Trying to fetch "
+		    "again.");
+		ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report,
+		                         sizeof(report));
+		if (ret < 0) {
+			VIVE_ERROR("Could not get feature report %d.",
+			           report.id);
+			return ret;
+		}
+
+		if (!report.gyro_range || !report.accel_range) {
+			VIVE_ERROR(
+			    "Unexpected range mode report: %02x %02x %02x",
+			    report.id, report.gyro_range, report.accel_range);
+			for (int i = 0; i < 61; i++)
+				printf(" %02x", report.unknown[i]);
+			printf("\n");
+			return -1;
+		}
+	}
+
+	if (report.gyro_range > 4 || report.accel_range > 4) {
+		VIVE_ERROR("Gyroscope or accelerometer range too large.");
+		VIVE_ERROR("Gyroscope: %d", report.gyro_range);
+		VIVE_ERROR("Accelerometer: %d", report.accel_range);
+		return -1;
+	}
+
+	/*
+	 * Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s,
+	 * +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range
+	 * (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s².
+	 */
+
+	*gyro_range = M_PI / 180.0 * (250 << report.gyro_range);
+	*acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range);
+
+	return 0;
+}
+
+int
+vive_read_firmware(struct os_hid_device *hid_dev,
+                   uint32_t *firmware_version,
+                   uint8_t *hardware_revision,
+                   uint8_t *hardware_version_micro,
+                   uint8_t *hardware_version_minor,
+                   uint8_t *hardware_version_major)
+{
+	struct vive_firmware_version_report report = {
+	    .id = VIVE_FIRMWARE_VERSION_REPORT_ID,
+	};
+
+	int ret;
+	ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report,
+	                         sizeof(report));
+	if (ret < 0)
+		return ret;
+
+	*firmware_version = __le32_to_cpu(report.firmware_version);
+	*hardware_revision = report.hardware_revision;
+	*hardware_version_major = report.hardware_version_major;
+	*hardware_version_minor = report.hardware_version_minor;
+	*hardware_version_micro = report.hardware_version_micro;
+
+	return 0;
+}

src/xrt/drivers/vive/vive_protocol.h

@@ -12,6 +12,7 @@
 
 #include <asm/byteorder.h>
 #include <stdint.h>
+#include "os/os_hid.h"
 
 #define VIVE_CONTROLLER_BUTTON_REPORT_ID 0x01
 
@@ -246,41 +247,21 @@ struct vive_controller_poweroff_report
 	uint8_t magic[4];
 } __attribute__((packed));
 
+const struct vive_headset_power_report power_on_report;
+const struct vive_headset_power_report power_off_report;
 
-const struct vive_headset_power_report power_on_report = {
+char *
-    .id = VIVE_HEADSET_POWER_REPORT_ID,
+vive_read_config(struct os_hid_device *hid_dev);
-    .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
-    .len = 56,
-    .unknown1 =
-        {
-            0x01,
-            0x00,
-            0x00,
-            0x00,
-            0x00,
-            0x00,
-            0x02,
-            0x00,
-            0x01,
-        },
-    .unknown2 = 0x7a,
-};
 
-const struct vive_headset_power_report power_off_report = {
+int
-    .id = VIVE_HEADSET_POWER_REPORT_ID,
+vive_get_imu_range_report(struct os_hid_device *hid_dev,
-    .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
+                          double *gyro_range,
-    .len = 56,
+                          double *acc_range);
-    .unknown1 =
+
-        {
+int
-            0x00,
+vive_read_firmware(struct os_hid_device *hid_dev,
-            0x00,
+                   uint32_t *firmware_version,
-            0x00,
+                   uint8_t *hardware_revision,
-            0x00,
+                   uint8_t *hardware_version_micro,
-            0x00,
+                   uint8_t *hardware_version_minor,
-            0x00,
+                   uint8_t *hardware_version_major);
-            0x02,
-            0x00,
-            0x00,
-        },
-    .unknown2 = 0x7c,
-};