STM32F4-Base/App/protocol/iec104/iec104_asdu.c

/*
 * 模块名称：IEC 60870-5-104 ASDU 编解码（应用层，纯逻辑）
 * 模块功能：实现 iec104_asdu.h——ASDU 头解析/组装、IOA 3B 小端、IEEE754
 *           短浮点小端、CP56Time2a ↔ epoch 毫秒（UTC 分解，Hinnant civil
 *           日历算法，不依赖 time.h → ARMCC/MSVC 双端可移植）。
 * 线程约定：全部纯函数，无状态 → 任意上下文可调。
 * 创建日期：2026-10-12（B4）
 */
#include "iec104_asdu.h"

#include <string.h>

/* COT 首字节：bit0..5 = 值，bit6 = PN，bit7 = T；RBT 复用 bit4（C_IC 语义） */
#define COT_VAL_MASK   0x3FU
#define COT_FLAG_MASK  0xC0U

/*
 * 函数功能：解析 ASDU 头（6B），信息体起点经 obj/obj_len 指出
 * 入口参数：buf/len = ASDU 首字节与总长；hdr = 输出
 * 返 回 值：0 成功 / -1 空指针或长度不足 6B
 */
int iec104_asdu_parse_hdr(const uint8_t *buf, uint16_t len, iec104_asdu_hdr_t *hdr) {
    if ((buf == NULL) || (hdr == NULL) || (len < 6u)) {
        return -1;
    }
    hdr->type    = buf[0];
    hdr->vsq     = buf[1];
    hdr->cot     = (uint8_t)(buf[2] & COT_VAL_MASK);
    hdr->cot_hi  = (uint8_t)(buf[2] & 0xC0u);
    hdr->oa      = buf[3];
    hdr->ca      = (uint16_t)(buf[4] | ((uint16_t)buf[5] << 8));
    hdr->obj     = &buf[6];
    hdr->obj_len = (uint16_t)(len - 6u);
    return 0;
}

/*
 * 函数功能：组装 ASDU 头（OA 恒 0，EVI 由调用方置 buf[3]；RBT 位透传）
 * 入口参数：cot = 6bit 原因值；cot_hi = 标志位（T/PN/RBT）；ca = 公共地址
 * 返 回 值：固定 6（头字节数）
 */
uint16_t iec104_asdu_put_hdr(uint8_t *buf, uint8_t type, uint8_t vsq,
                             uint8_t cot, uint8_t cot_hi, uint16_t ca) {
    buf[0] = type;
    buf[1] = vsq;
    buf[2] = (uint8_t)(((uint8_t)(cot & COT_VAL_MASK))
                       | (uint8_t)(cot_hi & COT_FLAG_MASK)
                       | (uint8_t)(cot_hi & 0x10u)); /* RBT 位透传 */
    buf[3] = 0u;                    /* OA = 0（EVI 由调用方按需置位） */
    buf[4] = (uint8_t)(ca & 0xFFu);
    buf[5] = (uint8_t)(ca >> 8);
    return 6u;
}

/*
 * 函数功能：IOA 3 字节小端写入
 */
void iec104_put_ioa(uint8_t *buf, uint32_t ioa) {
    buf[0] = (uint8_t)(ioa & 0xFFu);
    buf[1] = (uint8_t)((ioa >> 8) & 0xFFu);
    buf[2] = (uint8_t)((ioa >> 16) & 0xFFu);
}

/*
 * 函数功能：IOA 3 字节小端读取
 */
uint32_t iec104_get_ioa(const uint8_t *buf) {
    return (uint32_t)buf[0] | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2] << 16);
}

/*
 * 函数功能：IEEE754 短浮点 → 小端字节序（104 恒小端，宿主字节序无关）
 */
void iec104_put_float(uint8_t *buf, float v) {
    /* 位模式小端：memcpy 不假设宿主字节序，逐字节拆给目标（104 恒小端） */
    uint8_t tmp[sizeof(float)];
    uint32_t i;

    (void)memcpy(tmp, &v, sizeof(tmp));
    for (i = 0u; i < sizeof(tmp); i++) {
#if defined(__BYTE_ORDER__) && (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
        buf[i] = tmp[sizeof(tmp) - 1u - i];
#else
        buf[i] = tmp[i];
#endif
    }
}

/*
 * 函数功能：小端字节序 → IEEE754 短浮点
 */
float iec104_get_float(const uint8_t *buf) {
    uint8_t tmp[sizeof(float)];
    uint32_t i;
    float v;

    for (i = 0u; i < sizeof(tmp); i++) {
#if defined(__BYTE_ORDER__) && (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
        tmp[i] = buf[sizeof(tmp) - 1u - i];
#else
        tmp[i] = buf[i];
#endif
    }
    (void)memcpy(&v, tmp, sizeof(v));
    return v;
}

/* ---------------- CP56Time2a ↔ epoch ---------------- */

/* 2000-01-01 00:00:00 UTC 的 epoch 毫秒（钳位下限） */
#define EPOCH_2000_MS   946684800000ULL

