ab_gp_proc_dispatch.c

Go to the documentation of this file.

 
#include "ab_gp_proc_dispatch.h"
 
#include "ab_gp_ertms_engine.h"
#include "../ertms/train/ab_gp_ertms_train_message_codec.h"
#include "../state_machines/ab_gp_dispatch.h"
#include "../com/ab_gp_channel.h"
#include "../com/ab_gp_channel_stage.h"
#include "../com/ab_gp_saf_train.h"
#include "../../common/common_config.h"
#include "../../common/rbc_wire_types.h"
#include "../../common/site_config.h"
 
#include "rte/oal/memory/rte_mem_util.h"
#include "rte/oal/log/rte_log.h"

static rbc_timestamp_ms_t s_reply_t_train = 0U;
 
static rbc_timestamp_ms_t inbound_t_train(const rbc_train_inbound_msg_t *in)
{
    switch (in->nid_message)
    {
        case RBC_MSG_TRAIN_TO_TRACK_VALIDATED_TRAIN_DATA: return in->payload.msg_129.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_REQUEST_FOR_SHUNTING: return in->payload.msg_130.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_MA_REQUEST:           return in->payload.msg_132.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_POSITION_REPORT:      return in->payload.msg_136.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_ACKNOWLEDGEMENT:      return in->payload.msg_146.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_ACK_EMERGENCY_STOP:   return in->payload.msg_147.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_END_OF_MISSION:       return in->payload.msg_150.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_INIT_SESSION:         return in->payload.msg_155.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_TERMINATE_SESSION:    return in->payload.msg_156.header.t_train;
        case RBC_MSG_TRAIN_TO_TRACK_SESSION_ESTABLISHED:  return in->payload.msg_159.header.t_train;
        default:                                          return 0U;
    }
}

static bool build_and_stage_reply(ab_gp_channel_handle_t channel, uint8_t train_id,
                                  const rbc_train_outbound_msg_t *out_msg)
{
    uint8_t frame[RBC_TRAIN_ENVELOPE_WIRE_SIZE];
    size_t codec_bytes = 0U;
    rte_status_t status;
    ab_gp_saf_train_t *saf;
 
    rte_mem_set(frame, 0, sizeof(frame));
    frame[0] = (uint8_t)RBC_MSG_ERTMS_ENVELOPE;
    frame[1] = train_id;
 
    if (!rbc_train_codec_encode_track_to_train(out_msg, &frame[2], sizeof(frame) - 2U, &codec_bytes))
    {
        return false;
    }
 
    saf = ab_gp_channel_saf_train(channel);
    if ((saf != NULL) && saf->enabled)
    {
        /* REQ-SC-001/004: with the train link protected, a reply goes out only as a Sa-DATA frame of the train's
         * safe connection - never unprotected. No connection in DATA (or a PDU too large) drops it. */
        uint8_t saf_frame[AB_GP_SAF_FRAME_MAX];
        size_t saf_len = 0U;
 
        if (!ab_gp_saf_train_wrap(saf, train_id, &frame[2], codec_bytes, saf_frame, sizeof(saf_frame), &saf_len) ||
            !ab_gp_saf_envelope_encode(train_id, saf_frame, saf_len, frame))
        {
            return false;
        }
    }
 
    status = ab_gp_channel_stage_relay(channel, RTE_EXAMPLE_RELAY_KIND_TRAIN, 0U, frame, sizeof(frame));
    return (status == RTE_STATUS_OK);
}

