Finish implementing the LTK exchange

On the fake pod:
```
INFO[0067] LTK 21f4a9d825ce5e57bad5b4958c6ff95e
```
Logcat:
```
2021-02-28 18:21:04.763 19530-22490/info.nightscout.androidaps I/PUMPCOMM: [AsyncTask #4]: [OmnipodDashBleManagerImpl.connect():86]: Got LTK: 21f4a9d825ce5e57bad5b4958c6ff95e
``

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

@@ -21,6 +21,7 @@ import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.event.PodEven
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.pod.command.base.Command
 import io.reactivex.Observable
 import org.apache.commons.lang3.NotImplementedException
+import info.nightscout.androidaps.utils.extensions.toHex
 import java.util.concurrent.BlockingQueue
 import java.util.concurrent.LinkedBlockingDeque
 import java.util.concurrent.TimeoutException
@@ -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()
@@ -110,14 +113,9 @@ 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
+        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/ltk/LTK.kt

@@ -1,8 +1,7 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
 
-data class LTK(val ltk: ByteArray, val noncePrefix: ByteArray) {
+data class LTK(val ltk: 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

@@ -3,12 +3,14 @@ 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.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
@@ -23,21 +25,19 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
     private var podPublic = ByteArray(PUBLIC_KEY_SIZE)
     private var podNonce = ByteArray(NONCE_SIZE)
     private val pdmNonce = ByteArray(NONCE_SIZE)
-    private val confPdm = ByteArray(CONF_SIZE)
+    private val pdmConf = ByteArray(CMAC_SIZE)
-    private val confPod = ByteArray(CONF_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)
-    private var noncePrefix = ByteArray(0)
 
     init {
         val random = SecureRandom()
         random.nextBytes(pdmNonce)
     }
 
-    fun negotiateLTKAndNonce(): LTK? {
+    fun negotiateLTK(): LTK {
         // send SP1, SP2
         var sp1sp2 = sp1sp2(nodeId.address, sp2())
         msgIO.sendMesssage(sp1sp2.messagePacket)
@@ -49,7 +49,6 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
 
         // read SPS1
         val podSps1 = msgIO.receiveMessage()
-        aapsLogger.info(LTag.PUMPBTCOMM, "Received message: %s", podSps1)
         processSps1FromPod(podSps1)
         // now we have all the data to generate: confPod, confPdm, ltk and noncePrefix
         generateKeys()
@@ -67,22 +66,22 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
         msgIO.sendMesssage(sp0gp0().messagePacket)
         // read P0
 
-        //TODO: if we fail to read or validate p0 will lead to undefined state
+        // TODO: failing to read or validate p0 will lead to undefined state
-        // it could be that:
+        // 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 LTK(
             ltk = ltk,
-            noncePrefix = noncePrefix,
         )
     }
 
     private fun sp1sp2(sp1: ByteArray, sp2: ByteArray): PairMessage {
         val payload = StringLengthPrefixEncoding.formatKeys(
-            arrayOf("SP1=", ",SP2="),
+            arrayOf(SP1, SP2),
             arrayOf(sp1, sp2),
         )
         return PairMessage(
@@ -94,10 +93,9 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
     }
 
     private fun sps1(): PairMessage {
-        val publicKey = X25519.publicFromPrivate(pdmPrivate)
         val payload = StringLengthPrefixEncoding.formatKeys(
             arrayOf("SPS1="),
-            arrayOf(publicKey + pdmNonce),
+            arrayOf(pdmPublic + pdmNonce),
         )
         return PairMessage(
             sequenceNumber = seq,
@@ -109,20 +107,41 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
 
     private fun processSps1FromPod(msg: MessagePacket) {
         aapsLogger.debug(LTag.PUMPBTCOMM, "Received SPS1 from pod: ${msg.payload.toHex()}")
-        if (msg.payload.size != 48) {
+
+        val payload = parseKeys(arrayOf(SPS1), msg.payload)[0]
+        if (payload.size != 48) {
             throw MessageIOException("Invalid payload size")
         }
-        podPublic = msg.payload.copyOfRange(0, PUBLIC_KEY_SIZE)
+        podPublic = payload.copyOfRange(0, PUBLIC_KEY_SIZE)
-        podNonce = msg.payload.copyOfRange(PUBLIC_KEY_SIZE, PUBLIC_KEY_SIZE + NONCE_SIZE)
+        podNonce = payload.copyOfRange(PUBLIC_KEY_SIZE, PUBLIC_KEY_SIZE + NONCE_SIZE)
     }
 
     private fun sps2(): PairMessage {
-        TODO("implement")
+        val payload = StringLengthPrefixEncoding.formatKeys(
+            arrayOf(SPS2),
+            arrayOf(pdmConf),
+        )
+        return PairMessage(
+            sequenceNumber = seq,
+            source = controllerId,
+            destination = nodeId,
+            payload = payload,
+        )
     }
 
-    private fun validatePodSps2(podSps2: MessagePacket) {
+    private fun validatePodSps2(msg: MessagePacket) {
-        TODO("implement")
+        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 {
@@ -132,39 +151,76 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
     }
 
     private fun sp0gp0(): PairMessage {
-        TODO("implement")
+        val payload = SP0GP0.toByteArray()
+        return PairMessage(
+            sequenceNumber = seq,
+            source = controllerId,
+            destination = nodeId,
+            payload = payload,
+        )
     }
 
-    private fun validateP0(p0: MessagePacket) {
+    private fun validateP0(msg: MessagePacket) {
-        TODO("implement")
+        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)
-        aapsLogger.debug(LTag.PUMPBTCOMM, "LTK, donna key: ${curveLTK.toHex()}")
 
-        //first_key = data.pod_public[-4:] + data.pdm_public[-4:] + data.pod_nonce[-4:] + data.pdm_nonce[-4:]
         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 aesEngine = AESEngine()
-        val intermediateMac = CMac(aesEngine)
-        intermediateMac.init(KeyParameter(firstKey))
-        intermediateMac.update(curveLTK, 0, curveLTK.size)
         val intermediateKey = ByteArray(CMAC_SIZE)
-        intermediateMac.doFinal(intermediateKey, 0)
+        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()}")
-
-        val ltkMac = CMac(aesEngine)
-        ltkMac.init(KeyParameter(firstKey))
-        ltkMac.update(curveLTK, 0, curveLTK.size)
-        intermediateMac.doFinal(ltk, 0)
             aapsLogger.debug(LTag.PUMPBTCOMM, "LTK: ${ltk.toHex()}")
-
+            aapsLogger.debug(LTag.PUMPBTCOMM, "Conf KEY: ${confKey.toHex()}")
+        }
     }
 
     companion object {
@@ -172,7 +228,29 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
         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/message/MessageIO.kt

@@ -47,7 +47,7 @@ class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
             for (i in 1 until joiner.fullFragments + 1) {
                 joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
             }
-            if (joiner.oneExtra) {
+            if (joiner.oneExtraPacket) {
                 joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
             }
             val fullPayload = joiner.finalize()

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

@@ -76,8 +76,8 @@ data class MessagePacket(
             if (payload.size < HEADER_SIZE) {
                 throw CouldNotParseMessageException(payload)
             }
-            if (payload.copyOfRange(0, 2).toString() != "TW") {
+            if (payload.copyOfRange(0, 2).decodeToString() != MAGIC_PATTERN) {
-                throw info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.CouldNotParseMessageException(payload)
+                throw CouldNotParseMessageException(payload)
             }
             val f1 = Flag(payload[2].toInt())
             val sas = f1.get(3) != 0
@@ -96,7 +96,7 @@ data class MessagePacket(
             }
             val sequenceNumber = payload[4]
             val ackNumber = payload[5]
-            val size = (payload[6].toInt() shl 3) or (payload[7].toInt() ushr 5)
+            val size = (payload[6].toInt() shl 3) or (payload[7].toUnsignedInt() ushr 5)
             if (size + HEADER_SIZE > payload.size) {
                 throw CouldNotParseMessageException(payload)
             }
@@ -114,8 +114,8 @@ data class MessagePacket(
                 sas = sas,
                 tfs = tfs,
                 version = version,
-                sequenceNumber = payload[4],
+                sequenceNumber = sequenceNumber,
-                ackNumber = payload[5],
+                ackNumber = ackNumber,
                 source = Id(payload.copyOfRange(8, 12)),
                 destination = Id(payload.copyOfRange(12, 16)),
                 payload = payload.copyOfRange(16, payloadEnd),
@@ -142,3 +142,5 @@ private class Flag(var value: Int = 0) {
 
     }
 }
+
+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

@@ -13,14 +13,14 @@ import java.util.*
 
 class PayloadJoiner(private val firstPacket: ByteArray) {
 
-    var oneExtra: Boolean = false
+    var oneExtraPacket: Boolean = false
     val fullFragments: Int
     var crc: Long = 0
     private var expectedIndex = 0
     private val fragments: LinkedList<ByteArray> = LinkedList<ByteArray>()
 
     init {
-        if (firstPacket.size < 2) {
+        if (firstPacket.size < FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS) {
             throw IncorrectPacketException(0, firstPacket)
         }
         fullFragments = firstPacket[1].toInt()
@@ -32,20 +32,20 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
             fullFragments == 0                                                   -> {
                 crc = ByteBuffer.wrap(firstPacket.copyOfRange(2, 6)).int.toUnsignedLong()
                 val rest = firstPacket[6]
-                val end = min(rest + 7, BlePacket.MAX_LEN)
+                val end = min(rest + FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS, BlePacket.MAX_SIZE)
-                oneExtra = rest + 7 > end
+                oneExtraPacket = rest + FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS > end
-                fragments.add(firstPacket.copyOfRange(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))
             }
 
             // With middle packets
-            firstPacket.size < BlePacket.MAX_LEN                                 ->
+            firstPacket.size < BlePacket.MAX_SIZE                                ->
                 throw IncorrectPacketException(0, firstPacket)
 
             else                                                                 -> {
-                fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS, BlePacket.MAX_LEN))
+                fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS, BlePacket.MAX_SIZE))
             }
         }
     }
@@ -61,10 +61,10 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
         expectedIndex++
         when {
             idx < fullFragments  -> { // this is a middle fragment
-                if (packet.size < BlePacket.MAX_LEN) {
+                if (packet.size < BlePacket.MAX_SIZE) {
                     throw IncorrectPacketException(idx.toByte(), packet)
                 }
-                fragments.add(packet.copyOfRange(1, BlePacket.MAX_LEN))
+                fragments.add(packet.copyOfRange(1, BlePacket.MAX_SIZE))
             }
 
             idx == fullFragments -> { // this is the last fragment
@@ -73,12 +73,12 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
                 }
                 crc = ByteBuffer.wrap(packet.copyOfRange(2, 6)).int.toUnsignedLong()
                 val rest = packet[1].toInt()
-                val end = min(rest, BlePacket.MAX_LEN)
+                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)
                 }
-                oneExtra = rest + LastBlePacket.HEADER_SIZE > end
+                fragments.add(packet.copyOfRange(LastBlePacket.HEADER_SIZE, packet.size))
-                fragments.add(packet.copyOfRange(LastBlePacket.HEADER_SIZE, BlePacket.MAX_LEN))
             }
 
             idx > fullFragments  -> { // this is the extra fragment
@@ -106,4 +106,4 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
 
 }
 
-private fun Int.toUnsignedLong() = this.toLong() and 0xffffffffL
+internal fun Int.toUnsignedLong() = this.toLong() and 0xffffffffL

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

@@ -8,8 +8,7 @@ sealed class BlePacket {
 
     companion object {
 
-        const val MAX_LEN = 20
+        const val MAX_SIZE = 20
-        const val MAX_BLE_BUFFER_LEN = MAX_LEN + 1 // we use this as the size allocated for the ByteBuffer
     }
 }
 
@@ -17,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 {
@@ -27,9 +26,12 @@ 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
     }
 
@@ -38,8 +40,8 @@ data class FirstBlePacket(val totalFragments: Byte, val payload: ByteArray, val
         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_LEN - HEADER_SIZE_WITHOUT_MIDDLE_PACKETS // we are using all fields
+        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_LEN - HEADER_SIZE_WITH_MIDDLE_PACKETS // we are not using crc32 or size
+        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
     }
 }
@@ -60,34 +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 HEADER_SIZE = 6
-        internal const val CAPACITY = MAX_LEN - HEADER_SIZE
+        internal const val CAPACITY = MAX_SIZE - HEADER_SIZE
     }
 }
 
 data class LastOptionalPlusOneBlePacket(val index: Byte, val payload: ByteArray, val size: Byte) : BlePacket() {
 
     override fun asByteArray(): ByteArray {
-        return byteArrayOf(index, size) + payload
+        return byteArrayOf(index, size) + payload + ByteArray(MAX_SIZE - payload.size - 2)
     }
 
     companion object {
 
         internal const val HEADER_SIZE = 2
-        internal const val CAPACITY = MAX_LEN - HEADER_SIZE
     }
 }
 

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()
                 )