//---------------------------------------------------------------
// SD/MMC Interface: XJEase device file
// SD_MMC.xje Revision: 1.00
// (c)2004-2010 XJTAG Limited
//
// Disclaimer: XJTAG makes no guarantees whatsoever
// about this code.  You use it at your own risk ...
// This code requires XJTAG version 1.4 or later.
//
// This file tests Non-JTAG Memory Card using Test()(INT result)
//
//---------------------------------------------------------------


// Constants to define different commands
CONST INT CMD_INDEX_MASK   := 0x3F;     // 6 bits for command index
CONST INT RSP_LENGTH_OFFSET := 6;
CONST INT RSP_LENGTH_MASK  := 0x3 << RSP_LENGTH_OFFSET; // 2 bits for response length
CONST INT CHECK_CRC_MASK   := 0x1 << 8; // 1 bit to indicate if the CRC in the response should be checked
CONST INT CHECK_INDEX_MASK := 0x1 << 9; // 1 bit to indicate if the index in the response should be checked

CONST INT DATA_TYPE_OFFSET := 10;
CONST INT DATA_TYPE_MASK   := 0x3 << DATA_TYPE_OFFSET;

CONST INT DATA_IN_BYTES_MASK := 0x1 << 12;

// Give each response type a different ID value so we can debug which response is expected
CONST INT RSP_ID_OFFSET    := 13;
CONST INT RSP_ID_MASK      := 0xF << RSP_ID_OFFSET;

CONST INT CCC_OFFSET       := 17;
CONST INT CCC_MASK         := 0xFFF << CCC_OFFSET;

CONST INT RSP_LENGTH0   := 0x0 << RSP_LENGTH_OFFSET;
CONST INT RSP_LENGTH48  := 0x1 << RSP_LENGTH_OFFSET;
CONST INT RSP_LENGTH48B := 0x2 << RSP_LENGTH_OFFSET;
CONST INT RSP_LENGTH136 := 0x3 << RSP_LENGTH_OFFSET;

CONST INT CHECK_CRC        := CHECK_CRC_MASK;
CONST INT NOCHECK_CRC      := 0;

CONST INT CHECK_INDEX      := CHECK_INDEX_MASK;
CONST INT NOCHECK_INDEX    := 0;

CONST INT DATA_TYPE_NONE   := 0x0 << DATA_TYPE_OFFSET;
CONST INT DATA_TYPE_READ   := 0x1 << DATA_TYPE_OFFSET;
CONST INT DATA_TYPE_WRITE  := 0x2 << DATA_TYPE_OFFSET;

CONST INT DATA_IN_BYTES    := DATA_IN_BYTES_MASK;
CONST INT DATA_IN_BITS     := 0;

// Responses
CONST INT RSP_TYPE_MASK    := RSP_ID_MASK          | RSP_LENGTH_MASK  | CHECK_CRC_MASK | CHECK_INDEX_MASK;

CONST INT RSP_TYPE_NONE    := 0x0 << RSP_ID_OFFSET | RSP_LENGTH0      | NOCHECK_CRC | NOCHECK_INDEX;
CONST INT RSP_TYPE_R1      := 0x1 << RSP_ID_OFFSET | RSP_LENGTH48     | CHECK_CRC   | CHECK_INDEX;
CONST INT RSP_TYPE_R1B     := 0x2 << RSP_ID_OFFSET | RSP_LENGTH48B    | CHECK_CRC   | CHECK_INDEX;
CONST INT RSP_TYPE_R2      := 0x3 << RSP_ID_OFFSET | RSP_LENGTH136    | CHECK_CRC   | NOCHECK_INDEX;
CONST INT RSP_TYPE_R3      := 0x4 << RSP_ID_OFFSET | RSP_LENGTH48     | NOCHECK_CRC | NOCHECK_INDEX;
CONST INT RSP_TYPE_R4      := 0x5 << RSP_ID_OFFSET | RSP_LENGTH48     | NOCHECK_CRC | NOCHECK_INDEX; // Why does this not check index?
CONST INT RSP_TYPE_R5      := 0x6 << RSP_ID_OFFSET | RSP_LENGTH48     | CHECK_CRC   | CHECK_INDEX;
CONST INT RSP_TYPE_R6      := 0x7 << RSP_ID_OFFSET | RSP_LENGTH48     | CHECK_CRC   | CHECK_INDEX;
CONST INT RSP_TYPE_R7      := 0x8 << RSP_ID_OFFSET | RSP_LENGTH48     | CHECK_CRC   | CHECK_INDEX;

// Command Classes
CONST INT CCC_BASIC        := (0b1 << 0) << CCC_OFFSET;
CONST INT CCC_SEQ_READ     := (0b1 << 1) << CCC_OFFSET;
CONST INT CCC_BLOCK_READ   := (0b1 << 2) << CCC_OFFSET;
CONST INT CCC_SEQ_WRITE    := (0b1 << 3) << CCC_OFFSET;
CONST INT CCC_BLOCK_WRITE  := (0b1 << 4) << CCC_OFFSET;
CONST INT CCC_ERASE        := (0b1 << 5) << CCC_OFFSET;
CONST INT CCC_WRITE_PROTECT:= (0b1 << 6) << CCC_OFFSET;
CONST INT CCC_LOCK_CARD    := (0b1 << 7) << CCC_OFFSET;
CONST INT CCC_APP_SPECIFIC := (0b1 << 8) << CCC_OFFSET;
CONST INT CCC_IO_MODE      := (0b1 << 9) << CCC_OFFSET;
CONST INT CCC_SWITCH       := (0b1 << 10) << CCC_OFFSET;

// Commands
CONST INT CMD0             := 0  | RSP_TYPE_NONE | DATA_TYPE_NONE | CCC_BASIC;
CONST INT MMC_CMD1         := 1  | RSP_TYPE_R3   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD2             := 2  | RSP_TYPE_R2   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT MMC_CMD3         := 3  | RSP_TYPE_R1   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT SD_CMD3          := 3  | RSP_TYPE_R6   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD4             := 4  | RSP_TYPE_NONE | DATA_TYPE_NONE | CCC_BASIC;
CONST INT MMC_CMD6         := 6  | RSP_TYPE_R1B  | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD7_SELECT      := 7  | RSP_TYPE_R1B  | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD7_DESELECT    := 7  | RSP_TYPE_NONE | DATA_TYPE_NONE | CCC_BASIC;
CONST INT MMC_CMD8         := 8  | RSP_TYPE_R1   | DATA_TYPE_READ | DATA_IN_BYTES | CCC_BASIC;
CONST INT SD_CMD8          := 8  | RSP_TYPE_R7   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD9             := 9  | RSP_TYPE_R2   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD12            := 12 | RSP_TYPE_R1B  | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD13            := 13 | RSP_TYPE_R1   | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD15            := 15 | RSP_TYPE_NONE | DATA_TYPE_NONE | CCC_BASIC;
CONST INT CMD16            := 16 | RSP_TYPE_R1   | DATA_TYPE_NONE | CCC_BLOCK_READ | CCC_BLOCK_WRITE | CCC_LOCK_CARD;
CONST INT CMD17            := 17 | RSP_TYPE_R1   | DATA_TYPE_READ | DATA_IN_BYTES | CCC_BLOCK_READ;
CONST INT CMD24            := 24 | RSP_TYPE_R1   | DATA_TYPE_WRITE | DATA_IN_BYTES | CCC_BLOCK_WRITE;
CONST INT SD_ACMD6         := 6  | RSP_TYPE_R1   | DATA_TYPE_NONE | CCC_APP_SPECIFIC;
CONST INT SD_ACMD13        := 13 | RSP_TYPE_R1   | DATA_TYPE_READ | DATA_IN_BITS | CCC_APP_SPECIFIC;
CONST INT SD_ACMD41        := 41 | RSP_TYPE_R3   | DATA_TYPE_NONE | CCC_APP_SPECIFIC;
CONST INT SD_ACMD51        := 51 | RSP_TYPE_R1   | DATA_TYPE_READ | DATA_IN_BYTES | CCC_APP_SPECIFIC;
CONST INT CMD55            := 55 | RSP_TYPE_R1   | DATA_TYPE_NONE | CCC_APP_SPECIFIC;

// Timing constants
CONST INT CLOCKS_BEFORE_COMMAND := 8;
CONST INT CLOCKS_BEFORE_INIT    := 80;
LOCAL INT TAAC                  := 0; // Read from CSD register, in ms
LOCAL INT NSAC                  := 0; // Read from CSD register
LOCAL INT TRAN_SPEED            := 0; // Hz

CONST INT MAX_CLOCKS_BEFORE_RESPONSE := 64;
CONST INT MAX_CLOCKS_BEFORE_BUSY := 4;
CONST INT MAX_CLOCKS_BEFORE_CRC_STATUS := 4;
CONST INT MAX_BUSY_DELAY         := 5000; // 5s timeout on busy signal
CONST INT MAX_CLOCKS_BEFORE_DATA := 2000; // Ideally need to set this based on TAAC and NSAC

