ble: implement message reading&joining

Now are able to read the first message:
```
INFO[0005] Received SPS1  6b943ec06b594f8a0383f384a3c916da75e1c7846c3e1b73f72f86ee2dc48774b2b4e5ad62d798b76cfd06be1cd4c937
DEBU[0005] Donna LTK: b874cb3cbe487040442138452faeb02d284ac55f489f19593265ff52f7310f1f
DEBU[0005] First key 58cb3b742dc48774000000001cd4c937 :: 16
DEBU[0005] CMACY: 16
DEBU[0005] Intermediar key 4c13eebc4cf09795a07c50bf13786c18 :: 16
DEBU[0005] Pod public 2fe57da347cd62431528daac5fbb290730fff684afc4cfc2ed90995f58cb3b74 :: 32
DEBU[0005] Pod nonce 00000000000000000000000000000000 :: 16
DEBU[0005] Generated SPS1: 535053313d00302fe57da347cd62431528daac5fbb290730fff684afc4cfc2ed90995f58cb3b7400000000000000000000000000000000
TRAC[0005] CMD notification return: 4/00
TRAC[0005] received CMD:  01
TRAC[0005] Sending message: 54570003000006e00000109100001092535053313d00302fe57da347cd62431528daac5fbb290730fff684afc4cfc2ed90995f58cb3b7400000000000000000000000000000000
TRAC[0005] DATA notification return: 23/000354570003000006e000001091000010925350
TRAC[0005] DATA notification return: 23/0153313d00302fe57da347cd62431528daac5fbb
TRAC[0005] DATA notification return: 23/02290730fff684afc4cfc2ed90995f58cb3b7400
TRAC[0005] DATA notification return: 23/030f7d02931d0000000000000000000000000000
TRAC[0005] DATA notification return: 23/0401000000000000000000000000000000000000
TRAC[0005] received CMD:  04
```

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/OmnipodDashBleManagerImpl.kt

@@ -5,7 +5,6 @@ import android.bluetooth.BluetoothDevice
 import android.bluetooth.BluetoothManager
 import android.bluetooth.BluetoothProfile
 import android.content.Context
-import com.google.crypto.tink.subtle.X25519
 import info.nightscout.androidaps.logging.AAPSLogger
 import info.nightscout.androidaps.logging.LTag
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.BuildConfig
@@ -25,7 +24,6 @@ import org.apache.commons.lang3.NotImplementedException
 import java.util.concurrent.BlockingQueue
 import java.util.concurrent.LinkedBlockingDeque
 import java.util.concurrent.TimeoutException
-import javax.crypto.KeyAgreement
 import javax.inject.Inject
 import javax.inject.Singleton
 
@@ -112,6 +110,11 @@ class OmnipodDashBleManagerImpl @Inject constructor(private val context: Context
         TODO("not implemented")
     }
 
