saf.c

Go to the documentation of this file.

#include "safecomm/saf/saf.h"
 
#include <string.h>
 
/* Message Type Identifiers, Subset-037-2 Table 7 */
#define MTI_AU1 0x1U
#define MTI_AU2 0x2U
#define MTI_AU3 0x3U
#define MTI_DT  0x5U
#define MTI_DI  0x8U
#define MTI_AR  0x9U
 
#define SAF_SEQ_WINDOW 8U
 
/* -- ETCS ID (24-bit) and header-octet (ETY|MTI|DF) packing, Subset-037-2 §6.2.5.1/§6.2.5.2 -- */
 
static void pack_id24(uint32_t id, uint8_t buf[3])
{
    buf[0] = (uint8_t)((id >> 16U) & 0xFFU);
    buf[1] = (uint8_t)((id >> 8U) & 0xFFU);
    buf[2] = (uint8_t)(id & 0xFFU);
}
 
static uint32_t unpack_id24(const uint8_t buf[3])
{
    return ((uint32_t)buf[0] << 16U) | ((uint32_t)buf[1] << 8U) | (uint32_t)buf[2];
}
 
/* header = ETY(bits8-6) | MTI(bits5-2) | DF(bit1), Table 8 et al. */
static uint8_t pack_header(uint8_t ety, uint8_t mti, uint8_t df)
{
    return (uint8_t)(((ety & 0x07U) << 5U) | ((mti & 0x0FU) << 1U) | (df & 0x01U));
}
 
static uint8_t header_mti(uint8_t header)
{
    return (uint8_t)((header >> 1U) & 0x0FU);
}
 
static uint8_t header_df(uint8_t header)
{
    return (uint8_t)(header & 0x01U);
}
 
static void pack_u32_be(uint32_t val, uint8_t buf[4])
{
    buf[0] = (uint8_t)((val >> 24U) & 0xFFU);
    buf[1] = (uint8_t)((val >> 16U) & 0xFFU);
    buf[2] = (uint8_t)((val >> 8U) & 0xFFU);
    buf[3] = (uint8_t)(val & 0xFFU);
}
 
static uint32_t unpack_u32_be(const uint8_t buf[4])
{
    return ((uint32_t)buf[0] << 24U) | ((uint32_t)buf[1] << 16U) | ((uint32_t)buf[2] << 8U) | (uint32_t)buf[3];
}
 
static void generate_random_bytes(uint8_t *out, size_t len, uint32_t seed_hint)
{
    static uint32_t lcg_state = 0x12345678U;
    if (seed_hint != 0U)
    {
        lcg_state ^= seed_hint;
    }
    for (size_t i = 0U; i < len; i++)
    {
        lcg_state = (lcg_state * 1664525U) + 1013904223U;
        out[i] = (uint8_t)((lcg_state >> 16U) & 0xFFU);
        if (out[i] == 0U)
        {
            out[i] = (uint8_t)(i + 1U);
        }
    }
}

static safecomm_status_t mac_over_da_body(const safecomm_mac_ctx_t *mac, uint32_t da_id,
                                          const uint8_t *body, size_t body_len,
                                          uint8_t out_mac[SAFECOMM_MAC_LEN])
{
    /* length(2) + DA(3) + the largest body: a Sa-DATA header(1) + sequence(4) + SAFECOMM_SAF_MAX_PDU. */
    uint8_t buf[5U + 1U + 4U + SAFECOMM_SAF_MAX_PDU];
    size_t l = 3U + body_len;
    if ((body_len + 5U) > sizeof(buf))
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
    buf[0] = (uint8_t)((l >> 8U) & 0xFFU);
    buf[1] = (uint8_t)(l & 0xFFU);
    pack_id24(da_id, &buf[2]);
    (void)memcpy(&buf[5], body, body_len);
    return safecomm_mac_compute(mac, buf, 5U + body_len, out_mac);
}
 
