#!/usr/bin/env python3
"""抓 SOEM 自己发的 EtherCAT 帧，对照裸帧格式。
另开 pcap 句柄抓包，同时在同进程里跑 pysoem eeprom_read，hexdump 收发帧。
用法：ecat_sniff.py [adapter-substr]"""
import ctypes
import struct
import sys
import threading
import time

import pysoem

ECAT = b"\x88\xa4"


class PcapPktHdr(ctypes.Structure):
    _fields_ = [("tv_sec", ctypes.c_long), ("tv_usec", ctypes.c_long),
                ("caplen", ctypes.c_uint32), ("len", ctypes.c_uint32)]


def main():
    substr = sys.argv[1] if len(sys.argv) > 1 else "I226"
    adapter = None
    for a in pysoem.find_adapters():
        desc = a.desc.decode() if isinstance(a.desc, bytes) else a.desc
        if substr.lower() in desc.lower():
            adapter = a
            break
    if adapter is None:
        raise SystemExit("适配器未找到")
    name = adapter.name.decode() if isinstance(adapter.name, bytes) else adapter.name

    wpcap = ctypes.WinDLL("wpcap.dll")
    wpcap.pcap_open_live.restype = ctypes.c_void_p
    wpcap.pcap_open_live.argtypes = [ctypes.c_char_p] + [ctypes.c_int] * 3 + [ctypes.c_char_p]
    wpcap.pcap_next_ex.restype = ctypes.c_int
    wpcap.pcap_next_ex.argtypes = [ctypes.c_void_p,
                                   ctypes.POINTER(ctypes.POINTER(PcapPktHdr)),
                                   ctypes.POINTER(ctypes.POINTER(ctypes.c_ubyte))]
    wpcap.pcap_close.restype = None
    wpcap.pcap_close.argtypes = [ctypes.c_void_p]
    errbuf = ctypes.create_string_buffer(256)
    handle = wpcap.pcap_open_live(name.encode(), 65535, 1, 50, errbuf)
    if not handle:
        raise SystemExit(f"pcap_open_live 失败: {errbuf.value.decode(errors='replace')}")

    def soem_work():
        try:
            master = pysoem.Master()
            master.open(adapter.name)
            master.config_init()
            slave = master.slaves[0]
            slave.eeprom_read(0)
            master.close()
        except Exception as e:
            print(f"soem_work: {type(e).__name__}: {e}")

    t = threading.Thread(target=soem_work, daemon=True)
    t.start()

    frames = []
    deadline = time.time() + 4.0
    ph = ctypes.POINTER(PcapPktHdr)()
    pd = ctypes.POINTER(ctypes.c_ubyte)()
    while time.time() < deadline and len(frames) < 8:
        r = wpcap.pcap_next_ex(handle, ctypes.byref(ph), ctypes.byref(pd))
        if r != 1:
            continue
        data = bytes(bytearray(pd[:ph.contents.caplen]))
        if len(data) >= 14 and data[12:14] == ECAT:
            frames.append(data)
    wpcap.pcap_close(handle)

    print(f"抓到 {len(frames)} 个 EtherCAT 帧")
    for i, f in enumerate(frames):
        src = ":".join(f"{b:02X}" for b in f[6:12])
        print(f"\n[{i}] len={len(f)} src={src}")
        for off in range(14, min(len(f), 14 + 48), 16):
            chunk = f[off:off + 16]
            print(f"  +{off - 14:02d}: " + " ".join(f"{b:02X}" for b in chunk))
        # 解析前两个 datagram 头字段，确定头部布局
        elen = struct.unpack_from("<H", f, 14)[0] & 0x7FF
        print(f"  ecat_hdr.len={elen} type={struct.unpack_from('<H', f, 14)[0] >> 12}")
        d = f[16:]
        if len(d) >= 12:
            cmd, idx, adp, ado, ln = struct.unpack_from("<BBHHH", d, 0)
            print(f"  dgram: cmd=0x{cmd:02X} idx={idx} adp=0x{adp:04X} "
                  f"ado=0x{ado:04X} len_field={ln} (len={ln & 0x7FF})")


if __name__ == "__main__":
    main()