Merge pull request #10 from 0pen-dash/ble-message-receiving

Ble message receiving

build.gradle

@@ -23,6 +23,7 @@ buildscript {
         commonscodec_version = '1.15'
         jodatime_version = '2.10.10'
         work_version = '2.5.0'
+        tink_version = '1.5.0'
 
         junit_version = '4.13.2'
         mockitoVersion = '3.7.7'

core/core_dependencies.gradle

@@ -46,6 +46,7 @@ dependencies {
 
     //CryptoUtil
     api 'com.madgag.spongycastle:core:1.58.0.0'
+    api "com.google.crypto.tink:tink-android:$tink_version"
 
     // Graphview cannot be upgraded
     api "com.jjoe64:graphview:4.0.1"

omnipod-dash/build.gradle

@@ -21,7 +21,5 @@ dependencies {
     implementation "androidx.room:room-runtime:$room_version"
     implementation "androidx.room:room-rxjava2:$room_version"
     kapt "androidx.room:room-compiler:$room_version"
-
     implementation 'com.github.guepardoapps:kulid:1.1.2.0'
-
+}
-}

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

@@ -13,12 +13,13 @@ import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.command.
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.*
 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.ltk.LTKExchanger
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.MessageIO
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.pair.LTKExchanger
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.scan.PodScanner
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.status.ConnectionStatus
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.event.PodEvent
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.pod.command.base.Command
+import info.nightscout.androidaps.utils.extensions.toHex
 import io.reactivex.Observable
 import org.apache.commons.lang3.NotImplementedException
 import java.util.concurrent.BlockingQueue
@@ -96,8 +97,10 @@ class OmnipodDashBleManagerImpl @Inject constructor(private val context: Context
             val ltkExchanger = LTKExchanger(aapsLogger, msgIO)
             emitter.onNext(PodEvent.Pairing)
 
-            val ltk = ltkExchanger.negotiateLTKAndNonce()
+            val ltk = ltkExchanger.negotiateLTK()
-            aapsLogger.info(LTag.PUMPCOMM, "Got LTK and Nonce Prefix: ${ltk}")
+
+            aapsLogger.info(LTag.PUMPCOMM, "Got LTK: ${ltk.ltk.toHex()}")
+
             emitter.onNext(PodEvent.Connected(PodScanner.POD_ID_NOT_ACTIVATED)) // TODO supply actual pod id
 
             emitter.onComplete()
@@ -112,7 +115,7 @@ class OmnipodDashBleManagerImpl @Inject constructor(private val context: Context
 
     companion object {
 
-        private const val CONNECT_TIMEOUT_MS = 5000
+        private const val CONNECT_TIMEOUT_MS = 7000
         const val CONTROLLER_ID = 4242 // TODO read from preferences or somewhere else.
     }
 

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

@@ -1,5 +1,7 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.command
 
+import info.nightscout.androidaps.utils.extensions.toHex
+
 open class BleCommand(val data: ByteArray) {
 
     constructor(type: BleCommandType) : this(byteArrayOf(type.value))
@@ -17,6 +19,10 @@ open class BleCommand(val data: ByteArray) {
         return true
     }
 
+    override fun toString(): String {
+        return "Raw command: [${data.toHex()}]"
+    }
+
     override fun hashCode(): Int {
         return data.contentHashCode()
     }

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

@@ -0,0 +1,5 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions
+
+import info.nightscout.androidaps.utils.extensions.toHex
+
+class CouldNotParseMessageException(val payload: ByteArray) : Exception("Could not parse message payload: ${payload.toHex()}")

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

@@ -0,0 +1,6 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions
+
+class MessageIOException : Exception {
+    constructor(msg: String) : super(msg)
+    constructor(cause: Throwable) : super("Caught Exception during Message I/O", 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/LTK.kt

@@ -1,8 +0,0 @@
-package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
-
-data class LTK(val ltk: ByteArray, val noncePrefix: ByteArray) {
-    init {
-        require(ltk.size == 16)
-        require(noncePrefix.size == 16)
-    }
-}

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

@@ -1,66 +0,0 @@
-package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
-
-import info.nightscout.androidaps.logging.AAPSLogger
-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.message.MessageIO
-import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.StringLengthPrefixEncoding
-import info.nightscout.androidaps.utils.extensions.hexStringToByteArray
-
-internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgIO: MessageIO) {
-
-    fun negotiateLTKAndNonce(): LTK? {
-        // send SP1, SP2
-        // TODO: get this from somewhere(preferences?)
-        var seq: Byte = 1
-        val controllerId = Id.fromInt(OmnipodDashBleManagerImpl.CONTROLLER_ID)
-        val nodeId = controllerId.increment()
-
-        var sp1sp2 = sp1sp2(nodeId.address, sp2(), seq, controllerId, nodeId)
-        msgIO.sendMesssage(sp1sp2.messagePacket)
-
-/*
-        var sps1 =
-        msgIO.sendMesssage(sps1.messagePacket)
-        // send SPS1
-        // read SPS1
-        val podSps1 = msgIO.receiveMessage()
-
-        // send SPS2
-        var sps2 = PairMessage()
-        msgIO.sendMesssage(sps2.messagePacket)
-        // read SPS2
-        val podSps2 = msgIO.receiveMessage()
-
-        // send SP0GP0
-        msgIO.sendMesssage(sps2.messagePacket)
-        // read P0
-        val p0 = msgIO.receiveMessage()
-*/
-        return null
-    }
-
-    private fun sp2(): ByteArray {
-        // This is GetPodStatus command, with page 0 parameter.
-        // We could replace that in the future with the serialized GetPodStatus()
-        return GET_POD_STATUS_HEX_COMMAND.hexStringToByteArray()
-    }
-
-    fun sp1sp2(sp1: ByteArray, sp2: ByteArray, seq: Byte, controllerId: Id, nodeId: Id): PairMessage {
-        val payload = StringLengthPrefixEncoding.formatKeys(
-            arrayOf("SP1=", ",SP2="),
-            arrayOf(sp1, sp2),
-        )
-        return PairMessage(
-            sequenceNumber = seq,
-            source = controllerId,
-            destination = nodeId,
-            payload = payload,
-        )
-    }
-
-    companion object {
-
-        private val GET_POD_STATUS_HEX_COMMAND = "ffc32dbd08030e0100008a" // TODO for now we are assuming this command is build out of constant parameters, use a proper command builder for that.
-    }
-}

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 actual: Long, val payload: ByteArray) :
+    Exception("CRC mismatch. Actual: ${actual}. 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
@@ -38,33 +38,29 @@ class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
 
     fun receiveMessage(): MessagePacket {
         val expectRTS = bleIO.receivePacket(CharacteristicType.CMD)
-        if (BleCommand(expectRTS) != BleCommandCTS()) {
+        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.oneExtraPacket) {
-        if (joiner.oneExtra) {
+                joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
-            var 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/MessagePacket.kt

@@ -1,6 +1,7 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
 
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.Id
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.CouldNotParseMessageException
 import java.nio.ByteBuffer
 
 /***
@@ -69,9 +70,56 @@ data class MessagePacket(
     companion object {
 
         private val MAGIC_PATTERN = "TW" // all messages start with this string
+        private val HEADER_SIZE = 16
 
         fun parse(payload: ByteArray): MessagePacket {
-            TODO("implement message header parsing")
+            if (payload.size < HEADER_SIZE) {
+                throw CouldNotParseMessageException(payload)
+            }
+            if (payload.copyOfRange(0, 2).decodeToString() != MAGIC_PATTERN) {
+                throw CouldNotParseMessageException(payload)
+            }
+            val f1 = Flag(payload[2].toInt())
+            val sas = f1.get(3) != 0
+            val tfs = f1.get(4) != 0
+            val version = ((f1.get(0) shl 2) or (f1.get(1) shl 1) or (f1.get(2) shl 0)).toShort()
+            val eqos = (f1.get(7) or (f1.get(6) shl 1) or (f1.get(5) shl 2)).toShort()
+
+            val f2 = Flag(payload[3].toInt())
+            val ack = f2.get(0) != 0
+            val priority = f2.get(1) != 0
+            val lastMessage = f2.get(2) != 0
+            val gateway = f2.get(3) != 0
+            val type = MessageType.byValue((f1.get(7) or (f1.get(6) shl 1) or (f1.get(5) shl 2) or (f1.get(4) shl 3)).toByte())
+            if (version.toInt() != 0) {
+                throw CouldNotParseMessageException(payload)
+            }
+            val sequenceNumber = payload[4]
+            val ackNumber = payload[5]
+            val size = (payload[6].toInt() shl 3) or (payload[7].toUnsignedInt() ushr 5)
+            if (size + HEADER_SIZE > payload.size) {
+                throw CouldNotParseMessageException(payload)
+            }
+            val payloadEnd = 16 + size +
+                if (type == MessageType.ENCRYPTED) 8
+                else 0
+
+            return MessagePacket(
+                type = type,
+                ack = ack,
+                eqos = eqos,
+                priority = priority,
+                lastMessage = lastMessage,
+                gateway = gateway,
+                sas = sas,
+                tfs = tfs,
+                version = version,
+                sequenceNumber = sequenceNumber,
+                ackNumber = ackNumber,
+                source = Id(payload.copyOfRange(8, 12)),
+                destination = Id(payload.copyOfRange(12, 16)),
+                payload = payload.copyOfRange(16, payloadEnd),
+            )
         }
     }
 }
@@ -85,8 +133,14 @@ private class Flag(var value: Int = 0) {
         value = value or mask
     }
 
-    fun get(idx: Byte): Boolean {
+    fun get(idx: Byte): Int {
         val mask = 1 shl (7 - idx)
-        return value and mask != 0
+        if (value and mask == 0) {
+            return 0
+        }
+        return 1
+
     }
-}
+}
+
+internal fun Byte.toUnsignedInt() = this.toInt() and 0xff

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

@@ -1,31 +1,109 @@
 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
+class PayloadJoiner(private val firstPacket: ByteArray) {
 
-class PayloadJoinerActionAccept : PayloadJoinerAction()
+    var oneExtraPacket: Boolean = false
-class PayloadJoinerActionReject(val idx: Byte) : PayloadJoinerAction()
+    val fullFragments: Int
+    var crc: Long = 0
+    private var expectedIndex = 0
+    private val fragments: LinkedList<ByteArray> = LinkedList<ByteArray>()
 
-class PayloadJoiner {
+    init {
+        if (firstPacket.size < FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS) {
+            throw IncorrectPacketException(0, firstPacket)
+        }
+        fullFragments = firstPacket[1].toInt()
+        when {
+            // Without middle packets
+            firstPacket.size < FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS ->
+                throw IncorrectPacketException(0, firstPacket)
 
-    var oneExtra: Boolean = false
+            fullFragments == 0                                                   -> {
+                crc = ByteBuffer.wrap(firstPacket.copyOfRange(2, 6)).int.toUnsignedLong()
+                val rest = firstPacket[6]
+                val end = min(rest + FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS, BlePacket.MAX_SIZE)
+                oneExtraPacket = rest + FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS > end
+                if (end > firstPacket.size) {
+                    throw IncorrectPacketException(0, firstPacket)
+                }
+                fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS, end))
+            }
 
-    private val payload = ByteArrayOutputStream()
+            // With middle packets
+            firstPacket.size < BlePacket.MAX_SIZE                                ->
+                throw IncorrectPacketException(0, firstPacket)
 
-    fun start(payload: ByteArray): Int {
+            else                                                                 -> {
-        TODO("not implemented")
+                fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS, BlePacket.MAX_SIZE))
+            }
+        }
     }
 
-    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_SIZE) {
+                    throw IncorrectPacketException(idx.toByte(), packet)
+                }
+                fragments.add(packet.copyOfRange(1, BlePacket.MAX_SIZE))
+            }
+
+            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.toUnsignedLong()
+                val rest = packet[1].toInt()
+                val end = min(rest + LastBlePacket.HEADER_SIZE, BlePacket.MAX_SIZE)
+                oneExtraPacket = rest + LastBlePacket.HEADER_SIZE > end
+                if (packet.size < end) {
+                    throw IncorrectPacketException(idx.toByte(), packet)
+                }
+                fragments.add(packet.copyOfRange(LastBlePacket.HEADER_SIZE, packet.size))
+            }
+
+            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")
+
-    }
+internal fun Int.toUnsignedLong() = this.toLong() and 0xffffffffL
-}

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

@@ -25,6 +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(),
                 ))
             }
             return ret
@@ -53,9 +54,10 @@ internal class PayloadSplitter(private val payload: ByteArray) {
             payload = payload.copyOfRange(middleFragments * MiddleBlePacket.CAPACITY + FirstBlePacket.CAPACITY_WITH_MIDDLE_PACKETS, middleFragments * MiddleBlePacket.CAPACITY + FirstBlePacket.CAPACITY_WITH_MIDDLE_PACKETS + end),
             crc32 = crc32,
         ))
-        if (rest > 14) {
+        if (rest > LastBlePacket.CAPACITY) {
             ret.add(LastOptionalPlusOneBlePacket(
                 index = (middleFragments + 2).toByte(),
+                size = (rest - LastBlePacket.CAPACITY).toByte(),
                 payload = payload.copyOfRange(middleFragments * MiddleBlePacket.CAPACITY + FirstBlePacket.CAPACITY_WITH_MIDDLE_PACKETS + LastBlePacket.CAPACITY, payload.size),
             ))
         }
@@ -63,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/message/StringLengthPrefixEncoding.kt

@@ -1,5 +1,7 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
 
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.MessageIOException
+import info.nightscout.androidaps.utils.extensions.toHex
 import java.nio.ByteBuffer
 
 /***
@@ -9,8 +11,33 @@ class StringLengthPrefixEncoding {
 
     companion object {
 
-        fun parseKeys(keys: List<String>): List<ByteArray> {
+        private val LENGTH_BYTES = 2
-            TODO("not implemented")
+
+        fun parseKeys(keys: Array<String>, payload: ByteArray): Array<ByteArray> {
+            val ret = Array<ByteArray>(keys.size, { ByteArray(0) })
+            var remaining = payload
+            for ((index, key) in keys.withIndex()) {
+                when {
+                    remaining.size < key.length                                     ->
+                        throw MessageIOException("Payload too short: ${payload.toHex()} for key: ${key}")
+                    !(remaining.copyOfRange(0, key.length).decodeToString() == key) ->
+                        throw MessageIOException("Key not found: ${key} in ${payload.toHex()}")
+                    // last key can be empty, no length
+                    index == keys.size - 1 && remaining.size == key.length          ->
+                        return ret
+
+                    remaining.size < key.length + LENGTH_BYTES                      ->
+                        throw MessageIOException("Length not found: for ${key} in ${payload.toHex()}")
+                }
+                remaining = remaining.copyOfRange(key.length, remaining.size)
+                val length = (remaining[0].toUnsignedInt() shl 1) or remaining[1].toUnsignedInt()
+                if (length > remaining.size) {
+                    throw MessageIOException("Payload too short, looking for length ${length} for ${key} in ${payload.toHex()}")
+                }
+                ret[index] = remaining.copyOfRange(LENGTH_BYTES, LENGTH_BYTES + length)
+                remaining = remaining.copyOfRange(LENGTH_BYTES + length, remaining.size)
+            }
+            return ret
         }
 
         fun formatKeys(keys: Array<String>, payloads: Array<ByteArray>): ByteArray {

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

@@ -7,8 +7,8 @@ sealed class BlePacket {
     abstract fun asByteArray(): ByteArray
 
     companion object {
-        const val MAX_BLE_PACKET_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_SIZE = 20
     }
 }
 
@@ -16,7 +16,7 @@ data class FirstBlePacket(val totalFragments: Byte, val payload: ByteArray, val
 
     override fun asByteArray(): ByteArray {
         val bb = ByteBuffer
-            .allocate(MAX_BLE_BUFFER_LEN)
+            .allocate(MAX_SIZE)
             .put(0) // index
             .put(totalFragments) // # of fragments except FirstBlePacket and LastOptionalPlusOneBlePacket
         crc32?.let {
@@ -26,15 +26,22 @@ data class FirstBlePacket(val totalFragments: Byte, val payload: ByteArray, val
             bb.put(size)
         }
         bb.put(payload)
-        val ret = ByteArray(bb.position())
+
+        val pos = bb.position()
+        val ret = ByteArray(MAX_SIZE)
         bb.flip()
-        bb.get(ret)
+        bb.get(ret, 0, pos)
+
         return ret
     }
 
     companion object {
-        internal const val CAPACITY_WITHOUT_MIDDLE_PACKETS = 13 // 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_WITHOUT_MIDDLE_PACKETS = 7 // we are using all fields
+        internal const val HEADER_SIZE_WITH_MIDDLE_PACKETS = 2
+
+        internal const val CAPACITY_WITHOUT_MIDDLE_PACKETS = MAX_SIZE - HEADER_SIZE_WITHOUT_MIDDLE_PACKETS // we are using all fields
+        internal const val CAPACITY_WITH_MIDDLE_PACKETS = MAX_SIZE - HEADER_SIZE_WITH_MIDDLE_PACKETS // we are not using crc32 or size
         internal const val CAPACITY_WITH_THE_OPTIONAL_PLUS_ONE_PACKET = 18
     }
 }
@@ -46,6 +53,7 @@ data class MiddleBlePacket(val index: Byte, val payload: ByteArray) : BlePacket(
     }
 
     companion object {
+
         internal const val CAPACITY = 19
     }
 }
@@ -54,26 +62,34 @@ data class LastBlePacket(val index: Byte, val size: Byte, val payload: ByteArray
 
     override fun asByteArray(): ByteArray {
         val bb = ByteBuffer
-            .allocate(MAX_BLE_BUFFER_LEN)
+            .allocate(MAX_SIZE)
             .put(index)
             .put(size)
             .putInt(crc32.toInt())
             .put(payload)
-        val ret = ByteArray(bb.position())
+        val pos = bb.position()
+        val ret = ByteArray(MAX_SIZE)
         bb.flip()
-        bb.get(ret)
+        bb.get(ret, 0, pos)
         return ret
     }
 
     companion object {
-        internal const val CAPACITY = 14
+
+        internal const val HEADER_SIZE = 6
+        internal const val CAPACITY = MAX_SIZE - HEADER_SIZE
     }
 }
 
-data class LastOptionalPlusOneBlePacket(val index: Byte, val payload: ByteArray) : BlePacket() {
+data class LastOptionalPlusOneBlePacket(val index: Byte, val payload: ByteArray, val size: Byte) : BlePacket() {
 
     override fun asByteArray(): ByteArray {
-        return byteArrayOf(index) + payload
+        return byteArrayOf(index, size) + payload + ByteArray(MAX_SIZE - payload.size - 2)
+    }
+
+    companion object {
+
+        internal const val HEADER_SIZE = 2
     }
 }
 

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

@@ -0,0 +1,256 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.pair
+
+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
+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.exceptions.MessageIOException
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.MessageIO
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.MessagePacket
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.StringLengthPrefixEncoding
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.StringLengthPrefixEncoding.Companion.parseKeys
+import info.nightscout.androidaps.utils.extensions.hexStringToByteArray
+import info.nightscout.androidaps.utils.extensions.toHex
+import org.spongycastle.crypto.engines.AESEngine
+import org.spongycastle.crypto.macs.CMac
+import org.spongycastle.crypto.params.KeyParameter
+import java.security.SecureRandom
+
+internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgIO: MessageIO) {
+
+    private val pdmPrivate = X25519.generatePrivateKey()
+    private val pdmPublic = X25519.publicFromPrivate(pdmPrivate)
+    private var podPublic = ByteArray(PUBLIC_KEY_SIZE)
+    private var podNonce = ByteArray(NONCE_SIZE)
+    private val pdmNonce = ByteArray(NONCE_SIZE)
+    private val pdmConf = ByteArray(CMAC_SIZE)
+    private val podConf = ByteArray(CMAC_SIZE)
+    private val controllerId = Id.fromInt(OmnipodDashBleManagerImpl.CONTROLLER_ID)
+    val nodeId = controllerId.increment()
+    private var seq: Byte = 1
+    private var ltk = ByteArray(CMAC_SIZE)
+
+    init {
+        val random = SecureRandom()
+        random.nextBytes(pdmNonce)
+    }
+
+    fun negotiateLTK(): PairResult {
+        // send SP1, SP2
+        var sp1sp2 = sp1sp2(nodeId.address, sp2())
+        msgIO.sendMesssage(sp1sp2.messagePacket)
+
+        seq++
+        var sps1 = sps1()
+        msgIO.sendMesssage(sps1.messagePacket)
+        // send SPS1
+
+        // read SPS1
+        val podSps1 = msgIO.receiveMessage()
+        processSps1FromPod(podSps1)
+        // now we have all the data to generate: confPod, confPdm, ltk and noncePrefix
+        generateKeys()
+        seq++
+        // send SPS2
+        val sps2 = sps2()
+        msgIO.sendMesssage(sps2.messagePacket)
+        // read SPS2
+
+        val podSps2 = msgIO.receiveMessage()
+        validatePodSps2(podSps2)
+
+        seq++
+        // send SP0GP0
+        msgIO.sendMesssage(sp0gp0().messagePacket)
+        // read P0
+
+        // TODO: failing to read or validate p0 will lead to undefined state
+        // It could be that:
+        // - the pod answered with p0 and we did not receive/could not process the answer
+        // - the pod answered with some sort of error
+        // But if sps2 conf value is incorrect, then we would probablysee this when receiving the pod podSps2(to test)
+        val p0 = msgIO.receiveMessage()
+        validateP0(p0)
+
+        return PairResult(
+            ltk = ltk
+        )
+    }
+
+    private fun sp1sp2(sp1: ByteArray, sp2: ByteArray): PairMessage {
+        val payload = StringLengthPrefixEncoding.formatKeys(
+            arrayOf(SP1, SP2),
+            arrayOf(sp1, sp2),
+        )
+        return PairMessage(
+            sequenceNumber = seq,
+            source = controllerId,
+            destination = nodeId,
+            payload = payload
+        )
+    }
+
+    private fun sps1(): PairMessage {
+        val payload = StringLengthPrefixEncoding.formatKeys(
+            arrayOf("SPS1="),
+            arrayOf(pdmPublic + pdmNonce),
+        )
+        return PairMessage(
+            sequenceNumber = seq,
+            source = controllerId,
+            destination = nodeId,
+            payload = payload
+        )
+    }
+
+    private fun processSps1FromPod(msg: MessagePacket) {
+        aapsLogger.debug(LTag.PUMPBTCOMM, "Received SPS1 from pod: ${msg.payload.toHex()}")
+
+        val payload = parseKeys(arrayOf(SPS1), msg.payload)[0]
+        if (payload.size != 48) {
+            throw MessageIOException("Invalid payload size")
+        }
+        podPublic = payload.copyOfRange(0, PUBLIC_KEY_SIZE)
+        podNonce = payload.copyOfRange(PUBLIC_KEY_SIZE, PUBLIC_KEY_SIZE + NONCE_SIZE)
+    }
+
+    private fun sps2(): PairMessage {
+        val payload = StringLengthPrefixEncoding.formatKeys(
+            arrayOf(SPS2),
+            arrayOf(pdmConf),
+        )
+        return PairMessage(
+            sequenceNumber = seq,
+            source = controllerId,
+            destination = nodeId,
+            payload = payload
+        )
+    }
+
+    private fun validatePodSps2(msg: MessagePacket) {
+        aapsLogger.debug(LTag.PUMPBTCOMM, "Received SPS2 from pod: ${msg.payload.toHex()}")
+
+        val payload = parseKeys(arrayOf(SPS2), msg.payload)[0]
+        aapsLogger.debug(LTag.PUMPBTCOMM, "SPS2 payload from pod: ${payload.toHex()}")
+
+        if (payload.size != CMAC_SIZE) {
+            throw MessageIOException("Invalid payload size")
+        }
+        if (!podConf.contentEquals(payload)) {
+            aapsLogger.warn(LTag.PUMPBTCOMM, "Received invalid podConf. Expected: ${podConf.toHex()}. Got: ${payload.toHex()}")
+            throw MessageIOException("Invalid podConf value received")
+        }
+    }
+
+    private fun sp2(): ByteArray {
+        // This is GetPodStatus command, with page 0 parameter.
+        // We could replace that in the future with the serialized GetPodStatus()
+        return GET_POD_STATUS_HEX_COMMAND.hexStringToByteArray()
+    }
+
+    private fun sp0gp0(): PairMessage {
+        val payload = SP0GP0.toByteArray()
+        return PairMessage(
+            sequenceNumber = seq,
+            source = controllerId,
+            destination = nodeId,
+            payload = payload
+        )
+    }
+
+    private fun validateP0(msg: MessagePacket) {
+        aapsLogger.debug(LTag.PUMPBTCOMM, "Received P0 from pod: ${msg.payload.toHex()}")
+
+        val payload = parseKeys(arrayOf(P0), msg.payload)[0]
+        aapsLogger.debug(LTag.PUMPBTCOMM, "P0 payload from pod: ${payload.toHex()}")
+        if (!payload.contentEquals(UNKNOWN_P0_PAYLOAD)) {
+            throw MessageIOException("Invalid P0 payload received")
+        }
+    }
+
+    fun generateKeys() {
+        val curveLTK = X25519.computeSharedSecret(pdmPrivate, podPublic)
+
+        val firstKey = podPublic.copyOfRange(podPublic.size - 4, podPublic.size) +
+            pdmPublic.copyOfRange(pdmPublic.size - 4, pdmPublic.size) +
+            podNonce.copyOfRange(podNonce.size - 4, podNonce.size) +
+            pdmNonce.copyOfRange(pdmNonce.size - 4, pdmNonce.size)
+        aapsLogger.debug(LTag.PUMPBTCOMM, "LTK, first key: ${firstKey.toHex()}")
+
+        val intermediateKey = ByteArray(CMAC_SIZE)
+        aesCmac(firstKey, curveLTK, intermediateKey)
+
+        val ltkData = byteArrayOf(2.toByte()) +
+            INTERMEDIAR_KEY_MAGIC_STRING +
+            podNonce +
+            pdmNonce +
+            byteArrayOf(0.toByte(), 1.toByte())
+        aesCmac(intermediateKey, ltkData, ltk)
+
+        val confData = byteArrayOf(1.toByte()) +
+            INTERMEDIAR_KEY_MAGIC_STRING +
+            podNonce +
+            pdmNonce +
+            byteArrayOf(0.toByte(), 1.toByte())
+        val confKey = ByteArray(CMAC_SIZE)
+        aesCmac(intermediateKey, confData, confKey)
+
+        val pdmConfData = PDM_CONF_MAGIC_PREFIX +
+            pdmNonce +
+            podNonce
+        aesCmac(confKey, pdmConfData, pdmConf)
+        aapsLogger.debug(LTag.PUMPBTCOMM, "pdmConf: ${pdmConf.toHex()}")
+
+        val podConfData = POD_CONF_MAGIC_PREFIX +
+            podNonce +
+            pdmNonce
+        aesCmac(confKey, podConfData, podConf)
+        aapsLogger.debug(LTag.PUMPBTCOMM, "podConf: ${podConf.toHex()}")
+
+        if (BuildConfig.DEBUG) {
+            aapsLogger.debug(LTag.PUMPBTCOMM, "pdmPrivate: ${pdmPrivate.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "pdmPublic: ${pdmPublic.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "podPublic: ${podPublic.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "pdmNonce: ${pdmNonce.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "podNonce: ${podNonce.toHex()}")
+
+            aapsLogger.debug(LTag.PUMPBTCOMM, "LTK, donna key: ${curveLTK.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "Intermediate key: ${intermediateKey.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "LTK: ${ltk.toHex()}")
+            aapsLogger.debug(LTag.PUMPBTCOMM, "Conf KEY: ${confKey.toHex()}")
+        }
+    }
+
+    companion object {
+
+        private val PUBLIC_KEY_SIZE = 32
+        private val NONCE_SIZE = 16
+        private val CONF_SIZE = 16
+
+        private val CMAC_SIZE = 16
+
+        private val INTERMEDIAR_KEY_MAGIC_STRING = "TWIt".toByteArray()
+        private val PDM_CONF_MAGIC_PREFIX = "KC_2_U".toByteArray()
+        private val POD_CONF_MAGIC_PREFIX = "KC_2_V".toByteArray()
+
+        private val GET_POD_STATUS_HEX_COMMAND = "ffc32dbd08030e0100008a" // TODO for now we are assuming this command is build out of constant parameters, use a proper command builder for that.
+
+        private val SP1 = "SP1="
+        private val SP2 = ",SP2="
+        private val SPS1 = "SPS1="
+        private val SPS2 = "SPS2="
+        private val SP0GP0 = "SP0,GP0"
+        private val P0 = "P0="
+        private val UNKNOWN_P0_PAYLOAD = byteArrayOf(0xa5.toByte())
+    }
+}
+
+private fun aesCmac(key: ByteArray, data: ByteArray, result: ByteArray) {
+    val aesEngine = AESEngine()
+    val mac = CMac(aesEngine)
+    mac.init(KeyParameter(key))
+    mac.update(data, 0, data.size)
+    mac.doFinal(result, 0)
+}

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

@@ -1,4 +1,4 @@
-package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.pair
 
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.Id
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.MessagePacket

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

@@ -0,0 +1,9 @@
+package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.pair
+
+import info.nightscout.androidaps.utils.extensions.toHex
+
+data class PairResult(val ltk: ByteArray) {
+    init {
+        require(ltk.size == 16) { "LTK length must be 16 bytes. Received LTK: ${ltk.toHex()}" }
+    }
+}

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

@@ -31,7 +31,6 @@ class BleDiscoveredDevice(val scanResult: ScanResult, private val scanRecord: Sc
     @Throws(DiscoveredInvalidPodException::class)
 
     private fun validatePodId() {
-        val scanRecord = scanResult.scanRecord
         val serviceUUIDs = scanRecord.serviceUuids
         val hexPodId = extractUUID16(serviceUUIDs[3]) + extractUUID16(serviceUUIDs[4])
         val podId = hexPodId.toLong(16)

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/util/Functions.kt

@@ -33,7 +33,7 @@ fun mapProfileToBasalProgram(profile: Profile): BasalProgram {
         if (previousBasalValue != null) {
             entries.add(
                 BasalProgram.Segment(
-                    (previousBasalValue!!.timeAsSeconds / 1800).toShort(),
+                    (previousBasalValue.timeAsSeconds / 1800).toShort(),
                     startSlotIndex,
                     (PumpType.Omnipod_Dash.determineCorrectBasalSize(previousBasalValue.value) * 100).roundToInt()
                 )