DCU_APP/GD32E508_DCU_APP/comm/ethercat_can_bridge.cpp

/**
 * @file ethercat_can_bridge.cpp
 * @brief EtherCAT PDO ↔ CAN 转发桥实现。
 */
#include "comm/ethercat_can_bridge.h"
#include "device/canfd.h"
#include "device/motor_state.h"
#include "sys/motor_ota_relay.h"
#include "sys/systick.h"

#include "RTE_Components.h"
#include CMSIS_device_header   /* __DMB()：seqlock 的编译器/总线屏障 */

#include <cstring>

volatile uint8_t  EthercatCanBridge::s_lastSeq      = 0U;
volatile bool     EthercatCanBridge::s_seqPrimed    = false;
volatile bool     EthercatCanBridge::s_coldStartPending = false;
volatile uint8_t  EthercatCanBridge::s_lastTxResult = 0U;
volatile uint16_t EthercatCanBridge::s_txCount      = 0U;
volatile uint16_t EthercatCanBridge::s_dropCount    = 0U;
volatile uint32_t EthercatCanBridge::s_lastTxId     = 0U;
volatile uint16_t EthercatCanBridge::s_rx00ffCount  = 0U;
volatile uint8_t  EthercatCanBridge::s_outSeenSeq   = 0U;

volatile bool     EthercatCanBridge::s_rxValid      = false;
volatile bool     EthercatCanBridge::s_rxPublished  = false;
volatile uint32_t EthercatCanBridge::s_xrxSeq       = 0U;
volatile uint8_t  EthercatCanBridge::s_rxCh         = 0U;
volatile uint8_t  EthercatCanBridge::s_rxDlc        = 0U;
volatile bool     EthercatCanBridge::s_rxTruncated  = false;
volatile uint32_t EthercatCanBridge::s_rxId         = 0U;
volatile uint32_t EthercatCanBridge::s_rxCount      = 0U;
uint8_t           EthercatCanBridge::s_rxData[kEcatCanXRxDataMax] = {0};

volatile bool     EthercatCanBridge::s_verValid        = false;
volatile uint32_t EthercatCanBridge::s_verHoldUntilMs  = 0U;
uint8_t           EthercatCanBridge::s_verCh           = 0U;
uint8_t           EthercatCanBridge::s_verDlc          = 0U;
uint32_t          EthercatCanBridge::s_verId           = 0U;
uint8_t           EthercatCanBridge::s_verData[kEcatCanXRxDataMax] = {0};

namespace {
/** 版本应答在 PDO 段3 的保持时长（ms）。主站 1ms 周期下足够被采到。 */
constexpr uint32_t kVerHoldMs = 300U;

inline uint16_t canSubId(uint32_t id) { return static_cast<uint16_t>(id & 0xFFFFU); }
}  // namespace

uint8_t           EthercatCanBridge::s_motShadow[kEcatMotRegionBytes] = {0};
uint32_t          EthercatCanBridge::s_motPubSeq[kMotorMaxNodes + 1U] = {0};
uint8_t           EthercatCanBridge::s_motPubOnline[kMotorMaxNodes + 1U] = {0};
uint16_t          EthercatCanBridge::s_motPubMask   = 0U;
uint8_t           EthercatCanBridge::s_motPubCount  = 0U;
bool              EthercatCanBridge::s_motShadowValid = false;

namespace {
/** 小端读写：PDO 字节序按 EtherCAT 惯例固定小端。 */
inline uint32_t rd32le(const uint8_t *p)
{
    return (uint32_t)p[0] | ((uint32_t)p[1] << 8) |
           ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}
inline void wr32le(uint8_t *p, uint32_t v)
{
    p[0] = (uint8_t)(v);         p[1] = (uint8_t)(v >> 8);
    p[2] = (uint8_t)(v >> 16);   p[3] = (uint8_t)(v >> 24);
}
inline void wr16le(uint8_t *p, uint16_t v)
{
    p[0] = (uint8_t)(v);  p[1] = (uint8_t)(v >> 8);
}

/** 段 2 头 + count 槽在影子区中的总长度。 */
inline uint16_t motRegionLen(uint8_t count)
{
    return (uint16_t)(kEcatMotHdrSize + (uint16_t)count * kEcatMotSlotSize);
}
}  // namespace

