//------------------------------------------------------
// SD Card Interface: XJEase dveice file
// (c)2004-2005 XJTAG Limited
//
// Disclaimer: XJTAG makes no guarantees whatsoever
// about this code.  You use it at your own risk ...
// This code requires XJTAG version 1.3 or later.
//------------------------------------------------------

DEVICE NAME := "SD Card"

  PINS
    // SD Card mode
    DATA        := 1, 9, 8, 7;
    CMD         := 2;
    CLK         := 5;
    nCD         := 10;
    WP          := 11;
    nVDD_ENABLE := U17.2;    // Power enable (modify pin as required)
    // SPI Mode
    SPI_DOUT    := 7;
    SPI_DIN     := 2;
    SPI_CLK     := 5;
    SPI_NCS     := 1;
  END;

  DISABLE DEVICE
    nVDD_ENABLE := 1;
  END;

  TEST COVERAGE
    nCD := READ;
    CLK := WRITE;
    CMD := BIDIR;
    nVDD_ENABLE := WRITE;
    DATA := BIDIR;
  END;

END;

//--------------------------------------------------------------------------------
// Constants
//--------------------------------------------------------------------------------

// SD Commands
CONST INT GO_IDLE_STATE         := 0;
CONST INT ALL_SEND_CID          := 2;
CONST INT SEND_RELATIVE_ADDR    := 3;
CONST INT SET_BUS_WIDTH         := 6;
CONST INT SELECT_CARD           := 7;
CONST INT DESELECT_CARD         := 7;
CONST INT SEND_CSD              := 9;
CONST INT SEND_CID              := 10;
CONST INT SET_BLOCKLEN          := 16;
CONST INT READ_SINGLE_BLOCK     := 17;
CONST INT READ_MULTIPLE_BLOCK   := 18;
CONST INT APP_CMD               := 55;
CONST INT SD_APP_OP_COND        := 41;
CONST INT SET_CLR_CARD_DETECT   := 42;

CONST INT MY_OCR                := 0xff8000; // 2.8 to 3.6 V
CONST INT BLOCK_LENGTH_BYTES    := 512;      // 512 Max
CONST INT BLOCK_LENGTH_BITS     := (BLOCK_LENGTH_BYTES * 8);
CONST INT READ_DATA_WIDTH       := 4;        // Only 1 or 4 allowed
CONST INT INSERT_REMOVE_TIMEOUT := 10;

CONST INT RESULT_PASS WIDTH 1   := FALSE;
CONST INT RESULT_FAIL WIDTH 1   := TRUE;
CONST INT DEBUG                 := FALSE;