static void build_config_msg_24(rbc_msg_24_general_message_t *m24, rbc_nid_engine_t nid_engine)
{
    rte_mem_set(m24, 0, sizeof(*m24));
    m24->header.nid_message = RBC_MSG_TRACK_TO_TRAIN_GENERAL_MESSAGE;
    m24->header.t_train = s_reply_t_train;
    m24->header.m_ack = RBC_M_ACK_NOT_REQUIRED;
    m24->header.nid_lrbg.nid_bg = (uint16_t)nid_engine;
 
    m24->has_national_values = true;
    m24->national_values.v_nvshunt = 30U;
    m24->national_values.v_nvstff = 40U;
    m24->national_values.v_nvunfit = 100U;
    m24->national_values.t_nvcontact = 10U;
    m24->national_values.d_nvroll = 2U;
 
    m24->has_ma_request_params = true;
    m24->ma_request_params.t_mar_s = 5U;
    m24->ma_request_params.d_mar_m = 100U;
 
    m24->has_pr_params = true;
    m24->pr_params.t_cycloc_s = 2U;
    m24->pr_params.d_cycloc_m = 0U;
}
 
static void handle_msg_155(ab_gp_channel_handle_t channel, ab_gp_train_session_t *session, ab_gp_db_t *db,
                           uint8_t train_id, ab_gp_ertms_train_context_t *ctx,
                           const rbc_msg_155_init_session_t *m155, uint32_t cycle, uint32_t timestamp_ms)
{
    rbc_train_outbound_msg_t reply;
 
    /* Previously set by ab_gp_ertms_engine_step()'s own now-removed
     * auto-drive (off db_connected alone) - now set here, from the real
     * Msg 155 that actually identifies the train, the first point this
     * context has one to record. */
    ctx->nid_engine = m155->header.nid_engine;
    ctx->train_data.valid = false;
    ctx->train_data.config_sent = false;
    ctx->train_data.state = AB_GP_TRAIN_DATA_NONE;
    (void)ab_gp_proc_session_handle_initiation(ctx, cycle, timestamp_ms);
    /* This project's own simplified flow has no separate train-side
     * confirmation round trip before Full Supervision work can begin
     * (unlike the real protocol's own Msg 159) - drive straight to
     * ESTABLISHED once RBC's own accept is about to go out. */
    (void)ab_gp_proc_session_handle_established(ctx, 0U, timestamp_ms);
 
    if (session != NULL)
    {
        if (!session->in_use)
        {
            rte_mem_set(session, 0, sizeof(*session));
            session->in_use = true;
            session->train_id = train_id;
        }
        /* A new session starts a new CTC notification cycle (CTC_CONNECTED, then CTC_MA_GRANTED for its first MA)
         * even when the slot stayed in_use across the previous End of Mission (routes still held): otherwise the first
         * MA of this mission reaches CTC as CTC_MA_EXTENDED with ma_seq 1 - seen on both MA paths (the ERTMS MA
         * request and GA's route-connect grant), ISS-009. */
        session->ctc_connected_sent = false;
        session->ctc_ma_granted_sent = false;
    }
    if (db != NULL)
    {
        (void)ab_gp_train_handle_connect(db, train_id);
    }
 
    rte_mem_set(&reply, 0, sizeof(reply));
    reply.nid_message = RBC_MSG_TRACK_TO_TRAIN_GENERAL_MESSAGE;
    reply.payload.msg_24.header.nid_message = RBC_MSG_TRACK_TO_TRAIN_GENERAL_MESSAGE;
    reply.payload.msg_24.header.t_train = s_reply_t_train;
    reply.payload.msg_24.header.m_ack = RBC_M_ACK_NOT_REQUIRED;
    reply.payload.msg_24.header.nid_lrbg.nid_bg = (uint16_t)m155->header.nid_engine;
    /* Bare accept - no packets yet; Packet 3/57/58 ride the LATER Msg 24
     * once train data + first position are both in (see handle_msg_136). */
    (void)build_and_stage_reply(channel, train_id, &reply);
}
 
static void handle_msg_129(const rbc_msg_129_validated_train_data_t *m129, ab_gp_ertms_train_context_t *ctx,
                           uint8_t train_id, uint32_t timestamp_ms)
{
    (void)ctx; /* validate_train_data() reaches the same context internally via train_id */
    (void)ab_gp_ertms_validate_train_data(train_id, m129->l_train, (uint32_t)m129->v_maxtrain,
                                          m129->m_axleload, m129->m_loadinggauge, m129->m_airtight,
                                          timestamp_ms);
}

