saf.py

Go to the documentation of this file.

"""SaF - Python mirror of the safe-communication Safety Functional layer.
 
Byte-identical to ``Platform_Protocol_SafeComm`` (``src/mac/mac.c``, ``src/saf/saf.c``), the same way
``simcore.rbc_wire`` mirrors the C wire codec. The construction follows Subset-037-2 v4.0.0
("EuroRadio FIS - Safety Layer") directly:
 
* DES, pure Python so a sim needs no crypto package.
* CBC-MAC (Subset-037-2 SS6.2.2, "single DES with modified MAC algorithm 3"): every block but the last is
  single-DES under K1; the last block alone gets E(K3, D(K2, E(K1, .))). ISO/IEC 9797-1 Padding Method 1
  (zero bits, no extra block when the message is already a multiple of 64 bits). Verified against the
  official worked example in Subset-037-2 Annex B.
* KSMAC derivation (SS6.2.3.2.3.13-16): RA and RB are each split into 32-bit left/right halves; KS1/KS2 use
  the normal key order over the left/right halves, KS3 reuses the left half with the key order reversed.
* Frames AU1 (13) / AU2 (21) / AU3 (9) / AR (9) / DT (1+n+8) / DI (3, unauthenticated) per Subset-037-2
  SS6.2.5's tables: a packed ETY|MTI|DF header octet and 24-bit ETCS IDs. The MAC input for AU2/AU3/DT/AR
  follows SS6.2.3.2.1 (message origin authentication: length|DA|body) and SS6.2.3.2.2/Figure 6 (peer entity
  authentication) - DA is always the *receiver's own* identity.
* Sa-DISCONNECT carries no MAC or identity field at all (Table 6/13): SS6.2.3.3.3.2 says release-phase
  authentication is not required, since a forged one can only end the connection early, never inject or
  alter safety data. Any well-formed DI is honoured.
 
One deliberate, non-conformant addition: the official DT SaPDU carries no sequence number (Table 12) - that
defence is the Subset-098 SAI sub-layer's job (sequence numbering / Triple Time Stamping / EC counter,
SS5.4), which this module does not implement. In its place a 4-byte sequence number is carried inside the
MAC-covered user-data area, matching what ``saf.c`` does. This is a project extension, not itself
conformant to any subset - see ``docs/safety/SUBSET_MAPPING.md`` in the SafeComm library.
 
The session is transport-agnostic: the caller supplies ``send(bytes)`` and ``recv() -> bytes`` (one
frame per call). ``recv`` raises ``TimeoutError`` when nothing arrived in time.
 
This is a test tool. The MAC is cryptographically dated (see the C library's SUBSET_MAPPING.md).
"""
import os
 
# ---------------------------------------------------------------------------------------------------
# DES
# ---------------------------------------------------------------------------------------------------
_IP = (58, 50, 42, 34, 26, 18, 10, 2, 60, 52, 44, 36, 28, 20, 12, 4,
       62, 54, 46, 38, 30, 22, 14, 6, 64, 56, 48, 40, 32, 24, 16, 8,
       57, 49, 41, 33, 25, 17, 9, 1, 59, 51, 43, 35, 27, 19, 11, 3,
       61, 53, 45, 37, 29, 21, 13, 5, 63, 55, 47, 39, 31, 23, 15, 7)
_FP = (40, 8, 48, 16, 56, 24, 64, 32, 39, 7, 47, 15, 55, 23, 63, 31,
       38, 6, 46, 14, 54, 22, 62, 30, 37, 5, 45, 13, 53, 21, 61, 29,
       36, 4, 44, 12, 52, 20, 60, 28, 35, 3, 43, 11, 51, 19, 59, 27,
       34, 2, 42, 10, 50, 18, 58, 26, 33, 1, 41, 9, 49, 17, 57, 25)
