ble: implement message reading&joining

Now are able to read the first message:
```
INFO[0005] Received SPS1  6b943ec06b594f8a0383f384a3c916da75e1c7846c3e1b73f72f86ee2dc48774b2b4e5ad62d798b76cfd06be1cd4c937
DEBU[0005] Donna LTK: b874cb3cbe487040442138452faeb02d284ac55f489f19593265ff52f7310f1f
DEBU[0005] First key 58cb3b742dc48774000000001cd4c937 :: 16
DEBU[0005] CMACY: 16
DEBU[0005] Intermediar key 4c13eebc4cf09795a07c50bf13786c18 :: 16
DEBU[0005] Pod public 2fe57da347cd62431528daac5fbb290730fff684afc4cfc2ed90995f58cb3b74 :: 32
DEBU[0005] Pod nonce 00000000000000000000000000000000 :: 16
DEBU[0005] Generated SPS1: 535053313d00302fe57da347cd62431528daac5fbb290730fff684afc4cfc2ed90995f58cb3b7400000000000000000000000000000000
TRAC[0005] CMD notification return: 4/00
TRAC[0005] received CMD:  01
TRAC[0005] Sending message: 54570003000006e00000109100001092535053313d00302fe57da347cd62431528daac5fbb290730fff684afc4cfc2ed90995f58cb3b7400000000000000000000000000000000
TRAC[0005] DATA notification return: 23/000354570003000006e000001091000010925350
TRAC[0005] DATA notification return: 23/0153313d00302fe57da347cd62431528daac5fbb
TRAC[0005] DATA notification return: 23/02290730fff684afc4cfc2ed90995f58cb3b7400
TRAC[0005] DATA notification return: 23/030f7d02931d0000000000000000000000000000
TRAC[0005] DATA notification return: 23/0401000000000000000000000000000000000000
TRAC[0005] received CMD:  04
```
This commit is contained in:
Andrei Vereha 2021-02-27 13:28:19 +01:00
parent 81ad52ebce
commit 1aa6d02893
11 changed files with 157 additions and 56 deletions

View file

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

View file

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

View file

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

View file

@ -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) {

View file

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

View file

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

View file

@ -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}")

View file

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

View file

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

View file

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

View file

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