/*                                              -*- mode:C++; fill-column:100 -*-

  SFBSDRaw.cpp - Bit-banged SDCard raw disk access.
  Copyright (C) 2010 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA
*/

#include "SFBSDRaw.h"

#include <cstring>              // For memcpy, memset
#include "SFBAssert.h"          // For API_ASSERT_VALID_FACE, etc
#include "SFBPins.h"            // For FACE_D2_PIN, etc
#include "SFBWiring.h"          // For digitalWrite, etc
#include "SFBPower.h"           // For powerOut
#include "SFBSerial.h"          // For Faces
#include "SFBPrintf.h"          // For logNormal
#include "SFBRandom.h"          // For random()

#define DBGLOG 0
#define SPILOG 0

/* commands available in SPI mode */

#define CMD_GO_IDLE_STATE 0       /* 0x40 CMD0: response R1 */
#define CMD_SEND_OP_COND 1        /* 0x41 CMD1: response R1 */
#define CMD_SEND_IF_COND 8        /* 0x48 CMD8: response R7 */
#define CMD_SEND_CSD 9            /* 0x49 CMD9: response R2 */
#define CMD_SEND_CID 10           /* 0x4A CMD10: response R2 */
#define CMD_STOP_TRANSMISSION 12  /* 0x4C CMD12: response R1 */
#define CMD_SEND_STATUS 13        /* 0x4D CMD13: response R2 */
#define CMD_SET_BLOCKLEN 16       /* 0x50 CMD16: arg0[31:0]: block length, response R1 */
#define CMD_READ_SINGLE_BLOCK 17  /* 0x51 CMD17: arg0[31:0]: data address, response R1 */
#define CMD_READ_MULTIPLE_BLOCK 18/* 0x52 CMD18: arg0[31:0]: data address, response R1 */
#define CMD_WRITE_SINGLE_BLOCK 24 /* 0x58 CMD24: arg0[31:0]: data address, response R1 */
#define CMD_WRITE_MULTIPLE_BLOCK 25 /* 0x59 CMD25: arg0[31:0]: data address, response R1 */
#define CMD_PROGRAM_CSD 27        /* 0x5B CMD27: response R1 */
#define CMD_SET_WRITE_PROT 28     /* 0x5C CMD28: arg0[31:0]: data address, response R1b */
#define CMD_CLR_WRITE_PROT 29     /* 0x5D CMD29: arg0[31:0]: data address, response R1b */
#define CMD_SEND_WRITE_PROT 30    /* 0x5E CMD30: arg0[31:0]: write protect data address, response R1 */
#define CMD_TAG_SECTOR_START 32   /* 0x60 CMD32: arg0[31:0]: data address, response R1 */
#define CMD_TAG_SECTOR_END 33     /* 0x61 CMD33: arg0[31:0]: data address, response R1 */
#define CMD_UNTAG_SECTOR 34       /* 0x62 CMD34: arg0[31:0]: data address, response R1 */
#define CMD_TAG_ERASE_GROUP_START 35 /* 0x63 CMD35: arg0[31:0]: data address, response R1 */
#define CMD_TAG_ERASE_GROUP_END 36 /* 0x64 CMD36: arg0[31:0]: data address, response R1 */
#define CMD_UNTAG_ERASE_GROUP 37  /* 0x65 CMD37: arg0[31:0]: data address, response R1 */
#define CMD_ERASE 38              /* 0x66 CMD38: arg0[31:0]: stuff bits, response R1b */
#define CMD_LOCK_UNLOCK 42        /* 0x6A CMD42: arg0[31:0]: stuff bits, response R1b */
#define CMD_APP_CMD 55            /* 0x77 CMD55: response R1 */
#define CMD_READ_OCR 58           /* 0x7A CMD58: response R3 */
#define CMD_CRC_ON_OFF 59         /* 0x7B CMD59: arg0[31:1]: stuff bits, arg0[0:0]: crc option, response R1 */

#define SD_SEND_OP_COND 41        /* 0x69 ACMD41 */
#define SD_HOST_HCS 0x40000000    /* host capacity support */
#define SD_CARD_CCS_PUS 0xC0000000 /* card capacity status & power up status*/

