"""Exact, read-only firmware configuration decoder. Never serializes a write.""" import math from pathlib import Path import struct import xml.etree.ElementTree as ET def crc32c(data): value = 0xffffffff for byte in data: value ^= byte for _ in range(8): value = (value >> 1) ^ (0x82f63b78 if value & 1 else 0) return value ^ 0xffffffff def decode(data, kind): code, name = {"motor": (14, "mcconf"), "application": (17, "appconf")}[kind] xml = ET.parse(Path(__file__).parent / "schemas/5.02" / ("parameters_" + name + ".xml")).getroot() params = {p.tag: p for p in xml.find("Params")} order = [p.text for p in xml.find("SerOrder")] signature = "".join(n + params[n].findtext("type", "0") + params[n].findtext("vTx", "0") + "".join(x.text or "" for x in params[n].findall("enumNames")) for n in order) if len(data) < 5 or data[0] != code or int.from_bytes(data[1:5], "big") != crc32c(signature.encode()): raise ValueError("Configuration signature differs from firmware 5.02 schema") offset, result = 5, {} for name in order: p = params[name]; kind = int(p.findtext("type")); tx = int(p.findtext("vTx", "0")) if kind in (4, 5): fmt = "b" elif kind == 6: fmt = "B" elif kind == 2: fmt = {1: "B", 2: "b", 3: "H", 4: "h", 5: "I", 6: "i"}[tx] elif kind == 1: fmt = {7: "h", 8: "i", 9: "I"}[tx] else: raise ValueError("Unsupported configuration type") size = struct.calcsize(">" + fmt) if offset + size > len(data): raise ValueError("Truncated configuration") value = struct.unpack_from(">" + fmt, data, offset)[0]; offset += size if kind == 1: if tx == 9: exponent, fraction = (value >> 23) & 255, value & 0x7fffff part = fraction / 16777216.0 + 0.5 if exponent or fraction else 0.0 value = math.ldexp(-part if value & 0x80000000 else part, exponent - 126) else: value /= float(p.findtext("vTxDoubleScale", "1")) if not math.isfinite(value): raise ValueError("Non-finite parameter") result[name] = value if offset != len(data): raise ValueError("Unexpected configuration tail") return result