fix Dana 5 BLE comm

This commit is contained in:
Milos Kozak 2021-05-31 18:03:57 +02:00
parent 596012ab10
commit ffde47d531
8 changed files with 195 additions and 289 deletions

View file

@ -1,248 +0,0 @@
package info.nightscout.androidaps.danars.comm;
import android.annotation.TargetApi;
import android.os.Build;
import org.joda.time.DateTime;
import java.nio.charset.StandardCharsets;
import javax.inject.Inject;
import dagger.android.HasAndroidInjector;
import info.nightscout.androidaps.danars.encryption.BleEncryption;
import info.nightscout.androidaps.logging.AAPSLogger;
import info.nightscout.androidaps.logging.LTag;
import info.nightscout.androidaps.utils.DateUtil;
public class DanaRS_Packet {
@Inject AAPSLogger aapsLogger;
@Inject DateUtil dateUtil;
protected HasAndroidInjector injector;
private static final int TYPE_START = 0;
private static final int OPCODE_START = 1;
public static final int DATA_START = 2;
private boolean received;
public boolean failed;
protected int type = BleEncryption.DANAR_PACKET__TYPE_RESPONSE; // most of the messages, should be changed for others
protected int opCode;
public DanaRS_Packet(HasAndroidInjector injector) {
received = false;
failed = false;
this.injector = injector;
injector.androidInjector().inject(this);
}
public boolean success() {
return !failed;
}
public void setReceived() {
received = true;
}
public boolean isReceived() {
return received;
}
public int getType() {
return type;
}
public int getOpCode() {
return opCode;
}
public int getCommand() {
return ((type & 0xFF) << 8) + (opCode & 0xFF);
}
public byte[] getRequestParams() {
return null;
}
// STATIC FUNCTIONS
public int getCommand(byte[] data) {
int type = byteArrayToInt(getBytes(data, TYPE_START, 1));
int opCode = byteArrayToInt(getBytes(data, OPCODE_START, 1));
return ((type & 0xFF) << 8) + (opCode & 0xFF);
}
public void handleMessage(byte[] data) {
}
public void handleMessageNotReceived() {
failed = true;
}
public String getFriendlyName() {
return "UNKNOWN_PACKET";
}
protected byte[] getBytes(byte[] data, int srcStart, int srcLength) {
try {
byte[] ret = new byte[srcLength];
System.arraycopy(data, srcStart, ret, 0, srcLength);
return ret;
} catch (Exception e) {
aapsLogger.error(LTag.PUMPBTCOMM, "Unhandled exception", e);
}
return null;
}
protected static int byteArrayToInt(byte[] b) {
int ret;
switch (b.length) {
case 1:
ret = b[0] & 0x000000FF;
break;
case 2:
ret = ((b[1] & 0x000000FF) << 8) + (b[0] & 0x000000FF);
break;
case 3:
ret = ((b[2] & 0x000000FF) << 16) + ((b[1] & 0x000000FF) << 8) + (b[0] & 0x000000FF);
break;
case 4:
ret = ((b[3] & 0x000000FF) << 24) + ((b[2] & 0x000000FF) << 16) + ((b[1] & 0x000000FF) << 8) + (b[0] & 0x000000FF);
break;
default:
ret = -1;
break;
}
return ret;
}
public static synchronized long dateTimeSecFromBuff(byte[] buff, int offset) {
return
new DateTime(
2000 + intFromBuff(buff, offset, 1),
intFromBuff(buff, offset + 1, 1),
intFromBuff(buff, offset + 2, 1),
intFromBuff(buff, offset + 3, 1),
intFromBuff(buff, offset + 4, 1),
intFromBuff(buff, offset + 5, 1)
).getMillis();
}
protected static int intFromBuff(byte[] b, int srcStart, int srcLength) {
int ret;
switch (srcLength) {
case 1:
ret = b[DATA_START + srcStart + 0] & 0x000000FF;
break;
case 2:
ret = ((b[DATA_START + srcStart + 1] & 0x000000FF) << 8) + (b[DATA_START + srcStart + 0] & 0x000000FF);
break;
case 3:
ret = ((b[DATA_START + srcStart + 2] & 0x000000FF) << 16) + ((b[DATA_START + srcStart + 1] & 0x000000FF) << 8) + (b[DATA_START + srcStart + 0] & 0x000000FF);
break;
case 4:
ret = ((b[DATA_START + srcStart + 3] & 0x000000FF) << 24) + ((b[DATA_START + srcStart + 2] & 0x000000FF) << 16) + ((b[DATA_START + srcStart + 1] & 0x000000FF) << 8) + (b[DATA_START + srcStart + 0] & 0x000000FF);
break;
default:
ret = -1;
break;
}
return ret;
}
protected static int intFromBuffMsbLsb(byte[] b, int srcStart, int srcLength) {
int ret;
switch (srcLength) {
case 1:
ret = b[DATA_START + srcStart] & 0x000000FF;
break;
case 2:
ret = ((b[DATA_START + srcStart] & 0x000000FF) << 8) + (b[DATA_START + srcStart + 1] & 0x000000FF);
break;
case 3:
ret = ((b[DATA_START + srcStart] & 0x000000FF) << 16) + ((b[DATA_START + srcStart + 1] & 0x000000FF) << 8) + (b[DATA_START + srcStart + 2] & 0x000000FF);
break;
case 4:
ret = ((b[DATA_START + srcStart] & 0x000000FF) << 24) + ((b[DATA_START + srcStart + 1] & 0x000000FF) << 16) + ((b[DATA_START + srcStart + 2] & 0x000000FF) << 8) + (b[DATA_START + srcStart + 3] & 0x000000FF);
break;
default:
ret = -1;
break;
}
return ret;
}
@TargetApi(Build.VERSION_CODES.KITKAT)
public static String stringFromBuff(byte[] buff, int offset, int length) {
byte[] strbuff = new byte[length];
System.arraycopy(buff, offset, strbuff, 0, length);
return new String(strbuff, StandardCharsets.UTF_8);
}
public long dateFromBuff(byte[] buff, int offset) {
return
new DateTime(
2000 + byteArrayToInt(getBytes(buff, offset, 1)),
byteArrayToInt(getBytes(buff, offset + 1, 1)),
byteArrayToInt(getBytes(buff, offset + 2, 1)),
0,
0
).getMillis();
}
@TargetApi(Build.VERSION_CODES.KITKAT)
public static String asciiStringFromBuff(byte[] buff, int offset, int length) {
byte[] strbuff = new byte[length];
System.arraycopy(buff, offset, strbuff, 0, length);
return new String(strbuff, StandardCharsets.UTF_8);
}
public static String toHexString(byte[] buff) {
if (buff == null)
return "";
StringBuilder sb = new StringBuilder();
int count = 0;
for (byte element : buff) {
sb.append(String.format("%02X ", element));
if (++count % 4 == 0) sb.append(" ");
}
return sb.toString();
}
final private static char[] hexArray = "0123456789ABCDEF".toCharArray();
public static String bytesToHex(byte[] bytes) {
char[] hexChars = new char[bytes.length * 2];
for (int j = 0; j < bytes.length; j++) {
int v = bytes[j] & 0xFF;
hexChars[j * 2] = hexArray[v >>> 4];
hexChars[j * 2 + 1] = hexArray[v & 0x0F];
}
return new String(hexChars);
}
public static byte[] hexToBytes(String s) {
int len = s.length();
byte[] data = new byte[len / 2];
for (int i = 0; i < len; i += 2) {
data[i / 2] = (byte) ((Character.digit(s.charAt(i), 16) << 4)
+ Character.digit(s.charAt(i + 1), 16));
}
return data;
}
public static int ByteToInt(byte b) {
return b & 0x000000FF;
}
}

