Modbus-rtu/HostComputer/plsr_persistence_board_test.py

#!/usr/bin/env python3
"""Phase-based PLSR HSD/SFD persistence board acceptance tool.

Normal phases never corrupt storage and issue at most one SFD SAVE per
invocation.  Power removal/reset is intentionally manual so VBAT retention and
real reset behavior are tested rather than simulated by a software command.
"""

from __future__ import annotations

import argparse
import json
import tempfile
import time
from pathlib import Path

import serial

from plsr_modbus_frequency_test import RtuClient, choose_port, signed_dword_words


CONTROL_BASE = 1200
CONTROL_WINDOW_WORDS = 338
CALL_REQUEST = CONTROL_BASE + 8
CALL_RESPONSE = CONTROL_BASE + 24
COMMAND_REQUEST = CONTROL_BASE + 40
COMMAND_RESPONSE = CONTROL_BASE + 48
AXIS_STATUS_BASE = CONTROL_BASE + 64
AXIS_STATUS_WORDS = 48
PERSISTENCE_BASE = CONTROL_BASE + 270
PERSISTENCE_WORDS = 30
PERSISTENCE_REQUEST = CONTROL_BASE + 300
PERSISTENCE_RESPONSE = CONTROL_BASE + 308
PERSISTENCE_VERSION = 1
PERFORMANCE_VERSION = 7

S0_BASES = (1600, 1800, 2000, 2200)
S1_BASES = (1700, 1900, 2100, 2300)
KNOWN_POSITIONS = (123456, -234567, 345678, -456789)

RESULT_OK = 0
RESULT_QUEUED = 1
RESULT_BUSY = 8
RESULT_NOT_SUPPORTED = 10
PERSISTENCE_OK = 0
PERSISTENCE_NOT_IMPLEMENTED = 4
STATE_IDLE = 1
STATE_ACCEL = 2
STATE_RUN = 3
STATE_DECEL = 4
STATE_STOPPED = 8

CALL_COMMIT = 1
CALL_START = 2
CMD_STOP_IMMEDIATE = 2
CMD_SET_POSITION = 5
CMD_SAVE_CONFIG = 8

DESTRUCTIVE_MAGIC_A = 0xDA7A
DESTRUCTIVE_MAGIC_B = 0x51F0
DESTRUCTIVE_ARM = 0xA55A
DESTRUCTIVE_CONFIRM = "INVALIDATE-NEWEST-SLOT"


def put_u32(words: list[int], offset: int, value: int) -> None:
    words[offset : offset + 2] = signed_dword_words(value)


def put_u64(words: list[int], offset: int, value: int) -> None:
    raw = value & 0xFFFFFFFFFFFFFFFF
    words[offset : offset + 4] = [
        (raw >> shift) & 0xFFFF for shift in (0, 16, 32, 48)
    ]


def get_u32(words: list[int], offset: int) -> int:
    return words[offset] | (words[offset + 1] << 16)


def get_u64(words: list[int], offset: int, signed: bool = False) -> int:
    raw = sum(words[offset + index] << (16 * index) for index in range(4))
    if signed and raw & (1 << 63):
        return raw - (1 << 64)
    return raw