+    override fun getPodId(): Id {
+        // TODO: return something meaningful here
+        return Id.fromInt(4243)
+    }
+
     companion object {
 
         private const val CONNECT_TIMEOUT_MS = 5000

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/command/BleCommand.kt

@@ -20,7 +20,7 @@ open class BleCommand(val data: ByteArray) {
     }
 
     override fun toString(): String {
-        return "Raw command: [${data.toHex()}]";
+        return "Raw command: [${data.toHex()}]"
     }
 
     override fun hashCode(): Int {

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/MessageIOException.kt

@@ -0,0 +1,3 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions
+
+class MessageIOException(override val cause: Throwable) : Exception(cause)

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/io/BleIO.kt

@@ -50,7 +50,6 @@ class BleIO(private val aapsLogger: AAPSLogger, private val chars: Map<Character
             state = IOState.WRITING
         }
         aapsLogger.debug(LTag.PUMPBTCOMM, "BleIO: Sending data on " + characteristic.name + "/" + payload.toHex())
-        aapsLogger.debug(LTag.PUMPBTCOMM, "BleIO: Sending data on " + characteristic.name + "/" + payload.toHex())
         val ch = chars[characteristic]
         val set = ch!!.setValue(payload)
         if (!set) {

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTKExchanger.kt

@@ -3,21 +3,22 @@ package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
 import com.google.crypto.tink.subtle.X25519
 import info.nightscout.androidaps.logging.AAPSLogger
 import info.nightscout.androidaps.logging.LTag
-import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.OmnipodDashBleManagerImpl
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.Id
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.OmnipodDashBleManagerImpl
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.MessageIO
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.StringLengthPrefixEncoding
 import info.nightscout.androidaps.utils.extensions.hexStringToByteArray
 import java.security.SecureRandom
 
 internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgIO: MessageIO) {
+
     private val privateKey = X25519.generatePrivateKey()
     private val nonce = ByteArray(16)
     private val controllerId = Id.fromInt(OmnipodDashBleManagerImpl.CONTROLLER_ID)
     val nodeId = controllerId.increment()
     private var seq: Byte = 1
 
-    init{
+    init {
         val random = SecureRandom()
         random.nextBytes(nonce)
     }
@@ -73,7 +74,7 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
         val publicKey = X25519.publicFromPrivate(privateKey)
         val payload = StringLengthPrefixEncoding.formatKeys(
             arrayOf("SPS1="),
-            arrayOf(publicKey+nonce),
+            arrayOf(publicKey + nonce),
         )
         return PairMessage(
             sequenceNumber = seq,

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/CrcMismatchException.kt

@@ -0,0 +1,6 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
+
+import info.nightscout.androidaps.utils.extensions.toHex
+
+class CrcMismatchException(val expected: Long, val got: Long, val payload: ByteArray) :
+    Exception("CRC missmatch. Got: ${got}. Expected: ${expected}. Payload: ${payload.toHex()}")

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/IncorrectPacketException.kt

@@ -0,0 +1,5 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
+
+import info.nightscout.androidaps.utils.extensions.toHex
+
+class IncorrectPacketException(val expectedIndex: Byte, val payload: ByteArray) : Exception("Invalid payload: ${payload.toHex()}. Expected index: ${expectedIndex}")

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessageIO.kt

@@ -3,12 +3,11 @@ package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
 import info.nightscout.androidaps.logging.AAPSLogger
 import info.nightscout.androidaps.logging.LTag
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.command.*
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.MessageIOException
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.UnexpectedCommandException
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.BleIO
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.CharacteristicType
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.PayloadJoiner
-import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.PayloadJoinerActionAccept
-import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.PayloadJoinerActionReject
 import info.nightscout.androidaps.utils.extensions.toHex
 
 class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
@@ -21,10 +20,11 @@ class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
             throw UnexpectedCommandException(BleCommand(expectCTS))
         }
         val payload = msg.asByteArray()
+        aapsLogger.debug(LTag.PUMPBTCOMM, "Sending message: ${payload.toHex()}")
         val splitter = PayloadSplitter(payload)
         val packets = splitter.splitInPackets()
         for (packet in packets) {
-            aapsLogger.debug(LTag.PUMPBTCOMM, "Sending DATA: ", packet.asByteArray().toHex())
+            aapsLogger.debug(LTag.PUMPBTCOMM, "Sending DATA: ${packet.asByteArray().toHex()}")
             bleIO.sendAndConfirmPacket(CharacteristicType.DATA, packet.asByteArray())
         }
         // TODO: peek for NACKs
@@ -36,35 +36,32 @@ class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
         bleIO.flushIncomingQueues()
     }
 
+    @kotlin.ExperimentalUnsignedTypes
     fun receiveMessage(): MessagePacket {
         val expectRTS = bleIO.receivePacket(CharacteristicType.CMD)
         if (BleCommand(expectRTS) != BleCommandRTS()) {
             throw UnexpectedCommandException(BleCommand(expectRTS))
         }
         bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandCTS().data)
-        val joiner = PayloadJoiner()
+        try {
-        var data = bleIO.receivePacket(CharacteristicType.DATA)
+            val joiner = PayloadJoiner(bleIO.receivePacket(CharacteristicType.DATA))
-        val fragments = joiner.start(data)
+            for (i in 1 until joiner.fullFragments + 1) {
-        for (i in 1 until fragments) {
+                joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
-            data = bleIO.receivePacket(CharacteristicType.DATA)
-            val accumlateAction = joiner.accumulate(data)
-            if (accumlateAction is PayloadJoinerActionReject) {
-                bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandNack(accumlateAction.idx).data)
             }
-        }
+            if (joiner.oneExtra) {
-        if (joiner.oneExtra) {
+                joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
-            data = bleIO.receivePacket(CharacteristicType.DATA)
-            val accumulateAction = joiner.accumulate(data)
-            if (accumulateAction is PayloadJoinerActionReject) {
-                bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandNack(accumulateAction.idx).data)
             }
+            val fullPayload = joiner.finalize()
+            bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandSuccess().data)
+            return MessagePacket.parse(fullPayload)
+        } catch (e: IncorrectPacketException) {
+            aapsLogger.warn(LTag.PUMPBTCOMM, "Received incorrect packet: $e")
+            bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandNack(e.expectedIndex).data)
+            throw MessageIOException(cause = e)
+        } catch (e: CrcMismatchException) {
+            aapsLogger.warn(LTag.PUMPBTCOMM, "CRC mismatch: $e")
+            bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandFail().data)
+            throw MessageIOException(cause = e)
         }
-        val finalCmd = when (joiner.finalize()) {
-            is PayloadJoinerActionAccept -> BleCommandSuccess()
-            is PayloadJoinerActionReject -> BleCommandFail()
-        }
-        bleIO.sendAndConfirmPacket(CharacteristicType.CMD, finalCmd.data)
-        val fullPayload = joiner.bytes()
-        return MessagePacket.parse(fullPayload)
     }
 }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/PayloadJoiner.kt

