ble ltk: implement encryption and message parsing

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

@@ -1,3 +1,6 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions
 
-class MessageIOException(override val cause: Throwable) : Exception(cause)
+class MessageIOException : Exception {
+    constructor(msg: String): super(msg)
+    constructor(cause: Throwable): super(cause)
+}

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

@@ -1,9 +1,6 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
 
-import com.google.crypto.tink.mac.AesCmacKeyManager
-import com.google.crypto.tink.proto.AesCmac
 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.Id
@@ -15,6 +12,8 @@ import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message.
 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) {
@@ -30,7 +29,7 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
     private val controllerId = Id.fromInt(OmnipodDashBleManagerImpl.CONTROLLER_ID)
     val nodeId = controllerId.increment()
     private var seq: Byte = 1
-    private var ltk = ByteArray(0)
+    private var ltk = ByteArray(CMAC_SIZE)
     private var noncePrefix = ByteArray(0)
 
     init {
@@ -98,7 +97,7 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
         val publicKey = X25519.publicFromPrivate(pdmPrivate)
         val payload = StringLengthPrefixEncoding.formatKeys(
             arrayOf("SPS1="),
-            arrayOf(publicKey + nonce),
+            arrayOf(publicKey + pdmNonce),
         )
         return PairMessage(
             sequenceNumber = seq,
@@ -111,10 +110,10 @@ 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) {
-            throw MessageIOException()
+            throw MessageIOException("Invalid payload size")
         }
         podPublic = msg.payload.copyOfRange(0, PUBLIC_KEY_SIZE)
-        podNonce = msg.payload.copyOfRange(PUBLIC_KEY_SIZE, PUBLIC_KEY_SIZE+ NONCE_SIZE)
+        podNonce = msg.payload.copyOfRange(PUBLIC_KEY_SIZE, PUBLIC_KEY_SIZE + NONCE_SIZE)
     }
 
     private fun sps2(): PairMessage {
@@ -144,19 +143,36 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
     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)
+        //first_key = data.pod_public[-4:] + data.pdm_public[-4:] + data.pod_nonce[-4:] + data.pdm_nonce[-4:]
-            + pdmPublic.copyOfRange(pdmPublic.size-4, pdmPublic.size)
+        val firstKey = podPublic.copyOfRange(podPublic.size - 4, podPublic.size) +
-            + podNonce.copyOfRange(podNonce.size-4, podNonce.size)
+            pdmPublic.copyOfRange(pdmPublic.size - 4, pdmPublic.size)+
-            + pdmNonce.copyOfRange(pdmNonce.size-4, pdmNonce.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)
+        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()}")
+
     }
 
     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 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/MessageIO.kt

@@ -36,7 +36,6 @@ 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()) {

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).toString() != "TW") {
+                throw info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.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].toInt() 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 = payload[4],
+                ackNumber = payload[5],
+                source = Id(payload.copyOfRange(8, 12)),
+                destination = Id(payload.copyOfRange(12, 16)),
+                payload = payload.copyOfRange(16, payloadEnd),
+            )
         }
     }
 }
@@ -85,8 +133,12 @@ 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
+
     }
 }

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

@@ -11,7 +11,6 @@ import java.lang.Integer.min
 import java.nio.ByteBuffer
 import java.util.*
 
-@ExperimentalUnsignedTypes
 class PayloadJoiner(private val firstPacket: ByteArray) {
 
     var oneExtra: Boolean = false
@@ -30,8 +29,8 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
             firstPacket.size < FirstBlePacket.HEADER_SIZE_WITHOUT_MIDDLE_PACKETS ->
                 throw IncorrectPacketException(0, firstPacket)
 
-            fullFragments == 0                   -> {
+            fullFragments == 0                                                   -> {
-                crc = ByteBuffer.wrap(firstPacket.copyOfRange(2, 6)).int.toUInt().toLong()
+                crc = ByteBuffer.wrap(firstPacket.copyOfRange(2, 6)).int.toUnsignedLong()
                 val rest = firstPacket[6]
                 val end = min(rest + 7, BlePacket.MAX_LEN)
                 oneExtra = rest + 7 > end
@@ -42,10 +41,10 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
             }
 
             // With middle packets
-            firstPacket.size < BlePacket.MAX_LEN ->
+            firstPacket.size < BlePacket.MAX_LEN                                 ->
                 throw IncorrectPacketException(0, firstPacket)
 
-            else                                 -> {
+            else                                                                 -> {
                 fragments.add(firstPacket.copyOfRange(FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS, BlePacket.MAX_LEN))
             }
         }
@@ -72,7 +71,7 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
                 if (packet.size < LastBlePacket.HEADER_SIZE) {
                     throw IncorrectPacketException(idx.toByte(), packet)
                 }
-                crc = ByteBuffer.wrap(packet.copyOfRange(2, 6)).int.toUInt().toLong()
+                crc = ByteBuffer.wrap(packet.copyOfRange(2, 6)).int.toUnsignedLong()
                 val rest = packet[1].toInt()
                 val end = min(rest, BlePacket.MAX_LEN)
                 if (packet.size < end) {
@@ -105,4 +104,6 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
         return bytes.copyOfRange(0, bytes.size)
     }
 
-}
+}
+
+private fun Int.toUnsignedLong() = this.toLong() and 0xffffffffL