MFp4: boot on KB9202 correctly. Also, reduce the size of the SD/MMC driver

somewhat.

15 files changed, 331 insertions, 163 deletions

diff --git a/sys/boot/arm/at91/libat91/Makefile b/sys/boot/arm/at91/libat91/Makefile
index 53e7aa9..2d9e715 100644
--- a/sys/boot/arm/at91/libat91/Makefile
+++ b/sys/boot/arm/at91/libat91/Makefile
@@ -4,9 +4,9 @@
 
 LIB=		at91
 INTERNALLIB=
-SRCS=at91rm9200_lowlevel.c delay.c eeprom.c emac.c emac_init.c getc.c \
+SRCS=at91rm9200_lowlevel.c delay.c eeprom.c emac.c emac_init.c fpga.c getc.c \
 	putchar.c printf.c reset.c spi_flash.c xmodem.c \
-	sd-card.c mci_device.c strcvt.c strlen.c strcmp.c memcpy.c strcpy.c \
+	sd-card.c strcvt.c strlen.c strcmp.c memcpy.c strcpy.c \
 	memset.c memcmp.c
 SRCS+=ashldi3.c divsi3.S
 NO_MAN=
diff --git a/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c b/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c
index 551b222..6ea79e7 100644
--- a/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c
+++ b/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c
@@ -53,7 +53,6 @@ _init(void)
 	register unsigned	value;
 	volatile sdram_size_t *p = (sdram_size_t *)SDRAM_BASE;
 
-#if 0
 #ifdef BOOT_TSC
 	// For the TSC board, we turn ON the one LED we have while
 	// early in boot.
@@ -61,6 +60,15 @@ _init(void)
 	AT91C_BASE_PIOC->PIO_OER = AT91C_PIO_PC10;
 	AT91C_BASE_PIOC->PIO_CODR = AT91C_PIO_PC10;
 #endif
+
+#ifdef BOOT_KB920X
+	AT91C_BASE_PIOC->PIO_PER = AT91C_PIO_PC18 | AT91C_PIO_PC19 |
+	  AT91C_PIO_PC20;
+	AT91C_BASE_PIOC->PIO_OER = AT91C_PIO_PC18 | AT91C_PIO_PC19 |
+	  AT91C_PIO_PC20;
+	AT91C_BASE_PIOC->PIO_SODR = AT91C_PIO_PC18 | AT91C_PIO_PC19 |
+	  AT91C_PIO_PC20;
+	AT91C_BASE_PIOC->PIO_CODR = AT91C_PIO_PC18;
 #endif
 
 	// configure clocks
@@ -100,7 +108,7 @@ _init(void)
 	while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY))
 		continue;
 
-#ifdef BOOT_KB9202
+#ifdef BOOT_KB920X
 	// setup flash access (allow ample margin)
 	// 9 wait states, 1 setup, 1 hold, 1 float for 8-bit device
 	((AT91PS_SMC2)AT91C_BASE_SMC2)->SMC2_CSR[0] =
diff --git a/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.h b/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.h
index 7be7228..c6810f0 100644
--- a/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.h
+++ b/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.h
@@ -31,7 +31,7 @@
 
 #define SDRAM_BASE		0x20000000
 
-#ifdef BOOT_KB9202
+#ifdef BOOT_KB920X
 /* The following divisor sets PLLA frequency: e.g. 10/5 * 90 = 180MHz */
 #define OSC_MAIN_FREQ_DIV	5	/* for 10MHz osc */
 #define SDRAM_WIDTH	AT91C_SDRC_DBW_16_BITS
diff --git a/sys/boot/arm/at91/libat91/emac.c b/sys/boot/arm/at91/libat91/emac.c
index 3ea1b82..8995596 100644
--- a/sys/boot/arm/at91/libat91/emac.c
+++ b/sys/boot/arm/at91/libat91/emac.c
@@ -384,7 +384,7 @@ MII_GetLinkSpeed(AT91PS_EMAC pEmac)
 	unsigned sec;
 	int i;
 #endif
