Enable at91 systems to boot from high capacity SD cards.

This also fixes a few minor violations of the SD protocol, such as running
the bus at high speed during the card identification sequence.

The sdcard_init() routine now probes for SDHC cards so that later read
requests can make needed adjustments between block and byte offsets based
on card type.

There is a new MCI_readblocks() function that takes block number and block
count parameters instead of byte-offset values.  Using this routine, boot
loader code can load a kernel from any location on an SDHC or standard SD.

The old MCI_read() interface remains unchanged so that existing customized
boot loader code will still keep working without changes.  Using this
routine, boot loaders can load a kernel from anywhere in the first 4GB of
an SDHC card (or of course any location on a standard SD card).

A new sdcard_use4wire() routine allows boot loaders to request 4-bit
transfers; it should be called after sdcard_init().  The sdcard_init()
routine no longer assumes the hardware is 4-wire capable and by default
sets things up for 1-bit transfers.  (4-wire mode is unreliable on
at91rm9200, works on later SoCs.)

PR:		155894
Submitted by:	me.  years ago.

3 files changed, 278 insertions, 142 deletions

diff --git a/sys/boot/arm/at91/libat91/mci_device.h b/sys/boot/arm/at91/libat91/mci_device.h
index 903454c..c2b75ad 100644
--- a/sys/boot/arm/at91/libat91/mci_device.h
+++ b/sys/boot/arm/at91/libat91/mci_device.h
@@ -90,6 +90,8 @@ typedef struct _AT91S_MciDevice
 {
 	volatile unsigned char	state;
 	unsigned char	SDCard_bus_width;
+	unsigned char	IsSDv2;
+	unsigned char	IsSDHC;
 	unsigned int 	RCA;		// RCA
 	unsigned int	READ_BL_LEN;
 #ifdef REPORT_SIZE
@@ -202,7 +204,9 @@ typedef struct _AT91S_MciDevice
      MMC_MAXLAT)
 #define GO_INACTIVE_STATE_CMD \
      (15 | MMC_RSPTYP_NO)
-
+#define SD_SEND_IF_COND_CMD \
+     (8 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_MAXLAT)
 //*------------------------------------------------
 //* Class 2 commands: Block oriented Read commands
 //*------------------------------------------------
@@ -264,7 +268,7 @@ typedef struct _AT91S_MciDevice
 #define SDCARD_STATUS_CMD					(13 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
 #define SDCARD_SEND_NUM_WR_BLOCKS_CMD		(22 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
 #define SDCARD_SET_WR_BLK_ERASE_COUNT_CMD	(23 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
-#define SDCARD_APP_OP_COND_CMD			(41 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO )
+#define SDCARD_APP_OP_COND_CMD			(41 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO | MMC_MAXLAT)
 #define SDCARD_SET_CLR_CARD_DETECT_CMD	(42 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
 #define SDCARD_SEND_SCR_CMD				(51 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
 
@@ -306,7 +310,8 @@ typedef struct _AT91S_MciDevice
 #define AT91C_VDD_33_34					(1 << 21)
 #define AT91C_VDD_34_35					(1 << 22)
 #define AT91C_VDD_35_36					(1 << 23)
-#define AT91C_CARD_POWER_UP_BUSY		(1U << 31)
+#define AT91C_CCS					(1 << 30)
+#define AT91C_CARD_POWER_UP_DONE			(1U << 31)
 
 #define AT91C_MMC_HOST_VOLTAGE_RANGE	(AT91C_VDD_27_28 | AT91C_VDD_28_29  | \
     AT91C_VDD_29_30 | AT91C_VDD_30_31 | AT91C_VDD_31_32 | AT91C_VDD_32_33)
diff --git a/sys/boot/arm/at91/libat91/sd-card.c b/sys/boot/arm/at91/libat91/sd-card.c
index acc8e33..803d782 100644
--- a/sys/boot/arm/at91/libat91/sd-card.c
+++ b/sys/boot/arm/at91/libat91/sd-card.c
@@ -47,35 +47,38 @@
 #include "lib.h"
 #include "sd-card.h"
 
