#!/usr/bin/env python3
"""Exploratory cnc_rdparam (0x000e) v3 request-framing probe against a live FANUC.

The committed WireFocasClient sends arg1=number, arg2=number(when axis 0), arg3=0 and
gets EW_FUNC(1) for a VALID parameter on the 31i-B. cnc_rdparam(h, number, axis, length,
&IODBPSD) needs axis + length, so this tries a matrix of (arg ordering, axis, length,
reqClass, extra) and prints which combos return rc=0 with a non-empty payload.

Read-only. Usage: python3 param-probe.py <host> [port]
"""
import socket
import sys

MAGIC = bytes([0xA0, 0xA0, 0xA0, 0xA0])
EMIT_VERSION = 1


def build_pdu(version, type_, direction, body):
    h = bytearray(MAGIC) + version.to_bytes(2, "big") + bytes([type_, direction]) + len(body).to_bytes(2, "big")
    return bytes(h) + body


def build_block(cmd, a1=0, a2=0, a3=0, a4=0, a5=0, req_class=1, path_id=1, extra=b""):
    blk = bytearray()
    blk += (0x1C + len(extra)).to_bytes(2, "big")
    blk += req_class.to_bytes(2, "big")
    blk += path_id.to_bytes(2, "big")
    blk += cmd.to_bytes(2, "big")
    for a in (a1, a2, a3, a4):
        blk += int(a).to_bytes(4, "big", signed=True)
    blk += (a5 & 0xFFFF).to_bytes(2, "big")
    blk += len(extra).to_bytes(2, "big")
    blk += extra
    return bytes(blk)


def data_pdu(blocks):
    body = bytearray(len(blocks).to_bytes(2, "big"))
    for b in blocks:
        body += b
    return build_pdu(EMIT_VERSION, 0x21, 0x01, bytes(body))


def recv_exactly(sock, n):
    buf = b""
    while len(buf) < n:
        try:
            chunk = sock.recv(n - len(buf))
        except socket.timeout:
            return buf, False
        if not chunk:
            return buf, False
        buf += chunk
    return buf, True


def read_pdu(sock):
    header, ok = recv_exactly(sock, 10)
    if not ok or len(header) < 10:
        return None
    body_len = int.from_bytes(header[8:10], "big")
    body, _ = recv_exactly(sock, body_len)
    return body


def parse_first_block(body):
    """Return (cmd, rc, payload_hex) of the first response block, or None."""
    if not body or len(body) < 2:
        return None
    count = int.from_bytes(body[0:2], "big")
    if count == 0 or len(body) < 18:
        return (None, None, "")
    blk_len = int.from_bytes(body[2:4], "big")
    cmd = int.from_bytes(body[8:10], "big")
    rc = int.from_bytes(body[10:12], "big", signed=True)
    payload_len = int.from_bytes(body[16:18], "big")
    payload = body[18:18 + payload_len]
    return (cmd, rc, " ".join(f"{x:02x}" for x in payload) or "(empty)")


class Sess:
    def __init__(self, host, port):
        self.host, self.port = host, port
        self.s1 = self.s2 = None

    def _sock(self):
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
        s.settimeout(6)
        s.connect((self.host, self.port))
        return s

    def connect(self):
        self.s1 = self._sock()
        self.s1.sendall(build_pdu(EMIT_VERSION, 0x01, 0x01, (1).to_bytes(2, "big")))
        read_pdu(self.s1)
        self.s2 = self._sock()
        self.s2.sendall(build_pdu(EMIT_VERSION, 0x01, 0x01, (2).to_bytes(2, "big")))
        read_pdu(self.s2)
        # mirror the client's setup requests
        self.req([build_block(0x0018, path_id=1)])
        self.req([build_block(0x000E, 0x26F0, 0x26F0, path_id=1)])

    def req(self, blocks):
        self.s2.sendall(data_pdu(blocks))
        return read_pdu(self.s2)

    def close(self):
        for s in (self.s2, self.s1):
            try:
                s and s.close()
            except Exception:
                pass


def main():
    host = sys.argv[1] if len(sys.argv) > 1 else "10.201.31.5"
    port = int(sys.argv[2]) if len(sys.argv) > 2 else 8193
    # 8130 = total controlled axes (global, always present); 1320 = +stroke limit (axis);
    # 1825 = servo loop gain (axis); 3201 = setting. All exist on a 31i.
    params = [8130, 1320, 1825, 3201]
    # (label, builder(P)) — each builder returns a single request block for param P
    variants = [
        ("cur arg1=P,arg2=P,a3=0,rc1", lambda P: build_block(0x000E, P, P, 0, 0, req_class=1)),
        ("arg2=0(axis),a3=0,rc1", lambda P: build_block(0x000E, P, 0, 0, 0, req_class=1)),
        ("arg2=0,a3=8(len),rc1", lambda P: build_block(0x000E, P, 0, 8, 0, req_class=1)),
        ("arg2=0,a3=36(len),rc1", lambda P: build_block(0x000E, P, 0, 36, 0, req_class=1)),
        ("arg2=0,a3=4(len),rc1", lambda P: build_block(0x000E, P, 0, 4, 0, req_class=1)),
        ("arg2=-1(allaxis),a3=8,rc1", lambda P: build_block(0x000E, P, -1, 8, 0, req_class=1)),
        ("arg2=1(axis1),a3=8,rc1", lambda P: build_block(0x000E, P, 1, 8, 0, req_class=1)),
        ("arg2=0,a3=8,rc2", lambda P: build_block(0x000E, P, 0, 8, 0, req_class=2)),
        ("arg2=0,a3=36,rc2", lambda P: build_block(0x000E, P, 0, 36, 0, req_class=2)),
        ("len-first arg1=8,arg2=P,rc1", lambda P: build_block(0x000E, 8, P, 0, 0, req_class=1)),
        ("len-first arg1=36,arg2=P,rc1", lambda P: build_block(0x000E, 36, P, 0, 0, req_class=1)),
        ("arg2=0,a3=0,a4=8,rc1", lambda P: build_block(0x000E, P, 0, 0, 8, req_class=1)),
        ("arg2=0,a5=8,rc1", lambda P: build_block(0x000E, P, 0, 0, 0, a5=8, req_class=1)),
        ("extra: datano+type+len(8)", lambda P: build_block(0x000E, P, 0, 8, 0, req_class=1,
                                                            extra=P.to_bytes(2, "big") + b"\x00\x00" + (8).to_bytes(2, "big"))),
    ]

    sess = Sess(host, port)
    sess.connect()
    print(f"# cnc_rdparam v3 probe {host}:{port}\n")
    hits = []
    try:
        for P in params:
            print(f"--- param {P} ---")
            for label, build in variants:
                body = sess.req([build(P)])
                parsed = parse_first_block(body)
                if parsed is None:
                    print(f"  {label:32s} -> (no/short response)")
                    continue
                cmd, rc, payload = parsed
                mark = "  <== HIT" if rc == 0 and payload not in ("", "(empty)") else ""
                print(f"  {label:32s} -> rc={rc} payload={payload}{mark}")
                if mark:
                    hits.append((P, label, payload))
            print()
    finally:
        sess.close()

    print("# HITS (rc=0, non-empty payload):")
    for P, label, payload in hits:
        print(f"  param {P}: {label} -> {payload}")
    if not hits:
        print("  (none — cnc_rdparam may be genuinely restricted on this control)")


if __name__ == "__main__":
    main()