-#ifdef BOOT_KB9202
+#ifdef BOOT_KB920X
 	stat2 = AT91F_MII_ReadPhy(pEmac, MII_STS2_REG);
 	if (!(stat2 & MII_STS2_LINK))
 		return ;
diff --git a/sys/boot/arm/at91/libat91/emac.h b/sys/boot/arm/at91/libat91/emac.h
index 81e7f4a..de6dbf5 100644
--- a/sys/boot/arm/at91/libat91/emac.h
+++ b/sys/boot/arm/at91/libat91/emac.h
@@ -114,7 +114,7 @@ typedef struct {
 /* MII registers definition */
 #define MII_STS_REG	0x01
 #define MII_STS_LINK_STAT	0x04
-#ifdef BOOT_KB9202
+#ifdef BOOT_KB920X
 #define MII_STS2_REG	0x11
 #define MII_STS2_LINK	0x400
 #define MII_STS2_100TX	0x4000
diff --git a/sys/boot/arm/at91/libat91/emac_init.c b/sys/boot/arm/at91/libat91/emac_init.c
index 4d0bb37..2aeb83e 100644
--- a/sys/boot/arm/at91/libat91/emac_init.c
+++ b/sys/boot/arm/at91/libat91/emac_init.c
@@ -94,7 +94,7 @@ EMAC_SetMACAddress(unsigned char mac[6])
 	  AT91C_PA8_ETXEN | AT91C_PA16_EMDIO | AT91C_PA9_ETX0 |
 	  AT91C_PA10_ETX1 | AT91C_PA11_ECRS_ECRSDV | AT91C_PA15_EMDC |
 	  AT91C_PA7_ETXCK_EREFCK;
-#ifdef BOOT_KB9202	/* Really !RMII */
+#ifdef BOOT_KB920X	/* Really !RMII */
 	AT91C_BASE_PIOB->PIO_BSR =
 	  AT91C_PB12_ETX2 | AT91C_PB13_ETX3 | AT91C_PB14_ETXER |
 	  AT91C_PB15_ERX2 | AT91C_PB16_ERX3 | AT91C_PB17_ERXDV |
diff --git a/sys/boot/arm/at91/libat91/mci_device.h b/sys/boot/arm/at91/libat91/mci_device.h
index 6fee390..eeff0c9 100644
--- a/sys/boot/arm/at91/libat91/mci_device.h
+++ b/sys/boot/arm/at91/libat91/mci_device.h
@@ -83,43 +83,19 @@ typedef unsigned int AT91S_MCIDeviceStatus;
 // MMC & SDCard Structures 
 ///////////////////////////////////////////////////////////////////////////////
 
-/*-----------------------------------------------*/
-/* SDCard Device Descriptor Structure Definition */
-/*-----------------------------------------------*/
-typedef struct	_AT91S_MciDeviceDesc
-{
-    volatile unsigned char	state;
-	unsigned char			SDCard_bus_width;
-
-} AT91S_MciDeviceDesc, *AT91PS_MciDeviceDesc;
-
-/*---------------------------------------------*/
-/* MMC & SDCard Structure Device Features	   */
-/*---------------------------------------------*/
-typedef struct	_AT91S_MciDeviceFeatures
-{
-	unsigned char	Card_Inserted;				// (0=AT91C_CARD_REMOVED) (1=AT91C_MMC_CARD_INSERTED) (2=AT91C_SD_CARD_INSERTED)
-	unsigned int 	Relative_Card_Address;		// RCA
-	unsigned int	READ_BL_LEN;
-	unsigned int	WRITE_BL_LEN;
-	unsigned char	Read_Partial;				// READ_BL_PARTIAL
-	unsigned char	Write_Partial;				// WRITE_BL_PARTIAL
-	unsigned char	Erase_Block_Enable;			// ERASE_BLK_EN
-	unsigned char	Read_Block_Misalignment;	// READ_BLK_MISALIGN
-	unsigned char	Write_Block_Misalignment;	// WRITE_BLK_MISALIGN
-	unsigned char	Sector_Size;				// SECTOR_SIZE
-	unsigned int	Memory_Capacity;			// Size in bits of the device
-	
-}	AT91S_MciDeviceFeatures, *AT91PS_MciDeviceFeatures ;
-
 /*---------------------------------------------*/
 /* MCI Device Structure Definition 			   */
 /*---------------------------------------------*/
 typedef struct _AT91S_MciDevice
 {
-	AT91PS_MciDeviceDesc		 	pMCI_DeviceDesc;	// MCI device descriptor
-	AT91PS_MciDeviceFeatures		pMCI_DeviceFeatures;// Pointer on a MCI device features array  
-} AT91S_MciDevice, *AT91PS_MciDevice;
+	volatile unsigned char	state;
+	unsigned char	SDCard_bus_width;
+	unsigned int 	RCA;		// RCA
+	unsigned int	READ_BL_LEN;
+#ifdef REPORT_SIZE
+	unsigned int	Memory_Capacity;
+#endif
+} AT91S_MciDevice;
 
 #include <dev/mmc/mmcreg.h>
 
