diff options
Diffstat (limited to 'klippy/msgproto.py')
| -rw-r--r-- | klippy/msgproto.py | 215 |
1 files changed, 144 insertions, 71 deletions
diff --git a/klippy/msgproto.py b/klippy/msgproto.py index 25701df3..a2151a68 100644 --- a/klippy/msgproto.py +++ b/klippy/msgproto.py @@ -12,101 +12,131 @@ DefaultMessages = { MESSAGE_MIN = 5 MESSAGE_MAX = 64 -MESSAGE_HEADER_SIZE = 2 +MESSAGE_HEADER_SIZE = 2 MESSAGE_TRAILER_SIZE = 3 MESSAGE_POS_LEN = 0 MESSAGE_POS_SEQ = 1 -MESSAGE_TRAILER_CRC = 3 +MESSAGE_TRAILER_CRC = 3 MESSAGE_TRAILER_SYNC = 1 MESSAGE_PAYLOAD_MAX = MESSAGE_MAX - MESSAGE_MIN -MESSAGE_SEQ_MASK = 0x0f +MESSAGE_SEQ_MASK = 0x0F MESSAGE_DEST = 0x10 -MESSAGE_SYNC = 0x7e +MESSAGE_SYNC = 0x7E + class error(Exception): pass + def crc16_ccitt(buf): - crc = 0xffff + crc = 0xFFFF for data in buf: - data ^= crc & 0xff - data ^= (data & 0x0f) << 4 + data ^= crc & 0xFF + data ^= (data & 0x0F) << 4 crc = ((data << 8) | (crc >> 8)) ^ (data >> 4) ^ (data << 3) - return [crc >> 8, crc & 0xff] + return [crc >> 8, crc & 0xFF] + class PT_uint32: is_int = True is_dynamic_string = False max_length = 5 signed = False + def encode(self, out, v): - if v >= 0xc000000 or v < -0x4000000: out.append((v>>28) & 0x7f | 0x80) - if v >= 0x180000 or v < -0x80000: out.append((v>>21) & 0x7f | 0x80) - if v >= 0x3000 or v < -0x1000: out.append((v>>14) & 0x7f | 0x80) - if v >= 0x60 or v < -0x20: out.append((v>>7) & 0x7f | 0x80) - out.append(v & 0x7f) + if v >= 0xC000000 or v < -0x4000000: + out.append((v >> 28) & 0x7F | 0x80) + if v >= 0x180000 or v < -0x80000: + out.append((v >> 21) & 0x7F | 0x80) + if v >= 0x3000 or v < -0x1000: + out.append((v >> 14) & 0x7F | 0x80) + if v >= 0x60 or v < -0x20: + out.append((v >> 7) & 0x7F | 0x80) + out.append(v & 0x7F) + def parse(self, s, pos): c = s[pos] pos += 1 - v = c & 0x7f + v = c & 0x7F if (c & 0x60) == 0x60: v |= -0x20 while c & 0x80: c = s[pos] pos += 1 - v = (v<<7) | (c & 0x7f) + v = (v << 7) | (c & 0x7F) if not self.signed: - v = int(v & 0xffffffff) + v = int(v & 0xFFFFFFFF) return v, pos + class PT_int32(PT_uint32): signed = True + + class PT_uint16(PT_uint32): max_length = 3 + + class PT_int16(PT_int32): signed = True max_length = 3 + + class PT_byte(PT_uint32): max_length = 2 + class PT_string: is_int = False is_dynamic_string = True max_length = 64 + def encode(self, out, v): out.append(len(v)) out.extend(bytearray(v)) + def parse(self, s, pos): l = s[pos] - return bytes(bytearray(s[pos+1:pos+l+1])), pos+l+1 + return bytes(bytearray(s[pos + 1 : pos + l + 1])), pos + l + 1 + + class PT_progmem_buffer(PT_string): pass + + class PT_buffer(PT_string): pass + class enumeration_error(error): def __init__(self, enum_name, value): self.enum_name = enum_name self.value = value - error.__init__(self, "Unknown value '%s' in enumeration '%s'" - % (value, enum_name)) + error.__init__( + self, "Unknown value '%s' in enumeration '%s'" % (value, enum_name) + ) + def get_enum_params(self): return self.enum_name, self.value + class Enumeration: is_int = False is_dynamic_string = False + def __init__(self, pt, enum_name, enums): self.pt = pt self.max_length = pt.max_length self.enum_name = enum_name self.enums = enums self.reverse_enums = {v: k for k, v in enums.items()} + def encode(self, out, v): tv = self.enums.get(v) if tv is None: raise enumeration_error(self.enum_name, v) self.pt.encode(out, tv) + def parse(self, s, pos): v, pos = self.pt.parse(s, pos) tv = self.reverse_enums.get(v) @@ -114,51 +144,60 @@ class Enumeration: tv = "?%d" % (v,) return tv, pos + MessageTypes = { - '%u': PT_uint32(), '%i': PT_int32(), - '%hu': PT_uint16(), '%hi': PT_int16(), - '%c': PT_byte(), - '%s': PT_string(), '%.*s': PT_progmem_buffer(), '%*s': PT_buffer(), + "%u": PT_uint32(), + "%i": PT_int32(), + "%hu": PT_uint16(), + "%hi": PT_int16(), + "%c": PT_byte(), + "%s": PT_string(), + "%.*s": PT_progmem_buffer(), + "%*s": PT_buffer(), } + # Lookup the message types for a format string def lookup_params(msgformat, enumerations={}): out = [] - argparts = [arg.split('=') for arg in msgformat.split()[1:]] + argparts = [arg.split("=") for arg in msgformat.split()[1:]] for name, fmt in argparts: pt = MessageTypes[fmt] for enum_name, enums in enumerations.items(): - if name == enum_name or name.endswith('_' + enum_name): + if name == enum_name or name.endswith("_" + enum_name): pt = Enumeration(pt, enum_name, enums) break out.append((name, pt)) return out + # Lookup the message types for a debugging "output()" format string def lookup_output_params(msgformat): param_types = [] args = msgformat while 1: - pos = args.find('%') + pos = args.find("%") if pos < 0: break - if pos+1 >= len(args) or args[pos+1] != '%': + if pos + 1 >= len(args) or args[pos + 1] != "%": for i in range(4): - t = MessageTypes.get(args[pos:pos+1+i]) + t = MessageTypes.get(args[pos : pos + 1 + i]) if t is not None: param_types.append(t) break else: raise error("Invalid output format for '%s'" % (msgformat,)) - args = args[pos+1:] + args = args[pos + 1 :] return param_types + # Update the message format to be compatible with python's % operator def convert_msg_format(msgformat): - for c in ['%u', '%i', '%hu', '%hi', '%c', '%.*s', '%*s']: - msgformat = msgformat.replace(c, '%s') + for c in ["%u", "%i", "%hu", "%hi", "%c", "%.*s", "%*s"]: + msgformat = msgformat.replace(c, "%s") return msgformat + class MessageFormat: def __init__(self, msgid_bytes, msgformat, enumerations={}): self.msgid_bytes = msgid_bytes @@ -168,16 +207,19 @@ class MessageFormat: self.param_names = lookup_params(msgformat, enumerations) self.param_types = [t for name, t in self.param_names] self.name_to_type = dict(self.param_names) + def encode(self, params): out = list(self.msgid_bytes) for i, t in enumerate(self.param_types): t.encode(out, params[i]) return out + def encode_by_name(self, **params): out = list(self.msgid_bytes) for name, t in self.param_names: t.encode(out, params[name]) return out + def parse(self, s, pos): pos += len(self.msgid_bytes) out = {} @@ -185,6 +227,7 @@ class MessageFormat: v, pos = t.parse(s, pos) out[name] = v return out, pos + def format_params(self, params): out = [] for name, t in self.param_names: @@ -194,13 +237,16 @@ class MessageFormat: out.append(v) return self.debugformat % tuple(out) + class OutputFormat: - name = '#output' + name = "#output" + def __init__(self, msgid_bytes, msgformat): self.msgid_bytes = msgid_bytes self.msgformat = msgformat self.debugformat = convert_msg_format(msgformat) self.param_types = lookup_output_params(msgformat) + def parse(self, s, pos): pos += len(self.msgid_bytes) out = [] @@ -210,21 +256,27 @@ class OutputFormat: v = repr(v) out.append(v) outmsg = self.debugformat % tuple(out) - return {'#msg': outmsg}, pos + return {"#msg": outmsg}, pos + def format_params(self, params): - return "#output %s" % (params['#msg'],) + return "#output %s" % (params["#msg"],) + class UnknownFormat: - name = '#unknown' + name = "#unknown" + def parse(self, s, pos): msgid, param_pos = PT_int32().parse(s, pos) msg = bytes(bytearray(s)) - return {'#msgid': msgid, '#msg': msg}, len(s)-MESSAGE_TRAILER_SIZE + return {"#msgid": msgid, "#msg": msg}, len(s) - MESSAGE_TRAILER_SIZE + def format_params(self, params): - return "#unknown %s" % (repr(params['#msg']),) + return "#unknown %s" % (repr(params["#msg"]),) + class MessageParser: error = error + def __init__(self, warn_prefix=""): self.warn_prefix = warn_prefix self.unknown = UnknownFormat() @@ -238,8 +290,10 @@ class MessageParser: self.version = self.build_versions = "" self.raw_identify_data = "" self._init_messages(DefaultMessages) + def _error(self, msg, *params): raise error(self.warn_prefix + (msg % params)) + def check_packet(self, s): if len(s) < MESSAGE_MIN: return 0 @@ -252,14 +306,15 @@ class MessageParser: if len(s) < msglen: # Need more data return 0 - if s[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC: + if s[msglen - MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC: return -1 - msgcrc = s[msglen-MESSAGE_TRAILER_CRC:msglen-MESSAGE_TRAILER_CRC+2] - crc = crc16_ccitt(s[:msglen-MESSAGE_TRAILER_SIZE]) + msgcrc = s[msglen - MESSAGE_TRAILER_CRC : msglen - MESSAGE_TRAILER_CRC + 2] + crc = crc16_ccitt(s[: msglen - MESSAGE_TRAILER_SIZE]) if crc != list(msgcrc): - #logging.debug("got crc %s vs %s", repr(crc), repr(msgcrc)) + # logging.debug("got crc %s vs %s", repr(crc), repr(msgcrc)) return -1 return msglen + def dump(self, s): msgseq = s[MESSAGE_POS_SEQ] out = ["seq: %02x" % (msgseq,)] @@ -269,26 +324,29 @@ class MessageParser: mid = self.messages_by_id.get(msgid, self.unknown) params, pos = mid.parse(s, pos) out.append(mid.format_params(params)) - if pos >= len(s)-MESSAGE_TRAILER_SIZE: + if pos >= len(s) - MESSAGE_TRAILER_SIZE: break return out + def format_params(self, params): - name = params.get('#name') + name = params.get("#name") mid = self.messages_by_name.get(name) if mid is not None: return mid.format_params(params) - msg = params.get('#msg') + msg = params.get("#msg") if msg is not None: return "%s %s" % (name, msg) return str(params) + def parse(self, s): msgid, param_pos = self.msgid_parser.parse(s, MESSAGE_HEADER_SIZE) mid = self.messages_by_id.get(msgid, self.unknown) params, pos = mid.parse(s, MESSAGE_HEADER_SIZE) - if pos != len(s)-MESSAGE_TRAILER_SIZE: + if pos != len(s) - MESSAGE_TRAILER_SIZE: self._error("Extra data at end of message") - params['#name'] = mid.name + params["#name"] = mid.name return params + def encode_msgblock(self, seq, cmd): msglen = MESSAGE_MIN + len(cmd) seq = (seq & MESSAGE_SEQ_MASK) | MESSAGE_DEST @@ -296,16 +354,18 @@ class MessageParser: out.append(crc16_ccitt(out)) out.append(MESSAGE_SYNC) return out + def _parse_buffer(self, value): if not value: return [] tval = int(value, 16) out = [] for i in range(len(value) // 2): - out.append(tval & 0xff) + out.append(tval & 0xFF) tval >>= 8 out.reverse() return out + def lookup_command(self, msgformat): parts = msgformat.strip().split() msgname = parts[0] @@ -313,14 +373,15 @@ class MessageParser: if mp is None: self._error("Unknown command: %s", msgname) if msgformat != mp.msgformat: - self._error("Command format mismatch: %s vs %s", - msgformat, mp.msgformat) + self._error("Command format mismatch: %s vs %s", msgformat, mp.msgformat) return mp + def lookup_msgid(self, msgformat): msgid = self.msgid_by_format.get(msgformat) if msgid is None: self._error("Unknown command: %s", msgformat) return msgid + def create_command(self, msg): parts = msg.strip().split() if not parts: @@ -330,7 +391,7 @@ class MessageParser: if mp is None: self._error("Unknown command: %s", msgname) try: - argparts = dict(arg.split('=', 1) for arg in parts[1:]) + argparts = dict(arg.split("=", 1) for arg in parts[1:]) for name, value in argparts.items(): t = mp.name_to_type[name] if t.is_dynamic_string: @@ -343,16 +404,17 @@ class MessageParser: except error as e: raise except: - #logging.exception("Unable to extract params") + # logging.exception("Unable to extract params") self._error("Unable to extract params from: %s", msgname) try: cmd = mp.encode_by_name(**argparts) except error as e: raise except: - #logging.exception("Unable to encode") + # logging.exception("Unable to encode") self._error("Unable to encode: %s", msgname) return cmd + def fill_enumerations(self, enumerations): for add_name, add_enums in enumerations.items(): enums = self.enumerations.setdefault(add_name, {}) @@ -367,62 +429,70 @@ class MessageParser: enum_root = enum_root[:-1] start_enum = 0 if len(enum_root) != len(enum): - start_enum = int(enum[len(enum_root):]) + start_enum = int(enum[len(enum_root) :]) start_value, count = value for i in range(count): enums[enum_root + str(start_enum + i)] = start_value + i + def _init_messages(self, messages, command_ids=[], output_ids=[]): for msgformat, msgid in messages.items(): - msgtype = 'response' + msgtype = "response" if msgid in command_ids: - msgtype = 'command' + msgtype = "command" elif msgid in output_ids: - msgtype = 'output' + msgtype = "output" self.messages.append((msgid, msgtype, msgformat)) self.msgid_by_format[msgformat] = msgid msgid_bytes = [] self.msgid_parser.encode(msgid_bytes, msgid) - if msgtype == 'output': - self.messages_by_id[msgid] = OutputFormat(msgid_bytes, - msgformat) + if msgtype == "output": + self.messages_by_id[msgid] = OutputFormat(msgid_bytes, msgformat) else: msg = MessageFormat(msgid_bytes, msgformat, self.enumerations) self.messages_by_id[msgid] = msg self.messages_by_name[msg.name] = msg + def process_identify(self, data, decompress=True): try: if decompress: data = zlib.decompress(data) self.raw_identify_data = data data = json.loads(data) - self.fill_enumerations(data.get('enumerations', {})) - commands = data.get('commands') - responses = data.get('responses') - output = data.get('output', {}) + self.fill_enumerations(data.get("enumerations", {})) + commands = data.get("commands") + responses = data.get("responses") + output = data.get("output", {}) all_messages = dict(commands) all_messages.update(responses) all_messages.update(output) - self._init_messages(all_messages, commands.values(), - output.values()) - self.config.update(data.get('config', {})) - self.version = data.get('version', '') - self.build_versions = data.get('build_versions', '') + self._init_messages(all_messages, commands.values(), output.values()) + self.config.update(data.get("config", {})) + self.version = data.get("version", "") + self.build_versions = data.get("build_versions", "") except error as e: raise except Exception as e: logging.exception("process_identify error") self._error("Error during identify: %s", str(e)) + def get_raw_data_dictionary(self): return self.raw_identify_data + def get_version_info(self): return self.version, self.build_versions + def get_messages(self): return list(self.messages) + def get_enumerations(self): return dict(self.enumerations) + def get_constants(self): return dict(self.config) - class sentinel: pass + + class sentinel: + pass + def get_constant(self, name, default=sentinel, parser=str): if name not in self.config: if default is not self.sentinel: @@ -431,10 +501,13 @@ class MessageParser: try: value = parser(self.config[name]) except: - self._error("Unable to parse firmware constant %s: %s", - name, self.config[name]) + self._error( + "Unable to parse firmware constant %s: %s", name, self.config[name] + ) return value + def get_constant_float(self, name, default=sentinel): return self.get_constant(name, default, parser=float) + def get_constant_int(self, name, default=sentinel): return self.get_constant(name, default, parser=int) |