Implement packet bundling (#794)

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/FeatureFlags.kt

@@ -0,0 +1,71 @@
+package dev.slimevr.tracking.trackers.udp
+
+import java.nio.ByteBuffer
+
+/**
+ * Bit packed flags, enum values start with 0 and indicate which bit it is.
+ *
+ * Change the enums and `flagsEnabled` inside to extend.
+ */
+class FirmwareFeatures {
+	enum class FirmwareFeatureFlags {
+		// EXAMPLE_FEATURE,
+
+		// Add new flags here
+
+		BITS_TOTAL, ;
+	}
+
+	fun has(flag: FirmwareFeatureFlags): Boolean {
+		val bit = flag.ordinal
+		return (flags[bit / 8].toInt() and (1 shl (bit % 8))) != 0
+	}
+
+	/**
+	 * Whether the firmware supports the "feature flags" feature,
+	 * set to true when we've received flags packet from the firmware.
+	 */
+	var available = false
+		private set
+
+	companion object {
+		fun from(received: ByteBuffer, length: Int): FirmwareFeatures {
+			val res = FirmwareFeatures()
+			res.available = true
+			received.get(res.flags, 0, Math.min(res.flags.size, length))
+			return res
+		}
+	}
+
+	private val flags = ByteArray(FirmwareFeatureFlags.BITS_TOTAL.ordinal / 8 + 1)
+}
+
+enum class ServerFeatureFlags {
+	/** Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). */
+	PROTOCOL_BUNDLE_SUPPORT,
+
+	// Add new flags here
+
+	BITS_TOTAL, ;
+
+	companion object {
+		val flagsEnabled: Set<ServerFeatureFlags> = setOf(
+			PROTOCOL_BUNDLE_SUPPORT
+
+			// Add enabled flags here
+		)
+
+		val packed = run {
+			val byteLength = BITS_TOTAL.ordinal / 8 + 1
+			val tempPacked = ByteArray(byteLength)
+
+			for (flag in flagsEnabled) {
+				val bit = flag.ordinal
+				tempPacked[bit / 8] =
+					(tempPacked[bit / 8].toInt() or (1 shl (bit % 8))).toByte()
+			}
+
+			tempPacked
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt

@@ -132,6 +132,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 			}
 			connection
 		}
+		connection.firmwareFeatures = FirmwareFeatures()
 		bb.limit(bb.capacity())
 		bb.rewind()
 		parser.writeHandshakeResponse(bb, connection)
@@ -193,8 +194,9 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 					bb.limit(received.length)
 					bb.rewind()
 					val connection = synchronized(connections) { connectionsByAddress[received.address] }
-					val packet = parser.parse(bb, connection)
-					packet?.let { processPacket(received, it, connection) }
+					parser.parse(bb, connection)
+						.filterNotNull()
+						.forEach { processPacket(received, it, connection) }
 				} catch (ignored: SocketTimeoutException) {
 				} catch (e: Exception) {
 					LogManager.warning(
@@ -391,6 +393,17 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 					"[TrackerServer] User action from ${connection.descriptiveName } received. $name reset performed."
 				)
 			}
+
+			is UDPPacket22FeatureFlags -> {
+				if (connection == null) return
+				// Respond with server flags
+				bb.limit(bb.capacity())
+				bb.rewind()
+				parser.write(bb, connection, packet)
+				socket.send(DatagramPacket(rcvBuffer, bb.position(), connection.address))
+				connection.firmwareFeatures = packet.firmwareFeatures
+			}
+
 			is UDPPacket200ProtocolChange -> {}
 		}
 	}

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPDevice.kt

