ab_gp_ertms_train_packet_codec.c

Go to the documentation of this file.

 
#include "ab_gp_ertms_train_packet_codec.h"
 
#include "rte/utils/cast/rte_cast.h"
 
bool rbc_codec_read_u8(const uint8_t *buf, size_t len, size_t *offset, uint8_t *val)
{
    if (buf == NULL || offset == NULL || val == NULL || *offset >= len)
    {
        return false;
    }
    *val = buf[*offset];
    *offset += 1U;
    return true;
}
 
bool rbc_codec_read_u16_be(const uint8_t *buf, size_t len, size_t *offset, uint16_t *val)
{
    if (buf == NULL || offset == NULL || val == NULL || (*offset + 2U) > len)
    {
        return false;
    }
    *val = (uint16_t)(((uint16_t)buf[*offset] << 8) | (uint16_t)buf[*offset + 1U]);
    *offset += 2U;
    return true;
}
 
bool rbc_codec_read_u32_be(const uint8_t *buf, size_t len, size_t *offset, uint32_t *val)
{
    if (buf == NULL || offset == NULL || val == NULL || (*offset + 4U) > len)
    {
        return false;
    }
    *val = ((uint32_t)buf[*offset] << 24) |
           ((uint32_t)buf[*offset + 1U] << 16) |
           ((uint32_t)buf[*offset + 2U] << 8) |
           ((uint32_t)buf[*offset + 3U]);
    *offset += 4U;
    return true;
}
 
bool rbc_codec_write_u8(uint8_t *buf, size_t cap, size_t *offset, uint8_t val)
{
    if (buf == NULL || offset == NULL || *offset >= cap)
    {
        return false;
    }
    buf[*offset] = val;
    *offset += 1U;
    return true;
}
 
bool rbc_codec_write_u16_be(uint8_t *buf, size_t cap, size_t *offset, uint16_t val)
{
    if (buf == NULL || offset == NULL || (*offset + 2U) > cap)
    {
        return false;
    }
    (void)rte_cast_u16_to_u8((val >> 8) & 0xFFU, &buf[*offset]);
    (void)rte_cast_u16_to_u8(val & 0xFFU, &buf[*offset + 1U]);
    *offset += 2U;
    return true;
}
 
bool rbc_codec_write_u32_be(uint8_t *buf, size_t cap, size_t *offset, uint32_t val)
{
    if (buf == NULL || offset == NULL || (*offset + 4U) > cap)
    {
        return false;
    }
    (void)rte_cast_u32_to_u8((val >> 24) & 0xFFU, &buf[*offset]);
    (void)rte_cast_u32_to_u8((val >> 16) & 0xFFU, &buf[*offset + 1U]);
    (void)rte_cast_u32_to_u8((val >> 8) & 0xFFU, &buf[*offset + 2U]);
    (void)rte_cast_u32_to_u8(val & 0xFFU, &buf[*offset + 3U]);
    *offset += 4U;
    return true;
}

bool rbc_codec_write_u64_be(uint8_t *buf, size_t cap, size_t *offset, uint64_t val)
{
    uint32_t hi = (uint32_t)((val >> 32) & 0xFFFFFFFFU);
    uint32_t lo = (uint32_t)(val & 0xFFFFFFFFU);
 
    return rbc_codec_write_u32_be(buf, cap, offset, hi) && rbc_codec_write_u32_be(buf, cap, offset, lo);
}
 
/* ========================================================================= */
/* Packet decode                                                             */
/* ========================================================================= */

bool rbc_packet_read_0_position_report(const uint8_t *buf, size_t len, size_t *offset,
                                       rbc_packet_0_position_report_t *out_p0)
{
    uint8_t  q_scale = 0U;
    uint16_t nid_c = 0U;
    uint16_t nid_bg = 0U;
    uint32_t d_lrbg = 0U;
    uint8_t  q_dirlrbg = 0U;
    uint8_t  q_dlrbg = 0U;
    uint32_t l_doubtover = 0U;
    uint32_t l_doubtunder = 0U;
    uint16_t v_train = 0U;
    uint8_t m_mode = 0U;
    uint8_t m_level = 0U;
 
    if (out_p0 == NULL ||
        !rbc_codec_read_u8(buf, len, offset, &q_scale) ||
        !rbc_codec_read_u16_be(buf, len, offset, &nid_c) ||
        !rbc_codec_read_u16_be(buf, len, offset, &nid_bg) ||
        !rbc_codec_read_u32_be(buf, len, offset, &d_lrbg) ||
        !rbc_codec_read_u8(buf, len, offset, &q_dirlrbg) ||
        !rbc_codec_read_u8(buf, len, offset, &q_dlrbg) ||
        !rbc_codec_read_u32_be(buf, len, offset, &l_doubtover) ||
        !rbc_codec_read_u32_be(buf, len, offset, &l_doubtunder) ||
        !rbc_codec_read_u16_be(buf, len, offset, &v_train) ||
        !rbc_codec_read_u8(buf, len, offset, &m_mode) ||
        !rbc_codec_read_u8(buf, len, offset, &m_level))
    {
        return false;
    }
 
    out_p0->q_scale = (rbc_q_scale_t)q_scale;
    out_p0->nid_lrbg.nid_c = nid_c;
    out_p0->nid_lrbg.nid_bg = nid_bg;
    out_p0->d_lrbg = d_lrbg;
    out_p0->q_dirlrbg = (rbc_q_dirlrbg_t)q_dirlrbg;
    out_p0->q_dlrbg = (rbc_q_dlrbg_t)q_dlrbg;
    out_p0->l_doubtover = l_doubtover;
    out_p0->l_doubtunder = l_doubtunder;
    out_p0->v_train = (rbc_speed_kmh_t)v_train;
    out_p0->m_mode = (rbc_m_mode_t)m_mode;
    out_p0->m_level = (rbc_m_level_t)m_level;
    return true;
}

