/**
 * Initialize a SD flash memory card.
 * 
 * \param[in] slow Set SPI Frequency F_CPU/4 if true else F_CPU/2. 
 *  
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure. 
 *
 */  
uint8_t SdCard::init(uint8_t slow)
{
  pinMode(SS, OUTPUT);
  spiSSHigh();
  pinMode(MOSI, OUTPUT);
  pinMode(SCK, OUTPUT);
  //Enable SPI, Master, clock rate F_CPU/128
  SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (1 << SPR0);
  //must supply min of 74 clock cycles with CS high.
  for (uint8_t i = 0; i < 10; i++) spiSend(0XFF);
  spiSSLow();
  // next line prevent re-init hang by some cards (not sure why this works)
  for (uint16_t i = 0; i <= 512; i++) spiRec();
  uint8_t r = cardCommand(CMD0, 0);
  for (uint16_t retry = 0; r != R1_IDLE_STATE; retry++){
    if (retry == 10000) {
      error(SD_ERROR_CMD0, r);
      return false;
    }
    r = spiRec();
  }
  for (uint16_t retry = 0; ; retry++) {
    cardCommand(CMD55, 0);
    if ((r = cardCommand(ACMD41, 0)) == R1_READY_STATE)break;
    if (retry == 1000) {
      error(SD_ERROR_ACMD41, r);
      return false;
    }
  }
  // set SPI frequency
  SPCR &= ~((1 << SPR1) | (1 << SPR0)); // F_CPU/4
  if (!slow) SPSR |= (1 << SPI2X); // Doubled Clock Frequency to F_CPU/2
  spiSSHigh();
  return true;
}

//------------------------------------------------------------------------------
bool SdSpiCard::writeBlock(uint32_t blockNumber, const uint8_t* src) {
  SD_TRACE("WB", blockNumber);
  // use address if not SDHC card
  if (type() != SD_CARD_TYPE_SDHC) {
    blockNumber <<= 9;
  }
  if (cardCommand(CMD24, blockNumber)) {
    error(SD_CARD_ERROR_CMD24);
    goto fail;
  }
  if (!writeData(DATA_START_BLOCK, src)) {
    goto fail;
  }


#if CHECK_FLASH_PROGRAMMING
  // wait for flash programming to complete
  if (!waitNotBusy(SD_WRITE_TIMEOUT)) {
    error(SD_CARD_ERROR_WRITE_TIMEOUT);
    goto fail;
  }
  // response is r2 so get and check two bytes for nonzero
  if (cardCommand(CMD13, 0) || spiReceive()) {
    error(SD_CARD_ERROR_CMD13);
    goto fail;
  }
#endif  // CHECK_PROGRAMMING

  spiStop();
  return true;

fail:
  spiStop();
  return false;
}

/**
 * Writes a 512 byte block to an SD card.
 *
 * \param[in] blockNumber Logical block to be written.
 * \param[in] src Pointer to the location of the data to be written.
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.
 */
uint8_t Sd2Card::writeBlock(uint32_t blockNumber, const uint8_t* src) {
#if SD_PROTECT_BLOCK_ZERO
  // don't allow write to first block
  if (blockNumber == 0) {
    error(SD_CARD_ERROR_WRITE_BLOCK_ZERO);
    goto fail;
  }
#endif  // SD_PROTECT_BLOCK_ZERO
  // use address if not SDHC card
  if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
  if (cardCommand(CMD24, blockNumber)) {
    error(SD_CARD_ERROR_CMD24);
    goto fail;
  }
  if (!writeData(DATA_START_BLOCK, src)) goto fail;

  // wait for flash programming to complete
  if (!waitNotBusy(SD_WRITE_TIMEOUT)) {
    error(SD_CARD_ERROR_WRITE_TIMEOUT);
    goto fail;
  }

  // response is r2 so get and check two bytes for nonzero
  if (cardCommand(CMD13, 0) || spiRec()) {
    error(SD_CARD_ERROR_WRITE_PROGRAMMING);
    goto fail;
  }
  chipSelectHigh();
  return true;

 fail:
  chipSelectHigh();
  return false;
}

/** Erase a range of blocks.
 *
 * \param[in] firstBlock The address of the first block in the range.
 * \param[in] lastBlock The address of the last block in the range.
 *
 * \note This function requests the SD card to do a flash erase for a
 * range of blocks.  The data on the card after an erase operation is
 * either 0 or 1, depends on the card vendor.  The card must support
 * single block erase.
 *
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.
 */