//--------------------------------------------------------------------------------
Test()(INT result WIDTH 1)
//--------------------------------------------------------------------------------

  INT crc, i;
  INT ocr    WIDTH 32;
  INT cid    WIDTH 120;
  INT csd    WIDTH 120;
  INT rca    WIDTH 16;
  INT cmd    WIDTH 6;
  INT status WIDTH 32;
  INT data;

  PRINT("Testing Secure Digital (SD) Card\n");

  InsertCard(INSERT_REMOVE_TIMEOUT)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card insertion test failed (0)\n"); RETURN; END;

  // Reset card to the idle state
  SendCommand(GO_IDLE_STATE, 0)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (1)\n"); RETURN; END;

  // Wait for card not busy
  DO
    // Read OCR
    SendCommand(APP_CMD, 0)(result);
    IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (2)\n"); RETURN; END;
    GetResponse_R1()(result, cmd, status);
    IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (3)\n"); RETURN; END;
    DecodeCardStatus(status)(result);
    SendCommand(SD_APP_OP_COND, 0)(result);
    IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (4)\n"); RETURN; END;
    GetResponse_R3()(result, ocr);
    IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (5)\n"); RETURN; END;
  WHILE (!ocr[31]) END;

  // Write my OCR to the card, move to ready state
  SendCommand(APP_CMD, 0)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (6)\n"); RETURN; END;
  GetResponse_R1()(result, cmd, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (7)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (8)\n"); RETURN; END;
  SendCommand(SD_APP_OP_COND, MY_OCR)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (9)\n"); RETURN; END;
  GetResponse_R3()(result, ocr);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (10)\n"); RETURN; END;

  // Read card id register (CID), move to ident state
  SendCommand(ALL_SEND_CID, 0)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (11)\n"); RETURN; END;
  GetResponse_R2()(result, cid);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (12)\n"); RETURN; END;
  DecodeCID(cid)();
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (13)\n"); RETURN; END;

  // Read relative address (RCA) from card, move to stby state
  SendCommand(SEND_RELATIVE_ADDR, 0)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (14)\n"); RETURN; END;
  GetResponse_R6()(result, rca, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (15)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (16)\n"); RETURN; END;

  // Read card specific data (CSD) from RCA address, remain in stby state
  SendCommand(SEND_CSD, rca[15..0]:0[15..0])(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (17)\n"); RETURN; END;
  GetResponse_R2()(result, csd);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (18)\n"); RETURN; END;
  DecodeCSD(csd)();
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (19)\n"); RETURN; END;


  // Read card id from RCA address, remain in stby state
  SendCommand(SEND_CID, rca[15..0]:0[15..0])(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (20)\n"); RETURN; END;
  GetResponse_R2()(result, cid);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (21)\n"); RETURN; END;
  DecodeCID(cid)();
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (22)\n"); RETURN; END;

  // Select the card & move to trans state
  SendCommand(SELECT_CARD, rca[15..0]:0[15..0])(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (23)\n"); RETURN; END;
  GetResponse_R1()(result, cmd, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (24)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (25)\n"); RETURN; END;

  // Set the read block length
  SendCommand(SET_BLOCKLEN, BLOCK_LENGTH_BYTES)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (26)\n"); RETURN; END;
  GetResponse_R1()(result, cmd, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (27)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (28)\n"); RETURN; END;

  // Set the data bus width
  SendCommand(APP_CMD, rca[15..0]:0[15..0])(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (29)\n"); RETURN; END;
  GetResponse_R1()(result, cmd, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (30)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (31)\n"); RETURN; END;
  SendCommand(SET_BUS_WIDTH, 2*(READ_DATA_WIDTH-1))(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (32)\n"); RETURN; END;
  GetResponse_R1()(result, cmd, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (33)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (34)\n"); RETURN; END;

  // Read a block of data from the card
  ReadBlock(0)(result, data, status);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (35)\n"); RETURN; END;
  DecodeCardStatus(status)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (36)\n"); RETURN; END;

  // Reset to idle state
  SendCommand(GO_IDLE_STATE, 0)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card Tests failed (37)\n"); RETURN; END;

  //  Power down and Wait for card removal
  RemoveCard(INSERT_REMOVE_TIMEOUT)(result);
  IF (result != RESULT_PASS) THEN PRINT("SD Card removal test failed (38)\n"); RETURN; END;

  PRINT("SD Card Tests Completed OK\n");
  result := RESULT_PASS;
  RETURN;

END;

//--------------------------------------------------------------------------------
DecodeCardStatus(INT status WIDTH 32)(INT result)
//--------------------------------------------------------------------------------

  result := RESULT_PASS;

  IF (DEBUG) THEN
    PRINT("Status : 0x", HEX(status), "\n");

    IF (status[31]) THEN PRINT("\tCommand error or parameter out of range\n"); END;
    IF (status[30]) THEN PRINT("\tAddress error (read/write crosses block boundary)\n"); END;
    IF (status[29]) THEN PRINT("\tBlock length error\n"); END;
    IF (status[28]) THEN PRINT("\tErase sequence error\n"); END;
    IF (status[27]) THEN PRINT("\tErase parameter error\n"); END;
    IF (status[26]) THEN PRINT("\tWrite protect violation\n"); END;
    IF (status[25]) THEN PRINT("\tCard is locked\n"); END;
    IF (status[24]) THEN PRINT("\tLock unlock failed\n"); END;
    IF (status[23]) THEN PRINT("\tCom CRC error\n"); END;
    IF (status[22]) THEN PRINT("\tIllegal Command\n"); END;
    IF (status[21]) THEN PRINT("\tCard ECC failed\n"); END;
    IF (status[20]) THEN PRINT("\tCard controller error\n"); END;
    IF (status[19]) THEN PRINT("\tGeneral error\n"); END;
    IF (status[16]) THEN PRINT("\tCID or CSD overwrite error\n"); END;
    IF (status[15]) THEN PRINT("\tWrite protect caused erase skip\n"); END;
    IF (status[14]) THEN PRINT("\tCard ECC is disabled\n"); END;
    IF (status[13]) THEN PRINT("\tErase reset\n"); END;
    IF (status[08]) THEN PRINT("\tReady for data, buffer empty\n"); END;
    IF (status[05]) THEN PRINT("\tACMD expected or received\n"); END;
    IF (status[03]) THEN PRINT("\tAuthentication sequence error\n"); END;

    IF (status[12..9] = 0) THEN PRINT("\tState was Idle\n");  END;
    IF (status[12..9] = 1) THEN PRINT("\tState was Ready\n"); END;
    IF (status[12..9] = 2) THEN PRINT("\tState was Ident\n"); END;
    IF (status[12..9] = 3) THEN PRINT("\tState was Stby\n");  END;
    IF (status[12..9] = 4) THEN PRINT("\tState was Tran\n");  END;
    IF (status[12..9] = 5) THEN PRINT("\tState was Data\n");  END;
    IF (status[12..9] = 6) THEN PRINT("\tState was Rcv\n");   END;
    IF (status[12..9] = 7) THEN PRINT("\tState was Prog\n");  END;
    IF (status[12..9] = 8) THEN PRINT("\tState was Dis\n");   END;
  END;

  IF (status[3] || status[31..15]) THEN
    PRINT("\tCard Status Error\n");
    result := RESULT_FAIL;
  END;

END;

//--------------------------------------------------------------------------------
DecodeCID(INT cid WIDTH 120)()
//--------------------------------------------------------------------------------

  PRINT("Card ID :\n");
  IF (DEBUG) THEN PRINT("ID : 0x", HEX(cid), "\n"); END;

  PRINT("\tManufacturer Code  : 0x", HEX(cid[119..112]), "\n");
  PRINT("\tOEM/Application ID : 0x", HEX(cid[111..96]),  "\n");
  PRINT("\tProduct Name       : 0x", HEX(cid[95..56]),   "\n");
  PRINT("\tProduct Revision   : 0x", HEX(cid[55..48]),   "\n");
  PRINT("\tSerial Number      : 0x", HEX(cid[47..16]),   "\n");
  PRINT("\tManuf date code    : ", (2000+cid[11..4]), "/", cid[3..0], "\n");

END;

//--------------------------------------------------------------------------------
DecodeCSD(INT csd WIDTH 120)()
//--------------------------------------------------------------------------------

  INT blk, size, mult, format;

  PRINT("Card Specific Data : \n");
  IF DEBUG THEN PRINT("CSD : 0x", HEX(csd), "\n"); END;

  blk  := 1 << (csd[75..72]);
  mult := 1 << (2 + csd[41..39]);
  size := 1 + (csd[65..54]);
  format := csd[3..2];

  PRINT("\tMax read block len : ", blk, " bytes\n");
  PRINT("\tDevice Size        : ", size, " blocks\n");
  PRINT("\tDevice Size multpl : ", mult, "\n");
  PRINT("\tCard capacity      : ", (size * blk * mult), " bytes\n");
  PRINT("\tFile Format        : ");
  IF (format = 0) THEN PRINT ("Hard-disk (with partition table)\n"); END;
  IF (format = 1) THEN PRINT ("DOS FAT (without partition table)\n"); END;
  IF (format = 2) THEN PRINT ("Universal\n"); END;
  IF (format = 3) THEN PRINT ("Others/Unknown\n"); END;

END;

//--------------------------------------------------------------------------------
InsertCard(INT timeout)(INT result)
//--------------------------------------------------------------------------------
  INT not_detect WIDTH 1;
  INT last_time;

  result := RESULT_PASS;

  SET not_detect := nCD;
  timeout := NOW() + (timeout * 1000);
  IF (not_detect) THEN
    PRINT("Insert SD card, or press a key to abort.\n");
    last_time := NOW()/1000;
    DO
      SET not_detect := nCD;
      IF (NOW() / 1000 != last_time) THEN last_time := NOW()/1000; PRINT("\a"); END;
    WHILE (!GETKEY() && not_detect && (NOW() < timeout)) END;
    IF (not_detect) THEN
      result := RESULT_FAIL;
    END;
  END;

  IF (result = RESULT_PASS) THEN
    // Switch on power and enable (in SD Mode)
    SET nVDD_ENABLE := 0;
    SET CLK     := 0;
    SET CMD     := I;
    SET DATA    := I;
    SET SPI_NCS := 1;

    // Wait for card to start up.
    SLEEP(100);

    // Clocks required prior to 1st command
    Clocks(256);
  END;

END;

//--------------------------------------------------------------------------------
RemoveCard(INT timeout)(INT result)
//--------------------------------------------------------------------------------
  INT not_detect WIDTH 1;
  INT last_time;

  // Ensure all operations have completed
  Clocks(128);

  // Turn off power and disable
  SET nVDD_ENABLE := 1;
  SET CLK := 0;
  SET CMD := I;

  // Wait for removal
  SET not_detect := nCD;
  timeout := NOW() + (timeout * 1000);
  IF (not_detect) THEN
    result := RESULT_PASS;
  ELSE
    PRINT("Remove SD card, or press a key to abort.\n");
    last_time := NOW()/1000;
    DO
      SET not_detect := nCD;
      IF (NOW() / 1000 != last_time) THEN last_time := NOW()/1000; PRINT("\a"); END;
    WHILE (!GETKEY() && !not_detect && (NOW() < timeout)) END;
    IF (not_detect) THEN
      result := RESULT_PASS;
    ELSE
      result := RESULT_FAIL;
    END;
  END;

END;

//--------------------------------------------------------------------------------
SendCommand(INT command WIDTH 6, INT param WIDTH 32)(INT result)
//--------------------------------------------------------------------------------
  INT crc        WIDTH 7;
  INT data       WIDTH 48;
  INT i          WIDTH 8;
  INT crc_width  WIDTH 8;

  IF (DEBUG) THEN PRINT("\n"); END;

  crc_width := WIDTHOF(data[47..8]);

  data[47]     := 0;                         // Start flag
  data[46]     := 1;                         // Tx flag
  data[45..40] := command;                   // content
  data[39..8]  := param;                     // content
  CRC7(data[47..8], crc_width[7..0])(crc);   // Calculate the 7-bit CRC
  data[7..1]   := crc;                       // Append 7-bit crc
  data[0]      := 1;                         // End flag

  IF (DEBUG) THEN
    PRINT("Tx Command ", command, "(0x", HEX(command), "), param=0x", HEX(param), ", data=0x", HEX(data), "\n");
  END;

  // Min clocks between consecutive commands or from response to next command
  Clocks(2);

  // transmit command MSB first
  FOR i := 1 TO 48
    SET CMD := data[47];
    SET CLK := 1;
    SET CLK := 0;
    data    := data << 1;
  END;

  result := RESULT_PASS;

END;

//--------------------------------------------------------------------------------
GetResponse_R1()(INT result WIDTH 1, INT command_index WIDTH 6, INT card_status WIDTH 32)
//--------------------------------------------------------------------------------
  INT data      WIDTH 48 := 0;
  INT i         WIDTH 8;
  INT crc       WIDTH 7;
  INT bit       WIDTH 1;
  INT crc_width WIDTH 8;

  ReadResponse(48)(result, data);
  IF (result != RESULT_PASS) THEN
    RETURN;
  END;

  command_index  := data[45..40];
  card_status    := data[39..8];

  IF (DEBUG) THEN
    PRINT("R1 response : Command ", command_index, "(0x", HEX(command_index), "), status 0x", HEX(card_status), ", crc 0x", HEX(data[7..1]), "\n");
  END;

  IF (data[47..46] != 0b00) THEN
    PRINT("Invalid R1 response bits 47/46\n");
    result := RESULT_FAIL;
  END;

  IF (data[0] != 1) THEN
    PRINT("Invalid R1 response bit 0\n");
    result := RESULT_FAIL;
  END;

  crc_width := WIDTHOF(data[47..8]);
  CRC7(data[47..8], crc_width[7..0])(crc);
  IF (data[7..1] != crc) THEN
    PRINT("R1 Response CRC error, Rxvd 0x", HEX(data[7..1]), ", expected 0x", HEX(crc), "\n");
    result := RESULT_FAIL;
  END;

END;

//--------------------------------------------------------------------------------
GetResponse_R2()(INT result WIDTH 1, INT value WIDTH 120)
//--------------------------------------------------------------------------------
  INT data      WIDTH 136 := 0;
  INT i         WIDTH 8;
  INT crc       WIDTH 16;
  INT bit       WIDTH 1;
  INT crc_width WIDTH 8;

  ReadResponse(136)(result, data);
  IF (result != RESULT_PASS) THEN
    RETURN;
  END;

  value  := data[127..8];

  IF (DEBUG) THEN
    PRINT("R2 response : Reserved=", BIN(data[133..128]), "(0x", HEX(data[133..128]), "), data=0x", HEX(value), ", crc=0x", HEX(data[7..1]), "\n");
  END;

  IF (data[135..134] != 0b00) THEN
    PRINT("Invalid R2 response start 0b", BIN(data[135..134]), "\n");
    result := RESULT_FAIL;
  END;

  IF (data[133..128] != 0b111111) THEN
    PRINT("Invalid R2 response reserved bits 0b", BIN(data[133..128]), "\n");
    result := RESULT_FAIL;
  END;

  IF (data[0] != 1) THEN
    PRINT("Invalid R2 response end\n");
    result := RESULT_FAIL;
  END;

  crc_width := WIDTHOF(data[127..8]);
  CRC7(value, crc_width[7..0])(crc);
  IF (data[7..1] != crc) THEN
    PRINT("R2 Response CRC error, Rxvd 0x", HEX(data[16..1]), ", expected 0x", HEX(crc), "\n");
    result := RESULT_FAIL;
  END;

END;

//--------------------------------------------------------------------------------
GetResponse_R3()(INT result WIDTH 1, INT ocr WIDTH 32)
//--------------------------------------------------------------------------------
  INT data      WIDTH 48 := 0;
  INT i         WIDTH 8;
  INT bit       WIDTH 1;

  ReadResponse(48)(result, data);
  IF (result != RESULT_PASS) THEN
    RETURN;
  END;

  ocr  := data[39..8];

  IF (DEBUG) THEN
    PRINT("R3 response : Reserved1 ", BIN(data[45..40]), "(0x", HEX(data[45..40]), "), OCR 0x", HEX(ocr), ", Reserved2 0x", HEX(data[7..1]), "\n");
  END;

  IF (data[47..46] != 0b00) THEN
    PRINT("Invalid R3 response Start\n");
    result := RESULT_FAIL;
  END;

  IF (data[0] != 1) THEN
    PRINT("Invalid R3 response End\n");
    result := RESULT_FAIL;
  END;

END;

//--------------------------------------------------------------------------------
GetResponse_R6()(INT result WIDTH 1, INT rca WIDTH 16, INT status WIDTH 32)
//--------------------------------------------------------------------------------
  INT data      WIDTH 48 := 0;
  INT i         WIDTH 8;
  INT bit       WIDTH 1;
  INT crc_width WIDTH 8;
  INT crc       WIDTH 7;
  INT cmd       WIDTH 6;

  ReadResponse(48)(result, data);
  IF (result != RESULT_PASS) THEN
    RETURN;
  END;

  rca    := data[39..24];
  cmd    := data[45:40];

  // Extract card status from the result
  status         := 0;
  status[23..22] := data[23..22];
  status[19]     := data[21];
  status[12..0]  := data[20..8];

  IF (DEBUG) THEN
    PRINT("R6 response : cmd=", cmd, ", rca=0x", HEX(rca), ", status=0x", HEX(status), ", crc=0x", HEX(data[7..1]), "\n");
  END;

  IF (data[47..46] != 0b00) THEN
    PRINT("Invalid R6 response Start\n");
    result := RESULT_FAIL;
  END;

  IF (data[0] != 1) THEN
    PRINT("Invalid R6 response End\n");
    result := RESULT_FAIL;
  END;

  //IF (cmd != 3) THEN
  //  PRINT("Invalid R6 response command index\n");
  //  result := RESULT_FAIL;
  //END;

  crc_width := WIDTHOF(data[47..8]);
  CRC7(data[47..8], crc_width[7..0])(crc);
  IF (data[7..1] != crc) THEN
    PRINT("R6 Response CRC error, Rxvd 0x", HEX(data[16..1]), ", expected 0x", HEX(crc), "\n");
    result := RESULT_FAIL;
  END;

END;

//--------------------------------------------------------------------------------
Clocks(INT n)()
//--------------------------------------------------------------------------------
  INT i;

  SET CMD := I;
  FOR i:= 1 FOR n
    SET CLK := 1;
    SET CLK := 0;
  END;
END;

//--------------------------------------------------------------------------------
CRC7(INT data, INT nbits)(INT result WIDTH 7)
//--------------------------------------------------------------------------------
  INT crc_reg  WIDTH 7 := 0;
  INT crc_temp WIDTH 7;
  INT this_bit WIDTH 8;

  FOR this_bit := 1 TO nbits
    crc_temp      := crc_reg;
    crc_reg[6..1] := crc_temp[5..0];
    crc_reg[0]    := data[nbits-this_bit] ^ crc_temp[6];
    crc_reg[3]    := crc_temp[2] ^ crc_reg[0];
  END;

  IF DEBUG THEN
    PRINT("CRC7 : (", nbits, "bit) 0x", HEX(data), "=", BIN(crc_reg), "(0x", HEX(crc_reg), ")\n");
  END;

  result := crc_reg;
END;

//--------------------------------------------------------------------------------
CRC16(INT data, INT nbits)(INT result WIDTH 16)
//--------------------------------------------------------------------------------
  INT crc_reg  WIDTH 16 := 0;
  INT crc_temp WIDTH 16;
  INT this_bit WIDTH 16;

  FOR this_bit := 1 TO nbits
    crc_temp       := crc_reg;
    crc_reg[15..1] := crc_temp[14..0];
    crc_reg[0]     := (data[nbits-this_bit] ^ crc_temp[15]);
    crc_reg[5]     := (crc_temp[4] ^ crc_reg[0]);
    crc_reg[12]    := (crc_temp[11] ^ crc_reg[0]);
  END;

  IF DEBUG THEN
    PRINT("CRC16 : (", nbits, "bit) 0x", HEX(data), "=", BIN(crc_reg), "(0x", HEX(crc_reg), ")\n");
  END;

  result := crc_reg;
END;

//--------------------------------------------------------------------------------
ReadResponse(INT nbits)(INT result WIDTH 1, INT data)
//--------------------------------------------------------------------------------
  INT i;
  INT bit WIDTH 1;

  WaitResponse()(result);
  IF (result = RESULT_FAIL) THEN
    RETURN;
  END;

  // Get response MSB first
  data := 0;
  FOR i := 1 TO nbits-1
    SET CLK    := 1;
    SET CLK    := 0;
    SET bit    := CMD;
    data       := (data << 1) | bit;
  END;

  result := RESULT_PASS;

END;

//--------------------------------------------------------------------------------
WaitResponse()(INT result WIDTH 1)
//--------------------------------------------------------------------------------
  INT counter WIDTH 8 := 64;
  INT bit     WIDTH 1;

  SET CMD := I;
  DO
    SET CLK := 1;
    SET CLK := 0;
    SET bit := CMD;
    counter := counter - 1;
  WHILE (bit && counter) END;

  IF (bit) THEN
    PRINT("No response from card.\n");
    result := RESULT_FAIL;
  ELSE
    result := RESULT_PASS;
  END;

END;

//--------------------------------------------------------------------------------
ReadBlock(INT block_number)(INT result, INT data, INT card_status WIDTH 32)
//--------------------------------------------------------------------------------
  INT i;
  INT timeout_count;
  INT data_count;
  INT resp_count;
  INT data_bits;
  INT resp_bits;
  INT data0;
  INT data1;
  INT data2;
  INT data3;
  INT all_data      WIDTH BLOCK_LENGTH_BITS;
  INT cbit          WIDTH 1;
  INT dbit          WIDTH 1;
  INT dbits         WIDTH 4;
  INT response      WIDTH 48;
  INT data_crc      WIDTH 16;
  INT resp_crc      WIDTH 7;
  INT crc2          WIDTH 16;
  INT data_started1 WIDTH 1;
  INT data_started2 WIDTH 1;
  INT resp_started  WIDTH 1;
  INT data_finished WIDTH 1;
  INT resp_finished WIDTH 1;
  INT data_stop     WIDTH 1;
  INT resp_stop     WIDTH 1;
  INT command_index WIDTH 6;

  SendCommand(READ_SINGLE_BLOCK, (block_number*BLOCK_LENGTH_BYTES))(result);
  IF (result != RESULT_PASS) THEN
    RETURN;
  END;

  IF (READ_DATA_WIDTH != 1 && READ_DATA_WIDTH != 4) THEN
    PRINT("Invalid READ_DATA_WIDTH\n");
    EXIT;
  ELSE
    PRINT("Reading ", BLOCK_LENGTH_BITS, " bits (", BLOCK_LENGTH_BYTES, " bytes) on ", READ_DATA_WIDTH, " data pins\n");
  END;

  // Get the data and the response MSB first
  // Wait for a start bit on each line before starting capture
  // For the data line we seem to need to see a high to low transition to start
  SET CMD := I, DATA:= I;
  data_bits     := 17 + (BLOCK_LENGTH_BITS / READ_DATA_WIDTH);
  resp_bits     := 47;
  timeout_count := 1024 + data_bits;
  data_count    := 0;
  resp_count    := 0;
  data0         := 0;
  data1         := 0;
  data2         := 0;
  data3         := 0;
  response      := 0;
  resp_started  := FALSE;
  data_started1 := FALSE;
  data_started2 := FALSE;
  resp_finished := FALSE;
  data_finished := FALSE;
  DO
    SET CLK := 1;
    SET CLK := 0;
    SET cbit := CMD, dbits := DATA;
    //PRINT(HEX(dbits), ",");
    IF (data_started1 && data_started2 && !data_finished) THEN
      IF (data_count < data_bits) THEN
        data0 := (data0 << 1) | dbits[0];
        data1 := (data1 << 1) | dbits[1];
        data2 := (data2 << 1) | dbits[2];
        data3 := (data3 << 1) | dbits[3];
        data_count := data_count + 1;
      ELSE
        data_finished := TRUE;
      END;
    END;
    IF (resp_started && !resp_finished) THEN
      response := (response << 1) | cbit;
      resp_count := resp_count + 1;
      IF (resp_count = resp_bits) THEN resp_finished := TRUE; END;
    END;
    IF (READ_DATA_WIDTH = 4) THEN dbit := dbits[3]; ELSE dbit := dbits[0]; END;
    IF (!data_started1 && dbit) THEN data_started1 := TRUE; END;
    IF (data_started1 && !data_started2 && !dbit) THEN data_started2 := TRUE; END;
    IF (!cbit && !resp_started) THEN resp_started := TRUE; END;
    timeout_count := timeout_count - 1;
  UNTIL ((data_finished && resp_finished) || (!timeout_count)) END;

  // Fail if no resp/data start bit received
  IF (!timeout_count) THEN
    PRINT("Timeout - No data/response received from card.\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  // Extract the response fields
  resp_crc      := response[7..1];
  resp_stop     := response[0];
  command_index := response[45..40];
  card_status   := response[39..8];
  IF (DEBUG) THEN
    PRINT("\tResponse  = 0x", HEX(response), "\n");
    PRINT("\tResp CRC  = 0x", HEX(resp_crc), "\n");
    PRINT("\tResp Stop = ", resp_stop, "\n");
  END;

  // Check response stop bit
  IF (!resp_stop) THEN
    PRINT("Read response stop bit error.\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  // Check response CRC
  CRC7(response[47..8], 40)(i);
  IF (resp_crc != i) THEN
    PRINT("Read response CRC error.\n");
    result := RESULT_FAIL;
    RETURN;
  ELSE
    IF (DEBUG) THEN PRINT("\tResponse CRC OK.\n"); END;
  END;

  // Extract and test each data channel for stop bit and CRC
  FOR i := 0 TO (READ_DATA_WIDTH-1)

    // Only data widths 1 and 4 are valid
    IF i = 0 THEN data := data0; END;
    IF i = 1 THEN data := data1; END;
    IF i = 2 THEN data := data2; END;
    IF i = 3 THEN data := data3; END;
    IF i > 3 THEN PRINT("Inavalid READ_DATA_WIDTH\n"); EXIT; END;

    // Extract data portion, stop bit and CRC
    data_crc  := data[16..1];
    data_stop := data[0];
    data      := data >> 17;
    IF (DEBUG) THEN
      PRINT("\tRead data [", i, "] = 0x", HEX(data), "\n");
      PRINT("\tData CRC  [", i, "] = 0x", HEX(data_crc), "\n");
      PRINT("\tData Stop [", i, "] = ", data_stop, "\n");
    END;

    // Check data pin stop bit
    IF (!data_stop) THEN
      PRINT("Read data [", i, "] stop bit error\n");
      result := RESULT_FAIL;
      RETURN;
    END;

    // Check the data line CRC
    CRC16(data, (BLOCK_LENGTH_BITS / READ_DATA_WIDTH))(crc2);
    IF (data_crc != crc2) THEN
      PRINT("Read data [", i, "] CRC error\n");
      result := RESULT_FAIL;
      RETURN;
    ELSE
      IF (DEBUG) THEN PRINT("\tData [", i, "] CRC OK\n"); END;
    END;
  END;

  // Reconstruct the data from all the required bitstreams
  PRINT("\tReconstructing data\n");
  all_data := 0;
  data0 := data0 >> 17;
  data1 := data1 >> 17;
  data2 := data2 >> 17;
  data3 := data3 >> 17;
  IF (READ_DATA_WIDTH = 4) THEN
    FOR data_count := 0 FOR (BLOCK_LENGTH_BITS / 4)
      all_data := all_data >> 4;
      all_data[BLOCK_LENGTH_BITS-4] := data0[data_count];
      all_data[BLOCK_LENGTH_BITS-3] := data1[data_count];
      all_data[BLOCK_LENGTH_BITS-2] := data2[data_count];
      all_data[BLOCK_LENGTH_BITS-1] := data3[data_count];
    END;
  ELSE
    all_data := data0;
  END;

  data := all_data;
  IF (DEBUG) THEN PRINT("\tBlock ", block_number, " data = 0x", HEX(data), "\n"); END;
  PRINT("\tSD Card data tests passed OK.\n");

END;