DCU_APP/GD32E508_DCU_APP/comm/ethercat_pdo.cpp

/**
 * @file ethercat_pdo.cpp
 * @brief EtherCAT 过程数据（PDO）收发模块实现（1.5.55）。
 */
#include "comm/ethercat_pdo.h"
#include "comm/ethercat_can_bridge.h"
#include "device/canfd.h"
#include "sys/config.h"
#include "sys/cpu_load.h"
/* [DIFF-STATS] 临时仪表挂接（删除：去掉本块与 process() 内的 observe 块）。
 * 上一行的 sys/config.h 必须在此之前：宏未定义时 #if 会静默按 0 求值，
 * 开关就会被悄悄当成"关掉"。 */
#if CONFIG_ECAT_DIFF_STATS
#include "comm/ethercat_diff_stats.h"
#endif

#include <cstring>

/* alignas(4) 必须在类内声明与出类定义上同时标注：AC6(Clang) 否则报
 * "'alignas' must be specified on definition"。用途见 ethercat_pdo.h。 */
alignas(4) uint8_t EthercatPdo::s_out[EthercatPdo::kPdoOutBytes] = {0};
alignas(4) uint8_t EthercatPdo::s_in[EthercatPdo::kPdoInBytes]   = {0};
/* 输入 PDO 的工作缓冲：buildInput 填这里，填完整块发布到 s_in（见 process()）。
 * 读侧 writeInputs 只读 s_in，因此永远看不到"填到一半"的镜像。 */
alignas(4) uint8_t EthercatPdo::s_build[EthercatPdo::kPdoInBytes] = {0};
alignas(4) uint8_t EthercatPdo::s_escSh1[32] = {0};
alignas(4) uint8_t EthercatPdo::s_escSh2[ECAT_XRX_OFF_CH - 32U] = {0};
alignas(4) uint8_t EthercatPdo::s_escSh3[kEcatPdoInBytes - ECAT_XRX_OFF_CH] = {0};
uint8_t EthercatPdo::s_escFlush1 = 0U;
uint8_t EthercatPdo::s_escFlush2 = 0U;
uint8_t EthercatPdo::s_escFlush3 = 0U;
bool    EthercatPdo::s_escSh1Valid = false;
bool    EthercatPdo::s_escSh2Valid = false;
bool    EthercatPdo::s_escSh3Valid = false;

volatile uint32_t EthercatPdo::s_outputCycles = 0U;
volatile uint32_t EthercatPdo::s_inputCycles  = 0U;
volatile uint32_t EthercatPdo::s_reentryCount = 0U;

void EthercatPdo::readOutputs(const uint8_t *esc)
{
    if (esc == nullptr) {
        return;
    }
    memcpy(s_out, esc, kPdoOutBytes);
    ++s_outputCycles;
}

bool EthercatPdo::outputProbe(const uint8_t *head)
{
    return EthercatCanBridge::peekOutput(head);
}