@@ -450,15 +426,4 @@ typedef struct _AT91S_MciDevice
 #define	CSD_0_STRUCT_M 		0x03
 
 ///////////////////////////////////////////////////////////////////////////////
-
-void 				AT91F_MCI_Device_Handler(AT91PS_MciDevice,unsigned int);
-AT91S_MCIDeviceStatus 	AT91F_MCI_SDCard_Init (AT91PS_MciDevice);
-AT91S_MCIDeviceStatus 	AT91F_MCI_SetBlocklength(unsigned int);
-AT91S_MCIDeviceStatus 	AT91F_MCI_ReadBlock(AT91PS_MciDevice,int,unsigned int *,int);
-AT91S_MCIDeviceStatus 	AT91F_MCI_WriteBlock(AT91PS_MciDevice,int,unsigned int *,int);
-#if 0
-AT91S_MCIDeviceStatus AT91F_MCI_MMC_Init (AT91PS_MciDevice pMCI_Device);
-AT91S_MCIDeviceStatus AT91F_MCI_MMC_SelectCard(AT91PS_MciDevice pMCI_Device, unsigned int relative_card_address);
-#endif
-
 #endif
diff --git a/sys/boot/arm/at91/libat91/memcmp.c b/sys/boot/arm/at91/libat91/memcmp.c
index a9bd659..bc157cf 100644
--- a/sys/boot/arm/at91/libat91/memcmp.c
+++ b/sys/boot/arm/at91/libat91/memcmp.c
@@ -21,11 +21,11 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include "lib.h"
 
 int
diff --git a/sys/boot/arm/at91/libat91/memcpy.c b/sys/boot/arm/at91/libat91/memcpy.c
index 680de5e..f7544fe 100644
--- a/sys/boot/arm/at91/libat91/memcpy.c
+++ b/sys/boot/arm/at91/libat91/memcpy.c
@@ -21,11 +21,11 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include "lib.h"
 
 void
diff --git a/sys/boot/arm/at91/libat91/memset.c b/sys/boot/arm/at91/libat91/memset.c
index 1f3e890..3432e64 100644
--- a/sys/boot/arm/at91/libat91/memset.c
+++ b/sys/boot/arm/at91/libat91/memset.c
@@ -21,11 +21,11 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include "lib.h"
 
 void
diff --git a/sys/boot/arm/at91/libat91/sd-card.c b/sys/boot/arm/at91/libat91/sd-card.c
index ef17520..095b6a1 100644
--- a/sys/boot/arm/at91/libat91/sd-card.c
+++ b/sys/boot/arm/at91/libat91/sd-card.c
@@ -51,13 +51,7 @@
 #define BUFFER_SIZE_MCI_DEVICE	512
 #define MASTER_CLOCK		60000000
 
-//Private functions
-//static void initInts(void);
-static void AT91F_MCI_Handler(void);
-
 //* Global Variables