/* command responses */
/* R1: size 1 byte */
#define R1_IDLE_STATE 0
#define R1_ERASE_RESET 1
#define R1_ILL_COMMAND 2
#define R1_COM_CRC_ERR 3
#define R1_ERASE_SEQ_ERR 4
#define R1_ADDR_ERR 5
#define R1_PARAM_ERR 6
/* R1b: equals R1, additional busy bytes */
/* R2: size 2 bytes */
#define R2_CARD_LOCKED 0
#define R2_WP_ERASE_SKIP 1
#define R2_ERR 2
#define R2_CARD_ERR 3
#define R2_CARD_ECC_FAIL 4
#define R2_WP_VIOLATION 5
#define R2_INVAL_ERASE 6
#define R2_OUT_OF_RANGE 7
#define R2_CSD_OVERWRITE 7
#define R2_IDLE_STATE (R1_IDLE_STATE + 8)
#define R2_ERASE_RESET (R1_ERASE_RESET + 8)
#define R2_ILL_COMMAND (R1_ILL_COMMAND + 8)
#define R2_COM_CRC_ERR (R1_COM_CRC_ERR + 8)
#define R2_ERASE_SEQ_ERR (R1_ERASE_SEQ_ERR + 8)
#define R2_ADDR_ERR (R1_ADDR_ERR + 8)
#define R2_PARAM_ERR (R1_PARAM_ERR + 8)
/* R3: size 5 bytes */
#define R3_OCR_MASK (0xffffffffUL)
#define R3_IDLE_STATE (R1_IDLE_STATE + 32)
#define R3_ERASE_RESET (R1_ERASE_RESET + 32)
#define R3_ILL_COMMAND (R1_ILL_COMMAND + 32)
#define R3_COM_CRC_ERR (R1_COM_CRC_ERR + 32)
#define R3_ERASE_SEQ_ERR (R1_ERASE_SEQ_ERR + 32)
#define R3_ADDR_ERR (R1_ADDR_ERR + 32)
#define R3_PARAM_ERR (R1_PARAM_ERR + 32)
/* Data Response: size 1 byte */
#define DR_STATUS_MASK 0x0e
#define DR_STATUS_ACCEPTED 0x05
#define DR_STATUS_CRC_ERR 0x0a
#define DR_STATUS_WRITE_ERR 0x0c

void SDRaw::begin(u8 face) {
  API_ASSERT_VALID_FACE(face);
  begin(face,
        pinInFace(FACE_D2_PIN,face),  // sdCSel
        pinInFace(FACE_RX_PIN,face),  // sdClock
        pinInFace(FACE_D3_PIN,face),  // sdMOSI
        pinInFace(FACE_TX_PIN,face)); // sdMISO
}

static u8 rxTxOnFace(u8 dueToPin) {
  API_ASSERT_VALID_PIN(dueToPin);

  u8 faceOfPin;
  u8 facePinOfPin;
  bool facePin = isFacePin(dueToPin,&faceOfPin,&facePinOfPin);
  if (facePin && (facePinOfPin==FACE_RX_PIN || facePinOfPin==FACE_TX_PIN))
    return faceOfPin;
  return FACE_COUNT;
}

static void reserveFaceIfNeeded(u8 dueToPin) {
  u32 face = rxTxOnFace(dueToPin);
  if (face != FACE_COUNT) {
    if (DBGLOG) logNormal("SDRaw:reserving %c because of %d\n",FACE_CODE(face),dueToPin);
    Faces[face].reserve();
  }
}

void SDRaw::begin(u8 face, u8 CSPin, u8 clkPin, u8 MOSIPin, u8 MISOPin) {

  API_ASSERT_VALID_FACE(face);

  sdCSel = SFB_UNALIAS_PIN(CSPin); // Get 'real' pin numbers for speed later
  sdClk = SFB_UNALIAS_PIN(clkPin);
  sdMOSI = SFB_UNALIAS_PIN(MOSIPin);
  sdMISO = SFB_UNALIAS_PIN(MISOPin);

  reserveFaceIfNeeded(sdCSel);
  reserveFaceIfNeeded(sdClk);
  reserveFaceIfNeeded(sdMOSI);
  reserveFaceIfNeeded(sdMISO);

  cachedBlock = 0;
  cachedBlockNumber = 0xffffffff; // This is okay until we have a 2TB SD card?
  flags = 0;
  sdFace = face;

  pinMode(sdCSel,OUTPUT);
  pinMode(sdClk,OUTPUT);
  pinMode(sdMOSI,OUTPUT);
  pinMode(sdMISO,INPUT);
  digitalWrite(sdCSel,LOW);
  digitalWrite(sdClk,LOW);
  digitalWrite(sdMOSI,LOW);

  status = UNINITTED;
}

