diff options
Diffstat (limited to 'decode/qmux.py')
| -rw-r--r-- | decode/qmux.py | 187 |
1 files changed, 187 insertions, 0 deletions
diff --git a/decode/qmux.py b/decode/qmux.py new file mode 100644 index 00000000..36a2f302 --- /dev/null +++ b/decode/qmux.py @@ -0,0 +1,187 @@ +#!/usr/bin/python +# -*- Mode: python; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details: +# +# Copyright (C) 2011 Red Hat, Inc. +# + +import binascii +import defs +import struct + +from qmiprotocol import services + +TP_REQUEST = 0x00 +TP_RESPONSE = 0x02 +TP_INDICATION = 0x04 + +def unpack(data, direction): + return binascii.unhexlify(data) + +def service_to_string(s): + try: + return services[s][0] + except KeyError: + return "" + +def qmi_cmd_to_string(cmdno, service): + (name, cmds) = services[service] + return cmds[cmdno][0] + +class Tlv: + def __init__(self, tlvid, size, data, service, cmdno, direction): + self.id = tlvid + self.size = size + self.data = data + if size != len(data): + raise ValueError("Mismatched TLV size! (got %d expected %d)" % (len(data), size)) + self.service = service + self.cmdno = cmdno + self.direction = direction + + def show_data(self, prefix): + line = "" + for i in self.data: + line += " %02x" % ord(i) + print prefix + " Data: %s" % line + + def show(self, prefix): + svc = services[self.service] + cmd = svc[1][self.cmdno] + tlvlist = None + if self.direction == TP_REQUEST: + tlvlist = cmd[1] + elif self.direction == TP_RESPONSE: + tlvlist = cmd[2] + elif self.direction == TP_INDICATION: + tlvlist = cmd[3] + else: + raise ValueError("Unknown TLV dir0ection %s" % self.direction) + + tlvname = "!!! UNKNOWN !!!" + if self.service == 1 and self.cmdno == 77: # WDS/SET_IP_FAMILY + tlvname = "WDS/Set IP Family/IP Family !!! NOT DEFINED !!!" + else: + try: + tlvname = tlvlist[self.id] + except KeyError: + pass + + print prefix + " TLV: 0x%02x (%s)" % (self.id, tlvname) + print prefix + " Size: 0x%04x" % self.size + if self.id == 2: + # Status response + (status, error) = struct.unpack("<HH", self.data) + if status == 0: + sstatus = "SUCCESS" + else: + sstatus = "ERROR" + print prefix + " Status: %d (%s)" % (status, sstatus) + + print prefix + " Error: %d" % error + else: + self.show_data(prefix) + print "" + +def get_tlvs(data, service, cmdno, direction): + tlvs = [] + while len(data) >= 3: + (tlvid, size) = struct.unpack("<BH", data[:3]) + if size > len(data) - 3: + raise ValueError("Malformed TLV ID %d size %d (len left %d)" % (tlvid, size, len(data))) + tlvs.append(Tlv(tlvid, size, data[3:3 + size], service, cmdno, direction)) + data = data[size + 3:] + if len(data) != 0: + raise ValueError("leftover data parsing tlvs") + return tlvs + +def show(data, prefix, direction): + if len(data) < 7: + return + + qmuxfmt = "<BHBBB" + sz = struct.calcsize(qmuxfmt) + (ifc, l, sender, service, cid) = struct.unpack(qmuxfmt, data[:sz]) + + if ifc != 0x01: + raise ValueError("Packet not QMUX") + + print prefix + "QMUX Header:" + print prefix + " len: 0x%04x" % l + + ssender = "" + if sender == 0x00: + ssender = "(client)" + elif sender == 0x80: + ssender = "(service)" + print prefix + " sender: 0x%02x %s" % (sender, ssender) + + sservice = service_to_string(service) + print prefix + " svc: 0x%02x (%s)" % (service, sservice) + + scid = "" + if cid == 0xff: + scid = "(broadcast)" + print prefix + " cid: 0x%02x %s" % (cid, scid) + + print "" + + # QMI header + data = data[sz:] + if service == 0: + qmifmt = "<BBHH" + else: + qmifmt = "<BHHH" + + sz = struct.calcsize(qmifmt) + (flags, txnid, cmdno, size) = struct.unpack(qmifmt, data[:sz]) + + print prefix + "QMI Header:" + + sflags = "" + if service == 0: + # Besides the CTL service header being shorter, the flags are different + if flags == 0x00: + flags = TP_REQUEST + elif flags == 0x01: + flags = TP_RESPONSE + elif flags == 0x02: + flags = TP_INDICATION + + if flags == TP_REQUEST: + sflags = "(request)" + elif flags == TP_RESPONSE: + sflags = "(response)" + elif flags == TP_INDICATION: + sflags = "(indication)" + else: + raise ValueError("Unknown flags %d" % flags) + print prefix + " Flags: 0x%02x %s" % (flags, sflags) + + print prefix + " TXN: 0x%04x" % txnid + + scmd = qmi_cmd_to_string(cmdno, service) + print prefix + " Cmd: 0x%04x (%s)" % (cmdno, scmd) + + print prefix + " Size: 0x%04x" % size + print "" + + data = data[sz:] + tlvs = get_tlvs(data, service, cmdno, flags) + for tlv in tlvs: + tlv.show(prefix) + + print "" + +def get_funcs(): + return (unpack, show) + |