sai.c

Go to the documentation of this file.

#include "safecomm/sai/sai.h"
 
#include <string.h>
 
/* Message types (Subset-098 §5.4.6.1.3, hex values) */
#define SAI_MSG_EC_START 0x81U
#define SAI_MSG_EC_DATA  0x86U
 
/* Wire header (Subset-098 Figure 6, EC used -> TTS fields all zero): msgtype(1) + seq(2) + TTS(12) + EC(4) = 19 */
#define SAI_HEADER_LEN 19U
#define SAI_START_EXTRA_LEN 6U /* Version(4) + EC period(2), Figure 23 */
 
static void pack_u16_be(uint16_t v, uint8_t buf[2])
{
    buf[0] = (uint8_t)((v >> 8U) & 0xFFU);
    buf[1] = (uint8_t)(v & 0xFFU);
}
 
static uint16_t unpack_u16_be(const uint8_t buf[2])
{
    return (uint16_t)(((uint16_t)buf[0] << 8U) | (uint16_t)buf[1]);
}
 
static void pack_u32_be(uint32_t v, uint8_t buf[4])
{
    buf[0] = (uint8_t)((v >> 24U) & 0xFFU);
    buf[1] = (uint8_t)((v >> 16U) & 0xFFU);
    buf[2] = (uint8_t)((v >> 8U) & 0xFFU);
    buf[3] = (uint8_t)(v & 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];
}
 
/* §5.4.9.4.7: the current EC state depends only on the latest Delta. */
static int ec_state_from_delta(int32_t delta)
{
    if (delta <= 0) { return 0; }
    if (delta == 1) { return 1; }
    if (delta == 2) { return 2; }
    return 3; /* alarm */
}
 