void EthercatPdo::process()
{
    /* 重入防护（1.6.7）：SSC 的 ECAT_Application()→本函数 在**四个上下文**都会被
     * 调用 —— PDI_Isr / Sync0_Isr / Sync1_Isr / MainLoop（见 ssc/src/ecatappl.c
     * 388、499、527、641 行）。而 MainLoop 那条路只用 DISABLE_ESC_INT() 关了
     * EXTI1(ESC IRQ)，**挡不住 EXTI0(Sync0,preempt=0) 和 EXTI2(Sync1,preempt=1)**。
     *
     * 竞态：MainLoop 把 s_in 填到一半（如刚写完批量区第 1 条）时被 Sync0/Sync1
     * 抢占 → 内层重入本函数，memset 清空后重填整个输入 PDO，随后 PDO_InputMapping()
     * 把这一份发给 ESC；外层回来又按自己那轮的进度接着往下写。于是发出去的镜像里
     * 批量区相邻两条来自**不同轮次**，而两轮之间环形队列 tail 已推进 —— 第 2 条
     * 就带上了另一条帧的内容。实测特征与此吻合：批量区第 1 条几乎不坏(0.06%)、
     * 第 2 条 30.75% 坏且内容是一条完整 CONFIG 帧；段 1/段 3 干净，因为它们每轮
     * 内容相同，被覆盖也看不出来。
     *
     * 1.6.15 批量区改成定长覆盖窗口后，跨轮次混填不再产生"半新半旧"的假帧（每槽
     * 自带 seqlock，且槽位固定、语义本就是"最近 N 帧"），但本防护仍然保留：省掉
     * 一次无谓的整块重填，并让镜像保持同一轮次的一致性。
     *
     * 处理：已在填充中就直接返回。输入 PDO 是周期量，跳过一次下一拍即补，
     * 远好过发出一份跨轮次拼接的镜像。 */
    static volatile bool s_building = false;
    if (s_building) {
        ++s_reentryCount;
        return;
    }
    s_building = true;

    /* PDO ↔ CAN 转发：输出 PDO 的 seq 字节变化沿触发一次 CAN 发送，
     * 输入 PDO 回填桥状态 + 电机状态窗口（布局见 ethercat_can_bridge.h）。
     *
     * 顺序必须是先 processOutput 后 buildInput：buildInput 会把本周期刚执行的
     * seq 回显成 ack，主站因此能在下一周期立刻确认送达；反过来则 ack 恒滞后一拍。
     * processOutput 还负责取走主站请求的状态窗口下标，也必须在 buildInput 之前。
     *
     * 填的是**工作缓冲** s_build，填完整体发布到 s_in：只有重入门还不够 ——
     * 内层被门挡住后，它随后的 PDO_InputMapping() 仍会去读 s_in，若 buildInput
     * 直接写 s_in，读到的就是外层写到一半的半成品。填在别处、整块发布之后，
     * 读侧要么拿到上一份完整镜像、要么拿到这一份完整镜像，不存在中间态。 */
    EthercatCanBridge::processOutput(s_out);
    EthercatCanBridge::buildInput(s_build);
    fillDiagTail(s_build);
    memcpy(s_in, s_build, kPdoInBytes);

#if CONFIG_ECAT_DIFF_STATS
    /* [DIFF-STATS] 只测量：与上一份已发布镜像比对并累计到 1s 窗口，经 0x2000
     * sub13..23 上报。不改变上面的整块写行为，因此本版本固件的输出与 1.6.16
     * 逐字节一致。仪表自持上一份副本，这里不需要额外缓冲。 */
    EthercatDiffStats::observe(s_in);
#endif

    s_building = false;
}

/**
 * @brief 填充输入 PDO 段 5 设备诊断区（632..639，1.5.61）。
 * @param[out] in 全长输入过程数据缓冲（buildInput 刚清零+填完段 1~4）。
 * @note  负载率 1s 滚动一个窗口，seq 随窗口递增——主站只认"seq 在变"的负载值。
 *        每周期重写 6 字节比"值变化才写"的分支判断更便宜，也不引入隐藏状态。
 */
void EthercatPdo::fillDiagTail(uint8_t *in)
{
    const uint16_t load = CpuLoad::loadPermille();
    in[ECAT_DIAG_OFF_LOAD]     = (uint8_t)(load & 0xFFU);
    in[ECAT_DIAG_OFF_LOAD + 1] = (uint8_t)(load >> 8);

    /* CAN 取帧撕裂丢帧数（1.6.6）。只有 16 位，饱和不回绕 —— 主站看的是
     * "有没有涨、涨多快"，回绕会被误读成计数清零。 */
    const CanFd::Diag *cd = CanFd::diag();
    const uint32_t torn = (cd != nullptr) ? cd->rx_torn_count : 0U;
    const uint16_t torn16 = (torn > 0xFFFFU) ? 0xFFFFU : (uint16_t)torn;
    in[ECAT_DIAG_OFF_TORN]     = (uint8_t)(torn16 & 0xFFU);
    in[ECAT_DIAG_OFF_TORN + 1] = (uint8_t)(torn16 >> 8);

    const uint32_t seq = CpuLoad::windowSeq();
    const uint16_t seq16 = (uint16_t)seq;   /* 1.6.12：u32 拆成 u16，高 2 字节让给
                                               overfull 计数。主站只判"非 0 且在涨"，
                                               按 u32 读到 seq|overfull<<16 同样成立。 */
    in[ECAT_DIAG_OFF_SEQ]      = (uint8_t)(seq16 & 0xFFU);
    in[ECAT_DIAG_OFF_SEQ + 1]  = (uint8_t)(seq16 >> 8);

    /* CAN RX FIFO 满溢计数 RFO0（1.6.12 测量）：1.6.11 改"满则丢新"后拼接未消失，
     * 本计数与 reject@494 是否同涨，是"残余拼接是否仍源自溢出"的判决性观测。 */
    const uint32_t of = (cd != nullptr) ? cd->rx_fifo_overfull_count : 0U;
    const uint16_t of16 = (of > 0xFFFFU) ? 0xFFFFU : (uint16_t)of;
    in[ECAT_DIAG_OFF_OVERFULL]     = (uint8_t)(of16 & 0xFFU);
    in[ECAT_DIAG_OFF_OVERFULL + 1] = (uint8_t)(of16 >> 8);

    /* RFD0 释放握手超时计数（1.6.12，占用原 1.6.7 重入计数位 492——该计数引入
     * 至今恒 0，转为纯内部）。dequeue 握手没完成 = 窗口/读指针失同步前兆。 */
    const uint32_t rt = (cd != nullptr) ? cd->rx_release_timeout_count : 0U;
    const uint16_t rt16 = (rt > 0xFFFFU) ? 0xFFFFU : (uint16_t)rt;
    in[ECAT_DIAG_OFF_RELTIMEOUT]     = (uint8_t)(rt16 & 0xFFU);
    in[ECAT_DIAG_OFF_RELTIMEOUT + 1] = (uint8_t)(rt16 >> 8);

    /* 内容级拼接帧拦截次数（1.6.11）：MI/MP 校验挡不住的指针错位拼接由入队前
     * 结构自洽校验兜底，语义同上 —— 主站看"涨不涨"判断拼接是否仍在发生。 */
    const uint32_t rj = (cd != nullptr) ? cd->rx_content_reject_count : 0U;
    const uint16_t rj16 = (rj > 0xFFFFU) ? 0xFFFFU : (uint16_t)rj;
    in[ECAT_DIAG_OFF_REJECT]     = (uint8_t)(rj16 & 0xFFU);
    in[ECAT_DIAG_OFF_REJECT + 1] = (uint8_t)(rj16 >> 8);
}

