d/psvr: Switch to reading sensors from its own thread

Tested-by: number-g <g@imagination.eu.org>

src/xrt/drivers/psvr/psvr_device.c

@@ -1,5 +1,5 @@
 // Copyright 2016, Joey Ferwerda.
-// Copyright 2019-2021, Collabora, Ltd.
+// Copyright 2019-2022, Collabora, Ltd.
 // SPDX-License-Identifier: BSL-1.0
 /*!
  * @file
@@ -23,6 +23,7 @@
 #include "util/u_time.h"
 #include "util/u_debug.h"
 #include "util/u_device.h"
+#include "util/u_trace_marker.h"
 #include "util/u_distortion_mesh.h"
 
 #include "math/m_imu_3dof.h"
@@ -58,12 +59,19 @@ struct psvr_device
 {
 	struct xrt_device base;
 
+	//! Owned by the @ref oth thread.
 	hid_device *hid_sensor;
+
+	//! Owned and protected by the device_mutex.
 	hid_device *hid_control;
 	struct os_mutex device_mutex;
 
+	//! Used to read sensor packets.
+	struct os_thread_helper oth;
+
 	struct xrt_tracked_psvr *tracker;
 
+	//! Only touched from the sensor thread.
 	timepoint_ns last_sensor_time;
 
 	struct psvr_parsed_sensor last;
@@ -230,19 +238,19 @@ send_request_data(struct psvr_device *psvr, uint8_t id, uint8_t num)
 	return send_to_control(psvr, data, sizeof(data));
 }
 
-
-/*
- *
- * Packet reading code.
- *
- */
-
 static uint8_t
 scale_led_power(uint8_t power)
 {
 	return (uint8_t)((power / 255.0f) * 100.0f);
 }
 