bool rbc_packet_read_1_position_report_2bg(const uint8_t *buf, size_t len, size_t *offset,
                                           rbc_packet_1_position_report_2bg_t *out_p1)
{
    uint16_t nid_c = 0U;
    uint16_t nid_bg = 0U;
    uint16_t prv_nid_c = 0U;
    uint16_t prv_nid_bg = 0U;
    uint32_t d_lrbg = 0U;
    uint16_t v_train = 0U;
    uint8_t m_mode = 0U;
    uint8_t m_level = 0U;
 
    if (out_p1 == NULL ||
        !rbc_codec_read_u16_be(buf, len, offset, &nid_c) ||
        !rbc_codec_read_u16_be(buf, len, offset, &nid_bg) ||
        !rbc_codec_read_u16_be(buf, len, offset, &prv_nid_c) ||
        !rbc_codec_read_u16_be(buf, len, offset, &prv_nid_bg) ||
        !rbc_codec_read_u32_be(buf, len, offset, &d_lrbg) ||
        !rbc_codec_read_u16_be(buf, len, offset, &v_train) ||
        !rbc_codec_read_u8(buf, len, offset, &m_mode) ||
        !rbc_codec_read_u8(buf, len, offset, &m_level))
    {
        return false;
    }
 
    out_p1->nid_lrbg.nid_c = nid_c;
    out_p1->nid_lrbg.nid_bg = nid_bg;
    out_p1->nid_prvlrbg.nid_c = prv_nid_c;
    out_p1->nid_prvlrbg.nid_bg = prv_nid_bg;
    out_p1->d_lrbg = d_lrbg;
    out_p1->v_train = (rbc_speed_kmh_t)v_train;
    out_p1->m_mode = (rbc_m_mode_t)m_mode;
    out_p1->m_level = (rbc_m_level_t)m_level;
    return true;
}
 
/* ========================================================================= */
/* Packet encode                                                             */
/* ========================================================================= */
 
bool rbc_packet_write_15_ma(uint8_t *buf, size_t cap, size_t *offset, const rbc_packet_15_ma_t *ma)
{
    if (ma == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_15_LEVEL_2_3_MA_SSP) &&
           rbc_codec_write_u32_be(buf, cap, offset, ma->ma_seq) &&
           rbc_codec_write_u32_be(buf, cap, offset, ma->ma_length) &&
           rbc_codec_write_u16_be(buf, cap, offset, (uint16_t)ma->v_loa) &&
           rbc_codec_write_u32_be(buf, cap, offset, ma->t_loa_s);
}
 
bool rbc_packet_write_21_gradient_profile(uint8_t *buf, size_t cap, size_t *offset,
                                          const rbc_packet_21_gradient_profile_t *gradient)
{
    uint8_t i;
 
    if (gradient == NULL)
    {
        return false;
    }
    if (!rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_21_GRADIENT_PROFILE) ||
        !rbc_codec_write_u8(buf, cap, offset, gradient->segment_count))
    {
        return false;
    }
    for (i = 0U; i < gradient->segment_count; i++)
    {
        if (!rbc_codec_write_u32_be(buf, cap, offset, gradient->segments[i].d_gradient) ||
            !rbc_codec_write_u8(buf, cap, offset, (uint8_t)gradient->segments[i].g_a))
        {
            return false;
        }
    }
    return true;
}
 
bool rbc_packet_write_27_ssp(uint8_t *buf, size_t cap, size_t *offset, const rbc_packet_27_ssp_t *ssp)
{
    uint8_t i;
 
    if (ssp == NULL)
    {
        return false;
    }
    if (!rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_27_STATIC_SPEED_PROFILE) ||
        !rbc_codec_write_u8(buf, cap, offset, ssp->segment_count))
    {
        return false;
    }
    for (i = 0U; i < ssp->segment_count; i++)
    {
        if (!rbc_codec_write_u32_be(buf, cap, offset, ssp->segments[i].d_static) ||
            !rbc_codec_write_u8(buf, cap, offset, (uint8_t)ssp->segments[i].v_static))
        {
            return false;
        }
    }
    return true;
}
 