/*
 * 函数功能：公历（y>=1）→ 距 1970-01-01 天数（Hinnant civil_from_days 正向）
 * 说明：纯整数算法；m 取 1..12。
 */
static uint32_t s_days_from_civil(uint32_t y, uint32_t m, uint32_t d) {
    uint32_t yy  = y - ((m <= 2u) ? 1u : 0u);
    uint32_t era = yy / 400u;
    uint32_t yoe = yy - (era * 400u);
    uint32_t mp  = (m > 2u) ? (m - 3u) : (m + 9u);   /* 3 月 = 月序 0 */
    uint32_t doy = ((153u * mp) + 2u) / 5u + d - 1u;
    uint32_t doe = (yoe * 365u) + (yoe / 4u) - (yoe / 100u) + doy;

    return (era * 146097u) + doe - 719468u;
}

/*
 * 函数功能：距 1970-01-01 天数 → 公历（Hinnant 反向）
 * 入口参数：z_in = 天数；y/m/d = 输出
 */
static void s_civil_from_days(uint32_t z_in, uint32_t *y, uint32_t *m, uint32_t *d) {
    uint32_t z    = z_in + 719468u;
    uint32_t era  = z / 146097u;
    uint32_t doe  = z - (era * 146097u);
    uint32_t yoe  = (doe - (doe / 1460u) + (doe / 36524u) - (doe / 146096u)) / 365u;
    uint32_t yy   = yoe + (era * 400u);
    uint32_t doy  = doe - ((365u * yoe) + (yoe / 4u) - (yoe / 100u));
    uint32_t mp   = ((5u * doy) + 2u) / 153u;
    uint32_t mon  = mp + ((mp < 10u) ? 3u : 0u);

    *d = doy - (((153u * mp) + 2u) / 5u) + 1u;
    if (mp >= 10u) {
        mon = mp - 9u;
    }
    *m = mon;
    *y = yy + ((mon <= 2u) ? 1u : 0u);
}

/*
 * 函数功能：CP56Time2a → epoch 毫秒（UTC 分解）
 * 入口参数：buf = 7B；epoch_ms = 输出
 * 返 回 值：0 成功 / -2 IV（时间无效，调用方应拒绝）/ -1 字段越界
 */
int iec104_cp56_to_epoch_ms(const uint8_t *buf, uint64_t *epoch_ms) {
    uint32_t ms_min, minute, hour, day, mon, year;

    if ((buf == NULL) || (epoch_ms == NULL)) {
        return -1;
    }
    if ((buf[2] & 0x80u) != 0u) {
        return -2;                  /* IV：时间无效，调用方应拒绝 */
    }
    ms_min = (uint32_t)buf[0] | ((uint32_t)buf[1] << 8);   /* 0..59999 */
    minute = (uint32_t)buf[2] & 0x3Fu;
    hour   = (uint32_t)buf[3] & 0x1Fu;
    day    = (uint32_t)buf[4] & 0x1Fu;
    mon    = (uint32_t)buf[5] & 0x0Fu;
    year   = 2000u + ((uint32_t)buf[6] & 0x7Fu);
    if ((day == 0u) || (day > 31u) || (mon == 0u) || (mon > 12u)
        || (hour > 23u) || (minute > 59u) || (ms_min > 59999u)) {
        return -1;
    }
    *epoch_ms = ((uint64_t)s_days_from_civil(year, mon, day) * 86400000ULL)
                + ((uint64_t)hour * 3600000ULL)
                + ((uint64_t)minute * 60000ULL)
                + (uint64_t)ms_min;
    return 0;
}

/*
 * 函数功能：epoch 毫秒 → CP56Time2a（钳位 [2000-01-01, 2127-12-31]，IV 恒 0）
 * 返 回 值：固定 7（字节数）
 */
uint16_t iec104_epoch_ms_to_cp56(uint64_t epoch_ms, uint8_t *buf) {
    uint64_t days, rem, clamped;
    uint32_t y, m, d, ms_min;

    clamped = (epoch_ms < EPOCH_2000_MS) ? EPOCH_2000_MS : epoch_ms;
    days = clamped / 86400000ULL;
    rem  = clamped % 86400000ULL;
    s_civil_from_days((uint32_t)days, &y, &m, &d);
    if (y > 2127u) {                /* 年字段 7bit 上限（2000+127） */
        y = 2127u;
        m = 12u;
        d = 31u;
        rem = 86399999ULL;
    }
    ms_min = (uint32_t)(rem % 60000ULL);
    buf[0] = (uint8_t)(ms_min & 0xFFu);
    buf[1] = (uint8_t)(ms_min >> 8);
    buf[2] = (uint8_t)((rem / 60000ULL) % 60ULL);          /* min，IV=0 */
    buf[3] = (uint8_t)((rem / 3600000ULL) % 24ULL);        /* hour */
    buf[4] = (uint8_t)(d & 0x1Fu);                         /* day */
    buf[5] = (uint8_t)(m & 0x0Fu);                         /* month */
    buf[6] = (uint8_t)((y - 2000u) & 0x7Fu);               /* year */
    return 7u;
}