1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
|
#!/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 = 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 (complete, unpack, show)
|