static void handle_msg_130(ab_gp_channel_handle_t channel, uint8_t train_id, ab_gp_ertms_train_context_t *ctx,
                           const rbc_msg_130_request_for_shunting_t *m130, uint32_t cycle, uint32_t timestamp_ms)
{
    rbc_train_outbound_msg_t reply;
    const rbc_packet_0_position_report_t *pos = &m130->position_report;
    ab_gp_shunting_decision_t decision;
 
    (void)ab_gp_ertms_update_train_position(train_id, pos->nid_lrbg.nid_bg, pos->d_lrbg,
                                            pos->q_dirlrbg,
                                            (uint32_t)pos->v_train, (uint32_t)pos->m_mode,
                                            (uint32_t)pos->m_level, cycle, timestamp_ms);
 
    decision = ab_gp_proc_shunting_handle_request(ctx, train_id, (uint32_t)pos->v_train);
    RTE_EXAMPLE_LOG("cycle %u: ERTMS Msg 130 train %u: SH %s\n", cycle, (unsigned int)train_id,
                    ab_gp_proc_shunting_decision_name(decision));
 
    (void)ab_gp_proc_shunting_build_reply(decision, s_reply_t_train, m130->header.t_train, pos->nid_lrbg, &reply);
    if (!build_and_stage_reply(channel, train_id, &reply))
    {
        RTE_EXAMPLE_LOG("cycle %u: ERTMS Msg %u to train %u could not be staged\n", cycle,
                        (unsigned int)reply.nid_message, (unsigned int)train_id);
    }
}
 
static void handle_msg_136(ab_gp_channel_handle_t channel, ab_gp_train_session_t *session, ab_gp_db_t *db,
                           uint8_t train_id, ab_gp_ertms_train_context_t *ctx,
                           const rbc_msg_136_position_report_t *m136, uint32_t cycle, uint32_t timestamp_ms)
{
    rbc_train_outbound_msg_t reply;
    const rbc_packet_0_position_report_t *pos = &m136->position_report;
    /* A Packet-1 report while the session is already IDLE is the new
     * post-EoM idle heartbeat (see TrainRBCSim's own 6s Packet-1 cadence) -
     * a pure liveness ping, not a re-connect signal. Everything else
     * (Packet 0 at any session state, or a Packet-1 report arriving while
     * a session IS active/established) keeps today's exact behaviour:
     * session->in_use resurrection + db connect/position-report side
     * effects still fire, same as before this field existed. */
    const bool is_idle_heartbeat = m136->has_packet1 && (ctx->session_state == AB_GP_ERTMS_SESSION_IDLE);
 
    (void)ab_gp_ertms_update_train_position(train_id, pos->nid_lrbg.nid_bg, pos->d_lrbg,
                                            pos->q_dirlrbg,
                                            (uint32_t)pos->v_train, (uint32_t)pos->m_mode,
                                            (uint32_t)pos->m_level, cycle, timestamp_ms);
 
    if (!is_idle_heartbeat)
    {
        if (session != NULL)
        {
            if (!session->in_use)
            {
                rte_mem_set(session, 0, sizeof(*session));
                session->in_use = true;
                session->train_id = train_id;
            }
            /* Deliberately NOT session->cycle = cycle here: that field's own
             * doc is "Last M136 cycle number seen for this train" - the
             * FLAT scheme's own M136 handler (ab_gp_train.c) fills it from
             * env->cycle, a field the TRAIN itself reports on the wire. Real
             * Subset-026's own Msg 136/Packet 0 has no equivalent counter at
             * all, so there is nothing train-reported to put here - writing
             * this function's own @p cycle (GP's cyclic-executive count,
             * ctx->cyc_cycle) instead would silently give the field a
             * different, GP-side meaning depending on which scheme last
             * touched it (found live, not yet consequential: nothing
             * currently reads this field back for an ERTMS-managed session -
             * ab_gp_train_build_position_answer(), the one real reader, is
             * only ever called from the flat scheme's own output path). */
            session->d_lrbg = pos->d_lrbg;
        }
        if (db != NULL)
        {
            (void)ab_gp_train_handle_connect(db, train_id);
            (void)ab_gp_train_handle_position_report(db, train_id, pos->nid_lrbg.nid_bg);
        }
    }
 
    rte_mem_set(&reply, 0, sizeof(reply));
    reply.nid_message = RBC_MSG_TRACK_TO_TRAIN_GENERAL_MESSAGE;
    if (ctx->train_data.valid && !ctx->train_data.config_sent)
    {
        /* FR-RBC-SOM-003: first Msg 24 after train data + first position
         * report both arrived - confirms train data and injects config
         * packets. */
        build_config_msg_24(&reply.payload.msg_24, m136->header.nid_engine);
        ctx->train_data.config_sent = true;
    }
    else
    {
        /* FR-RBC-EOM-002 / Phase 2 cyclic ack: bare acknowledgement of a
         * later position report, no packets. */
        reply.payload.msg_24.header.nid_message = RBC_MSG_TRACK_TO_TRAIN_GENERAL_MESSAGE;
        reply.payload.msg_24.header.t_train = s_reply_t_train;
        reply.payload.msg_24.header.m_ack = RBC_M_ACK_NOT_REQUIRED;
        reply.payload.msg_24.header.nid_lrbg.nid_bg = (uint16_t)m136->header.nid_engine;
    }
    (void)build_and_stage_reply(channel, train_id, &reply);
}
 