safecomm_status_t safecomm_saf_init(safecomm_saf_conn_t *conn,
                                    const safecomm_saf_config_t *cfg,
                                    const safecomm_transport_t *transport)
{
    if ((conn == NULL) || (cfg == NULL) || (transport == NULL)
        || (transport->send == NULL) || (transport->recv == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    (void)memset(conn, 0, sizeof(*conn));
    conn->cfg       = *cfg;
    conn->transport = *transport;
    conn->state     = SAFECOMM_SAF_STATE_IDLE;
    return SAFECOMM_STATUS_OK;
}
 
/* REQ-SC-SAF-022: connect budget on the caller-supplied clock (0 disables); wrap-around safe */
static bool connect_expired(const safecomm_saf_conn_t *conn, uint32_t start_ms)
{
    return (conn->cfg.t_connect_ms != 0U) && ((uint32_t)(conn->now_ms - start_ms) > conn->cfg.t_connect_ms);
}
 
/* Responder, AU1 -> AU2 (Subset-037-2 Fig.6, us = A/called). Checks AU1, takes the caller's RB, derives KSMAC and
 * builds AU2 into out_au2 (SAFECOMM_SAF_AU2_LEN bytes). Ends in WAIT_AU3, or IDLE on any failure. */
static safecomm_status_t responder_process_au1(safecomm_saf_conn_t *conn, const uint8_t *au1, size_t au1_len,
                                               const uint8_t rb[SAFECOMM_MAC_RAND_LEN], uint8_t *out_au2)
{
    safecomm_status_t st;
 
    conn->state = SAFECOMM_SAF_STATE_IDLE;
    conn->supervision_armed = false;
    if ((au1_len != SAFECOMM_SAF_AU1_LEN) || (header_mti(au1[0]) != MTI_AU1) || (header_df(au1[0]) != 0U))
    {
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
    if (unpack_id24(&au1[1]) != conn->cfg.etcs_id_peer)
    {
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
    (void)memcpy(conn->ra, &au1[5], SAFECOMM_MAC_RAND_LEN);
    (void)memcpy(conn->rb, rb, SAFECOMM_MAC_RAND_LEN);
 
    st = safecomm_mac_derive_ksmac(conn->cfg.kmac, conn->ra, conn->rb, &conn->mac);
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
 
    out_au2[0] = pack_header(conn->cfg.ety, MTI_AU2, 1U);
    pack_id24(conn->cfg.etcs_id_self, &out_au2[1]);
    out_au2[4] = 0x01U;
    (void)memcpy(&out_au2[5], conn->rb, SAFECOMM_MAC_RAND_LEN);
    {
        uint8_t input[10 + SAFECOMM_MAC_RAND_LEN + SAFECOMM_MAC_RAND_LEN + 3];
        size_t body_len = 5U;
        size_t l = 3U + body_len;
        input[0] = (uint8_t)((l >> 8U) & 0xFFU);
        input[1] = (uint8_t)(l & 0xFFU);
        pack_id24(conn->cfg.etcs_id_peer, &input[2]); /* DA = peer (receiver of AU2) */
        (void)memcpy(&input[5], &out_au2[0], body_len);
        (void)memcpy(&input[10], conn->rb, SAFECOMM_MAC_RAND_LEN);
        (void)memcpy(&input[18], conn->ra, SAFECOMM_MAC_RAND_LEN);
        pack_id24(conn->cfg.etcs_id_peer, &input[26]);
        st = safecomm_mac_compute(&conn->mac, input, sizeof(input), &out_au2[13]);
    }
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
    conn->state = SAFECOMM_SAF_STATE_WAIT_AU3;
    return SAFECOMM_STATUS_OK;
}
 
/* Responder, AU3 -> AR. Verifies AU3's MAC and builds AR into out_ar (SAFECOMM_SAF_AR_LEN bytes). Ends in DATA with
 * both sequence counters at 1, or IDLE on any failure. The caller checks the connect budget first. */
static safecomm_status_t responder_process_au3(safecomm_saf_conn_t *conn, const uint8_t *au3, size_t au3_len,
                                               uint8_t *out_ar)
{
    safecomm_status_t st;
    uint8_t rmi[6 + SAFECOMM_MAC_RAND_LEN + SAFECOMM_MAC_RAND_LEN];
    uint8_t expected_mac[SAFECOMM_MAC_LEN];
    size_t bl = 1U;
    size_t rl = 3U + bl;
 
    if ((au3_len != SAFECOMM_SAF_AU3_LEN) || (header_mti(au3[0]) != MTI_AU3) || (header_df(au3[0]) != 0U))
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
    rmi[0] = (uint8_t)((rl >> 8U) & 0xFFU);
    rmi[1] = (uint8_t)(rl & 0xFFU);
    pack_id24(conn->cfg.etcs_id_self, &rmi[2]); /* DA = us (receiver of AU3) */
    rmi[5] = au3[0];
    (void)memcpy(&rmi[6], conn->ra, SAFECOMM_MAC_RAND_LEN);
    (void)memcpy(&rmi[14], conn->rb, SAFECOMM_MAC_RAND_LEN);
    st = safecomm_mac_compute(&conn->mac, rmi, sizeof(rmi), expected_mac);
    if ((st != SAFECOMM_STATUS_OK) || (memcmp(expected_mac, &au3[1], SAFECOMM_MAC_LEN) != 0))
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
 
    /* AR: header | MAC via the generic Procedure 1 over just the header (§6.2.3.2.1.11). */
    out_ar[0] = pack_header(0U, MTI_AR, 1U);
    st = mac_over_da_body(&conn->mac, conn->cfg.etcs_id_peer, &out_ar[0], 1U, &out_ar[1]);
    if (st != SAFECOMM_STATUS_OK)
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return st;
    }
    conn->seq_tx = 1U;
    conn->seq_rx = 1U;
    conn->state  = SAFECOMM_SAF_STATE_DATA;
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_connect(safecomm_saf_conn_t *conn)
{
    if (conn == NULL)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_IDLE)
    {
        return SAFECOMM_STATUS_STATE;
    }
    const uint32_t start_ms = conn->now_ms;
 
    if (conn->cfg.role == SAFECOMM_SAF_ROLE_INITIATOR)
    {
        /* ------------------------------------------------------------- */
        /* INITIATOR (AU1 -> wait AU2 -> AU3 -> DATA), Subset-037-2 Fig.6 with */
        /* B = us (calling/initiator), A = peer (called/responder).       */
        /* ------------------------------------------------------------- */
        conn->state = SAFECOMM_SAF_STATE_WAIT_AU2;
        conn->supervision_armed = false;
 
        uint8_t au1[13];
        au1[0] = pack_header(conn->cfg.ety, MTI_AU1, 0U);
        pack_id24(conn->cfg.etcs_id_self, &au1[1]);
        au1[4] = 0x01U; /* SaF: single DES with modified MAC algorithm 3 */
        generate_random_bytes(conn->ra, SAFECOMM_MAC_RAND_LEN, conn->cfg.etcs_id_self);
        (void)memcpy(&au1[5], conn->ra, SAFECOMM_MAC_RAND_LEN);
 
        safecomm_status_t st = conn->transport.send(conn->transport.user, au1, sizeof(au1));
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
 
        uint8_t au2[21];
        size_t au2_len = 0U;
        st = conn->transport.recv(conn->transport.user, au2, sizeof(au2), &au2_len);
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
        if (connect_expired(conn, start_ms))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_TIMEOUT;
        }
        if ((au2_len != sizeof(au2)) || (header_mti(au2[0]) != MTI_AU2) || (header_df(au2[0]) != 1U))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_AUTH_FAILED;
        }
        uint32_t peer_id = unpack_id24(&au2[1]);
        if (peer_id != conn->cfg.etcs_id_peer)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_AUTH_FAILED;
        }
        (void)memcpy(conn->rb, &au2[5], SAFECOMM_MAC_RAND_LEN);
 
        st = safecomm_mac_derive_ksmac(conn->cfg.kmac, conn->ra, conn->rb, &conn->mac);
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
 
        /* AU2 MAC = CBC-MAC(KS, Text3 | RA | RB | DA | pad); Text3 = l|DA|header|SA|SaF,
         * DA = us (the receiver of AU2). "RA"/"RB" here follow Fig.6's own labels:
         * RA = responder's random (rb), RB = initiator's random (ra). */
        uint8_t input[10 + SAFECOMM_MAC_RAND_LEN + SAFECOMM_MAC_RAND_LEN + 3];
        size_t body_len = 5U; /* header + SA(3) + SaF */
        size_t l = 3U + body_len;
        input[0] = (uint8_t)((l >> 8U) & 0xFFU);
        input[1] = (uint8_t)(l & 0xFFU);
        pack_id24(conn->cfg.etcs_id_self, &input[2]); /* DA = us */
        (void)memcpy(&input[5], &au2[0], body_len);
        (void)memcpy(&input[10], conn->rb, SAFECOMM_MAC_RAND_LEN);
        (void)memcpy(&input[18], conn->ra, SAFECOMM_MAC_RAND_LEN);
        pack_id24(conn->cfg.etcs_id_self, &input[26]);
 
        uint8_t expected_mac[SAFECOMM_MAC_LEN];
        st = safecomm_mac_compute(&conn->mac, input, sizeof(input), expected_mac);
        if ((st != SAFECOMM_STATUS_OK) || (memcmp(expected_mac, &au2[13], SAFECOMM_MAC_LEN) != 0))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_AUTH_FAILED;
        }
 
        /* AU3 = header | MAC(Text5 | RB | RA); Text5 = l|DA|header, DA = peer (receiver of AU3). */
        uint8_t au3[9];
        au3[0] = pack_header(0U, MTI_AU3, 0U);
        {
            uint8_t rmi[6 + SAFECOMM_MAC_RAND_LEN + SAFECOMM_MAC_RAND_LEN];
            size_t bl = 1U;
            size_t rl = 3U + bl;
            rmi[0] = (uint8_t)((rl >> 8U) & 0xFFU);
            rmi[1] = (uint8_t)(rl & 0xFFU);
            pack_id24(conn->cfg.etcs_id_peer, &rmi[2]); /* DA = peer */
            rmi[5] = au3[0];
            (void)memcpy(&rmi[6], conn->ra, SAFECOMM_MAC_RAND_LEN);
            (void)memcpy(&rmi[14], conn->rb, SAFECOMM_MAC_RAND_LEN);
            st = safecomm_mac_compute(&conn->mac, rmi, sizeof(rmi), &au3[1]);
        }
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
 
        st = conn->transport.send(conn->transport.user, au3, sizeof(au3));
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
 
        /* Wait for the AR confirmation (§6.2.4.2.12-14: "not required by the peer entity authentication
         * procedure ... added to provide an OSI-like confirmed service") before declaring DATA. */
        uint8_t ar[9];
        size_t ar_len = 0U;
        st = conn->transport.recv(conn->transport.user, ar, sizeof(ar), &ar_len);
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
        if (connect_expired(conn, start_ms))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_TIMEOUT;
        }
        if ((ar_len != sizeof(ar)) || (header_mti(ar[0]) != MTI_AR) || (header_df(ar[0]) != 1U))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_AUTH_FAILED;
        }
        uint8_t ar_expected[SAFECOMM_MAC_LEN];
        st = mac_over_da_body(&conn->mac, conn->cfg.etcs_id_self, &ar[0], 1U, ar_expected);
        if ((st != SAFECOMM_STATUS_OK) || (memcmp(ar_expected, &ar[1], SAFECOMM_MAC_LEN) != 0))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_AUTH_FAILED;
        }
 
        conn->seq_tx = 1U;
        conn->seq_rx = 1U;
        conn->state  = SAFECOMM_SAF_STATE_DATA;
        return SAFECOMM_STATUS_OK;
    }
    else
    {
        /* RESPONDER (wait AU1 -> AU2 -> wait AU3 -> AR -> DATA), same steps as the frame-driven API below. The blocking
         * form draws RB from this module's own generator (test and tool use only - see generate_random_bytes()); a vital
         * responder uses safecomm_saf_responder_on_au1() with an RB from a secure source. */
        uint8_t au1[SAFECOMM_SAF_AU1_LEN];
        uint8_t au2[SAFECOMM_SAF_AU2_LEN];
        uint8_t au3[SAFECOMM_SAF_AU3_LEN];
        uint8_t ar[SAFECOMM_SAF_AR_LEN];
        uint8_t rb[SAFECOMM_MAC_RAND_LEN];
        size_t au1_len = 0U;
        size_t au3_len = 0U;
        safecomm_status_t st = conn->transport.recv(conn->transport.user, au1, sizeof(au1), &au1_len);
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
        if (connect_expired(conn, start_ms))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_TIMEOUT;
        }
        generate_random_bytes(rb, SAFECOMM_MAC_RAND_LEN, conn->cfg.etcs_id_self);
        st = responder_process_au1(conn, au1, au1_len, rb, au2);
        if (st != SAFECOMM_STATUS_OK)
        {
            return st;
        }
        conn->connect_start_ms = start_ms;
        st = conn->transport.send(conn->transport.user, au2, sizeof(au2));
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
        st = conn->transport.recv(conn->transport.user, au3, sizeof(au3), &au3_len);
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
        if (connect_expired(conn, start_ms))
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return SAFECOMM_STATUS_TIMEOUT;
        }
        st = responder_process_au3(conn, au3, au3_len, ar);
        if (st != SAFECOMM_STATUS_OK)
        {
            return st;
        }
        st = conn->transport.send(conn->transport.user, ar, sizeof(ar));
        if (st != SAFECOMM_STATUS_OK)
        {
            conn->state = SAFECOMM_SAF_STATE_IDLE;
            return st;
        }
        return SAFECOMM_STATUS_OK;
    }
}
 
