Объединить SETProtocol v2 и общие кодеки #1
@@ -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