uint8_t EthercatCanBridge::sendBatch(const uint8_t *out)
{
    uint8_t want = out[ECAT_TXB_OFF_COUNT];
    if (want == 0U) {
        return 0U;
    }
    if (want > kEcatTxBatchMax) {
        want = kEcatTxBatchMax;
    }
    const bool stopOnError = (out[ECAT_TXB_OFF_FLAGS] & 0x01U) != 0U;

    uint8_t  sent = 0U;
    uint16_t off  = ECAT_TXB_OFF_ITEMS;

    for (uint8_t i = 0U; i < want; ++i) {
        if ((uint16_t)(off + ECAT_FRM_HDR_SIZE) > kEcatPdoOutBytes) {
            break;
        }
        const uint8_t *f = &out[off];
        uint8_t dlc = f[ECAT_FRM_OFF_DLC];
        if (dlc > kEcatCanFdDataMax) {
            dlc = kEcatCanFdDataMax;
        }
        if ((uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc) > kEcatPdoOutBytes) {
            break;
        }

        const uint8_t ch = f[ECAT_FRM_OFF_CH];
        CanFd::CanTxResult r;
        if (!CanFd::isChannelEnabled(ch)) {
            r = CanFd::CanTxResult::kErrInvalid;
        } else if (MotorOtaRelay::isChannelLocked(ch)) {
            /* 该通道正被电机 OTA 独占：PDO 运动帧一律不投递。
             * OTA 用单发模式(ARD=1)，一旦挤进同一个邮箱就会 abort 掉在途的
             * 0x101 固件包，或让电机 BOOT 收到非预期长度直接回 0x05/0x06。
             * 计数与 lastTxResult 走 0xE7，主站侧能看到「被 OTA 挡了」而不是「邮箱满」。 */
            r = CanFd::CanTxResult::kErrOtaLocked;
        } else {
            r = CanFd::enqueueTx(ch, rd32le(&f[ECAT_FRM_OFF_ID]),
                                 &f[ECAT_FRM_HDR_SIZE], dlc);
        }

        if (r == CanFd::CanTxResult::kOk) {
            ++s_txCount;
            ++sent;
            s_lastTxId = rd32le(&f[ECAT_FRM_OFF_ID]);
        } else {
            s_lastTxResult = (uint8_t)r;
            if (stopOnError) {
                break;
            }
        }
        off = (uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc);
    }
    return sent;
}

/**
 * 段 2：电机整轮密排。
 *
 * 1. 扫 MotorState，每个节点号 1..9 在 3 个通道里取 seq 最新的一条；
 * 2. 若节点集合与各节点 seq 均与上一拍发布相同 → 整段复用影子区（跳过重读）；
 * 3. 否则按 node_id 升序密排写入，空节点不占位；seqlock 奇数（写入中）的槽
 *    本拍省略，下拍自愈 —— 不发半新半旧的 48B。
 */
