swd_general_gpio.hpp

#pragma once
 
#include <cmath>
#include <cstdint>
 
#include "gpio.hpp"
#include "libxr_def.hpp"
#include "swd.hpp"
#include "timebase.hpp"
 
namespace LibXR::Debug
{
template <typename SwclkGpioType, typename SwdioGpioType>
class SwdGeneralGPIO final : public Swd
{
  static constexpr uint32_t MIN_HZ = 10'000u;
  static constexpr uint32_t MAX_HZ = 100'000'000u;
 
  static constexpr uint32_t NS_PER_SEC = 1'000'000'000u;
  static constexpr uint32_t LOOPS_SCALE = 1000u;           // ns -> us 的缩放分母
  static constexpr uint32_t CEIL_BIAS = LOOPS_SCALE - 1u;  // ceil(x/LOOPS_SCALE) 的偏置
 
  static constexpr uint32_t HalfPeriodNsFromHz(uint32_t hz)
  {
    // ceil(1e9 / (2*hz))
    return (NS_PER_SEC + (2u * hz) - 1u) / (2u * hz);
  }
 
  static constexpr uint32_t HALF_PERIOD_NS_MAX = HalfPeriodNsFromHz(MIN_HZ);
  static constexpr uint32_t MAX_LOOPS_PER_US =
      (UINT32_MAX - CEIL_BIAS) / HALF_PERIOD_NS_MAX;
 
  static_assert(MIN_HZ > 0u);
  static_assert(MAX_HZ >= MIN_HZ);
  static_assert(HALF_PERIOD_NS_MAX > 0u);
 
 public:
  explicit SwdGeneralGPIO(SwclkGpioType& swclk, SwdioGpioType& swdio,
                          uint32_t loops_per_us, uint32_t default_hz = DEFAULT_CLOCK_HZ)
      : swclk_(swclk), swdio_(swdio), loops_per_us_(loops_per_us)
  {
    if (loops_per_us_ > MAX_LOOPS_PER_US)
    {
      loops_per_us_ = MAX_LOOPS_PER_US;
    }
 
    // SWCLK baseline
    swclk_.SetConfig(
        {SwclkGpioType::Direction::OUTPUT_PUSH_PULL, SwclkGpioType::Pull::NONE});
    swclk_.Write(true);
 
    // SWDIO baseline
    (void)SetSwdioDriveMode();
    swdio_.Write(true);
 
    (void)SetClockHz(default_hz);
  }
 
  ~SwdGeneralGPIO() override = default;
 
  SwdGeneralGPIO(const SwdGeneralGPIO&) = delete;
  SwdGeneralGPIO& operator=(const SwdGeneralGPIO&) = delete;
 
  ErrorCode SetClockHz(uint32_t hz) override
  {
    if (hz == 0u)
    {
      clock_hz_ = 0u;
      half_period_ns_ = 0u;
      half_period_loops_ = 0u;
      return ErrorCode::OK;
    }
 
    if (hz < MIN_HZ)
    {
      hz = MIN_HZ;
    }
    if (hz > MAX_HZ)
    {
      hz = MAX_HZ;
    }
 
    clock_hz_ = hz;
 
    // 半周期计算改为浮点（double），最终再转为整型。Half period computed in double, then
    // converted to integer.
    const double DEN = 2.0 * static_cast<double>(hz);
    const double HALF_PERIOD_NS_F = std::ceil(static_cast<double>(NS_PER_SEC) / DEN);
    half_period_ns_ = static_cast<uint32_t>(HALF_PERIOD_NS_F);
 
    if (loops_per_us_ == 0u)
    {
      half_period_loops_ = 0u;
      return ErrorCode::OK;
    }
 
    // half_period_loops 使用浮点计算，最终再转为整型（ceil）。
    // 允许 < 1 时转换为 0，用于进入 no-delay 路径。
    // Compute loops in double, then convert to integer (ceil). Allow < 1 to become 0
    // to enter no-delay path.
    const double HALF_PERIOD_LOOPS_F =
        (static_cast<double>(loops_per_us_) * static_cast<double>(half_period_ns_)) /
        static_cast<double>(LOOPS_SCALE);
 
    if (HALF_PERIOD_LOOPS_F < 1.0)
    {
      half_period_loops_ = 0u;
    }
    else
    {
      const double LOOPS_CEIL_F = std::ceil(HALF_PERIOD_LOOPS_F);
      half_period_loops_ = (LOOPS_CEIL_F >= static_cast<double>(UINT32_MAX))
                               ? UINT32_MAX
                               : static_cast<uint32_t>(LOOPS_CEIL_F);
    }
 
    return ErrorCode::OK;
  }
 