View file

@ -0,0 +1,157 @@
package info.nightscout.androidaps.danars.comm
import dagger.android.HasAndroidInjector
import info.nightscout.androidaps.danars.encryption.BleEncryption
import info.nightscout.androidaps.logging.AAPSLogger
import info.nightscout.androidaps.utils.DateUtil
import org.joda.time.DateTime
import java.nio.charset.StandardCharsets
import javax.inject.Inject
open class DanaRS_Packet(protected var injector: HasAndroidInjector) {
@Inject lateinit var aapsLogger: AAPSLogger
@Inject lateinit var dateUtil: DateUtil
var isReceived = false
private set
var failed = false
var type = BleEncryption.DANAR_PACKET__TYPE_RESPONSE // most of the messages, should be changed for others
protected set
var opCode = 0
protected set
fun success(): Boolean = !failed
fun setReceived() {
isReceived = true
}
val command: Int
get() = (type and 0xFF shl 8) + (opCode and 0xFF)
open fun getRequestParams(): ByteArray = ByteArray(0)
fun getCommand(data: ByteArray): Int {
val type = byteArrayToInt(getBytes(data, TYPE_START, 1))
val opCode = byteArrayToInt(getBytes(data, OPCODE_START, 1))
return (type and 0xFF shl 8) + (opCode and 0xFF)
}
open fun handleMessage(data: ByteArray) {}
open fun handleMessageNotReceived() {
failed = true
}
open fun getFriendlyName(): String = "UNKNOWN_PACKET"
protected fun getBytes(data: ByteArray, srcStart: Int, srcLength: Int): ByteArray {
val ret = ByteArray(srcLength)
System.arraycopy(data, srcStart, ret, 0, srcLength)
return ret
}
fun dateFromBuff(buff: ByteArray, offset: Int): Long =
DateTime(
2000 + byteArrayToInt(getBytes(buff, offset, 1)),
byteArrayToInt(getBytes(buff, offset + 1, 1)),
byteArrayToInt(getBytes(buff, offset + 2, 1)),
0,
0
).millis
protected fun byteArrayToInt(b: ByteArray): Int =
when (b.size) {
1 -> b[0].toInt() and 0xFF
2 -> (b[1].toInt() and 0xFF shl 8) + (b[0].toInt() and 0xFF)
3 -> (b[2].toInt() and 0xFF shl 16) + (b[1].toInt() and 0xFF shl 8) + (b[0].toInt() and 0xFF)
4 -> (b[3].toInt() and 0xFF shl 24) + (b[2].toInt() and 0xFF shl 16) + (b[1].toInt() and 0xFF shl 8) + (b[0].toInt() and 0xFF)
else -> -1
}
@Synchronized fun dateTimeSecFromBuff(buff: ByteArray, offset: Int): Long =
DateTime(
2000 + intFromBuff(buff, offset, 1),
intFromBuff(buff, offset + 1, 1),
intFromBuff(buff, offset + 2, 1),
intFromBuff(buff, offset + 3, 1),
intFromBuff(buff, offset + 4, 1),
intFromBuff(buff, offset + 5, 1)
).millis
protected fun intFromBuff(b: ByteArray, srcStart: Int, srcLength: Int): Int =
when (srcLength) {
1 -> b[DATA_START + srcStart + 0].toInt() and 0xFF
2 -> (b[DATA_START + srcStart + 1].toInt() and 0xFF shl 8) + (b[DATA_START + srcStart + 0].toInt() and 0xFF)
3 -> (b[DATA_START + srcStart + 2].toInt() and 0xFF shl 16) + (b[DATA_START + srcStart + 1].toInt() and 0xFF shl 8) + (b[DATA_START + srcStart + 0].toInt() and 0xFF)
4 -> (b[DATA_START + srcStart + 3].toInt() and 0xFF shl 24) + (b[DATA_START + srcStart + 2].toInt() and 0xFF shl 16) + (b[DATA_START + srcStart + 1].toInt() and 0xFF shl 8) + (b[DATA_START + srcStart + 0].toInt() and 0xFF)
else -> -1
}
protected fun intFromBuffMsbLsb(b: ByteArray, srcStart: Int, srcLength: Int): Int =
when (srcLength) {
1 -> b[DATA_START + srcStart].toInt() and 0xFF
2 -> (b[DATA_START + srcStart].toInt() and 0xFF shl 8) + (b[DATA_START + srcStart + 1].toInt() and 0xFF)
3 -> (b[DATA_START + srcStart].toInt() and 0xFF shl 16) + (b[DATA_START + srcStart + 1].toInt() and 0xFF shl 8) + (b[DATA_START + srcStart + 2].toInt() and 0xFF)
4 -> (b[DATA_START + srcStart].toInt() and 0xFF shl 24) + (b[DATA_START + srcStart + 1].toInt() and 0xFF shl 16) + (b[DATA_START + srcStart + 2].toInt() and 0xFF shl 8) + (b[DATA_START + srcStart + 3].toInt() and 0xFF)
else -> -1
}
fun stringFromBuff(buff: ByteArray, offset: Int, length: Int): String {
val stringBuff = ByteArray(length)
System.arraycopy(buff, offset, stringBuff, 0, length)
return String(stringBuff, StandardCharsets.UTF_8)
}
companion object {
private const val TYPE_START = 0
private const val OPCODE_START = 1
const val DATA_START = 2
fun asciiStringFromBuff(buff: ByteArray, offset: Int, length: Int): String {
val stringBuff = ByteArray(length)
System.arraycopy(buff, offset, stringBuff, 0, length)
return String(stringBuff, StandardCharsets.UTF_8)
}
fun toHexString(buff: ByteArray?): String {
if (buff == null) return "null"
val sb = StringBuilder()
for ((count, element) in buff.withIndex()) {
sb.append(String.format("%02X ", element))
if ((count + 1) % 4 == 0) sb.append(" ")
}
return sb.toString()
}
@Suppress("SpellCheckingInspection")
private val hexArray = "0123456789ABCDEF".toCharArray()
fun bytesToHex(bytes: ByteArray): String {
val hexChars = CharArray(bytes.size * 2)
for (j in bytes.indices) {
val v: Int = bytes[j].toInt() and 0xFF
hexChars[j * 2] = hexArray[v ushr 4]
hexChars[j * 2 + 1] = hexArray[v and 0x0F]
}
return String(hexChars)
}
fun hexToBytes(s: String): ByteArray {
val len = s.length
val data = ByteArray(len / 2)
var i = 0
while (i < len) {
data[i / 2] = ((Character.digit(s[i], 16) shl 4)
+ Character.digit(s[i + 1], 16)).toByte()
i += 2
}
return data
}
}
init {
@Suppress("LeakingThis")
injector.androidInjector().inject(this)
}
}