uint8_t EthercatCanBridge::fillMotorRound(uint8_t *in)
{
    const MotorStateTable &mt = MotorState::table();

    /* ── 1. 每节点选一条最新槽 ── */
    const MotorSlot *picks[kMotorMaxNodes];
    uint8_t nodes[kMotorMaxNodes];
    uint8_t n = 0U;

    for (uint8_t node = 1U; node <= kMotorMaxNodes; ++node) {
        const MotorSlot *best = nullptr;
        for (uint8_t ch = 0U; ch < kMotorChannels; ++ch) {
            const MotorSlot &s =
                mt.slots[(uint8_t)((node - 1U) * kMotorChannels + ch)];
            if (s.node_id == 0U) {
                continue;
            }
            if (best == nullptr || s.seq >= best->seq) {
                best = &s;
            }
        }
        if (best != nullptr) {
            picks[n] = best;
            nodes[n] = node;
            ++n;
        }
    }

    /* ── 2. 判「上一轮已填充且未变」──
     * 节点集合一致 + 每节点 seq/online 一致 + 头字段（active/boot）一致 + 写侧
     * seqlock 偶数 → 复用影子。online 必须比：checkOffline 超时置 0 时不 bump
     * seq，只比 seq 会把「已离线」永远报成在线。 */
    bool unchanged = s_motShadowValid && (n == s_motPubCount)
                     && mt.active_count == s_motShadow[ECAT_MOTW_OFF_ACTIVE - ECAT_MOTW_OFF_COUNT]
                     && mt.boot_active   == s_motShadow[ECAT_MOTW_OFF_BOOT   - ECAT_MOTW_OFF_COUNT]
                     && s_motShadow[ECAT_MOTW_OFF_MAGIC - ECAT_MOTW_OFF_COUNT]
                            == kEcatMotLayoutMagic;
    if (unchanged) {
        for (uint8_t i = 0U; i < n; ++i) {
            const uint8_t node = nodes[i];
            const MotorSlot &s = *picks[i];
            if ((s.wr_seq & 1U) != 0U
                || s.seq != s_motPubSeq[node]
                || s.online != s_motPubOnline[node]
                || (s_motPubMask & (uint16_t)(1U << node)) == 0U) {
                unchanged = false;
                break;
            }
        }
    }

    const uint16_t region = motRegionLen(unchanged ? s_motPubCount : n);

    /* 集合+seq+online 未变：整段复用影子，避免 8kHz 下每拍重建密排区。
     * 头字段 ACTIVE/BOOT/MAGIC/GSEQ 在影子起点 COUNT 之后，一并复用。 */
    if (unchanged && s_motShadowValid && s_motPubCount > 0U) {
        memcpy(&in[ECAT_MOTW_OFF_COUNT], s_motShadow, region);
        return s_motPubCount;
    }

    /* ── 3. 重建密排区 ── */
    in[ECAT_MOTW_OFF_ACTIVE] = mt.active_count;
    in[ECAT_MOTW_OFF_BOOT]   = mt.boot_active;
    in[ECAT_MOTW_OFF_MAGIC]  = kEcatMotLayoutMagic;
    wr32le(&in[ECAT_MOTW_OFF_GLOBAL_SEQ], mt.global_seq);

    uint8_t  w = 0U;
    uint16_t newMask = 0U;
    for (uint8_t i = 0U; i < n; ++i) {
        const MotorSlot &s = *picks[i];
        if ((s.wr_seq & 1U) != 0U) {
            continue;   /* 写入中：本拍省略，下拍自愈 */
        }
        __DMB();

        uint8_t *p = &in[ECAT_MOTW_OFF_SLOTS + (uint16_t)w * kEcatMotSlotSize];
        p[ECAT_MOT_OFF_NODE]   = nodes[i];
        p[ECAT_MOT_OFF_CH]     = s.channel;
        p[ECAT_MOT_OFF_ONLINE] = s.online;
        p[ECAT_MOT_OFF_DLC]    = s.dlc;
        wr32le(&p[ECAT_MOT_OFF_SEQ], s.seq);
        memcpy(&p[ECAT_MOT_OFF_STATUS], s.data, kMotorStatusDlc);

        s_motPubSeq[nodes[i]]    = s.seq;
        s_motPubOnline[nodes[i]] = s.online;
        newMask |= (uint16_t)(1U << nodes[i]);
        ++w;
    }

    in[ECAT_MOTW_OFF_COUNT] = w;

    /* 更新影子：下次集合+seq 未变时整段复用 */
    memcpy(s_motShadow, &in[ECAT_MOTW_OFF_COUNT], motRegionLen(w));
    s_motPubMask   = newMask;
    s_motPubCount  = w;
    s_motShadowValid = true;
    return w;
}