  void Close() override
  {
    InvalidateSelectCache();
 
    // 安全状态：Safe state:
    // - SWCLK 高电平（历史行为）。SWCLK high (legacy).
    // - SWDIO 上拉输入（不驱动）。SWDIO input with pull-up (no drive).
    swclk_.Write(true);
    (void)SetSwdioSampleMode();
  }
 
  ErrorCode LineReset() override
  {
    InvalidateSelectCache();
    // SWD 线复位：SWDIO = 1 持续 >= 50 个周期；此处使用 64 个周期。SWD line reset: SWDIO
    // = 1 for >= 50 cycles; here use 64 cycles.
    (void)SetSwdioDriveMode();
    swdio_.Write(true);
    for (uint32_t i = 0; i < LINE_RESET_CYCLES; ++i)
    {
      GenOneClk();
    }
    return ErrorCode::OK;
  }
 
  ErrorCode EnterSwd() override
  {
    ErrorCode ec = LineReset();
    if (ec != ErrorCode::OK)
    {
      return ec;
    }
 
    WriteByteLSB(JTAG_TO_SWD_SEQ0);
    WriteByteLSB(JTAG_TO_SWD_SEQ1);
 
    ec = LineReset();
    if (ec != ErrorCode::OK)
    {
      return ec;
    }
 
    swdio_.Write(false);
    WriteByteLSB(0x00);
 
    return ErrorCode::OK;
  }
 
  ErrorCode Transfer(const SwdProtocol::Request& req,
                     SwdProtocol::Response& resp) override
  {
    // 目标：当 half_period_loops_ == 0 时，整次 Transfer 走无延时路径，
    // 避免每半周期 BusyLoop(0) 的空转判断开销。
    if (half_period_loops_ == 0u)
    {
      return TransferWithoutDelay(req, resp);
    }
    else
    {
      return TransferWithDelay(req, resp);
    }
  }
 
  void IdleClocks(uint32_t cycles) override
  {
    // CMSIS-DAP 空闲周期插入。CMSIS-DAP idle cycles insertion.
    // 保留历史序列：周期内驱动 SWDIO 高电平，结束后拉低。Keep the legacy sequence: drive
    // SWDIO high during cycles, then pull low.
    (void)SetSwdioDriveMode();
    swdio_.Write(true);
 
    for (uint32_t i = 0; i < cycles; ++i)
    {
      GenOneClk();
    }
 
    swdio_.Write(false);
  }
 
  ErrorCode SeqWriteBits(uint32_t cycles, const uint8_t* data_lsb_first) override
  {
    if (cycles == 0u)
    {
      swclk_.Write(false);
      return ErrorCode::OK;
    }
    if (data_lsb_first == nullptr)
    {
      return ErrorCode::ARG_ERR;
    }
 
    (void)SetSwdioDriveMode();
 
    // Keep legacy: start from SWCLK low and end low
    swclk_.Write(false);
 
    for (uint32_t i = 0; i < cycles; ++i)
    {
      const bool BIT = (((data_lsb_first[i / 8u] >> (i & 7u)) & 0x01u) != 0u);
      swdio_.Write(BIT);
 
      // one clock cycle: low->high (with DelayHalf inside GenOneClk)
      // NOTE: your GenOneClk ends at high; to keep "end low" legacy for SeqWrite,
      // we explicitly pull low after each cycle (no extra DelayHalf added).
      GenOneClk();
      swclk_.Write(false);
    }
 
    return ErrorCode::OK;
  }
 