bool Sd2Card::erase(uint32_t firstBlock, uint32_t lastBlock) {
  csd_t csd;
  if (!readCSD(&csd)) goto fail;
  // check for single block erase
  if (!csd.v1.erase_blk_en) {
    // erase size mask
    uint8_t m = (csd.v1.sector_size_high << 1) | csd.v1.sector_size_low;
    if ((firstBlock & m) != 0 || ((lastBlock + 1) & m) != 0) {
      // error card can't erase specified area
      error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK);
      goto fail;
    }
  }
  if (type_ != SD_CARD_TYPE_SDHC) {
    firstBlock <<= 9;
    lastBlock <<= 9;
  }
  if (cardCommand(CMD32, firstBlock)
    || cardCommand(CMD33, lastBlock)
    || cardCommand(CMD38, 0)) {
      error(SD_CARD_ERROR_ERASE);
      goto fail;
  }
  if (!waitNotBusy(SD_ERASE_TIMEOUT)) {
    error(SD_CARD_ERROR_ERASE_TIMEOUT);
    goto fail;
  }
  chipSelectHigh();
  return true;

 fail:
  chipSelectHigh();
  return false;
}

/** Erase a range of blocks.

    \param[in] firstBlock The address of the first block in the range.
    \param[in] lastBlock The address of the last block in the range.

    \note This function requests the SD card to do a flash erase for a
    range of blocks.  The data on the card after an erase operation is
    either 0 or 1, depends on the card vendor.  The card must support
    single block erase.

    \return The value one, true, is returned for success and
    the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::erase(uint32_t firstBlock, uint32_t lastBlock)
{
    if (!eraseSingleBlockEnable())
    {
        error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK);
        goto fail;
    }
    if (type_ != SD_CARD_TYPE_SDHC)
    {
        firstBlock <<= 9;
        lastBlock <<= 9;
    }
    if (cardCommand(CMD32, firstBlock)
            || cardCommand(CMD33, lastBlock)
            || cardCommand(CMD38, 0))
    {
        error(SD_CARD_ERROR_ERASE);
        goto fail;
    }
    if (!waitNotBusy(SD_ERASE_TIMEOUT))
    {
        error(SD_CARD_ERROR_ERASE_TIMEOUT);
        goto fail;
    }
    chipSelectHigh();
    return true;

fail:
    chipSelectHigh();
    return false;
}

//-----------------------------------------------------------------------------
bool SdioCard::erase(uint32_t firstBlock, uint32_t lastBlock) {
  // check for single block erase
  if (!m_csd.v1.erase_blk_en) {
    // erase size mask
    uint8_t m = (m_csd.v1.sector_size_high << 1) | m_csd.v1.sector_size_low;
    if ((firstBlock & m) != 0 || ((lastBlock + 1) & m) != 0) {
      // error card can't erase specified area
      return sdError(SD_CARD_ERROR_ERASE_SINGLE_BLOCK);
    }
  }
  if (!m_highCapacity) {
    firstBlock <<= 9;
    lastBlock <<= 9;
  }
  if (!cardCommand(CMD32_XFERTYP, firstBlock)) {
    return sdError(SD_CARD_ERROR_CMD32);
  }
  if (!cardCommand(CMD33_XFERTYP, lastBlock)) {
     return sdError(SD_CARD_ERROR_CMD33);
  }
  if (!cardCommand(CMD38_XFERTYP, 0)) {
    return sdError(SD_CARD_ERROR_CMD38);
  }
  if (waitTimeout(isBusyCMD13)) {
    return sdError(SD_CARD_ERROR_ERASE_TIMEOUT);
  }
  return true;
}

/** Start a write multiple blocks sequence.

    \param[in] blockNumber Address of first block in sequence.
    \param[in] eraseCount The number of blocks to be pre-erased.

    \note This function is used with writeData() and writeStop()
    for optimized multiple block writes.

    \return The value one, true, is returned for success and
    the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::writeStart(uint32_t blockNumber, uint32_t eraseCount)
{
#if SD_PROTECT_BLOCK_ZERO
    // don't allow write to first block
    if (blockNumber == 0)
    {
        error(SD_CARD_ERROR_WRITE_BLOCK_ZERO);
        goto fail;
    }
#endif  // SD_PROTECT_BLOCK_ZERO
    // send pre-erase count
    if (cardAcmd(ACMD23, eraseCount))
    {
        error(SD_CARD_ERROR_ACMD23);
        goto fail;
    }
    // use address if not SDHC card
    if (type() != SD_CARD_TYPE_SDHC)
    {
        blockNumber <<= 9;
    }
    if (cardCommand(CMD25, blockNumber))
    {
        error(SD_CARD_ERROR_CMD25);
        goto fail;
    }
    return true;

fail:
    chipSelectHigh();
    return false;
}

/**
 * Read part of a 512 byte block from a SD card.
 *  
 * \param[in] block Logical block to be read.
 * \param[in] offset Number of bytes to skip at start of block
 * \param[out] dst Pointer to the location that will receive the data. 
 * \param[in] count Number of bytes to read
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.      
 */
uint8_t Sd2Card::readData(uint32_t block, uint16_t offset, uint8_t *dst, uint16_t count)
{
  if (count == 0) return true;
  if ((count + offset) > 512) {
    return false;
  }
  if (!inBlock_ || block != block_ || offset < offset_) {
    block_ = block;
    //use address if not SDHC card
    if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
    if (cardCommand(CMD17, block)) {
      error(SD_CARD_ERROR_CMD17);
      return false;
    }
    if (!waitStartBlock()) {
      return false;
    }
    offset_ = 0;
    inBlock_ = 1;
  }
  
#ifdef OPTIMIZE_HARDWARE_SPI

  //start first spi transfer
  SPDR = 0XFF;
  
  //skip data before offset
  for (;offset_ < offset; offset_++) {
    while(!(SPSR & (1 << SPIF)));
    SPDR = 0XFF;
  }
  
  //transfer data
  uint16_t n = count - 1;
  for (uint16_t i = 0; i < n; i++) {
    while(!(SPSR & (1 << SPIF)));
    dst[i] = SPDR;
    SPDR = 0XFF;
  }
  
  // wait for last byte
  while(!(SPSR & (1 << SPIF)));
  dst[n] = SPDR;
  
#else // OPTIMIZE_HARDWARE_SPI
  
  // skip data before offset
  for (;offset_ < offset; offset_++) {
    spiRec();
  }
  //transfer data
  for (uint16_t i = 0; i < count; i++) {
    dst[i] = spiRec();
  }
#endif // OPTIMIZE_HARDWARE_SPI

  offset_ += count;
  if (!partialBlockRead_ || offset_ >= 512) readEnd();
  return true;
}

//-----------------------------------------------------------------------------
static bool isBusyCMD13() {
  if (!cardCommand(CMD13_XFERTYP, m_rca)) {
    // Caller will timeout.
    return true;
  }
  return !(SDHC_CMDRSP0 & CARD_STATUS_READY_FOR_DATA);
}

/**
 * Reads a 512 byte block from a storage device.
 *  
 * \param[in] blockNumber Logical block to be read.
 * \param[out] dst Pointer to the location that will receive the data. 
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.      
 */
uint8_t SdCard::readBlock(uint32_t blockNumber, uint8_t *dst)
{
  if (cardCommand(CMD17, blockNumber << 9)) {
    error(SD_ERROR_CMD17);
    return false;
  }
  return readTransfer(dst, 512);
}

uint8_t SdCard::readReg(uint8_t cmd, uint8_t *dst)
{
  if (cardCommand(cmd, 0)) {
    spiSSHigh();
    return false;
  }
  return readTransfer(dst, 16);
}

/** read CID or CSR register */
bool Sd2Card::readRegister(uint8_t cmd, void* buf) {
  uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
  if (cardCommand(cmd, 0)) {
    error(SD_CARD_ERROR_READ_REG);
    goto fail;
  }
  return readData(dst, 16);

 fail:
  chipSelectHigh();
  return false;
}

//-----------------------------------------------------------------------------
// Read 16 byte CID or CSD register.
static bool readReg16(uint32_t xfertyp, void* data) {
  uint8_t* d = reinterpret_cast<uint8_t*>(data);
  if (!cardCommand(xfertyp, m_rca)) {
    return false;  // Caller will set errorCode.
  }
  uint32_t sr[] = {SDHC_CMDRSP0, SDHC_CMDRSP1, SDHC_CMDRSP2, SDHC_CMDRSP3};
  for (int i = 0; i < 15; i++) {
    d[14 - i] = sr[i/4] >> 8*(i%4);
  }
  d[15] = 0;
  return true;
}

//------------------------------------------------------------------------------
bool SdSpiCard::readStop() {
  if (cardCommand(CMD12, 0)) {
    error(SD_CARD_ERROR_CMD12);
    goto fail;
  }
  spiStop();
  return true;

fail:
  spiStop();
  return false;
}

/** End a read multiple blocks sequence.
 *
* \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.
 */
bool Sd2Card::readStop() {
  chipSelectLow();
  if (cardCommand(CMD12, 0)) {
    error(SD_CARD_ERROR_CMD12);
    goto fail;
  }
  chipSelectHigh();
  return true;

 fail:
  chipSelectHigh();
  return false;
}