bool EthercatCanBridge::peekOutput(const uint8_t *head)
{
    /* 输出探针：12B 头里 SEQ@0 / FLAGS。seq 无变化沿且无清统计请求时返回 false，
     * PDO_OutputMapping 只补读 SM2 末字，跳过 384B 全量读。
     * 恒 true 会让 8kHz 每拍都全量读，单拍被 PRAM 打爆（6kHz 即掉速）。 */
    if (head == nullptr) {
        return false;
    }
    const uint8_t seq = head[ECAT_CAN_TX_OFF_SEQ];

    if (!s_seqPrimed) {
        s_seqPrimed        = true;
        s_lastSeq          = seq;
        s_coldStartPending = true;
        return true; /* 冷启动首包强制全量读，避免漏掉首帧命令 */
    }

    if (seq != s_lastSeq) {
        return true;
    }
    return (head[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U;
}

void EthercatCanBridge::resetSequenceState()
{
    s_seqPrimed        = false;
    s_lastSeq          = 0U;
    s_coldStartPending = false;
}

void EthercatCanBridge::processOutput(const uint8_t *out)
{
    if (out == nullptr) {
        return;
    }

    const uint8_t seq = out[ECAT_CAN_TX_OFF_SEQ];
    s_outSeenSeq = seq;   /* 诊断：processOutput 本拍看到的输出 seq */

    if (!s_seqPrimed) {
        s_seqPrimed        = true;
        s_coldStartPending = true;
    }

    /* 无论 seq 是否变化，优先处理非 seq 驱动的命令（FLAGS bit0 清统计） */
    if ((out[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U) {
        s_txCount     = 0U;
        s_dropCount   = 0U;
        s_rxCount     = 0U;
        s_rx00ffCount = 0U;
        s_rxPublished = true;
    }

    const uint8_t dlcRaw = out[ECAT_CAN_TX_OFF_DLC];
    const uint8_t batchCount = out[ECAT_TXB_OFF_COUNT];
    const bool hasPayload = (dlcRaw > 0U) || (batchCount > 0U);

    bool executeCommand = false;
    if (s_coldStartPending) {
        s_coldStartPending = false;
        /* 冷启动：有载荷就发（不依赖 s_lastSeq 是否已被探针碰过） */
        if (hasPayload) {
            executeCommand = true;
        } else {
            s_lastSeq = seq;
            return;
        }
    } else if (seq != s_lastSeq) {
        executeCommand = true;
    } else if (hasPayload && seq != s_lastSeq) {
        executeCommand = true;
    }

    if (!executeCommand) {
        return;
    }
    s_lastSeq = seq;

    const uint8_t ch  = out[ECAT_CAN_TX_OFF_CH];
    uint8_t       dlc = dlcRaw;
    if (dlc > kEcatCanTxDataMax) {
        dlc = kEcatCanTxDataMax;
    }

    if (dlc > 0U) {
        if (!CanFd::isChannelEnabled(ch)) {
            s_lastTxResult = (uint8_t)CanFd::CanTxResult::kErrInvalid;
        } else if (MotorOtaRelay::isChannelLocked(ch)) {
            /* 同 sendBatch：OTA 独占通道，周期运动帧不投邮箱（0xE7 上报主站）。 */
            s_lastTxResult = (uint8_t)CanFd::CanTxResult::kErrOtaLocked;
        } else {
            const CanFd::CanTxResult r =
                CanFd::enqueueTx(ch, rd32le(&out[ECAT_CAN_TX_OFF_ID]),
                                 &out[ECAT_CAN_TX_OFF_DATA], dlc);
            s_lastTxResult = (uint8_t)r;
            if (r == CanFd::CanTxResult::kOk) {
                ++s_txCount;
                s_lastTxId = rd32le(&out[ECAT_CAN_TX_OFF_ID]);
            }
        }
    }

    (void)sendBatch(out);
}

void EthercatCanBridge::buildInput(uint8_t *in)
{
    if (in == nullptr) {
        return;
    }
    /* 不整包 memset(600)：8kHz 下每拍 Process 都会走到这里，600B 清零
     * 与后续覆盖写重复。各段自己写满有效域；未用槽由 fillMotorRound
     * 按 count 截断，主站只迭代 count，空洞区可为残值。 */

    /* ── 段 1（0..31）：桥状态 + 短快照 ── */
    in[ECAT_CAN_RX_OFF_ACK]   = s_lastSeq;
    in[ECAT_CAN_RX_OFF_TXRES] = s_lastTxResult;
    wr16le(&in[ECAT_CAN_RX_OFF_TXCNT], s_txCount);
    wr16le(&in[ECAT_CAN_RX_OFF_DROP],  s_dropCount);
    wr32le(&in[ECAT_CAN_RX_OFF_RXCNT], s_rxCount);
    wr32le(&in[ECAT_CAN_RX_OFF_TXTXID], s_lastTxId);
    wr16le(&in[ECAT_CAN_RX_OFF_RX00FF], s_rx00ffCount);

    const bool xrxStable = ((s_xrxSeq & 1U) == 0U);

    if (xrxStable && s_rxValid) {
        uint8_t flags = 0x01U;
        if (s_rxTruncated) {
            flags |= 0x02U;
        }
        in[ECAT_CAN_RX_OFF_CH]    = s_rxCh;
        in[ECAT_CAN_RX_OFF_DLC]   = s_rxDlc;
        in[ECAT_CAN_RX_OFF_FLAGS] = flags;
        wr32le(&in[ECAT_CAN_RX_OFF_ID], s_rxId);
        memcpy(&in[ECAT_CAN_RX_OFF_DATA], s_rxData, kEcatCanRxDataMax);
        s_rxPublished = true;
    } else {
        in[ECAT_CAN_RX_OFF_FLAGS] = 0U;
        in[ECAT_CAN_RX_OFF_CH]    = 0U;
        in[ECAT_CAN_RX_OFF_DLC]   = 0U;
    }

    const uint8_t ch = s_rxValid ? s_rxCh : 0U;
    const uint32_t backlog = CanFd::isChannelEnabled(ch) ? CanFd::txQueueBacklog(ch) : 0U;
    in[ECAT_CAN_RX_OFF_BACKLOG] = (backlog > 255U) ? 255U : (uint8_t)backlog;

    /* ── 段 2（32..543）：电机整轮密排 ── */
    (void)fillMotorRound(in);

    /* ── 段 3（544..583）：长帧快照（通用，不专宠版本） ──
     * 语义 =「最近一帧非 STATUS 长帧」：CONFIG/版本/节点ID 谁后到谁上。
     * STATUS(0x0100) 由段2 密排承载，不进本槽（否则 48B STATUS 会淹没一切）。
     * 例外：仅当刚收到 0x00FF 的短粘滞窗口内，优先摆版本应答，
     * 给主站一个稳定采样窗；窗口结束后立刻交还给 CONFIG 等，避免其它长帧失效。 */
    if (xrxStable) {
        const uint32_t nowMs = SysTickTimer::getMs();
        const bool stickyLive = s_verValid && static_cast<int32_t>(nowMs - s_verHoldUntilMs) < 0;
        /* 段3 RSVD@547：processOutput 看到的输出 seq（诊断用，不参与解析） */
        in[ECAT_XRX_OFF_RSVD] = s_outSeenSeq;
        if (stickyLive) {
            in[ECAT_XRX_OFF_CH]    = s_verCh;
            in[ECAT_XRX_OFF_DLC]   = s_verDlc;
            in[ECAT_XRX_OFF_FLAGS] = 0x01U;
            wr32le(&in[ECAT_XRX_OFF_ID], s_verId);
            memcpy(&in[ECAT_XRX_OFF_DATA], s_verData, kEcatCanXRxDataMax);
        } else if (s_rxValid) {
            in[ECAT_XRX_OFF_CH]    = s_rxCh;
            in[ECAT_XRX_OFF_DLC]   = s_rxDlc;
            in[ECAT_XRX_OFF_FLAGS] = 0x01U;
            wr32le(&in[ECAT_XRX_OFF_ID], s_rxId);
            memcpy(&in[ECAT_XRX_OFF_DATA], s_rxData, kEcatCanXRxDataMax);
        }
    }

    /* 段 5 由 EthercatPdo::fillDiagTail 写。 */
}

void EthercatCanBridge::onCanFrame(uint8_t channel, uint32_t id,
                                   const uint8_t *data, uint8_t length)
{
    if (data == nullptr) {
        return;
    }

    /* STATUS(0x0100) 由 MotorState 段2 密排承载，不写入 xrx。
     * 原注释本就如此约定；实现曾漏过滤，导致高频 STATUS 盖掉 CONFIG/版本。 */
    if (canSubId(id) == 0x0100U) {
        return;
    }

    uint8_t copyLen = length;
    if (copyLen > kEcatCanXRxDataMax) {
        copyLen = kEcatCanXRxDataMax;
    }

    /* ── 快照 seqlock：奇数=正在写，消费者本拍跳过段 1/段 3 ── */
    ++s_xrxSeq;
    __DMB();

    if (s_rxValid && !s_rxPublished) {
        ++s_dropCount;
    }

    memcpy(s_rxData, data, copyLen);
    if (copyLen < kEcatCanXRxDataMax) {
        memset(&s_rxData[copyLen], 0, (size_t)(kEcatCanXRxDataMax - copyLen));
    }

    s_rxCh        = channel;
    s_rxId        = id;
    s_rxDlc       = length;
    s_rxTruncated = (length > kEcatCanRxDataMax);
    ++s_rxCount;
    s_rxValid     = true;
    s_rxPublished = false;

    if (canSubId(id) == 0x00FFU) {
        if (s_rx00ffCount < 0xFFFFU) {
            ++s_rx00ffCount;
        }
    }

    /* 版本应答：只做短粘滞，供主站在窗口内采到；不长期霸占 xrx */
    if (canSubId(id) == 0x00FFU) {
        s_verCh  = channel;
        s_verDlc = length;
        s_verId  = id;
        memcpy(s_verData, data, copyLen);
        if (copyLen < kEcatCanXRxDataMax) {
            memset(&s_verData[copyLen], 0, (size_t)(kEcatCanXRxDataMax - copyLen));
        }
        s_verHoldUntilMs = SysTickTimer::getMs() + kVerHoldMs;
        s_verValid       = true;
    }

    __DMB();
    ++s_xrxSeq;
}

extern "C" {

void EcatCan_ProcessOutput(const uint8_t *out)
{
    EthercatCanBridge::processOutput(out);
}

void EcatCan_BuildInput(uint8_t *in)
{
    EthercatCanBridge::buildInput(in);
}

void EcatCan_ResetSequenceState(void)
{
    EthercatCanBridge::resetSequenceState();
}

}  /* extern "C" */