_E = (32, 1, 2, 3, 4, 5, 4, 5, 6, 7, 8, 9, 8, 9, 10, 11, 12, 13,
      12, 13, 14, 15, 16, 17, 16, 17, 18, 19, 20, 21, 20, 21, 22, 23, 24, 25,
      24, 25, 26, 27, 28, 29, 28, 29, 30, 31, 32, 1)
_P = (16, 7, 20, 21, 29, 12, 28, 17, 1, 15, 23, 26, 5, 18, 31, 10,
      2, 8, 24, 14, 32, 27, 3, 9, 19, 13, 30, 6, 22, 11, 4, 25)
_PC1 = (57, 49, 41, 33, 25, 17, 9, 1, 58, 50, 42, 34, 26, 18,
        10, 2, 59, 51, 43, 35, 27, 19, 11, 3, 60, 52, 44, 36,
        63, 55, 47, 39, 31, 23, 15, 7, 62, 54, 46, 38, 30, 22,
        14, 6, 61, 53, 45, 37, 29, 21, 13, 5, 28, 20, 12, 4)
_PC2 = (14, 17, 11, 24, 1, 5, 3, 28, 15, 6, 21, 10, 23, 19, 12, 4,
        26, 8, 16, 7, 27, 20, 13, 2, 41, 52, 31, 37, 47, 55, 30, 40,
        51, 45, 33, 48, 44, 49, 39, 56, 34, 53, 46, 42, 50, 36, 29, 32)
_SHIFTS = (1, 1, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 1)
_SBOX = (
    (14, 4, 13, 1, 2, 15, 11, 8, 3, 10, 6, 12, 5, 9, 0, 7,
     0, 15, 7, 4, 14, 2, 13, 1, 10, 6, 12, 11, 9, 5, 3, 8,
     4, 1, 14, 8, 13, 6, 2, 11, 15, 12, 9, 7, 3, 10, 5, 0,
     15, 12, 8, 2, 4, 9, 1, 7, 5, 11, 3, 14, 10, 0, 6, 13),
    (15, 1, 8, 14, 6, 11, 3, 4, 9, 7, 2, 13, 12, 0, 5, 10,
     3, 13, 4, 7, 15, 2, 8, 14, 12, 0, 1, 10, 6, 9, 11, 5,
     0, 14, 7, 11, 10, 4, 13, 1, 5, 8, 12, 6, 9, 3, 2, 15,
     13, 8, 10, 1, 3, 15, 4, 2, 11, 6, 7, 12, 0, 5, 14, 9),
    (10, 0, 9, 14, 6, 3, 15, 5, 1, 13, 12, 7, 11, 4, 2, 8,
     13, 7, 0, 9, 3, 4, 6, 10, 2, 8, 5, 14, 12, 11, 15, 1,
     13, 6, 4, 9, 8, 15, 3, 0, 11, 1, 2, 12, 5, 10, 14, 7,
     1, 10, 13, 0, 6, 9, 8, 7, 4, 15, 14, 3, 11, 5, 2, 12),
    (7, 13, 14, 3, 0, 6, 9, 10, 1, 2, 8, 5, 11, 12, 4, 15,
     13, 8, 11, 5, 6, 15, 0, 3, 4, 7, 2, 12, 1, 10, 14, 9,
     10, 6, 9, 0, 12, 11, 7, 13, 15, 1, 3, 14, 5, 2, 8, 4,
     3, 15, 0, 6, 10, 1, 13, 8, 9, 4, 5, 11, 12, 7, 2, 14),
    (2, 12, 4, 1, 7, 10, 11, 6, 8, 5, 3, 15, 13, 0, 14, 9,
     14, 11, 2, 12, 4, 7, 13, 1, 5, 0, 15, 10, 3, 9, 8, 6,
     4, 2, 1, 11, 10, 13, 7, 8, 15, 9, 12, 5, 6, 3, 0, 14,
     11, 8, 12, 7, 1, 14, 2, 13, 6, 15, 0, 9, 10, 4, 5, 3),
    (12, 1, 10, 15, 9, 2, 6, 8, 0, 13, 3, 4, 14, 7, 5, 11,
     10, 15, 4, 2, 7, 12, 9, 5, 6, 1, 13, 14, 0, 11, 3, 8,
     9, 14, 15, 5, 2, 8, 12, 3, 7, 0, 4, 10, 1, 13, 11, 6,
     4, 3, 2, 12, 9, 5, 15, 10, 11, 14, 1, 7, 6, 0, 8, 13),
    (4, 11, 2, 14, 15, 0, 8, 13, 3, 12, 9, 7, 5, 10, 6, 1,
     13, 0, 11, 7, 4, 9, 1, 10, 14, 3, 5, 12, 2, 15, 8, 6,
     1, 4, 11, 13, 12, 3, 7, 14, 10, 15, 6, 8, 0, 5, 9, 2,
     6, 11, 13, 8, 1, 4, 10, 7, 9, 5, 0, 15, 14, 2, 3, 12),
    (13, 2, 8, 4, 6, 15, 11, 1, 10, 9, 3, 14, 5, 0, 12, 7,
     1, 15, 13, 8, 10, 3, 7, 4, 12, 5, 6, 11, 0, 14, 9, 2,
     7, 11, 4, 1, 9, 12, 14, 2, 0, 6, 10, 13, 15, 3, 5, 8,
     2, 1, 14, 7, 4, 10, 8, 13, 15, 12, 9, 0, 3, 5, 6, 11),
)
 
 
def _permute(value, table, width):
    """Bit permutation: ``table`` lists 1-based source bit positions, MSB first."""
    out = 0
    for pos in table:
        out = (out << 1) | ((value >> (width - pos)) & 1)
    return out
 
 