def next_sequence() -> int:
    value = int(time.time_ns() // 1_000_000) & 0x7FFFFFFF
    return value if value != 0 else 1


def is_retryable_read_error(error: RuntimeError) -> bool:
    message = str(error)
    return message.startswith("响应超时：") or message.startswith("响应 CRC 错误：")


def wait_response(
    client: RtuClient, address: int, count: int, sequence: int, timeout: float = 3.0
) -> list[int]:
    deadline = time.monotonic() + timeout
    latest: list[int] | None = None
    last_error: RuntimeError | None = None
    while time.monotonic() < deadline:
        try:
            latest = client.read_holding(address, count)
        except RuntimeError as error:
            if not is_retryable_read_error(error):
                raise
            last_error = error
            time.sleep(0.05)
            continue
        if get_u32(latest, 0) == sequence:
            return latest
    raise RuntimeError(
        f"等待序号 {sequence} 应答超时，最后应答={latest}，"
        f"最后通信错误={last_error}"
    )


def send_command(
    client: RtuClient,
    sequence: int,
    axis: int,
    opcode: int,
    argument: int = 0,
    response_timeout: float = 3.0,
) -> list[int]:
    request = [0] * 8
    put_u32(request, 0, sequence)
    request[2] = opcode
    request[3] = axis
    put_u64(request, 4, argument)
    client.write_multiple(COMMAND_REQUEST, request)
    return wait_response(
        client, COMMAND_RESPONSE, 8, sequence, timeout=response_timeout
    )


def read_axis_status(client: RtuClient, axis: int) -> dict[str, int]:
    words = client.read_holding(
        AXIS_STATUS_BASE + axis * AXIS_STATUS_WORDS, AXIS_STATUS_WORDS
    )
    generation_begin = get_u32(words, 0)
    generation_end = get_u32(words, 46)
    if generation_begin != generation_end or generation_begin & 1:
        raise RuntimeError(
            f"轴{axis}状态快照不一致：{generation_begin}/{generation_end}"
        )
    return {
        "state": words[2],
        "flags": get_u32(words, 3),
        "last_result": words[8],
        "last_sequence": get_u32(words, 10),
        "logical_position": get_u64(words, 16, signed=True),
        "task_pulses": get_u64(words, 20, signed=True),
        "current_frequency": get_u32(words, 38),
    }


def wait_command_applied(
    client: RtuClient, axis: int, sequence: int, timeout: float = 5.0
) -> dict[str, int]:
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    last_error: RuntimeError | None = None
    while time.monotonic() < deadline:
        try:
            latest = read_axis_status(client, axis)
        except RuntimeError as error:
            if not is_retryable_read_error(error):
                raise
            last_error = error
            time.sleep(0.05)
            continue
        if latest["last_sequence"] == sequence:
            return latest
    raise RuntimeError(
        f"等待命令 {sequence} 内核执行超时，最后状态={latest}，"
        f"最后通信错误={last_error}"
    )


def read_persistence(client: RtuClient) -> dict[str, int]:
    latest: list[int] | None = None
    for _ in range(5):
        latest = client.read_holding(PERSISTENCE_BASE, PERSISTENCE_WORDS)
        generation_begin = get_u32(latest, 0)
        generation_end = get_u32(latest, 28)
        if generation_begin == generation_end and not generation_begin & 1:
            if latest[2] != PERSISTENCE_VERSION:
                raise RuntimeError(f"持久化诊断版本错误：{latest[2]}")
            hsd_newest = latest[4] & 0xFF
            sfd_newest = (latest[4] >> 8) & 0xFF
            hsd_generations = (get_u32(latest, 12), get_u32(latest, 14))
            sfd_generations = (get_u32(latest, 16), get_u32(latest, 18))
            return {
                "generation": generation_begin,
                "hsd_valid_mask": latest[3] & 0xFF,
                "sfd_valid_mask": (latest[3] >> 8) & 0xFF,
                "hsd_newest_mask": hsd_newest,
                "sfd_newest_mask": sfd_newest,
                "flags": latest[5],
                "last_hsd_load": latest[6],
                "last_sfd_load": latest[7],
                "last_hsd_save": latest[8],
                "last_sfd_save": latest[9],
                "last_sfd_erase": latest[10],
                "hsd_generation_a": hsd_generations[0],
                "hsd_generation_b": hsd_generations[1],
                "sfd_generation_a": sfd_generations[0],
                "sfd_generation_b": sfd_generations[1],
                "hsd_selected_generation": (
                    hsd_generations[0]
                    if hsd_newest == 1
                    else hsd_generations[1] if hsd_newest == 2 else 0
                ),
                "sfd_selected_generation": (
                    sfd_generations[0]
                    if sfd_newest == 1
                    else sfd_generations[1] if sfd_newest == 2 else 0
                ),
                "hsd_save_count": get_u32(latest, 20),
                "sfd_save_count": get_u32(latest, 22),
                "hsd_crc32": get_u32(latest, 24),
                "sfd_crc32": get_u32(latest, 26),
            }
    raise RuntimeError(f"持久化诊断快照连续不一致：{latest}")


def wait_persistence_clean(client: RtuClient, timeout: float = 4.0) -> dict[str, int]:
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_persistence(client)
        if not latest["flags"] & 1 and latest["hsd_valid_mask"] != 0:
            return latest
    raise RuntimeError(f"HSD 检查点未在期限内完成：{latest}")


def wait_hsd_checkpoint_advanced(
    client: RtuClient, baseline: dict[str, int], timeout: float = 4.0
) -> dict[str, int]:
    """Wait for the START safety checkpoint while motion keeps HSD dirty.

    Runtime position publication legitimately sets HSD dirty again on every
    pulse, so a running-axis test must verify a committed generation/save-count
    advance instead of waiting for the dirty flag to remain clear.
    """
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_persistence(client)
        if (
            latest["hsd_valid_mask"] != 0
            and latest["last_hsd_save"] == PERSISTENCE_OK
            and latest["hsd_selected_generation"]
            != baseline["hsd_selected_generation"]
            and latest["hsd_save_count"] != baseline["hsd_save_count"]
        ):
            return latest
    raise RuntimeError(f"START 的 HSD busy 检查点未在期限内提交：{latest}")


def send_call(
    client: RtuClient, sequence: int, axis: int, operation: int
) -> list[int]:
    request = [0] * 16
    put_u32(request, 0, sequence)
    request[2] = 0
    put_u32(request, 3, S0_BASES[axis])
    request[5] = 0
    put_u32(request, 6, S1_BASES[axis])
    request[8] = 0
    put_u32(request, 10, 1)
    request[12] = axis
    request[13] = 0
    request[14] = operation
    client.write_multiple(CALL_REQUEST, request)
    return wait_response(client, CALL_RESPONSE, 12, sequence)


def start_long_motion(client: RtuClient, sequence: int) -> int:
    s0 = [0] * 20
    put_u32(s0, 0, 1)
    put_u32(s0, 10, 100_000)
    put_u32(s0, 12, 50_000_000)
    client.write_multiple(S0_BASES[0], s0)
    client.write_multiple(S1_BASES[0], [0] * 4)

    response = send_call(client, sequence, 0, CALL_COMMIT)
    if response[3] != RESULT_OK or response[11] != 1:
        raise RuntimeError(f"长任务 COMMIT 失败：{response}")
    sequence += 1
    response = send_call(client, sequence, 0, CALL_START)
    if response[3] != RESULT_QUEUED:
        raise RuntimeError(f"长任务 START 失败：{response}")

    deadline = time.monotonic() + 5.0
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_axis_status(client, 0)
        if (
            latest["last_sequence"] == sequence
            and latest["state"] in {STATE_ACCEL, STATE_RUN, STATE_DECEL}
            and latest["flags"] & (1 << 1)
        ):
            return sequence + 1
    raise RuntimeError(f"长任务未进入真实输出态：{latest}")


def load_state(path: Path) -> dict[str, object]:
    if not path.exists():
        raise RuntimeError(f"阶段状态文件不存在：{path}")
    return json.loads(path.read_text(encoding="utf-8"))


def save_state(path: Path, state: dict[str, object]) -> None:
    path.parent.mkdir(parents=True, exist_ok=True)
    path.write_text(json.dumps(state, indent=2, ensure_ascii=False), encoding="utf-8")


def print_diagnostics(diag: dict[str, int]) -> None:
    print(
        "持久化诊断："
        f"HSD valid=0x{diag['hsd_valid_mask']:02X}, "
        f"gen={diag['hsd_selected_generation']}, crc=0x{diag['hsd_crc32']:08X}, "
        f"boot saves={diag['hsd_save_count']}; "
        f"SFD valid=0x{diag['sfd_valid_mask']:02X}, "
        f"gen={diag['sfd_selected_generation']}, crc=0x{diag['sfd_crc32']:08X}, "
        f"boot saves={diag['sfd_save_count']}; flags=0x{diag['flags']:04X}"
    )


def phase_hsd_prepare(client: RtuClient, state_path: Path) -> None:
    sequence = next_sequence()
    for axis, position in enumerate(KNOWN_POSITIONS):
        response = send_command(client, sequence, axis, CMD_SET_POSITION, position)
        if response[4] != RESULT_QUEUED:
            raise RuntimeError(f"轴{axis} SET_POSITION 未排队：{response}")
        status = wait_command_applied(client, axis, sequence)
        if status["last_result"] != RESULT_OK or status["logical_position"] != position:
            raise RuntimeError(f"轴{axis}位置检查失败：{status}")
        sequence += 1
    diag = wait_persistence_clean(client)
    if diag["last_hsd_save"] != PERSISTENCE_OK:
        raise RuntimeError(f"HSD 保存结果不是 OK：{diag}")
    save_state(state_path, {"positions": list(KNOWN_POSITIONS)})
    print_diagnostics(diag)
    print("HSD prepare PASS。现在关闭主电源（保持 VBAT），再执行 --phase hsd-verify。")


def phase_hsd_verify(client: RtuClient, state_path: Path) -> None:
    expected = [int(value) for value in load_state(state_path)["positions"]]
    for axis, position in enumerate(expected):
        status = read_axis_status(client, axis)
        if status["logical_position"] != position or not status["flags"] & (1 << 5):
            raise RuntimeError(f"轴{axis} HSD 恢复失败：期望{position}，实测{status}")
        if status["flags"] & (1 << 1):
            raise RuntimeError(f"轴{axis} 上电后意外输出：{status}")
    diag = read_persistence(client)
    if diag["last_hsd_load"] != PERSISTENCE_OK:
        raise RuntimeError(f"HSD 上电加载结果不是 OK：{diag}")
    if not diag["flags"] & (1 << 2) or diag["flags"] & (1 << 3):
        raise RuntimeError(f"HSD restored flags 不符合正常停机：{diag}")
    print_diagnostics(diag)
    print("HSD VBAT 掉主电恢复 PASS。")


def phase_busy_prepare(client: RtuClient) -> None:
    baseline = read_persistence(client)
    sequence = start_long_motion(client, next_sequence())
    diag = wait_hsd_checkpoint_advanced(client, baseline)
    print_diagnostics(diag)
    print(
        f"运行中掉电已就绪（下一序号{sequence}）。现在直接关闭主电源或硬复位，"
        "不要先发 STOP；重启后执行 --phase busy-verify。"
    )


def phase_busy_verify(client: RtuClient) -> None:
    for axis in range(4):
        status = read_axis_status(client, axis)
        if status["flags"] & (1 << 1):
            raise RuntimeError(f"轴{axis} 重启后仍有输出：{status}")
        if status["flags"] & (1 << 5):
            raise RuntimeError(f"轴{axis} 运行中复位后 position_valid 未清除：{status}")
        if status["state"] != STATE_IDLE:
            raise RuntimeError(f"轴{axis} 重启后不是 IDLE：{status}")
    diag = read_persistence(client)
    if not diag["flags"] & (1 << 3) or diag["flags"] & (1 << 2):
        raise RuntimeError(f"busy-reset restored flags 错误：{diag}")
    print_diagnostics(diag)
    print("运行中掉电/复位 PASS：未自动续跑，所有轴 position_valid=0。")


def phase_sfd_save(client: RtuClient, state_path: Path) -> None:
    before = read_persistence(client)
    sequence = next_sequence()
    response = send_command(
        client, sequence, 0, CMD_SAVE_CONFIG, response_timeout=10.0
    )
    if response[4] != RESULT_QUEUED:
        raise RuntimeError(f"SAVE_CONFIG 未排队：{response}")
    status = wait_command_applied(client, 0, sequence, timeout=10.0)
    if status["last_result"] != RESULT_OK:
        raise RuntimeError(f"SAVE_CONFIG 内核执行失败：{status}")
    time.sleep(0.1)
    after = read_persistence(client)
    was_dirty = bool(before["flags"] & (1 << 1))
    if was_dirty:
        if after["sfd_save_count"] != before["sfd_save_count"] + 1:
            raise RuntimeError(f"SFD 实际保存次数不正确：before={before}, after={after}")
        if after["last_sfd_save"] != PERSISTENCE_OK:
            raise RuntimeError(f"SFD 保存结果不是 OK：{after}")
        if after["sfd_selected_generation"] == before["sfd_selected_generation"]:
            raise RuntimeError(f"SFD generation 未前进：before={before}, after={after}")
    elif after["sfd_save_count"] != before["sfd_save_count"]:
        raise RuntimeError("SFD clean no-op 不应擦写 Flash")
    if after["sfd_valid_mask"] == 0:
        raise RuntimeError(f"SFD 没有可验证的有效槽：{after}")
    state = load_state(state_path) if state_path.exists() else {}
    state["sfd_generation"] = after["sfd_selected_generation"]
    state["sfd_crc32"] = after["sfd_crc32"]
    save_state(state_path, state)
    print_diagnostics(after)
    if not was_dirty:
        print("SFD 当前为 clean，本次 SAVE 正确地没有重复擦写；验证现有提交记录。")
    print("SFD save PASS。现在硬复位/掉电重启，再执行 --phase sfd-verify。")


def phase_sfd_verify(client: RtuClient, state_path: Path) -> None:
    state = load_state(state_path)
    expected_generation = int(state["sfd_generation"])
    expected_crc = int(state["sfd_crc32"])
    diag = read_persistence(client)
    if diag["last_sfd_load"] != PERSISTENCE_OK:
        raise RuntimeError(f"SFD 上电加载结果不是 OK：{diag}")
    if (
        diag["sfd_selected_generation"] != expected_generation
        or diag["sfd_crc32"] != expected_crc
    ):
        raise RuntimeError(
            "SFD 上电记录不一致："
            f"期望 gen={expected_generation}, crc=0x{expected_crc:08X}；实测={diag}"
        )
    print_diagnostics(diag)
    print("SFD A/B + CRC 上电加载 PASS。")


def phase_motion_save_busy(client: RtuClient) -> None:
    sequence = start_long_motion(client, next_sequence())
    before = read_persistence(client)
    response = send_command(client, sequence, 0, CMD_SAVE_CONFIG)
    if response[4] != RESULT_QUEUED:
        raise RuntimeError(f"运行中 SAVE_CONFIG 未进入命令队列：{response}")
    status = wait_command_applied(client, 0, sequence)
    if status["last_result"] != RESULT_BUSY:
        raise RuntimeError(f"运行中 SAVE_CONFIG 未返回 BUSY：{status}")
    after = read_persistence(client)
    if after["sfd_save_count"] != before["sfd_save_count"]:
        raise RuntimeError("运行中 SAVE_CONFIG 意外擦写了 Flash")
    sequence += 1
    response = send_command(client, sequence, 0, CMD_STOP_IMMEDIATE)
    if response[4] != RESULT_QUEUED:
        raise RuntimeError(f"清理 STOP_IMMEDIATE 未排队：{response}")
    status = wait_command_applied(client, 0, sequence)
    if status["last_result"] != RESULT_OK or status["state"] != STATE_STOPPED:
        raise RuntimeError(f"清理停止失败：{status}")
    print_diagnostics(after)
    print("运行中禁止 SFD Flash 擦写 PASS：SAVE_CONFIG=BUSY，save_count 未增加。")


def phase_invalidate(
    client: RtuClient, target: str, allow: bool, confirmation: str | None
) -> None:
    if not allow or confirmation != DESTRUCTIVE_CONFIRM:
        raise RuntimeError(
            "破坏性诊断未授权；必须同时使用 --allow-destructive "
            f"--confirm {DESTRUCTIVE_CONFIRM}"
        )
    before = read_persistence(client)
    if not before["flags"] & (1 << 4):
        raise RuntimeError("固件未显式启用 PLSR_ENABLE_DESTRUCTIVE_PERSISTENCE_DIAG")
    mask_key = "hsd_valid_mask" if target == "hsd" else "sfd_valid_mask"
    if before[mask_key] != 3:
        raise RuntimeError(f"必须先有两个有效槽，当前诊断={before}")
    sequence = next_sequence()
    inverse = (~sequence) & 0xFFFFFFFF
    request = [DESTRUCTIVE_MAGIC_A, DESTRUCTIVE_MAGIC_B]
    request += signed_dword_words(sequence)
    request += signed_dword_words(inverse)
    request += [1 if target == "hsd" else 2, DESTRUCTIVE_ARM]
    client.write_multiple(PERSISTENCE_REQUEST, request)
    response = wait_response(client, PERSISTENCE_RESPONSE, 8, sequence)
    if response[3] == RESULT_NOT_SUPPORTED:
        raise RuntimeError("破坏性诊断被固件拒绝（normal build）")
    if response[3] != RESULT_OK:
        raise RuntimeError(f"失效 newest {target.upper()} 槽失败：{response}")
    after = read_persistence(client)
    if after[mask_key] not in {1, 2}:
        raise RuntimeError(f"失效后应只剩一个有效槽：{after}")
    print_diagnostics(after)
    print(f"受控失效 newest {target.upper()} 槽 PASS；请立即执行对应 LOAD/重启回退验证。")


def main() -> int:
    parser = argparse.ArgumentParser(description="PLSR HSD/SFD 真机掉电与 Flash 验收")
    parser.add_argument(
        "--phase",
        required=True,
        choices=(
            "diagnostics",
            "hsd-prepare",
            "hsd-verify",
            "busy-prepare",
            "busy-verify",
            "sfd-save",
            "sfd-verify",
            "motion-save-busy",
            "invalidate-hsd",
            "invalidate-sfd",
        ),
    )
    parser.add_argument("--port", default="COM5")
    parser.add_argument("--baud", type=int, default=9600)
    parser.add_argument("--slave", type=int, default=1)
    parser.add_argument(
        "--state-file",
        type=Path,
        default=Path(tempfile.gettempdir()) / "plsr_persistence_board_state.json",
    )
    parser.add_argument("--allow-destructive", action="store_true")
    parser.add_argument("--confirm")
    args = parser.parse_args()

    with serial.Serial(
        port=choose_port(args.port),
        baudrate=args.baud,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_EVEN,
        stopbits=serial.STOPBITS_ONE,
        timeout=1.0,
        write_timeout=1.0,
    ) as uart:
        client = RtuClient(uart, args.slave)
        header = client.read_holding(CONTROL_BASE, 8)
        if header[:5] != [0x504C, 0x5352, 0x0100, CONTROL_WINDOW_WORDS, 0x0007]:
            raise RuntimeError(f"控制窗口未就绪或固件过旧：{header}")
        if header[7] != PERFORMANCE_VERSION:
            raise RuntimeError(f"需要诊断版本 V{PERFORMANCE_VERSION}，当前 V{header[7]}")
        print(f"控制窗口 D1200~D1537 就绪；阶段状态文件：{args.state_file}")

        if args.phase == "diagnostics":
            print_diagnostics(read_persistence(client))
        elif args.phase == "hsd-prepare":
            phase_hsd_prepare(client, args.state_file)
        elif args.phase == "hsd-verify":
            phase_hsd_verify(client, args.state_file)
        elif args.phase == "busy-prepare":
            phase_busy_prepare(client)
        elif args.phase == "busy-verify":
            phase_busy_verify(client)
        elif args.phase == "sfd-save":
            phase_sfd_save(client, args.state_file)
        elif args.phase == "sfd-verify":
            phase_sfd_verify(client, args.state_file)
        elif args.phase == "motion-save-busy":
            phase_motion_save_busy(client)
        elif args.phase == "invalidate-hsd":
            phase_invalidate(client, "hsd", args.allow_destructive, args.confirm)
        else:
            phase_invalidate(client, "sfd", args.allow_destructive, args.confirm)
    return 0


if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except (RuntimeError, serial.SerialException, KeyError, ValueError) as error:
        print(f"测试失败：{error}")
        raise SystemExit(1)