CONST INT MAX_POWER_UP_DELAY    := 1200; // Give small amount of margin on top of 1s

CONST INT RESET_RETRY_COUNT     := 4; // The maximum number of attempts to reset and wait for the power-up to complete
CONST INT READ_BLOCK_RETRY_COUNT := 3; // The maximum number of attempts to read a block before failing

// MMC Commands

// CMD0 Values
CONST INT GO_IDLE_STATE     := 0;
CONST INT GO_PRE_IDLE_STATE := 0xF0F0F0F0;
CONST INT GO_BOOT_STATE     := 0xFFFFFFFA;

// Status Bits
CONST INT STATUS_ERROR_BITS WIDTH 32 := 0b11111101111111110000000010000000;
CONST INT STATUS_ILLEGAL_COMMAND WIDTH 32 := 1 << 22;

// EXT_CSD Access mode values
CONST INT ACCESS_COMMAND_SET WIDTH 2 := 0b00;
CONST INT ACCESS_SET_BITS    WIDTH 2 := 0b01;
CONST INT ACCESS_CLEAR_BITS  WIDTH 2 := 0b10;
CONST INT ACCESS_WRITE_BYTE  WIDTH 2 := 0b11;

CONST INT EXT_CSD_BUS_WIDTH  WIDTH 8 := 183;

// Relative card address
CONST INT DEFAULT_RCA WIDTH 16  := 0x0001;

// Required classes for us to do anything useful
CONST INT CCC_REQUIRED_MASK := 0b000000000101; // basic, block read;

// Values obtained from OCR.  The non-constant values are set when read from the device.
//
CONST INT OCR_2V7_3V6                  := 0x00FF8000;

CONST INT OCR_NOT_BUSY_BIT             := 31;
CONST INT OCR_NOT_BUSY                 := 1 << OCR_NOT_BUSY_BIT;

CONST INT HIGH_CAPACITY_SUPPORT_MASK   := 1 << 30;
CONST INT HIGH_CAPACITY_SUPPORT_SECTOR := HIGH_CAPACITY_SUPPORT_MASK;
CONST INT HIGH_CAPACITY_SUPPORT_BYTE   := 0;

CONST INT ACCESS_MODE_MASK             := 0x3 << 29;
CONST INT ACCESS_MODE_SECTOR           := 0x2 << 29;
CONST INT ACCESS_MODE_BYTE             := 0x0 << 29;

CONST INT DEVICE_TYPE_UNKNOWN := 0x0;
CONST INT DEVICE_TYPE_MMC     := 0x1;
CONST INT DEVICE_TYPE_SD      := 0x2;

CONST INT DEVICE_MODE_BYTE    := 0;
CONST INT DEVICE_MODE_SECTOR  := 1;

LOCAL INT deviceType := DEVICE_TYPE_UNKNOWN;
LOCAL INT deviceMode := DEVICE_MODE_BYTE;

LOCAL INT rca         WIDTH 16;
LOCAL INT ocr         WIDTH 32;
LOCAL INT cid         WIDTH 127;
LOCAL INT csd         WIDTH 127;
LOCAL INT ext_csd     WIDTH 512*8;
LOCAL INT ccc         WIDTH 12; // Supported classes
LOCAL INT sd_status   WIDTH 512;

LOCAL INT c_size_mult WIDTH 3;

LOCAL INT maxReadBlockLength := 0; // Bytes
LOCAL INT maxWriteBlockLength := 0; // Bytes

LOCAL INT deviceSize := 0;  // Bytes
LOCAL INT protectedAreaSize := 0; // Bytes;

LOCAL INT dataByteLength;
LOCAL INT dataInterfaceWidth;
LOCAL INT lastReadData;
LOCAL INT lastReadCRC;
LOCAL INT nextWriteData;

LOCAL INT disableNoResponseMessage := 0;