def _des_subkeys(key8):
    k = _permute(int.from_bytes(key8, "big"), _PC1, 64)
    c, d = k >> 28, k & 0xFFFFFFF
    keys = []
    for shift in _SHIFTS:
        c = ((c << shift) | (c >> (28 - shift))) & 0xFFFFFFF
        d = ((d << shift) | (d >> (28 - shift))) & 0xFFFFFFF
        keys.append(_permute((c << 28) | d, _PC2, 56))
    return keys
 
 
def _feistel(r, subkey):
    x = _permute(r, _E, 32) ^ subkey
    out = 0
    for i in range(8):
        chunk = (x >> (42 - 6 * i)) & 0x3F
        row = ((chunk & 0x20) >> 4) | (chunk & 1)
        col = (chunk >> 1) & 0xF
        out = (out << 4) | _SBOX[i][row * 16 + col]
    return _permute(out, _P, 32)
 
 
def _des_block(block8, subkeys):
    b = _permute(int.from_bytes(block8, "big"), _IP, 64)
    left, right = b >> 32, b & 0xFFFFFFFF
    for sk in subkeys:
        left, right = right, left ^ _feistel(right, sk)
    return _permute((right << 32) | left, _FP, 64).to_bytes(8, "big")
 
 
def des_encrypt_block(key8, block8):
    """Single DES, one 8-byte block."""
    return _des_block(block8, _des_subkeys(key8))
 
 
def des_decrypt_block(key8, block8):
    """Single DES decrypt, one 8-byte block."""
    return _des_block(block8, _des_subkeys(key8)[::-1])
 
 
def _algo3(key24, block8):
    """Subset-037-2 SS6.2.2.1 "single DES with modified MAC algorithm 3": E(K3, D(K2, E(K1, block)))."""
    t = des_encrypt_block(key24[0:8], block8)
    t = des_decrypt_block(key24[8:16], t)
    return des_encrypt_block(key24[16:24], t)
 
 