  ErrorCode SeqReadBits(uint32_t cycles, uint8_t* out_lsb_first) override
  {
    if (cycles == 0u)
    {
      swclk_.Write(false);
      return ErrorCode::OK;
    }
    if (out_lsb_first == nullptr)
    {
      return ErrorCode::ARG_ERR;
    }
 
    const uint32_t BYTES = (cycles + 7u) / 8u;
    Memory::FastSet(out_lsb_first, 0, BYTES);
 
    (void)SetSwdioSampleMode();
 
    // start from low, end low
    swclk_.Write(false);
 
    for (uint32_t i = 0; i < cycles; ++i)
    {
      // Use the updated read phase (CMSIS-style)
      bool bit = false;
      if (half_period_loops_ == 0u)
      {
        swclk_.Write(false);
        bit = swdio_.Read();
        swclk_.Write(true);
      }
      else
      {
        swclk_.Write(false);
        DelayHalf();
        bit = swdio_.Read();
        swclk_.Write(true);
        DelayHalf();
      }
 
      if (bit)
      {
        out_lsb_first[i / 8u] =
            static_cast<uint8_t>(out_lsb_first[i / 8u] | (1u << (i & 7u)));
      }
 
      // keep legacy end-low for next bit
      swclk_.Write(false);
    }
 
    return ErrorCode::OK;
  }
 
 private:
  static inline uint8_t MakeReq(bool apndp, bool rnw, uint8_t addr2b)
  {
    const uint8_t A2 = addr2b & 0x1u;
    const uint8_t A3 = (addr2b >> 1) & 0x1u;
    const uint8_t PAR = static_cast<uint8_t>((apndp ^ rnw ^ A2 ^ A3) & 0x1u);
 
    // 请求字节位域：start(1), APnDP, RnW, A2, A3, PAR, stop(0), park(1)。Request bit
    // fields: start(1), APnDP, RnW, A2, A3, PAR, stop(0), park(1).
    return static_cast<uint8_t>((1u << 0) | (static_cast<uint8_t>(apndp) << 1) |
                                (static_cast<uint8_t>(rnw) << 2) |
                                (static_cast<uint8_t>(A2) << 3) |
                                (static_cast<uint8_t>(A3) << 4) |
                                (static_cast<uint8_t>(PAR) << 5) | (0u << 6) | (1u << 7));
  }
 
  static inline uint8_t Parity32(uint32_t x)
  {
    x ^= x >> 16;
    x ^= x >> 8;
    x ^= x >> 4;
    x &= 0xFu;
    static constexpr uint8_t LUT[16] = {
        0, 1, 1, 0, 1, 0, 0, 1,
        1, 0, 0, 1, 0, 1, 1, 0};  
    return LUT[x];
  }
 
  static inline SwdProtocol::Ack DecodeAck(uint8_t ack_bits)
  {
    switch (ack_bits)
    {
      case 0x1:
        return SwdProtocol::Ack::OK;
      case 0x2:
        return SwdProtocol::Ack::WAIT;
      case 0x4:
        return SwdProtocol::Ack::FAULT;
      case 0x0:
        return SwdProtocol::Ack::NO_ACK;
      default:
        return SwdProtocol::Ack::PROTOCOL;
    }
  }
 
 private:
  enum class SwdioMode : uint8_t
  {
    UNKNOWN = 0,  
    DRIVE_OD,     
    SAMPLE_IN,    
  };
 
  ErrorCode SetSwdioDriveMode()
  {
    if (swdio_mode_ == SwdioMode::UNKNOWN)
    {
      const ErrorCode EC = swdio_.SetConfig(
          {SwdioGpioType::Direction::OUTPUT_OPEN_DRAIN, SwdioGpioType::Pull::UP});
      if (EC != ErrorCode::OK)
      {
        return EC;
      }
    }
 
    swdio_mode_ = SwdioMode::DRIVE_OD;
    return ErrorCode::OK;
  }
 
  ErrorCode SetSwdioSampleMode()
  {
    const ErrorCode EC = SetSwdioDriveMode();
    if (EC != ErrorCode::OK)
    {
      return EC;
    }
 
    // 约束：GPIO::Read() 需要在开漏输出模式下返回实际引脚电平（而不是输出锁存值）。
    // Constraint: GPIO::Read() must sample the physical pin level in open-drain output
    // mode (not just the output latch).
    //
    // 开漏输出高电平表示释放总线，目标可驱动 ACK/数据 / Open-drain high releases line
    // so target can drive ACK/data.
    swdio_.Write(true);
    swdio_mode_ = SwdioMode::SAMPLE_IN;
    return ErrorCode::OK;
  }
 
  inline void DelayHalf() { BusyLoop(half_period_loops_); }
 
