MFp4: First cut at making spi and/or sd card booting work, needs work

19 files changed, 2940 insertions, 119 deletions

diff --git a/sys/boot/arm/at91/bootiic/loader_prompt.c b/sys/boot/arm/at91/bootiic/loader_prompt.c
index 58b06ce..94e30d9 100644
--- a/sys/boot/arm/at91/bootiic/loader_prompt.c
+++ b/sys/boot/arm/at91/bootiic/loader_prompt.c
@@ -266,7 +266,7 @@ ParseCommand(char *buffer)
 		if (argc > 6) {
 			for (i = 0; i < 6; i++)
 				mac[i] = p_ASCIIToHex(argv[i + 1]);
-			SetMACAddress(mac);
+			EMAC_SetMACAddress(mac);
 		}
 		break;
 	}
diff --git a/sys/boot/arm/at91/bootspi/Makefile b/sys/boot/arm/at91/bootspi/Makefile
index 203d8a1..9205e29 100644
--- a/sys/boot/arm/at91/bootspi/Makefile
+++ b/sys/boot/arm/at91/bootspi/Makefile
@@ -2,9 +2,13 @@
 
 P=bootspi
 FILES=${P}
-SRCS=arm_init.s main.c
+SRCS=arm_init.S main.c loader_prompt.c env_vars.c
 NO_MAN=
 LDFLAGS=-e 0 -T ${.CURDIR}/../linker.cfg
 OBJS+=  ${SRCS:N*.h:R:S/$/.o/g}
 
 .include <bsd.prog.mk>