bool SDRaw::end() {
  if (status == NOT_BEGUN) return false;
  status = NOT_BEGUN;
  return true;
}

bool SDRaw::setBlockBuffer(u8 * buffer, u32 bufferLength) {
  API_ASSERT(bufferLength == 512 || ((bufferLength == 0) && (buffer == 0)), E_API_ARGUMENT);
  bool ret = sync();         // Flush if needed before changing buffer
  this->cachedBlock = buffer;
  return ret;
}

bool SDRaw::sync() {
  if (status != ACTIVE) {       // ?? What is the right return here?
    return false;
  }

  if (cachedBlock != 0 && (flags&BLOCK_DIRTY)) { // If buffering and buffer dirty
    flags &= ~BLOCK_DIRTY;                       // Clear dirty bit;
    if (!writeBlock(cachedBlockNumber, cachedBlock)) // Flush the block
      return false;                                 // Disk error (status already set)
  }
  return true;                  // OK.
}

void SDRaw::startTransaction() {
  digitalWrite(sdCSel,LOW);
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Lstart\n");
}

void SDRaw::endTransaction() {
  digitalWrite(sdCSel,HIGH);
  recvByte();                   // Give closing clocks
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Lend\n");
}

void SDRaw::sendBitsSlow(u32 data, u32 bitCount) {
  for (u32 bit = 1<<(bitCount-1); bit; bit>>=1) {
    digitalWrite(sdMOSI,(data&bit)?HIGH:LOW); // Set bit
    digitalWrite(sdClk,HIGH);                 // Clock it
    delayMicroseconds(3);                     // Go way slow
    digitalWrite(sdClk,LOW);                  // Clock it
  }
  digitalWrite(sdMOSI,HIGH); // Leave it high
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Ls>%0*x\n",(bitCount/4),data);

}

#define XX(bitNo)                                                            \
  SFB_PORT_WRITE_BIT_MASK(sendPort,sendMask,(data&(bit>>(bitNo)))?HIGH:LOW); \
  SFB_PORT_WRITE_BIT_MASK(clockPort,clockMask,HIGH);                         \
  SFB_PORT_WRITE_BIT_MASK(clockPort,clockMask,LOW);

void SDRaw::sendBits(u32 data, u32 bitCount) {
  if (status < ACTIVE) {        // Use slow clocking during init
    sendBitsSlow(data, bitCount);
    return;
  }

  u32 clockPort, clockMask;
  u32 sendPort, sendMask;
  SFB_GET_UNALIASED_PIN_PORT_AND_MASK(sdClk,clockPort,clockMask);
  SFB_GET_UNALIASED_PIN_PORT_AND_MASK(sdMOSI,sendPort,sendMask);

  // Without unrolling, this compiles to a six instruction loop, so we
  // could SPI no faster than 12MHz (assuming 72MHz and no stalls).
  // Unrolled eight times it's 43 instructions for an 11% speedup to
  // perhaps 13.4MHz (which is still is plenty under the 25MHz speed
  // limit for SDCard SPI mode.)
  for (u32 bit = 1<<(bitCount-1); bit; bit >>= 8) {
    XX(0);
    XX(1);
    XX(2);
    XX(3);
    XX(4);
    XX(5);
    XX(6);
    XX(7);
  }
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Lf>%0*x\n",bitCount/4,data);
  digitalWrite(sdMOSI,HIGH); // Leave it high

}
#undef XX

u32 SDRaw::recvBitsSlow(u32 bitCount) {
  u32 val = 0;

  for (u32 bit = 1<<(bitCount-1); bit; bit >>= 1) {
    if (digitalRead(sdMISO)==HIGH) val |= bit; // Read line
    digitalWrite(sdClk,HIGH);                 // Clock it
    delayMicroseconds(3);                     // Go way slow
    digitalWrite(sdClk,LOW);                  // Clock it
  }
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Ls<%0*x\n",(bitCount/4),val);
  return val;
}