//--------------------------------------------------------------------------------
Test(INT destructiveTest, INT testStartAddress)(INT result)
//--------------------------------------------------------------------------------
  INT readData;
  INT writeData;
  INT response;
  INT key;

  INT i;

  INT newWidth;

  InitialiseFaultMatrix(WIDTHOF(DATA))();

  InitialiseDevice()(result);
  IF result != RESULT_PASS THEN RETURN; END;

  SetBusWidth(1, WIDTHOF(DATA))(newWidth);

  IF newWidth THEN
    PRINT("Set data interface to use ", dataInterfaceWidth, " bits\n");

    IF dataInterfaceWidth != WIDTHOF(DATA) THEN
      PRINT("NOTE: Device file specifies ", WIDTHOF(DATA), " data bits, but this test is only testing ", dataInterfaceWidth, " of them.\n");
    END;
  ELSE
    PRINT("Unable to change data bus width\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  ResizeFaultMatrix(dataInterfaceWidth)();

  // Check the device's status
  SendCommand(CMD13, rca[15..0]:0[15..0])(result, response);
  IF result != RESULT_PASS THEN
    PRINT("Error sending command CMD13\n");
    RETURN;
  ELSIF response & STATUS_ERROR_BITS THEN
    PRINT("Status error bit set with CMD13\n");
    DecodeStatus(response);
    result := RESULT_FAIL;
    RETURN;
  ELSIF DEBUG THEN
    DecodeStatus(response);
  END;

  IF destructiveTest && PROMPT_BEFORE_DESTRUCTIVE_TEST THEN
    // Clear keyboard buffer
    DO WHILE GETKEY(); END;
    PRINT("About to commence destructive test on ", DEVICE_REF, ".\n");
    PRINT("Are you sure you want to overwrite data starting at address 0x", HEX(testStartAddress), "?\n");
    PRINT("Press Y to continue, any other key to exit.\n");
    ALERT();
    key := WAITKEY();

    IF (key != 'Y') && (key != 'y') THEN
      PRINT("Destructive test cancelled by operator\n");
      result := RESULT_FAIL;
      RETURN;
    END;
  END;

  IF destructiveTest THEN
    writeData[7..0] := NOW();

    FOR i := 1 FOR dataByteLength - 1
      writeData[(i+1)*8-1..i*8] := (writeData[i*8-1..(i-1)*8]) + 0b01001001;
    END;

    WriteDataBlock(testStartAddress, writeData)(result);

    IF result != RESULT_PASS THEN
      PRINT("Error writing data block\n");

      // Check the device's status
      SendCommand(CMD13, rca[15..0]:0[15..0])(result, response);
      IF result != RESULT_PASS THEN
        PRINT("Error sending command CMD13\n");
        RETURN;
      END;

      DecodeStatus(response);
      result := RESULT_FAIL;

      RETURN;
    END;

    ReadDataBlock(testStartAddress)(result, readData);
    IF result != RESULT_PASS THEN
      PRINT("Error reading data block\n");

      // Check the device's status
      SendCommand(CMD13, rca[15..0]:0[15..0])(result, response);
      IF result != RESULT_PASS THEN
        PRINT("Error sending command CMD13\n");
        RETURN;
      END;

      DecodeStatus(response);
      result := RESULT_FAIL;

      RETURN;
    ELSE
  //    PRINT("0x", HEX(readData), "\n");
    END;

    IF readData != writeData THEN
      PRINT("readData differs from writeData during destructive test\n");
      result := RESULT_FAIL;
      RETURN;
    END;
  ELSE
    // Don't do a destructive test, but try to check all the data bits
    ReadTest(testStartAddress)(result);
    IF result != RESULT_PASS THEN
      PRINT("Read data test failed\n");
      RETURN;
    END;
  END;

  PRINT("MMC/SD Test Completed OK\n");
  result := RESULT_PASS;
  RETURN;

END;

//--------------------------------------------------------------------------------
ReadTest(INT testStartAddress)(INT result)
//--------------------------------------------------------------------------------
  INT readData;
  INT data;
  INT dataAddress := testStartAddress;
  INT foundSomeData := FALSE;
  INT i, j, k;
  INT response;

  // Read blocks until we find enough data, or give up trying to find data
  DO WHILE (dataAddress + dataByteLength < deviceSize) && DataFaultPresent()
    PRINT("Reading data starting at address 0x", HEX(dataAddress), "\n");

    ReadDataBlock(dataAddress)(result, readData);
    IF result != RESULT_PASS THEN
      PRINT("Error reading data block\n");

      // Check the device's status
      SendCommand(CMD13, rca[15..0]:0[15..0])(result, response);
      IF result != RESULT_PASS THEN
        PRINT("Error sending command CMD13\n");
        RETURN;
      END;

      DecodeStatus(response);
      result := RESULT_FAIL;

      RETURN;
    END;

    IF (readData != ~0[dataByteLength * 8-1..0]) && (readData != 0) THEN
      foundSomeData := TRUE;

      // The data automatically gets analysed as it's read, so just need to check if there are
      // any errors outstanding

      dataAddress := dataAddress + dataByteLength;
    ELSE
      dataAddress := dataAddress + ((deviceSize / 20 / dataByteLength) + 1) * dataByteLength;
    END;
  END;

  result := RESULT_PASS;

  IF DataFaultPresent() THEN
    result := RESULT_FAIL;

    IF !foundSomeData THEN
      PRINT("Could not find any data to check data bits on device ", DEVICE_REF, "\n");
    ELSE
      PrintFaultResults();
      PRINT("The above errors may be because not enough data on device ", DEVICE_REF, " could be found.\n");
      PRINT("Either add more data onto this device or use the destructive test with this device.\n");
    END;
  END;
END;

//--------------------------------------------------------------------------------
InitialiseDevice()(INT result)
//--------------------------------------------------------------------------------
  INT response;
  INT dataByteLengthTmp;
  INT i;
  INT newWidth;

  // Clear all values associated with the device
  deviceType         := DEVICE_TYPE_UNKNOWN;
  rca                := 0;
  ocr                := 0;
  cid                := 0;
  csd                := 0;
  ext_csd            := 0;
  sd_status          := 0;
  maxReadBlockLength := 0;
  maxWriteBlockLength := 0;
  deviceSize         := 0;
  protectedAreaSize  := 0;
  dataByteLength     := 0;
  dataInterfaceWidth := 0;

  // Initially assume all card command classes are supported until we read what's actually
  // supported from the device
  ccc                := 0xFFF;

  FOR i := 0 FOR RESET_RETRY_COUNT
    ResetAndWaitForPowerUp()(result);
    IF result = RESULT_PASS THEN BREAK; END;
  END;

  IF result != RESULT_PASS THEN
    PRINT("Failed to power up device after ", RESET_RETRY_COUNT, " attempts\n");
    RETURN;
  END;

  SendCommand(CMD2, 0)(result, cid);
  IF result != RESULT_PASS THEN
    PRINT("Error sending command CMD2\n");
    RETURN;
  END;

  DecodeCID()();

  SWITCH deviceType
    CASE DEVICE_TYPE_SD
      // SD devices tell us the RCA they're going to use

      SendCommand(SD_CMD3, 0)(result, response);
      IF result != RESULT_PASS THEN
        PRINT("Error sending command SD_CMD3\n");
        RETURN;
      END;

      rca := response[31..16];
    END;

    CASE DEVICE_TYPE_MMC
      SendCommand(MMC_CMD3, rca:0[15..0])(result, response);
      // Check response is ok - it seems that ILLEGAL_COMMAND doesn't get updated
      // so may still be set from previous attempts with CMD55
      IF result != RESULT_PASS THEN
        PRINT("Error sending command MMC_CMD3\n");
        RETURN;
      ELSIF (response & STATUS_ERROR_BITS & ~STATUS_ILLEGAL_COMMAND) THEN
        PRINT("Status error bit set with MMC_CMD3\n");
        DecodeStatus(response);
        result := RESULT_FAIL;
        RETURN;
      END;
    END;

    DEFAULT
      PRINT("Unexpected device type\n");
      result := RESULT_FAIL;
      RETURN;
    END;
  END;

  // Read CSD
  SendCommand(CMD9, rca:0[15..0])(result, csd);
  IF result != RESULT_PASS THEN
    PRINT("Error sending command CMD9\n");
    RETURN;
  END;

  IF (deviceType = DEVICE_TYPE_SD) THEN
    IF (deviceMode = DEVICE_MODE_BYTE) && (csd[127..126] != 0b00) THEN
      PRINT("Detected a SD device working in byte mode (ie standard capacity), but without CSD structure Version 1.0\n");
      result := RESULT_FAIL;
      RETURN;
    ELSIF (deviceMode = DEVICE_MODE_SECTOR) && (csd[127..126] != 0b01) THEN
      PRINT("Detected a SD device working in sector mode (ie SDHC/SDXC device), but without CSD structure Version 2.0\n");
      result := RESULT_FAIL;
      RETURN;
    END;
  END;

  DecodeCSD()();

  IF (ccc & CCC_REQUIRED_MASK) != CCC_REQUIRED_MASK THEN
    PRINT("Card dose not support our minimum set of classes, testing cannot continue\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  // Select the device to put it into transfer state
  SendCommand(CMD7_SELECT, rca:0[15..0])(result, response);
  IF result != RESULT_PASS THEN
    PRINT("Error sending command CMD7_SELECT\n");
    RETURN;
  ELSIF (response & STATUS_ERROR_BITS) THEN
    PRINT("Status error bit set with CMD7_SELECT\n");
    DecodeStatus(response);
    result := RESULT_FAIL;
    RETURN;
  END;

  // Set the device's block length to always use 512 bytes, as all devices should support this
  dataByteLength := 512;
  SendCommand(CMD16, 512)(result, response);
  IF result != RESULT_PASS THEN
    PRINT("Error sending command CMD16\n");
    RETURN;
  ELSIF (response & STATUS_ERROR_BITS) THEN
    PRINT("Status error bit set with CMD16\n");
    DecodeStatus(response);
    result := RESULT_FAIL;
    RETURN;
  END;

  // Read the EXT_CSD if we need to get information from it.
  // At the moment this is just for MMC devices using sector mode
  IF (deviceType = DEVICE_TYPE_MMC) && (deviceMode = DEVICE_MODE_SECTOR) THEN
    // Try and read EXT_CSD using as many data bits as possible
    // assuming that if switching bus width fails, we're still using 1 bit access
    SetBusWidth(1, WIDTHOF(DATA))(newWidth);
    IF !newWidth THEN dataInterfaceWidth := 1; END;

    SendCommand(MMC_CMD8, 0)(result, response);
    IF result != RESULT_PASS THEN
      PRINT("Error sending command MMC_CMD8\n");
      RETURN;
    ELSIF (response & STATUS_ERROR_BITS) THEN
      PRINT("Status error bit set with MMC_CMD8\n");
      DecodeStatus(response);
      result := RESULT_FAIL;
      RETURN;
    END;

    ext_csd := lastReadData;

    DecodeEXT_CSD()();
  END;

  IF (deviceType = DEVICE_TYPE_SD) THEN

    SendCommand(CMD55, rca:0[15..0])(result, response);
    IF result != RESULT_PASS THEN
      PRINT("Error sending command CMD55\n");
      RETURN;
    ELSIF (response & STATUS_ERROR_BITS) THEN
      PRINT("Status error bit set with CMD55\n");
      DecodeStatus(response);
      result := RESULT_FAIL;
      RETURN;
    END;

    // Read 512 bits, rather than a complete block length of data
    dataByteLengthTmp := dataByteLength;
    dataByteLength := 512/8;
    SendCommand(SD_ACMD13, 0)(result, response);
    dataByteLength := dataByteLengthTmp;

    IF result != RESULT_PASS THEN
      PRINT("Error sending command SD_ACMD13\n");
      RETURN;
    ELSIF (response & STATUS_ERROR_BITS) THEN
      PRINT("Status error bit set with SD_ACMD13\n");
      DecodeStatus(response);
      result := RESULT_FAIL;
      RETURN;
    END;

    sd_status := lastReadData;

    DecodeSD_Status()();
  END;

  // Check the device's status
  SendCommand(CMD13, rca[15..0]:0[15..0])(result, response);
  IF result != RESULT_PASS THEN
    PRINT("Error sending command CMD13\n");
    RETURN;
  ELSIF response & STATUS_ERROR_BITS THEN
    PRINT("Status error bit set with CMD13\n");
    DecodeStatus(response);
    result := RESULT_FAIL;
    RETURN;
  ELSIF DEBUG THEN
    DecodeStatus(response);
  END;
END;

//--------------------------------------------------------------------------------
ResetAndWaitForPowerUp()(INT result)
//--------------------------------------------------------------------------------
  INT response;
  INT highCapacitySupport;
  INT timeout;

  deviceType := DEVICE_TYPE_UNKNOWN;
  dataInterfaceWidth := 1;

  Clocks(CLOCKS_BEFORE_INIT);

  // Reset the device, moving it to the Idle State
  SendCommand(CMD0, GO_IDLE_STATE)(result, response);
  IF (result != RESULT_PASS) THEN PRINT("Software Reset using CMD0 Failed\n"); RETURN; END;

  rca := DEFAULT_RCA;

  // Try to work out if we've got SD or MMC, and which version.
  // MMC_CMD8 is only valid at this point with SD V2 devices, and others will either have an error
  // or no response.  The high capacity support bit should only be set for SD devices that know
  // about high capacity mode!
  highCapacitySupport := HIGH_CAPACITY_SUPPORT_BYTE;
  disableNoResponseMessage := TRUE;

  SendCommand(SD_CMD8, 0x1AA)(result, response);
  IF (result = RESULT_PASS) THEN
    // SD V2 device if it responded without error
    IF (response[11..0] != 0x1AA) THEN
      PRINT("Device responded as SD V2 device, but returned an unsupported response\n");
      result := RESULT_FAIL;
      RETURN;
    END;

    highCapacitySupport := HIGH_CAPACITY_SUPPORT_SECTOR;
  END;

  // Try to use CMD55 - if this is ok at this point then we've got SD, otherwise we've got MMC
  // The illegal command status bit may already be set from sending SD_CMD8, so ignore that
  // for now.  Continuously do this until the device is no longer busy or we timeout.

  timeout := NOW() + MAX_POWER_UP_DELAY;

  DO
    SendCommand(CMD55, 0)(result, response);

    IF (result = RESULT_PASS) THEN
      IF (response & STATUS_ERROR_BITS & ~STATUS_ILLEGAL_COMMAND) THEN
        PRINT("Status error bit set with CMD55\n");
        DecodeStatus(response);
        result := RESULT_FAIL;
        RETURN;
      END;

      deviceType := DEVICE_TYPE_SD;
      ocr        := OCR_2V7_3V6 | highCapacitySupport;
      SendCommand(SD_ACMD41, ocr)(result, response);
    ELSE
      deviceType := DEVICE_TYPE_MMC;
      ocr        := OCR_2V7_3V6 | ACCESS_MODE_SECTOR;
      SendCommand(MMC_CMD1, ocr)(result, response);
    END;
  UNTIL ((result = RESULT_PASS) && response[OCR_NOT_BUSY_BIT]) || (NOW() > timeout)
  END;

  IF result != RESULT_PASS THEN PRINT("Error reading OCR register - check device is present\n"); RETURN; END;

  IF !response[OCR_NOT_BUSY_BIT] THEN
    PRINT("Timeout waiting for device to compelete power-up\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  // Now that the power up status bit is set, the Card Capacity Status (CCS) on
  // SD devices, and Access Mode on MMC devices should be valid.
  ocr := response;

  disableNoResponseMessage := FALSE;

  SWITCH deviceType
    CASE DEVICE_TYPE_SD
      SWITCH (ocr & HIGH_CAPACITY_SUPPORT_MASK)
        CASE HIGH_CAPACITY_SUPPORT_SECTOR deviceMode := DEVICE_MODE_SECTOR; END;
        CASE HIGH_CAPACITY_SUPPORT_BYTE   deviceMode := DEVICE_MODE_BYTE; END;
        DEFAULT
          PRINT("Unexpected Card Capacity Status value in ocr\n");
          result := RESULT_FAIL;
          RETURN;
        END;
      END;
    END;

    CASE DEVICE_TYPE_MMC
      SWITCH (ocr & ACCESS_MODE_MASK)
        CASE ACCESS_MODE_SECTOR deviceMode := DEVICE_MODE_SECTOR; END;
        CASE ACCESS_MODE_BYTE   deviceMode := DEVICE_MODE_BYTE; END;
        DEFAULT
          PRINT("Unexpected Access Mode value in ocr\n");
          result := RESULT_FAIL;
          RETURN;
        END;
      END;
    END;

    DEFAULT
      PRINT("Unexpected device type\n");
      result := RESULT_FAIL;
      RETURN;
    END;
  END;

  IF (ocr & OCR_2V7_3V6) != OCR_2V7_3V6 THEN
    PRINT("Device reports that it does not support voltage range 2V7 to 3V6, which we currently rely on\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  result := RESULT_PASS;
END;

//--------------------------------------------------------------------------------
PrintText(INT text,INT length)()
//--------------------------------------------------------------------------------
  INT i:=(length-1)*8;
  DO
    IF (text[i+7..i] >= 32) && (text[i+7..i] < 128) THEN
      PRINT(CHAR(text[i+7..i]));
    ELSIF (text[i+7..i] = 0) THEN
      RETURN;
    ELSE
      PRINT(".");
    END;

    length := length-1;
    i:= i-8;
  WHILE (length)
  END;
END;

//--------------------------------------------------------------------------------
DecodeStatus(INT status WIDTH 32)()
//--------------------------------------------------------------------------------
  PRINT("Status : 0x", FORMAT(status, "%08X"), "\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 blk 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("\tCommand CRC error\n"); END;
  IF (status[22]) THEN PRINT("\tIllegal Command\n"); END;
  IF (status[21]) THEN PRINT("\tCard Internal ECC failed to correct data\n"); END;
  IF (status[20]) THEN PRINT("\tCard controller error\n"); END;
  IF (status[19]) THEN PRINT("\tGeneral error\n"); END;
  IF (status[18]) THEN PRINT("\tUnderrun error\n"); END;
  IF (status[17]) THEN PRINT("\tOverrun 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[07]) THEN PRINT("\tSwitch error - device did not switch as requested\n"); END;
  IF (status[06]) THEN PRINT("\tDevice needs to perform background operations urgently\n"); END;
  IF (status[05]) THEN PRINT("\tACMD expected or received\n"); END;
  IF (status[03]) THEN PRINT("\tAuthentication sequence error\n"); END;

  SWITCH(status[12..9])
    CASE 0 PRINT("\tState was Idle\n");  END;
    CASE 1 PRINT("\tState was Ready\n"); END;
    CASE 2 PRINT("\tState was Ident\n"); END;
    CASE 3 PRINT("\tState was Stby\n");  END;
    CASE 4 PRINT("\tState was Tran\n");  END;
    CASE 5 PRINT("\tState was Data\n");  END;
    CASE 6 PRINT("\tState was Rcv\n");   END;
    CASE 7 PRINT("\tState was Prog\n");  END;
    CASE 8 PRINT("\tState was Dis\n");   END;
    CASE 9 PRINT("\tState was Btst\n");  END;
    CASE 10 PRINT("\tState was Slp\n");  END;
    DEFAULT PRINT("\tState was reserved value: 0x", HEX(status[12..9]), "\n");  END;
  END;
END;

//--------------------------------------------------------------------------------
DecodeCID()()
//--------------------------------------------------------------------------------
  PRINT("Card ID : 0x", FORMAT(cid[127..0], "%032X"), "\n");

  PRINT("\tManufacturer Code  : 0x", FORMAT(cid[127..120], "%02X"), "\n");

  SWITCH deviceType
    CASE DEVICE_TYPE_SD
      PRINT("\tOEM/Application ID : ");PrintText(cid[119..104],2);PRINT("\n");
      PRINT("\tProduct Name       : ");PrintText(cid[103..64],5);PRINT("\n");
      PRINT("\tProduct Revision   : ", HEX(cid[63..60]), ".", HEX(cid[59..56]),   "\n");
      PRINT("\tSerial Number      : 0x", FORMAT(cid[55..24], "%08X"),   "\n");
      PRINT("\tManuf date code    : ", (2000+cid[19..12]), "/", cid[11..8], "\n");
    END;

    CASE DEVICE_TYPE_MMC
      PRINT("\tDevice type        : ");
      SWITCH cid[113..112]
        CASE 0b00 PRINT("Card (removable)\n"); END;
        CASE 0b01 PRINT("BGA (discrete embedded)\n"); END;
        CASE 0b10 PRINT("POP\n"); END;
        CASE 0b11 PRINT("Reserved\n"); END;
      END;
      PRINT("\tOEM/Application ID : 0x", FORMAT(cid[111..104], "%02X"),  "\n");
      PRINT("\tProduct Name       : ");PrintText(cid[103..56],6);PRINT("\n");//, HEX(cid[95..56]),   "\n");
      PRINT("\tProduct Revision   : ", HEX(cid[55..52]), ".", HEX(cid[51..48]),   "\n");
      PRINT("\tSerial Number      : 0x", FORMAT(cid[47..16], "%08X"),   "\n");
      PRINT("\tManuf date code    : ", (1997+cid[11..8]), "/", cid[15..12], "\n");
    END;

    DEFAULT
      PRINT("Unexpected device type in DecodeCID\n");
    END;
  END;
END;

//--------------------------------------------------------------------------------
DecodeCSD()()
//--------------------------------------------------------------------------------
  INT version;
  INT tmp;
  INT i;

  PRINT("Card Specific Data (CSD): 0x", FORMAT(csd[127..0], "%032X"), "\n");

  version := csd[127..126];

  tmp := csd[119..112];
  PRINT("\tTAAC               : ");
  SWITCH(tmp[6..3])
    CASE 0 TAAC := 0; END;
    CASE 1 TAAC := 10; END;
    CASE 2 TAAC := 12; END;
    CASE 3 TAAC := 13; END;
    CASE 4 TAAC := 15; END;
    CASE 5 TAAC := 20; END;
    CASE 6 TAAC := 25; END;
    CASE 7 TAAC := 30; END;
    CASE 8 TAAC := 35; END;
    CASE 9 TAAC := 40; END;
    CASE 10 TAAC := 45; END;
    CASE 11 TAAC := 50; END;
    CASE 12 TAAC := 55; END;
    CASE 13 TAAC := 60; END;
    CASE 14 TAAC := 70; END;
    CASE 15 TAAC := 80; END;
  END;

  PRINT(TAAC / 10, ".", TAAC % 10, " * 10^", tmp[2..0], " ns\n");
  // Calculate TAAC as a time in ms
  FOR i := 0 FOR tmp[2..0]
    TAAC := TAAC * 10;
  END;
  TAAC := TAAC / 10000000;  // Turn the number '10' into 1 ns

  NSAC := csd[111..104];
  PRINT("\tNSAC               : ", NSAC, " * 100 CLK cycles\n");

  PRINT("\tTRAN_SPEED         : ");
  tmp := csd[103..96];
  SWITCH(tmp[6..3])
    CASE 0 TRAN_SPEED := 0; END;
    CASE 1 TRAN_SPEED := 10; END;
    CASE 2 TRAN_SPEED := 12; END;
    CASE 3 TRAN_SPEED := 13; END;
    CASE 4 TRAN_SPEED := 15; END;
    CASE 5 TRAN_SPEED := 20; END;
    CASE 6 TRAN_SPEED := 26; END;
    CASE 7 TRAN_SPEED := 30; END;
    CASE 8 TRAN_SPEED := 35; END;
    CASE 9 TRAN_SPEED := 40; END;
    CASE 10 TRAN_SPEED := 45; END;
    CASE 11 TRAN_SPEED := 52; END;
    CASE 12 TRAN_SPEED := 55; END;
    CASE 13 TRAN_SPEED := 60; END;
    CASE 14 TRAN_SPEED := 70; END;
    CASE 15 TRAN_SPEED := 80; END;
  END;

  PRINT(TRAN_SPEED / 10, ".", TRAN_SPEED % 10, " * 10^", tmp[2..0] + 5, " Hz\n");
  // Calculate TRAN_SPEED as a frequency in Hz
  FOR i := 0 FOR tmp[2..0] + 4
    TRAN_SPEED := TRAN_SPEED * 10;
  END;

  ccc := csd[95..84];
  PRINT("\tCOMMAND CLASS (CCC): ");
  PrintCCC(ccc)();
  PRINT("\n");

  maxReadBlockLength := 1 << csd[83..80];
  maxWriteBlockLength := 1 << csd[25..22];
  PRINT("\tMAX READ_BL_LEN    : ", maxReadBlockLength , " Bytes\n");
  PRINT("\tMAX WRITE_BL_LEN   : ", maxWriteBlockLength , " Bytes\n");

  PRINT("\tDevice Size (Bytes): ");
  deviceSize := 0;

  IF deviceMode = DEVICE_MODE_SECTOR THEN
    SWITCH deviceType
      CASE DEVICE_TYPE_SD
        // version2: C_SIZE field is [69..48], and device size is (C_SIZE + 1) * 512Kbyte
        deviceSize := (csd[69..48] + 1) * 512 * 1024;
        PRINT(deviceSize, " (0x", HEX(deviceSize), ")\n");
      END;

      CASE DEVICE_TYPE_MMC
        PRINT("Specified in EXT_CSD\n");
      END;

      DEFAULT
        PRINT("Unexpected device type in DecodeCSD\n");
      END;
    END;
  ELSE
    // Version 1: C_SIZE field is [73..62], C_SIZE_MULT field is [49..47]
    // and device size is (C_SIZE+1)*(2**(C_SIZE_MULT+2)*maxReadBlockLength
    c_size_mult := csd[49..47];

    deviceSize := (csd[73..62]+1) * (1 << (c_size_mult + 2)) * maxReadBlockLength;
    PRINT(deviceSize, " (0x", HEX(deviceSize), ")\n");

    //PRINT(csd[73..62], "\n");
    //PRINT(csd[49..47], "\n");
  END;
END;

//--------------------------------------------------------------------------------
PrintCCC(INT val)()
//--------------------------------------------------------------------------------
  IF val[0] THEN PRINT("basic (0), "); END;
  IF val[1] THEN PRINT("stream read (1), "); END;
  IF val[2] THEN PRINT("block read (2), "); END;
  IF val[3] THEN PRINT("stream write (3), "); END;
  IF val[4] THEN PRINT("block write (4), "); END;
  IF val[5] THEN PRINT("erase (5), "); END;
  IF val[6] THEN PRINT("write protection (6), "); END;
  IF val[7] THEN PRINT("lock card (7), "); END;
  IF val[8] THEN PRINT("application-specific (8), "); END;
  IF val[9] THEN PRINT("I/O mode (9), "); END;
  IF val[10] THEN PRINT("switch (10), "); END;
  IF val[11] THEN PRINT("reserved (11), "); END;
END;

//--------------------------------------------------------------------------------
DecodeEXT_CSD()()
//--------------------------------------------------------------------------------
  PRINT("Extended Card Specific Data (EXT_CSD):\n");

  // Device size is sector count (SEC_COUNT) * 512
  deviceSize := ext_csd[(215*8)+7..212*8] * 512;
  PRINT("\tDevice Size (Bytes): ", deviceSize, " (0x", HEX(deviceSize), ")\n");
END;

//--------------------------------------------------------------------------------
DecodeSD_Status()()
//--------------------------------------------------------------------------------
  // NOTE: This requires csd to have been read before we can determine the protected area size
  PRINT("SD Status:\n");

  IF deviceMode = DEVICE_MODE_SECTOR THEN
    protectedAreaSize := sd_status[479..448];
  ELSE
    protectedAreaSize := sd_status[479..448] * (1 << (c_size_mult + 2)) * maxReadBlockLength;
  END;

  PRINT("\tProtected (Bytes)  : ", protectedAreaSize, " (0x", HEX(protectedAreaSize), ")\n");

  PRINT("\tSD Card type       : ");

  SWITCH sd_status[495..480]
    CASE 0x0000 PRINT("Regular SD RD/WR Card\n"); END;
    CASE 0x0001 PRINT("SD ROM Card\n"); END;
    CASE 0x0002 PRINT("OTP\n"); END;
    DEFAULT PRINT("Unrecognised value: 0x", HEX(sd_status[495..480]), "\n"); END;
  END;

  PRINT("\tSpeed class        : ");

  SWITCH sd_status[447..440]
    CASE 0x00 PRINT("Class 0\n"); END;
    CASE 0x01 PRINT("Class 2\n"); END;
    CASE 0x02 PRINT("Class 4\n"); END;
    CASE 0x03 PRINT("Class 6\n"); END;
    CASE 0x04 PRINT("Class 10\n"); END;
    DEFAULT PRINT("Reserved value 0x", HEX(sd_status[447..440]), "\n"); END;
  END;
END;


//--------------------------------------------------------------------------------
// Set the bus width to the highest value supported between minWidth and maxWidth,
// returning the width selected
SetBusWidth(INT minWidth, INT maxWidth)(INT widthSelected)
//--------------------------------------------------------------------------------
  INT i;
  INT testWidth;
  INT response;
  INT result;

  widthSelected := 0;

  FOR i := 0 FOR maxWidth - minWidth + 1
    testWidth := maxWidth - i;

    SWITCH deviceType
      CASE DEVICE_TYPE_SD
        IF (testWidth = 1) || (testWidth = 4) THEN
          SendCommand(CMD55, rca:0[15..0])(result, response);

          IF (result = RESULT_PASS) THEN
            IF !(response & STATUS_ERROR_BITS) THEN
              // 1-bit bus sends 0b00, 4-bit bus sends 0b10
              SendCommand(SD_ACMD6, LOG(testWidth))(result, response);

              IF (result = RESULT_PASS) THEN
                IF !(response & STATUS_ERROR_BITS) THEN
                  widthSelected := testWidth;
                  dataInterfaceWidth := testWidth;
                  RETURN;
                ELSIF DEBUG THEN
                  // If this caused an error and we want to know about it then display it,
                  // but otherwise just carry on because we have other widths to try
                  PRINT("Status error bit set with SD_ACMD6\n");
                  DecodeStatus(response);
                END;
              ELSE
                PRINT("Error sending command SD_ACMD6\n");
              END;
            ELSIF DEBUG THEN
              // If this caused an error and we want to know about it then display it,
              // but otherwise just carry on because we have other widths to try
              PRINT("Status error bit set with CMD55\n");
              DecodeStatus(response);
            END;
          ELSE
            PRINT("Error sending command CMD55\n");
          END;
        END;
      END;

      CASE DEVICE_TYPE_MMC
        IF (testWidth = 1) || (testWidth = 4) || (testWidth = 8) THEN
          // 1-bit bus sends 0, 4-bit bus sends 1, 8-bit bus sends 2
          SendMMCSwitchCommand(ACCESS_WRITE_BYTE, EXT_CSD_BUS_WIDTH, testWidth >> 2)(result, response);

          IF (result = RESULT_PASS) THEN
            IF !(response & STATUS_ERROR_BITS) THEN

              // Read the status from the Switch command, because it is delayed by one command
              SendCommand(CMD13, rca[15..0]:0[15..0])(result, response);
              IF (result = RESULT_PASS) THEN
                IF !(response & STATUS_ERROR_BITS) THEN
                  widthSelected := testWidth;
                  dataInterfaceWidth := testWidth;
                  RETURN;
                ELSIF DEBUG THEN
                  // If this caused an error and we want to know about it then display it,
                  // but otherwise just carry on because we have other widths to try
                  PRINT("Status error bit set with CMD13, after sending MMC Switch command\n");
                  DecodeStatus(response);
                END;
              ELSE
                PRINT("Error sending command CMD13\n");
              END;
            ELSIF DEBUG THEN
              // If this caused an error and we want to know about it then display it,
              // but otherwise just carry on because we have other widths to try
              PRINT("Status error bit set when sending MMC Switch command\n");
              DecodeStatus(response);
            END;
          ELSE
            PRINT("Error sending MMC Switch command\n");
          END;
        END;
      END;

      DEFAULT
        PRINT("Unexpected device type in SetBusWidth\n");
      END;
    END;
  END;

  PRINT("Unable to set device data bus width to any value between ", minWidth, " and ", maxWidth, "\n");
END;

//--------------------------------------------------------------------------------
ReadDataBlock(INT startAddress)(INT result, INT data)
//--------------------------------------------------------------------------------
  INT response;
  INT commandAddr;
  INT retryCount := 0;

  IF (startAddress/dataByteLength) * dataByteLength != startAddress THEN
    PRINT("ReadDataBlock startAddress (0x", HEX(startAddress), ") must start at a block boundary, which is multiples of ", dataByteLength, " bytes\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  IF (startAddress + dataByteLength) > deviceSize THEN
    PRINT("Reading ", dataByteLength, " bytes starting at address 0x", HEX(startAddress), " will exceed the size of the device (0x", HEX(deviceSize), " bytes)\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  IF deviceMode = DEVICE_MODE_BYTE THEN
    commandAddr := startAddress;
  ELSE
    commandAddr := startAddress / dataByteLength;
  END;

  DO
    SendCommand(CMD17, commandAddr)(result, response);
    IF result != RESULT_PASS THEN
      PRINT("Error reading data block using command CMD17\n");
    ELSIF (response & STATUS_ERROR_BITS) THEN
      PRINT("Status error bit set when reading data block using command CMD17\n");
      DecodeStatus(response);
      result := RESULT_FAIL;
    END;

    data := lastReadData;
  WHILE (result != RESULT_PASS) && (retryCount < READ_BLOCK_RETRY_COUNT)
    retryCount := retryCount + 1;
    PRINT("Retrying data read...\n");
  END;
END;

//--------------------------------------------------------------------------------
WriteDataBlock(INT startAddress, INT data)(INT result)
//--------------------------------------------------------------------------------
  INT response;
  INT commandAddr;

  IF (startAddress/dataByteLength) * dataByteLength != startAddress THEN
    PRINT("WriteDataBlock startAddress (0x", HEX(startAddress), ") must start at a block boundary, which is multiples of ", dataByteLength, " bytes\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  IF (startAddress + dataByteLength) >= deviceSize THEN
    PRINT("Writing ", dataByteLength, " bytes starting at address 0x", HEX(startAddress), " will exceed the size of the device (0x", HEX(deviceSize), " bytes)\n");
    result := RESULT_FAIL;
    RETURN;
  END;

  IF deviceMode = DEVICE_MODE_BYTE THEN
    commandAddr := startAddress;
  ELSE
    commandAddr := startAddress / dataByteLength;
  END;

  nextWriteData := data;

  SendCommand(CMD24, commandAddr)(result, response);
  IF result != RESULT_PASS THEN
    PRINT("Error writing data block using command CMD24\n");
  ELSIF (response & STATUS_ERROR_BITS) THEN
    PRINT("Status error bit set when writing data block using command CMD24\n");
    DecodeStatus(response);
    result := RESULT_FAIL;
  END;
END;

//--------------------------------------------------------------------------------
SendMMCSwitchCommand(INT access WIDTH 2, INT index WIDTH 8, INT value WIDTH 8)(INT result, INT response)
//--------------------------------------------------------------------------------
  SendCommand(MMC_CMD6, 0[5..0]:access[1..0]:index[7..0]:value[7..0]:0[4..0]:0[2..0])(result, response);
END;

//--------------------------------------------------------------------------------
SendCommand(INT cmd, INT param WIDTH 32)(INT result, INT response)
//--------------------------------------------------------------------------------
  INT crc        WIDTH 7;
  INT data       WIDTH 48;
  INT i;
  INT responseResult, dataResult;
  INT dataFormattedAsBytes;

  INT readDataLength;
  INT waitForBusy;

  result := RESULT_PASS;

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

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

  IF !(((cmd & CCC_MASK) >> CCC_OFFSET) & ccc) THEN
    PRINT("Device does not support required Card Command Class (CCC) for command:\n");
    result := RESULT_FAIL;
  END;

  IF DEBUG || (result != RESULT_PASS) THEN
    PRINT("Tx Command ", cmd & CMD_INDEX_MASK, "(0x", HEX(cmd & CMD_INDEX_MASK), "), param=0x", HEX(param), ", data=0x", HEX(data), ", Response type=");
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_NONE THEN PRINT("None, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R1 THEN PRINT("R1, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R1B THEN PRINT("R1B, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R2 THEN PRINT("R2, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R3 THEN PRINT("R3, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R4 THEN PRINT("R4, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R5 THEN PRINT("R5, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R6 THEN PRINT("R6, "); END;
    IF (cmd & RSP_TYPE_MASK) = RSP_TYPE_R7 THEN PRINT("R7, "); END;

    PRINT("Data type: ");

    SWITCH cmd & DATA_TYPE_MASK
      CASE DATA_TYPE_NONE PRINT("None, "); END;
      CASE DATA_TYPE_READ PRINT("Read, "); END;
      CASE DATA_TYPE_WRITE PRINT("Write, "); END;
      DEFAULT PRINT("INVALID VALUE!, "); END;
    END;
    PRINT("\n");

    PRINT("Required command class: ");
    PrintCCC((cmd & CCC_MASK) >> CCC_OFFSET)();
    PRINT("\n");
  END;

  IF result != RESULT_PASS THEN RETURN; END;

  // Min clocks between consecutive commands
  Clocks(CLOCKS_BEFORE_COMMAND);

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

  response := 0;

  SET CMD := I;

  IF (cmd & DATA_TYPE_MASK) = DATA_TYPE_READ THEN
    readDataLength := dataByteLength;
  ELSE
    readDataLength := 0;
  END;

  waitForBusy := ((cmd & RSP_LENGTH_MASK) = RSP_LENGTH48B);

  dataFormattedAsBytes := ((cmd & DATA_IN_BYTES_MASK) = DATA_IN_BYTES);

  SWITCH (cmd & RSP_LENGTH_MASK)
    CASE RSP_LENGTH0
      result := RESULT_PASS;
    END;

    CASE RSP_LENGTH48, RSP_LENGTH48B
      ReadResponseAndData(48, readDataLength, dataInterfaceWidth, waitForBusy, dataFormattedAsBytes)(responseResult, dataResult, response, lastReadData, lastReadCRC);

      IF responseResult = RESULT_PASS THEN
        IF DEBUG THEN
          PRINT("Rx response : Command ", response[45..40], " (0x", HEX(response[45..40]), "), value 0x", HEX(response[39..8]), ", CRC 0x", HEX(response[7..1]), "\n");
        END;

        IF cmd & CHECK_INDEX_MASK THEN
          IF (response[45..40] != (cmd & CMD_INDEX_MASK)) THEN
            PRINT("Response Index error, Rxvd 0x", HEX(response[45..40]), ", expected 0x", HEX(cmd & CMD_INDEX_MASK), "\n");
            result := RESULT_FAIL;
            RETURN;
          END;
        END;

        IF cmd & CHECK_CRC_MASK THEN
          CRC7(response, 47, 8)(crc);

          IF (response[7..1] != crc) THEN
            PRINT("Response CRC error, Rxvd 0x", HEX(response[7..1]), ", expected 0x", HEX(crc), "\n");
            result := RESULT_FAIL;
            RETURN;
          END;
        END;
      END;

      response := response[39..8];

      IF (responseResult = RESULT_PASS) && (dataResult = RESULT_PASS) THEN
        result := RESULT_PASS;
      ELSE
        result := RESULT_FAIL;
        RETURN;
      END;
    END;

    CASE RSP_LENGTH136
      ReadResponseAndData(136, readDataLength, dataInterfaceWidth, waitForBusy, dataFormattedAsBytes)(responseResult, dataResult, response, lastReadData, lastReadCRC);

      IF responseResult = RESULT_PASS THEN
        IF DEBUG THEN
          PRINT("Rx response : Check bits 0x", HEX(response[133..128]), ", value 0x", HEX(response[127..8]), ", CRC 0x", HEX(response[7..1]), "\n");
        END;

        IF cmd & CHECK_INDEX_MASK THEN
          PRINT("Checking response index not supported for response length 136\n");
          EXIT();
        END;

        IF cmd & CHECK_CRC_MASK THEN
          CRC7(response, 127, 8)(crc);

          IF (response[7..1] != crc) THEN
            PRINT("Response CRC error, Rxvd 0x", HEX(response[7..1]), ", expected 0x", HEX(crc), "\n");
            result := RESULT_FAIL;
            RETURN;
          END;
        END;
      END;

      response := response[127..0];

      IF (responseResult = RESULT_PASS) && (dataResult = RESULT_PASS) THEN
        result := RESULT_PASS;
      ELSE
        result := RESULT_FAIL;
        RETURN;
      END;
    END;

    DEFAULT
      PRINT("Unexpected repsonse type for command 0x",HEX(cmd), "\n");
      result := RESULT_FAIL;
      RETURN;
    END;
  END;

  IF (cmd & DATA_TYPE_MASK) = DATA_TYPE_WRITE THEN
    // Clocks required before sending data
    Clocks(3);

    WriteData(dataByteLength, dataInterfaceWidth, dataFormattedAsBytes, nextWriteData)(result);
  END;
END;

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

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

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

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

  IF DEBUG THEN
    PRINT("CRC7 : (", msb-lsb+1, " bits) 0x", HEX(data[msb..lsb]), "=", BIN(crc_reg), "(0x", HEX(crc_reg), ")\n");
  END;

  result := crc_reg;
END;

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

  FOR this_bit := 0 FOR msb-lsb+1
    crc_temp       := crc_reg;
    crc_reg[15..1] := crc_temp[14..0];
    crc_reg[0]     := (data[msb-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 : (", msb-lsb+1, " bits) 0x", HEX(data[msb..lsb]), "=", BIN(crc_reg), "(0x", HEX(crc_reg), ")\n");
  END;

  result := crc_reg;
END;

//--------------------------------------------------------------------------------
ReadResponseAndData(INT nResponseBits, INT nDataBytes, INT dataBusWidth, INT waitForBusy, INT dataFormattedAsBytes)(INT responseResult, INT dataResult, INT response, INT data, INT dataCRC)
//--------------------------------------------------------------------------------
  INT i;

  // prefix: r = response, d = data/busy
  INT rCounter := 0;
  INT dCounter := 0;
  INT rStarted;
  INT dStarted;
  INT rFinished;
  INT dFinished;

  INT busyTimeout;

  INT bitsPerDataPin := 0;

  INT dataMixed;
  INT dataSorted;
  INT dataOffset;

  INT faultDetectData;

  INT crc;

    // response bit and data bits
  INT rBit, dBits, dBitsPrev;

  dataResult := RESULT_FAIL;
  responseResult := RESULT_FAIL;

  IF nDataBytes THEN
    IF waitForBusy THEN
      PRINT("ReadResponseAndData cannot read data and wait for busy - at most one can be selected simultaneously\n");
      RETURN;
    END;

    IF (8 / dataBusWidth) * dataBusWidth != 8 THEN
      PRINT("Invalid dataBusWidth - must be 1, 2, 4 or 8\n");
      RETURN;
    ELSIF DEBUG THEN
      PRINT("Reading ", nDataBytes, " bytes on ", dataBusWidth, " data pins\n");
    END;

    bitsPerDataPin := 17 + (nDataBytes * 8) / dataBusWidth;

    faultDetectData := 0[bitsPerDataPin * dataBusWidth - 1..0];
  END;

  // If we're not expecting either data (or busy) or a response, mark the signals as though that
  // has already completed
  rFinished := (nResponseBits = 0);
  dFinished := (nDataBytes = 0) && !waitForBusy;
  rStarted := rFinished;
  dStarted := dFinished;

  busyTimeout := NOW() + MAX_BUSY_DELAY;

  SET CMD := I, DATA := I;
  DO
    SET CLK := 0;
    dBitsPrev := dBits;
    SET CLK := 1, rBit := CMD, dBits := DATA;

    // Increment command and data counters
    rCounter := rCounter + 1;
    dCounter := dCounter + 1;

    IF !rStarted && !rBit THEN
      IF DEBUG THEN PRINT("Response start detected on clock edge ", rCounter, ".\n"); END;
      rStarted := TRUE;
      rCounter := 1;  // This low is actually the first bit we've received
    END;

    IF rStarted && !rFinished THEN
      response := (response << 1) | rBit;
      rFinished := (rCounter >= nResponseBits);
    END;

    IF !dStarted THEN
      IF waitForBusy && !dBits[0] THEN
        IF DEBUG THEN PRINT("Busy detected on clock edge ", dCounter, ".\n"); END;
        dStarted := TRUE;
        dCounter := 0;
      ELSIF (dBits[dataBusWidth-1..0] = 0) && (dBitsPrev[dataBusWidth-1..0] = ~0[dataBusWidth-1..0]) THEN
        IF DEBUG THEN PRINT("Data start detected on clock edge ", dCounter, ": data = 0x", HEX(dBits), ", prevData = 0x", HEX(dBitsPrev), "\n"); END;
        dStarted := TRUE;
        dCounter := 0;  // This low we're not concerned about and doesnt count towards the amount of data
      END;
    END;

    IF dStarted && !dFinished THEN
      IF waitForBusy THEN
        IF dBits[0] THEN
          IF DEBUG THEN PRINT("Busy released after ", dCounter, " clock edges.\n"); END;
          dFinished := TRUE;
        END;
      ELSIF dCounter > 0 THEN
        // We only get useful data when not on the same cycle as when dStarted got set
        faultDetectData[dCounter * dataBusWidth - 1..(dCounter - 1) * dataBusWidth] := dBits[dataBusWidth - 1..0];

        FOR i := 0 FOR dataBusWidth
          dataMixed[(bitsPerDataPin - dCounter) + bitsPerDataPin * i] := dBits[i];
        END;

        IF dataFormattedAsBytes THEN
          dataOffset := (dCounter - 1) * dataBusWidth;
          dataOffset[2..0] := 8 - dataBusWidth - dataOffset[2..0];

          dataSorted[dataOffset + dataBusWidth - 1..dataOffset] := dBits[dataBusWidth-1..0];
        ELSE
          dataOffset := ((nDataBytes * 8) / dataBusWidth + 1);
          IF dataOffset > dCounter THEN
            dataOffset := dataOffset - dCounter;

            dataSorted[dataOffset * dataBusWidth - 1..(dataOffset - 1) * dataBusWidth] := dBits[dataBusWidth-1..0];
          END;
        END;

        dFinished := (dCounter >= bitsPerDataPin);
      END;
    END;
  UNTIL (waitForBusy && dStarted && !dFinished && (NOW() > busyTimeout))
        || (waitForBusy && !nResponseBits && !dStarted && (dCounter >= MAX_CLOCKS_BEFORE_BUSY))
        || (!rStarted && (rCounter >= MAX_CLOCKS_BEFORE_RESPONSE))
        || (!dStarted && (dCounter >= MAX_CLOCKS_BEFORE_DATA) && nDataBytes)
        || (rFinished && (dFinished || (!dStarted && waitForBusy)));
  END;

  // The only time we generate an error with waitForBusy is if we detected it start but not finish
  IF waitForBusy && dStarted && !dFinished THEN
    PRINT("Busy signal detected, but did not clear within ", MAX_BUSY_DELAY, " ms.\n");
    RETURN;
  END;

  IF nResponseBits THEN
    IF !rStarted THEN
      IF !disableNoResponseMessage || DEBUG THEN PRINT("No response from device\n"); END;
      RETURN;
    END;

    IF response[nResponseBits-1..nResponseBits-2] != 0b00 THEN
      PRINT("Invalid response start: read 0b", BIN(response[nResponseBits-1..nResponseBits-2]), ", expected 0b00\n");
      RETURN;
    END;

    IF !response[0] THEN
      PRINT("Invalid response end: read 0b", response[0], " expected 0b1\n");
      RETURN;
    END;
  END;

  // By this point we know the response was ok (if we expected one)
  // Assume the data is also ok until we find something wrong with it
  responseResult := RESULT_PASS;
  dataResult := RESULT_PASS;

  IF nDataBytes THEN
    IF !dStarted THEN
      PRINT("No data received from device within timeout\n");
      dataResult := RESULT_FAIL;
      RETURN;
    END;

    AnalyseFaultDataCustomWidth(dataBusWidth, ~0[dataBusWidth - 1..0], bitsPerDataPin)(faultDetectData);

    dataCRC := 0;

    // Extract and test each data channel for stop bit and CRC
    FOR i := 0 FOR dataBusWidth

      data := dataMixed[(i+1)*bitsPerDataPin-1..i*bitsPerDataPin];

      IF DEBUG THEN
        PRINT("\tRead data [", i, "] = 0x", HEX(data >> 17), "\n");
        PRINT("\tData CRC  [", i, "] = 0x", HEX(data[16..1]), "\n");
        PRINT("\tData Stop [", i, "] = ", data[0], "\n");
      END;

      // Check data pin stop bit
      IF !data[0] THEN
        PRINT("Read data [", i, "] stop bit error\n");
        // Don't return yet, so we see how many of the data bits have errors
        dataResult := RESULT_FAIL;
      END;

      // Check the data line CRC
      CRC16(data, bitsPerDataPin, 17)(crc);
      IF (data[16..1]  != crc) THEN
        PRINT("Read data [", i, "] CRC error\n");
        dataResult := RESULT_FAIL;
        // Don't return now, so we can see how many data channels failed their CRC
      ELSE
        IF DEBUG THEN PRINT("\tData [", i, "] CRC OK\n"); END;
      END;

      // Concatenate the CRCs together to return them - primarily to be used
      // when checking for shorts between pins
      dataCRC[(i+1)*16-1..i*16] := data[16..1];
    END;

    // Reconstruct the data from all the required bitstreams
    data := dataSorted[(nDataBytes * 8)-1..0];
  END;
END;

//--------------------------------------------------------------------------------
WriteData(INT nDataBytes, INT dataBusWidth, INT dataFormattedAsBytes, INT data)(INT result)
//--------------------------------------------------------------------------------
  INT i, j;

  INT bitsPerDataPin := 0;

  INT dataMixed;
  INT dataOffset;

  INT dBits;

  INT crc;
  INT crcStatus;

  INT dBit;

  INT dStarted := FALSE;
  INT dCounter := 0;

  INT busyTimeout;

  result := RESULT_FAIL;

  IF !nDataBytes THEN
    PRINT("WriteData called with nDataBytes of 0\n");
    RETURN;
  ELSE
    IF (8 / dataBusWidth) * dataBusWidth != 8 THEN
      PRINT("Invalid dataBusWidth - must be 1, 2, 4 or 8\n");
      RETURN;
    ELSIF DEBUG THEN
      PRINT("Writing ", nDataBytes, " bytes on ", dataBusWidth, " data pins\n");
    END;

    bitsPerDataPin := 16 + (nDataBytes * 8) / dataBusWidth;
  END;

  IF !dataFormattedAsBytes THEN
    PRINT("WriteData only supports data formatted as bytes\n");
    RETURN;
  END;

  // Perpare data for writing
  FOR j := 0 FOR bitsPerDataPin
    dataOffset := (bitsPerDataPin - j - 1) * dataBusWidth;
    dataOffset[2..0] := 8 - dataBusWidth - dataOffset[2..0];

    FOR i := 0 FOR dataBusWidth
      dataMixed[j + bitsPerDataPin * i] := data[dataOffset + i];
    END;
  END;

  FOR i := 0 FOR dataBusWidth
    // Calculate CRCs
    CRC16(dataMixed, (i+1)*bitsPerDataPin - 1, i*bitsPerDataPin + 16)(crc);

    // Add CRC to end
    dataMixed[i*bitsPerDataPin+15..i*bitsPerDataPin] := crc[15..0];
  END;

  // Start sending the data
  // Start with a 0, making sure the transition on DATA is with CLK low
  SET CLK := 0;
  SET DATA[dataBusWidth-1..0] := 0;
  SET CLK := 1;

  // Now the rest of the data
  FOR j := 0 FOR bitsPerDataPin
    FOR i := 0 FOR dataBusWidth
      dBits [i] := dataMixed[bitsPerDataPin - j - 1 + bitsPerDataPin * i];
    END;

    SET CLK := 0, DATA[dataBusWidth-1..0] := dBits;
    SET CLK := 1;
  END;

  // Finally set all lines high again
  SET CLK := 0, DATA[dataBusWidth-1..0] := ~0[dataBusWidth-1..0];
  SET CLK := 1;
  SET CLK := 0;

  // Revert back to inputs and monitor for busy from the card
  SET DATA := I;

  DO
    SET CLK := 0;
    SET CLK := 1, dBit := DATA[0];

    dCounter := dCounter + 1;

    IF !dStarted && !dBit THEN
      IF DEBUG THEN PRINT("CRC Status start detected on clock edge ", dCounter, ".\n"); END;
      dStarted := TRUE;
      dCounter := 1; // Count this low as the first of the 5 bits
    END;

    IF dStarted THEN
      crcStatus := (crcStatus << 1) | dBit;
    END;
  UNTIL (dStarted && (dCounter = 5))
      || (!dStarted && (dCounter >= MAX_CLOCKS_BEFORE_CRC_STATUS))
  END;

  IF !dStarted THEN
    PRINT("No CRC Status response within timeout\n");
    RETURN;
  END;

  IF crcStatus[0] != 1 THEN
    PRINT("Invalid CRC Status response end: read 0b", crcStatus[0], " expected 0b1\n");
    RETURN;
  END;

  IF crcStatus[3..1] != 0b010 THEN
    PRINT("Invalid CRC Status: read 0b", crcStatus[3..1], " expected 0b010\n");
    RETURN;
  END;

  dStarted := FALSE;
  dCounter := 0;

  busyTimeout := NOW() + MAX_BUSY_DELAY;

  DO
    SET CLK := 0;
    SET CLK := 1, dBit := DATA[0];

    dCounter := dCounter + 1;

    IF !dStarted && !dBit THEN
      IF DEBUG THEN PRINT("Busy following CRC Status detected on clock edge ", dCounter, ".\n"); END;
      dStarted := 1;
      dCounter := 0;
    END;
  UNTIL (dStarted && (dBit || (NOW() > busyTimeout)))
      || (!dStarted && (dCounter >= MAX_CLOCKS_BEFORE_BUSY))
  END;

  IF dStarted THEN
    IF dBit THEN
      IF DEBUG THEN PRINT("Busy following CRC Status released after ", dCounter, " clock edges.\n"); END;
    ELSE
      PRINT("Busy signal detected following CRC Status, but did not clear within ", MAX_BUSY_DELAY, " ms.\n");
      RETURN;
    END;
  END;

  result := RESULT_PASS;
END;


/******************************** PROGRAMMING CODE ***************************/
/*
ProgramDevice()(INT result)
  INT tmp;
  INT dataset;
  INT startAddr;
  INT endAddr;
  INT currAddr;
  INT startTime := NOW();
  INT elapsedTime;
  STRING filename := "MLO.dat";

  PRINT("\nProgramming eMMC device ", DEVICE_REF, "...\n");

  InitialiseDevice()(result);
  IF result != RESULT_PASS THEN RETURN; END;

  // Select widest bus we can
  SetBusWidth(1, WIDTHOF(DATA))(tmp);

  ReadBINFile(filename, 0, 8, 8)(dataset, startAddr, endAddr, result);
  IF result != RESULT_PASS THEN RETURN; END;

  endAddr := endAddr - 1;

  // Get start of first page to program
  currAddr := startAddr - startAddr[8..0];

  DO WHILE currAddr <= endAddr
    WriteDataBlock(currAddr, dataset[((currAddr+512)*8)-1..currAddr*8])(result);
    IF result != RESULT_PASS THEN
      PRINT("Error writing to data block starting at address 0x", HEX(currAddr), "\n");
      RETURN;
    END;
    currAddr := currAddr + 512;

    PRINT(currAddr - startAddr, " of ", endAddr - startAddr + 1, " bytes complete\r");
  END;

  elapsedTime := NOW() - startTime;

  PRINT("Programming of device ", DEVICE_REF, " completed in ", elapsedTime / 1000, " seconds.\n");
END;

VerifyDevice()(INT result)
  INT tmp;
  INT dataset;
  INT startAddr;
  INT endAddr;
  INT currAddr;
  INT readData;
  INT dataStart;
  INT dataEnd;
  INT startTime := NOW();
  INT elapsedTime;
  STRING filename := "MLO.dat";

  PRINT("\nVerifying eMMC device ", DEVICE_REF, "...\n");

  InitialiseDevice()(result);
  IF result != RESULT_PASS THEN RETURN; END;

  // Select widest bus we can
  SetBusWidth(1, WIDTHOF(DATA))(tmp);

  ReadBINFile(filename, 0, 8, 8)(dataset, startAddr, endAddr, result);
  IF result != RESULT_PASS THEN RETURN; END;

  endAddr := endAddr - 1;

  // Get start of first page to verify
  currAddr := startAddr - startAddr[8..0];

  DO WHILE currAddr <= endAddr
    ReadDataBlock(currAddr)(result, readData);
    IF result != RESULT_PASS THEN
      PRINT("Error reading data block starting at address 0x", HEX(currAddr), "\n");
      RETURN;
    END;

    IF currAddr < startAddr THEN
      dataStart := startAddr;
    ELSE
      dataStart := currAddr;
    END;

    IF currAddr + 512 > endAddr THEN
      dataEnd := endAddr;
    ELSE
      dataEnd := currAddr + 511;
    END;

    IF readData[((dataEnd - currAddr + 1) * 8) - 1..(dataStart - currAddr) * 8] != dataset[((dataEnd + 1) * 8) - 1..dataStart * 8] THEN
      PRINT("Data mismatch detected in data block starting at address 0x", HEX(currAddr), "\n");
      result := RESULT_FAIL;
      RETURN;
    END;

    currAddr := currAddr + 512;

    PRINT(currAddr - startAddr, " of ", endAddr - startAddr + 1, " bytes complete\r");
  END;

  elapsedTime := NOW() - startTime;

  PRINT("Verifying of device ", DEVICE_REF, " completed in ", elapsedTime / 1000, " seconds.\n");
END;

*/