d/vive: Add vive_source to convert v4l2 timestamps into monotonic clock

Use a sink in the middle of the stream to correct for v4l2 timestamps with
hardware timestamps to monotonic clock.

This sink, together with other utilities related to data streaming, lives in a
new vive_source entity, with similar functionality to wmr_source or rs_source.

The vive_source lifetime is managed by the builder xfctx, which prevents
deallocation dependencies between vive_device and the v4l2_fs to cause segfaults.

src/xrt/drivers/CMakeLists.txt

@@ -177,6 +177,8 @@ if(XRT_BUILD_DRIVER_VIVE)
 		vive/vive_controller.c
 		vive/vive_lighthouse.h
 		vive/vive_lighthouse.c
+		vive/vive_source.h
+		vive/vive_source.c
 		)
 	target_link_libraries(
 		drv_vive

src/xrt/drivers/vive/vive.h

@@ -13,6 +13,7 @@
 #include <stdint.h>
 #include <asm/byteorder.h>
 
+#include "util/u_debug.h"
 #include "util/u_time.h"
 
 /*
@@ -27,7 +28,11 @@
 #define VIVE_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__)
 #define VIVE_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__)
 
+DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN)
+
 #define VIVE_CLOCK_FREQ 48e6 // 48 MHz
+#define CAMERA_FREQUENCY 54
+#define IMU_FREQUENCY 1000
 
 /*!
  * Helper function to convert raw device ticks to nanosecond timestamps.
@@ -45,12 +50,10 @@ ticks_to_ns(uint32_t sample_ticks_raw, uint32_t *inout_prev_ticks, timepoint_ns
 	uint32_t sample_ticks = __le32_to_cpu(sample_ticks_raw);
 	uint32_t prev_ticks = *inout_prev_ticks;
 
-	uint32_t delta_ticks = 0;
-	if (prev_ticks < sample_ticks) {
-		delta_ticks = sample_ticks - prev_ticks;
-	} else { // Handle overflow
-		delta_ticks = (UINT32_MAX - prev_ticks) + sample_ticks;
-	}
+	// From the C standard https://www.open-std.org/jtc1/sc22/wg14/www/docs/n2912.pdf
+	// "A computation involving unsigned operands can never produce an overflow,
+	// because arithmetic for the unsigned type is performed modulo 2^N"
+	uint32_t delta_ticks = sample_ticks - prev_ticks;
 
 	const double one_tick_in_s = (1 / VIVE_CLOCK_FREQ);
 	const double one_tick_in_ns = one_tick_in_s * U_TIME_1S_IN_NS;

src/xrt/drivers/vive/vive_controller.c

@@ -53,8 +53,6 @@
  *
  */
 
-DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN)
-
 enum vive_controller_input_index
 {
 	// common inputs

src/xrt/drivers/vive/vive_device.c

@@ -28,10 +28,10 @@
 #include "vive.h"
 #include "vive_device.h"
 #include "vive_protocol.h"
+#include "vive_source.h"
+#include "xrt/xrt_tracking.h"
 
 
-DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN)
-
 static bool
 vive_mainboard_power_off(struct vive_device *d);
 
@@ -416,6 +416,8 @@ update_imu(struct vive_device *d, const void *buffer)
 		rel.pose.orientation = d->fusion.i3dof.rot;
 		m_relation_history_push(d->fusion.relation_hist, &rel, now_ns);
 		os_mutex_unlock(&d->fusion.mutex);
+
+		vive_source_push_imu_packet(d->source, d->imu.last_sample_ts_ns, acceleration, angular_velocity);
 	}
 }
 
@@ -537,13 +539,14 @@ _decode_pulse_report(struct vive_device *d, const void *buffer)
 			continue;
 		}
 