  inline void GenOneClk()
  {
    swclk_.Write(false);
    DelayHalf();
    swclk_.Write(true);
    DelayHalf();
  }
 
  inline void GenOneClkWithoutDelay()
  {
    swclk_.Write(false);
    swclk_.Write(true);
  }
 
  inline void WriteBit(bool bit)
  {
    swdio_.Write(bit);
    GenOneClk();
  }
 
  inline void WriteBitWithoutDelay(bool bit)
  {
    swdio_.Write(bit);
    GenOneClkWithoutDelay();
  }
 
  inline void WriteByteLSB(uint8_t b)
  {
    for (uint32_t i = 0; i < BYTE_BITS; ++i)
    {
      WriteBit(((b >> i) & 0x1u) != 0u);
    }
  }
 
  inline void WriteByteLSBWithoutDelay(uint8_t b)
  {
    for (uint32_t i = 0; i < BYTE_BITS; ++i)
    {
      WriteBitWithoutDelay(((b >> i) & 0x1u) != 0u);
    }
  }
 
  inline bool ReadBitAndClock()
  {
    swclk_.Write(false);
    DelayHalf();
    const bool BIT = swdio_.Read();
    swclk_.Write(true);
    DelayHalf();
    return BIT;
  }
 
  inline bool ReadBitAndClockWithoutDelay()
  {
    swclk_.Write(false);
    const bool BIT = swdio_.Read();
    swclk_.Write(true);
    return BIT;
  }
 
  inline uint8_t ReadByteLSB()
  {
    uint8_t v = 0u;
    for (uint32_t i = 0; i < BYTE_BITS; ++i)
    {
      if (ReadBitAndClock())
      {
        v = static_cast<uint8_t>(v | (1u << i));
      }
    }
    return v;
  }
 
  inline uint8_t ReadByteLSBWithoutDelay()
  {
    uint8_t v = 0u;
    for (uint32_t i = 0; i < BYTE_BITS; ++i)
    {
      if (ReadBitAndClockWithoutDelay())
      {
        v = static_cast<uint8_t>(v | (1u << i));
      }
    }
    return v;
  }
 
  static void BusyLoop(uint32_t loops)
  {
    volatile uint32_t sink = loops;
    while (sink--)
    {
    }
  }
 
 private:
  ErrorCode TransferWithDelay(const SwdProtocol::Request& req,
                              SwdProtocol::Response& resp)
  {
    resp.ack = SwdProtocol::Ack::PROTOCOL;
    resp.rdata = 0u;
    resp.parity_ok = true;
 
    const bool APNDP = (req.port == SwdProtocol::Port::AP);
    const uint8_t REQUEST_BYTE = MakeReq(APNDP, req.rnw, req.addr2b);
 
    (void)SetSwdioDriveMode();
    WriteByteLSB(REQUEST_BYTE);
 
    (void)SetSwdioSampleMode();
    GenOneClk();  // turnaround Host -> Target
 
    // ACK: CMSIS SW_READ_BIT phase (sample in low phase)
    uint8_t ack_raw = 0u;
    for (uint32_t i = 0; i < ACK_BITS; ++i)
    {
      if (ReadBitAndClock())
      {
        ack_raw |= static_cast<uint8_t>(1u << i);
      }
    }
    resp.ack = DecodeAck(static_cast<uint8_t>(ack_raw & 0x7u));
 
    if (resp.ack != SwdProtocol::Ack::OK)
    {
      GenOneClk();  // turnaround Target -> Host (skip data)
      (void)SetSwdioDriveMode();
      swdio_.Write(true);
      swclk_.Write(false);
      return ErrorCode::OK;
    }
 
    if (req.rnw)
    {
      uint32_t data = 0u;
      for (uint32_t byte = 0; byte < 4u; ++byte)
      {
        const uint32_t B = ReadByteLSB();
        data |= (B << (8u * byte));
      }
 
      const bool PARITY_BIT = ReadBitAndClock();
      resp.rdata = data;
      resp.parity_ok = (static_cast<uint8_t>(PARITY_BIT) == Parity32(data));
 
      (void)SetSwdioDriveMode();
      swdio_.Write(true);
      GenOneClk();
 
      swclk_.Write(false);
    }
    else
    {
      (void)SetSwdioDriveMode();
      GenOneClk();
 
      const uint32_t DATA = req.wdata;
      for (uint32_t byte = 0; byte < 4u; ++byte)
      {
        const uint8_t B = static_cast<uint8_t>((DATA >> (8u * byte)) & 0xFFu);
        WriteByteLSB(B);
      }
 
      const bool PARITY_BIT = (Parity32(DATA) & 0x1u) != 0u;
      WriteBit(PARITY_BIT);
 
      swdio_.Write(true);
      swclk_.Write(false);
    }
 
    return ErrorCode::OK;
  }
 