#if 1
u32 SDRaw::recvBits(u32 bitCount) {

  digitalWrite(sdMOSI,HIGH); // Ensure we're sending 1's while we're reading
  if (status < ACTIVE)
    return recvBitsSlow(bitCount);

  u32 clockPort, clockMask;
  u32 recvPort, recvMask;
  SFB_GET_UNALIASED_PIN_PORT_AND_MASK(sdClk,clockPort,clockMask);
  SFB_GET_UNALIASED_PIN_PORT_AND_MASK(sdMISO,recvPort,recvMask);

  u32 val = 0;
  for (u32 bit = 1<<(bitCount-1); bit; bit>>=1) {
    if (SFB_PORT_GET_BIT_MASK(recvPort,recvMask)) val |= bit;
    SFB_PORT_WRITE_BIT_MASK(clockPort,clockMask,HIGH); // Clock it
    SFB_PORT_WRITE_BIT_MASK(clockPort,clockMask,LOW);
  }
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Lf<%0*x\n",bitCount/4,val);

  return val;
}

#else

// Tue Apr  6 10:04:49 2010 Ackley: 
// Reading fully-unrolled and maxed out is unreliable!  Getting CRC failures!  :(  
// Don't know why!  :(  Going with the slighly slower rolled-up loop above..

#define XX(bitNo)                                                            \
  if (SFB_PORT_GET_BIT_MASK(recvPort,recvMask)) val |= bit>>(bitNo);         \
  SFB_PORT_WRITE_BIT_MASK(clockPort,clockMask,HIGH);                         \
  SFB_PORT_WRITE_BIT_MASK(clockPort,clockMask,LOW);                          \

u32 SDRaw::recvBits(u32 bitCount) {

  digitalWrite(sdMOSI,HIGH); // Ensure we're sending 1's while we're reading
  if (status < ACTIVE)
    return recvBitsSlow(bitCount);

  API_ASSERT_ZERO(bitCount&0x7);

  u32 clockPort, clockMask;
  u32 recvPort, recvMask;
  SFB_GET_UNALIASED_PIN_PORT_AND_MASK(sdClk,clockPort,clockMask);
  SFB_GET_UNALIASED_PIN_PORT_AND_MASK(sdMISO,recvPort,recvMask);

  u32 val = 0;
  for (u32 bit = 1<<(bitCount-1); bit; bit>>=8) {
    XX(0);
    XX(1);
    XX(2);
    XX(3);
    XX(4);
    XX(5);
    XX(6);
    XX(7);
  }
  if (flags&SPI_LOG)
    facePrintf(getLogFace(),"Lf<%0*x\n",bitCount/4,val);

  return val;
}
#undef XX
#endif

u32 SDRaw::readStatus() {

  startTransaction();
  u8 byte1 = sendCmd(CMD_SEND_STATUS,0);
  u8 byte2 = recvByte();
  endTransaction();

  return (((u32) byte1)<<8)|byte2;
}

u16 crc16(u16 crc, u8 data) {
  crc  = (u8) (crc >> 8) | (crc << 8);
  crc ^= data;
  crc ^= (u8) (crc & 0xff) >> 4;
  crc ^= crc << 12;
  crc ^= (crc & 0xff) << 5;
  return crc;
}

u16 crc16Block(const u8 *p, u32 len) {
  u16 crc = 0;

  while (len--) 
    crc = crc16(crc, *p++);
  
  return crc;
}


static u8 crc7(u8 crc, u32 bits, u32 n) {
  for (u32 bit = 1<<(n-1); bit; bit >>= 1) {
    crc = (crc<<1) + ((bits&bit) ? 1 : 0);
    if (crc & 0x80)             // Feedback
      crc ^= 0x9;
  }
  return crc & 0x7f;           
}

u8 SDRaw::sendCmd(u8 cmd, u32 arg) {

  // First, feed clocks until card is bored
  for (u32 i = 0; true; ++i) {
    if (recvByte() == 0Xff) break; // OK, it's bored.
    if (i >= SHORT_TRIES)         // Something's wrong..
      return 0xf0;              // Return not idle, not zero, try to kill caller
  }

  cmd |= 0x40;                  // Add flag bit

  u8 crc = 0;                   // Init CRC7
  crc = crc7(crc, cmd, 8);      // Cmd byte with flag bit
  crc = crc7(crc, arg, 32);     // Four arg bytes
  crc = crc7(crc, 0, 7);        // Official trailing seven zeroes
  crc = (crc<<1)|1;             // Shift into position and add trailing 1 bit

  if (DBGLOG) logNormal("CMD%d(%d)->%02x,%d\n",cmd,arg,crc,crc);

  sendBits(cmd, 8);   // Send the cmd
  sendBits(arg,32);   // Writes all four bytes from left ((arg>>24)&0xff) to right (>>0&0xff)
  sendByte(crc);

  u8 response;
  for(u8 i = 0; true; ++i) {
    response = recvByte();
    if ((response&0x80)==0)     // Need to see 0 MSB for R1 response
      break;
    if (i >= SHORT_TRIES)         // Giving up.
      return 0xf0;              // Return not idle, not zero, try to kill caller
  }

  return response;
}

