158 lines
3.9 KiB
Python
158 lines
3.9 KiB
Python
#!/usr/bin/env python
|
|
|
|
'''
|
|
decode FTP file transfers from tlog
|
|
'''
|
|
|
|
|
|
import time
|
|
import struct
|
|
import sys
|
|
|
|
from argparse import ArgumentParser
|
|
parser = ArgumentParser(description=__doc__)
|
|
parser.add_argument("logs", metavar="LOG", nargs="+")
|
|
|
|
args = parser.parse_args()
|
|
|
|
from pymavlink import mavutil
|
|
|
|
class Block(object):
|
|
def __init__(self, offset, size, data):
|
|
self.offset = offset
|
|
self.size = size
|
|
self.data = data
|
|
|
|
class Transfer(object):
|
|
def __init__(self, filename):
|
|
self.filename = filename
|
|
self.blocks = []
|
|
|
|
def extract(self):
|
|
self.blocks.sort(key = lambda x: x.offset)
|
|
data = bytes()
|
|
for b in self.blocks:
|
|
if b.offset != len(data):
|
|
print("gap at %u" % len(data))
|
|
return None
|
|
data += bytes(b.data)
|
|
return data
|
|
|
|
def param_decode(data):
|
|
'''decode param packed data'''
|
|
magic = 0x671b
|
|
magic_def = 0x671c
|
|
magic2,num_params,total_params = struct.unpack("<HHH", data[0:6])
|
|
if magic != magic2 and magic_def != magic2:
|
|
print("paramftp: bad magic 0x%x expected 0x%x" % (magic2, magic))
|
|
return
|
|
with_defaults = magic2 == magic_def
|
|
data = data[6:]
|
|
|
|
# mapping of data type to type length and format
|
|
data_types = {
|
|
1: (1, 'b'),
|
|
2: (2, 'h'),
|
|
3: (4, 'i'),
|
|
4: (4, 'f'),
|
|
}
|
|
|
|
count = 0
|
|
params = []
|
|
|
|
if sys.version_info.major < 3:
|
|
pad_byte = chr(0)
|
|
last_name = ''
|
|
else:
|
|
pad_byte = 0
|
|
last_name = bytes()
|
|
ret = []
|
|
|
|
while True:
|
|
while len(data) > 0 and data[0] == pad_byte:
|
|
# skip pad bytes
|
|
data = data[1:]
|
|
|
|
if len(data) == 0:
|
|
break
|
|
|
|
ptype, plen = struct.unpack("<BB", data[0:2])
|
|
flags = (ptype>>4) & 0x0F
|
|
ptype &= 0x0F
|
|
|
|
if not ptype in data_types:
|
|
print("paramftp: bad type 0x%x" % ptype)
|
|
return
|
|
|
|
(type_len, type_format) = data_types[ptype]
|
|
|
|
if flags & 1:
|
|
default_len = type_len
|
|
else:
|
|
default_len = 0
|
|
|
|
name_len = ((plen>>4) & 0x0F) + 1
|
|
common_len = (plen & 0x0F)
|
|
name = last_name[0:common_len] + data[2:2+name_len]
|
|
vdata = data[2+name_len:2+name_len+type_len]
|
|
last_name = name
|
|
data = data[2+name_len+type_len+default_len:]
|
|
v, = struct.unpack("<" + type_format, vdata)
|
|
ret.append((name, v, ptype))
|
|
count += 1
|
|
return ret
|
|
|
|
|
|
|
|
# transfers indexed by session ID
|
|
ftp_transfers = {}
|
|
ftp_block_size = 0
|
|
|
|
FTP_OpenFileRO = 4
|
|
FTP_ReadFile = 5
|
|
FTP_BurstReadFile = 15
|
|
FTP_Ack = 128
|
|
FTP_NAck = 129
|
|
|
|
def ftp_add(m):
|
|
session = m.payload[2]
|
|
opcode = m.payload[3]
|
|
size = m.payload[4]
|
|
req_opcode = m.payload[5]
|
|
burst_complete = m.payload[6]
|
|
data = m.payload[12:12+size]
|
|
if opcode == FTP_OpenFileRO:
|
|
filename = data
|
|
ftp_transfers[session] = Transfer(bytes(filename))
|
|
if req_opcode in [FTP_ReadFile, FTP_BurstReadFile] and opcode == FTP_Ack:
|
|
if not session in ftp_transfers:
|
|
print("No session %u" % session)
|
|
return
|
|
offset, = struct.unpack("<I", bytes(m.payload[8:12]))
|
|
ftp_transfers[session].blocks.append(Block(offset,size,data))
|
|
|
|
def mavparse(logfile):
|
|
'''extract FTP transfers'''
|
|
mlog = mavutil.mavlink_connection(filename)
|
|
|
|
while True:
|
|
try:
|
|
m = mlog.recv_match(type=['FILE_TRANSFER_PROTOCOL'])
|
|
if m is None:
|
|
return
|
|
except Exception:
|
|
return
|
|
if m.get_type() == 'FILE_TRANSFER_PROTOCOL':
|
|
ftp_add(m)
|
|
|
|
for filename in args.logs:
|
|
mavparse(filename)
|
|
|
|
for session in ftp_transfers:
|
|
f = ftp_transfers[session]
|
|
print('# %s' % f.filename.decode())
|
|
if f.filename.decode().startswith('@PARAM/param.pck'):
|
|
plist = param_decode(f.extract())
|
|
for p in plist:
|
|
print(p[0].decode(), p[1])
|