ModemManager/decode/qmux.py

#!/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 complete(data, direction):
    # We don't handle QMUX frames spanning packets yet
    return True

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 = [ None, None, None ]
        try:
            cmd = svc[1][self.cmdno]
        except KeyError:
            pass
        except TypeError:
            pass
        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 direction %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
            except TypeError:
                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 = "!!! UNKNOWN !!!"
    try:
        scmd = qmi_cmd_to_string(cmdno, service)
    except KeyError:
        pass
    except TypeError:
        pass
    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 (complete, unpack, show)