From badf8fe24d47d24dc4d7a0d2d0b7b9fce8eeaf17 Mon Sep 17 00:00:00 2001
From: Andrei Vereha <avereha@gmail.com>
Date: Fri, 26 Feb 2021 16:52:55 +0100
Subject: [PATCH] ble: sending the first SP1/SP2 pairing command

from the fake pod logs
```
INFO[0007] Received SP1 SP2 payload 5350313d0004000010912c5350323d000bffc32dbd08030e0100008a
TRAC[0007] Read field: SP1= :: 00001091 :: 4
TRAC[0007] Read field: ,SP2= :: ffc32dbd08030e0100008a :: 11
INFO[0007] Received SP1 SP2: 00001091 :: ffc32dbd08030e0100008a
```
---
 .../pump/omnipod/dash/driver/comm/ltk/LTK.kt  |  2 +-
 .../dash/driver/comm/ltk/LTKExchanger.kt      | 10 +--
 .../dash/driver/comm/ltk/PairMessage.kt       |  1 -
 .../dash/driver/comm/message/MessageIO.kt     |  2 +-
 .../dash/driver/comm/message/MessagePacket.kt | 83 ++++++++++++-------
 .../message/StringLengthPrefixEncoding.kt     | 23 ++++-
 6 files changed, 83 insertions(+), 38 deletions(-)

diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTK.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTK.kt
index 3784ae9ffa..1f83940f2e 100644
--- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTK.kt
+++ b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/LTK.kt
@@ -4,7 +4,7 @@ import info.nightscout.androidaps.logging.AAPSLogger
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.BleIO
 
 data class LTK(val ltk: ByteArray, val noncePrefix: ByteArray) {
-    init{
+    init {
         require(ltk.size == 16)
         require(noncePrefix.size == 16)
     }
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 0d7e255543..526d9cfc42 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,13 +1,11 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.ltk
 
 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.message.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
-import info.nightscout.androidaps.utils.extensions.toHex
-import java.nio.ByteBuffer
 
 internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgIO: MessageIO) {
 
@@ -48,8 +46,10 @@ internal class LTKExchanger(private val aapsLogger: AAPSLogger, private val msgI
     }
 
     fun sp1sp2(sp1: ByteArray, sp2: ByteArray, seq: Byte, controllerId: Id, nodeId: Id): PairMessage {
-        val payload = "SP1=".toByteArray() + sp1
-        ",SP2=".toByteArray() + sp2
+        val payload = StringLengthPrefixEncoding.formatKeys(
+            arrayOf("SP1=", ",SP2="),
+            arrayOf(sp1, sp2),
+        )
         return PairMessage(
             sequenceNumber = seq,
             source = controllerId,
diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/PairMessage.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/PairMessage.kt
index bb88c60831..99466e3b4e 100644
--- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/PairMessage.kt
+++ b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ltk/PairMessage.kt
@@ -13,7 +13,6 @@ data class PairMessage(
         type=MessageType.PAIRING,
         source=source,
         destination = destination,
-        encryptedPayload=false,
         payload=payload,
         sequenceNumber=sequenceNumber,
         sas=true, // TODO: understand why this is true for PairMessages
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 73f31a7f62..d64d693edc 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
@@ -26,7 +26,7 @@ class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
         }
         // TODO: peek for NACKs
         val expectSuccess = bleIO.receivePacket(CharacteristicType.CMD)
-        if (BleCommand(expectSuccess) != BleCommandCTS()) {
+        if (BleCommand(expectSuccess) != BleCommandSuccess()) {
             throw UnexpectedCommandException(BleCommand(expectSuccess))
         }
         // TODO: handle NACKS/FAILS/etc
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 a918a2b551..f18cbc9369 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,34 +1,10 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
 
-import java.io.DataOutput
 import java.nio.ByteBuffer
 
 /***
  * MessagePacket contains header and raw payload for a message
  */
-
-class Flag:Byte{
-    fun set(idx: Byte, val: Boolean) {
-        val mask = 1 <lsh (7-idx)
-
-    }
-func (f *flag) set(index byte, val bool) {
-	var mask flag = 1 << (7 - index)
-	if !val {
-		return
-	}
-	*f |= mask
-}
-
-func (f flag) get(index byte) byte {
-	var mask flag = 1 << (7 - index)
-	if f&mask == 0 {
-		return 0
-	}
-	return 1
-}
-}
-
 data class MessagePacket(
     val type: MessageType,
     val source: Id,
@@ -46,19 +22,70 @@ data class MessagePacket(
     val version: Short = 0.toShort()) {
 
     fun asByteArray(): ByteArray {
-        val v = ByteBuffer.allocate(16+ payload.size)
-        v.put(MAGIC_PATTERN.toByteArray())
-        val f1: Byte=0
+        val bb = ByteBuffer.allocate(16 + payload.size)
+        bb.put(MAGIC_PATTERN.toByteArray())
 
+        val f1 = Flag()
+        f1.set(0, this.version.toInt() and 4 != 0)
+        f1.set(1, this.version.toInt() and 2 != 0)
+        f1.set(2, this.version.toInt() and 1 != 0)
+        f1.set(3, this.sas)
+        f1.set(4, this.tfs)
+        f1.set(5, this.eqos.toInt() and 4 != 0)
+        f1.set(6, this.eqos.toInt() and 2 != 0)
+        f1.set(7, this.eqos.toInt() and 1 != 0)
 
-        return this.payload
+        val f2 = Flag()
+        f2.set(0, this.ack)
+        f2.set(1, this.priority)
+        f2.set(2, this.lastMessage)
+        f2.set(3, this.gateway)
+        f2.set(4, this.type.value.toInt() and 8 != 0)
+        f2.set(5, this.type.value.toInt() and 4 != 0)
+        f2.set(6, this.type.value.toInt() and 2 != 0)
+        f2.set(7, this.type.value.toInt() and 1 != 0)
+
+        bb.put(f1.value.toByte())
+        bb.put(f2.value.toByte())
+        bb.put(this.sequenceNumber)
+        bb.put(this.ackNumber)
+
+        bb.put((this.payload.size ushr 3).toByte())
+        bb.put((this.payload.size shl 5).toByte())
+
+        bb.put(this.source.address)
+        bb.put(this.destination.address)
+
+        bb.put(this.payload)
+
+        val ret = ByteArray(bb.position())
+        bb.flip()
+        bb.get(ret)
+
+        return ret
     }
 
     companion object {
+
         private val MAGIC_PATTERN = "TW" // all messages start with this string
 
         fun parse(payload: ByteArray): MessagePacket {
             TODO("implement message header parsing")
         }
     }
+}
+
+private class Flag(var value: Int=0) {
+
+    fun set(idx: Byte, set: Boolean) {
+        val mask = 1 shl (7 - idx)
+        if (!set)
+            return
+        value = value or mask
+    }
+
+    fun get(idx: Byte): Boolean {
+        val mask = 1 shl (7 - idx)
+        return value and mask != 0
+    }
 }
\ No newline at end of file
diff --git a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/StringLengthPrefixEncoding.kt b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/StringLengthPrefixEncoding.kt
index d0c77d6662..b23e689d9d 100644
--- a/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/StringLengthPrefixEncoding.kt
+++ b/omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/StringLengthPrefixEncoding.kt
@@ -1,17 +1,36 @@
 package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
 
+import java.nio.ByteBuffer
+
 /***
  *  String prefix and length encoding and decoding. Example message:
  */
 class StringLengthPrefixEncoding {
 
     companion object {
+
         fun parseKeys(keys: List<String>): List<ByteArray> {
             TODO("not implemented")
         }
 
-        fun formatKeys(keys: List<String>, payload: List<ByteArray>, addLength: Boolean): ByteArray {
-            TODO("not implemented")
+        fun formatKeys(keys: Array<String>, payloads: Array<ByteArray>): ByteArray {
+            val payloadTotalSize = payloads.fold(0) { acc, i -> acc + i.size }
+            val keyTotalSize = keys.fold(0) { acc, i -> acc + i.length }
+
+            val bb = ByteBuffer.allocate(2 * keys.size + keyTotalSize + payloadTotalSize)
+            for (idx in keys.indices) {
+                val k = keys[idx]
+                val payload = payloads[idx]
+                bb.put(k.toByteArray())
+                bb.putShort(payload.size.toShort())
+                bb.put(payload)
+            }
+
+            val ret = ByteArray(bb.position())
+            bb.flip()
+            bb.get(ret)
+
+            return ret
         }
     }
 }
\ No newline at end of file