-AT91S_MciDeviceFeatures		MCI_Device_Features;
-AT91S_MciDeviceDesc		MCI_Device_Desc;
 AT91S_MciDevice			MCI_Device;
 char				Buffer[BUFFER_SIZE_MCI_DEVICE];
 
@@ -68,11 +62,11 @@ char				Buffer[BUFFER_SIZE_MCI_DEVICE];
 #define MCI_UNSUPP_OFFSET_ERROR 6
 
 //*----------------------------------------------------------------------------
-//* \fn    AT91F_MCIDeviceWaitReady
+//* \fn    MCIDeviceWaitReady
 //* \brief Wait for MCI Device ready
 //*----------------------------------------------------------------------------
 static void
-AT91F_MCIDeviceWaitReady(unsigned int timeout)
+MCIDeviceWaitReady(unsigned int timeout)
 {
 	volatile int status;
 	
@@ -83,56 +77,126 @@ AT91F_MCIDeviceWaitReady(unsigned int timeout)
 	}
 	while( !(status & AT91C_MCI_NOTBUSY)  && (timeout>0) );	
 
-	AT91F_MCI_Handler();
+	status = AT91C_BASE_MCI->MCI_SR;
+
+	// If End of Tx Buffer Empty interrupt occurred
+	if (MCI_Device.state == AT91C_MCI_TX_SINGLE_BLOCK && status & AT91C_MCI_TXBUFE) {
+		AT91C_BASE_MCI->MCI_IDR = AT91C_MCI_TXBUFE;
+ 		AT91C_BASE_PDC_MCI->PDC_PTCR = AT91C_PDC_TXTDIS;
+		MCI_Device.state = AT91C_MCI_IDLE;
+	}	// End of if AT91C_MCI_TXBUFF
+
+	// If End of Rx Buffer Full interrupt occurred
+	if (MCI_Device.state == AT91C_MCI_RX_SINGLE_BLOCK && status & AT91C_MCI_RXBUFF) {
+		AT91C_BASE_MCI->MCI_IDR = AT91C_MCI_RXBUFF;
+ 		AT91C_BASE_PDC_MCI->PDC_PTCR = AT91C_PDC_RXTDIS;
+		MCI_Device.state = AT91C_MCI_IDLE;
+	}	// End of if AT91C_MCI_RXBUFF
 }
 
-#if 0
-int
-MCI_write (unsigned dest, char* source, unsigned length)
+inline static unsigned int
+swap(unsigned int a)
 {
-	unsigned sectorLength = 1 << MCI_Device.pMCI_DeviceFeatures->WRITE_BL_LEN;
-	unsigned offset = dest % sectorLength;
-	AT91S_MCIDeviceStatus status;
-	int sizeToWrite;
+    return (((a & 0xff) << 24) | ((a & 0xff00) << 8) | ((a & 0xff0000) >> 8)
+      | ((a & 0xff000000) >> 24));
+}
 
-	//As long as there is data to write
-	while (length)
-	{
-		//See if we've got at least a sector to write
-		if (length > sectorLength) 
-			sizeToWrite = sectorLength;
-		//Else just write the remainder
-		else
-			sizeToWrite = length;
+inline static void
+wait_ready()
+{
+	int status;
 
-		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
-		//Do the writing
-		status = AT91F_MCI_WriteBlock(&MCI_Device, dest, (unsigned int*)source, sizeToWrite);
-		//TODO:Status checking
+	// wait for CMDRDY Status flag to read the response
+	do
+	{
+		status = AT91C_BASE_MCI->MCI_SR;
+	} while( !(status & AT91C_MCI_CMDRDY) );
+}
 
-		//Update counters & pointers
-		length -= sizeToWrite;
-		dest += sizeToWrite;
-		source += sizeToWrite;
+//*----------------------------------------------------------------------------
+//* \fn    MCI_SendCommand
+//* \brief Generic function to send a command to the MMC or SDCard
+//*----------------------------------------------------------------------------
+static int
+MCI_SendCommand(
+	unsigned int Cmd,
+	unsigned int Arg)
+{
+	unsigned int	error;
+
+	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);
 	}