-		if (sensor_id == 0xfd) {
-			/* TODO: handle camera sync timestamp */
+		if (sensor_id == 0xfd) { // Camera frame timestamp
+			vive_source_push_frame_ticks(d->source, pulse->timestamp);
 			continue;
 		}
 
 		if (sensor_id == 0xfb) {
-			/* TODO: Only turns on when the camera is running but not every frame. */
+			/* TODO: Only turns on when the camera is running but not every frame. It
+			 * seems to come with every 16h frame on an Index (~3.37hz) */
 			continue;
 		}
 
@@ -868,7 +871,7 @@ vive_setup_trackers(struct vive_device *d, struct vive_tracking_status status)
 	}
 
 	// SLAM tracker and hand tracker, if enabled, should've been initialized in
-	// the lighthouse builder.
+	// the lighthouse builder. The out_sinks fields will be set there as well.
 
 	return true;
 }
@@ -878,7 +881,8 @@ vive_device_create(struct os_hid_device *mainboard_dev,
                    struct os_hid_device *sensors_dev,
                    struct os_hid_device *watchman_dev,
                    enum VIVE_VARIANT variant,
-                   struct vive_tracking_status tstatus)
+                   struct vive_tracking_status tstatus,
+                   struct vive_source *vs)
 {
 	XRT_TRACE_MARKER();
 
@@ -1000,6 +1004,7 @@ vive_device_create(struct os_hid_device *mainboard_dev,
 	os_thread_helper_init(&d->sensors_thread);
 	os_thread_helper_init(&d->watchman_thread);
 
+	d->source = vs;
 	d->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
 	d->offset = (struct xrt_pose)XRT_POSE_IDENTITY;
 
@@ -1024,8 +1029,6 @@ vive_device_create(struct os_hid_device *mainboard_dev,
 		}
 	}
 
-	vive_setup_trackers(d, tstatus);
-
 	switch (d->config.variant) {
 	case VIVE_VARIANT_VIVE: snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive (vive)"); break;
 	case VIVE_VARIANT_PRO: snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive Pro (vive)"); break;
@@ -1034,6 +1037,13 @@ vive_device_create(struct os_hid_device *mainboard_dev,
 	}
 	snprintf(d->base.serial, XRT_DEVICE_NAME_LEN, "%s", d->config.firmware.device_serial_number);
 
+	bool trackers_set = vive_setup_trackers(d, tstatus);
+	if (!trackers_set) {
+		VIVE_ERROR(d, "Failed to setup trackers");
+		vive_device_destroy((struct xrt_device *)d);
+		return NULL;
+	}
+
 	ret = os_thread_helper_start(&d->sensors_thread, vive_sensors_run_thread, d);
 	if (ret != 0) {
 		VIVE_ERROR(d, "Failed to start sensors thread!");

src/xrt/drivers/vive/vive_device.h

@@ -100,6 +100,9 @@ struct vive_device
 	//! Whether to track the HMD with 6dof SLAM or fallback to the 3dof tracker
 	bool slam_over_3dof;
 
+	//! In charge of managing raw samples, redirects them for tracking
+	struct vive_source *source;
+
 	//! Last tracked pose
 	struct xrt_pose pose;
 
@@ -129,8 +132,8 @@ vive_device_create(struct os_hid_device *mainboard_dev,
                    struct os_hid_device *sensors_dev,
                    struct os_hid_device *watchman_dev,
                    enum VIVE_VARIANT variant,
-                   struct vive_tracking_status tstatus);
-
+                   struct vive_tracking_status tstatus,
+                   struct vive_source *vs);
 
 #ifdef __cplusplus
 }

src/xrt/drivers/vive/vive_prober.c

@@ -66,6 +66,7 @@ init_vive1(struct xrt_prober *xp,
            size_t device_count,
            enum u_logging_level log_level,
            struct vive_tracking_status tstatus,
+           struct vive_source *vs,
            struct xrt_device **out_xdev)
 {
 	log_vive_device(log_level, xp, dev);
@@ -120,7 +121,7 @@ init_vive1(struct xrt_prober *xp,
 		return 0;
 	}
 	struct vive_device *d =
-	    vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_VIVE, tstatus);
+	    vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_VIVE, tstatus, vs);
 	if (d == NULL) {
 		free(sensors_dev);
 		free(mainboard_dev);
@@ -139,6 +140,7 @@ init_vive_pro(struct xrt_prober *xp,
               size_t device_count,
               enum u_logging_level log_level,
               struct vive_tracking_status tstatus,
+              struct vive_source *vs,
               struct xrt_device **out_xdev)
 {
 	XRT_TRACE_MARKER();
@@ -195,7 +197,8 @@ init_vive_pro(struct xrt_prober *xp,
 		free(sensors_dev);
 		return 0;
 	}
-	struct vive_device *d = vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_PRO, tstatus);
+	struct vive_device *d =
+	    vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_PRO, tstatus, vs);
 	if (d == NULL) {
 		free(sensors_dev);
 		free(mainboard_dev);
@@ -214,6 +217,7 @@ init_valve_index(struct xrt_prober *xp,
                  size_t device_count,
                  enum u_logging_level log_level,
                  struct vive_tracking_status tstatus,
+                 struct vive_source *vs,
                  struct vive_config **out_vive_config,
                  struct xrt_device **out_xdevs)
 {
@@ -252,7 +256,7 @@ init_valve_index(struct xrt_prober *xp,
 		return 0;
 	}
 
-	struct vive_device *d = vive_device_create(NULL, sensors_dev, watchman_dev, VIVE_VARIANT_INDEX, tstatus);
+	struct vive_device *d = vive_device_create(NULL, sensors_dev, watchman_dev, VIVE_VARIANT_INDEX, tstatus, vs);
 	if (d == NULL) {
 		return 0;
 	}
@@ -272,6 +276,7 @@ vive_found(struct xrt_prober *xp,
            size_t index,
            cJSON *attached_data,
            struct vive_tracking_status tstatus,
+           struct vive_source *vs,
            struct vive_config **out_vive_config,
            struct xrt_device **out_xdev)
 {
@@ -289,10 +294,12 @@ vive_found(struct xrt_prober *xp,
 	}
 
 	switch (dev->product_id) {
-	case VIVE_PID: return init_vive1(xp, dev, devices, device_count, log_level, tstatus, out_xdev);
-	case VIVE_PRO_MAINBOARD_PID: return init_vive_pro(xp, dev, devices, device_count, log_level, tstatus, out_xdev);
+	case VIVE_PID: return init_vive1(xp, dev, devices, device_count, log_level, tstatus, vs, out_xdev);
+	case VIVE_PRO_MAINBOARD_PID:
+		return init_vive_pro(xp, dev, devices, device_count, log_level, tstatus, vs, out_xdev);
 	case VIVE_PRO_LHR_PID:
-		return init_valve_index(xp, dev, devices, device_count, log_level, tstatus, out_vive_config, out_xdev);
+		return init_valve_index(xp, dev, devices, device_count, log_level, tstatus, vs, out_vive_config,
+		                        out_xdev);
 	default: U_LOG_E("No product ids matched %.4x", dev->product_id); return 0;
 	}
 

src/xrt/drivers/vive/vive_prober.h

@@ -54,6 +54,7 @@ vive_found(struct xrt_prober *xp,
            size_t index,
            cJSON *attached_data,
            struct vive_tracking_status tstatus,
+           struct vive_source *vs,
            struct vive_config **out_vive_config,
            struct xrt_device **out_xdev);
 

src/xrt/drivers/vive/vive_source.c

@@ -0,0 +1,240 @@
+// Copyright 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Interface for vive data sources
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
+ * @ingroup drv_vive
+ */
+
+#include "os/os_threading.h"
+#include "util/u_deque.h"
+#include "util/u_logging.h"
+#include "xrt/xrt_frame.h"
+#include "xrt/xrt_tracking.h"
+
+#include "vive.h"
+
+/*!
+ * Manages the data streaming state related to a vive headset.
+ *
+ * @implements xrt_frame_node
+ */
+struct vive_source
+{
+	struct xrt_frame_node node;
+	enum u_logging_level log_level;
+
+	// Sinks
+	struct xrt_frame_sink sbs_sink;  //!< Intermediate sink for SBS frames
+	struct xrt_imu_sink imu_sink;    //!< Intermediate sink for IMU samples
+	struct xrt_slam_sinks in_sinks;  //!< Pointers to intermediate sinks
+	struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks
+
+	// V4L2 frame streaming state
+	bool timestamps_have_been_zero_until_now; //!< First v4l2 frames are zeroed
+	bool waiting_for_first_nonempty_frame;    //!< Whether the first good frame has been received
+
+	// Frame timestamps
+	struct u_deque_timepoint_ns frame_timestamps; //! Queue of yet unused frame hw timestamps
+	struct os_mutex frame_timestamps_lock;        //! Lock for accessing frame_timestamps
+	uint32_t last_frame_ticks;                    //! Last frame timestamp in device ticks
+	timepoint_ns last_frame_ts_ns;                //! Last frame timestamp in device nanoseconds
+
+	// Clock offsets
+	time_duration_ns hw2mono; //!< Estimated offset from IMU to monotonic clock
+	time_duration_ns hw2v4l2; //!< Estimated offset from IMU to V4L2 clock
+};
+
+//! Given a sample from two timestamp domains a and b that should have been
+//! sampled as close as possible, together with an estimate of the offset
+//! between a clock and b clock (or zero), it applies a smoothing average on the
+//! estimated offset and returns a in b clock.
+//! @todo Copy of clock_hw2mono in wmr_source.c, unify into a utility.
+static inline timepoint_ns
+clock_offset_a2b(double freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
+{
+	// Totally arbitrary way of computing alpha, if you have a better one, replace it
+	const double alpha = 1.0 - 12.5 / freq; // Weight to put on accumulated a2b
+	time_duration_ns old_a2b = *inout_a2b;
+	time_duration_ns got_a2b = b - a;
+	time_duration_ns new_a2b = old_a2b * alpha + got_a2b * (1.0 - alpha);
+	if (old_a2b == 0) { // a2b has not been set yet
+		new_a2b = got_a2b;
+	}
+	*inout_a2b = new_a2b;
+	return a + new_a2b;
+}
+
+/*
+ *
+ * Vive source methods
+ *
+ */
+
+//! Find the best corresponding hw timestamp from this v4l2 frame, return
+//! whether it was found.
+bool
+vive_source_try_convert_v4l2_timestamp(struct vive_source *vs, struct xrt_frame *xf)
+{
+	assert(xf->timestamp != 0 || vs->timestamps_have_been_zero_until_now);
+	if (xf->timestamp == 0) {
+		return false;
+	}
+	vs->timestamps_have_been_zero_until_now = false;
+
+	struct u_deque_timepoint_ns vive_timestamps = vs->frame_timestamps;
+	struct os_mutex *vive_timestamps_lock = &vs->frame_timestamps_lock;
+
+	timepoint_ns v4l2_ts = xf->timestamp;
+
+	size_t vive_ts_count = u_deque_timepoint_ns_size(vive_timestamps);
+	if (vive_ts_count == 0) { // This seems to happen in some runs
+		// This code assumes vive_timestamps will always be populated before v4l2
+		// receives a frame, thus if we reach this, this assumption has failed.
+		VIVE_ERROR(vs, "Received a v4l2 frame but thwere are no vive timestamps to use");
+		VIVE_ERROR(vs, "Will continue, but frame timestamps could be off by one");
+		return false;
+	}
+
+	os_mutex_lock(vive_timestamps_lock);
+
+	// Find i in vive_timestamps that would be closer to xf->timestamp in v4l2 clock
+	int closer_i = -1;
+	timepoint_ns vive_ts = -1;
+	time_duration_ns min_distance = INT64_MAX;
+	for (size_t i = 0; i < vive_ts_count; i++) {
+		vive_ts = u_deque_timepoint_ns_at(vive_timestamps, i);
+		timepoint_ns v4l2_ts_est = vive_ts + vs->hw2v4l2;
+		time_duration_ns distance = llabs(v4l2_ts_est - v4l2_ts);
+		if (distance < min_distance) {
+			closer_i = i;
+			min_distance = distance;
+		}
+	}
+
+	// Discard missed frames and set vive_timestamp to use in this frame
+	timepoint_ns vive_timestamp = 0;
+	for (; closer_i >= 0; closer_i--) {
+		u_deque_timepoint_ns_pop_front(vive_timestamps, &vive_timestamp);
+	}
+
+	os_mutex_unlock(vive_timestamps_lock);
+
+	// Our estimate is within a reasonable time distance
+	assert(min_distance < U_TIME_1S_IN_NS / CAMERA_FREQUENCY || vs->waiting_for_first_nonempty_frame);
+	vs->waiting_for_first_nonempty_frame = false;
+
+	// Update estimate of hw2v4l2 clock offset, only used for matching timestamps
+	clock_offset_a2b(CAMERA_FREQUENCY, vive_timestamp, xf->timestamp, &vs->hw2v4l2);
+
+	// Use vive_timestamp and put it in monotonic clock
+	xf->timestamp = vive_timestamp + vs->hw2mono; // Notice that we don't use hw2v4l2
+
+	return true;
+}
+
+static void
+vive_source_receive_sbs_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
+{
+	struct vive_source *vs = container_of(sink, struct vive_source, sbs_sink);
+	bool should_push = vive_source_try_convert_v4l2_timestamp(vs, xf);
+
+	if (!should_push) {
+		VIVE_TRACE(vs, "skipped sbs img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
+		return;
+	}
+
+	VIVE_TRACE(vs, "sbs img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
+
+	if (vs->out_sinks.left) { // The split into left right will happen downstream
+		xrt_sink_push_frame(vs->out_sinks.left, xf);
+	}
+}
+
+static void
+vive_source_receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
+{
+	struct vive_source *vs = container_of(sink, struct vive_source, imu_sink);
+	s->timestamp_ns = clock_offset_a2b(IMU_FREQUENCY, s->timestamp_ns, os_monotonic_get_ns(), &vs->hw2mono);
+	timepoint_ns ts = s->timestamp_ns;
+	struct xrt_vec3_f64 a = s->accel_m_s2;
+	struct xrt_vec3_f64 w = s->gyro_rad_secs;
+	VIVE_TRACE(vs, "imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
+
+	if (vs->out_sinks.imu) {
+		xrt_sink_push_imu(vs->out_sinks.imu, s);
+	}
+}
+
+static void
+vive_source_node_break_apart(struct xrt_frame_node *node)
+{}
+
+static void
+vive_source_node_destroy(struct xrt_frame_node *node)
+{
+	struct vive_source *vs = container_of(node, struct vive_source, node);
+	os_mutex_destroy(&vs->frame_timestamps_lock);
+	u_deque_timepoint_ns_destroy(&vs->frame_timestamps);
+}
+
+
+/*!
+ *
+ * Exported functions
+ *
+ */
+
+struct vive_source *
+vive_source_create(struct xrt_frame_context *xfctx)
+{
+	struct vive_source *vs = U_TYPED_CALLOC(struct vive_source);
+	vs->log_level = debug_get_log_option_vive_log();
+
+	// Setup sinks
+	vs->sbs_sink.push_frame = vive_source_receive_sbs_frame;
+	vs->imu_sink.push_imu = vive_source_receive_imu_sample;
+	vs->in_sinks.left = &vs->sbs_sink;
+	vs->in_sinks.right = NULL;
+	vs->in_sinks.imu = &vs->imu_sink;
+
+	vs->timestamps_have_been_zero_until_now = true;
+	vs->waiting_for_first_nonempty_frame = true;
+
+	vs->frame_timestamps = u_deque_timepoint_ns_create();
+	os_mutex_init(&vs->frame_timestamps_lock);
+
+	// Setup node
+	struct xrt_frame_node *xfn = &vs->node;
+	xfn->break_apart = vive_source_node_break_apart;
+	xfn->destroy = vive_source_node_destroy;
+	xrt_frame_context_add(xfctx, &vs->node);
+
+	VIVE_DEBUG(vs, "Vive source created");
+
+	return vs;
+}
+
+void
+vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g)
+{
+	struct xrt_vec3_f64 a64 = {a.x, a.y, a.z};
+	struct xrt_vec3_f64 g64 = {g.x, g.y, g.z};
+	struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = a64, .gyro_rad_secs = g64};
+	xrt_sink_push_imu(&vs->imu_sink, &sample);
+}
+
+void
+vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks)
+{
+	ticks_to_ns(ticks, &vs->last_frame_ticks, &vs->last_frame_ts_ns);
+	u_deque_timepoint_ns_push_back(vs->frame_timestamps, vs->last_frame_ts_ns);
+}
+
+void
+vive_source_hook_into_sinks(struct vive_source *vs, struct xrt_slam_sinks *sinks)
+{
+	vs->out_sinks = *sinks;
+	sinks->left = vs->in_sinks.left;
+}

src/xrt/drivers/vive/vive_source.h

@@ -0,0 +1,45 @@
+// Copyright 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Interface for vive data sources
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
+ * @ingroup drv_vive
+ */
+
+#pragma once
+
+#include "xrt/xrt_frame.h"
+#include "xrt/xrt_tracking.h"
+
+/*!
+ * Vive data sources
+ *
+ * @addtogroup drv_vive
+ * @{
+ */
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct vive_source *
+vive_source_create(struct xrt_frame_context *xfctx);
+
+void
+vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g);
+
+void
+vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks);
+
+void
+vive_source_hook_into_sinks(struct vive_source *vs, struct xrt_slam_sinks *sinks);
+
+/*!
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/targets/common/target_builder_lighthouse.c

@@ -36,6 +36,7 @@
 #ifdef XRT_BUILD_DRIVER_VIVE
 #include "vive/vive_prober.h"
 #include "vive/vive_device.h"
+#include "vive/vive_source.h"
 #endif
 
 #ifdef XRT_BUILD_DRIVER_SURVIVE
@@ -94,7 +95,6 @@ struct index_camera_finder
 {
 	struct xrt_fs *xfs;
 	struct xrt_frame_context *xfctx;
-	bool found;
 };
 
 /*
@@ -124,19 +124,6 @@ get_selected_mode(struct xrt_fs *xfs)
 	return selected_mode;
 }
 
-static bool
-visual_inertial_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
-{
-	// Stream frames
-	struct xrt_frame_sink *sbs_sink = sinks->left;
-	bool success = xrt_fs_stream_start(xfs, sbs_sink, XRT_FS_CAPTURE_TYPE_TRACKING, get_selected_mode(xfs));
-	if (!success) {
-		return false;
-	}
-
-	return true;
-}
-
 static void
 on_video_device(struct xrt_prober *xp,
                 struct xrt_prober_device *pdev,
@@ -462,13 +449,18 @@ stream_data_sources(struct u_system_devices *usysd, struct xrt_prober *xp, struc
 	}
 
 	bool success = false;
+	uint32_t mode = get_selected_mode(finder.xfs);
+
+	// If SLAM is enabled (only on vive driver) we intercept the data sink
 	if (lhs.slam_enabled) {
-		LH_ASSERT(false, "Not implemented");
-	} else {
-		uint32_t mode = get_selected_mode(finder.xfs);
-		success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode);
+		struct vive_device *d = (struct vive_device *)usysd->base.roles.head;
+		LH_ASSERT_(d != NULL && d->source != NULL);
+		struct vive_source *vs = d->source;
+		vive_source_hook_into_sinks(vs, &sinks);
 	}
 
+	success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode);
+
 	if (!success) {
 		LH_ERROR("Unable to start data streaming");
 		xrt_frame_context_destroy_nodes(&usysd->xfctx);
@@ -551,7 +543,8 @@ lighthouse_open_system(struct xrt_builder *xb,
 			case VIVE_PID:
 			case VIVE_PRO_MAINBOARD_PID:
 			case VIVE_PRO_LHR_PID: {
-				int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, tstatus, &hmd_config,
+				struct vive_source *vs = vive_source_create(&usysd->xfctx);
+				int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, tstatus, vs, &hmd_config,
 				                             &usysd->base.xdevs[usysd->base.xdev_count]);
 				usysd->base.xdev_count += num_devices;