diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/FeatureFlags.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/FeatureFlags.kt new file mode 100644 index 0000000000..a30d05faea --- /dev/null +++ b/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 = 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 + } + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index 5272f44a59..44d51121ee 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/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 -> {} } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPDevice.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPDevice.kt index 3faea3d15c..511d559261 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPDevice.kt +++ b/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() + var firmwareFeatures = FirmwareFeatures() + fun isNextPacket(packetId: Long): Boolean { if (packetId != 0L && packetId <= lastPacketNumber) return false lastPacketNumber = packetId diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt index c502c261cb..8c4c8cbbfa 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt +++ b/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, diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt index 97ff91d2fe..9592a62874 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt +++ b/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 { 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(128) init { HANDSHAKE_BUFFER[0] = 3