-
 	return 0;
 }
-#endif
 
-inline static unsigned int
-swap(unsigned int a)
+//*----------------------------------------------------------------------------
+//* \fn    MCI_GetStatus
+//* \brief Addressed card sends its status register
+//*----------------------------------------------------------------------------
+static unsigned int
+MCI_GetStatus()
 {
-    return (((a & 0xff) << 24) | ((a & 0xff00) << 8) | ((a & 0xff0000) >> 8)
-      | ((a & 0xff000000) >> 24));
+	if (MCI_SendCommand(SEND_STATUS_CMD, MCI_Device.RCA << 16))
+		return AT91C_CMD_SEND_ERROR;
+	return (AT91C_BASE_MCI->MCI_RSPR[0]);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    MCI_ReadBlock
+//* \brief Read an ENTIRE block or PARTIAL block
+//*----------------------------------------------------------------------------
+static int
+MCI_ReadBlock(int src, unsigned int *dataBuffer, int sizeToRead)
+{
+	unsigned log2sl = MCI_Device.READ_BL_LEN;
+	unsigned sectorLength = 1 << log2sl;
+
+	///////////////////////////////////////////////////////////////////////
+	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);
+	 
+	sizeToRead = sizeToRead / 4;
+
+	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;
+
+	// Send the Read single block command
+	if (MCI_SendCommand(READ_SINGLE_BLOCK_CMD, src))
+		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;
 }
 
 int
 MCI_read(char* dest, unsigned source, unsigned length)
 {
-	unsigned log2sl = MCI_Device.pMCI_DeviceFeatures->READ_BL_LEN;
+	unsigned log2sl = MCI_Device.READ_BL_LEN;
 	unsigned sectorLength = 1 << log2sl;
-	AT91S_MCIDeviceStatus status;
 	int sizeToRead;
 	unsigned int *walker;
 
@@ -144,17 +208,14 @@ MCI_read(char* dest, unsigned source, unsigned length)
 		else
 			sizeToRead = length;
 
-		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
 		//Do the reading
-		status = AT91F_MCI_ReadBlock(&MCI_Device, source,
-		  (unsigned int*)dest, sizeToRead);
-
-		//TODO:Status checking
-		if (status != AT91C_READ_OK)
+		if (MCI_ReadBlock(source,
+			(unsigned int*)dest, sizeToRead))
 			return -1;
 
 		//* Wait MCI Device Ready
-		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
 
 		// Fix erratum in MCI part
 		for (walker = (unsigned int *)dest;
@@ -170,38 +231,113 @@ MCI_read(char* dest, unsigned source, unsigned length)
 	return 0;
 }
 
+//*----------------------------------------------------------------------------
+//* \fn    MCI_SDCard_SendAppCommand
+//* \brief Specific function to send a specific command to the SDCard
+//*----------------------------------------------------------------------------
+static int
+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;
+
+	wait_ready();
+	// if an error occurs
+	if (AT91C_BASE_MCI->MCI_SR & AT91C_MCI_SR_ERROR)
+		return (1);
+	return (MCI_SendCommand(Cmd_App,Arg));
+}
 
 //*----------------------------------------------------------------------------
-//* \fn    AT91F_CfgDevice
-//* \brief This function is used to initialise MMC or SDCard Features
+//* \fn    MCI_GetCSD
+//* \brief Asks to the specified card to send its CSD
 //*----------------------------------------------------------------------------
-static void AT91F_CfgDevice(void)
+static int
+MCI_GetCSD(unsigned int rca, unsigned int *response)
 {
-	// Init Device Structure
+ 	
+ 	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[2] = AT91C_BASE_MCI->MCI_RSPR[2];
+	response[3] = AT91C_BASE_MCI->MCI_RSPR[3];
+    
+	return 0;
+}
 