View file

@ -18,6 +18,8 @@ import info.nightscout.androidaps.danars.encryption.BleEncryption
import info.nightscout.androidaps.danars.encryption.EncryptionType
import info.nightscout.androidaps.danars.events.EventDanaRSPairingSuccess
import info.nightscout.androidaps.events.EventPumpStatusChanged
import info.nightscout.androidaps.extensions.notify
import info.nightscout.androidaps.extensions.waitMillis
import info.nightscout.androidaps.interfaces.PumpSync
import info.nightscout.androidaps.logging.AAPSLogger
import info.nightscout.androidaps.logging.LTag
@ -28,8 +30,6 @@ import info.nightscout.androidaps.plugins.general.overview.notifications.Notific
import info.nightscout.androidaps.utils.DateUtil
import info.nightscout.androidaps.utils.T
import info.nightscout.androidaps.utils.ToastUtils
import info.nightscout.androidaps.extensions.notify
import info.nightscout.androidaps.extensions.waitMillis
import info.nightscout.androidaps.utils.resources.ResourceHelper
import info.nightscout.androidaps.utils.sharedPreferences.SP
import java.util.*
@ -62,6 +62,8 @@ class BLEComm @Inject internal constructor(
private const val PACKET_START_BYTE = 0xA5.toByte()
private const val PACKET_END_BYTE = 0x5A.toByte()
private const val BLE5_PACKET_START_BYTE = 0xAA.toByte()
private const val BLE5_PACKET_END_BYTE = 0xEE.toByte()
}
private var scheduledDisconnection: ScheduledFuture<*>? = null
@ -327,12 +329,8 @@ class BLEComm @Inject internal constructor(
private fun addToReadBuffer(buffer: ByteArray) {
//log.debug("addToReadBuffer " + DanaRS_Packet.toHexString(buffer));
if (buffer.isEmpty()) {
return
}
if (bufferLength == 1024) {
aapsLogger.debug(LTag.PUMPBTCOMM, "1024 XXXXXXXXXXXXXX")
}
if (buffer.isEmpty()) return
synchronized(readBuffer) {
// Append incoming data to input buffer
System.arraycopy(buffer, 0, readBuffer, bufferLength, buffer.size)
@ -343,7 +341,6 @@ class BLEComm @Inject internal constructor(
@kotlin.ExperimentalStdlibApi
private fun readDataParsing(receivedData: ByteArray) {
//aapsLogger.debug(LTag.PUMPBTCOMM, "<<<<< readDataParsing " + DanaRS_Packet.toHexString(receivedData))
var startSignatureFound = false
var packetIsValid = false
var isProcessing: Boolean
isProcessing = true
@ -363,10 +360,11 @@ class BLEComm @Inject internal constructor(
while (isProcessing) {
var length = 0
synchronized(readBuffer) {
// Find packet start [A5 A5]
// Find packet start [A5 A5] or [AA AA]
if (bufferLength >= 6) {
for (idxStartByte in 0 until bufferLength - 2) {
if (readBuffer[idxStartByte] == PACKET_START_BYTE && readBuffer[idxStartByte + 1] == PACKET_START_BYTE) {
if (readBuffer[idxStartByte] == PACKET_START_BYTE && readBuffer[idxStartByte + 1] == PACKET_START_BYTE ||
readBuffer[idxStartByte] == BLE5_PACKET_START_BYTE && readBuffer[idxStartByte + 1] == BLE5_PACKET_START_BYTE) {
if (idxStartByte > 0) {
// if buffer doesn't start with signature remove the leading trash
aapsLogger.debug(LTag.PUMPBTCOMM, "Shifting the input buffer by $idxStartByte bytes")
@ -374,29 +372,25 @@ class BLEComm @Inject internal constructor(
bufferLength -= idxStartByte
if (bufferLength < 0) bufferLength = 0
}
startSignatureFound = true
// A5 A5 LEN TYPE CODE PARAMS CHECKSUM1 CHECKSUM2 5A 5A or
// AA AA LEN TYPE CODE PARAMS CHECKSUM1 CHECKSUM2 EE EE
// ^---- LEN -----^
// total packet length 2 + 1 + readBuffer[2] + 2 + 2
length = readBuffer[2].toInt()
// test if there is enough data loaded
if (length + 7 > bufferLength)
return
// Verify packed end [5A 5A]
if (readBuffer[length + 5] == PACKET_END_BYTE && readBuffer[length + 6] == PACKET_END_BYTE ||
readBuffer[length + 5] == BLE5_PACKET_END_BYTE && readBuffer[length + 6] == BLE5_PACKET_END_BYTE) {
packetIsValid = true
} else {
aapsLogger.error(LTag.PUMPBTCOMM, "Error in input data. Resetting buffer.")
bufferLength = 0
}
break
}
}
}
// A5 A5 LEN TYPE CODE PARAMS CHECKSUM1 CHECKSUM2 5A 5A
// ^---- LEN -----^
// total packet length 2 + 1 + readBuffer[2] + 2 + 2
if (startSignatureFound) {
length = readBuffer[2].toInt()
// test if there is enough data loaded
if (length + 7 > bufferLength) return
// Verify packed end [5A 5A]
if (readBuffer[length + 5] == PACKET_END_BYTE && readBuffer[length + 6] == PACKET_END_BYTE) {
packetIsValid = true
} else if (readBuffer[length + 5] == readBuffer[length + 6]) {
// BLE5
packetIsValid = true
readBuffer[length + 5] = PACKET_END_BYTE
readBuffer[length + 6] = PACKET_END_BYTE
} else {
aapsLogger.error(LTag.PUMPBTCOMM, "Error in input data. Resetting buffer.")
bufferLength = 0
break
}
}
}
@ -414,9 +408,11 @@ class BLEComm @Inject internal constructor(
bufferLength -= length + 7
// now we have encrypted packet in inputBuffer
//aapsLogger.debug(LTag.PUMPBTCOMM, "<<<<< PROCESSING: " + DanaRS_Packet.toHexString(inputBuffer))
val decrypted = bleEncryption.getDecryptedPacket(inputBuffer)
//aapsLogger.debug(LTag.PUMPBTCOMM, "XXXXXX <<<<< PROCESSING: " + DanaRS_Packet.toHexString(inputBuffer))
//aapsLogger.debug(LTag.PUMPBTCOMM, "XXXXXY <<<<< PROCESSING: " + DanaRS_Packet.toHexString(decrypted))
// decrypt the packet
bleEncryption.getDecryptedPacket(inputBuffer)?.let { decryptedBuffer ->
decrypted?.let { decryptedBuffer ->
if (decryptedBuffer[0] == BleEncryption.DANAR_PACKET__TYPE_ENCRYPTION_RESPONSE.toByte()) {
when (decryptedBuffer[1]) {
// 1st packet exchange
@ -450,8 +446,9 @@ class BLEComm @Inject internal constructor(
// Retrieve message code from received buffer and last message sent
processMessage(decryptedBuffer)
}
} ?: throw IllegalStateException("Null decryptedInputBuffer")
startSignatureFound = false
}
if (decrypted == null)
throw IllegalStateException("Null decryptedInputBuffer")
packetIsValid = false
if (bufferLength < 6) {
// stop the loop
@ -722,8 +719,8 @@ class BLEComm @Inject internal constructor(
encryptedCommandSent = true
processedMessage = message
val command = byteArrayOf(message.type.toByte(), message.opCode.toByte())
val params = message.requestParams
aapsLogger.debug(LTag.PUMPBTCOMM, ">>>>> " + message.friendlyName + " " + DanaRS_Packet.toHexString(command) + " " + DanaRS_Packet.toHexString(params))
val params = message.getRequestParams()
aapsLogger.debug(LTag.PUMPBTCOMM, ">>>>> " + message.getFriendlyName() + " " + DanaRS_Packet.toHexString(command) + " " + DanaRS_Packet.toHexString(params))
var bytes = bleEncryption.getEncryptedPacket(message.opCode, params, null)
// aapsLogger.debug(LTag.PUMPBTCOMM, ">>>>> " + DanaRS_Packet.toHexString(bytes))
if (encryption != EncryptionType.ENCRYPTION_DEFAULT)
@ -783,7 +780,7 @@ class BLEComm @Inject internal constructor(
//SystemClock.sleep(200);
if (!message.isReceived) {
aapsLogger.warn(LTag.PUMPBTCOMM, "Reply not received " + message.friendlyName)
aapsLogger.warn(LTag.PUMPBTCOMM, "Reply not received " + message.getFriendlyName())
message.handleMessageNotReceived()
}
// verify encryption for v3
@ -803,7 +800,7 @@ class BLEComm @Inject internal constructor(
danaRSMessageHashTable.findMessage(receivedCommand)
}
if (message != null) {
aapsLogger.debug(LTag.PUMPBTCOMM, "<<<<< " + message.friendlyName + " " + DanaRS_Packet.toHexString(decryptedBuffer))
aapsLogger.debug(LTag.PUMPBTCOMM, "<<<<< " + message.getFriendlyName() + " " + DanaRS_Packet.toHexString(decryptedBuffer))
// process received data
message.handleMessage(decryptedBuffer)
message.setReceived()

View file

@ -49,6 +49,6 @@ class DanaRsMessageHashTableTest : DanaRSTestBase() {
val danaRSMessageHashTable = DanaRSMessageHashTable(packetInjector)
val forTesting: DanaRS_Packet = DanaRS_Packet_APS_Set_Event_History(packetInjector, info.nightscout.androidaps.dana.DanaPump.CARBS, 0, 0, 0)
val testPacket: DanaRS_Packet = danaRSMessageHashTable.findMessage(forTesting.command)
Assert.assertEquals(BleEncryption.DANAR_PACKET__OPCODE__APS_SET_EVENT_HISTORY.toLong(), testPacket.getOpCode().toLong())
Assert.assertEquals(BleEncryption.DANAR_PACKET__OPCODE__APS_SET_EVENT_HISTORY.toLong(), testPacket.opCode.toLong())
}
}