bool SDRaw::readBlock(u32 blockNumber, u8* block, u32 blockOffset, u32 length) {

  u32 arg = (flags&HC_CARD)?blockNumber:(blockNumber<<SDRAW_BLOCK_SIZE_BIT_COUNT);

  startTransaction();
  if (sendCmd(CMD_READ_SINGLE_BLOCK, arg)) 
    return dieWith(ERROR_READ_SINGLE_BLOCK);

  /* Wait for data block start byte */
  for (u32 i = 0; true; ++i) {
    if (recvByte() == 0xfe) break;
    if (i >= LONG_TRIES)
      return dieWith(ERROR_READ_TIMEOUT_DATA);
  }

  /* Now read and checksum block, store what we want */
  u16 crc = 0;
  u32 last = blockOffset+length;
  for(u32 i = 0; i < SDRAW_BLOCK_SIZE; ++i) {
    u8 ch = recvByte();
    crc = crc16(crc,ch);
    if (i >= blockOffset && i < last)
      *block++ = ch;
  }

  /* Read CRC16 */
  u16 gotcrc = recvBits(16);

  /* Deaddress card */
  endTransaction();

  if (gotcrc != crc) {
    logNormal("Bad CRC got (%04d) wanted (%04d)\n",gotcrc,crc);
    return dieWith(ERROR_READ_CRC_DATA);
  }

  return true;
}

bool SDRaw::readBuffered(u64 offset, u8* buffer, u32 length) {
  API_ASSERT_NONNULL(cachedBlock);

  if (status != ACTIVE) {
    status = INACTIVE_READ;
    return false;
  }

  u32 blockNumber = (u32) (offset >> SDRAW_BLOCK_SIZE_BIT_COUNT);
  u32 blockOffset = (u32) (offset & SDRAW_BLOCK_SIZE_BIT_MASK);
  u32 readLength = SDRAW_BLOCK_SIZE - blockOffset;

  while (length > 0) {

    if (readLength > length)
      readLength = length;

    if (!loadCache(blockNumber))
      return false;                // Code already set.

    memcpy(buffer, cachedBlock + blockOffset, readLength);

    length -= readLength;          // OK, we read that much
    buffer += readLength;          // And that part of buffer is full
    readLength = SDRAW_BLOCK_SIZE; // Now we'll read full blocks (at least til the last)
    blockOffset = 0;               // And we're reading from block start
    ++blockNumber;
  }

  return true;
}


bool SDRaw::read(u64 offset, u8* buffer, u32 length) {
  API_ASSERT(buffer != 0 || length == 0, E_API_ARGUMENT);

  if (status != ACTIVE) {
    status = INACTIVE_READ;
    return false;
  }

  if (cachedBlock)
    return readBuffered(offset,buffer,length);

  // Unbuffered read here

  u32 blockNumber = (u32) (offset >> SDRAW_BLOCK_SIZE_BIT_COUNT);
  u32 blockOffset = (u32) (offset & SDRAW_BLOCK_SIZE_BIT_MASK);
  u32 readLength = SDRAW_BLOCK_SIZE - blockOffset;

  while (length > 0) {

    if (readLength > length)
      readLength = length;

    if (!readBlock(blockNumber, buffer, blockOffset, readLength))
      return false;             // Code already set.

    length -= readLength;          // OK, we read that much
    buffer += readLength;
    readLength = SDRAW_BLOCK_SIZE; // Now we'll read full blocks (at least til the last)
    blockOffset = 0;               // And we're reading from block start
    ++blockNumber;
  }

  return true;
}