  ErrorCode TransferWithoutDelay(const SwdProtocol::Request& req,
                                 SwdProtocol::Response& resp)
  {
    resp.ack = SwdProtocol::Ack::PROTOCOL;
    resp.rdata = 0u;
    resp.parity_ok = true;
 
    const bool APNDP = (req.port == SwdProtocol::Port::AP);
    const uint8_t REQUEST_BYTE = MakeReq(APNDP, req.rnw, req.addr2b);
 
    (void)SetSwdioDriveMode();
    WriteByteLSBWithoutDelay(REQUEST_BYTE);
 
    (void)SetSwdioSampleMode();
    GenOneClkWithoutDelay();  // turnaround Host -> Target
 
    // ACK: CMSIS SW_READ_BIT phase (sample in low phase)
    uint8_t ack_raw = 0u;
    for (uint32_t i = 0; i < ACK_BITS; ++i)
    {
      if (ReadBitAndClockWithoutDelay())
      {
        ack_raw |= static_cast<uint8_t>(1u << i);
      }
    }
    resp.ack = DecodeAck(static_cast<uint8_t>(ack_raw & 0x7u));
 
    if (resp.ack != SwdProtocol::Ack::OK)
    {
      GenOneClkWithoutDelay();
      (void)SetSwdioDriveMode();
      swdio_.Write(true);
      swclk_.Write(false);
      return ErrorCode::OK;
    }
 
    if (req.rnw)
    {
      uint32_t data = 0u;
      for (uint32_t byte = 0; byte < 4u; ++byte)
      {
        const uint32_t B = ReadByteLSBWithoutDelay();
        data |= (B << (8u * byte));
      }
 
      const bool PARITY_BIT = ReadBitAndClockWithoutDelay();
      resp.rdata = data;
      resp.parity_ok = (static_cast<uint8_t>(PARITY_BIT) == Parity32(data));
 
      (void)SetSwdioDriveMode();
      swdio_.Write(true);
      GenOneClkWithoutDelay();
 
      swclk_.Write(false);
    }
    else
    {
      (void)SetSwdioDriveMode();
      GenOneClkWithoutDelay();
 
      const uint32_t DATA = req.wdata;
      for (uint32_t byte = 0; byte < 4u; ++byte)
      {
        const uint8_t B = static_cast<uint8_t>((DATA >> (8u * byte)) & 0xFFu);
        WriteByteLSBWithoutDelay(B);
      }
 
      const bool PARITY_BIT = (Parity32(DATA) & 0x1u) != 0u;
      WriteBitWithoutDelay(PARITY_BIT);
 
      swdio_.Write(true);
      swclk_.Write(false);
    }
 
    return ErrorCode::OK;
  }
 
 private:
  static constexpr uint32_t DEFAULT_CLOCK_HZ =
      500'000u;  
  static constexpr uint32_t LINE_RESET_CYCLES =
      64u;  
  static constexpr uint32_t BYTE_BITS = 8u;  
  static constexpr uint32_t ACK_BITS = 3u;   
 
  static constexpr uint8_t JTAG_TO_SWD_SEQ0 =
      0x9Eu;  
  static constexpr uint8_t JTAG_TO_SWD_SEQ1 =
      0xE7u;  
 
  SwclkGpioType& swclk_;  
  SwdioGpioType& swdio_;  
 
  uint32_t clock_hz_ = 0u;  
 
  uint32_t loops_per_us_ = 0u;       // 手调系数：BusyLoop 每微秒大约需要的迭代数
  uint32_t half_period_ns_ = 0u;     // 当前半周期（ns）
  uint32_t half_period_loops_ = 0u;  // 当前半周期对应的 BusyLoop 迭代数
 
  SwdioMode swdio_mode_ =
      SwdioMode::UNKNOWN;  
};
 
}  // namespace LibXR::Debug