@@ -1,31 +1,108 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io
 
-import java.io.ByteArrayOutputStream
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.CrcMismatchException
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.IncorrectPacketException
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.crc32
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.packet.BlePacket
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.packet.FirstBlePacket
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.packet.LastBlePacket
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.packet.LastOptionalPlusOneBlePacket
+import java.lang.Integer.min
+import java.nio.ByteBuffer
+import java.util.*
 
-sealed class PayloadJoinerAction
+@ExperimentalUnsignedTypes
-
+class PayloadJoiner(private val firstPacket: ByteArray) {
-class PayloadJoinerActionAccept : PayloadJoinerAction()
-class PayloadJoinerActionReject(val idx: Byte) : PayloadJoinerAction()
-
-class PayloadJoiner {
 
     var oneExtra: Boolean = false
+    val fullFragments: Int
+    var crc: Long = 0
+    private var expectedIndex = 0
+    private val fragments: LinkedList<ByteArray> = LinkedList<ByteArray>()
 
-    private val payload = ByteArrayOutputStream()
+    init {
+        if (firstPacket.size < 2) {
+            throw IncorrectPacketException(0, firstPacket)
+        }
+        fullFragments = firstPacket[1].toInt()
+        when {
+            // Without middle packets
+            firstPacket.size < FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS ->
+                throw IncorrectPacketException(0, firstPacket)
 
-    fun start(payload: ByteArray): Int {
+            fullFragments == 0                   -> {
-        TODO("not implemented")
+                crc = ByteBuffer.wrap(firstPacket.copyOfRange(2, 6)).int.toUInt().toLong()
+                val rest = firstPacket[6]
+                val end = min(rest + 7, BlePacket.MAX_LEN)
+                oneExtra = rest + 7 > end
+                fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS, end))
+                if (end > firstPacket.size) {
+                    throw IncorrectPacketException(0, firstPacket)
+                }
+            }
+
+            // With middle packets
+            firstPacket.size < BlePacket.MAX_LEN ->
+                throw IncorrectPacketException(0, firstPacket)
+
+            else                                 -> {
+                fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS, BlePacket.MAX_LEN))
+            }
+        }
     }
 