-#define AT91C_MCI_TIMEOUT	1000000   /* For AT91F_MCIDeviceWaitReady */
-#define BUFFER_SIZE_MCI_DEVICE	512
-#define MASTER_CLOCK		60000000
+#define AT91C_MCI_TIMEOUT       1000000   /* For AT91F_MCIDeviceWaitReady */
+#define SD_BLOCK_SIZE           512
 
 //* Global Variables
-AT91S_MciDevice			MCI_Device;
-char				Buffer[BUFFER_SIZE_MCI_DEVICE];
+static AT91S_MciDevice          MCI_Device;
 
 /******************************************************************************
 **Error return codes
 ******************************************************************************/
-#define MCI_UNSUPP_SIZE_ERROR		5
+#define MCI_UNSUPP_SIZE_ERROR   5
 #define MCI_UNSUPP_OFFSET_ERROR 6
 
 //*----------------------------------------------------------------------------
 //* \fn    MCIDeviceWaitReady
 //* \brief Wait for MCI Device ready
 //*----------------------------------------------------------------------------
-static void
+static unsigned int
 MCIDeviceWaitReady(unsigned int timeout)
 {
-	volatile int status;
-	
+	volatile unsigned int status;
+	int waitfor;
+
+	if (MCI_Device.state == AT91C_MCI_RX_SINGLE_BLOCK)
+		waitfor = AT91C_MCI_RXBUFF;
+	else
+		waitfor = AT91C_MCI_NOTBUSY;
 	do
 	{
 		status = AT91C_BASE_MCI->MCI_SR;
 		timeout--;
 	}
-	while( !(status & AT91C_MCI_NOTBUSY)  && (timeout>0) );	
+	while( !(status & waitfor) && (timeout>0) );	
 
 	status = AT91C_BASE_MCI->MCI_SR;
 
@@ -92,25 +95,39 @@ MCIDeviceWaitReady(unsigned int timeout)
  		AT91C_BASE_PDC_MCI->PDC_PTCR = AT91C_PDC_RXTDIS;
 		MCI_Device.state = AT91C_MCI_IDLE;
 	}	// End of if AT91C_MCI_RXBUFF
+
+	//printf("WaitReady returning status %x\n", status);
+
+	return status;
 }
 
 static inline unsigned int
-swap(unsigned int a)
+swap(unsigned int v)
 {
-    return (((a & 0xff) << 24) | ((a & 0xff00) << 8) | ((a & 0xff0000) >> 8)
-      | ((a & 0xff000000) >> 24));
+	unsigned int t1;
+
+	__asm __volatile("eor %1, %0, %0, ror #16\n"
+	    		"bic %1, %1, #0x00ff0000\n"
+			"mov %0, %0, ror #8\n"
+			"eor %0, %0, %1, lsr #8\n"
+			 : "+r" (v), "=r" (t1));
+	
+	return (v);
 }
 
-static inline void
+inline static unsigned int
 wait_ready()
 {
 	int status;
+	int timeout = AT91C_MCI_TIMEOUT;
 
 	// wait for CMDRDY Status flag to read the response
 	do
 	{
 		status = AT91C_BASE_MCI->MCI_SR;
-	} while( !(status & AT91C_MCI_CMDRDY) );
+	} while( !(status & AT91C_MCI_CMDRDY) && (--timeout > 0)  );
+
+	return status;
 }
 
 //*----------------------------------------------------------------------------