safecomm_status_t safecomm_sai_init(safecomm_sai_conn_t *conn, safecomm_saf_conn_t *saf,
                                    const safecomm_sai_config_t *cfg)
{
    if ((conn == NULL) || (saf == NULL) || (cfg == NULL) || (cfg->ec_period_ms == 0U))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    (void)memset(conn, 0, sizeof(*conn));
    conn->saf = saf;
    conn->cfg = *cfg;
    if (conn->cfg.seq_window == 0U)
    {
        conn->cfg.seq_window = 1U; /* no missing messages allowed, §5.5.5.1.1 default */
    }
    if (conn->cfg.ec_alarm_state == 0U)
    {
        conn->cfg.ec_alarm_state = 3U;
    }
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_sai_start(safecomm_sai_conn_t *conn)
{
    if (conn == NULL)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
 
    uint8_t out[SAI_HEADER_LEN + SAI_START_EXTRA_LEN];
    out[0] = SAI_MSG_EC_START;
    pack_u16_be(conn->seq_tx, &out[1]);
    (void)memset(&out[3], 0, 12U); /* TTS fields: not used */
    pack_u32_be(conn->ec_tx, &out[15]);
    pack_u32_be(1U, &out[19]);     /* Version */
    uint32_t period = conn->cfg.ec_period_ms;
    pack_u16_be((uint16_t)((period > 0xFFFFU) ? 0xFFFFU : period), &out[23]);
 
    safecomm_status_t st = safecomm_saf_send(conn->saf, out, sizeof(out));
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
    conn->seq_tx = (uint16_t)(conn->seq_tx + 1U);
 
    uint8_t in[SAFECOMM_SAF_MAX_PDU];
    size_t in_len = 0U;
    st = safecomm_saf_recv(conn->saf, in, sizeof(in), &in_len);
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
    if ((in_len != (SAI_HEADER_LEN + SAI_START_EXTRA_LEN)) || (in[0] != SAI_MSG_EC_START))
    {
        return SAFECOMM_STATUS_AUTH_FAILED;
    }
 
    conn->last_rx_ec         = unpack_u32_be(&in[15]);
    conn->peer_ec_period_ms  = unpack_u16_be(&in[23]);
    if (conn->peer_ec_period_ms == 0U)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    conn->seq_rx       = unpack_u16_be(&in[1]);
    conn->seq_rx_valid = true;
 
    /* §5.4.9.4.3: R = receiver (our) EC period / sender (peer) EC period.
     * §5.4.9.4.4: update Ex once immediately, using the peer's initial EC counter. */
    conn->ratio         = (double)conn->cfg.ec_period_ms / (double)conn->peer_ec_period_ms;
    conn->expected_ec   = (double)conn->last_rx_ec + conn->ratio;
    conn->ec_initialized = true;
    conn->ec_state       = 0;
 
    return SAFECOMM_STATUS_OK;
}
 
safecomm_status_t safecomm_sai_send(safecomm_sai_conn_t *conn, const uint8_t *pdu, size_t pdu_len)
{
    if ((conn == NULL) || ((pdu == NULL) && (pdu_len > 0U)))
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
    if ((SAI_HEADER_LEN + pdu_len) > SAFECOMM_SAF_MAX_PDU)
    {
        return SAFECOMM_STATUS_INVALID_PARAM;
    }
 
    uint8_t buf[SAFECOMM_SAF_MAX_PDU];
    buf[0] = SAI_MSG_EC_DATA;
    pack_u16_be(conn->seq_tx, &buf[1]);
    (void)memset(&buf[3], 0, 12U);
    pack_u32_be(conn->ec_tx, &buf[15]);
    if (pdu_len > 0U)
    {
        (void)memcpy(&buf[SAI_HEADER_LEN], pdu, pdu_len);
    }
 
    safecomm_status_t st = safecomm_saf_send(conn->saf, buf, SAI_HEADER_LEN + pdu_len);
    if (st == SAFECOMM_STATUS_OK)
    {
        conn->seq_tx = (uint16_t)(conn->seq_tx + 1U);
    }
    return st;
}
 
safecomm_status_t safecomm_sai_recv(safecomm_sai_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;
    }
 
    uint8_t in[SAFECOMM_SAF_MAX_PDU];
    size_t in_len = 0U;
    safecomm_status_t st = safecomm_saf_recv(conn->saf, in, sizeof(in), &in_len);
    if (st != SAFECOMM_STATUS_OK)
    {
        return st;
    }
    if ((in_len < SAI_HEADER_LEN) || (in[0] != SAI_MSG_EC_DATA))
    {
        return SAFECOMM_STATUS_STATE;
    }
 
    /* §5.4.7.2: sequence number check with up to (seq_window - 1) tolerated gaps. */
    uint16_t rx_seq = unpack_u16_be(&in[1]);
    if (conn->seq_rx_valid)
    {
        uint16_t diff = (uint16_t)(rx_seq - conn->seq_rx);
        if (diff >= conn->cfg.seq_window)
        {
            return SAFECOMM_STATUS_SEQ_ERROR;
        }
    }
    conn->seq_rx       = (uint16_t)(rx_seq + 1U);
    conn->seq_rx_valid = true;
 
    /* §5.4.9.4: freshness. last_rx_ec is used both now and at the next tick(s) until a fresher one arrives. */
    conn->last_rx_ec = unpack_u32_be(&in[15]);
    if (conn->ec_initialized)
    {
        int32_t delta = (int32_t)(conn->expected_ec - (double)conn->last_rx_ec);
        conn->ec_state = ec_state_from_delta(delta);
        if (conn->ec_state >= (int)conn->cfg.ec_alarm_state)
        {
            return SAFECOMM_STATUS_TIMEOUT;
        }
    }
 
    size_t payload_len = in_len - SAI_HEADER_LEN;
    if (payload_len > out_cap)
    {
        return SAFECOMM_STATUS_BUFFER_TOO_SMALL;
    }
    if (payload_len > 0U)
    {
        (void)memcpy(out, &in[SAI_HEADER_LEN], payload_len);
    }
    *out_len = payload_len;
    return SAFECOMM_STATUS_OK;
}
 
void safecomm_sai_tick(safecomm_sai_conn_t *conn)
{
    if ((conn == NULL) || (!conn->ec_initialized))
    {
        return;
    }
    int32_t delta = (int32_t)(conn->expected_ec - (double)conn->last_rx_ec);
    conn->ec_state = ec_state_from_delta(delta);
    conn->expected_ec += conn->ratio;
    conn->ec_tx++;
}