-	MCI_Device_Features.Relative_Card_Address = 0;
-	MCI_Device_Features.Card_Inserted 	= AT91C_SD_CARD_INSERTED;
-	MCI_Device_Features.READ_BL_LEN		= 0;
-	MCI_Device_Features.WRITE_BL_LEN	= 0;
-	MCI_Device_Features.Read_Partial 	= 0;
-	MCI_Device_Features.Write_Partial 	= 0;
-	MCI_Device_Features.Erase_Block_Enable 	= 0;
-	MCI_Device_Features.Sector_Size 	= 0;
-	MCI_Device_Features.Memory_Capacity 	= 0;
-	MCI_Device_Desc.state			= AT91C_MCI_IDLE;
-	MCI_Device_Desc.SDCard_bus_width	= AT91C_MCI_SCDBUS;
-	MCI_Device.pMCI_DeviceDesc 		= &MCI_Device_Desc;
-	MCI_Device.pMCI_DeviceFeatures 		= &MCI_Device_Features;
+//*----------------------------------------------------------------------------
+//* \fn    MCI_SDCard_GetOCR
+//* \brief Asks to all cards to send their operations conditions
+//*----------------------------------------------------------------------------
+static int
+MCI_SDCard_GetOCR()
+{
+	unsigned int	response=0x0;
+
+	// 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))
+			return 1;
+		response = AT91C_BASE_MCI->MCI_RSPR[0];
+	}
+	return (0);
 }
 
-static void AT91F_MCI_Handler(void)
+//*----------------------------------------------------------------------------
+//* \fn    MCI_SDCard_GetCID
+//* \brief Asks to the SDCard on the chosen slot to send its CID
+//*----------------------------------------------------------------------------
+static int
+MCI_SDCard_GetCID(unsigned int *response)
 {
-	int status;
+	if (MCI_SendCommand(ALL_SEND_CID_CMD, AT91C_NO_ARGUMENT))
+		return 1;
+	
+	response[0] = AT91C_BASE_MCI->MCI_RSPR[0];
+   	response[1] = AT91C_BASE_MCI->MCI_RSPR[1];
+	response[2] = AT91C_BASE_MCI->MCI_RSPR[2];
+	response[3] = AT91C_BASE_MCI->MCI_RSPR[3];
+    
+	return 0;
+}
 
-//	status = ( AT91C_BASE_MCI->MCI_SR & AT91C_BASE_MCI->MCI_IMR );
-	status = AT91C_BASE_MCI->MCI_SR;
+//*----------------------------------------------------------------------------
+//* \fn    MCI_SDCard_SetBusWidth
+//* \brief  Set bus width for SDCard
+//*----------------------------------------------------------------------------
+static int
+MCI_SDCard_SetBusWidth()
+{
+	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);
 
-	AT91F_MCI_Device_Handler(&MCI_Device, status);
+	// 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;
+
+	if (MCI_SDCard_SendAppCommand(
+	      SDCARD_SET_BUS_WIDTH_CMD,bus_width) != AT91C_CMD_SEND_OK)
+		return 1;
+
+	return 0;
 }
 
 //*----------------------------------------------------------------------------
@@ -211,26 +347,64 @@ static void AT91F_MCI_Handler(void)
 int
 sdcard_init(void)
 {
-///////////////////////////////////////////////////////////////////////////////
-//  MCI Init : common to MMC and SDCard
-///////////////////////////////////////////////////////////////////////////////
-
-	//initInts();
+	unsigned int	tab_response[4];
+#ifdef REPORT_SIZE
+	unsigned int	mult,blocknr;
+#endif
+	int i;
 
 	// Init MCI for MMC and SDCard interface
 	AT91F_MCI_CfgPIO();	
 	AT91F_MCI_CfgPMC();
 	AT91F_PDC_Open(AT91C_BASE_PDC_MCI);
 
-	// Init MCI Device Structures
-	AT91F_CfgDevice();
+	// Init Device Structure
+	MCI_Device.state		= AT91C_MCI_IDLE;
+	MCI_Device.SDCard_bus_width	= AT91C_MCI_SCDBUS;
+
+	//* Reset the MCI
+	AT91C_BASE_MCI->MCI_CR = AT91C_MCI_MCIEN | AT91C_MCI_PWSEN;
+	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);
+
+	for (i = 0; i < 100; i++) {
+		if (!MCI_SDCard_GetOCR(&MCI_Device))
+			break;
+		printf(".");
+	}
+	if (i >= 100)
+		return 0;
+	if (MCI_SDCard_GetCID(tab_response))
+		return 0;
+	if (MCI_SendCommand(SET_RELATIVE_ADDR_CMD, 0))
+		return 0;
 