static void handle_msg_132(ab_gp_channel_handle_t channel, ab_gp_train_session_t *session, ab_gp_db_t *db,
                           uint8_t train_id, ab_gp_ertms_train_context_t *ctx)
{
    uint16_t held_routes[RTE_EXAMPLE_MAX_ROUTES_PER_TRAIN];
    uint8_t held_count = 0U;
    uint16_t route_id = 0U;
    uint8_t i;
    uint8_t j;
    bool found = false;
    bool fresh_ma;
    rbc_train_outbound_msg_t reply;
 
    if (!ab_gp_db_train_get_routes(db, train_id, held_routes, &held_count))
    {
        return;
    }
 
    /* First IL-granted route this train holds that isn't already
     * reflected in its own active MA - matches this codebase's own
     * "IL grants/locks, GP turns an already-granted route into an MA"
     * split (ab_ga_il.c's handle_route_add() is untouched by this). */
    for (i = 0U; (i < held_count) && !found; i++)
    {
        bool already_in_ma = false;
        for (j = 0U; j < ctx->ma.route_count; j++)
        {
            if (ctx->ma.route_ids[j] == held_routes[i])
            {
                already_in_ma = true;
                break;
            }
        }
        if (!already_in_ma)
        {
            route_id = held_routes[i];
            found = true;
        }
    }
 
    if (!found)
    {
        return;
    }
 
    fresh_ma = !ctx->ma.has_active_ma;
    if (fresh_ma)
    {
        if (!ab_gp_ertms_calc_ma_for_route(train_id, route_id, 1U))
        {
            return;
        }
    }
    else
    {
        if (!ab_gp_ertms_extend_ma_for_route(train_id, route_id))
        {
            return;
        }
    }
 
    if (session != NULL)
    {
        session->granted_length = (uint32_t)ctx->ma.p15.ma_length;
        session->ma_seq = ctx->ma.ma_seq;
        /* Deliberately ctc_ma_notify_pending, NOT ma_pending_send: this
         * handler already sends its own RBC_MSG_ERTMS_ENVELOPE-wrapped
         * Msg 3 below (build_and_stage_reply()) - setting ma_pending_send
         * here too would make ab_gp_train_build_outputs() (the FLAT
         * scheme's own output builder) ALSO build and stage a duplicate
         * flat-kind M3 as an unwanted side effect (found live: every
         * real ERTMS grant was silently also emitting a second, flat M3
         * on the same Train relay channel, which train_sim.py's own
         * receive loop then auto-acked with an unrelated flat Msg 146).
         * ab_ga_ctc_build_outputs() (GA) checks this flag in addition to
         * ma_pending_send specifically so CTC still gets its
         * CTC_MA_GRANTED/_EXTENDED indication - see that function's own
         * comment and ab_gp_train_session_t's own doc on this flag. */
        session->ctc_ma_notify_pending = true;
        /* CTC is told GRANTED vs EXTENDED by the same decision made above (fresh MA vs extension), not by a latch
         * that can outlive the mission: a session slot kept in_use across an End of Mission (routes still held,
         * see the EoM case below) kept ctc_ma_granted_sent from the previous mission, so the next mission's first
         * MA (ma_seq 1) was reported as CTC_MA_EXTENDED (ISS-009). */
        if (fresh_ma)
        {
            session->ctc_ma_granted_sent = false;
        }
    }
 
    rte_mem_set(&reply, 0, sizeof(reply));
    reply.nid_message = RBC_MSG_TRACK_TO_TRAIN_MOVEMENT_AUTHORITY;
    reply.payload.msg_3.header.nid_message = RBC_MSG_TRACK_TO_TRAIN_MOVEMENT_AUTHORITY;
    reply.payload.msg_3.header.t_train = s_reply_t_train;
    reply.payload.msg_3.header.m_ack = RBC_M_ACK_REQUIRED;
    reply.payload.msg_3.ma = ctx->ma.p15;
    reply.payload.msg_3.has_gradient = (ctx->ma.p21.segment_count > 0U);
    reply.payload.msg_3.gradient = ctx->ma.p21;
    reply.payload.msg_3.has_ssp = (ctx->ma.p27.segment_count > 0U);
    reply.payload.msg_3.ssp = ctx->ma.p27;
    reply.payload.msg_3.has_linking = (ctx->ma.p5.link_count > 0U);
    reply.payload.msg_3.linking = ctx->ma.p5;
 
    (void)build_and_stage_reply(channel, train_id, &reply);
}
 