void EthercatPdo::writeInputs(uint8_t *esc)
{
    if (esc == nullptr) {
        return;
    }
    memcpy(esc, s_in, kPdoInBytes);
    ++s_inputCycles;
}

bool EthercatPdo::sparseEscWritePlan(uint16_t *off0, uint16_t *len0,
                                     uint16_t *off1, uint16_t *len1)
{
    if (off0 == nullptr || len0 == nullptr || off1 == nullptr || len1 == nullptr) {
        return false;
    }
#if !CONFIG_ECAT_SPARSE_ESC_WRITE
    return false;
#else
    /* 仅 1.6.21 密排布局：count 有效且两段之间确有空洞才分段。
     * magic 不对 / count 越界 / count 撑满到与段3相接 → 整包，避免陈旧空洞被误读。 */
    if (s_in[ECAT_MOTW_OFF_MAGIC] != kEcatMotLayoutMagic) {
        return false;
    }
    const uint8_t count = s_in[ECAT_MOTW_OFF_COUNT];
    if (count > kEcatMotSlotsPerPdo) {
        return false;
    }
    const uint16_t headEnd =
        (uint16_t)(ECAT_MOTW_OFF_SLOTS + (uint16_t)count * kEcatMotSlotSize);
    if (headEnd >= ECAT_XRX_OFF_CH) {
        return false;   /* 9 电机满配：A+B 无缝，整包更省一次 EXMC 事务 */
    }
    *off0 = 0U;
    *len0 = headEnd;
    *off1 = ECAT_XRX_OFF_CH;
    *len1 = (uint16_t)(kPdoInBytes - ECAT_XRX_OFF_CH);
    return true;
#endif
}

namespace {
/** 段2 有效长度（稀疏：到 count*56 为止；否则整段）。 */
uint16_t escSeg2WriteLen(const uint8_t *in)
{
#if CONFIG_ECAT_SPARSE_ESC_WRITE
    if (in[ECAT_MOTW_OFF_MAGIC] == kEcatMotLayoutMagic) {
        const uint8_t count = in[ECAT_MOTW_OFF_COUNT];
        if (count <= kEcatMotSlotsPerPdo) {
            const uint16_t headEnd =
                (uint16_t)(ECAT_MOTW_OFF_SLOTS + (uint16_t)count * kEcatMotSlotSize);
            if (headEnd < ECAT_XRX_OFF_CH) {
                return headEnd;
            }
        }
    }
#else
    (void)in;
#endif
    return ECAT_XRX_OFF_CH;
}

/** 判定是否需要写某区：无影子 / 冲刷中 / 内容变化。变化时刷新影子并重置冲刷=2。 */
bool diffNeedWrite(const uint8_t *cur, uint16_t off, uint16_t cmpLen,
                   uint8_t *shadow, bool &valid, uint8_t &flush)
{
    const bool changed = !valid || memcmp(cur + off, shadow, cmpLen) != 0;
    if (changed) {
        memcpy(shadow, cur + off, cmpLen);
        valid = true;
        flush = 2U; /* 本拍 + 再 2 拍，冲完 3-buffer */
        return true;
    }
    if (flush > 0U) {
        --flush;
        return true;
    }
    return false;
}
} /* anonymous namespace */