@@ -122,18 +139,24 @@ MCI_SendCommand(
 	unsigned int Cmd,
 	unsigned int Arg)
 {
-	unsigned int	error;
+	unsigned int error;
+	unsigned int errorMask = AT91C_MCI_SR_ERROR;
+	unsigned int opcode = Cmd & 0x3F;
+
+	//printf("SendCmd %d (%x) arg %x\n", opcode, Cmd, Arg);
+
+	// Don't check response CRC on ACMD41 (R3 response type).
+
+	if (opcode == 41)
+		errorMask &= ~AT91C_MCI_RCRCE;
 
 	AT91C_BASE_MCI->MCI_ARGR = Arg;
 	AT91C_BASE_MCI->MCI_CMDR = Cmd;
 
-//	printf("CMDR %x ARG %x\n", Cmd, Arg);
-	wait_ready();
-	// Test error  ==> if crc error and response R3 ==> don't check error
-	error = (AT91C_BASE_MCI->MCI_SR) & AT91C_MCI_SR_ERROR;
-	if (error != 0) {
-		if (error != AT91C_MCI_RCRCE)
-			return (1);
+	error = wait_ready();
+
+	if ((error & errorMask) != 0) {
+		return (1);
 	}
 	return 0;
 }
@@ -146,94 +169,98 @@ static unsigned int
 MCI_GetStatus()
 {
 	if (MCI_SendCommand(SEND_STATUS_CMD, MCI_Device.RCA << 16))
-		return AT91C_CMD_SEND_ERROR;
+		return 0;
 	return (AT91C_BASE_MCI->MCI_RSPR[0]);
+
 }
 
 //*----------------------------------------------------------------------------
 //* \fn    MCI_ReadBlock
-//* \brief Read an ENTIRE block or PARTIAL block
+//* \brief Start the read for a single 512-byte block
 //*----------------------------------------------------------------------------
 static int
-MCI_ReadBlock(int src, unsigned int *dataBuffer, int sizeToRead)
+MCI_StartReadBlock(unsigned blknum, void *dataBuffer)
 {
-//	unsigned log2sl = MCI_Device.READ_BL_LEN;
-//	unsigned sectorLength = 1 << log2sl;
-	unsigned sectorLength = 512;
-
-	///////////////////////////////////////////////////////////////////////
-	if (MCI_Device.state != AT91C_MCI_IDLE)
-		return 1;
-    
-	if ((MCI_GetStatus() & AT91C_SR_READY_FOR_DATA) == 0)
-		return 1;
-
-	///////////////////////////////////////////////////////////////////////
-      
         // Init Mode Register
-	AT91C_BASE_MCI->MCI_MR |= ((sectorLength << 16) | AT91C_MCI_PDCMODE);
+	AT91C_BASE_MCI->MCI_MR |= ((SD_BLOCK_SIZE << 16) | AT91C_MCI_PDCMODE);
 	 
-	sizeToRead = sizeToRead / 4;
-
+	// (PDC) Receiver Transfer Enable
 	AT91C_BASE_PDC_MCI->PDC_PTCR = (AT91C_PDC_TXTDIS | AT91C_PDC_RXTDIS);
 	AT91C_BASE_PDC_MCI->PDC_RPR  = (unsigned int)dataBuffer;
-	AT91C_BASE_PDC_MCI->PDC_RCR  = sizeToRead;
+	AT91C_BASE_PDC_MCI->PDC_RCR  = SD_BLOCK_SIZE / 4;;
+	AT91C_BASE_PDC_MCI->PDC_PTCR = AT91C_PDC_RXTEN;
+
+	// SDHC wants block offset, non-HC wants byte offset.
+	if (!MCI_Device.IsSDHC)
+		blknum *= SD_BLOCK_SIZE;
 
 	// Send the Read single block command
-	if (MCI_SendCommand(READ_SINGLE_BLOCK_CMD, src))
+	if (MCI_SendCommand(READ_SINGLE_BLOCK_CMD, blknum)) {
 		return AT91C_READ_ERROR;
+	}
 	MCI_Device.state = AT91C_MCI_RX_SINGLE_BLOCK;
 
-	// Enable AT91C_MCI_RXBUFF Interrupt
-	AT91C_BASE_MCI->MCI_IER = AT91C_MCI_RXBUFF;
-
-	// (PDC) Receiver Transfer Enable
-	AT91C_BASE_PDC_MCI->PDC_PTCR = AT91C_PDC_RXTEN;
-	
 	return 0;
 }
 
+//*----------------------------------------------------------------------------
+//* \fn    MCI_readblocks
+//* \brief Read one or more blocks
+//*----------------------------------------------------------------------------
 int
-MCI_read(char* dest, unsigned source, unsigned length)
+MCI_readblocks(char* dest, unsigned blknum, unsigned blkcount)
 {
-//	unsigned log2sl = MCI_Device.READ_BL_LEN;
-//	unsigned sectorLength = 1 << log2sl;
-	unsigned sectorLength = 512;
-	int sizeToRead;
+	unsigned int status;
 	unsigned int *walker;
 
-	//As long as there is data to read
-	while (length)
-	{
-		if (length > sectorLength)
-			sizeToRead = sectorLength;
-		else
-			sizeToRead = length;
+	if (MCI_Device.state != AT91C_MCI_IDLE) {
+		return 1;
+	}
+
+	if ((MCI_GetStatus() & AT91C_SR_READY_FOR_DATA) == 0) {
+		return 1;
+	}
 
-		MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+	// As long as there is data to read
+	while (blkcount)
+	{
 		//Do the reading
-		if (MCI_ReadBlock(source,
-			(unsigned int*)dest, sizeToRead))
+		if (MCI_StartReadBlock(blknum, dest))
 			return -1;
 
-		//* Wait MCI Device Ready
-		MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		// Wait MCI Device Ready
+		status = MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		if (status & AT91C_MCI_SR_ERROR)
+			return 1;
 
-		// Fix erratum in MCI part
+		// Fix erratum in MCI part - endian-swap all data.
 		for (walker = (unsigned int *)dest;
-		     walker < (unsigned int *)(dest + sizeToRead); walker++)
+		     walker < (unsigned int *)(dest + SD_BLOCK_SIZE); walker++)
 		    *walker = swap(*walker);
 
-		//Update counters & pointers
-		length -= sizeToRead;
-		dest += sizeToRead;
-		source += sizeToRead;
+		// Update counters & pointers
+		++blknum;
+		--blkcount;
+		dest += SD_BLOCK_SIZE;
 	}
 
+
 	return 0;
 }
 
 //*----------------------------------------------------------------------------
+//* \fn    MCI_read
+//* \brief Legacy read function, takes byte offset and length but was always
+//*  used to read full blocks; interface preserved for existing boot code.
+//*----------------------------------------------------------------------------
+int
+MCI_read(char* dest, unsigned byteoffset, unsigned length)
+{
+	return MCI_readblocks(dest, 
+	    byteoffset/SD_BLOCK_SIZE, length/SD_BLOCK_SIZE);
+}
+
+//*----------------------------------------------------------------------------
 //* \fn    MCI_SDCard_SendAppCommand
 //* \brief Specific function to send a specific command to the SDCard
 //*----------------------------------------------------------------------------
@@ -242,15 +269,11 @@ MCI_SDCard_SendAppCommand(
 	unsigned int Cmd_App,
 	unsigned int Arg)
 {
-	// Send the CMD55 for application specific command
-	AT91C_BASE_MCI->MCI_ARGR = (MCI_Device.RCA << 16 );
-	AT91C_BASE_MCI->MCI_CMDR = APP_CMD;
+	int status;
 
-	wait_ready();
-	// if an error occurs
-	if (AT91C_BASE_MCI->MCI_SR & AT91C_MCI_SR_ERROR)
-		return (1);
-	return (MCI_SendCommand(Cmd_App,Arg));
+	if ((status = MCI_SendCommand(APP_CMD, (MCI_Device.RCA << 16))) == 0)
+		status = MCI_SendCommand(Cmd_App,Arg);
+	return status;
 }
 
 //*----------------------------------------------------------------------------
@@ -260,12 +283,11 @@ MCI_SDCard_SendAppCommand(
 static int
 MCI_GetCSD(unsigned int rca, unsigned int *response)
 {
- 	
- 	if (MCI_SendCommand(SEND_CSD_CMD, (rca << 16)))
+	if (MCI_SendCommand(SEND_CSD_CMD, (rca << 16)))
 		return 1;
 	
 	response[0] = AT91C_BASE_MCI->MCI_RSPR[0];
-   	response[1] = AT91C_BASE_MCI->MCI_RSPR[1];
+	response[1] = AT91C_BASE_MCI->MCI_RSPR[1];
 	response[2] = AT91C_BASE_MCI->MCI_RSPR[2];
 	response[3] = AT91C_BASE_MCI->MCI_RSPR[3];
     
@@ -274,22 +296,64 @@ MCI_GetCSD(unsigned int rca, unsigned int *response)
 
 //*----------------------------------------------------------------------------
 //* \fn    MCI_SDCard_GetOCR
-//* \brief Asks to all cards to send their operations conditions
+//* \brief Wait for card to power up and determine whether it's SDHC or not.
 //*----------------------------------------------------------------------------
 static int
 MCI_SDCard_GetOCR()
 {
-	unsigned int	response=0x0;
+	unsigned int response;
+	unsigned int arg = AT91C_MMC_HOST_VOLTAGE_RANGE;
+	int          timeout = AT91C_MCI_TIMEOUT;
+
+	// Force card to idle state.
+
+	MCI_SendCommand(GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
+
+	// Begin probe for SDHC by sending CMD8; only v2.0 cards respond to it.
+	//
+	// Arg is vvpp where vv is voltage range and pp is an arbitrary bit
+	// pattern that gets echoed back in the response. The only voltage
+	// ranges defined are:
+	//   0x01 = 2.7 - 3.6
+	//   0x02 = "reserved for low voltage" whatever that means.
+	//
+	// If the card fails to respond then it's not v2.0. If it responds by
+	// echoing back exactly the arg we sent, then it's a v2.0 card and can
+	// run at our voltage.  That means that when we send the ACMD41 (in
+	// MCI_SDCard_GetOCR) we can include the HCS bit to inquire about SDHC.
+
+	if (MCI_SendCommand(SD_SEND_IF_COND_CMD, 0x01AA) == 0) {
+		MCI_Device.IsSDv2 = (AT91C_BASE_MCI->MCI_RSPR[0] == 0x01AA);
+	}
+
+	// If we've determined the card supports v2.0 functionality, set the
+	// HCS/CCS bit to indicate that we support SDHC.  This will cause a
+	// v2.0 card to report whether it is SDHC in the ACMD41 response.
+
+	if (MCI_Device.IsSDv2) {
+		arg |= AT91C_CCS;
+	}
+
+	// The RCA to be used for CMD55 in Idle state shall be the card's 
+	// default RCA=0x0000.
 
-	// The RCA to be used for CMD55 in Idle state shall be the card's default RCA=0x0000.
 	MCI_Device.RCA = 0x0;
- 	
- 	while( (response & AT91C_CARD_POWER_UP_BUSY) != AT91C_CARD_POWER_UP_BUSY ) {
-		if (MCI_SDCard_SendAppCommand(SDCARD_APP_OP_COND_CMD,
-			AT91C_MMC_HOST_VOLTAGE_RANGE))
+
+	// Repeat ACMD41 until the card comes out of power-up-busy state.
+
+	do {
+		if (MCI_SDCard_SendAppCommand(SDCARD_APP_OP_COND_CMD, arg)) {
 			return 1;
+		}
 		response = AT91C_BASE_MCI->MCI_RSPR[0];
+	} while (!(response & AT91C_CARD_POWER_UP_DONE) && (--timeout > 0));
+
+	// A v2.0 card sets CCS (card capacity status) in the response if it's SDHC.
+
+	if (MCI_Device.IsSDv2) {
+		MCI_Device.IsSDHC = ((response & AT91C_CCS) == AT91C_CCS);
 	}
+
 	return (0);
 }
 
@@ -304,7 +368,7 @@ MCI_SDCard_GetCID(unsigned int *response)
 		return 1;
 	
 	response[0] = AT91C_BASE_MCI->MCI_RSPR[0];
-   	response[1] = AT91C_BASE_MCI->MCI_RSPR[1];
+	response[1] = AT91C_BASE_MCI->MCI_RSPR[1];
 	response[2] = AT91C_BASE_MCI->MCI_RSPR[2];
 	response[3] = AT91C_BASE_MCI->MCI_RSPR[3];
     
@@ -312,47 +376,62 @@ MCI_SDCard_GetCID(unsigned int *response)
 }
 
 //*----------------------------------------------------------------------------
-//* \fn    MCI_SDCard_SetBusWidth
-//* \brief  Set bus width for SDCard
+//* \fn    sdcard_4wire
+//* \brief  Set bus width to 1-bit or 4-bit according to the parm.
+//*
+//* Unlike most functions in this file, the return value from this one is
+//* bool-ish; returns 0 on failure, 1 on success.
 //*----------------------------------------------------------------------------
-static int
-MCI_SDCard_SetBusWidth()
+int
+sdcard_use4wire(int use4wire)
 {
 	volatile int	ret_value;
-	char			bus_width;
 
 	do {
 		ret_value=MCI_GetStatus();
 	}
 	while((ret_value > 0) && ((ret_value & AT91C_SR_READY_FOR_DATA) == 0));
 
-	// Select Card
-	MCI_SendCommand(SEL_DESEL_CARD_CMD, (MCI_Device.RCA)<<16);
+	// If going to 4-wire mode, ask the card to turn off the DAT3 card detect
+	// pullup resistor, if going to 1-wire ask it to turn it back on.
 
-	// Set bus width for Sdcard
-	if (MCI_Device.SDCard_bus_width == AT91C_MCI_SCDBUS)
-		bus_width = AT91C_BUS_WIDTH_4BITS;
-	else
-		bus_width = AT91C_BUS_WIDTH_1BIT;
+	ret_value = MCI_SDCard_SendAppCommand(SDCARD_SET_CLR_CARD_DETECT_CMD, 
+					      use4wire ? 0 : 1);
+	if (ret_value != AT91C_CMD_SEND_OK)
+		return 0;
 
-	if (MCI_SDCard_SendAppCommand(
-	      SDCARD_SET_BUS_WIDTH_CMD,bus_width) != AT91C_CMD_SEND_OK)
-		return 1;
+	// Ask the card to go into the requested mode.
 
-	return 0;
+	ret_value = MCI_SDCard_SendAppCommand(SDCARD_SET_BUS_WIDTH_CMD,
+					      use4wire ? AT91C_BUS_WIDTH_4BITS : 
+					                 AT91C_BUS_WIDTH_1BIT);
+	if (ret_value != AT91C_CMD_SEND_OK)
+		return 0;
+
+	// Set the MCI device to match the mode we set in the card.
+
+	if (use4wire) {
+		MCI_Device.SDCard_bus_width = AT91C_BUS_WIDTH_4BITS;
+		AT91C_BASE_MCI->MCI_SDCR |= AT91C_MCI_SCDBUS;
+	} else {
+		MCI_Device.SDCard_bus_width = AT91C_BUS_WIDTH_1BIT;
+		AT91C_BASE_MCI->MCI_SDCR &= ~AT91C_MCI_SCDBUS;
+	}
+
+	return 1;
 }
 
 //*----------------------------------------------------------------------------
-//* \fn    main
-//* \brief main function
+//* \fn    sdcard_init
+//* \brief get the mci device ready to read from an SD or SDHC card.
+//*
+//* Unlike most functions in this file, the return value from this one is
+//* bool-ish; returns 0 on failure, 1 on success.
 //*----------------------------------------------------------------------------
 int
 sdcard_init(void)
 {
 	unsigned int	tab_response[4];
-#ifdef REPORT_SIZE
-	unsigned int	mult,blocknr;
-#endif
 	int i;
 
 	// Init MCI for MMC and SDCard interface
@@ -362,51 +441,77 @@ sdcard_init(void)
 
 	// Init Device Structure
 	MCI_Device.state		= AT91C_MCI_IDLE;
-	MCI_Device.SDCard_bus_width	= AT91C_MCI_SCDBUS;
+	MCI_Device.SDCard_bus_width	= 0;
+	MCI_Device.IsSDv2		= 0;
+	MCI_Device.IsSDHC		= 0;
 
-	//* Reset the MCI
-	AT91C_BASE_MCI->MCI_CR = AT91C_MCI_MCIEN | AT91C_MCI_PWSEN;
+	// Reset the MCI and set the bus speed.
+	// Using MCK/230 gives a legal (under 400khz) bus speed for the card id
+	// sequence for all reasonable master clock speeds.
+
+	AT91C_BASE_MCI->MCI_CR = AT91C_MCI_MCIDIS | 0x80;
 	AT91C_BASE_MCI->MCI_IDR = 0xFFFFFFFF;
 	AT91C_BASE_MCI->MCI_DTOR = AT91C_MCI_DTOR_1MEGA_CYCLES;
-	AT91C_BASE_MCI->MCI_MR = AT91C_MCI_PDCMODE;
-	AT91C_BASE_MCI->MCI_SDCR = AT91C_MCI_SDCARD_4BITS_SLOTA;
-	MCI_SendCommand(GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
+	AT91C_BASE_MCI->MCI_MR = AT91C_MCI_PDCMODE | 114; /* clkdiv 114 = MCK/230 */
+	AT91C_BASE_MCI->MCI_SDCR = AT91C_MCI_MMC_SLOTA;
+	AT91C_BASE_MCI->MCI_CR = AT91C_MCI_MCIEN|AT91C_MCI_PWSEN;
+
+	// Wait for the card to come out of power-up-busy state by repeatedly
+	// sending ACMD41.  This also probes for SDHC versus standard cards.
 
 	for (i = 0; i < 100; i++) {
-		if (!MCI_SDCard_GetOCR(&MCI_Device))
+		if (MCI_SDCard_GetOCR() == 0)
 			break;
-		printf(".");
+		if ((i & 0x01) == 0) {
+			printf(".");
+		}
 	}
 	if (i >= 100)
 		return 0;
+
 	if (MCI_SDCard_GetCID(tab_response))
 		return 0;
+
+	// Tell the card to set its address, and remember the result.
+
 	if (MCI_SendCommand(SET_RELATIVE_ADDR_CMD, 0))
 		return 0;
-
 	MCI_Device.RCA = (AT91C_BASE_MCI->MCI_RSPR[0] >> 16);
+
+	// After sending CMD3 (set addr) we can increase the clock to full speed.
+	// Using MCK/4 gives a legal (under 25mhz) bus speed for all reasonable
+	// master clock speeds.
+
+	AT91C_BASE_MCI->MCI_MR = AT91C_MCI_PDCMODE | 1; /* clkdiv 1 = MCK/4 */
+
 	if (MCI_GetCSD(MCI_Device.RCA,tab_response))
 		return 0;
 	MCI_Device.READ_BL_LEN = (tab_response[1] >> CSD_1_RD_B_LEN_S) &
 	    CSD_1_RD_B_LEN_M;
+
 #ifdef REPORT_SIZE
-	// compute MULT
-	mult = 1 << ( ((tab_response[2] >> CSD_2_C_SIZE_M_S) &
-	    CSD_2_C_SIZE_M_M) + 2 );
-	// compute MSB of C_SIZE
-	blocknr = ((tab_response[1] >> CSD_1_CSIZE_H_S) &
-	    CSD_1_CSIZE_H_M) << 2;
-	// compute MULT * (LSB of C-SIZE + MSB already computed + 1) = BLOCKNR
-	blocknr = mult * ((blocknr + ((tab_response[2] >> CSD_2_CSIZE_L_S) &
-	    CSD_2_CSIZE_L_M)) + 1);
-	MCI_Device.Memory_Capacity = (1 << MCI_Device.READ_BL_LEN) * blocknr;
+	{
+		unsigned int	mult,blocknr;
+		// compute MULT
+		mult = 1 << ( ((tab_response[2] >> CSD_2_C_SIZE_M_S) &
+		    CSD_2_C_SIZE_M_M) + 2 );
+		// compute MSB of C_SIZE
+		blocknr = ((tab_response[1] >> CSD_1_CSIZE_H_S) &
+		    CSD_1_CSIZE_H_M) << 2;
+		// compute MULT * (LSB of C-SIZE + MSB already computed + 1) = BLOCKNR
+		blocknr = mult * ((blocknr + ((tab_response[2] >> CSD_2_CSIZE_L_S) &
+		    CSD_2_CSIZE_L_M)) + 1);
+		MCI_Device.Memory_Capacity = (1 << MCI_Device.READ_BL_LEN) * blocknr;
+		printf("Found SD card %u bytes\n", MCI_Device.Memory_Capacity);
+	}
 #endif
-	if (MCI_SDCard_SetBusWidth())
+
+	// Select card and set block length for following transfers.
+
+	if (MCI_SendCommand(SEL_DESEL_CARD_CMD, (MCI_Device.RCA)<<16))
 		return 0;
-	if (MCI_SendCommand(SET_BLOCKLEN_CMD, 1 << MCI_Device.READ_BL_LEN))
+	if (MCI_SendCommand(SET_BLOCKLEN_CMD, SD_BLOCK_SIZE))
 		return 0;
-#ifdef REPORT_SIZE
-	printf("Found SD card %u bytes\n", MCI_Device.Memory_Capacity);
-#endif
+
 	return 1;
 }
diff --git a/sys/boot/arm/at91/libat91/sd-card.h b/sys/boot/arm/at91/libat91/sd-card.h
index 0a3ce69..378ad01 100644
--- a/sys/boot/arm/at91/libat91/sd-card.h
+++ b/sys/boot/arm/at91/libat91/sd-card.h
@@ -30,9 +30,35 @@
 #ifndef __SD_CARD_H
 #define __SD_CARD_H
 
-int MCI_write (unsigned dest, char* source, unsigned length);
-int MCI_read (char* dest, unsigned source, unsigned length);
+/* MCI_read() is the original read function, taking a byte offset and byte
+ * count.  It is preserved to support existing customized boot code that still
+ * refers to it; it will work fine even on SDHC cards as long as the kernel and
+ * the metadata for locating it all exist within the first 4GB of the card.
+ *
+ * MCI_readblocks() is the new read function, taking offset and length in terms
+ * of block counts (where the SD spec defines a block as 512 bytes), allowing
+ * the kernel and filesystem metadata to be located anywhere on an SDHC card.
+ *
+ * Returns 0 on success, non-zero on failure.
+ */
+
+int MCI_read (char* dest, unsigned bytenum, unsigned length);
+int MCI_readblocks (char* dest, unsigned blknum, unsigned blkcount);
+
+/* sdcard_init() - get things set up to read from an SD or SDHC card.
+ *
+ * Returns 0 on failure, non-zero on success.
+ */
+
 int sdcard_init(void);
 
+/* By default sdcard_init() sets things up for a 1-wire interface to the
+ * SD card.  Calling sdcard_4wire(true) after sdcard_init() allows customized
+ * boot code to change to 4-bit transfers when the hardware supports it.
+ *
+ * Returns 0 on failure, non-zero on success.
+ */
+int sdcard_use4wire(int use4wire);
+
 #endif