ab_gp_saf_train.c

Go to the documentation of this file.

#include "ab_gp_saf_train.h"
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
 
#include "rte/oal/random/rte_random.h"
#include "safecomm/euroradio/euroradio.h"
#include "../../common/rbc_wire_types.h"

#define AB_GP_SAF_ETY_RBC 1U
 
typedef struct
{
    bool     in_use;
    uint32_t peer_etcs_id;
    uint8_t  kmac[SAFECOMM_MAC_KEY_LEN];
} saf_key_t;
 
static saf_key_t s_keys[AB_GP_SAF_KEYS_MAX];
 
static safecomm_status_t no_send(void *user, const uint8_t *buf, size_t len)
{
    (void)user;
    (void)buf;
    (void)len;
    return SAFECOMM_STATUS_TRANSPORT;
}
 
static safecomm_status_t no_recv(void *user, uint8_t *buf, size_t cap, size_t *out_len)
{
    (void)user;
    (void)buf;
    (void)cap;
    (void)out_len;
    return SAFECOMM_STATUS_TRANSPORT;
}
 
static const saf_key_t *key_lookup(uint32_t peer_etcs_id)
{
    size_t i;
 
    for (i = 0U; i < (size_t)AB_GP_SAF_KEYS_MAX; i++)
    {
        if (s_keys[i].in_use && (s_keys[i].peer_etcs_id == peer_etcs_id))
        {
            return &s_keys[i];
        }
    }
    return NULL;
}
 
/* REQ-RBC-005: the index into slots[] by train_id for the frame, peer-RB and wrap paths; an out-of-range train_id
 * returns NULL to the caller before it indexes slots[]. On the wire paths the caller turns NULL into a rejection
 * that pre_execute.c logs ("frame rejected"); ab_gp_saf_train_wrap() returns false; its train_id was already
 * range-checked by ab_gp_proc_dispatch_train_inbound(). */
static ab_gp_saf_train_slot_t *slot_of(ab_gp_saf_train_t *s, uint8_t train_id)
{
    if ((s == NULL) || !s->enabled || (train_id < 1U) || (train_id > (uint8_t)RTE_EXAMPLE_MAX_TRAINS))
    {
        return NULL;
    }
    return &s->slots[train_id - 1U];
}
 
static void reject(ab_gp_saf_result_t *out, safecomm_status_t status)
{
    out->action = AB_GP_SAF_ACTION_REJECTED;
    out->status = status;
}
 
/* Sets up a fresh responder connection for the train named in AU1 and answers it with the given RB. */
static void respond_au1(ab_gp_saf_train_t *s, ab_gp_saf_train_slot_t *slot, const uint8_t *au1, const uint8_t *rb,
                        uint32_t now_ms, ab_gp_saf_result_t *out)
{
    uint32_t peer_id = 0U;
    uint8_t ra[SAFECOMM_MAC_RAND_LEN];
    safecomm_er_config_t er;
    safecomm_saf_config_t cfg;
    const safecomm_transport_t none = { no_send, no_recv, NULL, NULL };
    const saf_key_t *key;
    safecomm_status_t st;
 
    st = safecomm_saf_au1_fields(au1, SAFECOMM_SAF_AU1_LEN, &peer_id, ra);
    if (st != SAFECOMM_STATUS_OK)
    {
        reject(out, st);
        return;
    }
    key = key_lookup(peer_id);
    if (key == NULL)
    {
        reject(out, SAFECOMM_STATUS_AUTH_FAILED); /* no KMAC for this train: it cannot be authenticated */
        return;
    }
    (void)memset(&er, 0, sizeof(er));
    er.side = SAFECOMM_ER_SIDE_RBC;
    (void)memcpy(er.kmac, key->kmac, sizeof(er.kmac));
    er.etcs_id_self = s->rbc_etcs_id;
    er.etcs_id_peer = peer_id;
    (void)safecomm_er_make_saf_config(&er, &cfg);
    cfg.ety = AB_GP_SAF_ETY_RBC;
    (void)safecomm_saf_init(&slot->conn, &cfg, &none);
    (void)memset(&cfg, 0, sizeof(cfg)); /* do not leave a KMAC copy on the stack */
    safecomm_saf_set_time(&slot->conn, now_ms);
    st = safecomm_saf_responder_on_au1(&slot->conn, au1, SAFECOMM_SAF_AU1_LEN, rb, out->frame, sizeof(out->frame),
                                       &out->frame_len);
    if (st != SAFECOMM_STATUS_OK)
    {
        reject(out, st);
        return;
    }
    out->action = AB_GP_SAF_ACTION_REPLY;
    out->status = SAFECOMM_STATUS_OK;
}
 
void ab_gp_saf_keys_clear(void)
{
    (void)memset(s_keys, 0, sizeof(s_keys));
}
 
