The open source OpenXR runtime
d/wmr: Recover from bad camera transfers
Fix USB overflow problems when camera readout is
delayed by rounding up the transfer size to always
be a multiple of the endpoint transfer size, and
by discarding invalid frames that happen when
the headset ends up with too much data queued.
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2394>
authored by
Jan Schmidt
and committed by
Marge Bot
d93e4b92
4a905bbb
+31
-6
+31
-6
src/xrt/drivers/wmr/wmr_camera.c
+31
-6
src/xrt/drivers/wmr/wmr_camera.c
···
99
99
int slam_cam_count; //!< Number of tracking cameras used for SLAM
100
100
101
101
size_t xfer_size;
102
+
size_t frame_xfer_size;
102
103
uint32_t frame_width, frame_height;
103
104
uint8_t last_seq;
104
105
uint64_t last_frame_ts;
···
172
173
173
174
F = 26;
174
175
176
+
libusb_device *dev = libusb_get_device(cam->dev);
177
+
int ep_pkt_size = libusb_get_max_packet_size(dev, CAM_ENDPOINT);
178
+
if (ep_pkt_size < 0) {
179
+
WMR_CAM_ERROR(cam, "Failed to retrieve endpoint descriptor: result %d", ep_pkt_size);
180
+
return false;
181
+
}
182
+
175
183
for (i = 0; i < cam->tcam_count; i++) {
176
184
const struct wmr_camera_config *config = &cam->tcam_confs[i];
177
185
···
204
212
n_packets = F / (0x6000 - 32);
205
213
leftover = F - n_packets * (0x6000 - 32);
206
214
207
-
cam->xfer_size = n_packets * 0x6000 + 32 + leftover;
215
+
cam->frame_xfer_size = n_packets * 0x6000 + 32 + leftover;
216
+
217
+
// Round up to a multiple of the max packet size to avoid overflows
218
+
// in case of stalls reading things out
219
+
size_t round_up = ep_pkt_size - (cam->frame_xfer_size % ep_pkt_size);
220
+
cam->xfer_size = cam->frame_xfer_size + round_up;
221
+
WMR_CAM_DEBUG(cam, "Rounding up xfer size by %zu from frame size %zu to %zu", round_up, cam->frame_xfer_size,
222
+
cam->xfer_size);
208
223
209
224
cam->frame_width = width;
210
225
cam->frame_height = height;
···
288
303
goto out;
289
304
}
290
305
291
-
if (xfer->actual_length < xfer->length) {
292
-
WMR_CAM_DEBUG(cam, "Camera transfer only delivered %d bytes", xfer->actual_length);
306
+
if ((size_t)xfer->actual_length < cam->frame_xfer_size) {
307
+
WMR_CAM_DEBUG(cam, "Camera transfer only delivered %d bytes of %zu per frame", xfer->actual_length,
308
+
cam->frame_xfer_size);
293
309
goto out;
294
310
}
295
311
···
308
324
const size_t chunk_size = 0x6000 - 32;
309
325
310
326
DRV_TRACE_BEGIN(copy_to_frame);
311
-
while (dst_remain > 0) {
327
+
while (dst_remain >= 0x20) {
312
328
const size_t to_copy = dst_remain > chunk_size ? chunk_size : dst_remain;
313
329
314
330
/* 32 byte header seems to contain:
···
319
335
* but repeat every 8 slices. They're different each boot
320
336
* of the headset. Might just be uninitialised memory?
321
337
*/
338
+
uint32_t magic = __le32_to_cpu(*(__le32 *)(src));
339
+
if (magic != WMR_MAGIC) {
340
+
WMR_CAM_WARN(cam, "Invalid frame magic (got %x, expected %x). Dropping", magic, WMR_MAGIC);
341
+
goto drop_frame;
342
+
}
322
343
src += 0x20;
323
344
324
345
memcpy(dst, src, to_copy);
···
328
349
}
329
350
DRV_TRACE_END(copy_to_frame);
330
351
331
-
/* There should be exactly a 26 byte footer left over */
332
-
assert(xfer->buffer + xfer->length - src == 26);
352
+
/* There should be exactly a 26 byte footer left over if we completely consumed the right amount of data */
353
+
if (xfer->buffer + cam->frame_xfer_size - src != 26) {
354
+
WMR_CAM_WARN(cam, "Invalid frame. Dropping");
355
+
goto drop_frame;
356
+
}
333
357
334
358
/* Footer contains:
335
359
* __le64 start_ts; - 100ns unit timestamp, from same clock as video_timestamps on the IMU feed
···
401
425
}
402
426
}
403
427
428
+
drop_frame:
404
429
xrt_frame_reference(&xf, NULL);
405
430
406
431
out: