refactor(android): centralize Kotlin protocol codecs
This commit is contained in:
@@ -0,0 +1,338 @@
|
||||
package ru.setcorp.setflash.core
|
||||
|
||||
import ru.setcorp.setprotocol.NativeSetProtocol as NativeProtoCan
|
||||
|
||||
/**
|
||||
* Wire-compatible port of SETGUI `core/can_transport.py` and the
|
||||
* legacy frame module from `templates/c/set-protocol`.
|
||||
*
|
||||
* Frame layout:
|
||||
* `AA 55 | LEN | SEQ | FLAGS | CAN_ID little-endian | DATA[0..8] | CRC16 LE`.
|
||||
*/
|
||||
object CanBridgeProtocol {
|
||||
const val FLAG_IDE = 0x01
|
||||
const val FLAG_RTR = 0x02
|
||||
const val FLAG_DIRECTION = 0x04
|
||||
const val FLAG_ERROR = 0x08
|
||||
|
||||
const val STANDARD_ID_MAX = 0x7FFL
|
||||
const val EXTENDED_ID_MAX = 0x1FFF_FFFFL
|
||||
|
||||
private const val SOF0 = 0xAA
|
||||
private const val SOF1 = 0x55
|
||||
private const val MIN_LENGTH = 6
|
||||
private const val MAX_LENGTH = 14
|
||||
|
||||
data class Frame(
|
||||
val sequence: Int,
|
||||
val flags: Int,
|
||||
val canId: Long,
|
||||
val data: ByteArray,
|
||||
) {
|
||||
init {
|
||||
require(sequence in 0..0xFF) { "SEQ должен быть в диапазоне 0…255" }
|
||||
require(flags in 0..0xFF) { "FLAGS должен быть байтом" }
|
||||
require(data.size <= 8) { "DLC не может превышать 8 байт" }
|
||||
validateCanId(canId, isExtended)
|
||||
}
|
||||
|
||||
val isExtended: Boolean get() = flags and FLAG_IDE != 0
|
||||
val isRemote: Boolean get() = flags and FLAG_RTR != 0
|
||||
val toCan: Boolean get() = flags and FLAG_DIRECTION != 0
|
||||
val isError: Boolean get() = flags and FLAG_ERROR != 0
|
||||
val direction: String get() = if (isError) "ERR" else if (toCan) "TX" else "RX"
|
||||
|
||||
fun encode(): ByteArray {
|
||||
if (NativeProtoCan.available) {
|
||||
return requireNotNull(
|
||||
NativeProtoCan.nativeEncodeFrame(sequence, flags, canId, data),
|
||||
) { "SETProtocol отклонил CAN-кадр" }
|
||||
}
|
||||
val length = MIN_LENGTH + data.size
|
||||
val protected = ByteArray(1 + length)
|
||||
protected[0] = length.toByte()
|
||||
protected[1] = sequence.toByte()
|
||||
protected[2] = flags.toByte()
|
||||
repeat(4) { index -> protected[3 + index] = (canId ushr (index * 8)).toByte() }
|
||||
data.copyInto(protected, 7)
|
||||
val crc = crc16Ccitt(protected)
|
||||
return byteArrayOf(SOF0.toByte(), SOF1.toByte()) + protected +
|
||||
byteArrayOf(crc.toByte(), (crc ushr 8).toByte())
|
||||
}
|
||||
|
||||
override fun equals(other: Any?): Boolean = other is Frame &&
|
||||
sequence == other.sequence && flags == other.flags && canId == other.canId &&
|
||||
data.contentEquals(other.data)
|
||||
|
||||
override fun hashCode(): Int = 31 *
|
||||
(31 * (31 * sequence + flags) + canId.hashCode()) + data.contentHashCode()
|
||||
}
|
||||
|
||||
data class Stats(
|
||||
val frames: Int = 0,
|
||||
val crcErrors: Int = 0,
|
||||
val resyncBytes: Int = 0,
|
||||
val sequenceLost: Int = 0,
|
||||
)
|
||||
|
||||
fun buildFrame(
|
||||
canId: Long,
|
||||
data: ByteArray,
|
||||
sequence: Int = 0,
|
||||
extended: Boolean = true,
|
||||
remote: Boolean = false,
|
||||
toCan: Boolean = true,
|
||||
): Frame {
|
||||
validateCanId(canId, extended)
|
||||
require(data.size <= 8) { "DLC не может превышать 8 байт" }
|
||||
var flags = 0
|
||||
if (extended) flags = flags or FLAG_IDE
|
||||
if (remote) flags = flags or FLAG_RTR
|
||||
if (toCan) flags = flags or FLAG_DIRECTION
|
||||
return Frame(sequence and 0xFF, flags, canId, data.copyOf())
|
||||
}
|
||||
|
||||
fun validateCanId(canId: Long, extended: Boolean) {
|
||||
val maximum = if (extended) EXTENDED_ID_MAX else STANDARD_ID_MAX
|
||||
val kind = if (extended) "Расширенный (29 бит)" else "Стандартный (11 бит)"
|
||||
require(canId in 0..maximum) { "$kind CAN ID должен быть в диапазоне 0x0..0x${maximum.toString(16).uppercase()}" }
|
||||
}
|
||||
|
||||
fun crc16Ccitt(data: ByteArray, initial: Int = 0xFFFF): Int {
|
||||
if (NativeProtoCan.available && initial == 0xFFFF) {
|
||||
return NativeProtoCan.nativeCrc16(data)
|
||||
}
|
||||
var crc = initial and 0xFFFF
|
||||
data.forEach { value ->
|
||||
crc = crc xor ((value.toInt() and 0xFF) shl 8)
|
||||
repeat(8) {
|
||||
crc = if (crc and 0x8000 != 0) ((crc shl 1) xor 0x1021) and 0xFFFF
|
||||
else (crc shl 1) and 0xFFFF
|
||||
}
|
||||
}
|
||||
return crc
|
||||
}
|
||||
|
||||
fun parseHex(text: String): ByteArray {
|
||||
val compact = text.replace(Regex("(?i)0x"), "")
|
||||
.replace(Regex("[\\s,;:-]+"), "")
|
||||
require(compact.matches(Regex("[0-9A-Fa-f]*")) && compact.length % 2 == 0) {
|
||||
"DATA должна состоять из пар HEX-цифр"
|
||||
}
|
||||
require(compact.length <= 16) { "CAN 2.0 содержит не более 8 байт" }
|
||||
return ByteArray(compact.length / 2) { index ->
|
||||
compact.substring(index * 2, index * 2 + 2).toInt(16).toByte()
|
||||
}
|
||||
}
|
||||
|
||||
class Parser {
|
||||
private var buffer = byteArrayOf()
|
||||
private var lastSequence: Int? = null
|
||||
private var nativeHandle: Long = if (NativeProtoCan.available) {
|
||||
NativeProtoCan.nativeCreateParser()
|
||||
} else {
|
||||
0L
|
||||
}
|
||||
var stats = Stats()
|
||||
private set
|
||||
|
||||
fun reset(clearStats: Boolean = true) {
|
||||
buffer = byteArrayOf()
|
||||
lastSequence = null
|
||||
if (nativeHandle != 0L) {
|
||||
NativeProtoCan.nativeDestroyParser(nativeHandle)
|
||||
nativeHandle = NativeProtoCan.nativeCreateParser()
|
||||
}
|
||||
if (clearStats) stats = Stats()
|
||||
}
|
||||
|
||||
fun feed(chunk: ByteArray): List<Frame> {
|
||||
if (chunk.isEmpty()) return emptyList()
|
||||
if (nativeHandle != 0L) {
|
||||
val records = requireNotNull(
|
||||
NativeProtoCan.nativeFeedParser(nativeHandle, chunk),
|
||||
) { "SETProtocol parser failed" }
|
||||
require(records.size % 15 == 0) { "Некорректный ответ SETProtocol" }
|
||||
val result = ArrayList<Frame>(records.size / 15)
|
||||
for (offset in records.indices step 15) {
|
||||
val sequence = records[offset].toInt() and 0xFF
|
||||
val flags = records[offset + 1].toInt() and 0xFF
|
||||
var canId = 0L
|
||||
repeat(4) { index ->
|
||||
canId = canId or
|
||||
((records[offset + 2 + index].toLong() and 0xFF) shl (index * 8))
|
||||
}
|
||||
val dlc = records[offset + 6].toInt() and 0xFF
|
||||
result += Frame(
|
||||
sequence,
|
||||
flags,
|
||||
canId,
|
||||
records.copyOfRange(offset + 7, offset + 7 + dlc),
|
||||
)
|
||||
}
|
||||
NativeProtoCan.nativeParserStats(nativeHandle)?.let { values ->
|
||||
if (values.size >= 5) {
|
||||
stats = Stats(
|
||||
frames = values[0],
|
||||
crcErrors = values[1],
|
||||
resyncBytes = values[2] + values[3],
|
||||
sequenceLost = values[4],
|
||||
)
|
||||
}
|
||||
}
|
||||
return result
|
||||
}
|
||||
buffer += chunk
|
||||
val result = mutableListOf<Frame>()
|
||||
|
||||
while (true) {
|
||||
val start = findSignature(buffer)
|
||||
if (start < 0) {
|
||||
val keep = if (buffer.lastOrNull() == SOF0.toByte()) 1 else 0
|
||||
val dropped = buffer.size - keep
|
||||
if (dropped > 0) stats = stats.copy(resyncBytes = stats.resyncBytes + dropped)
|
||||
buffer = if (keep == 1) byteArrayOf(SOF0.toByte()) else byteArrayOf()
|
||||
break
|
||||
}
|
||||
if (start > 0) {
|
||||
stats = stats.copy(resyncBytes = stats.resyncBytes + start)
|
||||
buffer = buffer.copyOfRange(start, buffer.size)
|
||||
}
|
||||
if (buffer.size < 3) break
|
||||
|
||||
val length = buffer[2].toInt() and 0xFF
|
||||
if (length !in MIN_LENGTH..MAX_LENGTH) {
|
||||
stats = stats.copy(resyncBytes = stats.resyncBytes + 1)
|
||||
buffer = buffer.copyOfRange(1, buffer.size)
|
||||
continue
|
||||
}
|
||||
val total = 2 + 1 + length + 2
|
||||
if (buffer.size < total) break
|
||||
|
||||
val protected = buffer.copyOfRange(2, 3 + length)
|
||||
val receivedCrc = (buffer[3 + length].toInt() and 0xFF) or
|
||||
((buffer[4 + length].toInt() and 0xFF) shl 8)
|
||||
if (crc16Ccitt(protected) != receivedCrc) {
|
||||
stats = stats.copy(
|
||||
crcErrors = stats.crcErrors + 1,
|
||||
resyncBytes = stats.resyncBytes + 1,
|
||||
)
|
||||
buffer = buffer.copyOfRange(1, buffer.size)
|
||||
continue
|
||||
}
|
||||
|
||||
val sequence = protected[1].toInt() and 0xFF
|
||||
val flags = protected[2].toInt() and 0xFF
|
||||
var canId = 0L
|
||||
repeat(4) { index ->
|
||||
canId = canId or ((protected[3 + index].toLong() and 0xFF) shl (index * 8))
|
||||
}
|
||||
canId = canId and EXTENDED_ID_MAX
|
||||
val maximum = if (flags and FLAG_IDE != 0) EXTENDED_ID_MAX else STANDARD_ID_MAX
|
||||
if (canId > maximum) {
|
||||
buffer = buffer.copyOfRange(total, buffer.size)
|
||||
continue
|
||||
}
|
||||
val frame = Frame(sequence, flags, canId, protected.copyOfRange(7, protected.size))
|
||||
val previous = lastSequence
|
||||
val lost = if (previous == null) 0 else (sequence - previous - 1) and 0xFF
|
||||
lastSequence = sequence
|
||||
stats = stats.copy(frames = stats.frames + 1, sequenceLost = stats.sequenceLost + lost)
|
||||
result += frame
|
||||
buffer = buffer.copyOfRange(total, buffer.size)
|
||||
}
|
||||
return result
|
||||
}
|
||||
|
||||
private fun findSignature(data: ByteArray): Int {
|
||||
for (index in 0 until data.size - 1) {
|
||||
if (data[index] == SOF0.toByte() && data[index + 1] == SOF1.toByte()) return index
|
||||
}
|
||||
return -1
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** 29-bit SETCAN identifier layout from SETGUI `core/protocan.py`. */
|
||||
data class ProtoCanId(
|
||||
val raw: Long,
|
||||
val priority: Int,
|
||||
val pm: Int,
|
||||
val deviceType: Int,
|
||||
val device: Int,
|
||||
val messageType: Int,
|
||||
val body: Int,
|
||||
) {
|
||||
val messageTypeName: String get() = MESSAGE_TYPES[messageType] ?: "Reserved 0x${messageType.toString(16).uppercase()}"
|
||||
val deviceName: String? get() = DEVICE_NAMES[device]
|
||||
|
||||
fun summary(): String = "P$priority · PM$pm · Type $deviceType · Dev $device" +
|
||||
(deviceName?.let { " ($it)" } ?: "") + " · $messageTypeName · Body 0x%04X".format(body)
|
||||
|
||||
companion object {
|
||||
private val MESSAGE_TYPES = mapOf(
|
||||
0x0 to "Broadcast",
|
||||
0x1 to "Discrete",
|
||||
0x2 to "Analog",
|
||||
0x3 to "General Address Space",
|
||||
0x4 to "Modbus Coil",
|
||||
0x5 to "Modbus Discrete",
|
||||
0x6 to "Modbus Holding",
|
||||
0x7 to "Modbus Input",
|
||||
0x8 to "Error",
|
||||
0x9 to "Boot Control",
|
||||
0xA to "Boot Data A",
|
||||
0xB to "Boot Data B",
|
||||
0xC to "Boot Status",
|
||||
0xD to "Boot Discovery",
|
||||
0xE to "Settings",
|
||||
0xF to "Pulse",
|
||||
)
|
||||
private val DEVICE_NAMES = mapOf(
|
||||
0xD to "DS_CONTROL",
|
||||
0xE to "Android GUI",
|
||||
0xF to "SLCAN",
|
||||
)
|
||||
|
||||
fun parse(raw: Long): ProtoCanId {
|
||||
val value = raw and CanBridgeProtocol.EXTENDED_ID_MAX
|
||||
if (NativeProtoCan.available) {
|
||||
NativeProtoCan.nativeUnpackId(value)?.let { fields ->
|
||||
if (fields.size == 6) {
|
||||
return ProtoCanId(
|
||||
raw = value,
|
||||
priority = fields[0],
|
||||
pm = fields[1],
|
||||
deviceType = fields[2],
|
||||
device = fields[3],
|
||||
messageType = fields[4],
|
||||
body = fields[5],
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
return ProtoCanId(
|
||||
raw = value,
|
||||
priority = ((value ushr 28) and 0x1).toInt(),
|
||||
pm = ((value ushr 27) and 0x1).toInt(),
|
||||
deviceType = ((value ushr 24) and 0x7).toInt(),
|
||||
device = ((value ushr 20) and 0xF).toInt(),
|
||||
messageType = ((value ushr 16) and 0xF).toInt(),
|
||||
body = (value and 0xFFFF).toInt(),
|
||||
)
|
||||
}
|
||||
|
||||
fun build(priority: Int, pm: Int, deviceType: Int, device: Int, messageType: Int, body: Int): Long {
|
||||
require(priority in 0..1 && pm in 0..1 && deviceType in 0..7)
|
||||
require(device in 0..15 && messageType in 0..15 && body in 0..0xFFFF)
|
||||
if (NativeProtoCan.available) {
|
||||
return NativeProtoCan.nativePackId(
|
||||
priority, pm, deviceType, device, messageType, body,
|
||||
)
|
||||
}
|
||||
return (priority.toLong() shl 28) or (pm.toLong() shl 27) or
|
||||
(deviceType.toLong() shl 24) or (device.toLong() shl 20) or
|
||||
(messageType.toLong() shl 16) or body.toLong()
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,261 @@
|
||||
package ru.setcorp.setflash.core
|
||||
|
||||
import java.util.zip.CRC32
|
||||
import ru.setcorp.setprotocol.NativeSetProtocol as NativeProtoCan
|
||||
|
||||
object MessageType {
|
||||
const val PING = 0x01
|
||||
const val FIRMWARE_BEGIN = 0x0B
|
||||
const val FIRMWARE_DATA = 0x0C
|
||||
const val FIRMWARE_END = 0x0D
|
||||
const val FIRMWARE_ABORT = 0x0E
|
||||
const val FIRMWARE_STATUS = 0x0F
|
||||
const val SENSOR_SCAN = 0x20
|
||||
const val SENSOR_LIST = 0x21
|
||||
const val SENSOR_READ = 0x22
|
||||
const val SENSOR_DATA = 0x23
|
||||
const val SET_USER_BYTES = 0x24
|
||||
const val SET_RESOLUTION = 0x25
|
||||
const val SET_POLL_PERIOD = 0x26
|
||||
const val SEND_ID_CAN = 0x27
|
||||
const val EEPROM_SCAN = 0x2B
|
||||
const val EEPROM_INFO = 0x2C
|
||||
const val EEPROM_READ = 0x2D
|
||||
const val EEPROM_LIST = 0x2E
|
||||
const val NACK = 0x80
|
||||
const val ACK = 0x81
|
||||
const val ERROR = 0x82
|
||||
|
||||
fun name(value: Int): String = when (value) {
|
||||
PING -> "PING"
|
||||
FIRMWARE_BEGIN -> "FIRMWARE_BEGIN"
|
||||
FIRMWARE_DATA -> "FIRMWARE_DATA"
|
||||
FIRMWARE_END -> "FIRMWARE_END"
|
||||
FIRMWARE_ABORT -> "FIRMWARE_ABORT"
|
||||
FIRMWARE_STATUS -> "FIRMWARE_STATUS"
|
||||
SENSOR_SCAN -> "SENSOR_SCAN"
|
||||
SENSOR_LIST -> "SENSOR_LIST"
|
||||
SENSOR_READ -> "SENSOR_READ"
|
||||
SENSOR_DATA -> "SENSOR_DATA"
|
||||
SET_USER_BYTES -> "SET_USER_BYTES"
|
||||
SET_RESOLUTION -> "SET_RESOLUTION"
|
||||
SET_POLL_PERIOD -> "SET_POLL_PERIOD"
|
||||
SEND_ID_CAN -> "SEND_ID_CAN"
|
||||
EEPROM_SCAN -> "EEPROM_SCAN"
|
||||
EEPROM_INFO -> "EEPROM_INFO"
|
||||
EEPROM_READ -> "EEPROM_READ"
|
||||
EEPROM_LIST -> "EEPROM_LIST"
|
||||
NACK -> "NACK"
|
||||
ACK -> "ACK"
|
||||
ERROR -> "ERROR"
|
||||
else -> "0x${value.toString(16).padStart(2, '0').uppercase()}"
|
||||
}
|
||||
}
|
||||
|
||||
data class ProtocolFrame(
|
||||
val messageType: Int,
|
||||
val sequence: Int,
|
||||
val payload: ByteArray = byteArrayOf(),
|
||||
) {
|
||||
init {
|
||||
require(messageType in 0..0xFF) { "Тип сообщения вне диапазона" }
|
||||
require(sequence in 0..0xFFFF) { "Sequence вне диапазона" }
|
||||
require(payload.size <= GuiProtocol.MAX_PAYLOAD_SIZE) { "Payload превышает 512 байт" }
|
||||
}
|
||||
|
||||
override fun equals(other: Any?): Boolean = other is ProtocolFrame &&
|
||||
messageType == other.messageType && sequence == other.sequence &&
|
||||
payload.contentEquals(other.payload)
|
||||
|
||||
override fun hashCode(): Int = 31 * (31 * messageType + sequence) + payload.contentHashCode()
|
||||
}
|
||||
|
||||
object GuiProtocol {
|
||||
private const val VERSION = 0x01
|
||||
const val MAX_PAYLOAD_SIZE = 512
|
||||
private const val HEADER_SIZE = 8
|
||||
private const val CRC_SIZE = 4
|
||||
|
||||
fun buildFrame(frame: ProtocolFrame): ByteArray {
|
||||
if (NativeProtoCan.available) {
|
||||
return requireNotNull(
|
||||
NativeProtoCan.nativeGuiEncode(
|
||||
frame.messageType, frame.sequence, frame.payload,
|
||||
),
|
||||
) { "SETProtocol отклонил GUI-кадр" }
|
||||
}
|
||||
val protected = ByteArray(6 + frame.payload.size)
|
||||
protected[0] = VERSION.toByte()
|
||||
protected[1] = frame.messageType.toByte()
|
||||
protected[2] = (frame.sequence ushr 8).toByte()
|
||||
protected[3] = frame.sequence.toByte()
|
||||
protected[4] = (frame.payload.size ushr 8).toByte()
|
||||
protected[5] = frame.payload.size.toByte()
|
||||
frame.payload.copyInto(protected, 6)
|
||||
|
||||
val result = ByteArray(2 + protected.size + CRC_SIZE)
|
||||
result[0] = 0xA5.toByte()
|
||||
result[1] = 0x5A
|
||||
protected.copyInto(result, 2)
|
||||
putU32Le(result, result.size - CRC_SIZE, crc32(protected))
|
||||
return result
|
||||
}
|
||||
|
||||
fun crc32(data: ByteArray): Long {
|
||||
val crc = CRC32()
|
||||
crc.update(data)
|
||||
return crc.value
|
||||
}
|
||||
|
||||
fun u16Le(value: Int): ByteArray {
|
||||
require(value in 0..0xFFFF)
|
||||
return byteArrayOf(value.toByte(), (value ushr 8).toByte())
|
||||
}
|
||||
|
||||
fun u32Le(value: Long): ByteArray {
|
||||
require(value in 0..0xFFFF_FFFFL)
|
||||
return ByteArray(4).also { putU32Le(it, 0, value) }
|
||||
}
|
||||
|
||||
fun readU16Le(data: ByteArray, offset: Int = 0): Int {
|
||||
require(offset >= 0 && offset + 2 <= data.size) { "Payload не содержит u16" }
|
||||
return (data[offset].toInt() and 0xFF) or
|
||||
((data[offset + 1].toInt() and 0xFF) shl 8)
|
||||
}
|
||||
|
||||
fun readU32Le(data: ByteArray, offset: Int = 0): Long {
|
||||
require(offset >= 0 && offset + 4 <= data.size) { "Payload не содержит u32" }
|
||||
return (data[offset].toLong() and 0xFF) or
|
||||
((data[offset + 1].toLong() and 0xFF) shl 8) or
|
||||
((data[offset + 2].toLong() and 0xFF) shl 16) or
|
||||
((data[offset + 3].toLong() and 0xFF) shl 24)
|
||||
}
|
||||
|
||||
fun concat(vararg parts: ByteArray): ByteArray {
|
||||
val result = ByteArray(parts.sumOf(ByteArray::size))
|
||||
var offset = 0
|
||||
parts.forEach {
|
||||
it.copyInto(result, offset)
|
||||
offset += it.size
|
||||
}
|
||||
return result
|
||||
}
|
||||
|
||||
fun hex(data: ByteArray): String = data.joinToString(" ") {
|
||||
(it.toInt() and 0xFF).toString(16).padStart(2, '0').uppercase()
|
||||
}
|
||||
|
||||
private fun putU32Le(target: ByteArray, offset: Int, value: Long) {
|
||||
repeat(4) { index -> target[offset + index] = (value ushr (index * 8)).toByte() }
|
||||
}
|
||||
|
||||
class Parser {
|
||||
private var buffer = byteArrayOf()
|
||||
private var nativeHandle = if (NativeProtoCan.available) {
|
||||
NativeProtoCan.nativeCreateGuiParser()
|
||||
} else {
|
||||
0L
|
||||
}
|
||||
private var nativeCrcOffset = 0
|
||||
private var nativeVersionOffset = 0
|
||||
private var nativeLengthOffset = 0
|
||||
var crcErrors: Int = 0
|
||||
private set
|
||||
var versionErrors: Int = 0
|
||||
private set
|
||||
var lengthErrors: Int = 0
|
||||
private set
|
||||
|
||||
fun reset() {
|
||||
buffer = byteArrayOf()
|
||||
if (nativeHandle != 0L) {
|
||||
nativeCrcOffset = crcErrors
|
||||
nativeVersionOffset = versionErrors
|
||||
nativeLengthOffset = lengthErrors
|
||||
NativeProtoCan.nativeDestroyGuiParser(nativeHandle)
|
||||
nativeHandle = NativeProtoCan.nativeCreateGuiParser()
|
||||
}
|
||||
}
|
||||
|
||||
fun feed(chunk: ByteArray): List<ProtocolFrame> {
|
||||
if (chunk.isEmpty()) return emptyList()
|
||||
if (nativeHandle != 0L) {
|
||||
val records = requireNotNull(
|
||||
NativeProtoCan.nativeFeedGuiParser(nativeHandle, chunk),
|
||||
) { "SETProtocol GUI parser failed" }
|
||||
val frames = mutableListOf<ProtocolFrame>()
|
||||
var offset = 0
|
||||
while (offset < records.size) {
|
||||
require(offset + 5 <= records.size) { "Некорректный ответ SETProtocol" }
|
||||
val messageType = records[offset].toInt() and 0xFF
|
||||
val sequence = (records[offset + 1].toInt() and 0xFF) or
|
||||
((records[offset + 2].toInt() and 0xFF) shl 8)
|
||||
val size = (records[offset + 3].toInt() and 0xFF) or
|
||||
((records[offset + 4].toInt() and 0xFF) shl 8)
|
||||
offset += 5
|
||||
require(offset + size <= records.size) { "Некорректный payload SETProtocol" }
|
||||
frames += ProtocolFrame(
|
||||
messageType, sequence, records.copyOfRange(offset, offset + size),
|
||||
)
|
||||
offset += size
|
||||
}
|
||||
NativeProtoCan.nativeGuiParserStats(nativeHandle)?.let { values ->
|
||||
if (values.size >= 4) {
|
||||
crcErrors = nativeCrcOffset + values[1]
|
||||
versionErrors = nativeVersionOffset + values[2]
|
||||
lengthErrors = nativeLengthOffset + values[3]
|
||||
}
|
||||
}
|
||||
return frames
|
||||
}
|
||||
buffer += chunk
|
||||
val frames = mutableListOf<ProtocolFrame>()
|
||||
|
||||
while (true) {
|
||||
val sof = findSof(buffer)
|
||||
if (sof < 0) {
|
||||
buffer = if (buffer.lastOrNull() == 0xA5.toByte()) byteArrayOf(0xA5.toByte()) else byteArrayOf()
|
||||
break
|
||||
}
|
||||
if (sof > 0) buffer = buffer.copyOfRange(sof, buffer.size)
|
||||
if (buffer.size < HEADER_SIZE) break
|
||||
if ((buffer[2].toInt() and 0xFF) != VERSION) {
|
||||
versionErrors++
|
||||
buffer = buffer.copyOfRange(1, buffer.size)
|
||||
continue
|
||||
}
|
||||
val payloadSize = ((buffer[6].toInt() and 0xFF) shl 8) or
|
||||
(buffer[7].toInt() and 0xFF)
|
||||
if (payloadSize > MAX_PAYLOAD_SIZE) {
|
||||
lengthErrors++
|
||||
buffer = buffer.copyOfRange(1, buffer.size)
|
||||
continue
|
||||
}
|
||||
val total = HEADER_SIZE + payloadSize + CRC_SIZE
|
||||
if (buffer.size < total) break
|
||||
val protected = buffer.copyOfRange(2, total - CRC_SIZE)
|
||||
val received = readU32Le(buffer, total - CRC_SIZE)
|
||||
if (crc32(protected) != received) {
|
||||
crcErrors++
|
||||
buffer = buffer.copyOfRange(1, buffer.size)
|
||||
continue
|
||||
}
|
||||
frames += ProtocolFrame(
|
||||
messageType = buffer[3].toInt() and 0xFF,
|
||||
sequence = ((buffer[4].toInt() and 0xFF) shl 8) or
|
||||
(buffer[5].toInt() and 0xFF),
|
||||
payload = buffer.copyOfRange(HEADER_SIZE, HEADER_SIZE + payloadSize),
|
||||
)
|
||||
buffer = buffer.copyOfRange(total, buffer.size)
|
||||
}
|
||||
return frames
|
||||
}
|
||||
|
||||
private fun findSof(data: ByteArray): Int {
|
||||
for (index in 0 until data.size - 1) {
|
||||
if (data[index] == 0xA5.toByte() && data[index + 1] == 0x5A.toByte()) return index
|
||||
}
|
||||
return -1
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user