def _algo3_reversed(key24, block8):
    """The same construction with K1 and K3 swapped: E(K1, D(K2, E(K3, block))). Used only for KS3."""
    t = des_encrypt_block(key24[16:24], block8)
    t = des_decrypt_block(key24[8:16], t)
    return des_encrypt_block(key24[0:8], t)
 
 
# ---------------------------------------------------------------------------------------------------
# KSMAC and CBC-MAC (Subset-037-2 SS6.2.2 / SS6.2.3.2.3)
# ---------------------------------------------------------------------------------------------------
MAC_LEN = 8
RAND_LEN = 8
KEY_LEN = 24
 
 
def derive_ksmac(kmac, ra, rb):
    """Session key (SS6.2.3.2.3.13-16): RA = RA_L|RA_R, RB = RB_L|RB_R (32-bit halves).
    KS1 = algo3(RA_L|RB_L), KS2 = algo3(RA_R|RB_R), KS3 = algo3_reversed(RA_L|RB_L)."""
    if len(kmac) != KEY_LEN or len(ra) != RAND_LEN or len(rb) != RAND_LEN:
        raise ValueError("kmac 24 bytes, ra and rb 8 bytes")
    block_l = ra[0:4] + rb[0:4]
    block_r = ra[4:8] + rb[4:8]
    return _algo3(kmac, block_l) + _algo3(kmac, block_r) + _algo3_reversed(kmac, block_l)
 
 
def compute_mac(ksmac, msg):
    """CBC-MAC (SS6.2.2): every block but the last under K1 only; the last block via _algo3. ISO/IEC 9797-1
    Padding Method 1 (zero bits), no extra block when len(msg) is already a multiple of 8."""
    padded_len = len(msg) if (len(msg) % 8 == 0) else (len(msg) + (8 - len(msg) % 8))
    padded_len = padded_len or 8
    padded = bytes(msg) + bytes(padded_len - len(msg))
    nblocks = padded_len // 8
    state = bytes(8)
    for i in range(nblocks):
        block = padded[i * 8:(i + 1) * 8]
        state = bytes(a ^ b for a, b in zip(state, block))
        if i < nblocks - 1:
            state = des_encrypt_block(ksmac[0:8], state)
        else:
            state = _algo3(ksmac, state)
    return state
 
 
def _equal_ct(a, b):
    if len(a) != len(b):
        return False
    diff = 0
    for x, y in zip(a, b):
        diff |= x ^ y
    return diff == 0
 
 
def mac_over_da_body(ksmac, da_id, body):
    """[length(2, BE) | DA(3) | body], MACed (SS6.2.3.2.1.1 "Procedure 1"). compute_mac() zero-pads itself."""
    length = 3 + len(body)
    buf = length.to_bytes(2, "big") + _pack_id24(da_id) + bytes(body)
    return compute_mac(ksmac, buf)
 
 
# ---------------------------------------------------------------------------------------------------
# PDU framing (Subset-037-2 SS6.2.5)
# ---------------------------------------------------------------------------------------------------
MTI_AU1, MTI_AU2, MTI_AU3, MTI_DT, MTI_DI, MTI_AR = 1, 2, 3, 5, 8, 9
SEQ_WINDOW = 8
MAX_PDU = 1024
 
 
def _pack_id24(id_):
    return ((id_ & 0xFFFFFF)).to_bytes(3, "big")
 
 
def _unpack_id24(buf):
    return int.from_bytes(buf, "big")
 
 
def _pack_header(ety, mti, df):
    return bytes([((ety & 0x7) << 5) | ((mti & 0xF) << 1) | (df & 1)])
 
 
def _header_mti(header_byte):
    return (header_byte >> 1) & 0xF
 
 
def _header_df(header_byte):
    return header_byte & 1
 
 
class SafError(Exception):
    """A SaF failure; ``status`` names the matching C ``SAFECOMM_STATUS_*`` value."""
 
    def __init__(self, status, detail=""):
        super().__init__(f"{status}{': ' + detail if detail else ''}")
        self.status = status
 
 