bool ab_gp_proc_dispatch_train_inbound(ab_gp_channel_handle_t channel,
                                       ab_gp_train_session_t sessions[],
                                       ab_gp_db_t *db, uint8_t train_id,
                                       const uint8_t *payload, size_t payload_len,
                                       uint32_t cycle, uint32_t timestamp_ms)
{
    rbc_train_inbound_msg_t inbound;
    ab_gp_ertms_train_context_t *ctx;
    ab_gp_train_session_t *session = NULL;
 
    /* REQ-RBC-005: the wire-supplied train_id is range-checked before it indexes sessions[] or the ERTMS context.
     * The plain-ERTMS caller (pre_execute.c) logs the false return; on the SaF path an out-of-range id never gets
     * here, ab_gp_saf_train_on_frame() rejects it first and that rejection is logged. */
    if ((payload == NULL) || (train_id < 1U) || (train_id > (uint8_t)RTE_EXAMPLE_MAX_TRAINS))
    {
        return false;
    }
 
    if (!rbc_train_codec_decode_train_to_track(payload, payload_len, &inbound))
    {
        return false;
    }
 
    ctx = ab_gp_ertms_engine_get_train_context(train_id);
    if (ctx == NULL)
    {
        return false;
    }
    s_reply_t_train = inbound_t_train(&inbound);
 
    if (sessions != NULL)
    {
        session = &sessions[train_id - 1U];
    }
 
    switch (inbound.nid_message)
    {
        case RBC_MSG_TRAIN_TO_TRACK_INIT_SESSION:
            handle_msg_155(channel, session, db, train_id, ctx, &inbound.payload.msg_155, cycle, timestamp_ms);
            return true;
 
        case RBC_MSG_TRAIN_TO_TRACK_VALIDATED_TRAIN_DATA:
            handle_msg_129(&inbound.payload.msg_129, ctx, train_id, timestamp_ms);
            return true;
 
        case RBC_MSG_TRAIN_TO_TRACK_POSITION_REPORT:
            handle_msg_136(channel, session, db, train_id, ctx, &inbound.payload.msg_136, cycle, timestamp_ms);
            /* Raise the semantic event too - reaches the (stub) position
             * FSM. handle_msg_136() above still does the real work. */
            (void)ab_gp_dispatch(ctx, AB_GP_EVENT_POSITION_REPORTED);
            return true;
 
        case RBC_MSG_TRAIN_TO_TRACK_MA_REQUEST:
            handle_msg_132(channel, session, db, train_id, ctx);
            (void)ab_gp_dispatch(ctx, AB_GP_EVENT_MA_REQUESTED);
            return true;
 
        case RBC_MSG_TRAIN_TO_TRACK_REQUEST_FOR_SHUNTING:
            handle_msg_130(channel, train_id, ctx, &inbound.payload.msg_130, cycle, timestamp_ms);
            return true;
 
        case RBC_MSG_TRAIN_TO_TRACK_ACKNOWLEDGEMENT:
            /* Real Msg 146 acks by timestamp, not ma_seq (this codec's
             * own rbc_msg_146_ack_t only carries t_train_ack) - this
             * simplified flow has at most one outstanding MA per train,
             * so acking the currently-active ma_seq is a safe, deliberate
             * simplification, not a wire-format gap. */
            (void)ab_gp_proc_ma_handle_ack(ctx, ctx->ma.ma_seq);
            if (session != NULL)
            {
                session->ma_acked = true;
            }
            (void)ab_gp_dispatch(ctx, AB_GP_EVENT_MA_ACKNOWLEDGED);
            return true;
 
        case RBC_MSG_TRAIN_TO_TRACK_TERMINATE_SESSION:
        {
            /* FR-RBC-EOM-003: releases every route this train holds and
             * returns the session to IDLE - same teardown shape
             * RBC_MSG_TRAIN_DISCONNECT's own handler already uses. */
            bool db_removed = true;
 
            (void)ab_gp_proc_session_handle_termination(ctx, db);
            (void)ab_gp_dispatch(ctx, AB_GP_EVENT_END_OF_MISSION);
            if (db != NULL)
            {
                /* ab_gp_train_remove() itself refuses to remove a train
                 * still holding routes in db (route_count > 0) - see its
                 * own doc. If that ever happens, session->in_use must
                 * NOT be cleared below regardless: doing so unconditionally
                 * would leave sessions[] saying "not in use" while db
                 * still says "connected", the exact session/db drift
                 * this dispatcher's own session-sync exists to prevent
                 * (see ab_gp_proc_dispatch.h's own doc). Logged, not
                 * silently swallowed with (void), so a real occurrence is
                 * actually visible. */
                db_removed = ab_gp_train_remove(db, train_id);
                if (!db_removed)
                {
                    RTE_EXAMPLE_LOG(
                        "cycle %u: ERTMS EoM for train %u could not remove db train slot "
                        "(routes still held) - leaving session in_use to avoid session/db drift\n",
                        cycle, (unsigned int)train_id);
                }
            }
            if ((session != NULL) && db_removed)
            {
                session->in_use = false;
            }
            return true;
        }
 
        case RBC_MSG_TRAIN_TO_TRACK_SESSION_ESTABLISHED:
            (void)ab_gp_proc_session_handle_established(ctx, inbound.payload.msg_159.m_version, timestamp_ms);
            return true;
 
        default:
            /* REQ-RBC-002: a decoded-but-unhandled nid_message is
             * rejected here, not silently forwarded - the caller logs it. */
            return false;
    }
}