bool ab_gp_saf_keys_install(uint32_t peer_etcs_id, const uint8_t kmac[SAFECOMM_MAC_KEY_LEN])
{
    size_t i;
    saf_key_t *slot = NULL;
 
    if (kmac == NULL)
    {
        return false;
    }
    for (i = 0U; (i < (size_t)AB_GP_SAF_KEYS_MAX) && (slot == NULL); i++)
    {
        if (s_keys[i].in_use && (s_keys[i].peer_etcs_id == peer_etcs_id))
        {
            slot = &s_keys[i];
        }
    }
    for (i = 0U; (i < (size_t)AB_GP_SAF_KEYS_MAX) && (slot == NULL); i++)
    {
        if (!s_keys[i].in_use)
        {
            slot = &s_keys[i];
        }
    }
    if (slot == NULL)
    {
        return false;
    }
    slot->in_use = true;
    slot->peer_etcs_id = peer_etcs_id;
    (void)memcpy(slot->kmac, kmac, sizeof(slot->kmac));
    return true;
}
 
static int hex_nibble(char c)
{
    if ((c >= '0') && (c <= '9'))
    {
        return c - '0';
    }
    if ((c >= 'a') && (c <= 'f'))
    {
        return (c - 'a') + 10;
    }
    if ((c >= 'A') && (c <= 'F'))
    {
        return (c - 'A') + 10;
    }
    return -1;
}
 
/* Parses one non-comment line "<id> <48 hex>"; false on any deviation. */
static bool parse_key_line(char *line, uint32_t *out_id, uint8_t out_kmac[SAFECOMM_MAC_KEY_LEN])
{
    char *id_str = strtok(line, " \t\r\n");
    char *hex = strtok(NULL, " \t\r\n");
    char *extra = strtok(NULL, " \t\r\n");
    char *end = NULL;
    unsigned long id;
    size_t i;
 
    if ((id_str == NULL) || (hex == NULL) || (extra != NULL) || (strlen(hex) != (2U * SAFECOMM_MAC_KEY_LEN)))
    {
        return false;
    }
    id = strtoul(id_str, &end, 0);
    if ((end == id_str) || (*end != '\0') || (id > 0xFFFFFFUL))
    {
        return false;
    }
    for (i = 0U; i < SAFECOMM_MAC_KEY_LEN; i++)
    {
        int hi = hex_nibble(hex[2U * i]);
        int lo = hex_nibble(hex[(2U * i) + 1U]);
 
        if ((hi < 0) || (lo < 0))
        {
            return false;
        }
        out_kmac[i] = (uint8_t)((hi << 4) | lo);
    }
    *out_id = (uint32_t)id;
    return true;
}
 
bool ab_gp_saf_keys_load_file(const char *path, uint32_t *out_count)
{
    FILE *fp;
    char line[160];
    bool ok = true;
 
    if ((path == NULL) || (out_count == NULL))
    {
        return false;
    }
    *out_count = 0U;
    fp = fopen(path, "r");
    if (fp == NULL)
    {
        return false;
    }
    while (ok && (fgets(line, (int)sizeof(line), fp) != NULL))
    {
        char *hash = strchr(line, '#');
        char *p = line;
        uint32_t id = 0U;
        uint8_t kmac[SAFECOMM_MAC_KEY_LEN];
 
        if (hash != NULL)
        {
            *hash = '\0';
        }
        while ((*p == ' ') || (*p == '\t') || (*p == '\r') || (*p == '\n'))
        {
            p++;
        }
        if (*p == '\0')
        {
            continue;
        }
        ok = parse_key_line(p, &id, kmac) && ab_gp_saf_keys_install(id, kmac);
        (void)memset(kmac, 0, sizeof(kmac));
        if (ok)
        {
            (*out_count)++;
        }
    }
    (void)memset(line, 0, sizeof(line));
    (void)fclose(fp);
    return ok;
}
 
void ab_gp_saf_train_init(ab_gp_saf_train_t *s, bool enabled, bool is_role_a, uint32_t rbc_etcs_id)
{
    if (s == NULL)
    {
        return;
    }
    (void)memset(s, 0, sizeof(*s));
    s->enabled = enabled;
    s->is_role_a = is_role_a;
    s->rbc_etcs_id = rbc_etcs_id;
}
 