+
+/*
+ *
+ * Sensor functions.
+ *
+ */
+
 static void
 read_sample_and_apply_calibration(struct psvr_device *psvr,
                                   struct psvr_parsed_sample *sample,
@@ -296,7 +304,7 @@ read_sample_and_apply_calibration(struct psvr_device *psvr,
 }
 
 static void
-update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
+update_fusion_locked(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
 {
 	struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
 	(void)mag;
@@ -314,6 +322,14 @@ update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint6
 	}
 }
 
+static void
+update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
+{
+	os_mutex_lock(&psvr->device_mutex);
+	update_fusion_locked(psvr, sample, timestamp_ns);
+	os_mutex_unlock(&psvr->device_mutex);
+}
+
 static uint32_t
 calc_delta_and_handle_rollover(uint32_t next, uint32_t last)
 {
@@ -400,6 +416,65 @@ handle_tracker_sensor_msg(struct psvr_device *psvr, unsigned char *buffer, int s
 	update_fusion(psvr, &s->samples[1], timestamp_ns);
 }
 
+static void
+sensor_clear_queue(struct psvr_device *psvr)
+{
+	uint8_t buffer[FEATURE_BUFFER_SIZE];
+
+	while (hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE) > 0) {
+		// Just drop the packets.
+	}
+}
+
+static int
+sensor_read_one_packet(struct psvr_device *psvr)
+{
+	uint8_t buffer[FEATURE_BUFFER_SIZE];
+
+	// Try for one second to get packates.
+	int size = hid_read_timeout(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE, 1000);
+	if (size <= 0) {
+		return size;
+	}
+
+	handle_tracker_sensor_msg(psvr, buffer, size);
+
+	return 0;
+}
+
+static void *
+sensor_thread(void *ptr)
+{
+	U_TRACE_SET_THREAD_NAME("PS VR");
+
+	struct psvr_device *psvr = (struct psvr_device *)ptr;
+	int ret = 0;
+
+	// Empty the queue.
+	sensor_clear_queue(psvr);
+
+	os_thread_helper_lock(&psvr->oth);
+
+	while (os_thread_helper_is_running_locked(&psvr->oth) && ret >= 0) {
+		os_thread_helper_unlock(&psvr->oth);
+
+		ret = sensor_read_one_packet(psvr);
+
+		os_thread_helper_lock(&psvr->oth);
+	}
+
+	os_thread_helper_unlock(&psvr->oth);
+
+	return NULL;
+}
+
+
+/*
+ *
+ * Control device handling.
+ *
+ */
+
 static void
 handle_control_status_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
 {
@@ -506,25 +581,6 @@ handle_control_0xA0(struct psvr_device *psvr, unsigned char *buffer, int size)
 	PSVR_DEBUG(psvr, "%02x %02x %02x %02x", buffer[0], buffer[1], buffer[2], buffer[3]);
 }
 
-static int
-read_sensor_packets(struct psvr_device *psvr)
-{
-	uint8_t buffer[FEATURE_BUFFER_SIZE];
-	int size = 0;
-
-	do {
-		size = hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE);
-		if (size == 0) {
-			return 0;
-		}
-		if (size < 0) {
-			return -1;
-		}
-
-		handle_tracker_sensor_msg(psvr, buffer, size);
-	} while (true);
-}
-
 static int
 read_control_packets(struct psvr_device *psvr)
 {
@@ -647,7 +703,6 @@ static int
 wait_for_power(struct psvr_device *psvr, bool on)
 {
 	for (int i = 0; i < 5000; i++) {
-		read_sensor_packets(psvr);
 		read_control_packets(psvr);
 
 		if (psvr->powered_on == on) {
@@ -664,7 +719,6 @@ static int
 wait_for_vr_mode(struct psvr_device *psvr, bool on)
 {
 	for (int i = 0; i < 5000; i++) {
-		read_sensor_packets(psvr);
 		read_control_packets(psvr);
 
 		if (psvr->in_vr_mode == on) {
@@ -690,6 +744,7 @@ control_power_and_wait(struct psvr_device *psvr, bool on)
 		PSVR_ERROR(psvr, "Failed to switch %s the headset! '%i'", status, ret);
 	}
 
+
 	ret = wait_for_power(psvr, on);
 	if (ret < 0) {
 		PSVR_ERROR(psvr, "Failed to wait for headset power %s! '%i'", status, ret);
@@ -828,11 +883,6 @@ disco_leds(struct psvr_device *psvr)
 
 		// Sleep for a tenth of a second while polling for packages.
 		for (int k = 0; k < 100; k++) {
-			ret = read_sensor_packets(psvr);
-			if (ret < 0) {
-				return ret;
-			}
-
 			ret = read_control_packets(psvr);
 			if (ret < 0) {
 				return ret;
@@ -845,12 +895,22 @@ disco_leds(struct psvr_device *psvr)
 	return 0;
 }
 
+
+/*
+ *
+ * Misc functions.
+ *
+ */
+
 static void
 teardown(struct psvr_device *psvr)
 {
 	// Stop the variable tracking.
 	u_var_remove_root(psvr);
 
+	// Shutdown the sensor thread early.
+	os_thread_helper_stop_and_wait(&psvr->oth);
+
 	// Includes null check, and sets to null.
 	xrt_tracked_psvr_destroy(&psvr->tracker);
 
@@ -872,6 +932,7 @@ teardown(struct psvr_device *psvr)
 	// Destroy the fusion.
 	m_imu_3dof_close(&psvr->fusion);
 
+	os_thread_helper_destroy(&psvr->oth);
 	os_mutex_destroy(&psvr->device_mutex);
 }
 
@@ -889,7 +950,6 @@ psvr_device_update_inputs(struct xrt_device *xdev)
 
 	os_mutex_lock(&psvr->device_mutex);
 
-	read_sensor_packets(psvr);
 	update_leds_if_changed(psvr);
 
 	os_mutex_unlock(&psvr->device_mutex);
@@ -911,7 +971,6 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev,
 	os_mutex_lock(&psvr->device_mutex);
 
 	// Read all packets.
-	read_sensor_packets(psvr);
 	read_control_packets(psvr);
 
 	// Clear out the relation.
@@ -1024,8 +1083,9 @@ psvr_device_create_auto_prober(struct hid_device_info *sensor_hid_info,
 	snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");
 	snprintf(psvr->base.serial, XRT_DEVICE_NAME_LEN, "PS VR Headset");
 
-	// Do mutex init before any call to teardown happens.
+	// Do mutex and thread init before any call to teardown happens.
 	os_mutex_init(&psvr->device_mutex);
+	os_thread_helper_init(&psvr->oth);
 
 	ret = open_hid(psvr, sensor_hid_info, &psvr->hid_sensor);
 	if (ret != 0) {
@@ -1037,6 +1097,11 @@ psvr_device_create_auto_prober(struct hid_device_info *sensor_hid_info,
 		goto cleanup;
 	}
 
+	ret = os_thread_helper_start(&psvr->oth, sensor_thread, (void *)psvr);
+	if (ret < 0) {
+		goto cleanup;
+	}
+
 	if (control_power_and_wait(psvr, true) < 0 || control_vrmode_and_wait(psvr, true) < 0) {
 		goto cleanup;
 	}