-	AT91F_MCI_Configure(AT91C_BASE_MCI,
-	    AT91C_MCI_DTOR_1MEGA_CYCLES,
-	    AT91C_MCI_PDCMODE,
-	    AT91C_MCI_SDCARD_4BITS_SLOTA);
-	
-	if (AT91F_MCI_SDCard_Init(&MCI_Device) != AT91C_INIT_OK)
+	MCI_Device.RCA = (AT91C_BASE_MCI->MCI_RSPR[0] >> 16);
+	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;
+#endif
+	if (MCI_SDCard_SetBusWidth())
 		return 0;
+	if (MCI_SendCommand(SET_BLOCKLEN_CMD, 1 << MCI_Device.READ_BL_LEN))
+		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/strcmp.c b/sys/boot/arm/at91/libat91/strcmp.c
index f04d1f7..de1d9e0 100644
--- a/sys/boot/arm/at91/libat91/strcmp.c
+++ b/sys/boot/arm/at91/libat91/strcmp.c
@@ -21,11 +21,11 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include "lib.h"
 
 int
diff --git a/sys/boot/arm/at91/libat91/strcpy.c b/sys/boot/arm/at91/libat91/strcpy.c
index c31a94e..0bdd3a5 100644
--- a/sys/boot/arm/at91/libat91/strcpy.c
+++ b/sys/boot/arm/at91/libat91/strcpy.c
@@ -21,11 +21,11 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include "lib.h"
 
 char *
diff --git a/sys/boot/arm/at91/libat91/strcvt.c b/sys/boot/arm/at91/libat91/strcvt.c
index d1e385c..64442cc 100644
--- a/sys/boot/arm/at91/libat91/strcvt.c
+++ b/sys/boot/arm/at91/libat91/strcvt.c
@@ -21,11 +21,11 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 /******************************************************************************
  *
  * Filename: p_string.c
@@ -47,7 +47,6 @@
  * owners.  This software is not copyrighted and is intended for reference
  * only.
  * END_BLOCK
- *
  *****************************************************************************/
 
 #include "lib.h"
diff --git a/sys/boot/arm/at91/libat91/strlen.c b/sys/boot/arm/at91/libat91/strlen.c
index 227e516..47009b1 100644
--- a/sys/boot/arm/at91/libat91/strlen.c
+++ b/sys/boot/arm/at91/libat91/strlen.c
@@ -21,12 +21,34 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * This software is derived from software provide by Kwikbyte who specifically
- * disclaimed copyright on the code.
- *
- * $FreeBSD$
  */
 
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+/******************************************************************************
+ *
+ * Filename: p_string.c
+ *
+ * Instantiation of basic string operations to prevent inclusion of full
+ * string library.  These are simple implementations not necessarily optimized
+ * for speed, but rather to show intent.
+ *
+ * Revision information:
+ *
+ * 20AUG2004	kb_admin	initial creation
+ * 12JAN2005	kb_admin	minor updates
+ *
+ * BEGIN_KBDD_BLOCK
+ * No warranty, expressed or implied, is included with this software.  It is
+ * provided "AS IS" and no warranty of any kind including statutory or aspects
+ * relating to merchantability or fitness for any purpose is provided.  All
+ * intellectual property rights of others is maintained with the respective
+ * owners.  This software is not copyrighted and is intended for reference
+ * only.
+ * END_BLOCK
+ *****************************************************************************/
+
 #include "lib.h"
 
 /*