Glasses: count frames dropped for a bad checksum

app/src/main/java/com/droiddeck/launcher/glasses/Glasses.kt

@@ -284,6 +284,7 @@ object Glasses {
             .put("brightness", s.brightness).put("volume", s.volume)
             .put("brightnessMax", s.brightnessMax).put("volumeMax", s.volumeMax).put("sunblock", s.sunblock)
             .put("tracking", s.tracking).put("simulating", simulating.get() != null).put("imuPackets", imuPackets)
+            .put("badFrames", link?.badFrames ?: 0L)
             .put("pose", q?.let { JSONArray().put(it[0].toDouble()).put(it[1].toDouble()).put(it[2].toDouble()).put(it[3].toDouble()) } ?: JSONObject.NULL)
             .put("fovDeg", s.diagonalFovDeg.toDouble()).put("noSignal", s.noSignal).put("sbsSupport", s.sbsSupport)
             .put("settings", JSONObject().put("refreshHz", g.refreshHz).put("sbsHz", g.sbsHz).put("sunblock", g.sunblock ?: JSONObject.NULL)

app/src/main/java/com/droiddeck/launcher/glasses/VitureLink.kt

@@ -8,6 +8,7 @@ import android.hardware.usb.UsbInterface
 import android.hardware.usb.UsbManager
 import android.os.SystemClock
 import android.util.Log
+import java.util.concurrent.atomic.AtomicLong
 
 internal class VitureLink private constructor(
     private val connection: UsbDeviceConnection,
@@ -28,20 +29,29 @@ internal class VitureLink private constructor(
     private val writeLock = Any()
     private var lastSendAt = 0L
     private val readers = mutableListOf<Thread>()
+    private val bad = AtomicLong()
 
     val hasImu: Boolean get() = imuIn != null
+    val badFrames: Long get() = bad.get()
 
     private fun startReaders() {
-        mcuIn?.let { ep -> readers += reader("viture-mcu", ep) { buf, n -> VitureProtocol.parse(buf, n)?.takeIf { it.crcOk }?.let(listener::onFrame) } }
+        mcuIn?.let { ep -> readers += reader("viture-mcu", ep) { buf, n -> validFrame(buf, n)?.let(listener::onFrame) } }
         imuIn?.let { ep ->
             readers += reader("viture-imu", ep) { buf, n ->
-                val frame = VitureProtocol.parse(buf, n)?.takeIf { it.crcOk } ?: return@reader
+                val frame = validFrame(buf, n) ?: return@reader
                 if (frame.magic == VitureProtocol.MAGIC_IMU) listener.onImu(frame.payload, SystemClock.elapsedRealtimeNanos())
                 else listener.onFrame(frame)
             }
         }
     }
 
+    private fun validFrame(buf: ByteArray, n: Int): VitureProtocol.Frame? {
+        val frame = VitureProtocol.parse(buf, n) ?: return null
+        if (frame.crcOk) return frame
+        if (bad.incrementAndGet() == 1L) Log.w(TAG, "dropping frames with a bad checksum")
+        return null
+    }
+
     private fun reader(name: String, ep: UsbEndpoint, onPacket: (ByteArray, Int) -> Unit): Thread =
         Thread({
             val buf = ByteArray(VitureProtocol.PACKET_SIZE)

app/src/test/java/com/droiddeck/launcher/glasses/VitureLinkTest.kt

@@ -0,0 +1,141 @@
+package com.droiddeck.launcher.glasses
+
+import android.content.Context
+import android.hardware.usb.UsbConfiguration
+import android.hardware.usb.UsbConstants
+import android.hardware.usb.UsbDevice
+import android.hardware.usb.UsbDeviceConnection
+import android.hardware.usb.UsbEndpoint
+import android.hardware.usb.UsbInterface
+import android.hardware.usb.UsbManager
+import android.os.Parcelable
+import org.junit.After
+import org.junit.Assert.assertArrayEquals
+import org.junit.Assert.assertEquals
+import org.junit.Assert.assertNotNull
+import org.junit.Assert.assertNull
+import org.junit.Before
+import org.junit.Test
+import org.junit.runner.RunWith
+import org.robolectric.RobolectricTestRunner
+import org.robolectric.RuntimeEnvironment
+import org.robolectric.annotation.Config
+import org.robolectric.annotation.Implementation
+import org.robolectric.annotation.Implements
+import org.robolectric.util.ReflectionHelpers
+import org.robolectric.util.ReflectionHelpers.ClassParameter.from
+import java.util.concurrent.ConcurrentHashMap
+import java.util.concurrent.LinkedBlockingQueue
+import java.util.concurrent.TimeUnit
+
+@RunWith(RobolectricTestRunner::class)
+@Config(sdk = [28], manifest = Config.NONE, shadows = [VitureLinkTest.QueuedConnection::class])
+class VitureLinkTest {
+    private val frames = LinkedBlockingQueue<VitureProtocol.Frame>()
+    private val imu = LinkedBlockingQueue<ByteArray>()
+    private var link: VitureLink? = null
+
+    private val listener = object : VitureLink.Listener {
+        override fun onFrame(frame: VitureProtocol.Frame) { frames += frame }
+        override fun onImu(payload: ByteArray, timeNs: Long) { imu += payload }
+        override fun onLost() {}
+    }
+
+    @Before fun prepare() = incoming.clear()
+
+    @After fun close() { link?.close() }
+
+    @Test fun framesWithABadChecksumAreDroppedAndCounted() {
+        val usb = RuntimeEnvironment.getApplication().getSystemService(Context.USB_SERVICE) as UsbManager
+        val l = VitureLink.open(usb, glasses(), listener)
+        assertNotNull(l)
+        link = l
+        val report = packet(VitureProtocol.MAGIC_REPORT, VitureProtocol.EVENT_BRIGHTNESS, byteArrayOf(6))
+        val pose = ByteArray(36) { it.toByte() }
+        val imuPacket = packet(VitureProtocol.MAGIC_IMU, 0, pose)
+        feed(MCU_IN, corrupt(report), report)
+        feed(IMU_IN, corrupt(imuPacket), imuPacket)
+
+        val frame = frames.poll(2, TimeUnit.SECONDS)
+        assertNotNull(frame)
+        assertEquals(VitureProtocol.EVENT_BRIGHTNESS, frame!!.msgId)
+        assertArrayEquals(byteArrayOf(6), frame.payload)
+        assertArrayEquals(pose, imu.poll(2, TimeUnit.SECONDS))
+        assertNull(frames.poll(200, TimeUnit.MILLISECONDS))
+        assertNull(imu.poll(0, TimeUnit.MILLISECONDS))
+        assertEquals(2L, l!!.badFrames)
+    }
+
+    private fun packet(magic: Int, msgId: Int, payload: ByteArray) =
+        VitureProtocol.packet(msgId, payload).also { it[1] = magic.toByte() }
+
+    private fun corrupt(packet: ByteArray) = packet.copyOf().also { it[18] = (it[18].toInt() xor 0x01).toByte() }
+
+    private fun feed(address: Int, vararg packets: ByteArray) {
+        val queue = incoming.getOrPut(address) { LinkedBlockingQueue() }
+        packets.forEach(queue::put)
+    }
+
+    private fun glasses(): UsbDevice {
+        fun endpoint(address: Int): UsbEndpoint = ReflectionHelpers.callConstructor(
+            UsbEndpoint::class.java,
+            from(Int::class.javaPrimitiveType, address),
+            from(Int::class.javaPrimitiveType, UsbConstants.USB_ENDPOINT_XFER_INT),
+            from(Int::class.javaPrimitiveType, VitureProtocol.PACKET_SIZE),
+            from(Int::class.javaPrimitiveType, 1),
+        )
+        fun hid(id: Int, vararg endpoints: UsbEndpoint): UsbInterface = ReflectionHelpers.callConstructor(
+            UsbInterface::class.java,
+            from(Int::class.javaPrimitiveType, id),
+            from(Int::class.javaPrimitiveType, 0),
+            from(String::class.java, "hid$id"),
+            from(Int::class.javaPrimitiveType, UsbConstants.USB_CLASS_HID),
+            from(Int::class.javaPrimitiveType, 0),
+            from(Int::class.javaPrimitiveType, 0),
+        ).also { hidden(it, "setEndpoints", *endpoints) }
+        val config: UsbConfiguration = ReflectionHelpers.callConstructor(
+            UsbConfiguration::class.java,
+            from(Int::class.javaPrimitiveType, 1),
+            from(String::class.java, "default"),
+            from(Int::class.javaPrimitiveType, 0),
+            from(Int::class.javaPrimitiveType, 250),
+        )
+        hidden(config, "setInterfaces", hid(0, endpoint(IMU_IN)), hid(1, endpoint(MCU_IN), endpoint(MCU_OUT)))
+        return ReflectionHelpers.callConstructor(
+            UsbDevice::class.java,
+            from(String::class.java, "/dev/bus/usb/001/002"),
+            from(Int::class.javaPrimitiveType, VitureProtocol.VENDOR_ID),
+            from(Int::class.javaPrimitiveType, 0x101d),
+            from(Int::class.javaPrimitiveType, 0),
+            from(Int::class.javaPrimitiveType, 0),
+            from(Int::class.javaPrimitiveType, 0),
+            from(String::class.java, "VITURE"),
+            from(String::class.java, "VITURE Pro XR"),
+            from(String::class.java, "1.0"),
+            from(String::class.java, "test"),
+        ).also { hidden(it, "setConfigurations", config) }
+    }
+
+    private fun hidden(target: Any, method: String, vararg items: Parcelable) {
+        ReflectionHelpers.callInstanceMethod<Unit>(target, method, from(Array<Parcelable>::class.java, arrayOf(*items)))
+    }
+
+    @Implements(UsbDeviceConnection::class)
+    class QueuedConnection {
+        @Implementation fun claimInterface(intf: UsbInterface, force: Boolean) = true
+        @Implementation fun releaseInterface(intf: UsbInterface) = true
+        @Implementation fun bulkTransfer(endpoint: UsbEndpoint, buffer: ByteArray, length: Int, timeout: Int): Int {
+            if (endpoint.direction == UsbConstants.USB_DIR_OUT) return length
+            val packet = incoming[endpoint.address]?.poll(timeout.toLong(), TimeUnit.MILLISECONDS) ?: return 0
+            packet.copyInto(buffer, endIndex = minOf(packet.size, length))
+            return minOf(packet.size, length)
+        }
+    }
+
+    companion object {
+        private const val IMU_IN = 0x81
+        private const val MCU_IN = 0x82
+        private const val MCU_OUT = 0x02
+        private val incoming = ConcurrentHashMap<Int, LinkedBlockingQueue<ByteArray>>()
+    }
+}

docs/agent-control.md

@@ -132,6 +132,7 @@ tools/droiddeckctl glasses raw 0x0007
 Glasses status reports `deviceMode` from the MCU separately from `sbs`, which
 requires a physical stereo display mode. `switching3d` marks the bounded
 confirmation window. A successful mode-command reply alone does not confirm 3D.
+`badFrames` counts reports and IMU packets dropped for a bad checksum.
 
 A sleeping device suspends the session and hides the app's windows from `ui dump`; `wake` first.