bool SDRaw::writeBlock(u32 blockNumber, const u8 * block) {
  API_ASSERT_NONNULL(block);
  if (DBGLOG) logNormal("writeBlock %d\n",blockNumber);
  if (SPILOG) setSpiLog(true);
  startTransaction();
  u32 arg = (flags&HC_CARD) ? blockNumber : blockNumber<<SDRAW_BLOCK_SIZE_BIT_COUNT;
  if (sendCmd(CMD_WRITE_SINGLE_BLOCK,arg)) 
    return dieWith(ERROR_WRITE_SINGLE_BLOCK);

  if (SPILOG) setSpiLog(false);

  /* send start byte */
  sendByte(0xfe);

  u16 crc = crc16Block(block,SDRAW_BLOCK_SIZE);

  /* write block, packed into words -- COULD DO BETTER HERE, with u32 and byte swapping? */
  u32 word = 0;
  u32 shift = 32;
  for(u32 i = 0; i < SDRAW_BLOCK_SIZE; ++i) {
    shift -= 8;
    word |= *block++<<shift;
    if (shift==0) {
      sendBits(word,32);
      word = 0;
      shift = 32;
    }
  }

  /* write crc16 */
  sendBits(crc,16);

  /* wait while card is busy */
  for (u32 i = 0; true; ++i) {
    if (recvByte() == 0xff) break;
    if (i > LONG_TRIES)
      return dieWith(ERROR_WRITE_TIMEOUT_DATA);
  }

  /* deaddress card */
  endTransaction();

  return true;
}

bool SDRaw::write(u64 offset, const u8 * buffer, u32 length) {
  if (status != ACTIVE) {
    status = INACTIVE_WRITE;
    return false;
  }

  if (cachedBlock)
    return writeBuffered(offset,buffer,length);

  API_ASSERT((offset&SDRAW_BLOCK_SIZE_BIT_MASK)==0, E_API_ILLEGAL_STATE); // Then even blocks
  API_ASSERT((length&SDRAW_BLOCK_SIZE_BIT_MASK)==0, E_API_ILLEGAL_STATE); // And full blocks

  u32 blockNumber = (u32) (offset>>SDRAW_BLOCK_SIZE_BIT_COUNT);
  while (length) {
    if (!writeBlock(blockNumber, buffer)) {
      // Status already set
      return false;
    }
    ++blockNumber;
    buffer += SDRAW_BLOCK_SIZE;
    length -= SDRAW_BLOCK_SIZE;
  }
  return true;
}

bool SDRaw::loadCache(u32 blockNumber) {
  API_ASSERT_NONNULL(cachedBlock);

  // If cache hits, yay.
  if (blockNumber == cachedBlockNumber)
    return true;

  // Else flush it
  if (!sync()) {
    // status already set
    return false;
  }
  cachedBlockNumber = blockNumber;
  return readBlock(blockNumber,cachedBlock);
}

bool SDRaw::writeBuffered(u64 offset, const u8 * buffer, u32 length) {
  API_ASSERT_NONNULL(cachedBlock);
  if (length)
    API_ASSERT_NONNULL(buffer);

  u32 blockNumber = (u32) (offset >> SDRAW_BLOCK_SIZE_BIT_COUNT);
  u32 blockOffset = (u32) (offset & SDRAW_BLOCK_SIZE_BIT_MASK);
  u32 writeLength = SDRAW_BLOCK_SIZE - blockOffset; /* write up to block border */;

  while (length > 0) {

    /* determine byte count to write at once */
    if(writeLength > length)
      writeLength = length;

    if (DBGLOG)
      logNormal("Writing %d to block %d offset %d\n",
                writeLength, blockNumber, blockOffset);

    // If writing a partial block, we need the prior block content
    if (blockOffset != 0 || writeLength < SDRAW_BLOCK_SIZE) {
      if (!loadCache(blockNumber))        // So load that
        return false;                     // Foo, it died.
    } else {                              // Otherwise we need to use the whole writeBuffer
      if (!sync())                        // So make sure it's available
        return false;                     // Unless that dies..
      cachedBlockNumber = blockNumber;    // Record it's (about to be) holding our new block
    }

    // Copy data into the write buffer and mark it
    memcpy(cachedBlock + blockOffset, buffer, writeLength);
    flags |= BLOCK_DIRTY;

    if (DBGLOG)
      logNormal("Copied %d @ %d (end %d), flags = %d\n",
                writeLength, blockOffset, writeLength+blockOffset, flags);

    // Move on
    buffer += writeLength;
    length -= writeLength;
    writeLength =  SDRAW_BLOCK_SIZE;      // Now do full blocks until perhaps the last
    blockOffset = 0;                      // So, starting at the top of the block..
    ++blockNumber;
  }

  if (DBGLOG)
    logNormal("Writing ended, flags=%d cachedBlockNumber=%d\n",
              flags, cachedBlockNumber);

  return true;
}

