From ddb1c1834935fd1353dea1e41bfa20bd318d0093 Mon Sep 17 00:00:00 2001 From: Andrei Vereha Date: Sat, 27 Feb 2021 20:45:01 +0100 Subject: [PATCH] ble ltk: implement encryption and message parsing --- .../CouldNotParseMessageException.kt | 5 ++ .../comm/exceptions/MessageIOException.kt | 5 +- .../dash/driver/comm/ltk/LTKExchanger.kt | 40 +++++++++---- .../dash/driver/comm/message/MessageIO.kt | 1 - .../dash/driver/comm/message/MessagePacket.kt | 58 ++++++++++++++++++- .../dash/driver/comm/message/PayloadJoiner.kt | 15 ++--- 6 files changed, 100 insertions(+), 24 deletions(-) create mode 100644 omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/CouldNotParseMessageException.kt diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/CouldNotParseMessageException.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/CouldNotParseMessageException.kt new file mode 100644 index 0000000000..65df8175e6 --- /dev/null +++ b/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()}") \ No newline at end of file diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/MessageIOException.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/MessageIOException.kt index c5d3d0ead1..ea4201bc02 100644 --- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/exceptions/MessageIOException.kt +++ b/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) \ No newline at end of file +class MessageIOException : Exception { + constructor(msg: String): super(msg) + constructor(cause: Throwable): super(cause) +} \ No newline at end of file diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTKExchanger.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTKExchanger.kt index 455f0f1575..cd9dde9b41 100644 --- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTKExchanger.kt +++ b/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) - + pdmPublic.copyOfRange(pdmPublic.size-4, pdmPublic.size) - + podNonce.copyOfRange(podNonce.size-4, podNonce.size) - + pdmNonce.copyOfRange(pdmNonce.size-4, pdmNonce.size) + //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) + 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. } } \ No newline at end of file diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessageIO.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessageIO.kt index 88e6a5dde4..4b67f33167 100644 --- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessageIO.kt +++ b/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()) { diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessagePacket.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessagePacket.kt index d9cd686b4c..3563745ffe 100644 --- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessagePacket.kt +++ b/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 + } } \ No newline at end of file diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/PayloadJoiner.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/PayloadJoiner.kt index 51af76eef1..9b383b5f22 100644 --- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/PayloadJoiner.kt +++ b/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 -> { - crc = ByteBuffer.wrap(firstPacket.copyOfRange(2, 6)).int.toUInt().toLong() + fullFragments == 0 -> { + 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) } -} \ No newline at end of file +} + +private fun Int.toUnsignedLong() = this.toLong() and 0xffffffffL \ No newline at end of file