-    fun accumulate(payload: ByteArray): PayloadJoinerAction {
+    fun accumulate(packet: ByteArray) {
-        TODO("not implemented")
+        if (packet.size < 3) { // idx, size, at least 1 byte of payload
+            throw IncorrectPacketException((expectedIndex + 1).toByte(), packet)
+        }
+        val idx = packet[0].toInt()
+        if (idx != expectedIndex + 1) {
+            throw IncorrectPacketException((expectedIndex + 1).toByte(), packet)
+        }
+        expectedIndex++
+        when {
+            idx < fullFragments  -> { // this is a middle fragment
+                if (packet.size < BlePacket.MAX_LEN) {
+                    throw IncorrectPacketException(idx.toByte(), packet)
+                }
+                fragments.add(packet.copyOfRange(1, BlePacket.MAX_LEN))
+            }
+
+            idx == fullFragments -> { // this is the last fragment
+                if (packet.size < LastBlePacket.HEADER_SIZE) {
+                    throw IncorrectPacketException(idx.toByte(), packet)
+                }
+                crc = ByteBuffer.wrap(packet.copyOfRange(2, 6)).int.toUInt().toLong()
+                val rest = packet[1].toInt()
+                val end = min(rest, BlePacket.MAX_LEN)
+                if (packet.size < end) {
+                    throw IncorrectPacketException(idx.toByte(), packet)
+                }
+                oneExtra = rest + LastBlePacket.HEADER_SIZE > end
+                fragments.add(packet.copyOfRange(LastBlePacket.HEADER_SIZE, BlePacket.MAX_LEN))
+            }
+
+            idx > fullFragments  -> { // this is the extra fragment
+                val size = packet[1].toInt()
+                if (packet.size < LastOptionalPlusOneBlePacket.HEADER_SIZE + size) {
+                    throw IncorrectPacketException(idx.toByte(), packet)
+                }
+
+                fragments.add(packet.copyOfRange(LastOptionalPlusOneBlePacket.HEADER_SIZE, LastOptionalPlusOneBlePacket.HEADER_SIZE + size))
+            }
+        }
     }
 
-    fun finalize(): PayloadJoinerAction {
+    fun finalize(): ByteArray {
-        TODO("not implemented")
+        val totalLen = fragments.fold(0, { acc, elem -> acc + elem.size })
+        val bb = ByteBuffer.allocate(totalLen)
+        fragments.map { fragment -> bb.put(fragment) }
+        bb.flip()
+        val bytes = bb.array()
+        if (bytes.crc32() != crc) {
+            throw CrcMismatchException(bytes.crc32(), crc, bytes)
+        }
+        return bytes.copyOfRange(0, bytes.size)
     }
 
-    fun bytes(): ByteArray {
-        TODO("not implemented")
-    }
 }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/PayloadSplitter.kt

@@ -25,7 +25,7 @@ internal class PayloadSplitter(private val payload: ByteArray) {
                 ret.add(LastOptionalPlusOneBlePacket(
                     index = 1,
                     payload = payload.copyOfRange(end, payload.size),
-                    size = (payload.size-end).toByte(),
+                    size = (payload.size - end).toByte(),
                 ))
             }
             return ret
@@ -57,7 +57,7 @@ internal class PayloadSplitter(private val payload: ByteArray) {
         if (rest > LastBlePacket.CAPACITY) {
             ret.add(LastOptionalPlusOneBlePacket(
                 index = (middleFragments + 2).toByte(),
-                size = (rest-LastBlePacket.CAPACITY).toByte(),
+                size = (rest - LastBlePacket.CAPACITY).toByte(),
                 payload = payload.copyOfRange(middleFragments * MiddleBlePacket.CAPACITY + FirstBlePacket.CAPACITY_WITH_MIDDLE_PACKETS + LastBlePacket.CAPACITY, payload.size),
             ))
         }
@@ -65,7 +65,7 @@ internal class PayloadSplitter(private val payload: ByteArray) {
     }
 }
 
-private fun ByteArray.crc32(): Long {
+internal fun ByteArray.crc32(): Long {
     val crc = CRC32()
     crc.update(this)
     return crc.value

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/packet/BlePacket.kt

@@ -8,8 +8,8 @@ sealed class BlePacket {
 
     companion object {
 
-        const val MAX_BLE_PACKET_LEN = 20
+        const val MAX_LEN = 20
-        const val MAX_BLE_BUFFER_LEN = MAX_BLE_PACKET_LEN + 1 // we use this as the size allocated for the ByteBuffer
+        const val MAX_BLE_BUFFER_LEN = MAX_LEN + 1 // we use this as the size allocated for the ByteBuffer
     }
 }
 
@@ -35,8 +35,11 @@ data class FirstBlePacket(val totalFragments: Byte, val payload: ByteArray, val
 
     companion object {
 
-        internal const val CAPACITY_WITHOUT_MIDDLE_PACKETS = 13 // we are using all fields
+        internal const val HEADER_SIZE_WITHOUT_MIDDLE_PACKETS = 7 // we are using all fields
-        internal const val CAPACITY_WITH_MIDDLE_PACKETS = 18 // we are not using crc32 or size
+        internal const val HEADER_SIZE_WITH_MIDDLE_PACKETS = 2
+
+        internal const val CAPACITY_WITHOUT_MIDDLE_PACKETS = MAX_LEN - HEADER_SIZE_WITHOUT_MIDDLE_PACKETS // we are using all fields
+        internal const val CAPACITY_WITH_MIDDLE_PACKETS = MAX_LEN - HEADER_SIZE_WITH_MIDDLE_PACKETS // we are not using crc32 or size
         internal const val CAPACITY_WITH_THE_OPTIONAL_PLUS_ONE_PACKET = 18
     }
 }
@@ -70,7 +73,8 @@ data class LastBlePacket(val index: Byte, val size: Byte, val payload: ByteArray
 
     companion object {
 
-        internal const val CAPACITY = 14
+        internal const val HEADER_SIZE = 6
+        internal const val CAPACITY = MAX_LEN - HEADER_SIZE
     }
 }
 
@@ -79,5 +83,11 @@ data class LastOptionalPlusOneBlePacket(val index: Byte, val payload: ByteArray,
     override fun asByteArray(): ByteArray {
         return byteArrayOf(index, size) + payload
     }
+
+    companion object {
+
+        internal const val HEADER_SIZE = 2
+        internal const val CAPACITY = MAX_LEN - HEADER_SIZE
+    }
 }