/**
 * Copyright (c) 2011-2025 Bill Greiman
 * This file is part of the SdFat library for SD memory cards.
 *
 * MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */
#ifdef ARDUINO_ARCH_RP2040
#include <math.h>

#include <algorithm>  // Required for std::max, std::min
#define DEBUG_FILE "PioSdioCard.cpp"
#include "../SdCardInfo.h"
#include "DbgLog.h"
#include "PioSdioCard.h"
#include "PioSdioCard.pio.h"
//------------------------------------------------------------------------------
// USE_DEBUG_MODE 0 - no debug, 1 - print message, 2 - Use scope/analyzer.
#define USE_DEBUG_MODE 1

const uint PIO_CLK_DIV_RUN = 1;

const uint PIN_SDIO_UNDEFINED = 63u;

const uint DAT_FIFO_DEPTH = 8;
//==============================================================================
// Command definitions.
enum { RSP_R0 = 0, RSP_R1 = 1, RSP_R2 = 2, RSP_R3 = 3, RSP_R6 = 6, RSP_R7 = 7 };
static const CmdRsp_t CMD0_R0(CMD0, RSP_R0);
static const CmdRsp_t CMD2_R2(CMD2, RSP_R2);
static const CmdRsp_t CMD3_R6(CMD3, RSP_R6);
static const CmdRsp_t CMD6_R1(CMD6, RSP_R1);
static const CmdRsp_t CMD7_R1(CMD7, RSP_R1);
static const CmdRsp_t CMD8_R7(CMD8, RSP_R7);
static const CmdRsp_t CMD9_R2(CMD9, RSP_R2);
static const CmdRsp_t CMD10_R2(CMD10, RSP_R2);
static const CmdRsp_t CMD12_R1(CMD12, RSP_R1);
static const CmdRsp_t CMD13_R1(CMD13, RSP_R1);
static const CmdRsp_t CMD18_R1(CMD18, RSP_R1);
static const CmdRsp_t CMD25_R1(CMD25, RSP_R1);
static const CmdRsp_t CMD32_R1(CMD32, RSP_R1);
static const CmdRsp_t CMD33_R1(CMD33, RSP_R1);
static const CmdRsp_t CMD38_R1(CMD38, RSP_R1);
static const CmdRsp_t CMD55_R1(CMD55, RSP_R1);
static const CmdRsp_t ACMD6_R1(ACMD6, RSP_R1);
static const CmdRsp_t ACMD13_R1(ACMD13, RSP_R1);
static const CmdRsp_t ACMD41_R3(ACMD41, RSP_R3);
static const CmdRsp_t ACMD51_R1(ACMD51, RSP_R1);
//==============================================================================
class Timeout {
 public:
  explicit Timeout(uint ms) : _usStart(0), _usTimeout(1000 * ms) {}
  bool timedOut() {
    if (_usStart) {
      return (usSinceBoot() - _usStart) > _usTimeout;
    }
    _usStart = usSinceBoot();
    return false;
  }
  uint32_t usSinceBoot() { return to_us_since_boot(get_absolute_time()); }
  uint32_t _usStart;
  uint32_t _usTimeout;
};
//==============================================================================
#if USE_DEBUG_MODE
//------------------------------------------------------------------------------
static inline void gpioStatus(uint gpio) {
  logmsgln("gpio", gpio, " drive: ", gpio_get_drive_strength(gpio));
  //  logmsgln("gpio", gpio,
  //           " drive: ", static_cast<int>(gpio_get_drive_strength(gpio)));
  logmsgln("gpio", gpio, " slew:  ", gpio_get_slew_rate(gpio));
  logmsgln("gpio", gpio, " hyst:  ", gpio_is_input_hysteresis_enabled(gpio));
  logmsgln("gpio", gpio, " pull:  ", gpio_is_pulled_up(gpio));
}
//------------------------------------------------------------------------------
static inline void pioRegs(PIO pio) {
  logmsgln("ctrl:    0b", Bin(pio->ctrl));
  logmsgln("fstat:   0b", Bin(pio->fstat));
  logmsgln("fdebug:  0b", Bin(pio->fdebug));
  logmsgln("flevel:  0b", Bin(pio->flevel));
  logmsgln("padout:  0b", Bin(pio->dbg_padout));
  logmsgln("padoe:   0b", Bin(pio->dbg_padoe));
  logmsgln("cfginfo: 0x", Hex(pio->dbg_cfginfo));
  logmsgln("sync_bypass: 0b", Bin(pio->input_sync_bypass));
}
//------------------------------------------------------------------------------
static inline void pioSmRegs(PIO pio, uint sm) {
  logmsgln("sm", sm, " clkdiv: 0x", Hex(pio->sm[sm].clkdiv));
  logmsgln("sm", sm, " execctrl: 0x", Hex(pio->sm[sm].execctrl));
  logmsgln("sm", sm, " shiftctrl: 0x", Hex(pio->sm[sm].shiftctrl));
  logmsgln("sm", sm, " addr: 0x", Hex(pio->sm[sm].addr));
  logmsgln("sm", sm, " pinctrl: 0x", Hex(pio->sm[sm].pinctrl));
}
//------------------------------------------------------------------------------
#endif  // USE_DEBUG_MODE
#define sdError(code)               \
  {                                 \
    setSdErrorCode(code, __LINE__); \
    DBG_MSG(#code);                 \
  }
#define SDIO_FAIL() DBG_MSG("SDIO_FAIL")
//==============================================================================
// CRC functions.
//------------------------------------------------------------------------------
// See this library's extras folder.
static const uint8_t crc7_table[256] = {
    0x00, 0x12, 0x24, 0x36, 0x48, 0x5a, 0x6c, 0x7e,  // 00 - 07
    0x90, 0x82, 0xb4, 0xa6, 0xd8, 0xca, 0xfc, 0xee,  // 08 - 0f
    0x32, 0x20, 0x16, 0x04, 0x7a, 0x68, 0x5e, 0x4c,  // 10 - 17
    0xa2, 0xb0, 0x86, 0x94, 0xea, 0xf8, 0xce, 0xdc,  // 18 - 1f
    0x64, 0x76, 0x40, 0x52, 0x2c, 0x3e, 0x08, 0x1a,  // 20 - 27
    0xf4, 0xe6, 0xd0, 0xc2, 0xbc, 0xae, 0x98, 0x8a,  // 28 - 2f
    0x56, 0x44, 0x72, 0x60, 0x1e, 0x0c, 0x3a, 0x28,  // 30 - 37
    0xc6, 0xd4, 0xe2, 0xf0, 0x8e, 0x9c, 0xaa, 0xb8,  // 38 - 3f
    0xc8, 0xda, 0xec, 0xfe, 0x80, 0x92, 0xa4, 0xb6,  // 40 - 47
    0x58, 0x4a, 0x7c, 0x6e, 0x10, 0x02, 0x34, 0x26,  // 48 - 4f
    0xfa, 0xe8, 0xde, 0xcc, 0xb2, 0xa0, 0x96, 0x84,  // 50 - 57
    0x6a, 0x78, 0x4e, 0x5c, 0x22, 0x30, 0x06, 0x14,  // 58 - 5f
    0xac, 0xbe, 0x88, 0x9a, 0xe4, 0xf6, 0xc0, 0xd2,  // 60 - 67
    0x3c, 0x2e, 0x18, 0x0a, 0x74, 0x66, 0x50, 0x42,  // 68 - 6f
    0x9e, 0x8c, 0xba, 0xa8, 0xd6, 0xc4, 0xf2, 0xe0,  // 70 - 77
    0x0e, 0x1c, 0x2a, 0x38, 0x46, 0x54, 0x62, 0x70,  // 78 - 7f
    0x82, 0x90, 0xa6, 0xb4, 0xca, 0xd8, 0xee, 0xfc,  // 80 - 87
    0x12, 0x00, 0x36, 0x24, 0x5a, 0x48, 0x7e, 0x6c,  // 88 - 8f
    0xb0, 0xa2, 0x94, 0x86, 0xf8, 0xea, 0xdc, 0xce,  // 90 - 97
    0x20, 0x32, 0x04, 0x16, 0x68, 0x7a, 0x4c, 0x5e,  // 98 - 9f
    0xe6, 0xf4, 0xc2, 0xd0, 0xae, 0xbc, 0x8a, 0x98,  // a0 - a7
    0x76, 0x64, 0x52, 0x40, 0x3e, 0x2c, 0x1a, 0x08,  // a8 - af
    0xd4, 0xc6, 0xf0, 0xe2, 0x9c, 0x8e, 0xb8, 0xaa,  // b0 - b7
    0x44, 0x56, 0x60, 0x72, 0x0c, 0x1e, 0x28, 0x3a,  // b8 - bf
    0x4a, 0x58, 0x6e, 0x7c, 0x02, 0x10, 0x26, 0x34,  // c0 - c7
    0xda, 0xc8, 0xfe, 0xec, 0x92, 0x80, 0xb6, 0xa4,  // c8 - cf
    0x78, 0x6a, 0x5c, 0x4e, 0x30, 0x22, 0x14, 0x06,  // d0 - d7
    0xe8, 0xfa, 0xcc, 0xde, 0xa0, 0xb2, 0x84, 0x96,  // d8 - df
    0x2e, 0x3c, 0x0a, 0x18, 0x66, 0x74, 0x42, 0x50,  // e0 - e7
    0xbe, 0xac, 0x9a, 0x88, 0xf6, 0xe4, 0xd2, 0xc0,  // e8 - ef
    0x1c, 0x0e, 0x38, 0x2a, 0x54, 0x46, 0x70, 0x62,  // f0 - f7
    0x8c, 0x9e, 0xa8, 0xba, 0xc4, 0xd6, 0xe0, 0xf2   // f8 - ff
};
//------------------------------------------------------------------------------
inline static uint8_t CRC7(const uint8_t* data, uint8_t n) {
  uint8_t crc = 0;
  for (uint8_t i = 0; i < n; i++) {
    crc = crc7_table[crc ^ data[i]];
  }
  return crc | 1;
}
//------------------------------------------------------------------------------
// Modified from sdio_crc16_4bit_checksum() in
// https://github.com/ZuluSCSI/ZuluSCSI-firmware
//
static inline __attribute__((always_inline)) uint64_t crc16(uint64_t crc,
                                                            uint32_t data_in) {
  // Shift out 8 bits for each line
  uint32_t data_out = crc >> 32;
  crc <<= 32;

  // XOR outgoing data to itself with 4 bit delay
  data_out ^= (data_out >> 16);

  // XOR incoming data to outgoing data with 4 bit delay
  data_out ^= (data_in >> 16);

  // XOR outgoing and incoming data to accumulator at each tap
  uint64_t xorred = data_out ^ data_in;
  crc ^= xorred;
  crc ^= xorred << (5 * 4);
  crc ^= xorred << (12 * 4);
  return crc;
}
//------------------------------------------------------------------------------
static bool claimPio(PIO pio, const pio_program_t* program) {
  uint mask = 0;
  if (!pio_can_add_program(pio, program)) {
    DBG_MSG("pio_can_add_program");
    return false;
  }
  for (uint i = 0; i < NUM_PIO_STATE_MACHINES; i++) {
    int sm = pio_claim_unused_sm(pio, false);
    if (sm < 0) {
      break;
    }
    mask |= 1u << sm;
  }
  if (mask == ((1u << NUM_PIO_STATE_MACHINES) - 1)) {
    return true;
  }
  for (uint sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) {
    if ((1u << sm) & mask) {
      pio_sm_unclaim(pio, sm);
    }
  }
  DBG_MSG("pio_can_add_program");
  return false;
}
//==============================================================================
// add to PioSdioCard class int the future.
// PioSdioCard::PioSdioCard()
// PioSdioCard::~PioSdioCard()
//------------------------------------------------------------------------------
bool PioSdioCard::cardAcmd(uint32_t rca, CmdRsp_t cmdRsp, uint32_t arg) {
  return cardCommand(CMD55_R1, rca) && cardCommand(cmdRsp, arg);
}
//------------------------------------------------------------------------------
bool PioSdioCard::begin(PioSdioConfig sdioConfig) {
  pioEnd();
  Timeout timeout(SD_INIT_TIMEOUT);
  uint32_t arg;
  m_curState = IDLE_STATE;
  m_errorCode = SD_CARD_ERROR_NONE;
  m_highCapacity = false;
  m_initDone = false;
  m_version2 = false;
  m_clkPin = sdioConfig.clkPin();
  m_cmdPin = sdioConfig.cmdPin();
  m_dat0Pin = sdioConfig.dat0Pin();

  // Four PIO cycles per SD CLK cycle.
  m_clkDiv = ceil((0.00025 * clock_get_hz(clk_sys)) / SD_MAX_INIT_RATE_KHZ);

#if USE_DEBUG_MODE == 2
  Serial.println();
  pioRegs(m_pio);
  pioSmRegs(m_pio, m_sm0);
  pioSmRegs(m_pio, m_sm1);
  gpioStatus(m_clkPin);
  gpioStatus(m_cmdPin);
  while (Serial.read() >= 0) {
  }
  Serial.println("Logic Analyzer on then type any char");
  while (!Serial.available()) {
  }
#endif  // USE_DEBUG_MODE
  // A few cards still require clocks after power-up.
  powerUpClockCycles();
  if (!pioInit()) {
    goto fail;
  }
  pioConfig(m_clkDiv);
  if (!cardCommand(CMD0_R0, 0)) {
    sdError(SD_CARD_ERROR_CMD0);
    goto fail;
  }
  if (cardCommand(CMD8_R7, 0X1AA)) {
    if (m_cardRsp != 0X1AA) {
      sdError(SD_CARD_ERROR_CMD8);
      goto fail;
    }
    m_version2 = true;
  } else {
    m_version2 = false;
    m_errorCode = SD_CARD_ERROR_NONE;
  }
  arg = m_version2 ? 0X40300000 : 0x00300000;
  while (true) {
    if (!cardAcmd(0, ACMD41_R3, arg)) {
      sdError(SD_CARD_ERROR_ACMD41);
      goto fail;
    }
    if (m_cardRsp & 0x80000000) {
      break;
    }
    if (timeout.timedOut()) {
      sdError(SD_CARD_ERROR_ACMD41);
      goto fail;
    }
  }
  m_ocr = m_cardRsp;
  if (m_cardRsp & 0x40000000) {
    // Is high capacity.
    m_highCapacity = true;
  }
  if (!cardCommand(CMD2_R2, 0)) {
    sdError(SD_CARD_ERROR_CMD2);
    goto fail;
  }
  if (!cardCommand(CMD3_R6, 0)) {
    sdError(SD_CARD_ERROR_CMD3);
    goto fail;
  }
  m_rca = m_cardRsp & 0xFFFF0000;
  if (!cardCommand(CMD9_R2, m_rca, &m_csd)) {
    sdError(SD_CARD_ERROR_CMD9);
    goto fail;
  }
  if (!cardCommand(CMD10_R2, m_rca, &m_cid)) {
    sdError(SD_CARD_ERROR_CMD10);
    goto fail;
  }
  if (!cardCommand(CMD7_R1, m_rca)) {
    sdError(SD_CARD_ERROR_CMD7);
    goto fail;
  }

  if (!cardAcmd(m_rca, ACMD6_R1, 2)) {
    sdError(SD_CARD_ERROR_ACMD6);
    goto fail;
  }
  m_clkDiv = sdioConfig.clkDiv();
  pioConfig(m_clkDiv);
  if (!cardAcmd(m_rca, ACMD51_R1, 0)) {
    sdError(SD_CARD_ERROR_ACMD51);
    goto fail;
  }
  if (!readData(&m_scr, sizeof(m_scr))) {
    DBG_MSG("readData");
    goto fail;
  }
  if (!cardAcmd(m_rca, ACMD13_R1, 0)) {
    sdError(SD_CARD_ERROR_ACMD13);
    SDIO_FAIL();
    goto fail;
  }

  if (!readData(&m_sds, sizeof(m_sds))) {
    SDIO_FAIL();
    goto fail;
  }
  m_initDone = true;

  return true;
fail:
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::cardCMD6(uint32_t arg, uint8_t* status) {
  if (!cardCommand(CMD6_R1, arg)) {
    sdError(SD_CARD_ERROR_CMD6);
    goto fail;
  }
  if (!readData(status, 64)) {
    SDIO_FAIL();
    goto fail;
  }
  return true;
fail:
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::cardCommand(CmdRsp_t cmd, uint32_t arg, void* rsp) {
  uint8_t buf[6];
  uint nRsp = cmd.rsp == RSP_R0 ? 0 : cmd.rsp == RSP_R2 ? 17 : 6;
  const io_ro_8* rxFifo = reinterpret_cast<io_ro_8*>(&m_pio->rxf[m_sm0]);
  io_wo_8* txFifo = reinterpret_cast<io_wo_8*>(&m_pio->txf[m_sm0]);
  pio_sm_set_enabled(m_pio, m_sm1, false);
  pio_sm_init(m_pio, m_sm0, m_cmdRspOffset, &m_cmdConfig);
  pio_sm_exec(m_pio, m_sm0, pio_encode_set(pio_pindirs, 1));
  *txFifo = 55;
  pio_sm_exec(m_pio, m_sm0, pio_encode_out(pio_x, 8));
  *txFifo = nRsp ? 8 * nRsp - 1 : 0;
  pio_sm_exec(m_pio, m_sm0, pio_encode_out(pio_y, 8));
  pio_sm_set_enabled(m_pio, m_sm0, true);
  uint n = 0;
  buf[n++] = (uint8_t)(cmd.idx | 0x40);
  buf[n++] = (uint8_t)(arg >> 24U);
  buf[n++] = (uint8_t)(arg >> 16U);
  buf[n++] = (uint8_t)(arg >> 8U);
  buf[n++] = (uint8_t)arg;
  buf[n++] = CRC7(buf, 5);
  *txFifo = 0XFF;
  for (uint i = 0; i < n; i++) {
    while (pio_sm_is_tx_fifo_full(m_pio, m_sm0)) {
    }
    *txFifo = buf[i];
  }

  Timeout timeout(SD_CMD_TIMEOUT);
  if (!nRsp) {
    uint32_t fdebug_tx_stall = 1u << (PIO_FDEBUG_TXSTALL_LSB + m_sm0);
    m_pio->fdebug = fdebug_tx_stall;
    while (!(m_pio->fdebug & fdebug_tx_stall)) {
      if (timeout.timedOut()) {
        goto fail;
      }
    }
    goto done;
  }
  uint8_t rtn[20];

  for (uint i = 0; i < nRsp; i++) {
    while (pio_sm_is_rx_fifo_empty(m_pio, m_sm0)) {
      if (timeout.timedOut()) {
        goto fail;
      }
    }
    rtn[i] = *rxFifo;
  }
  if (cmd.rsp == RSP_R3) {
    if (rtn[0] != 0X3F || rtn[5] != 0XFF) {
      goto fail;
    }
  } else {
    uint8_t crc;
    if (cmd.rsp == RSP_R2) {
      crc = CRC7(rtn + 1, nRsp - 2);
    } else {
      crc = CRC7(rtn, nRsp - 1);
    }
    if (rtn[nRsp - 1] != crc) {
#if USE_DEBUG_MODE
      Serial.printf("CHK: %02X, CRC: %02X\n", rtn[nRsp - 1], crc);
      for (uint i = 0; i < nRsp; i++) {
        Serial.printf(" %02X", rtn[i]);
      }
      Serial.println();
#endif  // USE_DEBUG_MODE
      sdError(SD_CARD_ERROR_READ_CRC);
      goto fail;
    }
  }
  if (nRsp == 6) {
    m_cardRsp = (rtn[1] << 24) | (rtn[2] << 16) | (rtn[3] << 8) | rtn[4];
    if (rsp) {
      *reinterpret_cast<uint32_t*>(rsp) = m_cardRsp;
    }
  } else if (rsp && nRsp == 17) {
    memcpy(rsp, rtn + 1, 16);
  }

done:
  pio_sm_set_enabled(m_pio, m_sm0, false);
  return true;

fail:
#if USE_DEBUG_MODE
  DBG_MSG("CMD", cmd.idx, " failed");
#endif  // USE_DEBUG_MODE
  pio_sm_set_enabled(m_pio, m_sm0, false);
  return false;
}
//------------------------------------------------------------------------------
void PioSdioCard::end() { pioEnd(); }
//------------------------------------------------------------------------------
bool PioSdioCard::erase(uint32_t firstSector, uint32_t lastSector) {
  Timeout timeout(SD_ERASE_TIMEOUT);
  if (!syncDevice()) {
    SDIO_FAIL();
    goto fail;
  }
  // check for single sector erase
  if (!m_csd.eraseSingleBlock()) {
    // erase size mask
    uint8_t m = m_csd.eraseSize() - 1;
    if ((firstSector & m) != 0 || ((lastSector + 1) & m) != 0) {
      // error card can't erase specified area
      sdError(SD_CARD_ERROR_ERASE_SINGLE_SECTOR);
      goto fail;
    }
  }
  if (!m_highCapacity) {
    firstSector <<= 9;
    lastSector <<= 9;
  }
  if (!cardCommand(CMD32_R1, firstSector)) {
    sdError(SD_CARD_ERROR_CMD32);
    goto fail;
  }
  if (!cardCommand(CMD33_R1, lastSector)) {
    sdError(SD_CARD_ERROR_CMD33);
    goto fail;
  }
  if (!cardCommand(CMD38_R1, 0)) {
    sdError(SD_CARD_ERROR_CMD38);
    goto fail;
  }
  while (isBusy()) {
    if (timeout.timedOut()) {
      sdError(SD_CARD_ERROR_ERASE_TIMEOUT);
      goto fail;
    }
  }
  return true;
fail:
  return false;
}
//------------------------------------------------------------------------------
uint8_t PioSdioCard::errorCode() const { return m_errorCode; }
//------------------------------------------------------------------------------
uint32_t PioSdioCard::errorData() const { return m_cardRsp; }
//------------------------------------------------------------------------------
uint32_t PioSdioCard::errorLine() const { return m_errorLine; }
//------------------------------------------------------------------------------
bool PioSdioCard::isBusy() {
  return gpio_get(m_dat0Pin) ? false : !(status() & CARD_STATUS_READY_FOR_DATA);
}
//------------------------------------------------------------------------------
void PioSdioCard::pioConfig(float clkDiv) {
  m_cmdConfig =
      pio_cmd_rsp_program_config(m_cmdRspOffset, m_cmdPin, m_clkPin, clkDiv);
  m_rdClkConfig =
      pio_rd_clk_program_config(m_rdClkOffset, m_dat0Pin, m_clkPin, clkDiv);
  m_rdDataConfig =
      pio_rd_data_program_config(m_rdDataOffset, m_dat0Pin, clkDiv);
  m_wrDataConfig =
      pio_wr_data_program_config(m_wrDataOffset, m_dat0Pin, m_clkPin, clkDiv);
  m_wrRespConfig =
      pio_wr_resp_program_config(m_wrRespOffset, m_dat0Pin, m_clkPin, clkDiv);
}
//------------------------------------------------------------------------------
void PioSdioCard::pioEnd() {
  if (!m_pio) {
    return;
  }
  for (uint sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) {
    pio_sm_unclaim(m_pio, sm);
  }
  if (m_cmdRspOffset >= 0) {
    pio_remove_program(m_pio, &cmd_rsp_program, m_cmdRspOffset);
    m_cmdRspOffset = -1;
  }
  if (m_rdClkOffset >= 0) {
    pio_remove_program(m_pio, &rd_clk_program, m_rdClkOffset);
    m_rdClkOffset = -1;
  }
  if (m_rdDataOffset >= 0) {
    pio_remove_program(m_pio, &rd_data_program, m_rdDataOffset);
    m_rdDataOffset = -1;
  }
  if (m_wrDataOffset >= 0) {
    pio_remove_program(m_pio, &wr_data_program, m_wrDataOffset);
    m_wrDataOffset = -1;
  }
  if (m_wrRespOffset >= 0) {
    pio_remove_program(m_pio, &wr_resp_program, m_wrRespOffset);
    m_wrRespOffset = -1;
  }
  m_pio = nullptr;
}
//------------------------------------------------------------------------------
bool PioSdioCard::pioInit() {
  uint pin[] = {m_clkPin,      m_cmdPin,      m_dat0Pin,
                m_dat0Pin + 1, m_dat0Pin + 2, m_dat0Pin + 3};
  uint16_t pio_instructions[PIO_INSTRUCTION_COUNT];
  pio_program_t pio_program = {.instructions = nullptr,
                               .length = PIO_INSTRUCTION_COUNT,
                               .origin = -1,
                               .pio_version = 0,
#if PICO_PIO_VERSION > 0
                               .used_gpio_ranges = 0x0
#endif
  };
  if (claimPio(pio0, &pio_program)) {
    m_pio = pio0;
  } else if (claimPio(pio1, &pio_program)) {
    m_pio = pio1;
#if NUM_PIOS > 2
  } else if (claimPio(pio2, &pio_program)) {
    m_pio = pio2;
#endif
  } else {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  m_sm0 = 0;
  m_sm1 = 1;
#if PICO_PIO_USE_GPIO_BASE
  if (std::max({m_clkPin, m_cmdPin, m_dat0Pin + 3}) > 31) {
    if (std::min({m_clkPin, m_cmdPin, m_dat0Pin}) < 16 ||
        pio_set_gpio_base(m_pio, 16) != PICO_OK) {
      sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
      goto fail;
    }
  } else if (pio_set_gpio_base(m_pio, 0) != PICO_OK) {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
#endif  // PICO_PIO_USE_GPIO_BASE
  rd_data_patch_program(&pio_program, pio_instructions, m_clkPin);
  m_rdDataOffset = pio_add_program(m_pio, &pio_program);
  if (m_rdDataOffset < 0) {
    DBG_MSG("m_rdDataOffset: ", m_rdDataOffset);
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  m_cmdRspOffset = pio_add_program(m_pio, &cmd_rsp_program);
  if (m_cmdRspOffset < 0) {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  m_rdClkOffset = pio_add_program(m_pio, &rd_clk_program);
  if (m_rdClkOffset < 0) {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  m_wrDataOffset = pio_add_program(m_pio, &wr_data_program);
  if (m_wrDataOffset < 0) {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  m_wrRespOffset = pio_add_program(m_pio, &wr_resp_program);
  if (m_wrRespOffset < 0) {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  for (uint i = 0; i < 6U; i++) {
    gpio_pull_up(pin[i]);
  }
  gpio_set_drive_strength(m_clkPin, GPIO_DRIVE_STRENGTH_8MA);
  gpio_set_slew_rate(m_clkPin, GPIO_SLEW_RATE_FAST);

  for (uint i = 0; i < 6U; i++) {
    pio_gpio_init(m_pio, pin[i]);
  }
  m_pio->input_sync_bypass |=
      (1 << m_clkPin) | (1 << m_cmdPin) | (0XF << m_dat0Pin);
  if (pio_sm_set_consecutive_pindirs(m_pio, m_sm0, m_clkPin, 1, true) !=
          PICO_OK ||
      pio_sm_set_consecutive_pindirs(m_pio, m_sm0, m_cmdPin, 1, true) !=
          PICO_OK ||
      pio_sm_set_consecutive_pindirs(m_pio, m_sm0, m_dat0Pin, 4, false) !=
          PICO_OK) {
    sdError(SD_CARD_ERROR_ADD_PIO_PROGRAM);
    goto fail;
  }
  return true;

fail:
  pioEnd();
  return false;
}
//------------------------------------------------------------------------------
// A few cards still need at least 74 clocks after power-up. from the spec:
//
// After 1 ms VDD stable time, host provides at least 74 clocks while keeping
// CMD high before issuing the first command.  In the case of SPI mode, CS
// shall be held high during 74 clock cycles.
//
// The original April 15, 2001 spec explains the 74 clocks:
//
// The additional 10 clocks (over the 64 clocks after which the card should be
// ready for communication) is provided to eliminate power-up synchronization
// problems.
//
void PioSdioCard::powerUpClockCycles() {
  // Two clk_sys per SD clock cycle.
  uint32_t nWait = ceil(0.0005 * clock_get_hz(clk_sys) / SD_MAX_INIT_RATE_KHZ);
  gpio_init(m_cmdPin);
  gpio_set_drive_strength(m_cmdPin, GPIO_DRIVE_STRENGTH_8MA);
  gpio_set_dir(m_cmdPin, true);
  gpio_put(m_cmdPin, 1);
  gpio_init(m_clkPin);
  gpio_set_drive_strength(m_clkPin, GPIO_DRIVE_STRENGTH_8MA);
  gpio_set_dir(m_clkPin, true);

  // Send 80 SD CLK cycles with CMD high.  End with CLK low.
  for (uint i = 0; i <= 160; i++) {
    gpio_put(m_clkPin, 1 & i);
    busy_wait_at_least_cycles(nWait);
  }
}
//------------------------------------------------------------------------------
bool PioSdioCard::readCID(cid_t* cid) {
  memcpy(cid, &m_cid, sizeof(cid_t));
  return true;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readCSD(csd_t* csd) {
  memcpy(csd, &m_csd, sizeof(csd_t));
  return true;
}
//------------------------------------------------------------------------------
bool __time_critical_func(PioSdioCard::readData)(void* dst, size_t count) {
  uint32_t buf[128];
  uint n32 = count / 4;
  uint nr = n32 + 2;
  const uint mask = (1ul << m_sm0) | (1ul << m_sm1);
  io_wo_8* txFifo = reinterpret_cast<io_wo_8*>(&m_pio->txf[m_sm1]);
  pio_sm_init(m_pio, m_sm0, m_rdDataOffset, &m_rdDataConfig);
  pio_sm_init(m_pio, m_sm1, m_rdClkOffset, &m_rdClkConfig);
  pio_set_sm_mask_enabled(m_pio, mask, true);

  uint nf = nr < DAT_FIFO_DEPTH ? nr : DAT_FIFO_DEPTH;
  for (uint it = 0; it < nf; it++) {
    *txFifo = 0XFF;
  }
  io_ro_32* rxFifo = reinterpret_cast<io_ro_32*>(&m_pio->rxf[m_sm0]);
  uint32_t* dst32 = (uint)dst & 3 ? buf : reinterpret_cast<uint32_t*>(dst);
  uint64_t crc = 0;
  uint64_t chk = 0;
  Timeout timeout(SD_READ_TIMEOUT);
  uint ir = 0;
  if (nf < nr) {
    uint nb = nr - nf;
    while (true) {
      while (pio_sm_get_rx_fifo_level(m_pio, m_sm0) < 4) {
        if (timeout.timedOut()) {
          sdError(SD_CARD_ERROR_READ_TIMEOUT);
          goto fail;
        }
      }
      uint32_t tmp = *rxFifo;
      *txFifo = 0XFF;
      dst32[ir++] = __builtin_bswap32(tmp);
      crc = crc16(crc, tmp);
      tmp = *rxFifo;
      *txFifo = 0XFF;
      dst32[ir++] = __builtin_bswap32(tmp);
      crc = crc16(crc, tmp);
      if (ir == nb) {
        break;
      }
      tmp = *rxFifo;
      *txFifo = 0XFF;
      dst32[ir++] = __builtin_bswap32(tmp);
      crc = crc16(crc, tmp);
      tmp = *rxFifo;
      *txFifo = 0XFF;
      dst32[ir++] = __builtin_bswap32(tmp);
      crc = crc16(crc, tmp);
    }
  }
  for (; ir < nr; ir++) {
    while (pio_sm_is_rx_fifo_empty(m_pio, m_sm0)) {
      if (timeout.timedOut()) {
        sdError(SD_CARD_ERROR_READ_TIMEOUT);
        goto fail;
      }
    }
    uint32_t tmp = *rxFifo;
    if (ir < n32) {
      dst32[ir] = __builtin_bswap32(tmp);
      crc = crc16(crc, tmp);
    } else {
      chk <<= 32;
      chk |= tmp;
    }
  }
  if (crc != chk) {
#if USE_DEBUG_MODE
    Serial.printf("crc: %llX\r\nchk: %llX\r\n", crc, chk);
#endif  // USE_DEBUG_MODE
    sdError(SD_CARD_ERROR_READ_CRC);
    goto fail;
  }
  pio_set_sm_mask_enabled(m_pio, mask, false);
  if (dst32 == buf) {
    memcpy(dst, buf, count);
  }
  return true;

fail:
  pio_set_sm_mask_enabled(m_pio, mask, false);
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readData(uint8_t* dst) { return readData(dst, 512); }
//------------------------------------------------------------------------------
bool PioSdioCard::readOCR(uint32_t* ocr) {
  *ocr = m_ocr;
  return true;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readSCR(scr_t* scr) {
  memcpy(scr, &m_scr, sizeof(scr_t));
  return true;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readSDS(sds_t* sds) {
  memcpy(sds, &m_sds, sizeof(sds_t));
  return true;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readSector(Sector_t sector, uint8_t* dst) {
  if (m_curState != READ_STATE || sector != m_curSector) {
    if (!syncDevice()) {
      SDIO_FAIL();
      goto fail;
    }
    if (!readStart(sector)) {
      sdError(SD_CARD_ERROR_READ_START);
      goto fail;
    }
    m_curSector = sector;
    m_curState = READ_STATE;
  }
  if (!readData(dst, 512)) {
    SDIO_FAIL();
    goto fail;
  }
  m_curSector++;
  return true;

fail:
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readSectors(Sector_t sector, uint8_t* dst, size_t ns) {
  for (size_t i = 0; i < ns; i++) {
    if (!readSector(sector + i, dst + i * 512UL)) {
      SDIO_FAIL();
      goto fail;
    }
  }
  return true;

fail:
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readStart(Sector_t sector) {
  uint arg = m_highCapacity ? sector : 512 * sector;
  if (!cardCommand(CMD18_R1, arg)) {
    sdError(SD_CARD_ERROR_CMD18);
    goto fail;
  }
  return true;

fail:
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::readStop() {
  if (!syncDevice()) {
    SDIO_FAIL();
    return false;
  }
  return true;
}
//------------------------------------------------------------------------------
uint32_t PioSdioCard::status() {
  return cardCommand(CMD13_R1, m_rca) ? m_cardRsp : CARD_STATUS_ERROR;
}
//------------------------------------------------------------------------------
Sector_t PioSdioCard::sectorCount() { return m_csd.capacity(); }
//------------------------------------------------------------------------------
bool PioSdioCard::syncDevice() {
  if (m_curState != IDLE_STATE) {
    Timeout timeout(SD_INIT_TIMEOUT);
    while (!gpio_get(m_dat0Pin)) {
      if (timeout.timedOut()) {
        sdError(SD_CARD_ERROR_CMD12);
        goto fail;
      }
    }
    if (!cardCommand(CMD12_R1, 0)) {
      sdError(SD_CARD_ERROR_CMD12);
      goto fail;
    }
    while (isBusy()) {
      if (timeout.timedOut()) {
        sdError(SD_CARD_ERROR_CMD12);
        goto fail;
      }
    }
    m_curState = IDLE_STATE;
  }
  return true;

fail:
  return false;
}
//------------------------------------------------------------------------------
uint8_t PioSdioCard::type() const {
  return !m_initDone       ? 0
         : !m_version2     ? SD_CARD_TYPE_SD1
         : !m_highCapacity ? SD_CARD_TYPE_SD2
                           : SD_CARD_TYPE_SDHC;
}
//------------------------------------------------------------------------------
bool PioSdioCard::writeSector(Sector_t sector, const uint8_t* src) {
  return writeSectors(sector, src, 1);
}
//------------------------------------------------------------------------------
bool PioSdioCard::writeSectors(Sector_t sector, const uint8_t* src, size_t ns) {
  if (m_curState != WRITE_STATE || m_curSector != sector) {
    if (!syncDevice()) {
      SDIO_FAIL();
      goto fail;
    }
    if (!writeStart(sector)) {
      sdError(SD_CARD_ERROR_WRITE_START);
      goto fail;
    }
    m_curSector = sector;
    m_curState = WRITE_STATE;
  }
  for (size_t i = 0; i < ns; i++, src += 512) {
    if (!writeData(src)) {
      sdError(SD_CARD_ERROR_WRITE_DATA);
      goto fail;
    }
  }
  m_curSector += ns;
  return true;
fail:
  return false;
}
//------------------------------------------------------------------------------
bool __time_critical_func(PioSdioCard::writeData)(const uint8_t* src) {
  const uint32_t* src32;
  uint32_t buf[128];
  if ((uint)src & 3) {
    memcpy(buf, src, 512);
    src32 = (const uint32_t*)buf;
  } else {
    src32 = (const uint32_t*)src;
  }
  uint32_t tmp;
  io_wo_32* txFifo = reinterpret_cast<io_wo_32*>(&m_pio->txf[m_sm0]);
  io_ro_32* rxFifo = reinterpret_cast<io_ro_32*>(&m_pio->rxf[m_sm1]);
  uint8_t rsp;
  uint mask = (1ul << m_sm0) | (1ul << m_sm1);
  uint64_t crc = 0;

  Timeout timeout(SD_WRITE_TIMEOUT);
  while (!gpio_get(m_dat0Pin)) {
    if (timeout.timedOut()) {
      sdError(SD_CARD_ERROR_WRITE_TIMEOUT);
      goto fail;
    }
  }
  pio_sm_init(m_pio, m_sm0, m_wrDataOffset, &m_wrDataConfig);
  pio_sm_init(m_pio, m_sm1, m_wrRespOffset, &m_wrRespConfig);
  *txFifo = 1048;  // 8 + 1024 + 16 + 1 - 1;
  pio_sm_exec(m_pio, m_sm0, pio_encode_out(pio_x, 32));
  pio_sm_exec(m_pio, m_sm0, pio_encode_set(pio_pindirs, 0XF));
  *txFifo = 0xFFFFFFF0;
  pio_set_sm_mask_enabled(m_pio, mask, true);
  for (int i = 0; i < 128;) {
    while (pio_sm_get_tx_fifo_level(m_pio, m_sm0) > 4) {
      if (timeout.timedOut()) {
        sdError(SD_CARD_ERROR_WRITE_FIFO);
        goto fail;
      }
    }
    tmp = __builtin_bswap32(src32[i++]);
    crc = crc16(crc, tmp);
    *txFifo = tmp;
    tmp = __builtin_bswap32(src32[i++]);
    crc = crc16(crc, tmp);
    *txFifo = tmp;
    tmp = __builtin_bswap32(src32[i++]);
    crc = crc16(crc, tmp);
    *txFifo = tmp;
    tmp = __builtin_bswap32(src32[i++]);
    crc = crc16(crc, tmp);
    *txFifo = tmp;
  }
  while (pio_sm_get_tx_fifo_level(m_pio, m_sm0) > 5) {
    if (timeout.timedOut()) {
      sdError(SD_CARD_ERROR_WRITE_FIFO);
      goto fail;
    }
  }
  *txFifo = static_cast<uint32_t>(crc >> 32);
  *txFifo = static_cast<uint32_t>(crc);
  *txFifo = 0xFFFFFFFF;

  while (pio_sm_is_rx_fifo_empty(m_pio, m_sm1)) {
    if (timeout.timedOut()) {
      sdError(SD_CARD_ERROR_READ_FIFO);
      goto fail;
    }
  }
  rsp = *rxFifo;
  if ((rsp & 0X1F) != 0b101) {
#if USE_DEBUG_MODE
    Serial.printf("wr rsp: %02X\n", rsp);
#endif  // USE_DEBUG_MODE
    sdError(SD_CARD_ERROR_WRITE_DATA);
    goto fail;
  }
  return true;
fail:
  pio_set_sm_mask_enabled(m_pio, mask, false);
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::writeStart(Sector_t sector) {
  uint arg = m_highCapacity ? sector : 512 * sector;
  if (!cardCommand(CMD25_R1, arg)) {
    sdError(SD_CARD_ERROR_CMD25);
    goto fail;
  }
  return true;
fail:
  return false;
}
//------------------------------------------------------------------------------
bool PioSdioCard::writeStop() {
  if (!syncDevice()) {
    SDIO_FAIL();
    return false;
  }
  return true;
}
#endif  //  ARDUINO_ARCH_RP2040