+
+.if ${MK_FPGA} != "no"
+CFLAGS += -DTSC_FPGA
+.endif
diff --git a/sys/boot/arm/at91/bootspi/arm_init.s b/sys/boot/arm/at91/bootspi/arm_init.S
index f9e6655..f9e6655 100644
--- a/sys/boot/arm/at91/bootspi/arm_init.s
+++ b/sys/boot/arm/at91/bootspi/arm_init.S
diff --git a/sys/boot/arm/at91/bootspi/env_vars.c b/sys/boot/arm/at91/bootspi/env_vars.c
new file mode 100644
index 0000000..94b1fbd
--- /dev/null
+++ b/sys/boot/arm/at91/bootspi/env_vars.c
@@ -0,0 +1,130 @@
+/******************************************************************************
+ *
+ * Filename: env_vars.c
+ *
+ * Instantiation of environment variables, structures, and other globals.
+ *
+ * Revision information:
+ *
+ * 20AUG2004	kb_admin	initial creation
+ *
+ * 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
+ *
+ * $FreeBSD$
+ *****************************************************************************/
+
+#include "env_vars.h"
+#include "loader_prompt.h"
+#include "lib.h"
+
+/******************************* GLOBALS *************************************/
+char	boot_commands[MAX_BOOT_COMMANDS][MAX_INPUT_SIZE];
+
+char	env_table[MAX_ENV_SIZE_BYTES];
+
+extern char	BootCommandSection;
+
+/************************** PRIVATE FUNCTIONS ********************************/
+
+
+static int	currentIndex;
+static int	currentOffset;
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * int ReadCharFromEnvironment(char *)
+ *  This private function reads characters from the enviroment variables
+ * to service the command prompt during auto-boot or just to setup the
+ * default environment.  Returns positive value if valid character was
+ * set in the pointer.  Returns negative value to signal input stream
+ * terminated.  Returns 0 to indicate _wait_ condition.
+ * .KB_C_FN_DEFINITION_END
+ */
+static int
+ReadCharFromEnvironment(int timeout)
+{
+	int ch;
+
+	if (currentIndex < MAX_BOOT_COMMANDS) {
+		ch = boot_commands[currentIndex][currentOffset++];
+		if (ch == '\0' || (currentOffset >= MAX_INPUT_SIZE)) {
+			currentOffset = 0;
+			++currentIndex;
+			ch = '\r';
+		}
+		return (ch);
+	}
+
+	return (-1);
+}
+
+
+/*************************** GLOBAL FUNCTIONS ********************************/
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void DumpBootCommands(void)
+ *  This global function displays the current boot commands.
+ * .KB_C_FN_DEFINITION_END
+ */
+void
+DumpBootCommands(void)
+{
+	int	i;
+
+	for (i = 0; boot_commands[i][0]; i++)
+		printf("0x%x : %s[E]\r\n", i, boot_commands[i]);
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void LoadBootCommands(void)
+ *  This global function loads the existing boot commands from raw format and
+ * coverts it to the standard, command-index format.  Notice, the processed
+ * boot command table has much more space allocated than the actual table
+ * stored in non-volatile memory.  This is because the processed table
+ * exists in RAM which is larger than the non-volatile space.
+ * .KB_C_FN_DEFINITION_END
+ */
+void
+LoadBootCommands(void)
+{
+	int	index, j;
+	char	*cptr;
+
+	p_memset((char*)boot_commands, 0, sizeof(boot_commands));
+	cptr = &BootCommandSection;
+	for (index = 0; *cptr; index++) {
+		for (j = 0; *cptr; j++)
+			boot_commands[index][j] = *cptr++;
+		cptr++;
+	}
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void ExecuteEnvironmentFunctions(void)
+ *  This global function executes applicable entries in the environment.
+ * .KB_C_FN_DEFINITION_END
+ */
+void
+ExecuteEnvironmentFunctions(void)
+{
+	currentIndex = 0;
+	currentOffset = 0;
+
+	DumpBootCommands();
+	printf("Autoboot...\r\n");
+	Bootloader(ReadCharFromEnvironment);
+}
diff --git a/sys/boot/arm/at91/bootspi/env_vars.h b/sys/boot/arm/at91/bootspi/env_vars.h
new file mode 100644
index 0000000..c6e46b4
--- /dev/null
+++ b/sys/boot/arm/at91/bootspi/env_vars.h
@@ -0,0 +1,54 @@
+/******************************************************************************
+ *
+ * Filename: env_vars.h
+ *
+ * Definition of environment variables, structures, and other globals.
+ *
+ * Revision information:
+ *
+ * 20AUG2004	kb_admin	initial creation
+ *
+ * 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
+ *
+ * $FreeBSD$
+ *****************************************************************************/
+
+#ifndef _ENV_VARS_H_
+#define _ENV_VARS_H_
+
+/* each environment variable is a string following the standard command	*/
+/* definition used by the interactive loader in the following format:	*/
+/*  <command> <parm1> <parm2> ...					*/
+/* all environment variables (or commands) are stored in a string	*/
+/* format: NULL-terminated.						*/
+/* this implies that commands can never utilize 0-values: actual 0, not	*/
+/* the string '0'.  this is not an issue as the string '0' is handled	*/
+/* by the command parse routine.					*/
+
+/* the following defines the maximum size of the environment for 	*/
+/* including variables.							*/
+/* this value must match that declared in the low-level file that	*/
+/* actually reserves the space for the non-volatile environment.	*/
+#define	MAX_ENV_SIZE_BYTES	0x100
+
+#define MAX_BOOT_COMMANDS	10
+
+/* C-style reference section						*/
+#ifndef __ASSEMBLY__
+
+extern void	WriteCommandTable(void);
+extern void	SetBootCommand(int index, char *command);
+extern void	DumpBootCommands(void);
+extern void	LoadBootCommands(void);
+extern void	ExecuteEnvironmentFunctions(void);
+
+#endif /* !__ASSEMBLY__ */
+
+#endif /* _ENV_VARS_H_ */
diff --git a/sys/boot/arm/at91/bootspi/loader_prompt.c b/sys/boot/arm/at91/bootspi/loader_prompt.c
new file mode 100644
index 0000000..a947261
--- /dev/null
+++ b/sys/boot/arm/at91/bootspi/loader_prompt.c
@@ -0,0 +1,360 @@
+/******************************************************************************
+ *
+ * Filename: loader_prompt.c
+ *
+ * Instantiation of the interactive loader functions.
+ *
+ * Revision information:
+ *
+ * 20AUG2004	kb_admin	initial creation
+ * 12JAN2005	kb_admin	massive changes for tftp, strings, and more
+ * 05JUL2005	kb_admin	save tag address, and set registers on boot
+ *
+ * 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
+ *
+ * $FreeBSD$
+ *****************************************************************************/
+
+#include "at91rm9200_lowlevel.h"
+#include "at91rm9200.h"
+#include "emac.h"
+#include "loader_prompt.h"
+#include "env_vars.h"
+#include "lib.h"
+#include "spi_flash.h"
+
+/******************************* GLOBALS *************************************/
+
+
+/*********************** PRIVATE FUNCTIONS/DATA ******************************/
+
+static char	inputBuffer[MAX_INPUT_SIZE];
+static int	buffCount;
+
+// argv pointer are either NULL or point to locations in inputBuffer
+static char	*argv[MAX_COMMAND_PARAMS];
+
+#define FLASH_OFFSET (0 * FLASH_PAGE_SIZE)
+#define FPGA_OFFSET  (15 * FLASH_PAGE_SIZE)
+#define FPGA_LEN     (212608)
+#define KERNEL_OFFSET (220 * FLASH_PAGE_SIZE)
+#define KERNEL_LEN (6 * 1024 * FLASH_PAGE_SIZE)
+static const char *backspaceString = "\010 \010";
+
+static const command_entry_t	CommandTable[] = {
+	{COMMAND_DUMP, "d"},
+	{COMMAND_EXEC, "e"},
+	{COMMAND_LOCAL_IP, "ip"},
+	{COMMAND_MAC, "m"},
+	{COMMAND_SERVER_IP, "server_ip"},
+	{COMMAND_TFTP, "tftp"},
+	{COMMAND_XMODEM, "x"},
+	{COMMAND_RESET, "R"},
+	{COMMAND_LOAD_SPI_KERNEL, "k"},
+	{COMMAND_REPLACE_KERNEL_VIA_XMODEM, "K"},
+	{COMMAND_REPLACE_FLASH_VIA_XMODEM, "I"},
+	{COMMAND_REPLACE_FPGA_VIA_XMODEM, "F"},
+	{COMMAND_REPLACE_ID_EEPROM, "E"},
+	{COMMAND_FINAL_FLAG, 0}
+};
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * unsigned BuildIP(void)
+ *  This private function packs the test IP info to an unsigned value.
+ * .KB_C_FN_DEFINITION_END
+ */
+static unsigned
+BuildIP(void)
+{
+	return ((p_ASCIIToDec(argv[1]) << 24) |
+	    (p_ASCIIToDec(argv[2]) << 16) |
+	    (p_ASCIIToDec(argv[3]) << 8) |
+	    p_ASCIIToDec(argv[4]));
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * int StringToCommand(char *cPtr)
+ *  This private function converts a command string to a command code.
+ * .KB_C_FN_DEFINITION_END
+ */
+static int
+StringToCommand(char *cPtr)
+{
+	int	i;
+
+	for (i = 0; CommandTable[i].command != COMMAND_FINAL_FLAG; ++i)
+		if (!p_strcmp(CommandTable[i].c_string, cPtr))
+			return (CommandTable[i].command);
+
+	return (COMMAND_INVALID);
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * int BreakCommand(char *)
+ *  This private function splits the buffer into separate strings as pointed
+ * by argv and returns the number of parameters (< 0 on failure).
+ * .KB_C_FN_DEFINITION_END
+ */
+static int
+BreakCommand(char *buffer)
+{
+	int	pCount, cCount, state;
+
+	state = pCount = 0;
+	p_memset((char*)argv, 0, sizeof(argv));
+
+	for (cCount = 0; cCount < MAX_INPUT_SIZE; ++cCount) {
+
+		if (!state) {
+			/* look for next command */
+			if (!p_IsWhiteSpace(buffer[cCount])) {
+				argv[pCount++] = &buffer[cCount];
+				state = 1;
+			} else {
+				buffer[cCount] = 0;
+			}
+		} else {
+			/* in command, find next white space */
+			if (p_IsWhiteSpace(buffer[cCount])) {
+				buffer[cCount] = 0;
+				state = 0;
+			}
+		}
+
+		if (pCount >= MAX_COMMAND_PARAMS) {
+			return (-1);
+		}
+	}
+
+	return (pCount);
+}
+
+#if 0
+static void
+UpdateEEProm(int eeaddr)
+{
+	char *addr = (char *)SDRAM_BASE + (1 << 20); /* Load to base + 1MB */
+	int len;
+
+	while ((len = xmodem_rx(addr)) == -1)
+		continue;
+	printf("\r\nDownloaded %u bytes.\r\n", len);
+	WriteEEPROM(eeaddr, 0, addr, len);
+}
+#endif
+
+static void
+UpdateFlash(int offset)
+{
+	char *addr = (char *)SDRAM_BASE + (1 << 20); /* Load to base + 1MB */
+	int len, i, off;
+
+	while ((len = xmodem_rx(addr)) == -1)
+		continue;
+	printf("\r\nDownloaded %u bytes.\r\n", len);
+	for (i = 0; i < len; i+= FLASH_PAGE_SIZE) {
+		off = i + offset;
+		SPI_WriteFlash(off, addr + i, FLASH_PAGE_SIZE);
+	}
+}
+
+static void
+LoadKernelFromSpi(char *addr)
+{
+	int i, off;
+
+	for (i = 0; i < KERNEL_LEN; i+= FLASH_PAGE_SIZE) {
+		off = i + KERNEL_OFFSET;
+		SPI_ReadFlash(off, addr + i, FLASH_PAGE_SIZE);
+	}
+}
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void ParseCommand(char *)
+ *  This private function executes matching functions.
+ * .KB_C_FN_DEFINITION_END
+ */
+static void
+ParseCommand(char *buffer)
+{
+	int		argc, i;
+
+	if ((argc = BreakCommand(buffer)) < 1)
+		return;
+
+	switch (StringToCommand(argv[0])) {
+	case COMMAND_DUMP:
+		// display boot commands
+		DumpBootCommands();
+		break;
+
+	case COMMAND_EXEC:
+	{
+		// "e <address>"
+		// execute at address
+		void (*execAddr)(unsigned, unsigned);
+
+		if (argc > 1) {
+			/* in future, include machtypes (MACH_KB9200 = 612) */
+			execAddr = (void (*)(unsigned, unsigned))
+			    p_ASCIIToHex(argv[1]);
+			(*execAddr)(0, 612);
+		}
+		break;
+	}
+
+	case COMMAND_TFTP:
+	{
+		// "tftp <local_dest_addr filename>"
+		//  tftp download
+		unsigned address = 0;
+
+		if (argc > 2)
+			address = p_ASCIIToHex(argv[1]);
+		TFTP_Download(address, argv[2]);
+		break;
+	}
+
+	case COMMAND_SERVER_IP:
+		// "server_ip <server IP 192 200 1 20>"
+		// set download server address
+		if (argc > 4)
+			SetServerIPAddress(BuildIP());
+		break;
+
+	case COMMAND_LOCAL_IP:
+		// "local_ip <local IP 192 200 1 21>
+		// set ip of this module
+		if (argc > 4)
+			SetLocalIPAddress(BuildIP());
+		break;
+
+	case COMMAND_MAC:
+	{
+		// "m <mac address 12 34 56 78 9a bc>
+		// set mac address using 6 byte values
+		unsigned char mac[6];
+
+		if (argc > 6) {
+			for (i = 0; i < 6; i++)
+				mac[i] = p_ASCIIToHex(argv[i + 1]);
+			EMAC_SetMACAddress(mac);
+		}
+		break;
+	}
+
+	case COMMAND_LOAD_SPI_KERNEL:
+		// "k <address>"
+		if (argc > 1)
+			LoadKernelFromSpi((char *)p_ASCIIToHex(argv[1]));
+		break;
+
+	case COMMAND_XMODEM:
+		// "x <address>"
+		// download X-modem record at address
+		if (argc > 1)
+			xmodem_rx((char *)p_ASCIIToHex(argv[1]));
+		break;
+
+	case COMMAND_RESET:
+		printf("Reset\r\n");
+		reset();
+		while (1) continue;
+		break;
+
+	case COMMAND_REPLACE_KERNEL_VIA_XMODEM:
+		printf("Updating KERNEL image\r\n");
+		UpdateFlash(KERNEL_OFFSET);
+		break;
+	case COMMAND_REPLACE_FPGA_VIA_XMODEM:
+		printf("Updating FPGA image\r\n");
+		UpdateFlash(FPGA_OFFSET);
+		break;
+	case COMMAND_REPLACE_FLASH_VIA_XMODEM: 
+		printf("Updating FLASH image\r\n");
+		UpdateFlash(FLASH_OFFSET);
+		break;
+
+	default:
+		break;
+	}
+
+	printf("\r\n");
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void ServicePrompt(char)
+ *  This private function process each character checking for valid commands.
+ * This function is only executed if the character is considered valid.
+ * Each command is terminated with NULL (0) or '\r'.
+ * .KB_C_FN_DEFINITION_END
+ */
+static void
+ServicePrompt(char p_char)
+{
+	if (p_char == '\r')
+		p_char = 0;
+
+	if (p_char == '\010') {
+		if (buffCount) {
+			/* handle backspace BS */
+			inputBuffer[--buffCount] = 0;
+			printf(backspaceString);
+		}
+		return;
+	}
+	if (buffCount < MAX_INPUT_SIZE - 1) {
+		inputBuffer[buffCount++] = p_char;
+		putchar(p_char);
+	}
+	if (!p_char) {
+		printf("\r\n");
+		ParseCommand(inputBuffer);
+		p_memset(inputBuffer, 0, MAX_INPUT_SIZE);
+		buffCount = 0;
+		printf("\r\n>");
+	}
+}
+
+
+/* ************************** GLOBAL FUNCTIONS ********************************/
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void Bootloader(void *inputFunction)
+ *  This global function is the entry point for the bootloader.  If the
+ * inputFunction pointer is NULL, the loader input will be serviced from
+ * the uart.  Otherwise, inputFunction is called to get characters which
+ * the loader will parse.
+ * .KB_C_FN_DEFINITION_END
+ */
+void
+Bootloader(int(*inputFunction)(int))
+{
+	int	ch = 0;
+
+	p_memset((void*)inputBuffer, 0, sizeof(inputBuffer));
+	buffCount = 0;
+
+	printf("\r\n>");
+
+	while (1)
+		if ((ch = ((*inputFunction)(0))) > 0)
+			ServicePrompt(ch);
+}
diff --git a/sys/boot/arm/at91/bootspi/loader_prompt.h b/sys/boot/arm/at91/bootspi/loader_prompt.h
new file mode 100644
index 0000000..70d7514
--- /dev/null
+++ b/sys/boot/arm/at91/bootspi/loader_prompt.h
@@ -0,0 +1,62 @@
+/******************************************************************************
+ *
+ * Filename: loader_prompt.h
+ *
+ * Definition of the interactive loader functions.
+ *
+ * Revision information:
+ *
+ * 20AUG2004	kb_admin	initial creation
+ *
+ * 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
+ *
+ * $FreeBSD$
+ *****************************************************************************/
+
+#ifndef _LOADER_PROMPT_H_
+#define _LOADER_PROMPT_H_
+
+#define MAX_INPUT_SIZE		256
+#define MAX_COMMAND_PARAMS	10
+
+enum {
+	COMMAND_INVALID	= 0,
+	COMMAND_COPY,
+	COMMAND_DUMP,
+	COMMAND_EXEC,
+	COMMAND_HELP,
+	COMMAND_LOCAL_IP,
+	COMMAND_MAC,
+	COMMAND_SERVER_IP,
+	COMMAND_SET,
+	COMMAND_TAG,
+	COMMAND_TFTP,
+	COMMAND_WRITE,
+	COMMAND_XMODEM,
+	COMMAND_RESET,
+	COMMAND_LOAD_SPI_KERNEL,
+	COMMAND_REPLACE_KERNEL_VIA_XMODEM,
+	COMMAND_REPLACE_FLASH_VIA_XMODEM,
+	COMMAND_REPLACE_FPGA_VIA_XMODEM,
+	COMMAND_REPLACE_ID_EEPROM,
+	COMMAND_FINAL_FLAG
+} e_cmd_t;
+
+
+typedef struct {
+	int		command;
+	const char	*c_string;
+} command_entry_t;
+
+void EnterInteractiveBootloader(int(*inputFunction)(int));
+void Bootloader(int(*inputFunction)(int));
+void fpga_load(void);
+
+#endif /* _LOADER_PROMPT_H_ */
diff --git a/sys/boot/arm/at91/bootspi/main.c b/sys/boot/arm/at91/bootspi/main.c
index a187eb4b..1a39f72 100644
--- a/sys/boot/arm/at91/bootspi/main.c
+++ b/sys/boot/arm/at91/bootspi/main.c
@@ -1,57 +1,58 @@
-/*******************************************************************************
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
  *
- * Filename: main.c
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
  *
- * Basic entry points for top-level functions
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * Revision information:
+ * This software is derived from software provided by kwikbyte without
+ * copyright as follows:
  *
- * 20AUG2004	kb_admin	initial creation
- * 12JAN2005	kb_admin	cosmetic changes
- * 29APR2005	kb_admin	modified boot delay
- *
- * 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
  *
  * $FreeBSD$
- ******************************************************************************/
+ */
 
 #include "env_vars.h"
+#include "at91rm9200.h"
 #include "at91rm9200_lowlevel.h"
 #include "loader_prompt.h"
 #include "emac.h"
 #include "lib.h"
+#include "spi_flash.h"
 
-/*
- * .KB_C_FN_DEFINITION_START
- * int main(void)
- *  This global function waits at least one second, but not more than two 
- * seconds, for input from the serial port.  If no response is recognized,
- * it acts according to the parameters specified by the environment.  For 
- * example, the function might boot an operating system.  Do not return
- * from this function.
- * .KB_C_FN_DEFINITION_END
- */
 int
 main(void)
 {
-
+	printf("\r\nBoot\r\n");
+	SPI_InitFlash();
 	EMAC_Init();
-
 	LoadBootCommands();
-
-	printf("\r\nSPI Boot loader.\r\nAutoboot...\r\n");
-
-	if (getc(1) == -1)
+	if (getc(1) == -1) {
+		start_wdog(30);
 		ExecuteEnvironmentFunctions();
-
-	Bootloader(0);
-
+	}
+	Bootloader(getc);
 	return (1);
 }
diff --git a/sys/boot/arm/at91/libat91/Makefile b/sys/boot/arm/at91/libat91/Makefile
index 7c323b0..b016065 100644
--- a/sys/boot/arm/at91/libat91/Makefile
+++ b/sys/boot/arm/at91/libat91/Makefile
@@ -4,8 +4,9 @@
 
 LIB=		at91
 INTERNALLIB=
-SRCS=at91rm9200_lowlevel.c eeprom.c emac.c getc.c \
-	p_string.c putchar.c printf.c spi_flash.c xmodem.c
+SRCS=at91rm9200_lowlevel.c delay.c eeprom.c emac.c emac_init.c getc.c \
+	p_string.c putchar.c printf.c reset.c spi_flash.c xmodem.c \
+	sd-card.c mci_device.c
 NO_MAN=
 
 .if ${MK_TAG_LIST} != "no"
diff --git a/sys/boot/arm/at91/libat91/delay.c b/sys/boot/arm/at91/libat91/delay.c
new file mode 100644
index 0000000..390e067
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/delay.c
@@ -0,0 +1,43 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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 "at91rm9200.h"
+#include "spi_flash.h"
+#include "lib.h"
+
+void
+Delay(int us)
+{
+	unsigned later, now;
+
+	now = AT91C_BASE_ST->ST_CRTR;
+	later = (now + us / 25 + 1) & AT91C_ST_CRTV;
+	while (later != AT91C_BASE_ST->ST_CRTR)
+		continue;
+}
diff --git a/sys/boot/arm/at91/libat91/emac.c b/sys/boot/arm/at91/libat91/emac.c
index fa8d6fe..037e6b4 100644
--- a/sys/boot/arm/at91/libat91/emac.c
+++ b/sys/boot/arm/at91/libat91/emac.c
@@ -31,22 +31,14 @@
 
 /* ********************** PRIVATE FUNCTIONS/DATA ******************************/
 
-static unsigned localMACSet, serverMACSet;
-static unsigned char localMACAddr[6], serverMACAddr[6];
-static unsigned localMAClow, localMAChigh;
-static unsigned localIPSet, serverIPSet;
+static char serverMACAddr[6];
 static unsigned char localIPAddr[4], serverIPAddr[4];
-static unsigned short	serverPort, localPort;
 static int	ackBlock;
-static unsigned	lastSize;
 static char *dlAddress;
 
 static unsigned transmitBuffer[1024 / sizeof(unsigned)];
 static unsigned tftpSendPacket[256 / sizeof(unsigned)];
 
-receive_descriptor_t *p_rxBD;
-
-
 /*
  * .KB_C_FN_DEFINITION_START
  * unsigned short IP_checksum(unsigned short *p, int len)
@@ -474,62 +466,6 @@ AT91F_EmacEntry(void)
 
 /* ************************** GLOBAL FUNCTIONS ********************************/
 
-
-/*
- * .KB_C_FN_DEFINITION_START
- * void SetMACAddress(unsigned low_address, unsigned high_address)
- *  This global function sets the MAC address.  low_address is the first
- * four bytes while high_address is the last 2 bytes of the 48-bit value.
- * .KB_C_FN_DEFINITION_END
- */
-void
-SetMACAddress(unsigned char mac[6])
-{
-	AT91PS_PMC	pPMC = AT91C_BASE_PMC;
-	AT91PS_EMAC	pEmac = AT91C_BASE_EMAC;
-
-	/* enable the peripheral clock before using EMAC */
-	pPMC->PMC_PCER = ((unsigned) 1 << AT91C_ID_EMAC);
-
-	p_memcpy(localMACAddr, mac, 6);
-	localMAClow = (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) | mac[5];
-	localMAChigh = (mac[0] << 8) | mac[1];
-	localMACSet = 1;
-
-	AT91C_BASE_PMC->PMC_PCER = 1u << AT91C_ID_EMAC;
-	AT91C_BASE_PIOA->PIO_ASR = 
-	  AT91C_PA14_ERXER | AT91C_PA12_ERX0 | AT91C_PA13_ERX1 |
-	  AT91C_PA8_ETXEN | AT91C_PA16_EMDIO | AT91C_PA9_ETX0 |
-	  AT91C_PA10_ETX1 | AT91C_PA11_ECRS_ECRSDV | AT91C_PA15_EMDC |
-	  AT91C_PA7_ETXCK_EREFCK;
-	AT91C_BASE_PIOA->PIO_PDR = 
-	  AT91C_PA14_ERXER | AT91C_PA12_ERX0 | AT91C_PA13_ERX1 |
-	  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 */
-	AT91C_BASE_PIOB->PIO_BSR =
-	  AT91C_PB12_ETX2 | AT91C_PB13_ETX3 | AT91C_PB14_ETXER |
-	  AT91C_PB15_ERX2 | AT91C_PB16_ERX3 | AT91C_PB17_ERXDV |
-	  AT91C_PB18_ECOL | AT91C_PB19_ERXCK;
-	AT91C_BASE_PIOB->PIO_PDR =
-	  AT91C_PB12_ETX2 | AT91C_PB13_ETX3 | AT91C_PB14_ETXER |
-	  AT91C_PB15_ERX2 | AT91C_PB16_ERX3 | AT91C_PB17_ERXDV |
-	  AT91C_PB18_ECOL | AT91C_PB19_ERXCK;
-#endif
-	pEmac->EMAC_CTL  = 0;
-
-	pEmac->EMAC_CFG  = (pEmac->EMAC_CFG & ~(AT91C_EMAC_CLK)) |
-#ifdef BOOT_TSC
-	    AT91C_EMAC_RMII |
-#endif
-	    AT91C_EMAC_CLK_HCLK_32 | AT91C_EMAC_CAF;
-	// the sequence write EMAC_SA1L and write EMAC_SA1H must be respected
-	pEmac->EMAC_SA1L = localMAClow;
-	pEmac->EMAC_SA1H = localMAChigh;
-}
-
-
 /*
  * .KB_C_FN_DEFINITION_START
  * void SetServerIPAddress(unsigned address)
@@ -624,22 +560,3 @@ TFTP_Download(unsigned address, char *filename)
 	if (timeout == 0)
 		printf("TFTP TIMEOUT!\r\n");
 }
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * void EMAC_Init(void)
- *  This global function initializes variables used in tftp transfers.
- * .KB_C_FN_DEFINITION_END
- */
-void
-EMAC_Init(void)
-{
-	p_rxBD = (receive_descriptor_t*)RX_BUFFER_START;
-	localMACSet = 0;
-	serverMACSet = 0;
-	localIPSet = 0;
-	serverIPSet = 0;
-	localPort = SWAP16(0x8002);
-	lastSize = 0;
-}
diff --git a/sys/boot/arm/at91/libat91/emac.h b/sys/boot/arm/at91/libat91/emac.h
index e99759e..b8521fa 100644
--- a/sys/boot/arm/at91/libat91/emac.h
+++ b/sys/boot/arm/at91/libat91/emac.h
@@ -24,7 +24,7 @@
 #ifndef _EMAC_H_
 #define _EMAC_H_
 
-extern void SetMACAddress(unsigned char addr[6]);
+extern void EMAC_SetMACAddress(unsigned char addr[6]);
 extern void SetServerIPAddress(unsigned address);
 extern void SetLocalIPAddress(unsigned address);
 extern void EMAC_Init(void);
@@ -127,4 +127,12 @@ typedef struct {
 #define MII_SSTS_10HDX	0x1000
 #endif
 
+extern unsigned char localMACAddr[6];
+extern unsigned localMAClow, localMAChigh;
+extern unsigned localMACSet, serverMACSet;
+extern receive_descriptor_t *p_rxBD;
+extern unsigned	lastSize;
+extern unsigned localIPSet, serverIPSet;
+extern unsigned short	serverPort, localPort;
+
 #endif /* _EMAC_H_ */
diff --git a/sys/boot/arm/at91/libat91/emac_init.c b/sys/boot/arm/at91/libat91/emac_init.c
new file mode 100644
index 0000000..37844f9
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/emac_init.c
@@ -0,0 +1,139 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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$
+ */
+
+/******************************************************************************
+ *
+ * Filename: emac.c
+ *
+ * Instantiation of routines for MAC/ethernet functions supporting tftp.
+ *
+ * Revision information:
+ *
+ * 28AUG2004	kb_admin	initial creation
+ * 08JAN2005	kb_admin	added tftp download
+ *					also adapted from external sources
+ *
+ * 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 "at91rm9200.h"
+#include "at91rm9200_lowlevel.h"
+#include "emac.h"
+#include "lib.h"
+
+/* ****************************** GLOBALS *************************************/
+
+unsigned	lastSize;
+unsigned localMACSet, serverMACSet;
+unsigned char localMACAddr[6];
+unsigned localMAClow, localMAChigh;
+unsigned localIPSet, serverIPSet;
+unsigned short	serverPort, localPort;
+receive_descriptor_t *p_rxBD;
+
+/* ********************** PRIVATE FUNCTIONS/DATA ******************************/
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void EMAC_SetMACAddress(unsigned low_address, unsigned high_address)
+ *  This global function sets the MAC address.  low_address is the first
+ * four bytes while high_address is the last 2 bytes of the 48-bit value.
+ * .KB_C_FN_DEFINITION_END
+ */
+void
+EMAC_SetMACAddress(unsigned char mac[6])
+{
+	AT91PS_PMC	pPMC = AT91C_BASE_PMC;
+	AT91PS_EMAC	pEmac = AT91C_BASE_EMAC;
+
+	/* enable the peripheral clock before using EMAC */
+	pPMC->PMC_PCER = ((unsigned) 1 << AT91C_ID_EMAC);
+
+	p_memcpy(localMACAddr, mac, 6);
+	localMAClow = (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) | mac[5];
+	localMAChigh = (mac[0] << 8) | mac[1];
+	localMACSet = 1;
+
+	AT91C_BASE_PMC->PMC_PCER = 1u << AT91C_ID_EMAC;
+	AT91C_BASE_PIOA->PIO_ASR = 
+	  AT91C_PA14_ERXER | AT91C_PA12_ERX0 | AT91C_PA13_ERX1 |
+	  AT91C_PA8_ETXEN | AT91C_PA16_EMDIO | AT91C_PA9_ETX0 |
+	  AT91C_PA10_ETX1 | AT91C_PA11_ECRS_ECRSDV | AT91C_PA15_EMDC |
+	  AT91C_PA7_ETXCK_EREFCK;
+	AT91C_BASE_PIOA->PIO_PDR = 
+	  AT91C_PA14_ERXER | AT91C_PA12_ERX0 | AT91C_PA13_ERX1 |
+	  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 */
+	AT91C_BASE_PIOB->PIO_BSR =
+	  AT91C_PB12_ETX2 | AT91C_PB13_ETX3 | AT91C_PB14_ETXER |
+	  AT91C_PB15_ERX2 | AT91C_PB16_ERX3 | AT91C_PB17_ERXDV |
+	  AT91C_PB18_ECOL | AT91C_PB19_ERXCK;
+	AT91C_BASE_PIOB->PIO_PDR =
+	  AT91C_PB12_ETX2 | AT91C_PB13_ETX3 | AT91C_PB14_ETXER |
+	  AT91C_PB15_ERX2 | AT91C_PB16_ERX3 | AT91C_PB17_ERXDV |
+	  AT91C_PB18_ECOL | AT91C_PB19_ERXCK;
+#endif
+	pEmac->EMAC_CTL  = 0;
+
+	pEmac->EMAC_CFG  = (pEmac->EMAC_CFG & ~(AT91C_EMAC_CLK)) |
+#ifdef BOOT_TSC
+	    AT91C_EMAC_RMII |
+#endif
+	    AT91C_EMAC_CLK_HCLK_32 | AT91C_EMAC_CAF;
+	// the sequence write EMAC_SA1L and write EMAC_SA1H must be respected
+	pEmac->EMAC_SA1L = localMAClow;
+	pEmac->EMAC_SA1H = localMAChigh;
+}
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * void EMAC_Init(void)
+ *  This global function initializes variables used in tftp transfers.
+ * .KB_C_FN_DEFINITION_END
+ */
+void
+EMAC_Init(void)
+{
+	p_rxBD = (receive_descriptor_t*)RX_BUFFER_START;
+	localMACSet = 0;
+	serverMACSet = 0;
+	localIPSet = 0;
+	serverIPSet = 0;
+	localPort = SWAP16(0x8002);
+	lastSize = 0;
+}
diff --git a/sys/boot/arm/at91/libat91/lib_AT91RM9200.h b/sys/boot/arm/at91/libat91/lib_AT91RM9200.h
new file mode 100644
index 0000000..d1bf488
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/lib_AT91RM9200.h
@@ -0,0 +1,639 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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$
+ */
+
+#ifndef __LIBAT91RM9200_H
+#define __LIBAT91RM9200_H
+
+#include "at91rm9200.h"
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PMC_EnablePeriphClock
+//* \brief Enable peripheral clock
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PMC_EnablePeriphClock (
+	AT91PS_PMC pPMC, // \arg pointer to PMC controller
+	unsigned int periphIds)  // \arg IDs of peripherals to enable
+{
+	pPMC->PMC_PCER = periphIds;
+}
+
+/* *****************************************************************************
+                SOFTWARE API FOR PIO
+   ***************************************************************************** */
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PIO_CfgPeriph
+//* \brief Enable pins to be drived by peripheral
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_PIO_CfgPeriph(
+	AT91PS_PIO pPio,             // \arg pointer to a PIO controller
+	unsigned int periphAEnable,  // \arg PERIPH A to enable
+	unsigned int periphBEnable)  // \arg PERIPH B to enable
+
+{
+	pPio->PIO_ASR = periphAEnable;
+	pPio->PIO_BSR = periphBEnable;
+	pPio->PIO_PDR = (periphAEnable | periphBEnable); // Set in Periph mode
+}
+
+/* *****************************************************************************
+                SOFTWARE API FOR MCI
+   ***************************************************************************** */
+//* Classic MCI Mode Register Configuration with PDC mode enabled and MCK = MCI Clock
+#define AT91C_MCI_MR_PDCMODE	(AT91C_MCI_CLKDIV |\
+                                AT91C_MCI_PWSDIV |\
+                                (AT91C_MCI_PWSDIV<<1) |\
+                                AT91C_MCI_PDCMODE) 
+
+//* Classic MCI Data Timeout Register Configuration with 1048576 MCK cycles between 2 data transfer
+#define AT91C_MCI_DTOR_1MEGA_CYCLES	(AT91C_MCI_DTOCYC | AT91C_MCI_DTOMUL)
+
+//* Classic MCI SDCard Register Configuration with 1-bit data bus on slot A
+#define AT91C_MCI_MMC_SLOTA	(AT91C_MCI_SCDSEL & 0x0)
+
+//* Classic MCI SDCard Register Configuration with 1-bit data bus on slot B
+#define AT91C_MCI_MMC_SLOTB	(AT91C_MCI_SCDSEL)
+
+//* Classic MCI SDCard Register Configuration with 4-bit data bus on slot A
+#define AT91C_MCI_SDCARD_4BITS_SLOTA	( (AT91C_MCI_SCDSEL & 0x0) | AT91C_MCI_SCDBUS )
+
+//* Classic MCI SDCard Register Configuration with 4-bit data bus on slot B
+#define AT91C_MCI_SDCARD_4BITS_SLOTB	(AT91C_MCI_SCDSEL | AT91C_MCI_SCDBUS)
+
+
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_Configure
+//* \brief Configure the MCI
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_MCI_Configure(
+        AT91PS_MCI pMCI,  			 // \arg pointer to a MCI controller
+        unsigned int DTOR_register,  // \arg Data Timeout Register to be programmed
+        unsigned int MR_register,  	 // \arg Mode Register to be programmed
+        unsigned int SDCR_register)  // \arg SDCard Register to be programmed
+{
+    //* Reset the MCI
+    pMCI->MCI_CR = AT91C_MCI_MCIEN | AT91C_MCI_PWSEN;
+
+    //* Disable all the interrupts
+    pMCI->MCI_IDR = 0xFFFFFFFF;
+
+    //* Set the Data Timeout Register
+    pMCI->MCI_DTOR = DTOR_register;
+
+    //* Set the Mode Register
+    pMCI->MCI_MR = MR_register;
+
+    //* Set the SDCard Register
+    pMCI->MCI_SDCR = SDCR_register;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_EnableIt
+//* \brief Enable MCI IT
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_MCI_EnableIt(
+        AT91PS_MCI pMCI, // \arg pointer to a MCI controller
+        unsigned int flag)   // \arg IT to be enabled
+{
+    //* Write to the IER register
+    pMCI->MCI_IER = flag;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_DisableIt
+//* \brief Disable MCI IT
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_MCI_DisableIt(
+        AT91PS_MCI pMCI, // \arg pointer to a MCI controller
+        unsigned int flag)   // \arg IT to be disabled
+{
+    //* Write to the IDR register
+    pMCI->MCI_IDR = flag;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_Enable_Interface
+//* \brief Enable the MCI Interface
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_MCI_Enable_Interface(
+        AT91PS_MCI pMCI)     // \arg pointer to a MCI controller
+{
+    //* Enable the MCI
+    pMCI->MCI_CR = AT91C_MCI_MCIEN;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_Disable_Interface
+//* \brief Disable the MCI Interface
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_MCI_Disable_Interface(
+        AT91PS_MCI pMCI)     // \arg pointer to a MCI controller
+{
+    //* Disable the MCI
+    pMCI->MCI_CR = AT91C_MCI_MCIDIS;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_Cfg_ModeRegister
+//* \brief Configure the MCI Mode Register
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_MCI_Cfg_ModeRegister(
+        AT91PS_MCI pMCI, // \arg pointer to a MCI controller
+        unsigned int mode_register)   // \arg value to set in the mode register
+{
+    //* Configure the MCI MR
+    pMCI->MCI_MR = mode_register;
+}
+
+/* *****************************************************************************
+                SOFTWARE API FOR AIC
+   ***************************************************************************** */
+#define AT91C_AIC_BRANCH_OPCODE ((void (*) ()) 0xE51FFF20) // ldr, pc, [pc, #-&F20]
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_ConfigureIt
+//* \brief Interrupt Handler Initialization
+//*----------------------------------------------------------------------------
+static inline unsigned int
+AT91F_AIC_ConfigureIt(
+	AT91PS_AIC pAic,  // \arg pointer to the AIC registers
+	unsigned int irq_id,     // \arg interrupt number to initialize
+	unsigned int priority,   // \arg priority to give to the interrupt
+	unsigned int src_type,   // \arg activation and sense of activation
+	void (*newHandler) (void) ) // \arg address of the interrupt handler
+{
+	unsigned int oldHandler;
+    unsigned int mask ;
+
+    oldHandler = pAic->AIC_SVR[irq_id];
+
+    mask = 0x1 << irq_id ;
+    //* Disable the interrupt on the interrupt controller
+    pAic->AIC_IDCR = mask ;
+    //* Save the interrupt handler routine pointer and the interrupt priority
+    pAic->AIC_SVR[irq_id] = (unsigned int) newHandler ;
+    //* Store the Source Mode Register
+    pAic->AIC_SMR[irq_id] = src_type | priority  ;
+    //* Clear the interrupt on the interrupt controller
+    pAic->AIC_ICCR = mask ;
+
+	return oldHandler;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_EnableIt
+//* \brief Enable corresponding IT number
+//*----------------------------------------------------------------------------
+static inline
+void AT91F_AIC_EnableIt(
+	AT91PS_AIC pAic,      // \arg pointer to the AIC registers
+	unsigned int irq_id ) // \arg interrupt number to initialize
+{
+    //* Enable the interrupt on the interrupt controller
+    pAic->AIC_IECR = 0x1 << irq_id ;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_DisableIt
+//* \brief Disable corresponding IT number
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_AIC_DisableIt(
+	AT91PS_AIC pAic,      // \arg pointer to the AIC registers
+	unsigned int irq_id ) // \arg interrupt number to initialize
+{
+    unsigned int mask = 0x1 << irq_id;
+    //* Disable the interrupt on the interrupt controller
+    pAic->AIC_IDCR = mask ;
+    //* Clear the interrupt on the Interrupt Controller ( if one is pending )
+    pAic->AIC_ICCR = mask ;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_ClearIt
+//* \brief Clear corresponding IT number
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_AIC_ClearIt(
+	AT91PS_AIC pAic,     // \arg pointer to the AIC registers
+	unsigned int irq_id) // \arg interrupt number to initialize
+{
+    //* Clear the interrupt on the Interrupt Controller ( if one is pending )
+    pAic->AIC_ICCR = (0x1 << irq_id);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_AcknowledgeIt
+//* \brief Acknowledge corresponding IT number
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_AIC_AcknowledgeIt(
+	AT91PS_AIC pAic)     // \arg pointer to the AIC registers
+{
+    pAic->AIC_EOICR = pAic->AIC_EOICR;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_SetExceptionVector
+//* \brief Configure vector handler
+//*----------------------------------------------------------------------------
+static inline unsigned int
+AT91F_AIC_SetExceptionVector(
+	unsigned int *pVector, // \arg pointer to the AIC registers
+	void (*Handler)(void) )   // \arg Interrupt Handler
+{
+	unsigned int oldVector = *pVector;
+
+	if ((unsigned int) Handler == (unsigned int) AT91C_AIC_BRANCH_OPCODE)
+		*pVector = (unsigned int) AT91C_AIC_BRANCH_OPCODE;
+	else
+		*pVector = (((((unsigned int) Handler) - ((unsigned int) pVector) - 0x8) >> 2) & 0x00FFFFFF) | 0xEA000000;
+
+	return oldVector;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_Trig
+//* \brief Trig an IT
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_AIC_Trig(
+	AT91PS_AIC pAic,     // \arg pointer to the AIC registers
+	unsigned int irq_id) // \arg interrupt number
+{
+	pAic->AIC_ISCR = (0x1 << irq_id) ;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_IsActive
+//* \brief Test if an IT is active
+//*----------------------------------------------------------------------------
+static inline unsigned int
+AT91F_AIC_IsActive(
+	AT91PS_AIC pAic,     // \arg pointer to the AIC registers
+	unsigned int irq_id) // \arg Interrupt Number
+{
+	return (pAic->AIC_ISR & (0x1 << irq_id));
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_IsPending
+//* \brief Test if an IT is pending
+//*----------------------------------------------------------------------------
+static inline unsigned int
+AT91F_AIC_IsPending(
+	AT91PS_AIC pAic,     // \arg pointer to the AIC registers
+	unsigned int irq_id) // \arg Interrupt Number
+{
+	return (pAic->AIC_IPR & (0x1 << irq_id));
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_Open
+//* \brief Set exception vectors and AIC registers to default values
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_AIC_Open(
+	AT91PS_AIC pAic,        // \arg pointer to the AIC registers
+	void (*IrqHandler)(void),  // \arg Default IRQ vector exception
+	void (*FiqHandler)(void),  // \arg Default FIQ vector exception
+	void (*DefaultHandler)(void), // \arg Default Handler set in ISR
+	void (*SpuriousHandler)(void), // \arg Default Spurious Handler
+	unsigned int protectMode)   // \arg Debug Control Register
+{
+	int i;
+
+	// Disable all interrupts and set IVR to the default handler
+	for (i = 0; i < 32; ++i) {
+		AT91F_AIC_DisableIt(pAic, i);
+		AT91F_AIC_ConfigureIt(pAic, i, AT91C_AIC_PRIOR_LOWEST, AT91C_AIC_SRCTYPE_INT_LEVEL_SENSITIVE, DefaultHandler);
+	}
+
+	// Set the IRQ exception vector
+	AT91F_AIC_SetExceptionVector((unsigned int *) 0x18, IrqHandler);
+	// Set the Fast Interrupt exception vector
+	AT91F_AIC_SetExceptionVector((unsigned int *) 0x1C, FiqHandler);
+
+	pAic->AIC_SPU = (unsigned int) SpuriousHandler;
+	pAic->AIC_DCR = protectMode;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_CfgPMC
+//* \brief Enable Peripheral clock in PMC for  MCI
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_MCI_CfgPMC(void)
+{
+	AT91F_PMC_EnablePeriphClock(
+		AT91C_BASE_PMC, // PIO controller base address
+		((unsigned int) 1 << AT91C_ID_MCI));
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_CfgPIO
+//* \brief Configure PIO controllers to drive MCI signals
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_MCI_CfgPIO(void)
+{
+	// Configure PIO controllers to periph mode
+	AT91F_PIO_CfgPeriph(
+		AT91C_BASE_PIOA, // PIO controller base address
+		((unsigned int) AT91C_PA28_MCCDA   ) |
+		((unsigned int) AT91C_PA29_MCDA0   ) |
+		((unsigned int) AT91C_PA27_MCCK    ), // Peripheral A
+		0); // Peripheral B
+	// Configure PIO controllers to periph mode
+	AT91F_PIO_CfgPeriph(
+		AT91C_BASE_PIOB, // PIO controller base address
+		0, // Peripheral A
+		((unsigned int) AT91C_PB5_MCDA3   ) |
+		((unsigned int) AT91C_PB3_MCDA1   ) |
+		((unsigned int) AT91C_PB4_MCDA2   )); // Peripheral B
+}
+
+
+/* *****************************************************************************
+                SOFTWARE API FOR PDC
+   ***************************************************************************** */
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_SetNextRx
+//* \brief Set the next receive transfer descriptor
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_SetNextRx (
+	AT91PS_PDC pPDC,     // \arg pointer to a PDC controller
+	char *address,       // \arg address to the next bloc to be received
+	unsigned int bytes)  // \arg number of bytes to be received
+{
+	pPDC->PDC_RNPR = (unsigned int) address;
+	pPDC->PDC_RNCR = bytes;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_SetNextTx
+//* \brief Set the next transmit transfer descriptor
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_SetNextTx(
+	AT91PS_PDC pPDC,       // \arg pointer to a PDC controller
+	char *address,         // \arg address to the next bloc to be transmitted
+	unsigned int bytes)    // \arg number of bytes to be transmitted
+{
+	pPDC->PDC_TNPR = (unsigned int) address;
+	pPDC->PDC_TNCR = bytes;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_SetRx
+//* \brief Set the receive transfer descriptor
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_SetRx(
+	AT91PS_PDC pPDC,       // \arg pointer to a PDC controller
+	char *address,         // \arg address to the next bloc to be received
+	unsigned int bytes)    // \arg number of bytes to be received
+{
+	pPDC->PDC_RPR = (unsigned int) address;
+	pPDC->PDC_RCR = bytes;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_SetTx
+//* \brief Set the transmit transfer descriptor
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_SetTx(
+	AT91PS_PDC pPDC,       // \arg pointer to a PDC controller
+	char *address,         // \arg address to the next bloc to be transmitted
+	unsigned int bytes)    // \arg number of bytes to be transmitted
+{
+	pPDC->PDC_TPR = (unsigned int) address;
+	pPDC->PDC_TCR = bytes;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_EnableTx
+//* \brief Enable transmit
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_EnableTx(
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	pPDC->PDC_PTCR = AT91C_PDC_TXTEN;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_EnableRx
+//* \brief Enable receive
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_EnableRx(
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	pPDC->PDC_PTCR = AT91C_PDC_RXTEN;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_DisableTx
+//* \brief Disable transmit
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_DisableTx(
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	pPDC->PDC_PTCR = AT91C_PDC_TXTDIS;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_DisableRx
+//* \brief Disable receive
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_DisableRx(
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	pPDC->PDC_PTCR = AT91C_PDC_RXTDIS;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_IsTxEmpty
+//* \brief Test if the current transfer descriptor has been sent
+//*----------------------------------------------------------------------------
+static inline int
+AT91F_PDC_IsTxEmpty( // \return return 1 if transfer is complete
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	return !(pPDC->PDC_TCR);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_IsNextTxEmpty
+//* \brief Test if the next transfer descriptor has been moved to the current td
+//*----------------------------------------------------------------------------
+static inline int
+AT91F_PDC_IsNextTxEmpty( // \return return 1 if transfer is complete
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	return !(pPDC->PDC_TNCR);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_IsRxEmpty
+//* \brief Test if the current transfer descriptor has been filled
+//*----------------------------------------------------------------------------
+static inline int
+AT91F_PDC_IsRxEmpty( // \return return 1 if transfer is complete
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	return !(pPDC->PDC_RCR);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_IsNextRxEmpty
+//* \brief Test if the next transfer descriptor has been moved to the current td
+//*----------------------------------------------------------------------------
+static inline int
+AT91F_PDC_IsNextRxEmpty( // \return return 1 if transfer is complete
+	AT91PS_PDC pPDC )       // \arg pointer to a PDC controller
+{
+	return !(pPDC->PDC_RNCR);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_Open
+//* \brief Open PDC: disable TX and RX reset transfer descriptors, re-enable RX and TX
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_Open(
+	AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
+{
+    //* Disable the RX and TX PDC transfer requests
+	AT91F_PDC_DisableRx(pPDC);
+	AT91F_PDC_DisableTx(pPDC);
+
+	//* Reset all Counter register Next buffer first
+	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
+
+    //* Enable the RX and TX PDC transfer requests
+	AT91F_PDC_EnableRx(pPDC);
+	AT91F_PDC_EnableTx(pPDC);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_Close
+//* \brief Close PDC: disable TX and RX reset transfer descriptors
+//*----------------------------------------------------------------------------
+static inline void
+AT91F_PDC_Close(
+	AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
+{
+    //* Disable the RX and TX PDC transfer requests
+	AT91F_PDC_DisableRx(pPDC);
+	AT91F_PDC_DisableTx(pPDC);
+
+	//* Reset all Counter register Next buffer first
+	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
+
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_SendFrame
+//* \brief Close PDC: disable TX and RX reset transfer descriptors
+//*----------------------------------------------------------------------------
+static inline unsigned int
+AT91F_PDC_SendFrame(
+	AT91PS_PDC pPDC,
+	char *pBuffer,
+	unsigned int szBuffer,
+	char *pNextBuffer,
+	unsigned int szNextBuffer )
+{
+	if (AT91F_PDC_IsTxEmpty(pPDC)) {
+		//* Buffer and next buffer can be initialized
+		AT91F_PDC_SetTx(pPDC, pBuffer, szBuffer);
+		AT91F_PDC_SetNextTx(pPDC, pNextBuffer, szNextBuffer);
+		return 2;
+	}
+	else if (AT91F_PDC_IsNextTxEmpty(pPDC)) {
+		//* Only one buffer can be initialized
+		AT91F_PDC_SetNextTx(pPDC, pBuffer, szBuffer);
+		return 1;
+	}
+	else {
+		//* All buffer are in use...
+		return 0;
+	}
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_ReceiveFrame
+//* \brief Close PDC: disable TX and RX reset transfer descriptors
+//*----------------------------------------------------------------------------
+static inline unsigned int
+AT91F_PDC_ReceiveFrame(
+	AT91PS_PDC pPDC,
+	char *pBuffer,
+	unsigned int szBuffer,
+	char *pNextBuffer,
+	unsigned int szNextBuffer )
+{
+	if (AT91F_PDC_IsRxEmpty(pPDC)) {
+		//* Buffer and next buffer can be initialized
+		AT91F_PDC_SetRx(pPDC, pBuffer, szBuffer);
+		AT91F_PDC_SetNextRx(pPDC, pNextBuffer, szNextBuffer);
+		return 2;
+	}
+	else if (AT91F_PDC_IsNextRxEmpty(pPDC)) {
+		//* Only one buffer can be initialized
+		AT91F_PDC_SetNextRx(pPDC, pBuffer, szBuffer);
+		return 1;
+	}
+	else {
+		//* All buffer are in use...
+		return 0;
+	}
+}
+
+#endif
diff --git a/sys/boot/arm/at91/libat91/mci_device.c b/sys/boot/arm/at91/libat91/mci_device.c
new file mode 100644
index 0000000..f6dae8d
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/mci_device.c
@@ -0,0 +1,616 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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$
+ */
+
+//*----------------------------------------------------------------------------
+//*         ATMEL Microcontroller Software Support  -  ROUSSET  -
+//*----------------------------------------------------------------------------
+//* The software is delivered "AS IS" without warranty or condition of any
+//* kind, either express, implied or statutory. This includes without
+//* limitation any warranty or condition with respect to merchantability or
+//* fitness for any particular purpose, or against the infringements of
+//* intellectual property rights of others.
+//*----------------------------------------------------------------------------
+//* File Name           : mci_device.c
+//* Object              : TEST DataFlash Functions
+//* Creation            : FB   26/11/2002
+//*
+//*----------------------------------------------------------------------------
+#include "at91rm9200.h"
+#include "lib_AT91RM9200.h"
+
+#include "mci_device.h"
+
+#include "lib.h"
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SendCommand
+//* \brief Generic function to send a command to the MMC or SDCard
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_SendCommand(
+	unsigned int Cmd,
+	unsigned int Arg)
+{
+	unsigned int	error,status;
+
+	AT91C_BASE_MCI->MCI_ARGR = Arg;
+	AT91C_BASE_MCI->MCI_CMDR = Cmd;
+
+	// wait for CMDRDY Status flag to read the response
+	do
+	{
+		status = AT91C_BASE_MCI->MCI_SR;
+	} while( !(status & AT91C_MCI_CMDRDY) );
+
+	// 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 the command is SEND_OP_COND the CRC error flag is always present (cf : R3 response)
+		if ( (Cmd != AT91C_SDCARD_APP_OP_COND_CMD) && (Cmd != AT91C_MMC_SEND_OP_COND_CMD) )
+			return ((AT91C_BASE_MCI->MCI_SR) & AT91C_MCI_SR_ERROR);
+		if (error != AT91C_MCI_RCRCE)
+			return ((AT91C_BASE_MCI->MCI_SR) & AT91C_MCI_SR_ERROR);
+	}
+	return AT91C_CMD_SEND_OK;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SDCard_SendAppCommand
+//* \brief Specific function to send a specific command to the SDCard
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_SDCard_SendAppCommand(
+	AT91PS_MciDevice pMCI_Device,
+	unsigned int Cmd_App,
+	unsigned int Arg)
+{
+	unsigned int status;
+
+	// Send the CMD55 for application specific command
+	AT91C_BASE_MCI->MCI_ARGR = (pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address << 16 );
+	AT91C_BASE_MCI->MCI_CMDR = AT91C_APP_CMD;
+
+	// wait for CMDRDY Status flag to read the response
+	do
+	{
+		status = AT91C_BASE_MCI->MCI_SR;
+	}
+	while( !(status & AT91C_MCI_CMDRDY) );
+
+	// if an error occurs
+	if (((AT91C_BASE_MCI->MCI_SR) & AT91C_MCI_SR_ERROR) != 0 )
+		return ((AT91C_BASE_MCI->MCI_SR) & AT91C_MCI_SR_ERROR);
+
+	// check if it is a specific command and then send the command
+	if ( (Cmd_App && AT91C_SDCARD_APP_ALL_CMD) == 0)
+		return AT91C_CMD_SEND_ERROR;
+
+	return(AT91F_MCI_SendCommand(Cmd_App,Arg));
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_GetStatus
+//* \brief Addressed card sends its status register
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_GetStatus(unsigned int relative_card_address)
+{
+	if (AT91F_MCI_SendCommand(AT91C_SEND_STATUS_CMD,
+		relative_card_address <<16) == AT91C_CMD_SEND_OK)
+		return (AT91C_BASE_MCI->MCI_RSPR[0]);
+	return AT91C_CMD_SEND_ERROR;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_Device_Handler
+//* \brief MCI C interrupt handler
+//*----------------------------------------------------------------------------
+void
+AT91F_MCI_Device_Handler(
+	AT91PS_MciDevice pMCI_Device,
+	unsigned int status)
+{
+	// If End of Tx Buffer Empty interrupt occurred
+	if (pMCI_Device->pMCI_DeviceDesc->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;
+		pMCI_Device->pMCI_DeviceDesc->state = AT91C_MCI_IDLE;
+	}	// End of if AT91C_MCI_TXBUFF
+
+	// If End of Rx Buffer Full interrupt occurred
+	if (pMCI_Device->pMCI_DeviceDesc->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;
+		pMCI_Device->pMCI_DeviceDesc->state = AT91C_MCI_IDLE;
+	}	// End of if AT91C_MCI_RXBUFF
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_ReadBlock
+//* \brief Read an ENTIRE block or PARTIAL block
+//*----------------------------------------------------------------------------
+AT91S_MCIDeviceStatus
+AT91F_MCI_ReadBlock(
+	AT91PS_MciDevice pMCI_Device,
+	int src,
+	unsigned int *dataBuffer,
+	int sizeToRead)
+{
+	///////////////////////////////////////////////////////////////////////
+	if (pMCI_Device->pMCI_DeviceDesc->state != AT91C_MCI_IDLE) {
+#if IMP_DEBUG
+	    printf("1 state is 0x%x\r\n", pMCI_Device->pMCI_DeviceDesc->state);
+#endif
+	    return AT91C_READ_ERROR;
+	}
+	
+    
+	if ((AT91F_MCI_GetStatus(
+	    pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address) & AT91C_SR_READY_FOR_DATA) !=
+	    AT91C_SR_READY_FOR_DATA) {
+#if IMP_DEBUG
+	    printf("2\r\n");
+#endif
+	    return AT91C_READ_ERROR;
+	}
+    	
+	if ( (src + sizeToRead) > pMCI_Device->pMCI_DeviceFeatures->Memory_Capacity ) {
+#if IMP_DEBUG
+	    printf("3\r\n");
+#endif
+	    return AT91C_READ_ERROR;
+	}
+
+	// If source does not fit a begin of a block
+	if ((src & ((1 << pMCI_Device->pMCI_DeviceFeatures->READ_BL_LEN) - 1)) != 0) {
+#if IMP_DEBUG
+	    printf("4\r\n");
+#endif
+	    return AT91C_READ_ERROR;
+	}
+   
+	// Test if the MMC supports Partial Read Block
+	// ALWAYS SUPPORTED IN SD Memory Card
+	if( (sizeToRead < pMCI_Device->pMCI_DeviceFeatures->Max_Read_DataBlock_Length) 
+	    && (pMCI_Device->pMCI_DeviceFeatures->Read_Partial == 0x00) ) {
+#if IMP_DEBUG
+	    printf("5\r\n");
+#endif
+	    return AT91C_READ_ERROR;
+	}
+   		
+	if( sizeToRead > pMCI_Device->pMCI_DeviceFeatures->Max_Read_DataBlock_Length) {
+#if IMP_DEBUG
+	    printf("6\r\n");
+#endif
+	    return AT91C_READ_ERROR;
+	}
+	///////////////////////////////////////////////////////////////////////
+      
+        // Init Mode Register
+	AT91C_BASE_MCI->MCI_MR |= ((pMCI_Device->pMCI_DeviceFeatures->Max_Read_DataBlock_Length << 16) | AT91C_MCI_PDCMODE);
+	 
+	if (sizeToRead %4)
+		sizeToRead = (sizeToRead /4)+1;
+	else
+		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 (AT91F_MCI_SendCommand(AT91C_READ_SINGLE_BLOCK_CMD, src) != AT91C_CMD_SEND_OK)
+		return AT91C_READ_ERROR;
+	pMCI_Device->pMCI_DeviceDesc->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 AT91C_READ_OK;
+}
+
+#if 0
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_WriteBlock
+//* \brief  Write an ENTIRE block but not always PARTIAL block !!!
+//*----------------------------------------------------------------------------
+AT91S_MCIDeviceStatus
+AT91F_MCI_WriteBlock(
+	AT91PS_MciDevice pMCI_Device,
+	int dest,
+	unsigned int *dataBuffer,
+	int sizeToWrite )
+{
+	///////////////////////////////////////////////////////////////////////
+	if( pMCI_Device->pMCI_DeviceDesc->state != AT91C_MCI_IDLE)
+		return AT91C_WRITE_ERROR;
+    
+	if( (AT91F_MCI_GetStatus(pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address) & AT91C_SR_READY_FOR_DATA) != AT91C_SR_READY_FOR_DATA)
+		return AT91C_WRITE_ERROR;
+    	
+	if ((dest + sizeToWrite) > pMCI_Device->pMCI_DeviceFeatures->Memory_Capacity)
+		return AT91C_WRITE_ERROR;
+
+    // If source does not fit a begin of a block
+	if ( (dest % pMCI_Device->pMCI_DeviceFeatures->Max_Read_DataBlock_Length) != 0 )
+		return AT91C_WRITE_ERROR;
+   
+    // Test if the MMC supports Partial Write Block 
+	if( (sizeToWrite < pMCI_Device->pMCI_DeviceFeatures->Max_Write_DataBlock_Length) 
+	    && (pMCI_Device->pMCI_DeviceFeatures->Write_Partial == 0x00) )
+   		return AT91C_WRITE_ERROR;
+   		
+   	if( sizeToWrite > pMCI_Device->pMCI_DeviceFeatures->Max_Write_DataBlock_Length )
+   		return AT91C_WRITE_ERROR;
+	///////////////////////////////////////////////////////////////////////
+  
+	// Init Mode Register
+	AT91C_BASE_MCI->MCI_MR |= ((pMCI_Device->pMCI_DeviceFeatures->Max_Write_DataBlock_Length << 16) | AT91C_MCI_PDCMODE);
+	
+	if (sizeToWrite %4)
+		sizeToWrite = (sizeToWrite /4)+1;
+	else
+		sizeToWrite = sizeToWrite/4;
+
+	// Init PDC for write sequence
+	AT91C_BASE_PDC_MCI->PDC_PTCR = (AT91C_PDC_TXTDIS | AT91C_PDC_RXTDIS);
+	AT91C_BASE_PDC_MCI->PDC_TPR = (unsigned int) dataBuffer;
+	AT91C_BASE_PDC_MCI->PDC_TCR = sizeToWrite;
+
+	// Send the write single block command
+	if ( AT91F_MCI_SendCommand(AT91C_WRITE_BLOCK_CMD, dest) != AT91C_CMD_SEND_OK)
+		return AT91C_WRITE_ERROR;
+
+	pMCI_Device->pMCI_DeviceDesc->state = AT91C_MCI_TX_SINGLE_BLOCK;
+
+	// Enable AT91C_MCI_TXBUFE Interrupt
+	AT91C_BASE_MCI->MCI_IER = AT91C_MCI_TXBUFE;
+  
+  	// Enables TX for PDC transfert requests
+	AT91C_BASE_PDC_MCI->PDC_PTCR = AT91C_PDC_TXTEN;
+  
+	return AT91C_WRITE_OK;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_MMC_SelectCard
+//* \brief Toggles a card between the Stand_by and Transfer states or between Programming and Disconnect states
+//*----------------------------------------------------------------------------
+AT91S_MCIDeviceStatus
+AT91F_MCI_MMC_SelectCard(AT91PS_MciDevice pMCI_Device, unsigned int relative_card_address)
+{
+    int status;
+	
+	//* Check if the MMC card chosen is already the selected one
+	status = AT91F_MCI_GetStatus(relative_card_address);
+
+	if (status < 0)
+		return AT91C_CARD_SELECTED_ERROR;
+
+	if ((status & AT91C_SR_CARD_SELECTED) == AT91C_SR_CARD_SELECTED)
+		return AT91C_CARD_SELECTED_OK;
+
+	//* Search for the MMC Card to be selected, status = the Corresponding Device Number
+	status = 0;
+	while( (pMCI_Device->pMCI_DeviceFeatures[status].Relative_Card_Address != relative_card_address)
+		   && (status < AT91C_MAX_MCI_CARDS) )
+		status++;
+
+	if (status > AT91C_MAX_MCI_CARDS)
+    	return AT91C_CARD_SELECTED_ERROR;
+
+    if (AT91F_MCI_SendCommand(AT91C_SEL_DESEL_CARD_CMD,
+	  pMCI_Device->pMCI_DeviceFeatures[status].Relative_Card_Address << 16) == AT91C_CMD_SEND_OK)
+    	return AT91C_CARD_SELECTED_OK;
+    return AT91C_CARD_SELECTED_ERROR;
+}
+#endif
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_GetCSD
+//* \brief Asks to the specified card to send its CSD
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_GetCSD(unsigned int relative_card_address , unsigned int * response)
+{
+ 	
+ 	if(AT91F_MCI_SendCommand(AT91C_SEND_CSD_CMD,
+	       (relative_card_address << 16)) != AT91C_CMD_SEND_OK)
+		return AT91C_CMD_SEND_ERROR;
+	
+	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 AT91C_CMD_SEND_OK;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SetBlocklength
+//* \brief Select a block length for all following block commands (R/W)
+//*----------------------------------------------------------------------------
+AT91S_MCIDeviceStatus
+AT91F_MCI_SetBlocklength(unsigned int length)
+{
+	return( AT91F_MCI_SendCommand(AT91C_SET_BLOCKLEN_CMD, length) );
+}
+
+#if 0
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_MMC_GetAllOCR
+//* \brief Asks to all cards to send their operations conditions
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_MMC_GetAllOCR()
+{
+	unsigned int	response =0x0;
+ 	
+ 	while(1) {
+		response = AT91F_MCI_SendCommand(AT91C_MMC_SEND_OP_COND_CMD,
+		    AT91C_MMC_HOST_VOLTAGE_RANGE);
+		if (response != AT91C_CMD_SEND_OK)
+			return AT91C_INIT_ERROR;
+		response = AT91C_BASE_MCI->MCI_RSPR[0];
+		if ( (response & AT91C_CARD_POWER_UP_BUSY) == AT91C_CARD_POWER_UP_BUSY)
+			return(response);	
+	}
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_MMC_GetAllCID
+//* \brief Asks to the MMC on the chosen slot to send its CID
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_MMC_GetAllCID(AT91PS_MciDevice pMCI_Device, unsigned int *response)
+{
+	int Nb_Cards_Found=-1;
+  
+	while (1) {
+	 	if(AT91F_MCI_SendCommand(AT91C_MMC_ALL_SEND_CID_CMD,
+		       AT91C_NO_ARGUMENT) != AT91C_CMD_SEND_OK)
+			return Nb_Cards_Found;
+		else {		
+			Nb_Cards_Found = 0;
+			//* Assignation of the relative address to the MMC CARD
+			pMCI_Device->pMCI_DeviceFeatures[Nb_Cards_Found].Relative_Card_Address = Nb_Cards_Found + AT91C_FIRST_RCA;
+			//* Set the insert flag
+			pMCI_Device->pMCI_DeviceFeatures[Nb_Cards_Found].Card_Inserted = AT91C_MMC_CARD_INSERTED;
+	
+			if (AT91F_MCI_SendCommand(
+				AT91C_MMC_SET_RELATIVE_ADDR_CMD,
+				(Nb_Cards_Found + AT91C_FIRST_RCA) << 16) != AT91C_CMD_SEND_OK)
+				return AT91C_CMD_SEND_ERROR;
+				 
+			//* If no error during assignation address ==> Increment Nb_cards_Found
+			Nb_Cards_Found++ ;
+		}
+	}
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_MMC_Init
+//* \brief Return the MMC initialisation status
+//*----------------------------------------------------------------------------
+AT91S_MCIDeviceStatus
+AT91F_MCI_MMC_Init (AT91PS_MciDevice pMCI_Device)
+{
+	unsigned int	tab_response[4];
+	unsigned int	mult,blocknr;
+	unsigned int 	i,Nb_Cards_Found=0;
+	AT91PS_MciDeviceFeatures f;
+
+	//* Resets all MMC Cards in Idle state
+	AT91F_MCI_SendCommand(AT91C_MMC_GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
+
+	if (AT91F_MCI_MMC_GetAllOCR(pMCI_Device) == AT91C_INIT_ERROR)
+		return AT91C_INIT_ERROR;
+
+	Nb_Cards_Found = AT91F_MCI_MMC_GetAllCID(pMCI_Device,tab_response);
+	if (Nb_Cards_Found == AT91C_CMD_SEND_ERROR)
+		return AT91C_INIT_ERROR;
+
+	//* Set the Mode Register
+	AT91C_BASE_MCI->MCI_MR = AT91C_MCI_MR_PDCMODE;
+	for(i = 0; i < Nb_Cards_Found; i++) {
+	    	f = pMCI_Device->pMCI_DeviceFeatures + i;
+		if (AT91F_MCI_GetCSD(f->Relative_Card_Address, tab_response) !=
+		    AT91C_CMD_SEND_OK) {
+			f->Relative_Card_Address = 0;
+			continue;
+		}
+		f->READ_BL_LEN = ((tab_response[1] >> AT91C_CSD_RD_B_LEN_S) & AT91C_CSD_RD_B_LEN_M);
+		f->WRITE_BL_LEN = ((tab_response[3] >> AT91C_CSD_WBLEN_S) & AT91C_CSD_WBLEN_M );
+		f->Max_Read_DataBlock_Length = 1 << f->READ_BL_LEN;
+		f->Max_Write_DataBlock_Length = 1 << f->WRITE_BL_LEN;
+		f->Sector_Size = 1 + ((tab_response[2] >> AT91C_CSD_v22_SECT_SIZE_S) & AT91C_CSD_v22_SECT_SIZE_M );
+		f->Read_Partial = (tab_response[1] >> AT91C_CSD_RD_B_PAR_S) & AT91C_CSD_RD_B_PAR_M;
+		f->Write_Partial = (tab_response[3] >> AT91C_CSD_WBLOCK_P_S) & AT91C_CSD_WBLOCK_P_M;
+				
+		// None in MMC specification version 2.2
+		f->Erase_Block_Enable = 0;
+		f->Read_Block_Misalignment = (tab_response[1] >> AT91C_CSD_RD_B_MIS_S) & AT91C_CSD_RD_B_MIS_M;
+		f->Write_Block_Misalignment = (tab_response[1] >> AT91C_CSD_WR_B_MIS_S) & AT91C_CSD_WR_B_MIS_M;
+			
+		//// Compute Memory Capacity
+		// compute MULT
+		mult = 1 << ( ((tab_response[2] >> AT91C_CSD_C_SIZE_M_S) & AT91C_CSD_C_SIZE_M_M) + 2 );
+		// compute MSB of C_SIZE
+		blocknr = ((tab_response[1] >> AT91C_CSD_CSIZE_H_S) & AT91C_CSD_CSIZE_H_M) << 2;
+		// compute MULT * (LSB of C-SIZE + MSB already computed + 1) = BLOCKNR
+		blocknr = mult * ( ( blocknr + ( (tab_response[2] >> AT91C_CSD_CSIZE_L_S) & AT91C_CSD_CSIZE_L_M) ) + 1 );
+		f->Memory_Capacity =  f->Max_Read_DataBlock_Length * blocknr;
+		//// End of Compute Memory Capacity
+	}
+	// XXX warner hacked this
+	return AT91C_INIT_OK;
+}
+#endif
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SDCard_GetOCR
+//* \brief Asks to all cards to send their operations conditions
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_SDCard_GetOCR (AT91PS_MciDevice pMCI_Device)
+{
+	unsigned int	response =0x0;
+
+	// The RCA to be used for CMD55 in Idle state shall be the card's default RCA=0x0000.
+	pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address = 0x0;
+ 	
+ 	while( (response & AT91C_CARD_POWER_UP_BUSY) != AT91C_CARD_POWER_UP_BUSY ) {
+		response = AT91F_MCI_SDCard_SendAppCommand(pMCI_Device,
+		    AT91C_SDCARD_APP_OP_COND_CMD,
+		    AT91C_MMC_HOST_VOLTAGE_RANGE);
+		if (response != AT91C_CMD_SEND_OK)
+			return AT91C_INIT_ERROR;
+		response = AT91C_BASE_MCI->MCI_RSPR[0];
+	}
+	
+	return(AT91C_BASE_MCI->MCI_RSPR[0]);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SDCard_GetCID
+//* \brief Asks to the SDCard on the chosen slot to send its CID
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_SDCard_GetCID(unsigned int *response)
+{
+	if (AT91F_MCI_SendCommand(AT91C_ALL_SEND_CID_CMD,
+		AT91C_NO_ARGUMENT) != AT91C_CMD_SEND_OK)
+		return AT91C_CMD_SEND_ERROR;
+	
+	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 AT91C_CMD_SEND_OK;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SDCard_SetBusWidth
+//* \brief  Set bus width for SDCard
+//*----------------------------------------------------------------------------
+static AT91S_MCIDeviceStatus
+AT91F_MCI_SDCard_SetBusWidth(AT91PS_MciDevice pMCI_Device)
+{
+	volatile int	ret_value;
+	char			bus_width;
+
+	do {
+		ret_value =AT91F_MCI_GetStatus(pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address);
+	}
+	while((ret_value > 0) && ((ret_value & AT91C_SR_READY_FOR_DATA) == 0));
+
+	// Select Card
+	AT91F_MCI_SendCommand(AT91C_SEL_DESEL_CARD_CMD,
+	    (pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address)<<16);
+
+	// Set bus width for Sdcard
+	if (pMCI_Device->pMCI_DeviceDesc->SDCard_bus_width == AT91C_MCI_SCDBUS)
+		bus_width = AT91C_BUS_WIDTH_4BITS;
+	else
+		bus_width = AT91C_BUS_WIDTH_1BIT;
+
+	if (AT91F_MCI_SDCard_SendAppCommand(pMCI_Device,AT91C_SDCARD_SET_BUS_WIDTH_CMD,bus_width) != AT91C_CMD_SEND_OK)
+		return AT91C_CMD_SEND_ERROR;
+
+	return AT91C_CMD_SEND_OK;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCI_SDCard_Init
+//* \brief Return the SDCard initialisation status
+//*----------------------------------------------------------------------------
+AT91S_MCIDeviceStatus AT91F_MCI_SDCard_Init (AT91PS_MciDevice pMCI_Device)
+{
+	unsigned int	tab_response[4];
+	unsigned int	mult,blocknr;
+	AT91PS_MciDeviceFeatures f;
+
+	AT91F_MCI_SendCommand(AT91C_GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
+
+	if (AT91F_MCI_SDCard_GetOCR(pMCI_Device) == AT91C_INIT_ERROR)
+		return AT91C_INIT_ERROR;
+
+	f = pMCI_Device->pMCI_DeviceFeatures;
+	if (AT91F_MCI_SDCard_GetCID(tab_response) != AT91C_CMD_SEND_OK)
+		return AT91C_INIT_ERROR;
+	f->Card_Inserted = AT91C_SD_CARD_INSERTED;
+	if (AT91F_MCI_SendCommand(AT91C_SET_RELATIVE_ADDR_CMD, 0) !=
+	    AT91C_CMD_SEND_OK)
+		return AT91C_INIT_ERROR;
+	f->Relative_Card_Address = (AT91C_BASE_MCI->MCI_RSPR[0] >> 16);
+	if (AT91F_MCI_GetCSD(f->Relative_Card_Address,tab_response)
+	    != AT91C_CMD_SEND_OK)
+		return AT91C_INIT_ERROR;
+	f->READ_BL_LEN = 1 << ((tab_response[1] >> AT91C_CSD_RD_B_LEN_S) &
+	    AT91C_CSD_RD_B_LEN_M);
+	f->WRITE_BL_LEN = 1 << ((tab_response[3] >> AT91C_CSD_WBLEN_S) &
+	    AT91C_CSD_WBLEN_M);
+	f->Max_Read_DataBlock_Length = 1 << f->READ_BL_LEN;
+	f->Max_Write_DataBlock_Length = 1 << f->WRITE_BL_LEN;
+	f->Sector_Size = 1 + ((tab_response[2] >> AT91C_CSD_v21_SECT_SIZE_S) &
+	    AT91C_CSD_v21_SECT_SIZE_M);
+	f->Read_Partial = (tab_response[1] >> AT91C_CSD_RD_B_PAR_S) &
+	    AT91C_CSD_RD_B_PAR_M;
+	f->Write_Partial = (tab_response[3] >> AT91C_CSD_WBLOCK_P_S) &
+	    AT91C_CSD_WBLOCK_P_M;
+	f->Erase_Block_Enable = (tab_response[3] >> AT91C_CSD_v21_ER_BLEN_EN_S) &
+	    AT91C_CSD_v21_ER_BLEN_EN_M;
+	f->Read_Block_Misalignment = (tab_response[1] >> AT91C_CSD_RD_B_MIS_S) &
+	    AT91C_CSD_RD_B_MIS_M;
+	f->Write_Block_Misalignment = (tab_response[1] >> AT91C_CSD_WR_B_MIS_S) &
+	    AT91C_CSD_WR_B_MIS_M;
+	//// Compute Memory Capacity
+	// compute MULT
+	mult = 1 << ( ((tab_response[2] >> AT91C_CSD_C_SIZE_M_S) &
+	    AT91C_CSD_C_SIZE_M_M) + 2 );
+	// compute MSB of C_SIZE
+	blocknr = ((tab_response[1] >> AT91C_CSD_CSIZE_H_S) &
+	    AT91C_CSD_CSIZE_H_M) << 2;
+	// compute MULT * (LSB of C-SIZE + MSB already computed + 1) = BLOCKNR
+	blocknr = mult * ((blocknr + ((tab_response[2] >> AT91C_CSD_CSIZE_L_S) &
+	    AT91C_CSD_CSIZE_L_M)) + 1);
+	f->Memory_Capacity =  f->Max_Read_DataBlock_Length * blocknr;
+	//// End of Compute Memory Capacity
+	if (AT91F_MCI_SDCard_SetBusWidth(pMCI_Device) != AT91C_CMD_SEND_OK)
+		return AT91C_INIT_ERROR;
+	if (AT91F_MCI_SetBlocklength(f->Max_Read_DataBlock_Length) !=
+	    AT91C_CMD_SEND_OK)
+		return AT91C_INIT_ERROR;
+	return AT91C_INIT_OK;
+}
diff --git a/sys/boot/arm/at91/libat91/mci_device.h b/sys/boot/arm/at91/libat91/mci_device.h
new file mode 100644
index 0000000..197004b
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/mci_device.h
@@ -0,0 +1,417 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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$
+ */
+
+//*---------------------------------------------------------------------------
+//*         ATMEL Microcontroller Software Support  -  ROUSSET  -
+//*---------------------------------------------------------------------------
+//* The software is delivered "AS IS" without warranty or condition of any
+//* kind, either express, implied or statutory. This includes without
+//* limitation any warranty or condition with respect to merchantability or
+//* fitness for any particular purpose, or against the infringements of
+//* intellectual property rights of others.
+//*---------------------------------------------------------------------------
+//* File Name           : AT91C_MCI_Device.h
+//* Object              : Data Flash Atmel Description File
+//* Translator          :
+//*
+//* 1.0 26/11/02 FB		: Creation
+//*---------------------------------------------------------------------------
+
+#ifndef __MCI_Device_h
+#define __MCI_Device_h
+
+
+typedef unsigned int AT91S_MCIDeviceStatus;
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define AT91C_CARD_REMOVED			0
+#define AT91C_MMC_CARD_INSERTED		1
+#define AT91C_SD_CARD_INSERTED		2
+
+#define AT91C_NO_ARGUMENT			0x0
+
+#define AT91C_FIRST_RCA				0xCAFE
+#define AT91C_MAX_MCI_CARDS			10
+
+#define AT91C_BUS_WIDTH_1BIT		0x00
+#define AT91C_BUS_WIDTH_4BITS		0x02
+
+/* Driver State */
+#define AT91C_MCI_IDLE       		0x0
+#define AT91C_MCI_TIMEOUT_ERROR		0x1
+#define AT91C_MCI_RX_SINGLE_BLOCK	0x2
+#define AT91C_MCI_RX_MULTIPLE_BLOCK	0x3
+#define AT91C_MCI_RX_STREAM			0x4
+#define AT91C_MCI_TX_SINGLE_BLOCK	0x5
+#define AT91C_MCI_TX_MULTIPLE_BLOCK	0x6
+#define AT91C_MCI_TX_STREAM 		0x7
+
+/* TimeOut */
+#define AT91C_TIMEOUT_CMDRDY		30
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+// 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 int 	Max_Read_DataBlock_Length;	// 2^(READ_BL_LEN) in CSD 
+	unsigned int 	Max_Write_DataBlock_Length;	// 2^(WRITE_BL_LEN) in CSD
+	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;
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+// MCI_CMD Register Value 
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+#define AT91C_POWER_ON_INIT						(0	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_INIT | AT91C_MCI_OPDCMD)
+
+/////////////////////////////////////////////////////////////////	
+// Class 0 & 1 commands: Basic commands and Read Stream commands
+/////////////////////////////////////////////////////////////////
+
+#define AT91C_GO_IDLE_STATE_CMD					(0 	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE )
+#define AT91C_MMC_GO_IDLE_STATE_CMD				(0 	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE  | AT91C_MCI_OPDCMD)
+#define AT91C_MMC_SEND_OP_COND_CMD				(1	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48 | AT91C_MCI_OPDCMD)
+#define AT91C_ALL_SEND_CID_CMD					(2	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_136 )
+#define AT91C_MMC_ALL_SEND_CID_CMD				(2	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_136 | AT91C_MCI_OPDCMD)
+#define AT91C_SET_RELATIVE_ADDR_CMD				(3	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48		| AT91C_MCI_MAXLAT )
+#define AT91C_MMC_SET_RELATIVE_ADDR_CMD			(3	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48		| AT91C_MCI_MAXLAT | AT91C_MCI_OPDCMD)
+
+#define AT91C_SET_DSR_CMD						(4	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_NO		| AT91C_MCI_MAXLAT )	// no tested
+
+#define AT91C_SEL_DESEL_CARD_CMD				(7	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48  		| AT91C_MCI_MAXLAT )
+#define AT91C_SEND_CSD_CMD						(9	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_136 		| AT91C_MCI_MAXLAT )
+#define AT91C_SEND_CID_CMD						(10	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_136 		| AT91C_MCI_MAXLAT )
+#define AT91C_MMC_READ_DAT_UNTIL_STOP_CMD		(11	| AT91C_MCI_TRTYP_STREAM| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRDIR	| AT91C_MCI_TRCMD_START | AT91C_MCI_MAXLAT )
+
+#define AT91C_STOP_TRANSMISSION_CMD				(12	| AT91C_MCI_TRCMD_STOP 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48 		| AT91C_MCI_MAXLAT )
+#define AT91C_STOP_TRANSMISSION_SYNC_CMD		(12	| AT91C_MCI_TRCMD_STOP 	| AT91C_MCI_SPCMD_SYNC	| AT91C_MCI_RSPTYP_48 		| AT91C_MCI_MAXLAT )
+#define AT91C_SEND_STATUS_CMD					(13	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48 		| AT91C_MCI_MAXLAT )
+#define AT91C_GO_INACTIVE_STATE_CMD				(15	| AT91C_MCI_RSPTYP_NO )
+
+//*------------------------------------------------
+//* Class 2 commands: Block oriented Read commands
+//*------------------------------------------------
+
+#define AT91C_SET_BLOCKLEN_CMD					(16 | AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48		| AT91C_MCI_MAXLAT )
+#define AT91C_READ_SINGLE_BLOCK_CMD				(17 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48 	| AT91C_MCI_TRCMD_START	| AT91C_MCI_TRTYP_BLOCK	| AT91C_MCI_TRDIR	| AT91C_MCI_MAXLAT)
+#define AT91C_READ_MULTIPLE_BLOCK_CMD			(18 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48 	| AT91C_MCI_TRCMD_START	| AT91C_MCI_TRTYP_MULTIPLE	| AT91C_MCI_TRDIR	| AT91C_MCI_MAXLAT)
+
+//*--------------------------------------------
+//* Class 3 commands: Sequential write commands
+//*--------------------------------------------
+
+#define AT91C_MMC_WRITE_DAT_UNTIL_STOP_CMD		(20 | AT91C_MCI_TRTYP_STREAM| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48 & ~(AT91C_MCI_TRDIR) | AT91C_MCI_TRCMD_START | AT91C_MCI_MAXLAT )	// MMC
+
+//*------------------------------------------------
+//* Class 4 commands: Block oriented write commands
+//*------------------------------------------------
+	
+#define AT91C_WRITE_BLOCK_CMD					(24 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_START	| (AT91C_MCI_TRTYP_BLOCK 	&  ~(AT91C_MCI_TRDIR))	| AT91C_MCI_MAXLAT)
+#define AT91C_WRITE_MULTIPLE_BLOCK_CMD			(25 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_START	| (AT91C_MCI_TRTYP_MULTIPLE	&  ~(AT91C_MCI_TRDIR)) 	| AT91C_MCI_MAXLAT)
+#define AT91C_PROGRAM_CSD_CMD					(27 | AT91C_MCI_RSPTYP_48 )
+
+
+//*----------------------------------------
+//* Class 6 commands: Group Write protect
+//*----------------------------------------
+
+#define AT91C_SET_WRITE_PROT_CMD				(28	| AT91C_MCI_RSPTYP_48 )
+#define AT91C_CLR_WRITE_PROT_CMD				(29	| AT91C_MCI_RSPTYP_48 )
+#define AT91C_SEND_WRITE_PROT_CMD				(30	| AT91C_MCI_RSPTYP_48 )
+
+
+//*----------------------------------------
+//* Class 5 commands: Erase commands
+//*----------------------------------------
+
+#define AT91C_TAG_SECTOR_START_CMD				(32 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_TAG_SECTOR_END_CMD  				(33 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_MMC_UNTAG_SECTOR_CMD				(34 | AT91C_MCI_RSPTYP_48 )
+#define AT91C_MMC_TAG_ERASE_GROUP_START_CMD		(35 | AT91C_MCI_RSPTYP_48 )
+#define AT91C_MMC_TAG_ERASE_GROUP_END_CMD		(36 | AT91C_MCI_RSPTYP_48 )
+#define AT91C_MMC_UNTAG_ERASE_GROUP_CMD			(37 | AT91C_MCI_RSPTYP_48 )
+#define AT91C_ERASE_CMD							(38 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT )
+
+//*----------------------------------------
+//* Class 7 commands: Lock commands
+//*----------------------------------------
+
+#define AT91C_LOCK_UNLOCK						(42 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)	// no tested
+
+//*-----------------------------------------------
+// Class 8 commands: Application specific commands
+//*-----------------------------------------------
+
+#define AT91C_APP_CMD							(55 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO | AT91C_MCI_MAXLAT)
+#define AT91C_GEN_CMD							(56 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO | AT91C_MCI_MAXLAT)	// no tested
+
+#define AT91C_SDCARD_SET_BUS_WIDTH_CMD			(6 	| AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_SDCARD_STATUS_CMD					(13 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_SDCARD_SEND_NUM_WR_BLOCKS_CMD		(22 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_SDCARD_SET_WR_BLK_ERASE_COUNT_CMD	(23 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_SDCARD_APP_OP_COND_CMD			(41 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO )
+#define AT91C_SDCARD_SET_CLR_CARD_DETECT_CMD	(42 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+#define AT91C_SDCARD_SEND_SCR_CMD				(51 | AT91C_MCI_SPCMD_NONE	| AT91C_MCI_RSPTYP_48	| AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+
+#define AT91C_SDCARD_APP_ALL_CMD				(AT91C_SDCARD_SET_BUS_WIDTH_CMD +\
+												AT91C_SDCARD_STATUS_CMD +\
+												AT91C_SDCARD_SEND_NUM_WR_BLOCKS_CMD +\
+												AT91C_SDCARD_SET_WR_BLK_ERASE_COUNT_CMD +\
+												AT91C_SDCARD_APP_OP_COND_CMD +\
+												AT91C_SDCARD_SET_CLR_CARD_DETECT_CMD +\
+												AT91C_SDCARD_SEND_SCR_CMD)
+
+//*----------------------------------------
+//* Class 9 commands: IO Mode commands
+//*----------------------------------------
+
+#define AT91C_MMC_FAST_IO_CMD					(39 | AT91C_MCI_SPCMD_NONE | AT91C_MCI_RSPTYP_48 | AT91C_MCI_MAXLAT)
+#define AT91C_MMC_GO_IRQ_STATE_CMD				(40 | AT91C_MCI_SPCMD_NONE | AT91C_MCI_RSPTYP_48 | AT91C_MCI_TRCMD_NO	| AT91C_MCI_MAXLAT)
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+// Functions returnals
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+#define AT91C_CMD_SEND_OK					0		// Command ok
+#define AT91C_CMD_SEND_ERROR				-1		// Command failed
+#define AT91C_INIT_OK						2		// Init Successfull
+#define AT91C_INIT_ERROR					3		// Init Failed
+#define AT91C_READ_OK						4		// Read Successfull
+#define AT91C_READ_ERROR					5		// Read Failed
+#define AT91C_WRITE_OK						6		// Write Successfull
+#define AT91C_WRITE_ERROR					7		// Write Failed
+#define AT91C_ERASE_OK						8		// Erase Successfull
+#define AT91C_ERASE_ERROR					9		// Erase Failed
+#define AT91C_CARD_SELECTED_OK				10		// Card Selection Successfull
+#define AT91C_CARD_SELECTED_ERROR			11		// Card Selection Failed
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+// MCI_SR Errors
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+#define 	AT91C_MCI_SR_ERROR		(AT91C_MCI_UNRE |\
+									 AT91C_MCI_OVRE |\
+									 AT91C_MCI_DTOE |\
+									 AT91C_MCI_DCRCE |\
+									 AT91C_MCI_RTOE |\
+									 AT91C_MCI_RENDE |\
+									 AT91C_MCI_RCRCE |\
+									 AT91C_MCI_RDIRE |\
+									 AT91C_MCI_RINDE)
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// OCR Register
+////////////////////////////////////////////////////////////////////////////////////////////////////
+#define AT91C_VDD_16_17					(1 << 4)
+#define AT91C_VDD_17_18					(1 << 5)
+#define AT91C_VDD_18_19					(1 << 6)
+#define AT91C_VDD_19_20					(1 << 7)
+#define AT91C_VDD_20_21					(1 << 8)
+#define AT91C_VDD_21_22					(1 << 9)
+#define AT91C_VDD_22_23					(1 << 10)
+#define AT91C_VDD_23_24					(1 << 11)
+#define AT91C_VDD_24_25					(1 << 12)
+#define AT91C_VDD_25_26					(1 << 13)
+#define AT91C_VDD_26_27					(1 << 14)
+#define AT91C_VDD_27_28					(1 << 15)
+#define AT91C_VDD_28_29					(1 << 16)
+#define AT91C_VDD_29_30					(1 << 17)
+#define AT91C_VDD_30_31					(1 << 18)
+#define AT91C_VDD_31_32					(1 << 19)
+#define AT91C_VDD_32_33					(1 << 20)
+#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		(1 << 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)
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// CURRENT_STATE & READY_FOR_DATA in SDCard Status Register definition (response type R1)
+////////////////////////////////////////////////////////////////////////////////////////////////////
+#define AT91C_SR_READY_FOR_DATA				(1 << 8)	// corresponds to buffer empty signalling on the bus
+#define AT91C_SR_IDLE						(0 << 9)
+#define AT91C_SR_READY						(1 << 9)
+#define AT91C_SR_IDENT						(2 << 9)
+#define AT91C_SR_STBY						(3 << 9)
+#define AT91C_SR_TRAN						(4 << 9)
+#define AT91C_SR_DATA						(5 << 9)
+#define AT91C_SR_RCV						(6 << 9)
+#define AT91C_SR_PRG						(7 << 9)
+#define AT91C_SR_DIS						(8 << 9)
+
+#define AT91C_SR_CARD_SELECTED				(AT91C_SR_READY_FOR_DATA + AT91C_SR_TRAN)
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+// MMC CSD register header File					
+// AT91C_CSD_xxx_S	for shift value
+// AT91C_CSD_xxx_M	for mask  value
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// First Response INT <=> CSD[3] : bits 0 to 31
+#define	AT91C_CSD_BIT0_S			0		// [0:0]			
+#define	AT91C_CSD_BIT0_M			0x01				
+#define	AT91C_CSD_CRC_S				1		// [7:1]
+#define	AT91C_CSD_CRC_M				0x7F
+#define	AT91C_CSD_MMC_ECC_S			8		// [9:8]		reserved for MMC compatibility
+#define	AT91C_CSD_MMC_ECC_M			0x03
+#define	AT91C_CSD_FILE_FMT_S		10		// [11:10]
+#define	AT91C_CSD_FILE_FMT_M		0x03
+#define	AT91C_CSD_TMP_WP_S			12		// [12:12]
+#define	AT91C_CSD_TMP_WP_M			0x01
+#define	AT91C_CSD_PERM_WP_S 		13		// [13:13]
+#define	AT91C_CSD_PERM_WP_M 		0x01
+#define	AT91C_CSD_COPY_S	 		14		// [14:14]
+#define	AT91C_CSD_COPY_M 			0x01
+#define	AT91C_CSD_FILE_FMT_GRP_S	15		// [15:15]
+#define	AT91C_CSD_FILE_FMT_GRP_M	0x01
+//	reserved						16		// [20:16]
+//	reserved						0x1F
+#define	AT91C_CSD_WBLOCK_P_S 		21		// [21:21]
+#define	AT91C_CSD_WBLOCK_P_M 		0x01
+#define	AT91C_CSD_WBLEN_S 			22		// [25:22]
+#define	AT91C_CSD_WBLEN_M 			0x0F
+#define	AT91C_CSD_R2W_F_S 			26		// [28:26]
+#define	AT91C_CSD_R2W_F_M 			0x07
+#define	AT91C_CSD_MMC_DEF_ECC_S		29		// [30:29]		reserved for MMC compatibility
+#define	AT91C_CSD_MMC_DEF_ECC_M		0x03
+#define	AT91C_CSD_WP_GRP_EN_S		31		// [31:31]
+#define	AT91C_CSD_WP_GRP_EN_M 		0x01
+
+// Seconde Response INT <=> CSD[2] : bits 32 to 63
+#define	AT91C_CSD_v21_WP_GRP_SIZE_S	0		// [38:32]				
+#define	AT91C_CSD_v21_WP_GRP_SIZE_M	0x7F				
+#define	AT91C_CSD_v21_SECT_SIZE_S	7		// [45:39]
+#define	AT91C_CSD_v21_SECT_SIZE_M	0x7F
+#define	AT91C_CSD_v21_ER_BLEN_EN_S	14		// [46:46]
+#define	AT91C_CSD_v21_ER_BLEN_EN_M	0x01
+
+#define	AT91C_CSD_v22_WP_GRP_SIZE_S	0		// [36:32]				
+#define	AT91C_CSD_v22_WP_GRP_SIZE_M	0x1F				
+#define	AT91C_CSD_v22_ER_GRP_SIZE_S	5		// [41:37]
+#define	AT91C_CSD_v22_ER_GRP_SIZE_M	0x1F
+#define	AT91C_CSD_v22_SECT_SIZE_S	10		// [46:42]
+#define	AT91C_CSD_v22_SECT_SIZE_M	0x1F
+
+#define	AT91C_CSD_C_SIZE_M_S		15		// [49:47]
+#define	AT91C_CSD_C_SIZE_M_M		0x07
+#define	AT91C_CSD_VDD_WMAX_S 		18		// [52:50]
+#define	AT91C_CSD_VDD_WMAX_M 		0x07
+#define	AT91C_CSD_VDD_WMIN_S	 	21		// [55:53]
+#define	AT91C_CSD_VDD_WMIN_M 		0x07
+#define	AT91C_CSD_RCUR_MAX_S 		24		// [58:56]
+#define	AT91C_CSD_RCUR_MAX_M 		0x07
+#define	AT91C_CSD_RCUR_MIN_S 		27		// [61:59]
+#define	AT91C_CSD_RCUR_MIN_M 		0x07
+#define	AT91C_CSD_CSIZE_L_S 		30		// [63:62] <=> 2 LSB of CSIZE
+#define	AT91C_CSD_CSIZE_L_M 		0x03
+
+// Third Response INT <=> CSD[1] : bits 64 to 95
+#define	AT91C_CSD_CSIZE_H_S 		0		// [73:64]	<=> 10 MSB of CSIZE
+#define	AT91C_CSD_CSIZE_H_M 		0x03FF
+// reserved							10		// [75:74]
+// reserved							0x03		
+#define	AT91C_CSD_DSR_I_S 			12		// [76:76]
+#define	AT91C_CSD_DSR_I_M 			0x01
+#define	AT91C_CSD_RD_B_MIS_S 		13		// [77:77]
+#define	AT91C_CSD_RD_B_MIS_M 		0x01
+#define	AT91C_CSD_WR_B_MIS_S 		14		// [78:78]
+#define	AT91C_CSD_WR_B_MIS_M 		0x01
+#define	AT91C_CSD_RD_B_PAR_S 		15		// [79:79]
+#define	AT91C_CSD_RD_B_PAR_M 		0x01
+#define	AT91C_CSD_RD_B_LEN_S 		16		// [83:80]
+#define	AT91C_CSD_RD_B_LEN_M 		0x0F
+#define	AT91C_CSD_CCC_S	 			20		// [95:84]
+#define	AT91C_CSD_CCC_M 			0x0FFF
+
+// Fourth Response INT <=> CSD[0] : bits 96 to 127
+#define	AT91C_CSD_TRANS_SPEED_S 	0		// [103:96]
+#define	AT91C_CSD_TRANS_SPEED_M 	0xFF
+#define	AT91C_CSD_NSAC_S 			8		// [111:104]
+#define	AT91C_CSD_NSAC_M 			0xFF
+#define	AT91C_CSD_TAAC_S 			16		// [119:112]
+#define	AT91C_CSD_TAAC_M 			0xFF
+//	reserved						24		// [121:120]
+//	reserved						0x03
+#define	AT91C_CSD_MMC_SPEC_VERS_S	26		// [125:122]	reserved for MMC compatibility
+#define	AT91C_CSD_MMC_SPEC_VERS_M	0x0F
+#define	AT91C_CSD_STRUCT_S			30		// [127:126]
+#define	AT91C_CSD_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/reset.c b/sys/boot/arm/at91/libat91/reset.c
new file mode 100644
index 0000000..b1f083a
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/reset.c
@@ -0,0 +1,57 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#include "at91rm9200.h"
+#include "lib.h"
+
+/*
+ * void reset()
+ * 
+ * Forces a reset of the system.  Uses watchdog timer of '1', which
+ * corresponds to 128 / SLCK seconds (SLCK is 32,768 Hz, so 128/32768 is
+ * 1 / 256 ~= 5.4ms
+ */
+void
+reset(void)
+{
+	// The following should effect a reset.
+	AT91C_BASE_ST->ST_WDMR = 1 | AT91C_ST_RSTEN;
+	AT91C_BASE_ST->ST_CR = AT91C_ST_WDRST;
+}
+
+/*
+ * void start_wdog()
+ *
+ * Starts a watchdog timer.  We force the boot process to get to the point
+ * it can kick the watch dog part of the ST part for the OS's driver.
+ */
+void
+start_wdog(int n)
+{
+	// The following should effect a reset after N seconds.
+	AT91C_BASE_ST->ST_WDMR = (n * (32768 / 128)) | AT91C_ST_RSTEN;
+	AT91C_BASE_ST->ST_CR = AT91C_ST_WDRST;
+}
diff --git a/sys/boot/arm/at91/libat91/sd-card.c b/sys/boot/arm/at91/libat91/sd-card.c
new file mode 100644
index 0000000..ae1f01f
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/sd-card.c
@@ -0,0 +1,335 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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$
+ */
+
+//*----------------------------------------------------------------------------
+//*         ATMEL Microcontroller Software Support  -  ROUSSET  -
+//*----------------------------------------------------------------------------
+//* The software is delivered "AS IS" without warranty or condition of any
+//* kind, either express, implied or statutory. This includes without
+//* limitation any warranty or condition with respect to merchantability or
+//* fitness for any particular purpose, or against the infringements of
+//* intellectual property rights of others.
+//*----------------------------------------------------------------------------
+//* File Name           : main.c
+//* Object              : main application written in C
+//* Creation            : FB   21/11/2002
+//*
+//*----------------------------------------------------------------------------
+#include "at91rm9200.h"
+#include "lib_AT91RM9200.h"
+#include "mci_device.h"
+#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
+
+//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];
+
+/******************************************************************************
+**Error return codes
+******************************************************************************/
+#define MCI_UNSUPP_SIZE_ERROR		5
+#define MCI_UNSUPP_OFFSET_ERROR 6
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_MCIDeviceWaitReady
+//* \brief Wait for MCI Device ready
+//*----------------------------------------------------------------------------
+static void
+AT91F_MCIDeviceWaitReady(unsigned int timeout)
+{
+	volatile int status;
+	
+	do
+	{
+		status = AT91C_BASE_MCI->MCI_SR;
+		timeout--;
+	}
+	while( !(status & AT91C_MCI_NOTBUSY)  && (timeout>0) );	
+
+#if IMP_DEBUG
+	if (timeout == 0)
+	    printf("Timeout, status is 0x%x\r\n", status);
+#endif
+	
+	//TODO: Make interrupts work!
+	AT91F_MCI_Handler();
+}
+
+#if 0
+int
+MCI_write (unsigned dest, char* source, unsigned length)
+{
+	unsigned sectorLength = MCI_Device.pMCI_DeviceFeatures->Max_Read_DataBlock_Lenfgth;
+	unsigned offset = dest % sectorLength;
+	AT91S_MCIDeviceStatus status;
+	int sizeToWrite;
+
+#if IMP_DEBUG
+	printf("\r\n");
+#endif
+
+	//See if we are requested to write partial sectors, and have the capability to do so
+	if ((length % sectorLength) && !(MCI_Device_Features.Write_Partial))
+		//Return error if appropriat
+		return MCI_UNSUPP_SIZE_ERROR;
+
+	//See if we are requested to write to anywhere but a sectors' boundary
+	//and have the capability to do so
+	if ((offset) && !(MCI_Device_Features.Write_Partial))
+		//Return error if appropriat
+		return MCI_UNSUPP_OFFSET_ERROR;
+
+	//If the address we're trying to write != sector boundary
+	if (offset)
+	{
+		//* Wait MCI Device Ready
+		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+
+		//Calculate the nr of bytes to write
+		sizeToWrite = sectorLength - offset;
+		//Do the writing
+		status = AT91F_MCI_WriteBlock(&MCI_Device, dest, (unsigned int*)source, sizeToWrite);
+		//TODO:Status checking
+
+		//Update counters & pointers
+		length -= sizeToWrite;
+		dest += sizeToWrite;
+		source += sizeToWrite;
+	}
+
+	//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;
+
+		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		//Do the writing
+		status = AT91F_MCI_WriteBlock(&MCI_Device, dest, (unsigned int*)source, sizeToWrite);
+		//TODO:Status checking
+
+		//Update counters & pointers
+		length -= sizeToWrite;
+		dest += sizeToWrite;
+		source += sizeToWrite;
+	}
+
+	return 0;
+}
+#endif
+
+inline static unsigned int
+swap(unsigned int a)
+{
+    return (((a & 0xff) << 24) | ((a & 0xff00) << 8) | ((a & 0xff0000) >> 8)
+      | ((a & 0xff000000) >> 24));
+}
+
+int
+MCI_read(char* dest, unsigned source, unsigned length)
+{
+	unsigned sectorLength = MCI_Device.pMCI_DeviceFeatures->Max_Read_DataBlock_Length;
+	unsigned log2sl = MCI_Device.pMCI_DeviceFeatures->READ_BL_LEN;
+	unsigned slmask = ((1 << log2sl) - 1);
+//	unsigned sector = (unsigned)source >> log2sl;
+	unsigned offset = (unsigned)source & slmask;
+	AT91S_MCIDeviceStatus status;
+	int sizeToRead;
+	unsigned int *walker;
+
+#if IMP_DEBUG
+	printf("Reading 0x%x bytes into ARM Addr 0x%x from card offset 0x%x\r\n",
+	  length, dest, source);
+#endif
+	
+
+	//See if we are requested to read partial sectors, and have the capability to do so
+	if ((length & slmask) && !(MCI_Device_Features.Read_Partial))
+		//Return error if appropriat
+		return MCI_UNSUPP_SIZE_ERROR;
+
+	//See if we are requested to read from anywhere but a sectors' boundary
+	//and have the capability to do so
+	if ((offset) && !(MCI_Device_Features.Read_Partial))
+		//Return error if appropriat
+		return MCI_UNSUPP_OFFSET_ERROR;
+
+	//If the address we're trying to read != sector boundary
+	if (offset) {
+		//* Wait MCI Device Ready
+		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+
+		//Calculate the nr of bytes to read
+		sizeToRead = sectorLength - offset;
+		//Do the writing
+		status = AT91F_MCI_ReadBlock(&MCI_Device, source, (unsigned int*)dest, sizeToRead);
+		//TODO:Status checking
+		if (status != AT91C_READ_OK) {
+#if IMP_DEBUG
+		    printf("STATUS is 0x%x\r\n", status);
+#endif
+		    return -1;
+		}
+		
+		//* Wait MCI Device Ready
+		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		// Fix erratum in MCI part
+		for (walker = (unsigned int *)dest;
+		     walker < (unsigned int *)(dest + sizeToRead); walker++)
+		    *walker = swap(*walker);
+
+		//Update counters & pointers
+		length -= sizeToRead;
+		dest += sizeToRead;
+		source += sizeToRead;
+	}
+
+	//As long as there is data to read
+	while (length)
+	{
+		//See if we've got at least a sector to read
+		if (length > sectorLength)
+			sizeToRead = sectorLength;
+		//Else just write the remainder
+		else
+			sizeToRead = length;
+
+		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+		//Do the writing
+		status = AT91F_MCI_ReadBlock(&MCI_Device, source, (unsigned int*)dest, sizeToRead);
+#if IMP_DEBUG
+		printf("Reading 0x%x Addr 0x%x card 0x%x\r\n",
+		  sizeToRead, dest, source);
+#endif
+
+		//TODO:Status checking
+		if (status != AT91C_READ_OK) {
+#if IMP_DEBUG
+		        printf("STATUS is 0x%x\r\n", status);
+#endif
+			return -1;
+		}
+
+		//* Wait MCI Device Ready
+		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
+
+		// Fix erratum in MCI part
+		for (walker = (unsigned int *)dest;
+		     walker < (unsigned int *)(dest + sizeToRead); walker++)
+		    *walker = swap(*walker);
+
+		//Update counters & pointers
+		length -= sizeToRead;
+		dest += sizeToRead;
+		source += sizeToRead;
+	}
+
+	return 0;
+}
+
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_CfgDevice
+//* \brief This function is used to initialise MMC or SDCard Features
+//*----------------------------------------------------------------------------
+static void AT91F_CfgDevice(void)
+{
+	// Init Device Structure
+
+	MCI_Device_Features.Relative_Card_Address 	= 0;
+	MCI_Device_Features.Card_Inserted 		= AT91C_SD_CARD_INSERTED;
+	MCI_Device_Features.Max_Read_DataBlock_Length	= 0;
+	MCI_Device_Features.Max_Write_DataBlock_Length 	= 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;
+
+}
+
+static void AT91F_MCI_Handler(void)
+{
+	int status;
+
+//	status = ( AT91C_BASE_MCI->MCI_SR & AT91C_BASE_MCI->MCI_IMR );
+	status = AT91C_BASE_MCI->MCI_SR;
+
+	AT91F_MCI_Device_Handler(&MCI_Device,status);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    main
+//* \brief main function
+//*----------------------------------------------------------------------------
+int
+sdcard_init(void)
+{
+///////////////////////////////////////////////////////////////////////////////
+//  MCI Init : common to MMC and SDCard
+///////////////////////////////////////////////////////////////////////////////
+
+	//initInts();
+
+	// 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();
+
+	AT91F_MCI_Configure(AT91C_BASE_MCI,
+	    AT91C_MCI_DTOR_1MEGA_CYCLES,
+	    AT91C_MCI_MR_PDCMODE,			// 15MHz for MCK = 60MHz (CLKDIV = 1)
+	    AT91C_MCI_SDCARD_4BITS_SLOTA);
+	
+	if (AT91F_MCI_SDCard_Init(&MCI_Device) != AT91C_INIT_OK)
+		return 0;
+	return 1;
+}
diff --git a/sys/boot/arm/at91/libat91/sd-card.h b/sys/boot/arm/at91/libat91/sd-card.h
new file mode 100644
index 0000000..0a3ce69
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/sd-card.h
@@ -0,0 +1,38 @@
+/*-
+ * Copyright (c) 2006 M. Warner Losh.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (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$
+ */
+
+#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);
+int sdcard_init(void);
+
+#endif
+