"""_probe_wedge.py — 楔死现场逐环节定位(纠正之前 0x0810/0x0818=SM2/SM3 的标号错误,真邮箱 SM 是 0x0800/0x0808)。

环节定位:
  A. FPWR 写 SM0 RAM 的 WKC —— 写都进不去 = SM0 硬件层被拒(disabled/错址)
  B. 写入后 0x220 bit0(SM0 写事件)是否置位 —— ESC 没置事件 = ESC 硬件楔死
  C. 200ms 后 bit0 是否被 SSC 清掉 + SM1 status bit3(读邮箱满)是否置位 —— 没处理 = SSC 软件死了
  D. SM1 RAM 里有没有滞留的旧响应(bSendMbxIsFull 死锁证据)
"""
import struct
import sys
import time

import ecat_server as es
import pysoem

r = es.api_open({"substr": "I226"})
if not r.get("ok"):
    print("open 失败,板子可能已断电", flush=True)
    sys.exit(1)
m = es.g_master
s = m.slaves[0]


def fprd(addr, size):
    try:
        return bytes(s._fprd(addr, size, 20000))
    except Exception:
        return None


def dump(tag):
    al = fprd(0x0130, 8)
    ev = fprd(0x0220, 2)
    am = fprd(0x0204, 2)
    sm0 = fprd(0x0800, 8)
    sm1 = fprd(0x0808, 8)
    def fmt(d):
        if d is None:
            return "ERR"
        start, length = struct.unpack("<HH", d[:4])
        return f"start=0x{start:04X} len={length} ctl=0x{d[4]:02X} st=0x{d[5]:02X} act=0x{d[6]:02X} pdi=0x{d[7]:02X}"
    al_s = f"AL=0x{struct.unpack('<H', al[:2])[0]:04X} code=0x{struct.unpack('<H', al[4:6])[0]:04X}" if al else "AL=ERR"
    ev_v = struct.unpack("<H", ev)[0] if ev else -1
    am_v = struct.unpack("<H", am)[0] if am else -1
    print(f"[{tag}] {al_s} 0x220=0x{ev_v:04X}(bit0={ev_v & 1}bit1={(ev_v >> 1) & 1}) 0x204=0x{am_v:04X}", flush=True)
    print(f"        SM0(0x0800): {fmt(sm0)}", flush=True)
    print(f"        SM1(0x0808): {fmt(sm1)}", flush=True)
    return sm0, sm1, ev_v


# --- 阶段 1: PREOP 下探(邮箱 SM 应处于使能态) ---
m.state = pysoem.INIT_STATE | pysoem.STATE_ACK
m.write_state()
m.state_check(pysoem.INIT_STATE, 1_000_000)
m.config_init()
m.state = pysoem.PREOP_STATE | pysoem.STATE_ACK
m.write_state()
st = m.state_check(pysoem.PREOP_STATE, 1_000_000)
print(f"PREOP state_check = 0x{st & 0x0F:02X}", flush=True)
sm0, sm1, ev = dump("PREOP 稳态")

if sm0 is None or sm1 is None:
    print("SM0/SM1 读取失败", flush=True)
    es.api_close({})
    sys.exit(1)

# SM0/SM1 RAM 内容(滞留响应证据)
mbx0_addr, mbx0_len = struct.unpack("<HH", sm0[:4])
mbx1_addr, mbx1_len = struct.unpack("<HH", sm1[:4])
print(f"邮箱 RAM: SM0@0x{mbx0_addr:04X}({mbx0_len}B) SM1@0x{mbx1_addr:04X}({mbx1_len}B)", flush=True)
for name, addr, ln in (("SM0", mbx0_addr, mbx0_len), ("SM1", mbx1_addr, mbx1_len)):
    d = fprd(addr, 16)
    if d:
        print(f"        {name} RAM[0:16] = {d.hex().upper()}", flush=True)

# --- 阶段 2: 手动注入一个合法 CoE SDO 读请求(0x1000:00 expedited upload) ---
# 邮箱帧: len=0x000A addr=0x0000 pri=0 type=0x03(CoE, counter=0) | CANOpen=0x1000+8(UP_REQ) idx=0x1000 sub=0
req = struct.pack("<HHBB", 10, 0, 0, 0x03) + struct.pack("<HBBB", 0x1000, 0x100 & 0xFF, 0x00, 0x00)[:5]
req = bytes([0x0A, 0x00, 0x00, 0x00, 0x00, 0x03, 0x10, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
print(f"\n--- 阶段A: FPWR 写 SM0 RAM(0x{mbx0_addr:04X}) {req.hex().upper()} ---", flush=True)
try:
    s._fpwr(mbx0_addr, req)
    print("  写入 OK (WKC=1) —— SM0 硬件接受写入", flush=True)
except Exception as e:
    print(f"  写入失败: {type(e).__name__}: {e} —— SM0 硬件拒绝(环节A死点)", flush=True)

ev2 = fprd(0x0220, 2)
if ev2:
    v = struct.unpack("<H", ev2)[0]
    print(f"  阶段B: 0x220=0x{v:04X} bit0(SM0写事件)={v & 1} —— {'已置位' if v & 1 else '未置位!ESC 硬件楔死'}", flush=True)

# --- 阶段 3: 给 SSC 200ms 处理时间 ---
time.sleep(0.2)
sm0b, sm1b, ev3 = dump("注入请求 200ms 后")
sm1_st = sm1b[5] if sm1b else 0
print(f"  阶段C: 0x220 bit0={ev3 & 1}(应=0,SSC 已读走) SM1 st=0x{sm1_st:02X}(bit3={sm1_st >> 3 & 1},应=1,响应就绪)", flush=True)
if ev3 is not None and (ev3 & 1):
    print("  >>> bit0 仍挂起:SSC 轮询没处理邮箱(环节C死点:MainLoop/ECAT_Main 邮箱分支死了)", flush=True)
elif sm1_st & 0x08:
    d = fprd(mbx1_addr, 16)
    print(f"  阶段D: SM1 响应 RAM = {d.hex().upper() if d else 'ERR'}", flush=True)
    print("  >>> SSC 活着!邮箱处理正常 —— 楔死只在特定条件下", flush=True)
else:
    print("  >>> bit0 被清了但 SM1 无响应:SSC 收到了但没回(处理链中途死)", flush=True)

es.api_close({})
print("DONE", flush=True)