safecomm_saf_frame_kind_t safecomm_saf_frame_kind(const uint8_t *frame, size_t frame_len)
{
    safecomm_saf_frame_kind_t kind = SAFECOMM_SAF_FRAME_UNKNOWN;
 
    if ((frame != NULL) && (frame_len > 0U))
    {
        switch (header_mti(frame[0]))
        {
            case MTI_AU1: kind = SAFECOMM_SAF_FRAME_AU1; break;
            case MTI_AU2: kind = SAFECOMM_SAF_FRAME_AU2; break;
            case MTI_AU3: kind = SAFECOMM_SAF_FRAME_AU3; break;
            case MTI_DT:  kind = SAFECOMM_SAF_FRAME_DATA; break;
            case MTI_DI:  kind = SAFECOMM_SAF_FRAME_DISCONNECT; break;
            case MTI_AR:  kind = SAFECOMM_SAF_FRAME_AR; break;
            default:      kind = SAFECOMM_SAF_FRAME_UNKNOWN; break;
        }
    }
    return kind;
}
 
safecomm_status_t safecomm_saf_au1_fields(const uint8_t *au1, size_t au1_len, uint32_t *out_peer_id,
                                          uint8_t out_ra[SAFECOMM_MAC_RAND_LEN])
{
    if ((au1 == NULL) || (out_peer_id == NULL) || (out_ra == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if ((au1_len != SAFECOMM_SAF_AU1_LEN) || (header_mti(au1[0]) != MTI_AU1) || (header_df(au1[0]) != 0U))
    {
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
    *out_peer_id = unpack_id24(&au1[1]);
    (void)memcpy(out_ra, &au1[5], SAFECOMM_MAC_RAND_LEN);
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_responder_on_au1(safecomm_saf_conn_t *conn, const uint8_t *au1, size_t au1_len,
                                                const uint8_t rb[SAFECOMM_MAC_RAND_LEN],
                                                uint8_t *out_au2, size_t out_cap, size_t *out_len)
{
    safecomm_status_t st;
 
    if ((conn == NULL) || (au1 == NULL) || (rb == NULL) || (out_au2 == NULL) || (out_len == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->cfg.role != SAFECOMM_SAF_ROLE_RESPONDER)
    {
        return SAFECOMM_STATUS_STATE;
    }
    if (out_cap < SAFECOMM_SAF_AU2_LEN)
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
    /* A new AU1 is legal in any state: it is how a peer that restarted opens a fresh connection. Like Sa-DISCONNECT it
     * is unauthenticated, so a forged one can only end the current connection (availability), never inject data. */
    st = responder_process_au1(conn, au1, au1_len, rb, out_au2);
    if (st == SAFECOMM_STATUS_OK)
    {
        conn->connect_start_ms = conn->now_ms;
        *out_len = SAFECOMM_SAF_AU2_LEN;
    }
    return st;
}
 
safecomm_status_t safecomm_saf_responder_on_au3(safecomm_saf_conn_t *conn, const uint8_t *au3, size_t au3_len,
                                                uint8_t *out_ar, size_t out_cap, size_t *out_len)
{
    safecomm_status_t st;
 
    if ((conn == NULL) || (au3 == NULL) || (out_ar == NULL) || (out_len == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if ((conn->cfg.role != SAFECOMM_SAF_ROLE_RESPONDER) || (conn->state != SAFECOMM_SAF_STATE_WAIT_AU3))
    {
        return SAFECOMM_STATUS_STATE;
    }
    if (out_cap < SAFECOMM_SAF_AR_LEN)
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
    if (connect_expired(conn, conn->connect_start_ms))
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_TIMEOUT;
    }
    st = responder_process_au3(conn, au3, au3_len, out_ar);
    if (st == SAFECOMM_STATUS_OK)
    {
        *out_len = SAFECOMM_SAF_AR_LEN;
    }
    return st;
}
 
safecomm_status_t safecomm_saf_frame_sent(safecomm_saf_conn_t *conn)
{
    if (conn == NULL)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_DATA)
    {
        return SAFECOMM_STATUS_STATE;
    }
    conn->seq_tx++;
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_disconnect_frame(safecomm_saf_role_t role, uint8_t *out, size_t out_cap,
                                                size_t *out_len)
{
    if ((out == NULL) || (out_len == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (out_cap < SAFECOMM_SAF_DI_LEN)
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
    out[0] = pack_header(0U, MTI_DI, (uint8_t)((role == SAFECOMM_SAF_ROLE_INITIATOR) ? 0U : 1U));
    out[1] = 0x00U; /* reason: normal release */
    out[2] = 0x00U; /* sub-reason */
    *out_len = SAFECOMM_SAF_DI_LEN;
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_prepare_send(safecomm_saf_conn_t *conn,
                                            const uint8_t *pdu, size_t pdu_len,
                                            uint8_t *out_frame, size_t out_frame_cap,
                                            size_t *out_frame_len)
{
    if ((conn == NULL) || (out_frame == NULL) || (out_frame_len == NULL) ||
        ((pdu == NULL) && (pdu_len > 0U)) || (pdu_len > SAFECOMM_SAF_MAX_PDU))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_DATA)
    {
        return SAFECOMM_STATUS_STATE;
    }
 
    /* [header(1)][seq(4, our sequence extension, see file header)][pdu(n)][MAC(8)] */
    size_t body_len = 1U + 4U + pdu_len;
    size_t total_len = body_len + SAFECOMM_MAC_LEN;
    if (out_frame_cap < total_len)
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
 
    uint8_t df = (uint8_t)((conn->cfg.role == SAFECOMM_SAF_ROLE_INITIATOR) ? 0U : 1U);
    out_frame[0] = pack_header(0U, MTI_DT, df);
    pack_u32_be(conn->seq_tx, &out_frame[1]);
    if (pdu_len > 0U)
    {
        (void)memcpy(&out_frame[5], pdu, pdu_len);
    }
 
    safecomm_status_t st = mac_over_da_body(&conn->mac, conn->cfg.etcs_id_peer,
                                            out_frame, body_len, &out_frame[body_len]);
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
 
    *out_frame_len = total_len;
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_commit_send(safecomm_saf_conn_t *conn,
                                           const uint8_t *frame, size_t frame_len)
{
    if ((conn == NULL) || (frame == NULL) || (frame_len == 0U))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_DATA)
    {
        return SAFECOMM_STATUS_STATE;
    }
 
    safecomm_status_t st = conn->transport.send(conn->transport.user, frame, frame_len);
    if (st == SAFECOMM_STATUS_OK)
    {
        conn->seq_tx++;
    }
    return st;
}
 
safecomm_status_t safecomm_saf_send(safecomm_saf_conn_t *conn,
                                    const uint8_t *pdu, size_t pdu_len)
{
    uint8_t frame_buf[SAFECOMM_SAF_MAX_PDU + SAFECOMM_SAF_DATA_OVERHEAD];
    size_t frame_len = 0U;
 
    safecomm_status_t st = safecomm_saf_prepare_send(conn, pdu, pdu_len, frame_buf, sizeof(frame_buf), &frame_len);
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
    return safecomm_saf_commit_send(conn, frame_buf, frame_len);
}
 
safecomm_status_t safecomm_saf_recv(safecomm_saf_conn_t *conn,
                                    uint8_t *out, size_t out_cap, size_t *out_len)
{
    if ((conn == NULL) || (out == NULL) || (out_len == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_DATA)
    {
        return SAFECOMM_STATUS_STATE;
    }
 
    uint8_t rx_buf[SAFECOMM_SAF_MAX_PDU + SAFECOMM_SAF_DATA_OVERHEAD];
    size_t rx_len = 0U;
 
    safecomm_status_t st = conn->transport.recv(conn->transport.user, rx_buf, sizeof(rx_buf), &rx_len);
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
    return safecomm_saf_accept_frame(conn, rx_buf, rx_len, out, out_cap, out_len);
}
 
safecomm_status_t safecomm_saf_accept_frame(safecomm_saf_conn_t *conn, const uint8_t *rx_buf, size_t rx_len,
                                            uint8_t *out, size_t out_cap, size_t *out_len)
{
    safecomm_status_t st;
 
    if ((conn == NULL) || (rx_buf == NULL) || (out == NULL) || (out_len == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_DATA)
    {
        return SAFECOMM_STATUS_STATE;
    }
 
    if (rx_len < 1U)
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_TRANSPORT;
    }
 
    /* Sa-DISCONNECT carries no MAC (Subset-037-2 §6.2.3.3.3.2: release-phase authentication
     * is not required — it can only affect availability, and the connection can be released
     * independently at the transport layer regardless). Any correctly-shaped DI is honoured. */
    if (header_mti(rx_buf[0]) == MTI_DI)
    {
        if (rx_len != 3U)
        {
            return SAFECOMM_STATUS_TRANSPORT; /* malformed, not a real disconnect: connection unchanged */
        }
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_PEER_DISCONNECT;
    }
 
    uint8_t expected_df = (uint8_t)((conn->cfg.role == SAFECOMM_SAF_ROLE_INITIATOR) ? 1U : 0U);
    if ((header_mti(rx_buf[0]) != MTI_DT) || (rx_len < (1U + 4U + SAFECOMM_MAC_LEN)))
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_STATE;
    }
    if (header_df(rx_buf[0]) != expected_df)
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
 
    size_t body_len = rx_len - SAFECOMM_MAC_LEN;
    uint8_t expected_mac[SAFECOMM_MAC_LEN];
    st = mac_over_da_body(&conn->mac, conn->cfg.etcs_id_self, rx_buf, body_len, expected_mac);
    if ((st != SAFECOMM_STATUS_OK) || (memcmp(expected_mac, &rx_buf[body_len], SAFECOMM_MAC_LEN) != 0))
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_MAC_MISMATCH;
    }
 
    uint32_t rx_seq = unpack_u32_be(&rx_buf[1]);
    /* Sequence window check: rx_seq must be seq_rx or up to SAF_SEQ_WINDOW ahead. The unsigned difference rejects a repeated
     * or older number (it wraps to a huge value) and cannot overflow near the top of the counter. */
    if ((uint32_t)(rx_seq - conn->seq_rx) > (uint32_t)SAF_SEQ_WINDOW)
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        return SAFECOMM_STATUS_SEQ_ERROR;
    }
 
    conn->seq_rx = rx_seq + 1U;
    conn->last_rx_ms = conn->now_ms; /* a valid frame refreshes supervision */
    conn->supervision_armed = true;
 
    size_t payload_len = body_len - 5U;
    if (payload_len > out_cap)
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
    if (payload_len > 0U)
    {
        (void)memcpy(out, &rx_buf[5], payload_len);
    }
    *out_len = payload_len;
 
    return SAFECOMM_STATUS_OK;
}
 
void safecomm_saf_set_time(safecomm_saf_conn_t *conn, uint32_t now_ms)
{
    if (conn != NULL)
    {
        conn->now_ms = now_ms;
    }
}
 
safecomm_status_t safecomm_saf_supervise(safecomm_saf_conn_t *conn)
{
    if (conn == NULL)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if (conn->state != SAFECOMM_SAF_STATE_DATA)
    {
        conn->supervision_armed = false;
        return SAFECOMM_STATUS_STATE;
    }
    if (conn->cfg.t_supervision_ms == 0U)
    {
        return SAFECOMM_STATUS_OK;
    }
    if (!conn->supervision_armed)
    {
        conn->last_rx_ms = conn->now_ms;
        conn->supervision_armed = true;
        return SAFECOMM_STATUS_OK;
    }
    if ((uint32_t)(conn->now_ms - conn->last_rx_ms) > conn->cfg.t_supervision_ms)
    {
        conn->state = SAFECOMM_SAF_STATE_IDLE;
        conn->supervision_armed = false;
        return SAFECOMM_STATUS_TIMEOUT;
    }
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_disconnect(safecomm_saf_conn_t *conn)
{
    if (conn == NULL)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
 
    if (conn->state != SAFECOMM_SAF_STATE_IDLE)
    {
        uint8_t disconn[SAFECOMM_SAF_DI_LEN];
        size_t disconn_len = 0U;
 
        (void)safecomm_saf_disconnect_frame(conn->cfg.role, disconn, sizeof(disconn), &disconn_len);
        (void)conn->transport.send(conn->transport.user, disconn, disconn_len);
    }
 
    conn->state = SAFECOMM_SAF_STATE_IDLE;
    return SAFECOMM_STATUS_OK;
}
 
safecomm_saf_state_t safecomm_saf_state(const safecomm_saf_conn_t *conn)
{
    return (conn != NULL) ? conn->state : SAFECOMM_SAF_STATE_IDLE;
}
 
safecomm_status_t safecomm_saf_export_session_state(const safecomm_saf_conn_t *conn,
                                                    safecomm_session_vital_state_t *out_state)
{
    if ((conn == NULL) || (out_state == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
 
    (void)memset(out_state, 0, sizeof(*out_state));
    out_state->state                = conn->state;
    out_state->seq_tx               = conn->seq_tx;
    out_state->seq_rx               = conn->seq_rx;
    out_state->seq_rx_last_accepted = (conn->seq_rx > 0U) ? (conn->seq_rx - 1U) : 0U;
    (void)memcpy(out_state->ksmac, conn->mac.ksmac, SAFECOMM_MAC_KEY_LEN);
    (void)memcpy(out_state->ra, conn->ra, SAFECOMM_MAC_RAND_LEN);
    (void)memcpy(out_state->rb, conn->rb, SAFECOMM_MAC_RAND_LEN);
    out_state->etcs_id_self         = conn->cfg.etcs_id_self;
    out_state->etcs_id_peer         = conn->cfg.etcs_id_peer;
 
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_saf_import_session_state(safecomm_saf_conn_t *conn,
                                                    const safecomm_session_vital_state_t *in_state)
{
    if ((conn == NULL) || (in_state == NULL))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
 
    conn->state                = in_state->state;
    conn->seq_tx               = in_state->seq_tx;
    conn->seq_rx               = in_state->seq_rx;
    conn->mac.alg              = SAFECOMM_MAC_ALG_3DES_SS037;
    (void)memcpy(conn->mac.ksmac, in_state->ksmac, SAFECOMM_MAC_KEY_LEN);
    (void)memcpy(conn->ra, in_state->ra, SAFECOMM_MAC_RAND_LEN);
    (void)memcpy(conn->rb, in_state->rb, SAFECOMM_MAC_RAND_LEN);
    conn->cfg.etcs_id_self     = in_state->etcs_id_self;
    conn->cfg.etcs_id_peer     = in_state->etcs_id_peer;
 
    return SAFECOMM_STATUS_OK;
}