void ab_gp_saf_train_on_frame(ab_gp_saf_train_t *s, uint8_t train_id, const uint8_t *frame, size_t frame_len,
                              uint32_t now_ms, ab_gp_saf_result_t *out)
{
    ab_gp_saf_train_slot_t *slot;
    safecomm_status_t st;
 
    if (out == NULL)
    {
        return;
    }
    (void)memset(out, 0, sizeof(*out));
    slot = slot_of(s, train_id);
    if ((slot == NULL) || (frame == NULL) || (frame_len == 0U))
    {
        reject(out, SAFECOMM_STATUS_INVALID_PARAM);
        return;
    }
    safecomm_saf_set_time(&slot->conn, now_ms);
 
    switch (safecomm_saf_frame_kind(frame, frame_len))
    {
        case SAFECOMM_SAF_FRAME_AU1:
        {
            uint32_t peer_id = 0U;
            uint8_t ra[SAFECOMM_MAC_RAND_LEN];
 
            st = safecomm_saf_au1_fields(frame, frame_len, &peer_id, ra);
            if (st != SAFECOMM_STATUS_OK)
            {
                reject(out, st);
                return;
            }
            if (s->is_role_a)
            {
                uint8_t rb[SAFECOMM_MAC_RAND_LEN];
 
                if (rte_random_fill(rb, sizeof(rb)) != RTE_STATUS_OK)
                {
                    reject(out, SAFECOMM_STATUS_NOT_INITIALISED); /* no secure random source: never answer */
                    return;
                }
                respond_au1(s, slot, frame, rb, now_ms, out);
                if (out->action == AB_GP_SAF_ACTION_REPLY)
                {
                    out->share_rb = true;
                    out->rb_share[0] = train_id;
                    (void)memcpy(&out->rb_share[1], ra, SAFECOMM_MAC_RAND_LEN);
                    (void)memcpy(&out->rb_share[1U + SAFECOMM_MAC_RAND_LEN], rb, SAFECOMM_MAC_RAND_LEN);
                }
                (void)memset(rb, 0, sizeof(rb));
                return;
            }
            if (slot->rb_parked && (memcmp(slot->rb_ra, ra, sizeof(ra)) == 0))
            {
                slot->rb_parked = false;
                respond_au1(s, slot, frame, slot->rb, now_ms, out);
                return;
            }
            (void)memcpy(slot->au1, frame, SAFECOMM_SAF_AU1_LEN);
            slot->au1_parked = true;
            out->action = AB_GP_SAF_ACTION_NONE;
            return;
        }
        case SAFECOMM_SAF_FRAME_AU3:
            st = safecomm_saf_responder_on_au3(&slot->conn, frame, frame_len, out->frame, sizeof(out->frame),
                                               &out->frame_len);
            if (st != SAFECOMM_STATUS_OK)
            {
                reject(out, st);
                return;
            }
            out->action = AB_GP_SAF_ACTION_REPLY;
            return;
        case SAFECOMM_SAF_FRAME_DATA:
        case SAFECOMM_SAF_FRAME_DISCONNECT:
            st = safecomm_saf_accept_frame(&slot->conn, frame, frame_len, out->app, sizeof(out->app), &out->app_len);
            if (st == SAFECOMM_STATUS_OK)
            {
                out->action = AB_GP_SAF_ACTION_APP;
            }
            else if (st == SAFECOMM_STATUS_PEER_DISCONNECT)
            {
                out->action = AB_GP_SAF_ACTION_DISCONNECTED;
            }
            else if ((st == SAFECOMM_STATUS_STATE) && (safecomm_saf_state(&slot->conn) == SAFECOMM_SAF_STATE_IDLE) &&
                     (safecomm_saf_frame_kind(frame, frame_len) == SAFECOMM_SAF_FRAME_DATA) &&
                     (safecomm_saf_disconnect_frame(SAFECOMM_SAF_ROLE_RESPONDER, out->frame, sizeof(out->frame),
                                                    &out->frame_len) == SAFECOMM_STATUS_OK))
            {
                /* Sa-DATA for a connection this channel does not hold (typically: this process restarted under a
                 * running train). Release it so the train reconnects now rather than after its supervision time. The
                 * data itself is still dropped. Not while a handshake is in progress: a stale frame must not abort
                 * the new connection. */
                out->action = AB_GP_SAF_ACTION_REPLY;
            }
            else
            {
                reject(out, st);
            }
            out->status = st;
            return;
        default:
            reject(out, SAFECOMM_STATUS_STATE);
            return;
    }
}
 
void ab_gp_saf_train_on_peer_rb(ab_gp_saf_train_t *s, const uint8_t *payload, size_t size, uint32_t now_ms,
                                ab_gp_saf_result_t *out)
{
    ab_gp_saf_train_slot_t *slot;
    const uint8_t *ra;
    const uint8_t *rb;
 
    if (out == NULL)
    {
        return;
    }
    (void)memset(out, 0, sizeof(*out));
    if ((s == NULL) || s->is_role_a || (payload == NULL) || (size < (size_t)AB_GP_SAF_PEER_RB_SIZE))
    {
        reject(out, SAFECOMM_STATUS_INVALID_PARAM);
        return;
    }
    slot = slot_of(s, payload[0]);
    if (slot == NULL)
    {
        reject(out, SAFECOMM_STATUS_INVALID_PARAM);
        return;
    }
    ra = &payload[1];
    rb = &payload[1U + SAFECOMM_MAC_RAND_LEN];
    if (slot->au1_parked && (memcmp(&slot->au1[5], ra, SAFECOMM_MAC_RAND_LEN) == 0))
    {
        slot->au1_parked = false;
        respond_au1(s, slot, slot->au1, rb, now_ms, out);
        return;
    }
    (void)memcpy(slot->rb_ra, ra, SAFECOMM_MAC_RAND_LEN);
    (void)memcpy(slot->rb, rb, SAFECOMM_MAC_RAND_LEN);
    slot->rb_parked = true;
    out->action = AB_GP_SAF_ACTION_NONE;
}
 