bool SDRaw::dieWith(u32 newStatus) {
  status = newStatus;
  endTransaction();
  if (DBGLOG) logNormal("dieWith(%d)\n",status);
  return false;
}

bool SDRaw::init() {

  status = UNINITTED;

  /* Power cycle the card */

  digitalWrite(sdCSel,LOW);   // All output pins low during power cycle
  digitalWrite(sdClk,LOW);
  digitalWrite(sdMOSI,LOW);

  powerOut(sdFace,0);         // Clean start
  delay(200);                 // Cool things off.  WARNING: 150ms here is NOT ENOUGH!

  endTransaction();           // Just to be sure
  powerOut(sdFace,1);         // Now power up card
  for(u32 i = 0; i<10; i++)   // Kill time (> 74 clocks) after power up
    recvByte();

  /* Reset card */
  u8 response;

  startTransaction();
  for(u32 i = 0; true; ++i)  {
    response = sendCmd(CMD_GO_IDLE_STATE, 0);
    if (response == (1 << R1_IDLE_STATE)) break;
    if (i >= 1) return dieWith(RESET_FAILED);
  }

  /* Enable CRC */
  response = sendCmd(CMD_CRC_ON_OFF, 1);
  if (response != (1 << R1_IDLE_STATE)) 
    return dieWith(CRC_ON_FAILED);

  /* See if card is SD version 2 */
  bool v2Card = false;            // Assume it's not
  u32 vcheck = 0x100|random(256); // 0x100: 2.6-3.3V, plus random check pattern

  if ((sendCmd(CMD_SEND_IF_COND,vcheck) & (1<<R1_ILL_COMMAND))==0) { // If handles CMD8..
    if ((recvBits(32)&0x1ff) != vcheck)  // Check accepted voltage and check pattern
      return dieWith(TYPE_CHECK_FAILED);
    v2Card = true;
  } /* else not v2..  No HC for you */

  /* Boot the card into transfer mode.  This is the slow step. */
  for(u32 i = 0; true; ++i)  {

    sendCmd(CMD_APP_CMD,0);
    response = sendCmd(SD_SEND_OP_COND, v2Card ? SD_HOST_HCS : 0); // We support SDHC if v2..
    if ((response == (1 << R1_IDLE_STATE))==0) // Need a backwards non-idle result to pass here
      break;
    if (i >= SHORT_TRIES) return dieWith(OP_COND_FAILED); // short since we have a delay 

    delay(25);    // Wait a good while. (But must poll at < 50ms, due to P1PLSSV2-4.4)
  }

  /* If v2, read OCR register to check for SDHC card */
  if (v2Card) {
    if (sendCmd(CMD_READ_OCR, 0)) return dieWith(READ_OCR_FAILED);
    if ((recvBits(32) & SD_CARD_CCS_PUS) == SD_CARD_CCS_PUS)
      flags |= HC_CARD;
  }

  /* Set block size to 512 bytes */
  if (sendCmd(CMD_SET_BLOCKLEN, 512) & ~(1<<R1_IDLE_STATE)) 
    return dieWith(CONFIG_FAILED);

  /* All done! */
  endTransaction();

  status = ACTIVE;

  /* Preload disk size.  Too important not to have when ACTIVE. */
  {
    SDRawDiskInfo info;
    if (!getInfo(info)) return false; // Status already set
    blockCapacity = info.blockCount;
  }

  return true;
}

bool SDRaw::readRegister(u32 cmdRegister, u8 reg[16]) {

  if (!isActive()) return false;

  startTransaction();

  /* read specified register */
  if (sendCmd(cmdRegister, 0)) {
    status = ERROR_READ_REGISTER;
    endTransaction();
    return false;
  }

  // Block for data start

  for (u32 i = 0; true; ++i) {
    if (recvByte() == 0xfe)
      break;
    if (i >= LONG_TRIES) 
      return dieWith(ERROR_READ_TIMEOUT_DATA);
  }

  memset(reg, 0, 16);

  for (u32 i = 0; i < 16; ++i)
    reg[i] = recvByte();

  // crc 'internal' to reg?  recvBits(16);                 // Blow off the crc

  endTransaction();
  return true;
}