bool rbc_packet_write_5_linking(uint8_t *buf, size_t cap, size_t *offset, const rbc_packet_5_linking_t *linking)
{
    uint8_t i;
 
    if (linking == NULL)
    {
        return false;
    }
    if (!rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_5_LINKING_INFO) ||
        !rbc_codec_write_u8(buf, cap, offset, linking->link_count))
    {
        return false;
    }
    for (i = 0U; i < linking->link_count; i++)
    {
        if (!rbc_codec_write_u16_be(buf, cap, offset, linking->links[i].nid_bg) ||
            !rbc_codec_write_u32_be(buf, cap, offset, linking->links[i].d_link) ||
            !rbc_codec_write_u8(buf, cap, offset, (uint8_t)linking->links[i].q_linkdir))
        {
            return false;
        }
    }
    return true;
}
 
bool rbc_packet_write_3_national_values(uint8_t *buf, size_t cap, size_t *offset,
                                        const rbc_packet_3_national_values_t *nv)
{
    if (nv == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_3_NATIONAL_VALUES) &&
           rbc_codec_write_u16_be(buf, cap, offset, (uint16_t)nv->v_nvshunt) &&
           rbc_codec_write_u16_be(buf, cap, offset, (uint16_t)nv->v_nvstff) &&
           rbc_codec_write_u16_be(buf, cap, offset, (uint16_t)nv->v_nvunfit) &&
           rbc_codec_write_u16_be(buf, cap, offset, nv->t_nvcontact) &&
           rbc_codec_write_u16_be(buf, cap, offset, nv->d_nvroll);
}
 
bool rbc_packet_write_57_ma_request_params(uint8_t *buf, size_t cap, size_t *offset,
                                           const rbc_packet_57_ma_request_params_t *params)
{
    if (params == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_57_MA_REQUEST_PARAMS) &&
           rbc_codec_write_u32_be(buf, cap, offset, params->t_mar_s) &&
           rbc_codec_write_u32_be(buf, cap, offset, params->d_mar_m);
}
 
bool rbc_packet_write_58_pr_params(uint8_t *buf, size_t cap, size_t *offset,
                                   const rbc_packet_58_pr_params_t *params)
{
    if (params == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_58_POSITION_REPORT_PARAMS) &&
           rbc_codec_write_u32_be(buf, cap, offset, params->t_cycloc_s) &&
           rbc_codec_write_u32_be(buf, cap, offset, params->d_cycloc_m);
}
 
bool rbc_packet_write_65_tsr(uint8_t *buf, size_t cap, size_t *offset, const rbc_packet_65_tsr_t *tsr)
{
    if (tsr == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_65_TEMPORARY_SPEED_RESTRICT) &&
           rbc_codec_write_u16_be(buf, cap, offset, tsr->nid_tsr) &&
           rbc_codec_write_u32_be(buf, cap, offset, tsr->d_tsr) &&
           rbc_codec_write_u32_be(buf, cap, offset, tsr->l_tsr) &&
           rbc_codec_write_u16_be(buf, cap, offset, (uint16_t)tsr->v_tsr) &&
           rbc_codec_write_u8(buf, cap, offset, (uint8_t)tsr->q_front);
}
 
bool rbc_packet_write_66_tsr_revoke(uint8_t *buf, size_t cap, size_t *offset,
                                    const rbc_packet_66_tsr_revoke_t *revoke)
{
    if (revoke == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_66_TSR_REVOCATION) &&
           rbc_codec_write_u16_be(buf, cap, offset, revoke->nid_tsr);
}
 
bool rbc_packet_write_72_text_message(uint8_t *buf, size_t cap, size_t *offset,
                                      const rbc_packet_72_text_message_t *text)
{
    uint32_t text_len = 0U;
    uint32_t i;
 
    if (text == NULL)
    {
        return false;
    }
    while ((text_len < (uint32_t)RBC_MAX_TEXT_MESSAGE_LEN) && (text->text[text_len] != '\0'))
    {
        text_len++;
    }
    if (!rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_72_TEXT_MESSAGE) ||
        !rbc_codec_write_u8(buf, cap, offset, (uint8_t)(text->ack_required ? 1U : 0U)) ||
        !rbc_codec_write_u32_be(buf, cap, offset, text->duration_s) ||
        !rbc_codec_write_u8(buf, cap, offset, (uint8_t)text_len))
    {
        return false;
    }
    for (i = 0U; i < text_len; i++)
    {
        if (!rbc_codec_write_u8(buf, cap, offset, (uint8_t)text->text[i]))
        {
            return false;
        }
    }
    return true;
}
 
bool rbc_packet_write_131_rbc_transition(uint8_t *buf, size_t cap, size_t *offset,
                                         const rbc_packet_131_rbc_transition_t *transition)
{
    if (transition == NULL)
    {
        return false;
    }
    return rbc_codec_write_u8(buf, cap, offset, RBC_PACKET_131_RBC_TRANSITION_ORDER) &&
           rbc_codec_write_u32_be(buf, cap, offset, transition->d_rbctr) &&
           rbc_codec_write_u16_be(buf, cap, offset, transition->nid_c) &&
           rbc_codec_write_u16_be(buf, cap, offset, transition->nid_rbc) &&
           rbc_codec_write_u64_be(buf, cap, offset, transition->nid_radio);
}