bool ab_gp_saf_train_wrap(ab_gp_saf_train_t *s, uint8_t train_id, const uint8_t *pdu, size_t pdu_len,
                          uint8_t *out_frame, size_t out_cap, size_t *out_len)
{
    ab_gp_saf_train_slot_t *slot = slot_of(s, train_id);
 
    if ((slot == NULL) || (pdu == NULL) || (pdu_len > (size_t)AB_GP_SAF_PDU_MAX) || (out_frame == NULL) ||
        (out_len == NULL))
    {
        return false;
    }
    if (safecomm_saf_prepare_send(&slot->conn, pdu, pdu_len, out_frame, out_cap, out_len) != SAFECOMM_STATUS_OK)
    {
        return false;
    }
    return safecomm_saf_frame_sent(&slot->conn) == SAFECOMM_STATUS_OK;
}
 
uint32_t ab_gp_saf_train_supervise(ab_gp_saf_train_t *s, uint32_t now_ms)
{
    uint32_t lost = 0U;
    uint8_t i;
 
    if ((s == NULL) || !s->enabled)
    {
        return 0U;
    }
    for (i = 0U; i < (uint8_t)RTE_EXAMPLE_MAX_TRAINS; i++)
    {
        safecomm_saf_conn_t *conn = &s->slots[i].conn;
 
        if (safecomm_saf_state(conn) == SAFECOMM_SAF_STATE_DATA)
        {
            safecomm_saf_set_time(conn, now_ms);
            if (safecomm_saf_supervise(conn) == SAFECOMM_STATUS_TIMEOUT)
            {
                lost |= (uint32_t)1U << i;
            }
        }
    }
    return lost;
}
 
bool ab_gp_saf_train_is_connected(const ab_gp_saf_train_t *s, uint8_t train_id)
{
    /* REQ-RBC-005: an out-of-range train_id returns false to the caller before it indexes slots[]. */
    if ((s == NULL) || !s->enabled || (train_id < 1U) || (train_id > (uint8_t)RTE_EXAMPLE_MAX_TRAINS))
    {
        return false;
    }
    return safecomm_saf_state(&s->slots[train_id - 1U].conn) == SAFECOMM_SAF_STATE_DATA;
}
 
bool ab_gp_saf_envelope_encode(uint8_t train_id, const uint8_t *frame, size_t frame_len,
                               uint8_t out[RBC_TRAIN_ENVELOPE_WIRE_SIZE])
{
    if ((frame == NULL) || (out == NULL) || (frame_len == 0U) || (frame_len > (size_t)AB_GP_SAF_FRAME_MAX))
    {
        return false;
    }
    (void)memset(out, 0, (size_t)RBC_TRAIN_ENVELOPE_WIRE_SIZE);
    out[0] = (uint8_t)RBC_MSG_SAF_ENVELOPE;
    out[1] = train_id;
    out[2] = (uint8_t)((frame_len >> 8U) & 0xFFU);
    out[3] = (uint8_t)(frame_len & 0xFFU);
    (void)memcpy(&out[AB_GP_SAF_ENVELOPE_HEADER], frame, frame_len);
    return true;
}
 
bool ab_gp_saf_envelope_decode(const uint8_t env[RBC_TRAIN_ENVELOPE_WIRE_SIZE], uint8_t *out_train_id,
                               const uint8_t **out_frame, size_t *out_len)
{
    size_t len;
 
    if ((env == NULL) || (out_train_id == NULL) || (out_frame == NULL) || (out_len == NULL) ||
        (env[0] != (uint8_t)RBC_MSG_SAF_ENVELOPE))
    {
        return false;
    }
    len = ((size_t)env[2] << 8U) | (size_t)env[3];
    if ((len == 0U) || (len > (size_t)AB_GP_SAF_FRAME_MAX) || (env[1] < 1U) ||
        (env[1] > (uint8_t)RTE_EXAMPLE_MAX_TRAINS))
    {
        return false;
    }
    *out_train_id = env[1];
    *out_frame = &env[AB_GP_SAF_ENVELOPE_HEADER];
    *out_len = len;
    return true;
}