134 lines
5.8 KiB
Python
134 lines
5.8 KiB
Python
"""Portable wire readers based on templates/c/set-protocol (no native DLL)."""
|
|
import binascii
|
|
|
|
|
|
V2 = {1: 'PING', 2: 'DEVICE_INFO', 3: 'CAPABILITIES', 8: 'DIAGNOSTICS',
|
|
9: 'READ', 10: 'WRITE', 16: 'LOG_READ', 17: 'CATALOG',
|
|
18: 'SUBSCRIBE', 19: 'PUBLISH', 20: 'UNSUBSCRIBE'}
|
|
V2.update(dict((256 + i, 'FW_' + name) for i, name in enumerate(
|
|
('BEGIN', 'DATA', 'END', 'ABORT', 'STATUS', 'ACTIVATE'))))
|
|
V1 = dict(enumerate(('PING DEVICE_INFO GET_OBJECT SET_OBJECT GET_OBJECT_LIST '
|
|
'GET_OBJECT_INFO COMMAND_STATUS DIAGNOSTICS READ_REGISTERS WRITE_REGISTERS '
|
|
'FIRMWARE_BEGIN FIRMWARE_DATA FIRMWARE_END FIRMWARE_ABORT FIRMWARE_STATUS '
|
|
'READ_LOGS GAS_CATALOG GAS_WATCH_SET GAS_WATCH_DATA').split(), 1))
|
|
V1.update({128: 'NACK', 129: 'ACK', 130: 'ERROR'})
|
|
V1.update(dict(enumerate(('SENSOR_SCAN SENSOR_LIST SENSOR_READ SENSOR_DATA '
|
|
'SET_USER_BYTES SET_RESOLUTION SET_POLL_PERIOD SEND_ID_CAN UI_KEY UI_READ '
|
|
'UI_STATE EEPROM_SCAN EEPROM_INFO EEPROM_READ EEPROM_LIST').split(), 32)))
|
|
|
|
|
|
def number(data, order='little'):
|
|
return int.from_bytes(bytes(data), order)
|
|
|
|
|
|
def crc16(data, modbus=False):
|
|
crc = 0xffff
|
|
for byte in data:
|
|
crc ^= byte if modbus else byte << 8
|
|
for _ in range(8):
|
|
if modbus:
|
|
crc = (crc >> 1) ^ (0xa001 if crc & 1 else 0)
|
|
else:
|
|
crc = ((crc << 1) ^ (0x1021 if crc & 0x8000 else 0)) & 0xffff
|
|
return crc
|
|
|
|
|
|
def parse_frame(raw):
|
|
raw = bytes(raw)
|
|
bridge = raw[:2] == b'\xaa\x55'
|
|
if bridge:
|
|
if len(raw) < 11 or not 6 <= raw[2] <= 14 or len(raw) != raw[2] + 5:
|
|
raise ValueError('Invalid ProtoCAN length')
|
|
crc, received = crc16(raw[2:-2]), number(raw[-2:])
|
|
ident, flags = number(raw[5:9]), raw[4]
|
|
if ident > (0x1fffffff if flags & 1 else 0x7ff) or flags & 0xf0:
|
|
raise ValueError('Invalid ProtoCAN ID/flags')
|
|
result = dict(protocol='ProtoCAN bridge', sequence=raw[3], flags=flags,
|
|
can_id=ident, payload=raw[9:-2])
|
|
result['summary'] = 'ProtoCAN seq=%d ID=%08X flags=%02X' % (raw[3], ident, flags)
|
|
else:
|
|
if len(raw) < 12 or raw[:2] != b'\xa5\x5a' or raw[2] not in (1, 2):
|
|
raise ValueError('Invalid SET signature/version')
|
|
version = raw[2]
|
|
header = 14 if version == 2 else 8
|
|
if len(raw) < header + 4:
|
|
raise ValueError('Incomplete SET header')
|
|
size = number(raw[12:14]) if version == 2 else number(raw[6:8], 'big')
|
|
if size > 512 or len(raw) != header + size + 4:
|
|
raise ValueError('Invalid SET payload length')
|
|
crc, received = binascii.crc32(raw[2:-4]) & 0xffffffff, number(raw[-4:])
|
|
msg = number(raw[4:6]) if version == 2 else raw[3]
|
|
seq = number(raw[10:12]) if version == 2 else number(raw[4:6], 'big')
|
|
result = dict(protocol='SET v2' if version == 2 else 'SETGUI v1',
|
|
message_type=msg, sequence=seq, payload=raw[header:-4])
|
|
name = (V2 if version == 2 else V1).get(msg, 'TYPE_%04X' % msg)
|
|
result['summary'] = '%s %s seq=%d len=%d' % (result['protocol'], name, seq, size)
|
|
if version == 2:
|
|
result.update(source=number(raw[6:8]), destination=number(raw[8:10]), flags=raw[3])
|
|
result['summary'] += ' %04X->%04X flags=%02X' % (result['source'], result['destination'], raw[3])
|
|
if raw[3] & 0xc0:
|
|
raise ValueError('Reserved SET flags')
|
|
if raw[3] & 1:
|
|
if size < 2:
|
|
raise ValueError('SET response missing status')
|
|
result['status'] = number(result['payload'][:2])
|
|
result['summary'] += ' status=%d' % result['status']
|
|
if crc != received:
|
|
raise ValueError('CRC mismatch: received %X expected %X' % (received, crc))
|
|
result['raw'] = raw
|
|
return result
|
|
|
|
|
|
class StreamParser:
|
|
"""Bounded byte stream; keep sample positions during resynchronization."""
|
|
def __init__(self, mode='auto'):
|
|
self.mode = mode
|
|
self.buffer = []
|
|
|
|
def feed(self, value, start, end):
|
|
self.buffer.append((value, start, end))
|
|
events = []
|
|
while len(self.buffer) >= 3:
|
|
raw = bytes(item[0] for item in self.buffer)
|
|
bridge = raw[:2] == b'\xaa\x55' and self.mode in ('auto', 'bridge')
|
|
modern = raw[:2] == b'\xa5\x5a' and self.mode != 'bridge'
|
|
if not bridge and not modern:
|
|
del self.buffer[0]
|
|
continue
|
|
error = None
|
|
if bridge:
|
|
total = raw[2] + 5
|
|
if not 6 <= raw[2] <= 14:
|
|
error = 'Invalid ProtoCAN length'
|
|
else:
|
|
if raw[2] not in (1, 2) or self.mode in ('v1', 'v2') and raw[2] != int(self.mode[1]):
|
|
error = 'Unexpected SET version'
|
|
header = 14 if raw[2] == 2 else 8
|
|
if not error and len(raw) < header:
|
|
break
|
|
size = number(raw[12:14]) if raw[2] == 2 else number(raw[6:8], 'big')
|
|
total = header + size + 4
|
|
if size > 512:
|
|
error = 'Payload exceeds 512 bytes'
|
|
if not error and len(raw) < total:
|
|
break
|
|
last = min(len(raw), total) - 1
|
|
result = None
|
|
if not error:
|
|
try:
|
|
result = parse_frame(raw[:total])
|
|
except ValueError as exc:
|
|
error = str(exc)
|
|
events.append((self.buffer[0][1], self.buffer[last][2], result, error))
|
|
del self.buffer[:1 if error else total]
|
|
return events
|
|
|
|
def flush(self):
|
|
if not self.buffer:
|
|
return []
|
|
pending = self.buffer
|
|
self.buffer = []
|
|
if pending[0][0] not in (0xa5, 0xaa):
|
|
return []
|
|
return [(pending[0][1], pending[-1][2], None, 'Incomplete frame')]
|