init pro_sdk
This commit is contained in:
cyy_mac
212
tools/_compare_lowcmd.py
Normal file
212
tools/_compare_lowcmd.py
Normal file
@@ -0,0 +1,212 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
解析 pcap, 取出真实 LowCmd 密文 → Blowfish 解密 → 跟本地 buildCmd() 字节对比
|
||||
找出剩余差异 (SN/version/frameReserve/CRC/padding 等)
|
||||
"""
|
||||
import sys, os, struct, argparse
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '../..'))
|
||||
sys.path.insert(0, os.path.dirname(__file__))
|
||||
from blowfish_codec import Blowfish
|
||||
from ucl.lowCmd import lowCmd
|
||||
from ucl.complex import motorCmd, motorCmdArray
|
||||
|
||||
|
||||
# LowCmd 字段布局 (基于 buildCmd 源码)
|
||||
LAYOUT = [
|
||||
( 0, 2, 'head'),
|
||||
( 2, 3, 'levelFlag'),
|
||||
( 3, 4, 'frameReserve'),
|
||||
( 4, 12, 'SN'),
|
||||
( 12, 20, 'version'),
|
||||
( 20, 22, 'bandWidth'),
|
||||
( 22, 562, 'motorCmd[20]'),
|
||||
(562, 566, 'bms'),
|
||||
(566, 606, 'wirelessRemote'),
|
||||
(606, 610, 'reserve'),
|
||||
(610, 614, 'CRC'),
|
||||
(614, 616, 'padding'),
|
||||
]
|
||||
|
||||
|
||||
def field_at(offset):
|
||||
"""根据 offset 返回字段名"""
|
||||
for start, end, name in LAYOUT:
|
||||
if start <= offset < end:
|
||||
return name, start, end
|
||||
return '?', -1, -1
|
||||
|
||||
|
||||
def parse_pcap(path):
|
||||
"""提取所有 UDP payload (兼容 SLL / Ethernet)"""
|
||||
packets = []
|
||||
with open(path, 'rb') as f:
|
||||
gh = f.read(24)
|
||||
if len(gh) < 24: return packets
|
||||
magic = struct.unpack('<I', gh[:4])[0]
|
||||
endian = '<' if magic == 0xa1b2c3d4 else '>'
|
||||
linktype = struct.unpack(f'{endian}I', gh[20:24])[0]
|
||||
|
||||
while True:
|
||||
ph = f.read(16)
|
||||
if len(ph) < 16: break
|
||||
ts_sec, ts_usec, caplen, _ = struct.unpack(f'{endian}IIII', ph)
|
||||
pkt = f.read(caplen)
|
||||
if len(pkt) < caplen: break
|
||||
|
||||
if linktype == 1:
|
||||
if len(pkt) < 14 or struct.unpack('>H', pkt[12:14])[0] != 0x0800: continue
|
||||
ip_off = 14
|
||||
elif linktype == 113: # SLL
|
||||
if len(pkt) < 16 or struct.unpack('>H', pkt[14:16])[0] != 0x0800: continue
|
||||
ip_off = 16
|
||||
elif linktype == 276: # SLL2
|
||||
if len(pkt) < 20 or struct.unpack('>H', pkt[:2])[0] != 0x0800: continue
|
||||
ip_off = 20
|
||||
else:
|
||||
continue
|
||||
|
||||
if len(pkt) < ip_off + 20: continue
|
||||
ihl = (pkt[ip_off] & 0x0f) * 4
|
||||
if pkt[ip_off + 9] != 17: continue
|
||||
udp_off = ip_off + ihl
|
||||
if len(pkt) < udp_off + 8: continue
|
||||
_, _, ulen, _ = struct.unpack('>HHHH', pkt[udp_off:udp_off+8])
|
||||
payload = pkt[udp_off+8:udp_off+ulen]
|
||||
packets.append(payload)
|
||||
return packets
|
||||
|
||||
|
||||
def build_local_lowcmd(encrypted=True):
|
||||
"""本地生成 LowCmd (跟 test_sin_leg / lowcmd_codec 一致)"""
|
||||
lcmd = lowCmd()
|
||||
lcmd.SN = bytearray.fromhex('0402030508020100')
|
||||
lcmd.version = bytearray.fromhex('000109000200fc7f')
|
||||
mca = motorCmdArray()
|
||||
for i in range(20):
|
||||
mca.setMotorCmd(i, motorCmd(mode=0, q=0, dq=0, tau=0, Kp=0, Kd=0))
|
||||
lcmd.motorCmd = mca
|
||||
plain = lcmd.buildCmd(debug=False) # 614B
|
||||
if not encrypted:
|
||||
return plain
|
||||
bf = Blowfish.from_state_file('data/lowlevel_captures/blowfish_state.bin')
|
||||
padded = plain + b'\x00' * (8 - len(plain) % 8)
|
||||
return padded, bf.encrypt_ecb(padded) # (明文616, 密文616)
|
||||
|
||||
|
||||
def hex_dump(b, indent=' '):
|
||||
for i in range(0, len(b), 32):
|
||||
print(f"{indent}{i:04x}: {b[i:i+32].hex()}")
|
||||
|
||||
|
||||
def diff(a, b, label_a='REAL', label_b='LOCAL'):
|
||||
"""对比两段字节, 按字段分组报告差异"""
|
||||
n = min(len(a), len(b))
|
||||
print(f"\n{'='*70}")
|
||||
print(f"对比 {label_a} ({len(a)}B) vs {label_b} ({len(b)}B)")
|
||||
print(f"{'='*70}")
|
||||
if len(a) != len(b):
|
||||
print(f"⚠️ 长度不一致")
|
||||
|
||||
# 按字段分组
|
||||
diffs_by_field = {}
|
||||
for i in range(n):
|
||||
if a[i] != b[i]:
|
||||
name, _, _ = field_at(i)
|
||||
diffs_by_field.setdefault(name, []).append(i)
|
||||
|
||||
if not diffs_by_field:
|
||||
print(f"✅ 完全一致")
|
||||
return
|
||||
|
||||
print(f"\n字段级差异:")
|
||||
for name in [n for _, _, n in LAYOUT]:
|
||||
if name not in diffs_by_field: continue
|
||||
idxs = diffs_by_field[name]
|
||||
start, end = None, None
|
||||
for s, e, nm in LAYOUT:
|
||||
if nm == name: start, end = s, e; break
|
||||
seg_a = a[start:end]
|
||||
seg_b = b[start:end]
|
||||
print(f"\n 📍 {name} [{start}..{end}] ({len(idxs)}/{end-start} 字节不同):")
|
||||
print(f" {label_a}: {seg_a.hex()}")
|
||||
print(f" {label_b}: {seg_b.hex()}")
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('pcap')
|
||||
parser.add_argument('--cipher-out', help='保存第一帧密文')
|
||||
parser.add_argument('--plain-out', help='保存第一帧解密后明文')
|
||||
parser.add_argument('--state', default='data/lowlevel_captures/blowfish_state.bin')
|
||||
args = parser.parse_args()
|
||||
|
||||
print(f"\n📥 解析 {args.pcap}...")
|
||||
packets = parse_pcap(args.pcap)
|
||||
print(f" 共 {len(packets)} 个 UDP 包")
|
||||
if not packets:
|
||||
print(" ❌ 空 pcap")
|
||||
return 1
|
||||
|
||||
# 长度统计
|
||||
from collections import Counter
|
||||
sizes = Counter(len(p) for p in packets)
|
||||
print(f" 包大小分布:")
|
||||
for sz, cnt in sorted(sizes.items()):
|
||||
print(f" {sz}B: {cnt}")
|
||||
|
||||
# 取第一帧
|
||||
real_cipher = packets[0]
|
||||
print(f"\n📦 真实 LowCmd 密文 ({len(real_cipher)}B):")
|
||||
hex_dump(real_cipher[:64])
|
||||
print(f" ...")
|
||||
hex_dump(real_cipher[-32:])
|
||||
|
||||
# 解密
|
||||
bf = Blowfish.from_state_file(args.state)
|
||||
aligned = (len(real_cipher) // 8) * 8
|
||||
real_plain = bf.decrypt_ecb(real_cipher[:aligned])
|
||||
print(f"\n🔓 解密后 ({len(real_plain)}B):")
|
||||
hex_dump(real_plain[:64])
|
||||
print(f" ...")
|
||||
hex_dump(real_plain[-32:])
|
||||
|
||||
if not real_plain.startswith(b'\xfe\xef\xff'):
|
||||
print(f"\n❌ 解密失败! 包头不是 feef ff")
|
||||
print(f" Blowfish state 可能不对, 或者不是 LowCmd 流")
|
||||
return 1
|
||||
print(f"\n✅ 包头匹配 feef ff")
|
||||
|
||||
# 保存
|
||||
if args.cipher_out:
|
||||
with open(args.cipher_out, 'wb') as f:
|
||||
f.write(real_cipher)
|
||||
print(f"💾 密文 → {args.cipher_out}")
|
||||
if args.plain_out:
|
||||
with open(args.plain_out, 'wb') as f:
|
||||
f.write(real_plain)
|
||||
print(f"💾 明文 → {args.plain_out}")
|
||||
|
||||
# 本地生成对比
|
||||
local_plain, local_cipher = build_local_lowcmd(encrypted=True)
|
||||
print(f"\n🛠 本地生成 LowCmd (明文 614B padded 616B):")
|
||||
hex_dump(local_plain[:64])
|
||||
print(f" ...")
|
||||
hex_dump(local_plain[-32:])
|
||||
|
||||
# 明文对比 (核心!)
|
||||
diff(real_plain, local_plain, '真实(解密后)', '本地(buildCmd)')
|
||||
|
||||
# 密文对比 (验证加密是否一致)
|
||||
diff(real_cipher, local_cipher, '真实密文', '本地加密')
|
||||
|
||||
# 看连续两帧真实包的变化 (说明哪些字段是动态的)
|
||||
if len(packets) > 1:
|
||||
p2 = bf.decrypt_ecb(packets[1][:aligned])
|
||||
diff(real_plain, p2, '真实#0', '真实#1')
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user