IDLE, WAIT_AU2, WAIT_AU3, WAIT_AR, DATA = "IDLE", "WAIT_AU2", "WAIT_AU3", "WAIT_AR", "DATA"
 
 
class SafSession:
    """One SaF connection. ``role`` is "initiator" or "responder".
 
    ``send(frame: bytes)`` transmits one frame; ``recv() -> bytes`` returns one frame or raises
    ``TimeoutError``. ``clock()`` returns milliseconds (default: monotonic). ``rng(n)`` returns n random
    bytes (default ``os.urandom``); tests inject a fixed one. ``ety`` is the 3-bit ETCS ID type
    (Subset-037-2 Table 8: 1=RBC, 2=Engine, ...).
    """
 
    def __init__(self, role, id_self, id_peer, kmac, send, recv, *, ety=0, t_connect_ms=0,
                 t_supervision_ms=0, clock=None, rng=None):
        if role not in ("initiator", "responder"):
            raise ValueError("role must be initiator or responder")
        if len(kmac) != KEY_LEN:
            raise ValueError("kmac must be 24 bytes")
        self.role, self.id_self, self.id_peer, self.ety = role, id_self, id_peer, ety
        self._kmac, self._send, self._recv = bytes(kmac), send, recv
        self.t_connect_ms, self.t_supervision_ms = t_connect_ms, t_supervision_ms
        self._clock = clock or _monotonic_ms
        self._rng = rng or os.urandom
        self.state = IDLE
        self.seq_tx = self.seq_rx = 0
        self.ksmac = None
        self.ra = self.rb = None
        self._last_rx_ms = 0
        self._handshake_start_ms = 0
 
    # -- helpers ------------------------------------------------------------------------------------
    def _fail(self, status, detail=""):
        self.state = IDLE
        raise SafError(status, detail)
 
    def _recv_frame(self):
        try:
            return self._recv()
        except TimeoutError:
            self._fail("TIMEOUT", "no frame")
 
    def _connect_deadline(self):
        return (self._clock(), self.t_connect_ms) if self.t_connect_ms else None
 
    def _check_deadline(self, deadline):
        if deadline is not None and (self._clock() - deadline[0]) & 0xFFFFFFFF > deadline[1]:
            self._fail("TIMEOUT", "connect timeout")
 
    # -- handshake (SS6.2.4.2 / Figure 6, initiator = "B", responder = "A") -------------------------
    def connect(self):
        """Run AU1-AU2-AU3-AR. Raises ``SafError`` (state returns to IDLE) on any failure or timeout."""
        deadline = self._connect_deadline()
        if self.role == "initiator":
            self._connect_initiator(deadline)
        else:
            self._connect_responder(deadline)
        self.seq_tx = self.seq_rx = 1
        self.state = DATA
        self._last_rx_ms = self._clock()
 
    def _connect_initiator(self, deadline):
        self._send(self.initiator_start())
        au2 = self._recv_frame()
        self._check_deadline(deadline)
        self._send(self.initiator_on_frame(au2))
        self._check_deadline(deadline)
        ar = self._recv_frame()
        self._check_deadline(deadline)
        self.initiator_on_frame(ar)
 
    # -- frame-driven initiator: for a caller that cannot block (e.g. a sim whose receive loop serves many trains).
    # Same checks as connect(); each call consumes one received frame and returns the next frame to send, if any.
    def initiator_start(self):
        """Begin a handshake: returns AU1 to send; the session waits for AU2."""
        if self.role != "initiator":
            raise SafError("STATE", "not an initiator")
        self.state = WAIT_AU2
        self.ra = self._rng(RAND_LEN)
        self._handshake_start_ms = self._clock()
        return _pack_header(self.ety, MTI_AU1, 0) + _pack_id24(self.id_self) + b"\x01" + self.ra
 
    def handshake_expired(self):
        """True while a handshake is in progress and has run longer than ``t_connect_ms`` (0 = never)."""
        return (self.state in (WAIT_AU2, WAIT_AR) and bool(self.t_connect_ms)
                and (self._clock() - self._handshake_start_ms) & 0xFFFFFFFF > self.t_connect_ms)
 
    def initiator_on_frame(self, frame):
        """AU2 -> returns AU3 (waits for AR); AR -> returns None (now DATA). Raises ``SafError`` (to IDLE)."""
        if self.state == WAIT_AU2:
            if len(frame) != 21 or _header_mti(frame[0]) != MTI_AU2 or _header_df(frame[0]) != 1:
                self._fail("AUTH_FAILED", "AU2 shape")
            if _unpack_id24(frame[1:4]) != self.id_peer:
                self._fail("AUTH_FAILED", "AU2 sender")
            self.rb = frame[5:13]
            self.ksmac = derive_ksmac(self._kmac, self.ra, self.rb)
            mac_input = (8 .to_bytes(2, "big") + _pack_id24(self.id_self) + frame[0:5] + self.rb + self.ra
                         + _pack_id24(self.id_self))
            if not _equal_ct(compute_mac(self.ksmac, mac_input), frame[13:21]):
                self._fail("AUTH_FAILED", "AU2 MAC")
            au3_header = _pack_header(0, MTI_AU3, 0)
            rmi = 4 .to_bytes(2, "big") + _pack_id24(self.id_peer) + au3_header + self.ra + self.rb
            self.state = WAIT_AR
            return au3_header + compute_mac(self.ksmac, rmi)
        if self.state == WAIT_AR:
            if len(frame) != 9 or _header_mti(frame[0]) != MTI_AR or _header_df(frame[0]) != 1:
                self._fail("AUTH_FAILED", "AR shape")
            if not _equal_ct(mac_over_da_body(self.ksmac, self.id_self, frame[0:1]), frame[1:9]):
                self._fail("AUTH_FAILED", "AR MAC")
            self.seq_tx = self.seq_rx = 1
            self.state = DATA
            self._last_rx_ms = self._clock()
            return None
        raise SafError("STATE", "no handshake in progress")
 
    def _connect_responder(self, deadline):
        au1 = self._recv_frame()
        self._check_deadline(deadline)
        if len(au1) != 13 or _header_mti(au1[0]) != MTI_AU1 or _header_df(au1[0]) != 0:
            self._fail("AUTH_FAILED", "AU1 shape")
        if _unpack_id24(au1[1:4]) != self.id_peer:
            self._fail("AUTH_FAILED", "AU1 sender")
        self.ra = au1[5:13]
        self.state = WAIT_AU3
        self.rb = self._rng(RAND_LEN)
        self.ksmac = derive_ksmac(self._kmac, self.ra, self.rb)
 
        au2_body = _pack_header(self.ety, MTI_AU2, 1) + _pack_id24(self.id_self) + b"\x01" + self.rb
        mac_input = (8 .to_bytes(2, "big") + _pack_id24(self.id_peer) + au2_body[0:5]
                      + self.rb + self.ra + _pack_id24(self.id_peer))
        au2 = au2_body + compute_mac(self.ksmac, mac_input)
        self._send(au2)
 
        au3 = self._recv_frame()
        self._check_deadline(deadline)
        if len(au3) != 9 or _header_mti(au3[0]) != MTI_AU3 or _header_df(au3[0]) != 0:
            self._fail("AUTH_FAILED", "AU3 shape")
        rmi = 4 .to_bytes(2, "big") + _pack_id24(self.id_self) + au3[0:1] + self.ra + self.rb
        if not _equal_ct(compute_mac(self.ksmac, rmi), au3[1:9]):
            self._fail("AUTH_FAILED", "AU3 MAC")
 
        ar_header = _pack_header(0, MTI_AR, 1)
        ar = ar_header + mac_over_da_body(self.ksmac, self.id_peer, ar_header)
        self._send(ar)
 
    # -- data (Sa-DATA / DT, SS6.2.5.3, with the project's own sequence extension - see module docstring) --
    def build_frame(self, pdu):
        """The DT frame for ``pdu`` at the current transmit sequence number (does not advance it)."""
        if self.state != DATA:
            raise SafError("STATE", "not connected")
        if len(pdu) > MAX_PDU:
            raise SafError("INVALID_PARAM", "pdu too long")
        df = 0 if self.role == "initiator" else 1
        body = _pack_header(0, MTI_DT, df) + self.seq_tx.to_bytes(4, "big") + bytes(pdu)
        return body + mac_over_da_body(self.ksmac, self.id_peer, body)
 
    def send_pdu(self, pdu):
        frame = self.build_frame(pdu)
        self._send(frame)
        self.seq_tx = (self.seq_tx + 1) & 0xFFFFFFFF
 
    def recv_pdu(self):
        """Receive one authenticated PDU. Any protection failure returns the session to IDLE."""
        if self.state != DATA:
            raise SafError("STATE", "not connected")
        try:
            frame = self._recv()
        except TimeoutError:
            raise SafError("TIMEOUT", "no frame") from None
        return self.accept_frame(frame)
 
    def accept_frame(self, frame):
        """Verify one already-received Sa-DATA / Sa-DISCONNECT frame and return its PDU (``recv_pdu``'s checks)."""
        if self.state != DATA:
            raise SafError("STATE", "not connected")
        if len(frame) < 1:
            self._fail("TRANSPORT", "empty frame")
        if _header_mti(frame[0]) == MTI_DI:
            # Sa-DISCONNECT: no MAC, no identity field (SS6.2.3.3.3.2). Any well-formed one is honoured.
            if len(frame) != 3:
                raise SafError("TRANSPORT", "malformed disconnect")
            self.state = IDLE
            raise SafError("PEER_DISCONNECT")
        expected_df = 1 if self.role == "initiator" else 0
        if _header_mti(frame[0]) != MTI_DT or len(frame) < (1 + 4 + MAC_LEN):
            self._fail("STATE", "unexpected pdu type")
        if _header_df(frame[0]) != expected_df:
            self._fail("AUTH_FAILED", "direction flag (reflection)")
        body = frame[:-MAC_LEN]
        if not _equal_ct(mac_over_da_body(self.ksmac, self.id_self, body), frame[-MAC_LEN:]):
            self._fail("MAC_MISMATCH")
        rx_seq = int.from_bytes(frame[1:5], "big")
        if (rx_seq - self.seq_rx) & 0xFFFFFFFF > SEQ_WINDOW:
            self._fail("SEQ_ERROR", f"got {rx_seq}, expected {self.seq_rx}..+{SEQ_WINDOW}")
        self.seq_rx = (rx_seq + 1) & 0xFFFFFFFF
        self._last_rx_ms = self._clock()
        return body[5:]
 
    def supervise(self):
        """Call periodically: drop the session when nothing valid arrived within ``t_supervision_ms``."""
        if self.state != DATA:
            raise SafError("STATE", "not connected")
        if self.t_supervision_ms and (self._clock() - self._last_rx_ms) & 0xFFFFFFFF > self.t_supervision_ms:
            self._fail("TIMEOUT", "supervision")
 
    def disconnect(self):
        """Send an unauthenticated Sa-DISCONNECT (reason 0, sub-reason 0) and go to IDLE."""
        if self.state == DATA:
            df = 0 if self.role == "initiator" else 1
            frame = _pack_header(0, MTI_DI, df) + b"\x00\x00"
            try:
                self._send(frame)
            except OSError:
                pass
        self.state = IDLE
 
 
def _monotonic_ms():
    import time
    return int(time.monotonic() * 1000) & 0xFFFFFFFF