@@ -53,6 +53,8 @@ class UDPDevice(
 	var timedOut = false
 	override val trackers = ConcurrentHashMap<Int, Tracker>()
 
+	var firmwareFeatures = FirmwareFeatures()
+
 	fun isNextPacket(packetId: Long): Boolean {
 		if (packetId != 0L && packetId <= lastPacketNumber) return false
 		lastPacketNumber = packetId

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt

@@ -8,7 +8,7 @@ import java.nio.BufferUnderflowException
 import java.nio.ByteBuffer
 
 sealed class UDPPacket(val packetId: Int) {
-	@Throws(IOException::class)
+	@Throws(IOException::class, BufferUnderflowException::class)
 	open fun readData(buf: ByteBuffer) {}
 
 	@Throws(IOException::class)
@@ -320,6 +320,19 @@ data class UDPPacket21UserAction(var type: Int = 0) : UDPPacket(21) {
 	}
 }
 
+class UDPPacket22FeatureFlags(
+	var firmwareFeatures: FirmwareFeatures = FirmwareFeatures(),
+) :
+	UDPPacket(22) {
+	override fun readData(buf: ByteBuffer) {
+		firmwareFeatures = FirmwareFeatures.from(buf, buf.remaining())
+	}
+
+	override fun writeData(buf: ByteBuffer) {
+		buf.put(ServerFeatureFlags.packed)
+	}
+}
+
 data class UDPPacket200ProtocolChange(
 	var targetProtocol: Int = 0,
 	var targetProtocolVersion: Int = 0,

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt

@@ -6,7 +6,7 @@ import java.nio.charset.StandardCharsets
 
 class UDPProtocolParser {
 	@Throws(IOException::class)
-	fun parse(buf: ByteBuffer, connection: UDPDevice?): UDPPacket? {
+	fun parse(buf: ByteBuffer, connection: UDPDevice?): Array<UDPPacket?> {
 		val packetId = buf.int
 		val packetNumber = buf.long
 		if (connection != null) {
@@ -18,6 +18,26 @@ class UDPProtocolParser {
 			}
 			connection.lastPacket = System.currentTimeMillis()
 		}
+		if (packetId == PACKET_BUNDLE) {
+			bundlePackets.clear()
+			while (buf.hasRemaining()) {
+				val bundlePacketLen = Math.min(buf.short.toInt(), buf.remaining())
+				if (bundlePacketLen == 0) continue
+
+				val bundlePacketStart = buf.position()
+				val bundleBuf = buf.slice(bundlePacketStart, bundlePacketLen)
+				val bundlePacketId = bundleBuf.int
+				val newPacket = getNewPacket(bundlePacketId)
+				newPacket?.let {
+					newPacket.readData(bundleBuf)
+					bundlePackets.add(newPacket)
+				}
+
+				buf.position(bundlePacketStart + bundlePacketLen)
+			}
+			return bundlePackets.toTypedArray()
+		}
+
 		val newPacket = getNewPacket(packetId)
 		if (newPacket != null) {
 			newPacket.readData(buf)
@@ -27,7 +47,7 @@ class UDPProtocolParser {
 // 					packetId + " from " + connection
 // 			)
 		}
-		return newPacket
+		return arrayOf(newPacket)
 	}
 
 	@Throws(IOException::class)
@@ -71,6 +91,7 @@ class UDPProtocolParser {
 			PACKET_SIGNAL_STRENGTH -> UDPPacket19SignalStrength()
 			PACKET_TEMPERATURE -> UDPPacket20Temperature()
 			PACKET_USER_ACTION -> UDPPacket21UserAction()
+			PACKET_FEATURE_FLAGS -> UDPPacket22FeatureFlags()
 			PACKET_PROTOCOL_CHANGE -> UDPPacket200ProtocolChange()
 			else -> null
 		}
@@ -103,8 +124,11 @@ class UDPProtocolParser {
 		const val PACKET_SIGNAL_STRENGTH = 19
 		const val PACKET_TEMPERATURE = 20
 		const val PACKET_USER_ACTION = 21
+		const val PACKET_FEATURE_FLAGS = 22
+		const val PACKET_BUNDLE = 100
 		const val PACKET_PROTOCOL_CHANGE = 200
 		private val HANDSHAKE_BUFFER = ByteArray(64)
+		private val bundlePackets = ArrayList<UDPPacket>(128)
 
 		init {
 			HANDSHAKE_BUFFER[0] = 3