/**
 * Read a 512 byte block from an SD card.
 *
 * \param[in] blockNumber Logical block to be read.
 * \param[out] dst Pointer to the location that will receive the data.
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.
 */
bool Sd2Card::readBlock(uint32_t blockNumber, uint8_t* dst) {
#if ENABLED(SD_CHECK_AND_RETRY)
  uint8_t retryCnt = 3;
  // use address if not SDHC card
  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
 retry2:
  retryCnt --;
  if (cardCommand(CMD17, blockNumber)) {
    error(SD_CARD_ERROR_CMD17);
    if (retryCnt > 0) goto retry;
    goto fail;
  }
  if (!readData(dst, 512))
  {
    if (retryCnt > 0) goto retry;
    goto fail;
  }
  return true;
 retry:
   chipSelectHigh();
   cardCommand(CMD12, 0);//Try sending a stop command, but ignore the result.
   errorCode_ = 0;
   goto retry2;
#else
  // use address if not SDHC card
  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
  if (cardCommand(CMD17, blockNumber)) {
    error(SD_CARD_ERROR_CMD17);
    goto fail;
  }
  return readData(dst, 512);
#endif

 fail:
  chipSelectHigh();
  return false;
}

//------------------------------------------------------------------------------
bool SdSpiCard::writeStart(uint32_t blockNumber) {
  // use address if not SDHC card
  if (type() != SD_CARD_TYPE_SDHC) {
    blockNumber <<= 9;
  }
  if (cardCommand(CMD25, blockNumber)) {
    error(SD_CARD_ERROR_CMD25);
    goto fail;
  }
  return true;

fail:
  spiStop();
  return false;
}

/** read CID or CSR register */
bool SdSpiCard::readRegister(uint8_t cmd, void* buf) {
  uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
  if (cardCommand(cmd, 0)) {
    error(SD_CARD_ERROR_READ_REG);
    goto fail;
  }
  if (!readData(dst, 16)) {
    goto fail;
  }
  spiStop();
  return true;

fail:
  spiStop();
  return false;
}

/**
 * Read part of a 512 byte block from a SD card.
 *  
 * \param[in] block Logical block to be read.
 * \param[in] offset Number of bytes to skip at start of block
 * \param[out] dst Pointer to the location that will receive the data. 
 * \param[in] count Number of bytes to read
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.      
 */
uint8_t SdReader::readData(uint32_t block,
                           uint16_t offset, uint8_t *dst, uint16_t count) {
///Serial.print("readSD, offset="); Serial.print(offset, DEC); Serial.print(", count="); Serial.println(count, DEC);
  if (count == 0) return true;
  if ((count + offset) > 512) {
    return false;
  }
  if (!inBlock_ || block != block_ || offset < offset_) {
    block_ = block;
    
    // use address if not SDHC card
    if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
    if (cardCommand(CMD17, block)) {
      error(SD_CARD_ERROR_CMD17);
      return false;
    }
    if (!waitStartBlock()) {
      return false;
    }
    offset_ = 0;
    inBlock_ = 1;
  }
  
  // start first SPI transfer
  SPDR = 0XFF;
  
  // skip data before offset
  for (;offset_ < offset; offset_++) {
    while(!(SPSR & (1 << SPIF)));
    SPDR = 0XFF;
  }
  
  // transfer data
  uint16_t n = count - 1;
  for (uint16_t i = 0; i < n; i++) {
    while(!(SPSR & (1 << SPIF)));
    dst[i] = SPDR;
    SPDR = 0XFF;
  }
  
  // wait for last byte
  while(!(SPSR & (1 << SPIF)));
  dst[n] = SPDR;
  offset_ += count;
  if (!partialBlockRead_ || offset_ >= 512) readEnd();
  return true;
}

//-----------------------------------------------------------------------------
// SDHC will do Auto CMD12 after count blocks.
bool SdioCard::readStart(uint32_t lba, uint32_t count) {
  DBG_IRQSTAT();
  if (count > 0XFFFF) {
    return sdError(SD_CARD_ERROR_READ_START);
  }
  if (yieldTimeout(isBusyCMD13)) {
    return sdError(SD_CARD_ERROR_CMD13);
  }
  if (count > 1) {
    SDHC_PROCTL |= SDHC_PROCTL_SABGREQ;
  }
  SDHC_BLKATTR = SDHC_BLKATTR_BLKCNT(count) | SDHC_BLKATTR_BLKSIZE(512);
  if (!cardCommand(CMD18_PGM_XFERTYP, m_highCapacity ? lba : 512*lba)) {
    return sdError(SD_CARD_ERROR_CMD18);
  }
  return true;
}