static bool unpackCSDv1(u8 reg[16], SDRawDiskInfo & info) {
  u32 csdVersion = ((reg[0]>>6)&0x3)+1;
  if (csdVersion != 1) return false;

  u32 csd_read_bl_len;
  csd_read_bl_len = reg[5] & 0x0f;  

  u32 csd_c_size;
  csd_c_size = (reg[6] & 0x03) << 8;
  csd_c_size |= reg[7];
  csd_c_size <<= 2;
  csd_c_size |= reg[8] >> 6;
  ++csd_c_size;

  u32 csd_c_size_mult;
  csd_c_size_mult = (reg[9] & 0x03) << 1;
  csd_c_size_mult |= reg[10] >> 7;

  u32 byteExponent = (csd_c_size_mult + csd_read_bl_len + 2);
  u32 blockExponent = byteExponent-9;

  info.blockCount = csd_c_size << blockExponent;

  if (DBGLOG)
    logNormal("csize=%d, cmult=%d, readbl=%d, blocks=%d bytes=%d000\n",
            csd_c_size,csd_c_size_mult, csd_read_bl_len, info.blockCount,info.blockCount/2);

  if(reg[14] & 0x40)
    info.copy = true;
  if(reg[14] & 0x20)
    info.writeProtect = true;
  if(reg[14] & 0x10)
    info.writeProtectTemporary = 1;

  info.format = (reg[14] & 0x0c) >> 2;

  return true;
}

/*
  CSD v2 as bytes/bits: .=ignored bit, r=reserved bit
 reg[0] =  01rrrrrr  v=version
 reg[1] =  ........  (TAAC)
 reg[2] =  ........  (NSAC)
 reg[3] =  ........  (TRAN_SPEED)
 reg[4] =  cccccccc  c=CCC [high 8 bits]
 reg[5] =  cccc....  c=CCC [low 4 bits]    (READ_BL_LEN)
 reg[6] =  ...drrrr  (various flags) d=DSR_IMP
 reg[7] =  rrssssss  s=C_SIZE [high 6 bits]
 reg[8] =  ssssssss  s=C_SIZE [middle 8 bits]
 reg[9] =  ssssssss  s=C_SIZE [bottom 8 bits]
 reg[10]=  r.......  (ERASE_BLK_EN, SECTOR_SIZE)
 reg[11]=  ........  (SECTOR_SIZE, WP_GRP_SIZE)
 reg[12]=  .rr.....  (WP_GRP_ENABLE, R2W_FACTOR, WRITE_BL_LEN)
 reg[13]=  ...rrrrr  (WRITE_BL_LEN, WRITE_BL_PARTIAL)
 reg[14]=  .cpt..rr  c=COPY, p=PERM_WRITE_PROTECT, t=TMP_WRITE_PROTECT
 reg[15]=  ccccccc1  c=CRC7
 */
static bool unpackCSDv2(u8 reg[16], SDRawDiskInfo & info) {
  u32 csdVersion = ((reg[0]>>6)&0x3)+1;
  if (csdVersion != 2) return false;

  u32 cSize;
  cSize = reg[7]&0x3f;
  cSize = (cSize<<8) | reg[8];
  cSize = (cSize<<8) | reg[9];
  ++cSize;
  cSize *= 1000; // Dammit, or is it 1024 here?
  info.blockCount = cSize;
  info.copy = ((reg[14]&0x40) != 0);
  info.writeProtect = ((reg[14]&0x20) != 0);
  info.writeProtectTemporary = ((reg[14]&0x10) != 0);
  info.format = (reg[14] & 0x0c) >> 2;

  return true;
}

bool SDRaw::getInfo(SDRawDiskInfo & info) {

  u8 reg[16];

  if (!readRegister(CMD_SEND_CSD,reg)) return false;

  memset(&info, 0, sizeof(info));

  if (!unpackCSDv1(reg,info) && !unpackCSDv2(reg,info)) 
    return dieWith(ERROR_BAD_CSD_VERSION);

  if (!readRegister(CMD_SEND_CID,reg)) return false;

  for(u32 i = 0; i < sizeof(reg); ++i) {
    u8 b = reg[i];

    switch(i) {
    case 0:
      info.manufacturer = b;
      break;

    case 1: case 2:
      info.oem[i - 1] = b;
      break;

    case 3: case 4: case 5: case 6: case 7:
      info.product[i - 3] = b;
      break;

    case 8:
      info.revision = b;
      break;

    case 9: case 10: case 11: case 12:
      info.serialNumber |= (u32) b << ((12 - i) * 8);
      break;

    case 13:
      info.year = b << 4;
      break;

    case 14:
      info.year |= b >> 4;
      info.month = b & 0x0f;
      break;
    }
  }

  return true;
}