uint8_t EthercatPdo::escWritePlan(uint16_t *offs, uint16_t *lens)
{
    if (offs == nullptr || lens == nullptr) {
        return 0U;
    }

#if !CONFIG_ECAT_DIFF_ESC_WRITE
    /* 回退：稀疏两段或整包 */
    uint8_t n = 0U;
    uint16_t o0, l0, o1, l1;
    if (sparseEscWritePlan(&o0, &l0, &o1, &l1)) {
        offs[n] = o0; lens[n] = l0; ++n;
        offs[n] = o1; lens[n] = l1; ++n;
    } else {
        offs[0] = 0U; lens[0] = kPdoInBytes; n = 1U;
    }
    return n;
#else
    /* 三区差分：段1 桥状态 / 段2 电机密排(稀疏写有效长) / 段3+5 快照诊断。
     * 段1 计数器几乎每拍变，段2 稳定时省最大头，段3+5 多数拍稳定。 */
    uint8_t n = 0U;
    if (diffNeedWrite(s_in, 0U, 32U, s_escSh1, s_escSh1Valid, s_escFlush1)) {
        offs[n] = 0U;
        lens[n] = 32U;
        ++n;
    }
    if (diffNeedWrite(s_in, 32U, (uint16_t)(ECAT_XRX_OFF_CH - 32U),
                      s_escSh2, s_escSh2Valid, s_escFlush2)) {
        offs[n] = 32U;
        /* 写长：稀疏时只写到 count*56，但影子比对的是完整 32..543（含清零空洞）。
         * 空洞不写入 ESC 无妨：主站只按 count 迭代。 */
        const uint16_t wlen = escSeg2WriteLen(s_in);
        lens[n] = (wlen > 32U) ? (uint16_t)(wlen - 32U) : 0U;
        if (lens[n] == 0U) {
            /* count=0：段2 头 8 字节仍要写（count/active/magic） */
            lens[n] = 8U;
        }
        ++n;
    }
    if (diffNeedWrite(s_in, ECAT_XRX_OFF_CH, (uint16_t)(kPdoInBytes - ECAT_XRX_OFF_CH),
                      s_escSh3, s_escSh3Valid, s_escFlush3)) {
        offs[n] = ECAT_XRX_OFF_CH;
        lens[n] = (uint16_t)(kPdoInBytes - ECAT_XRX_OFF_CH);
        ++n;
    }
    if (n == 0U) {
        offs[0] = (uint16_t)(kPdoInBytes - 2U);
        lens[0] = 2U;
        n = 1U;
    }
    return n;
#endif
}

void EthercatPdo::snapshotOutput(uint8_t *dst)
{
    if (dst == nullptr) {
        return;
    }
    memcpy(dst, s_out, kPdoOutBytes);
}

extern "C" {

/* 1.6.0:输出探针,SSC PDI_Isr 的 PDO_OutputMapping 每周期调用(C 链接)。
 * 返回 0/1;SSC 侧声明为 UINT8,小返回值零扩展,调用约定兼容。 */
uint8_t EcatPdo_OutputProbe(const uint8_t *head)
{
    return EthercatPdo::outputProbe(head) ? 1U : 0U;
}

void EcatPdo_ReadOutputs(const uint8_t *esc)
{
    EthercatPdo::readOutputs(esc);
}

void EcatPdo_Process(void)
{
    EthercatPdo::process();
}

void EcatPdo_WriteInputs(uint8_t *esc)
{
    EthercatPdo::writeInputs(esc);
}

/* 1.6.22 稀疏写计划：SSC PDO_InputMapping 据此决定 HW_EscWriteIsr 写几段。
 * 返回 1=两段有效；0=回退整包。 */
uint8_t EcatPdo_SparseEscPlan(uint16_t *off0, uint16_t *len0,
                              uint16_t *off1, uint16_t *len1)
{
    return EthercatPdo::sparseEscWritePlan(off0, len0, off1, len1) ? 1U : 0U;
}

/* 1.6.25 差分写计划：SSC PDO_InputMapping 逐段 HW_EscWriteIsr。 */
uint8_t EcatPdo_EscWritePlan(uint16_t *offs, uint16_t *lens, uint8_t maxSegs)
{
    if (offs == nullptr || lens == nullptr || maxSegs == 0U
        || maxSegs > EthercatPdo::kEscPlanMaxSegs) {
        return 0U;
    }
    uint16_t o[EthercatPdo::kEscPlanMaxSegs];
    uint16_t l[EthercatPdo::kEscPlanMaxSegs];
    const uint8_t n = EthercatPdo::escWritePlan(o, l);
    if (n == 0U || n > maxSegs) {
        return 0U;
    }
    for (uint8_t i = 0U; i < n; ++i) {
        offs[i] = o[i];
        lens[i] = l[i];
    }
    return n;
}

}  /* extern "C" */