MFp4:

Massive update.  The highlights:
	o dramatically cut memory usage by writing better, less intertwingled
	  code.
	o implement booting off mmc/sd cards (sd only tested one at the moment)
	o start to split out board specific stuff for boot2.

25 files changed, 1165 insertions, 1058 deletions

diff --git a/sys/boot/arm/at91/boot2/Makefile b/sys/boot/arm/at91/boot2/Makefile
new file mode 100644
index 0000000..94a9af5
--- /dev/null
+++ b/sys/boot/arm/at91/boot2/Makefile
@@ -0,0 +1,19 @@
+#  $FreeBSD$
+
+.PATH: ${.CURDIR}/../bootspi
+
+P=boot2
+FILES=${P}
+SRCS=arm_init.S boot2.c kb920x_board.c ee.c
+NO_MAN=
+LDFLAGS=-e 0 -T ${.CURDIR}/../linker.cfg
+OBJS+=  ${SRCS:N*.h:R:S/$/.o/g}
+
+.include <bsd.prog.mk>
+
+CFLAGS+= \
+	-I${.CURDIR}/../bootspi \
+	-I${.CURDIR}/../../../common \
+	-I${.CURDIR}/../../../.. \
+	-D_KERNEL \
+	-DUFS1_ONLY
diff --git a/sys/boot/arm/at91/boot2/boot2.c b/sys/boot/arm/at91/boot2/boot2.c
new file mode 100644
index 0000000..0882ca5
--- /dev/null
+++ b/sys/boot/arm/at91/boot2/boot2.c
@@ -0,0 +1,318 @@
+/*-
+ * Copyright (c) 1998 Robert Nordier
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms are freely
+ * permitted provided that the above copyright notice and this
+ * paragraph and the following disclaimer are duplicated in all
+ * such forms.
+ *
+ * This software is provided "AS IS" and without any express or
+ * implied warranties, including, without limitation, the implied
+ * warranties of merchantability and fitness for a particular
+ * purpose.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/disklabel.h>
+#include <sys/diskmbr.h>
+#include <sys/dirent.h>
+#include <sys/reboot.h>
+
+#include <machine/elf.h>
+
+#include <stdarg.h>
+
+#include "emac.h"
+#include "lib.h"
+#include "sd-card.h"
+#include "ee.h"
+
+#define RBX_ASKNAME	0x0	/* -a */
+#define RBX_SINGLE	0x1	/* -s */
+/* 0x2 is reserved for log2(RB_NOSYNC). */
+/* 0x3 is reserved for log2(RB_HALT). */
+/* 0x4 is reserved for log2(RB_INITNAME). */
+#define RBX_DFLTROOT	0x5	/* -r */
+/* #define RBX_KDB 	0x6	   -d */
+/* 0x7 is reserved for log2(RB_RDONLY). */
+/* 0x8 is reserved for log2(RB_DUMP). */
+/* 0x9 is reserved for log2(RB_MINIROOT). */
+#define RBX_CONFIG	0xa	/* -c */
+#define RBX_VERBOSE	0xb	/* -v */
+/* #define RBX_SERIAL	0xc	   -h */
+/* #define RBX_CDROM	0xd	   -C */
+/* 0xe is reserved for log2(RB_POWEROFF). */
+#define RBX_GDB 	0xf	/* -g */
+/* #define RBX_MUTE	0x10	   -m */
+/* 0x11 is reserved for log2(RB_SELFTEST). */
+/* 0x12 is reserved for boot programs. */
+/* 0x13 is reserved for boot programs. */
+/* #define RBX_PAUSE	0x14	   -p */
+/* #define RBX_QUIET	0x15	   -q */
+/* #define RBX_NOINTR	0x1c	   -n */
+/* 0x1d is reserved for log2(RB_MULTIPLE) and is just misnamed here. */
+/* #define RBX_DUAL	0x1d	   -D */
+/* 0x1f is reserved for log2(RB_BOOTINFO). */
+
+/* pass: -a, -s, -r, -v, -g */
+#define RBX_MASK	(OPT_SET(RBX_ASKNAME) | OPT_SET(RBX_SINGLE) | \
+			OPT_SET(RBX_DFLTROOT) | \
+			OPT_SET(RBX_VERBOSE) | \
+			OPT_SET(RBX_GDB))
+
+#define PATH_CONFIG	"/boot.config"
+//#define PATH_KERNEL	"/boot/kernel/kernel"
+#define PATH_KERNEL	"/kernel.gz.tramp"
+
+#define NOPT		5
+
+#define OPT_SET(opt)	(1 << (opt))
+#define OPT_CHECK(opt)	((opts) & OPT_SET(opt))
+
+extern uint32_t _end;
+
+static const char optstr[NOPT] = "agrsv";
+static const unsigned char flags[NOPT] = {
+    RBX_ASKNAME,
+    RBX_GDB,
+    RBX_DFLTROOT,
+    RBX_SINGLE,
+    RBX_VERBOSE
+};
+
+unsigned char mac[6] = { 0x42, 0x53, 0x44, 0, 0, 1 };
+
+unsigned dsk_start;
+static char cmd[512];
+static char kname[1024];
+static uint32_t opts;
+static int dsk_meta;
+
+static void load(void);
+static int parse(void);
+static int xfsread(ino_t, void *, size_t);
+static int dskread(void *, unsigned, unsigned);
+static int drvread(void *, unsigned, unsigned);
+
+#include "ufsread.c"
+
+static inline int
+xfsread(ino_t inode, void *buf, size_t nbyte)
+{
+    if ((size_t)fsread(inode, buf, nbyte) != nbyte)
+	return -1;
+    return 0;
+}
+
+static inline void
+getstr(int c)
+{
+    char *s;
+
+    s = cmd;
+    if (c)
+	*s++ = c;
+    for (;;) {
+	c = getc(10000);
+
+	switch (c = getc(10000)) {
+	case 0:
+	    break;
+	case '\177':
+	case '\b':
+	    if (s > cmd) {
+		s--;
+		printf("\b \b");
+	    }
+	    break;
+	case '\n':
+	case '\r':
+	    *s = 0;
+	    return;
+	default:
+	    if (s - cmd < sizeof(cmd) - 1)
+		*s++ = c;
+	    xputchar(c);
+	}
+    }
+}
+
+// Each board has to provide one of these.
+void board_init(void);
+
+int
+main(void)
+{
+    int autoboot, c = 0;
+    ino_t ino;
+
+    board_init();
+    EMAC_Init();
+    sdcard_init();
+    EMAC_SetMACAddress(mac);
+
+    dmadat = (void *)(0x20000000 + (16 << 20));
+    /* Process configuration file */
+
+    autoboot = 1;
+
+    if ((ino = lookup(PATH_CONFIG)))
+	fsread(ino, cmd, sizeof(cmd));
+
+    if (*cmd) {
+	if (parse())
+	    autoboot = 0;
+	printf("%s: %s", PATH_CONFIG, cmd);
+	/* Do not process this command twice */
+	*cmd = 0;
+    }
+
+    /* Present the user with the boot2 prompt. */
+
+    strcpy(kname, PATH_KERNEL);
+    for (;;) {
+	printf("\nDefault: %s\nboot: ", kname);
+	if (!autoboot || (c = getc(2)) != -1)
+	    getstr(c);
+	xputchar('\n');
+	autoboot = 0;
+	c = 0;
+	printf("cmd is '%s'\n", cmd);
+	if (parse())
+	    xputchar('\a');
+#ifdef XMODEM_DL
+	else if (*cmd == '*')
+		Update();
+	else if (*cmd == '#')
+		reset();
+#endif
+	else
+	    load();
+    }
+}
+
+static void
+load(void)
+{
+    Elf32_Ehdr eh;
+    static Elf32_Phdr ep[2];
+    caddr_t p;
+    ino_t ino;
+    uint32_t addr;
+    int i, j;
+
+    if (!(ino = lookup(kname))) {
+	if (!ls)
+	    printf("No %s\n", kname);
+	return;
+    }
+    if (xfsread(ino, &eh, sizeof(eh)))
+	return;
+    if (!IS_ELF(eh)) {
+	printf("Invalid %s\n", "format");
+	return;
+    }
+    fs_off = eh.e_phoff;
+    for (j = i = 0; i < eh.e_phnum && j < 2; i++) {
+	if (xfsread(ino, ep + j, sizeof(ep[0])))
+	    return;
+	if (ep[j].p_type == PT_LOAD)
+	    j++;
+    }
+    for (i = 0; i < 2; i++) {
+	p = (caddr_t)ep[i].p_paddr;
+	fs_off = ep[i].p_offset;
+	if (xfsread(ino, p, ep[i].p_filesz))
+	    return;
+    }
+    addr = eh.e_entry;
+    ((void(*)(int))addr)(RB_BOOTINFO | (opts & RBX_MASK));
+}
+
+static int
+parse()
+{
+    char *arg = cmd;
+    char *ep, *p;
+    int c, i;
+
+    while ((c = *arg++)) {
+	if (c == ' ' || c == '\t' || c == '\n')
+	    continue;
+	for (p = arg; *p && *p != '\n' && *p != ' ' && *p != '\t'; p++);
+	ep = p;
+	if (*p)
+	    *p++ = 0;
+	if (c == '-') {
+	    while ((c = *arg++)) {
+		for (i = 0; c != optstr[i]; i++)
+		    if (i == NOPT - 1)
+			return -1;
+		opts ^= OPT_SET(flags[i]);
+	    }
+	} else {
+	    if ((i = ep - arg)) {
+		if ((size_t)i >= sizeof(kname))
+		    return -1;
+		memcpy(kname, arg, i + 1);
+	    }
+	}
+	arg = p;
+    }
+    return 0;
+}
+
+static int
+dskread(void *buf, unsigned lba, unsigned nblk)
+{
+    struct dos_partition *dp;
+    struct disklabel *d;
+    char *sec;
+    int i;
+
+    if (!dsk_meta) {
+	sec = dmadat->secbuf;
+	dsk_start = 0;
+	if (drvread(sec, DOSBBSECTOR, 1))
+	    return -1;
+	dp = (void *)(sec + DOSPARTOFF);
+	for (i = 0; i < NDOSPART; i++) {
+	    if (dp[i].dp_typ == DOSPTYP_386BSD)
+		    break;
+	}
+	if (i == NDOSPART)
+		return -1;
+	// Although dp_start is aligned within the disk partition structure,
+	// DOSPARTOFF is 446, which is only word (2) aligned, not longword (4)
+	// aligned.  Cope by using memcpy to fetch the start of this partition.
+	memcpy(&dsk_start, &dp[i].dp_start, 4);
+	if (drvread(sec, dsk_start + LABELSECTOR, 1))
+		return -1;
+	d = (void *)(sec + LABELOFFSET);
+	if (d->d_magic != DISKMAGIC || d->d_magic2 != DISKMAGIC) {
+		printf("Invalid %s\n", "label");
+		return -1;
+	}
+	if (!d->d_partitions[0].p_size) {
+		printf("Invalid %s\n", "partition");
+		return -1;
+	}
+	dsk_start += d->d_partitions[0].p_offset;
+	dsk_start -= d->d_partitions[RAW_PART].p_offset;
+	dsk_meta++;
+    }
+    return drvread(buf, dsk_start + lba, nblk);
+}
+
+static int
+drvread(void *buf, unsigned lba, unsigned nblk)
+{
+    static unsigned c = 0x2d5c7c2f;
+
+    printf("%c\b", c = c << 8 | c >> 24);
+    return (MCI_read((char *)buf, lba << 9, nblk << 9));
+}
diff --git a/sys/boot/arm/at91/boot2/kb920x_board.c b/sys/boot/arm/at91/boot2/kb920x_board.c
new file mode 100644
index 0000000..d171231
--- /dev/null
+++ b/sys/boot/arm/at91/boot2/kb920x_board.c
@@ -0,0 +1,35 @@
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+
+#include "emac.h"
+#include "lib.h"
+#include "ee.h"
+
+extern unsigned char mac[];
+
+static void
+MacFromEE()
+{	
+	uint32_t sig;
+	sig = 0;
+	EERead(12 * 1024, (uint8_t *)&sig, sizeof(sig));
+	if (sig != 0xaa55aa55)
+		return;
+	EERead(12 * 1024 + 4, mac, 6);
+	printf("MAC %x:%x:%x:%x:%x:%x\n", mac[0],
+	  mac[1], mac[2], mac[3], mac[4], mac[5]);
+}
+
+void
+Update(void)
+{
+}
+
+void
+board_init(void)
+{
+    EEInit();
+    MacFromEE();
+}
diff --git a/sys/boot/arm/at91/libat91/Makefile b/sys/boot/arm/at91/libat91/Makefile
index b016065..984ae31 100644
--- a/sys/boot/arm/at91/libat91/Makefile
+++ b/sys/boot/arm/at91/libat91/Makefile
@@ -4,9 +4,11 @@
 
 LIB=		at91
 INTERNALLIB=
-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
+SRCS=at91rm9200_lowlevel.c delay.c eeprom.c emac.c emac_init.c fpga.c getc.c \
+	putchar.c printf.c reset.c spi_flash.c xmodem.c \
+	sd-card.c mci_device.c strcvt.c strlen.c strcmp.c memcpy.c strcpy.c \
+	memset.c memcmp.c
+SRCS+=ashldi3.c divsi3.S
 NO_MAN=
 
 .if ${MK_TAG_LIST} != "no"
diff --git a/sys/boot/arm/at91/libat91/at91rm9200.h b/sys/boot/arm/at91/libat91/at91rm9200.h
index 2b7dde4..4de14f2 100644
--- a/sys/boot/arm/at91/libat91/at91rm9200.h
+++ b/sys/boot/arm/at91/libat91/at91rm9200.h
@@ -1156,28 +1156,6 @@ typedef struct _AT91S_MCI {
 #define	AT91C_MCI_SCDSEL      (0x1u <<  0) // (MCI) SD Card Selector
 #define	AT91C_MCI_SCDBUS      (0x1u <<  7) // (MCI) SD Card Bus Width
 // -------- MCI_CMDR : (MCI Offset: 0x14) MCI Command Register -------- 
-#define	AT91C_MCI_CMDNB       (0x1Fu <<  0) // (MCI) Command Number
-#define	AT91C_MCI_RSPTYP      (0x3u <<  6) // (MCI) Response Type
-#define		AT91C_MCI_RSPTYP_NO                   (0x0u <<  6) // (MCI) No response
-#define		AT91C_MCI_RSPTYP_48                   (0x1u <<  6) // (MCI) 48-bit response
-#define		AT91C_MCI_RSPTYP_136                  (0x2u <<  6) // (MCI) 136-bit response
-#define	AT91C_MCI_SPCMD       (0x7u <<  8) // (MCI) Special CMD
-#define		AT91C_MCI_SPCMD_NONE                 (0x0u <<  8) // (MCI) Not a special CMD
-#define		AT91C_MCI_SPCMD_INIT                 (0x1u <<  8) // (MCI) Initialization CMD
-#define		AT91C_MCI_SPCMD_SYNC                 (0x2u <<  8) // (MCI) Synchronized CMD
-#define		AT91C_MCI_SPCMD_IT_CMD               (0x4u <<  8) // (MCI) Interrupt command
-#define		AT91C_MCI_SPCMD_IT_REP               (0x5u <<  8) // (MCI) Interrupt response
-#define	AT91C_MCI_OPDCMD      (0x1u << 11) // (MCI) Open Drain Command
-#define	AT91C_MCI_MAXLAT      (0x1u << 12) // (MCI) Maximum Latency for Command to respond
-#define	AT91C_MCI_TRCMD       (0x3u << 16) // (MCI) Transfer CMD
-#define		AT91C_MCI_TRCMD_NO                   (0x0u << 16) // (MCI) No transfer
-#define		AT91C_MCI_TRCMD_START                (0x1u << 16) // (MCI) Start transfer
-#define		AT91C_MCI_TRCMD_STOP                 (0x2u << 16) // (MCI) Stop transfer
-#define	AT91C_MCI_TRDIR       (0x1u << 18) // (MCI) Transfer Direction
-#define	AT91C_MCI_TRTYP       (0x3u << 19) // (MCI) Transfer Type
-#define		AT91C_MCI_TRTYP_BLOCK                (0x0u << 19) // (MCI) Block Transfer type
-#define		AT91C_MCI_TRTYP_MULTIPLE             (0x1u << 19) // (MCI) Multiple Block transfer type
-#define		AT91C_MCI_TRTYP_STREAM               (0x2u << 19) // (MCI) Stream transfer type
 // -------- MCI_SR : (MCI Offset: 0x40) MCI Status Register -------- 
 #define	AT91C_MCI_CMDRDY      (0x1u <<  0) // (MCI) Command Ready flag
 #define	AT91C_MCI_RXRDY       (0x1u <<  1) // (MCI) RX Ready flag
diff --git a/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c b/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c
index 9e196d5..551b222 100644
--- a/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c
+++ b/sys/boot/arm/at91/libat91/at91rm9200_lowlevel.c
@@ -30,6 +30,9 @@
 #include "at91rm9200.h"
 #include "at91rm9200_lowlevel.h"
 
+extern int __bss_start__[];
+extern int __bss_end__[];
+
 #define BAUD	115200
 #define AT91C_US_ASYNC_MODE (AT91C_US_USMODE_NORMAL | AT91C_US_NBSTOP_1_BIT | \
 		AT91C_US_PAR_NONE | AT91C_US_CHRL_8_BITS | AT91C_US_CLKS_CLOCK)
@@ -42,12 +45,15 @@
 void
 _init(void)
 {
+	int *i;
+
 	AT91PS_USART pUSART = (AT91PS_USART)AT91C_BASE_DBGU;
 	AT91PS_PDC pPDC = (AT91PS_PDC)&(pUSART->US_RPR);
 
 	register unsigned	value;
 	volatile sdram_size_t *p = (sdram_size_t *)SDRAM_BASE;
 
+#if 0
 #ifdef BOOT_TSC
 	// For the TSC board, we turn ON the one LED we have while
 	// early in boot.
@@ -55,6 +61,7 @@ _init(void)
 	AT91C_BASE_PIOC->PIO_OER = AT91C_PIO_PC10;
 	AT91C_BASE_PIOC->PIO_CODR = AT91C_PIO_PC10;
 #endif
+#endif
 
 	// configure clocks
 	// assume:
@@ -195,4 +202,9 @@ _init(void)
 	pUSART->US_MR = AT91C_US_ASYNC_MODE;
 	pUSART->US_CR = AT91C_US_TXEN;
 	pUSART->US_CR = AT91C_US_RXEN;
+
+	/* Zero BSS now that we have memory setup */
+	i = (int *)__bss_start__;
+	while (i < (int *)__bss_end__)
+		*i++ = 0;
 }
diff --git a/sys/boot/arm/at91/libat91/emac.c b/sys/boot/arm/at91/libat91/emac.c
index 037e6b4..3ea1b82 100644
--- a/sys/boot/arm/at91/libat91/emac.c
+++ b/sys/boot/arm/at91/libat91/emac.c
@@ -31,7 +31,13 @@
 
 /* ********************** PRIVATE FUNCTIONS/DATA ******************************/
 
-static char serverMACAddr[6];
+static receive_descriptor_t *p_rxBD;
+static unsigned short localPort;
+static unsigned short serverPort;
+static unsigned serverMACSet;
+static unsigned localIPSet, serverIPSet;
+static unsigned	lastSize;
+static unsigned char serverMACAddr[6];
 static unsigned char localIPAddr[4], serverIPAddr[4];
 static int	ackBlock;
 static char *dlAddress;
@@ -75,7 +81,7 @@ GetServerAddress(void)
 
 	p_memset((char*)p_ARP->dest_mac, 0xFF, 6);
 
-	p_memcpy((char*)p_ARP->src_mac, (char*)localMACAddr, 6);
+	memcpy(p_ARP->src_mac, localMACAddr, 6);
 
 	p_ARP->frame_type = SWAP16(PROTOCOL_ARP);
 	p_ARP->hard_type  = SWAP16(1);
@@ -84,13 +90,10 @@ GetServerAddress(void)
 	p_ARP->prot_size  = 4;
 	p_ARP->operation  = SWAP16(ARP_REQUEST);
 
-	p_memcpy((char*)p_ARP->sender_mac, (char*)localMACAddr, 6);
-
-	p_memcpy((char*)p_ARP->sender_ip, (char*)localIPAddr, 4);
-
+	memcpy(p_ARP->sender_mac, localMACAddr, 6);
+	memcpy(p_ARP->sender_ip, localIPAddr, 4);
 	p_memset((char*)p_ARP->target_mac, 0, 6);
-
-	p_memcpy((char*)p_ARP->target_ip, (char*)serverIPAddr, 4);
+	memcpy(p_ARP->target_ip, serverIPAddr, 4);
 
 	// wait until transmit is available
 	while (!(*AT91C_EMAC_TSR & AT91C_EMAC_BNQ)) ;
@@ -115,10 +118,8 @@ Send_TFTP_Packet(char *tftpData, unsigned tftpLength)
 	udp_header_t		*udpHdr;
 	unsigned		t_checksum;
 
-	p_memcpy((char*)macHdr->dest_mac, (char*)serverMACAddr, 6);
-
-	p_memcpy((char*)macHdr->src_mac, (char*)localMACAddr, 6);
-
+	memcpy(macHdr->dest_mac, serverMACAddr, 6);
+	memcpy(macHdr->src_mac, localMACAddr, 6);
 	macHdr->proto_mac = SWAP16(PROTOCOL_IP);
 
 	ipHdr = (ip_header_t*)&macHdr->packet_length;
@@ -132,9 +133,8 @@ Send_TFTP_Packet(char *tftpData, unsigned tftpLength)
 	ipHdr->ip_p = PROTOCOL_UDP;
 	ipHdr->ip_sum = 0;
 
-	p_memcpy((char*)ipHdr->ip_src, (char*)localIPAddr, 4);
-
-	p_memcpy((char*)ipHdr->ip_dst, (char*)serverIPAddr, 4);
+	memcpy(ipHdr->ip_src, localIPAddr, 4);
+	memcpy(ipHdr->ip_dst, serverIPAddr, 4);
 
 	ipHdr->ip_sum = SWAP16(IP_checksum((unsigned short*)ipHdr, 20));
 
@@ -145,7 +145,7 @@ Send_TFTP_Packet(char *tftpData, unsigned tftpLength)
 	udpHdr->udp_len   = SWAP16(8 + tftpLength);
 	udpHdr->udp_cksum = 0;
 
-	p_memcpy((char*)udpHdr+8, tftpData, tftpLength);
+	memcpy((char *)udpHdr+8, tftpData, tftpLength);
 
 	t_checksum = IP_checksum((unsigned short*)ipHdr + 6, (16 + tftpLength));
 
@@ -182,8 +182,8 @@ TFTP_RequestFile(char *filename)
 
 	cPtr = (char*)&(tftpHeader.block_num);
 
-	ePtr = p_strcpy(cPtr, filename);
-	mPtr = p_strcpy(ePtr, "octet");
+	ePtr = strcpy(cPtr, filename);
+	mPtr = strcpy(ePtr, "octet");
 
 	length = mPtr - cPtr;
 	length += 2;
@@ -205,7 +205,7 @@ TFTP_ACK_Data(char *data, unsigned short block_num, unsigned short len)
 
 	if (block_num == (ackBlock + 1)) {
 		++ackBlock;
-		p_memcpy(dlAddress, data, len);
+		memcpy(dlAddress, data, len);
 		dlAddress += len;
 		lastSize += len;
 		if (ackBlock % 128 == 0)
@@ -216,7 +216,7 @@ TFTP_ACK_Data(char *data, unsigned short block_num, unsigned short len)
 	Send_TFTP_Packet((char*)&tftpHeader, 4);
 	if (len < 512) {
 		ackBlock = -2;
-		printf("tftp: %u byte\r\n", lastSize);
+		printf("tftp: %u byte\n", lastSize);
 	}
 }
 
@@ -249,7 +249,6 @@ CheckForNewPacket(ip_header_t *pHeader)
 		
 	if (!process)
 		return (0);
-						
 	process = i;
 		
 	pFrameType = (unsigned short *)((p_rxBD[i].address & 0xFFFFFFFC) + 12);
@@ -266,9 +265,7 @@ CheckForNewPacket(ip_header_t *pHeader)
 					(char*)serverIPAddr, 4)))) {
 
 				serverMACSet = 1;
-
-				p_memcpy((char*)serverMACAddr,
-					(char*)p_ARP->sender_mac, 6);
+				memcpy(serverMACAddr, p_ARP->sender_mac, 6);
 			}
 		} else if (p_ARP->operation == SWAP16(ARP_REQUEST)) {
 			// ARP REPLY operation
@@ -299,7 +296,7 @@ CheckForNewPacket(ip_header_t *pHeader)
 		break;
 	case SWAP16(PROTOCOL_IP):
 		pIpHeader = (ip_header_t*)(pData + 14);			
-		p_memcpy((char*)pHeader, (char*)pIpHeader,sizeof(ip_header_t));
+		memcpy(pHeader, pIpHeader, sizeof(ip_header_t));
 		
 		if (pIpHeader->ip_p == PROTOCOL_UDP) {
 			udp_header_t	*udpHdr;
@@ -424,7 +421,7 @@ MII_GetLinkSpeed(AT91PS_EMAC pEmac)
 		printf(" FDX");
 		update |= AT91C_EMAC_FD;
 	}
-	printf("\r\n");
+	printf("\n");
 #endif
 	pEmac->EMAC_CFG = update;
 }
@@ -443,6 +440,9 @@ AT91F_EmacEntry(void)
 	char		*pRxPacket = (char*)RX_DATA_START;
 	AT91PS_EMAC	pEmac = AT91C_BASE_EMAC;
 
+	p_rxBD = (receive_descriptor_t*)RX_BUFFER_START;
+	localPort = SWAP16(0x8002);
+
 	for (i = 0; i < MAX_RX_PACKETS; ++i) {
 
 		p_rxBD[i].address = (unsigned)pRxPacket;
@@ -553,10 +553,10 @@ TFTP_Download(unsigned address, char *filename)
 				// Be sure to send a NAK, which is done by
 				// ACKing the last block we got.
 				TFTP_ACK_Data(0, ackBlock, 512);
-				printf("\nNAK %u\r\n", ackBlock);
+				printf("\nNAK %u\n", ackBlock);
 			}
 		}
 	}
 	if (timeout == 0)
-		printf("TFTP TIMEOUT!\r\n");
+		printf("TFTP TIMEOUT!\n");
 }
diff --git a/sys/boot/arm/at91/libat91/emac.h b/sys/boot/arm/at91/libat91/emac.h
index b8521fa..81e7f4a 100644
--- a/sys/boot/arm/at91/libat91/emac.h
+++ b/sys/boot/arm/at91/libat91/emac.h
@@ -129,10 +129,7 @@ typedef struct {
 
 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;
+extern unsigned localMACSet;
+#define EMAC_Init()
 
 #endif /* _EMAC_H_ */
diff --git a/sys/boot/arm/at91/libat91/emac_init.c b/sys/boot/arm/at91/libat91/emac_init.c
index 37844f9..4d0bb37 100644
--- a/sys/boot/arm/at91/libat91/emac_init.c
+++ b/sys/boot/arm/at91/libat91/emac_init.c
@@ -56,13 +56,9 @@
 
 /* ****************************** GLOBALS *************************************/
 
-unsigned	lastSize;
-unsigned localMACSet, serverMACSet;
+unsigned localMACSet;
 unsigned char localMACAddr[6];
 unsigned localMAClow, localMAChigh;
-unsigned localIPSet, serverIPSet;
-unsigned short	serverPort, localPort;
-receive_descriptor_t *p_rxBD;
 
 /* ********************** PRIVATE FUNCTIONS/DATA ******************************/
 
@@ -82,7 +78,7 @@ EMAC_SetMACAddress(unsigned char mac[6])
 	/* enable the peripheral clock before using EMAC */
 	pPMC->PMC_PCER = ((unsigned) 1 << AT91C_ID_EMAC);
 
-	p_memcpy(localMACAddr, mac, 6);
+	memcpy(localMACAddr, mac, 6);
 	localMAClow = (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) | mac[5];
 	localMAChigh = (mac[0] << 8) | mac[1];
 	localMACSet = 1;
@@ -119,21 +115,3 @@ EMAC_SetMACAddress(unsigned char mac[6])
 	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.h b/sys/boot/arm/at91/libat91/lib.h
index 8e3b25b..d88b791 100644
--- a/sys/boot/arm/at91/libat91/lib.h
+++ b/sys/boot/arm/at91/libat91/lib.h
@@ -29,6 +29,7 @@
 
 int getc(int);
 void putchar(int);
+void xputchar(int);
 void printf(const char *fmt,...);
 
 /* The following function write eeprom at ee_addr using data 	*/
@@ -56,9 +57,9 @@ unsigned p_ASCIIToDec(const char *buf);
 
 void p_memset(char *buffer, char value, int size);
 int p_strlen(const char *buffer);
-char *p_strcpy(char *to, const char *from);
-void p_memcpy(char *to, const char *from, unsigned size);
+char *strcpy(char *to, const char *from);
+void memcpy(void *to, const void *from, unsigned size);
 int p_memcmp(const char *to, const char *from, unsigned size);
-int p_strcmp(const char *to, const char *from);
+int strcmp(const char *to, const char *from);
 
 #endif
diff --git a/sys/boot/arm/at91/libat91/lib_AT91RM9200.h b/sys/boot/arm/at91/libat91/lib_AT91RM9200.h
index d1bf488..d5a5b67 100644
--- a/sys/boot/arm/at91/libat91/lib_AT91RM9200.h
+++ b/sys/boot/arm/at91/libat91/lib_AT91RM9200.h
@@ -38,10 +38,10 @@
 //*----------------------------------------------------------------------------
 static inline void
 AT91F_PMC_EnablePeriphClock (
-	AT91PS_PMC pPMC, // \arg pointer to PMC controller
-	unsigned int periphIds)  // \arg IDs of peripherals to enable
+       AT91PS_PMC pPMC, // \arg pointer to PMC controller
+       unsigned int periphIds)  // \arg IDs of peripherals to enable
 {
-	pPMC->PMC_PCER = periphIds;
+       pPMC->PMC_PCER = periphIds;
 }
 
 /* *****************************************************************************
@@ -58,20 +58,16 @@ void AT91F_PIO_CfgPeriph(
 	unsigned int periphBEnable)  // \arg PERIPH B to enable
 
 {
-	pPio->PIO_ASR = periphAEnable;
-	pPio->PIO_BSR = periphBEnable;
+	if (periphAEnable)
+		pPio->PIO_ASR = periphAEnable;
+	if (periphBEnable)
+		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)
 
@@ -117,242 +113,6 @@ void AT91F_MCI_Configure(
 }
 
 //*----------------------------------------------------------------------------
-//* \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
 //*----------------------------------------------------------------------------
@@ -492,50 +252,6 @@ AT91F_PDC_DisableRx(
 }
 
 //*----------------------------------------------------------------------------
-//* \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
 //*----------------------------------------------------------------------------
@@ -558,82 +274,4 @@ AT91F_PDC_Open(
 	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
index f6dae8d..5eb6b9f 100644
--- a/sys/boot/arm/at91/libat91/mci_device.c
+++ b/sys/boot/arm/at91/libat91/mci_device.c
@@ -48,6 +48,8 @@
 
 #include "lib.h"
 
+#define MMC_SUPPORT 0
+
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_MCI_SendCommand
 //* \brief Generic function to send a command to the MMC or SDCard
@@ -62,6 +64,7 @@ AT91F_MCI_SendCommand(
 	AT91C_BASE_MCI->MCI_ARGR = Arg;
 	AT91C_BASE_MCI->MCI_CMDR = Cmd;
 
+//	printf("CMDR %x ARG %x\n", Cmd, Arg);
 	// wait for CMDRDY Status flag to read the response
 	do
 	{
@@ -71,8 +74,10 @@ AT91F_MCI_SendCommand(
 	// 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) )
+		// if the command is SEND_OP_COND the CRC error flag is
+		// always present (cf : R3 response)
+		if ((Cmd != SDCARD_APP_OP_COND_CMD) &&
+		    (Cmd != 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);
@@ -94,7 +99,7 @@ AT91F_MCI_SDCard_SendAppCommand(
 
 	// 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;
+	AT91C_BASE_MCI->MCI_CMDR = APP_CMD;
 
 	// wait for CMDRDY Status flag to read the response
 	do
@@ -107,10 +112,6 @@ AT91F_MCI_SDCard_SendAppCommand(
 	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));
 }
 
@@ -121,7 +122,7 @@ AT91F_MCI_SDCard_SendAppCommand(
 static AT91S_MCIDeviceStatus
 AT91F_MCI_GetStatus(unsigned int relative_card_address)
 {
-	if (AT91F_MCI_SendCommand(AT91C_SEND_STATUS_CMD,
+	if (AT91F_MCI_SendCommand(SEND_STATUS_CMD,
 		relative_card_address <<16) == AT91C_CMD_SEND_OK)
 		return (AT91C_BASE_MCI->MCI_RSPR[0]);
 	return AT91C_CMD_SEND_ERROR;
@@ -162,71 +163,30 @@ AT91F_MCI_ReadBlock(
 	unsigned int *dataBuffer,
 	int sizeToRead)
 {
+	unsigned log2sl = pMCI_Device->pMCI_DeviceFeatures->READ_BL_LEN;
+	unsigned sectorLength = 1 << log2sl;
+
 	///////////////////////////////////////////////////////////////////////
-	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 (pMCI_Device->pMCI_DeviceDesc->state != AT91C_MCI_IDLE)
+		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;
-	}
+	     pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address) & AT91C_SR_READY_FOR_DATA) == 0)
+		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);
+	AT91C_BASE_MCI->MCI_MR |= ((sectorLength << 16) | AT91C_MCI_PDCMODE);
 	 
-	if (sizeToRead %4)
-		sizeToRead = (sizeToRead /4)+1;
-	else
-		sizeToRead = sizeToRead/4;
+	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)
+	if (AT91F_MCI_SendCommand(READ_SINGLE_BLOCK_CMD, src) != AT91C_CMD_SEND_OK)
 		return AT91C_READ_ERROR;
 	pMCI_Device->pMCI_DeviceDesc->state = AT91C_MCI_RX_SINGLE_BLOCK;
 
@@ -239,7 +199,7 @@ AT91F_MCI_ReadBlock(
 	return AT91C_READ_OK;
 }
 
-#if 0
+#if MMC_SUPPORT
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_MCI_WriteBlock
 //* \brief  Write an ENTIRE block but not always PARTIAL block !!!
@@ -251,36 +211,36 @@ AT91F_MCI_WriteBlock(
 	unsigned int *dataBuffer,
 	int sizeToWrite )
 {
+	unsigned log2sl = pMCI_Device->pMCI_DeviceFeatures->WRITE_BL_LEN;
+	unsigned sectorLength = 1 << log2sl;
+
 	///////////////////////////////////////////////////////////////////////
 	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)
+	if( (AT91F_MCI_GetStatus(pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address) & AT91C_SR_READY_FOR_DATA) == 0)
 		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 )
+	if ( dest % sectorLength != 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) )
+	if ((sizeToWrite < sectorLength) 
+	    && (pMCI_Device->pMCI_DeviceFeatures->Write_Partial == 0x00))
    		return AT91C_WRITE_ERROR;
    		
-   	if( sizeToWrite > pMCI_Device->pMCI_DeviceFeatures->Max_Write_DataBlock_Length )
+   	if (sizeToWrite > sectorLength)
    		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;
+	AT91C_BASE_MCI->MCI_MR |= ((1 << pMCI_Device->pMCI_DeviceFeatures->WRITE_BL_LEN) << 16) | AT91C_MCI_PDCMODE;
+
+	sizeToWrite = sizeToWrite / 4;
 
 	// Init PDC for write sequence
 	AT91C_BASE_PDC_MCI->PDC_PTCR = (AT91C_PDC_TXTDIS | AT91C_PDC_RXTDIS);
@@ -288,7 +248,7 @@ AT91F_MCI_WriteBlock(
 	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)
+	if ( AT91F_MCI_SendCommand(WRITE_BLOCK_CMD, dest) != AT91C_CMD_SEND_OK)
 		return AT91C_WRITE_ERROR;
 
 	pMCI_Device->pMCI_DeviceDesc->state = AT91C_MCI_TX_SINGLE_BLOCK;
@@ -309,7 +269,7 @@ AT91F_MCI_WriteBlock(
 AT91S_MCIDeviceStatus
 AT91F_MCI_MMC_SelectCard(AT91PS_MciDevice pMCI_Device, unsigned int relative_card_address)
 {
-    int status;
+	int status;
 	
 	//* Check if the MMC card chosen is already the selected one
 	status = AT91F_MCI_GetStatus(relative_card_address);
@@ -320,19 +280,20 @@ AT91F_MCI_MMC_SelectCard(AT91PS_MciDevice pMCI_Device, unsigned int relative_car
 	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
+	//* 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;
+		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;
+	if (AT91F_MCI_SendCommand(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
 
@@ -344,7 +305,7 @@ static AT91S_MCIDeviceStatus
 AT91F_MCI_GetCSD(unsigned int relative_card_address , unsigned int * response)
 {
  	
- 	if(AT91F_MCI_SendCommand(AT91C_SEND_CSD_CMD,
+ 	if(AT91F_MCI_SendCommand(SEND_CSD_CMD,
 	       (relative_card_address << 16)) != AT91C_CMD_SEND_OK)
 		return AT91C_CMD_SEND_ERROR;
 	
@@ -363,10 +324,10 @@ AT91F_MCI_GetCSD(unsigned int relative_card_address , unsigned int * response)
 AT91S_MCIDeviceStatus
 AT91F_MCI_SetBlocklength(unsigned int length)
 {
-	return( AT91F_MCI_SendCommand(AT91C_SET_BLOCKLEN_CMD, length) );
+	return( AT91F_MCI_SendCommand(SET_BLOCKLEN_CMD, length) );
 }
 
-#if 0
+#if MMC_SUPPORT
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_MCI_MMC_GetAllOCR
 //* \brief Asks to all cards to send their operations conditions
@@ -377,7 +338,7 @@ AT91F_MCI_MMC_GetAllOCR()
 	unsigned int	response =0x0;
  	
  	while(1) {
-		response = AT91F_MCI_SendCommand(AT91C_MMC_SEND_OP_COND_CMD,
+		response = AT91F_MCI_SendCommand(MMC_SEND_OP_COND_CMD,
 		    AT91C_MMC_HOST_VOLTAGE_RANGE);
 		if (response != AT91C_CMD_SEND_OK)
 			return AT91C_INIT_ERROR;
@@ -397,7 +358,7 @@ 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,
+	 	if(AT91F_MCI_SendCommand(MMC_ALL_SEND_CID_CMD,
 		       AT91C_NO_ARGUMENT) != AT91C_CMD_SEND_OK)
 			return Nb_Cards_Found;
 		else {		
@@ -408,7 +369,7 @@ AT91F_MCI_MMC_GetAllCID(AT91PS_MciDevice pMCI_Device, unsigned int *response)
 			pMCI_Device->pMCI_DeviceFeatures[Nb_Cards_Found].Card_Inserted = AT91C_MMC_CARD_INSERTED;
 	
 			if (AT91F_MCI_SendCommand(
-				AT91C_MMC_SET_RELATIVE_ADDR_CMD,
+				MMC_SET_RELATIVE_ADDR_CMD,
 				(Nb_Cards_Found + AT91C_FIRST_RCA) << 16) != AT91C_CMD_SEND_OK)
 				return AT91C_CMD_SEND_ERROR;
 				 
@@ -431,7 +392,7 @@ AT91F_MCI_MMC_Init (AT91PS_MciDevice pMCI_Device)
 	AT91PS_MciDeviceFeatures f;
 
 	//* Resets all MMC Cards in Idle state
-	AT91F_MCI_SendCommand(AT91C_MMC_GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
+	AT91F_MCI_SendCommand(MMC_GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
 
 	if (AT91F_MCI_MMC_GetAllOCR(pMCI_Device) == AT91C_INIT_ERROR)
 		return AT91C_INIT_ERROR;
@@ -449,27 +410,25 @@ AT91F_MCI_MMC_Init (AT91PS_MciDevice pMCI_Device)
 			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;
+		f->READ_BL_LEN = ((tab_response[1] >> CSD_1_RD_B_LEN_S) & CSD_1_RD_B_LEN_M);
+		f->WRITE_BL_LEN = ((tab_response[3] >> CSD_3_WBLEN_S) & CSD_3_WBLEN_M );
+		f->Sector_Size = 1 + ((tab_response[2] >> CSD_2_v22_SECT_SIZE_S) & CSD_2_v22_SECT_SIZE_M );
+		f->Read_Partial = (tab_response[1] >> CSD_1_RD_B_PAR_S) & CSD_1_RD_B_PAR_M;
+		f->Write_Partial = (tab_response[3] >> CSD_3_WBLOCK_P_S) & CSD_3_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;
+		f->Read_Block_Misalignment = (tab_response[1] >> CSD_1_RD_B_MIS_S) & CSD_1_RD_B_MIS_M;
+		f->Write_Block_Misalignment = (tab_response[1] >> CSD_1_WR_B_MIS_S) & CSD_1_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 );
+		mult = 1 << ( ((tab_response[2] >> CSD_2_C_SIZE_M_S) & CSD_2_C_SIZE_M_M) + 2 );
 		// compute MSB of C_SIZE
-		blocknr = ((tab_response[1] >> AT91C_CSD_CSIZE_H_S) & AT91C_CSD_CSIZE_H_M) << 2;
+		blocknr = ((tab_response[1] >> CSD_1_CSIZE_H_S) & CSD_1_CSIZE_H_M) << 2;
 		// compute MULT * (LSB of C-SIZE + MSB already computed + 1) = BLOCKNR
-		blocknr = mult * ( ( blocknr + ( (tab_response[2] >> AT91C_CSD_CSIZE_L_S) & AT91C_CSD_CSIZE_L_M) ) + 1 );
-		f->Memory_Capacity =  f->Max_Read_DataBlock_Length * blocknr;
+		blocknr = mult * ( ( blocknr + ( (tab_response[2] >> CSD_2_CSIZE_L_S) & CSD_2_CSIZE_L_M) ) + 1 );
+		f->Memory_Capacity =  (1 << f->READ_BL_LEN) * blocknr;
 		//// End of Compute Memory Capacity
 	}
 	// XXX warner hacked this
@@ -491,7 +450,7 @@ AT91F_MCI_SDCard_GetOCR (AT91PS_MciDevice pMCI_Device)
  	
  	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,
+		    SDCARD_APP_OP_COND_CMD,
 		    AT91C_MMC_HOST_VOLTAGE_RANGE);
 		if (response != AT91C_CMD_SEND_OK)
 			return AT91C_INIT_ERROR;
@@ -508,7 +467,7 @@ AT91F_MCI_SDCard_GetOCR (AT91PS_MciDevice pMCI_Device)
 static AT91S_MCIDeviceStatus
 AT91F_MCI_SDCard_GetCID(unsigned int *response)
 {
-	if (AT91F_MCI_SendCommand(AT91C_ALL_SEND_CID_CMD,
+	if (AT91F_MCI_SendCommand(ALL_SEND_CID_CMD,
 		AT91C_NO_ARGUMENT) != AT91C_CMD_SEND_OK)
 		return AT91C_CMD_SEND_ERROR;
 	
@@ -536,7 +495,7 @@ AT91F_MCI_SDCard_SetBusWidth(AT91PS_MciDevice pMCI_Device)
 	while((ret_value > 0) && ((ret_value & AT91C_SR_READY_FOR_DATA) == 0));
 
 	// Select Card
-	AT91F_MCI_SendCommand(AT91C_SEL_DESEL_CARD_CMD,
+	AT91F_MCI_SendCommand(SEL_DESEL_CARD_CMD,
 	    (pMCI_Device->pMCI_DeviceFeatures->Relative_Card_Address)<<16);
 
 	// Set bus width for Sdcard
@@ -545,7 +504,8 @@ AT91F_MCI_SDCard_SetBusWidth(AT91PS_MciDevice pMCI_Device)
 	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)
+	if (AT91F_MCI_SDCard_SendAppCommand(pMCI_Device,
+	      SDCARD_SET_BUS_WIDTH_CMD,bus_width) != AT91C_CMD_SEND_OK)
 		return AT91C_CMD_SEND_ERROR;
 
 	return AT91C_CMD_SEND_OK;
@@ -555,13 +515,14 @@ AT91F_MCI_SDCard_SetBusWidth(AT91PS_MciDevice pMCI_Device)
 //* \fn    AT91F_MCI_SDCard_Init
 //* \brief Return the SDCard initialisation status
 //*----------------------------------------------------------------------------
-AT91S_MCIDeviceStatus AT91F_MCI_SDCard_Init (AT91PS_MciDevice pMCI_Device)
+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);
+	AT91F_MCI_SendCommand(GO_IDLE_STATE_CMD, AT91C_NO_ARGUMENT);
 
 	if (AT91F_MCI_SDCard_GetOCR(pMCI_Device) == AT91C_INIT_ERROR)
 		return AT91C_INIT_ERROR;
@@ -570,47 +531,45 @@ AT91S_MCIDeviceStatus AT91F_MCI_SDCard_Init (AT91PS_MciDevice pMCI_Device)
 	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) !=
+	if (AT91F_MCI_SendCommand(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;
+	f->READ_BL_LEN = (tab_response[1] >> CSD_1_RD_B_LEN_S) &
+	    CSD_1_RD_B_LEN_M;
+	f->WRITE_BL_LEN = (tab_response[3] >> CSD_3_WBLEN_S) &
+	    CSD_3_WBLEN_M;
+	f->Sector_Size = 1 + ((tab_response[2] >> CSD_2_v21_SECT_SIZE_S) &
+	    CSD_2_v21_SECT_SIZE_M);
+	f->Read_Partial = (tab_response[1] >> CSD_1_RD_B_PAR_S) &
+	    CSD_1_RD_B_PAR_M;
+	f->Write_Partial = (tab_response[3] >> CSD_3_WBLOCK_P_S) &
+	    CSD_3_WBLOCK_P_M;
+	f->Erase_Block_Enable = (tab_response[2] >> CSD_2_v21_ER_BLEN_EN_S) &
+	    CSD_2_v21_ER_BLEN_EN_M;
+	f->Read_Block_Misalignment = (tab_response[1] >> CSD_1_RD_B_MIS_S) &
+	    CSD_1_RD_B_MIS_M;
+	f->Write_Block_Misalignment = (tab_response[1] >> CSD_1_WR_B_MIS_S) &
+	    CSD_1_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 );
+	mult = 1 << ( ((tab_response[2] >> CSD_2_C_SIZE_M_S) &
+	    CSD_2_C_SIZE_M_M) + 2 );
 	// compute MSB of C_SIZE
-	blocknr = ((tab_response[1] >> AT91C_CSD_CSIZE_H_S) &
-	    AT91C_CSD_CSIZE_H_M) << 2;
+	blocknr = ((tab_response[1] >> CSD_1_CSIZE_H_S) &
+	    CSD_1_CSIZE_H_M) << 2;
 	// compute MULT * (LSB of C-SIZE + MSB already computed + 1) = BLOCKNR
-	blocknr = mult * ((blocknr + ((tab_response[2] >> AT91C_CSD_CSIZE_L_S) &
-	    AT91C_CSD_CSIZE_L_M)) + 1);
-	f->Memory_Capacity =  f->Max_Read_DataBlock_Length * blocknr;
+	blocknr = mult * ((blocknr + ((tab_response[2] >> CSD_2_CSIZE_L_S) &
+	    CSD_2_CSIZE_L_M)) + 1);
+	f->Memory_Capacity = (1 << f->READ_BL_LEN) * 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)
+	if (AT91F_MCI_SetBlocklength(1 << f->READ_BL_LEN) != AT91C_CMD_SEND_OK)
 		return AT91C_INIT_ERROR;
+	printf("Found SD card %u bytes\n", f->Memory_Capacity);
 	return AT91C_INIT_OK;
 }
diff --git a/sys/boot/arm/at91/libat91/mci_device.h b/sys/boot/arm/at91/libat91/mci_device.h
index 197004b..6fee390 100644
--- a/sys/boot/arm/at91/libat91/mci_device.h
+++ b/sys/boot/arm/at91/libat91/mci_device.h
@@ -46,10 +46,11 @@
 #ifndef __MCI_Device_h
 #define __MCI_Device_h
 
+#include <sys/types.h>
 
 typedef unsigned int AT91S_MCIDeviceStatus;
 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
 
 #define AT91C_CARD_REMOVED			0
 #define AT91C_MMC_CARD_INSERTED		1
@@ -76,9 +77,11 @@ typedef unsigned int AT91S_MCIDeviceStatus;
 /* TimeOut */
 #define AT91C_TIMEOUT_CMDRDY		30
 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+///////////////////////////////////////////////////////////////////////////////
 // MMC & SDCard Structures 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
 
 /*-----------------------------------------------*/
 /* SDCard Device Descriptor Structure Definition */
@@ -99,8 +102,6 @@ typedef struct	_AT91S_MciDeviceFeatures
 	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
@@ -118,149 +119,197 @@ 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;
+} AT91S_MciDevice, *AT91PS_MciDevice;
+
+#include <dev/mmc/mmcreg.h>
 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+// 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
+
+#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)
+
+#define	MMC_CMDNB       (0x1Fu <<  0)		// Command Number
+#define	MMC_RSPTYP      (0x3u <<  6)		// Response Type
+#define	    MMC_RSPTYP_NO      (0x0u <<  6)	// No response
+#define	    MMC_RSPTYP_48      (0x1u <<  6)	// 48-bit response
+#define	    MMC_RSPTYP_136     (0x2u <<  6)	// 136-bit response
+#define	MMC_SPCMD       (0x7u <<  8)		// Special CMD
+#define	    MMC_SPCMD_NONE     (0x0u <<  8)	// Not a special CMD
+#define	    MMC_SPCMD_INIT     (0x1u <<  8)	// Initialization CMD
+#define	    MMC_SPCMD_SYNC     (0x2u <<  8)	// Synchronized CMD
+#define	    MMC_SPCMD_IT_CMD   (0x4u <<  8)	// Interrupt command
+#define	    MMC_SPCMD_IT_REP   (0x5u <<  8)	// Interrupt response
+#define	MMC_OPDCMD      (0x1u << 11)		// Open Drain Command
+#define	MMC_MAXLAT      (0x1u << 12)		// Maximum Latency for Command to respond
+#define	MMC_TRCMD       (0x3u << 16)		// Transfer CMD
+#define	    MMC_TRCMD_NO       (0x0u << 16)	// No transfer
+#define	    MMC_TRCMD_START    (0x1u << 16)	// Start transfer
+#define	    MMC_TRCMD_STOP     (0x2u << 16)	// Stop transfer
+#define	MMC_TRDIR       (0x1u << 18)		// Transfer Direction
+#define	MMC_TRTYP       (0x3u << 19)		// Transfer Type
+#define	    MMC_TRTYP_BLOCK    (0x0u << 19)	// Block Transfer type
+#define	    MMC_TRTYP_MULTIPLE (0x1u << 19)	// Multiple Block transfer type
+#define	    MMC_TRTYP_STREAM   (0x2u << 19)	// Stream transfer type
+
+///////////////////////////////////////////////////////////////////////////////
 // MCI_CMD Register Value 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
-#define AT91C_POWER_ON_INIT						(0	| AT91C_MCI_TRCMD_NO 	| AT91C_MCI_SPCMD_INIT | AT91C_MCI_OPDCMD)
+///////////////////////////////////////////////////////////////////////////////
+#define POWER_ON_INIT	\
+    (0 | MMC_TRCMD_NO | MMC_SPCMD_INIT | MMC_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 )
+#define GO_IDLE_STATE_CMD	\
+    (0 | MMC_TRCMD_NO | MMC_SPCMD_NONE )
+#define MMC_GO_IDLE_STATE_CMD \
+    (0 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_OPDCMD)
+#define MMC_SEND_OP_COND_CMD \
+    (1 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_OPDCMD)
+
+#define ALL_SEND_CID_CMD \
+    (2 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_136)
+#define MMC_ALL_SEND_CID_CMD \
+    (2 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_136 | \
+    MMC_OPDCMD)
+
+#define SET_RELATIVE_ADDR_CMD \
+    (3 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_MAXLAT)
+#define MMC_SET_RELATIVE_ADDR_CMD \
+    (3 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_MAXLAT | MMC_OPDCMD)
+
+#define SET_DSR_CMD \
+    (4 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_NO | \
+     MMC_MAXLAT)	// no tested
+
+#define SEL_DESEL_CARD_CMD \
+    (7 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_MAXLAT)
+#define SEND_CSD_CMD \
+    (9 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_136 | \
+     MMC_MAXLAT)
+#define SEND_CID_CMD \
+    (10 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_136 | \
+     MMC_MAXLAT)
+#define MMC_READ_DAT_UNTIL_STOP_CMD \
+    (11 | MMC_TRTYP_STREAM | MMC_SPCMD_NONE | \
+     MMC_RSPTYP_48 | MMC_TRDIR | MMC_TRCMD_START | \
+     MMC_MAXLAT)
+
+#define STOP_TRANSMISSION_CMD \
+    (12 | MMC_TRCMD_STOP | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_MAXLAT)
+#define STOP_TRANSMISSION_SYNC_CMD \
+    (12 | MMC_TRCMD_STOP | MMC_SPCMD_SYNC | MMC_RSPTYP_48 | \
+     MMC_MAXLAT)
+#define SEND_STATUS_CMD \
+    (13 | MMC_TRCMD_NO | MMC_SPCMD_NONE | MMC_RSPTYP_48 | \
+     MMC_MAXLAT)
+#define GO_INACTIVE_STATE_CMD \
+     (15 | MMC_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)
+#define SET_BLOCKLEN_CMD					(16 | MMC_TRCMD_NO 	| MMC_SPCMD_NONE	| MMC_RSPTYP_48		| MMC_MAXLAT )
+#define READ_SINGLE_BLOCK_CMD				(17 | MMC_SPCMD_NONE	| MMC_RSPTYP_48 	| MMC_TRCMD_START	| MMC_TRTYP_BLOCK	| MMC_TRDIR	| MMC_MAXLAT)
+#define READ_MULTIPLE_BLOCK_CMD			(18 | MMC_SPCMD_NONE	| MMC_RSPTYP_48 	| MMC_TRCMD_START	| MMC_TRTYP_MULTIPLE	| MMC_TRDIR	| MMC_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
+#define MMC_WRITE_DAT_UNTIL_STOP_CMD		(20 | MMC_TRTYP_STREAM| MMC_SPCMD_NONE	| MMC_RSPTYP_48 & ~(MMC_TRDIR) | MMC_TRCMD_START | MMC_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 )
+#define WRITE_BLOCK_CMD					(24 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_START	| (MMC_TRTYP_BLOCK 	&  ~(MMC_TRDIR))	| MMC_MAXLAT)
+#define WRITE_MULTIPLE_BLOCK_CMD			(25 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_START	| (MMC_TRTYP_MULTIPLE	&  ~(MMC_TRDIR)) 	| MMC_MAXLAT)
+#define PROGRAM_CSD_CMD					(27 | MMC_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 )
+#define SET_WRITE_PROT_CMD				(28	| MMC_RSPTYP_48 )
+#define CLR_WRITE_PROT_CMD				(29	| MMC_RSPTYP_48 )
+#define SEND_WRITE_PROT_CMD				(30	| MMC_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 )
+#define TAG_SECTOR_START_CMD				(32 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define TAG_SECTOR_END_CMD  				(33 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define MMC_UNTAG_SECTOR_CMD				(34 | MMC_RSPTYP_48 )
+#define MMC_TAG_ERASE_GROUP_START_CMD		(35 | MMC_RSPTYP_48 )
+#define MMC_TAG_ERASE_GROUP_END_CMD		(36 | MMC_RSPTYP_48 )
+#define MMC_UNTAG_ERASE_GROUP_CMD			(37 | MMC_RSPTYP_48 )
+#define ERASE_CMD							(38 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_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
+#define LOCK_UNLOCK						(42 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_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)
+#define APP_CMD							(55 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO | MMC_MAXLAT)
+#define GEN_CMD							(56 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO | MMC_MAXLAT)	// no tested
+
+#define SDCARD_SET_BUS_WIDTH_CMD			(6 	| MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define SDCARD_STATUS_CMD					(13 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define SDCARD_SEND_NUM_WR_BLOCKS_CMD		(22 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define SDCARD_SET_WR_BLK_ERASE_COUNT_CMD	(23 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define SDCARD_APP_OP_COND_CMD			(41 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO )
+#define SDCARD_SET_CLR_CARD_DETECT_CMD	(42 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+#define SDCARD_SEND_SCR_CMD				(51 | MMC_SPCMD_NONE	| MMC_RSPTYP_48	| MMC_TRCMD_NO	| MMC_MAXLAT)
+
+#define SDCARD_APP_ALL_CMD				(SDCARD_SET_BUS_WIDTH_CMD +\
+												SDCARD_STATUS_CMD +\
+												SDCARD_SEND_NUM_WR_BLOCKS_CMD +\
+												SDCARD_SET_WR_BLK_ERASE_COUNT_CMD +\
+												SDCARD_APP_OP_COND_CMD +\
+												SDCARD_SET_CLR_CARD_DETECT_CMD +\
+												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)
+#define MMC_FAST_IO_CMD					(39 | MMC_SPCMD_NONE | MMC_RSPTYP_48 | MMC_MAXLAT)
+#define MMC_GO_IRQ_STATE_CMD				(40 | MMC_SPCMD_NONE | MMC_RSPTYP_48 | MMC_TRCMD_NO	| MMC_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)
@@ -283,16 +332,12 @@ typedef struct _AT91S_MciDevice
 #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)
+#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)
@@ -306,103 +351,105 @@ typedef struct _AT91S_MciDevice
 
 #define AT91C_SR_CARD_SELECTED				(AT91C_SR_READY_FOR_DATA + AT91C_SR_TRAN)
 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+#define MMC_FIRST_RCA				0xCAFE
+
+///////////////////////////////////////////////////////////////////////////////
 // MMC CSD register header File					
-// AT91C_CSD_xxx_S	for shift value
-// AT91C_CSD_xxx_M	for mask  value
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+// CSD_x_xxx_S	for shift value for word x
+// CSD_x_xxx_M	for mask  value for word x
+///////////////////////////////////////////////////////////////////////////////
 
 // 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
+#define	CSD_3_BIT0_S		0		// [0:0]			
+#define	CSD_3_BIT0_M		0x01				
+#define	CSD_3_CRC_S		1		// [7:1]
+#define	CSD_3_CRC_M		0x7F
+#define	CSD_3_MMC_ECC_S		8		// [9:8] reserved for MMC compatibility
+#define	CSD_3_MMC_ECC_M		0x03
+#define	CSD_3_FILE_FMT_S	10		// [11:10]
+#define	CSD_3_FILE_FMT_M	0x03
+#define	CSD_3_TMP_WP_S		12		// [12:12]
+#define	CSD_3_TMP_WP_M		0x01
+#define	CSD_3_PERM_WP_S 	13		// [13:13]
+#define	CSD_3_PERM_WP_M 	0x01
+#define	CSD_3_COPY_S		14		// [14:14]
+#define	CSD_3_COPY_M 		0x01
+#define	CSD_3_FILE_FMT_GRP_S	15		// [15:15]
+#define	CSD_3_FILE_FMT_GRP_M	0x01
+//	reserved		16		// [20:16]
+//	reserved		0x1F
+#define	CSD_3_WBLOCK_P_S	21		// [21:21]
+#define	CSD_3_WBLOCK_P_M	0x01
+#define	CSD_3_WBLEN_S 		22		// [25:22]
+#define	CSD_3_WBLEN_M 		0x0F
+#define	CSD_3_R2W_F_S 		26		// [28:26]
+#define	CSD_3_R2W_F_M 		0x07
+#define	CSD_3_MMC_DEF_ECC_S	29		// [30:29] reserved for MMC compatibility
+#define	CSD_3_MMC_DEF_ECC_M	0x03
+#define	CSD_3_WP_GRP_EN_S	31		// [31:31]
+#define	CSD_3_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
+#define	CSD_2_v21_WP_GRP_SIZE_S	0		// [38:32]				
+#define	CSD_2_v21_WP_GRP_SIZE_M	0x7F				
+#define	CSD_2_v21_SECT_SIZE_S	7		// [45:39]
+#define	CSD_2_v21_SECT_SIZE_M	0x7F
+#define	CSD_2_v21_ER_BLEN_EN_S	14		// [46:46]
+#define	CSD_2_v21_ER_BLEN_EN_M	0x01
+
+#define	CSD_2_v22_WP_GRP_SIZE_S	0		// [36:32]				
+#define	CSD_2_v22_WP_GRP_SIZE_M	0x1F				
+#define	CSD_2_v22_ER_GRP_SIZE_S	5		// [41:37]
+#define	CSD_2_v22_ER_GRP_SIZE_M	0x1F
+#define	CSD_2_v22_SECT_SIZE_S	10		// [46:42]
+#define	CSD_2_v22_SECT_SIZE_M	0x1F
+
+#define	CSD_2_C_SIZE_M_S	15		// [49:47]
+#define	CSD_2_C_SIZE_M_M	0x07
+#define	CSD_2_VDD_WMAX_S	18		// [52:50]
+#define	CSD_2_VDD_WMAX_M	0x07
+#define	CSD_2_VDD_WMIN_S 	21		// [55:53]
+#define	CSD_2_VDD_WMIN_M	0x07
+#define	CSD_2_RCUR_MAX_S	24		// [58:56]
+#define	CSD_2_RCUR_MAX_M	0x07
+#define	CSD_2_RCUR_MIN_S	27		// [61:59]
+#define	CSD_2_RCUR_MIN_M	0x07
+#define	CSD_2_CSIZE_L_S		30		// [63:62] <=> 2 LSB of CSIZE
+#define	CSD_2_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
+#define	CSD_1_CSIZE_H_S		0	// [73:64]	<=> 10 MSB of CSIZE
+#define	CSD_1_CSIZE_H_M		0x03FF
+// reserved			10		// [75:74]
+// reserved			0x03		
+#define	CSD_1_DSR_I_S 		12		// [76:76]
+#define	CSD_1_DSR_I_M 		0x01
+#define	CSD_1_RD_B_MIS_S	13		// [77:77]
+#define	CSD_1_RD_B_MIS_M	0x01
+#define	CSD_1_WR_B_MIS_S	14		// [78:78]
+#define	CSD_1_WR_B_MIS_M	0x01
+#define	CSD_1_RD_B_PAR_S	15		// [79:79]
+#define	CSD_1_RD_B_PAR_M	0x01
+#define	CSD_1_RD_B_LEN_S	16		// [83:80]
+#define	CSD_1_RD_B_LEN_M	0x0F
+#define	CSD_1_CCC_S		20		// [95:84]
+#define	CSD_1_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
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+#define	CSD_0_TRANS_SPEED_S 	0		// [103:96]
+#define	CSD_0_TRANS_SPEED_M 	0xFF
+#define	CSD_0_NSAC_S		8		// [111:104]
+#define	CSD_0_NSAC_M		0xFF
+#define	CSD_0_TAAC_S		16		// [119:112]
+#define	CSD_0_TAAC_M 		0xFF
+//	reserved		24		// [121:120]
+//	reserved		0x03
+#define	CSD_0_MMC_SPEC_VERS_S	26		// [125:122]	reserved for MMC compatibility
+#define	CSD_0_MMC_SPEC_VERS_M	0x0F
+#define	CSD_0_STRUCT_S		30	// [127:126]
+#define	CSD_0_STRUCT_M 		0x03
+
+///////////////////////////////////////////////////////////////////////////////
 
 void 				AT91F_MCI_Device_Handler(AT91PS_MciDevice,unsigned int);
 AT91S_MCIDeviceStatus 	AT91F_MCI_SDCard_Init (AT91PS_MciDevice);
diff --git a/sys/boot/arm/at91/libat91/memcmp.c b/sys/boot/arm/at91/libat91/memcmp.c
new file mode 100644
index 0000000..a9bd659
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/memcmp.c
@@ -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$
+ */
+#include "lib.h"
+
+int
+p_memcmp(const char *to, const char *from, unsigned size)
+{
+	while ((--size) && (*to++ == *from++))
+		continue;
+
+	return (*to != *from);
+}
diff --git a/sys/boot/arm/at91/libat91/memcpy.c b/sys/boot/arm/at91/libat91/memcpy.c
new file mode 100644
index 0000000..680de5e
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/memcpy.c
@@ -0,0 +1,39 @@
+/*-
+ * 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 "lib.h"
+
+void
+memcpy(void *dst, const void *src, unsigned len)
+{
+    const char *s = src;
+    char *d = dst;
+
+    while (len--)
+        *d++ = *s++;
+}
diff --git a/sys/boot/arm/at91/libat91/memset.c b/sys/boot/arm/at91/libat91/memset.c
new file mode 100644
index 0000000..1f3e890
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/memset.c
@@ -0,0 +1,36 @@
+/*-
+ * 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 "lib.h"
+
+void
+p_memset(char *buffer, char value, int size)
+{
+	while (size--)
+		*buffer++ = value;
+}
diff --git a/sys/boot/arm/at91/libat91/p_string.c b/sys/boot/arm/at91/libat91/p_string.c
index 6d38e4f..8e7bbab 100644
--- a/sys/boot/arm/at91/libat91/p_string.c
+++ b/sys/boot/arm/at91/libat91/p_string.c
@@ -27,41 +27,6 @@
 
 /*
  * .KB_C_FN_DEFINITION_START
- * int p_IsWhiteSpace(char)
- *  This global function returns true if the character is not considered
- * a non-space character.
- * .KB_C_FN_DEFINITION_END
- */
-int
-p_IsWhiteSpace(char cValue)
-{
-	return ((cValue == ' ') ||
-		(cValue == '\t') ||
-		(cValue == 0) ||
-		(cValue == '\r') ||
-		(cValue == '\n'));
-}
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * unsigned p_HexCharValue(char)
- *  This global function returns the decimal value of the validated hex char.
- * .KB_C_FN_DEFINITION_END
- */
-unsigned
-p_HexCharValue(char cValue)
-{
-	if (cValue < ('9' + 1))
-		return (cValue - '0');
-	if (cValue < ('F' + 1))
-		return (cValue - 'A' + 10);
-	return (cValue - 'a' + 10);
-}
-
-
-/*
- * .KB_C_FN_DEFINITION_START
  * void p_memset(char *buffer, char value, int size)
  *  This global function sets memory at the pointer for the specified
  * number of bytes to value.
@@ -74,104 +39,6 @@ p_memset(char *buffer, char value, int size)
 		*buffer++ = value;
 }
 
-
-/*
- * .KB_C_FN_DEFINITION_START
- * int p_strlen(char *)
- *  This global function returns the number of bytes starting at the pointer
- * before (not including) the string termination character ('/0').
- * .KB_C_FN_DEFINITION_END
- */
-int
-p_strlen(const char *buffer)
-{
-	int	len = 0;
-	if (buffer)
-		while (buffer[len])
-			len++;
-	return (len);
-}
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * char *p_strcpy(char *to, char *from)
- *  This global function returns a pointer to the end of the destination string
- * after the copy operation (after the '/0').
- * .KB_C_FN_DEFINITION_END
- */
-char *
-p_strcpy(char *to, const char *from)
-{
-	while (*from)
-		*to++ = *from++;
-	*to++ = '\0';
-	return (to);
-}
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * unsigned p_ASCIIToHex(char *)
- *  This global function set the unsigned value equal to the converted
- * hex number passed as a string.  No error checking is performed; the
- * string must be valid hex value, point at the start of string, and be
- * NULL-terminated.
- * .KB_C_FN_DEFINITION_END
- */
-unsigned
-p_ASCIIToHex(const char *buf)
-{
-	unsigned	lValue = 0;
-
-	if ((*buf == '0') && ((buf[1] == 'x') || (buf[1] == 'X')))
-		buf += 2;
-
-	while (*buf) {
-		lValue <<= 4;
-		lValue += p_HexCharValue(*buf++);
-	}
-	return (lValue);
-}
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * unsigned p_ASCIIToDec(char *)
- *  This global function set the unsigned value equal to the converted
- * decimal number passed as a string.  No error checking is performed; the
- * string must be valid decimal value, point at the start of string, and be
- * NULL-terminated.
- * .KB_C_FN_DEFINITION_END
- */
-unsigned
-p_ASCIIToDec(const char *buf)
-{
-	unsigned v = 0;
-
-	while (*buf) {
-		v *= 10;
-		v += (*buf++) - '0';
-	}
-	return (v);
-}
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * void p_memcpy(char *, char *, unsigned)
- *  This global function copies data from the first pointer to the second
- * pointer for the specified number of bytes.
- * .KB_C_FN_DEFINITION_END
- */
-void
-p_memcpy(char *to, const char *from, unsigned size)
-{
-	while (size--)
-		*to++ = *from++;
-}
-
-
 /*
  * .KB_C_FN_DEFINITION_START
  * int p_memcmp(char *to, char *from, unsigned size)
@@ -188,23 +55,3 @@ p_memcmp(const char *to, const char *from, unsigned size)
 
 	return (*to != *from);
 }
-
-
-/*
- * .KB_C_FN_DEFINITION_START
- * int p_strcmp(char *to, char *from)
- *  This global function compares string at to against string at from.
- * Returns 0 if the locations are equal.
- * .KB_C_FN_DEFINITION_END
- */
-int
-p_strcmp(const char *to, const char *from)
-{
-
-	while (*to && *from && (*to == *from)) {
-		++to;
-		++from;
-	}
-
-	return (!((!*to) && (*to == *from)));
-}
diff --git a/sys/boot/arm/at91/libat91/printf.c b/sys/boot/arm/at91/libat91/printf.c
index 81b48c5..f2622b8 100644
--- a/sys/boot/arm/at91/libat91/printf.c
+++ b/sys/boot/arm/at91/libat91/printf.c
@@ -36,12 +36,13 @@ printf(const char *fmt,...)
 			c = *fmt++;
 			switch (c) {
 			case 'c':
-				putchar(va_arg(ap, int));
+				xputchar(va_arg(ap, int));
 				continue;
 			case 's':
 				for (s = va_arg(ap, char *); *s; s++)
-					putchar(*s);
+					xputchar(*s);
 				continue;
+			case 'd':	/* A lie, always prints unsigned */
 			case 'u':
 				u = va_arg(ap, unsigned);
 				s = buf;
@@ -50,7 +51,7 @@ printf(const char *fmt,...)
 				while (u /= 10U);
 			dumpbuf:;
 				while (--s >= buf)
-					putchar(*s);
+					xputchar(*s);
 				continue;
 			case 'x':
 				u = va_arg(ap, unsigned);
@@ -61,7 +62,7 @@ printf(const char *fmt,...)
 				goto dumpbuf;
 			}
 		}
-		putchar(c);
+		xputchar(c);
 	}
 	va_end(ap);
 
diff --git a/sys/boot/arm/at91/libat91/putchar.c b/sys/boot/arm/at91/libat91/putchar.c
index 137c2e3..2b805c9 100644
--- a/sys/boot/arm/at91/libat91/putchar.c
+++ b/sys/boot/arm/at91/libat91/putchar.c
@@ -52,3 +52,11 @@ putchar(int ch)
 		continue;
 	pUSART->US_THR = (ch & 0xFF);
 }
+
+void
+xputchar(int ch)
+{
+    if (ch == '\n')
+	putchar('\r');
+    putchar(ch);
+}
diff --git a/sys/boot/arm/at91/libat91/sd-card.c b/sys/boot/arm/at91/libat91/sd-card.c
index ae1f01f..ef17520 100644
--- a/sys/boot/arm/at91/libat91/sd-card.c
+++ b/sys/boot/arm/at91/libat91/sd-card.c
@@ -83,12 +83,6 @@ AT91F_MCIDeviceWaitReady(unsigned int 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();
 }
 
@@ -96,44 +90,11 @@ AT91F_MCIDeviceWaitReady(unsigned int timeout)
 int
 MCI_write (unsigned dest, char* source, unsigned length)
 {
-	unsigned sectorLength = MCI_Device.pMCI_DeviceFeatures->Max_Read_DataBlock_Lenfgth;
+	unsigned sectorLength = 1 << MCI_Device.pMCI_DeviceFeatures->WRITE_BL_LEN;
 	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)
 	{
@@ -169,87 +130,28 @@ swap(unsigned int a)
 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;
+	unsigned sectorLength = 1 << log2sl;
 	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
+		//Do the reading
+		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
+		if (status != AT91C_READ_OK)
 			return -1;
-		}
 
 		//* Wait MCI Device Ready
 		AT91F_MCIDeviceWaitReady(AT91C_MCI_TIMEOUT);
@@ -277,20 +179,19 @@ 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;
-
+	MCI_Device_Features.Relative_Card_Address = 0;
+	MCI_Device_Features.Card_Inserted 	= AT91C_SD_CARD_INSERTED;
+	MCI_Device_Features.READ_BL_LEN		= 0;
+	MCI_Device_Features.WRITE_BL_LEN	= 0;
+	MCI_Device_Features.Read_Partial 	= 0;
+	MCI_Device_Features.Write_Partial 	= 0;
+	MCI_Device_Features.Erase_Block_Enable 	= 0;
+	MCI_Device_Features.Sector_Size 	= 0;
+	MCI_Device_Features.Memory_Capacity 	= 0;
+	MCI_Device_Desc.state			= AT91C_MCI_IDLE;
+	MCI_Device_Desc.SDCard_bus_width	= AT91C_MCI_SCDBUS;
+	MCI_Device.pMCI_DeviceDesc 		= &MCI_Device_Desc;
+	MCI_Device.pMCI_DeviceFeatures 		= &MCI_Device_Features;
 }
 
 static void AT91F_MCI_Handler(void)
@@ -300,7 +201,7 @@ static void AT91F_MCI_Handler(void)
 //	status = ( AT91C_BASE_MCI->MCI_SR & AT91C_BASE_MCI->MCI_IMR );
 	status = AT91C_BASE_MCI->MCI_SR;
 
-	AT91F_MCI_Device_Handler(&MCI_Device,status);
+	AT91F_MCI_Device_Handler(&MCI_Device, status);
 }
 
 //*----------------------------------------------------------------------------
@@ -326,7 +227,7 @@ sdcard_init(void)
 
 	AT91F_MCI_Configure(AT91C_BASE_MCI,
 	    AT91C_MCI_DTOR_1MEGA_CYCLES,
-	    AT91C_MCI_MR_PDCMODE,			// 15MHz for MCK = 60MHz (CLKDIV = 1)
+	    AT91C_MCI_PDCMODE,
 	    AT91C_MCI_SDCARD_4BITS_SLOTA);
 	
 	if (AT91F_MCI_SDCard_Init(&MCI_Device) != AT91C_INIT_OK)
diff --git a/sys/boot/arm/at91/libat91/spi_flash.c b/sys/boot/arm/at91/libat91/spi_flash.c
index 3df2e91..78804b5 100644
--- a/sys/boot/arm/at91/libat91/spi_flash.c
+++ b/sys/boot/arm/at91/libat91/spi_flash.c
@@ -234,5 +234,5 @@ SPI_InitFlash(void)
 	AT91C_BASE_ST->ST_RTMR = 1;
 
 	if (((value = GetFlashStatus()) & 0xFC) != 0xBC)
-		printf(" Bad SPI status: 0x%x\r\n", value);
+		printf(" Bad SPI status: 0x%x\n", value);
 }
diff --git a/sys/boot/arm/at91/libat91/strcmp.c b/sys/boot/arm/at91/libat91/strcmp.c
new file mode 100644
index 0000000..f04d1f7
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/strcmp.c
@@ -0,0 +1,36 @@
+/*-
+ * 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 "lib.h"
+
+int
+strcmp(const char *s1, const char *s2)
+{
+    for (; *s1 == *s2 && *s1; s1++, s2++);
+    return (unsigned char)*s1 - (unsigned char)*s2;
+}
diff --git a/sys/boot/arm/at91/libat91/strcpy.c b/sys/boot/arm/at91/libat91/strcpy.c
new file mode 100644
index 0000000..c31a94e
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/strcpy.c
@@ -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$
+ */
+#include "lib.h"
+
+char *
+strcpy(char *to, const char *from)
+{
+	while (*from)
+		*to++ = *from++;
+	*to++ = '\0';
+	return (to);
+}
diff --git a/sys/boot/arm/at91/libat91/strcvt.c b/sys/boot/arm/at91/libat91/strcvt.c
new file mode 100644
index 0000000..d1e385c
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/strcvt.c
@@ -0,0 +1,133 @@
+/*-
+ * 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: p_string.c
+ *
+ * Instantiation of basic string operations to prevent inclusion of full
+ * string library.  These are simple implementations not necessarily optimized
+ * for speed, but rather to show intent.
+ *
+ * Revision information:
+ *
+ * 20AUG2004	kb_admin	initial creation
+ * 12JAN2005	kb_admin	minor updates
+ *
+ * BEGIN_KBDD_BLOCK
+ * No warranty, expressed or implied, is included with this software.  It is
+ * provided "AS IS" and no warranty of any kind including statutory or aspects
+ * relating to merchantability or fitness for any purpose is provided.  All
+ * intellectual property rights of others is maintained with the respective
+ * owners.  This software is not copyrighted and is intended for reference
+ * only.
+ * END_BLOCK
+ *
+ *****************************************************************************/
+
+#include "lib.h"
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * int p_IsWhiteSpace(char)
+ *  This global function returns true if the character is not considered
+ * a non-space character.
+ * .KB_C_FN_DEFINITION_END
+ */
+int
+p_IsWhiteSpace(char cValue)
+{
+	return ((cValue == ' ') ||
+		(cValue == '\t') ||
+		(cValue == 0) ||
+		(cValue == '\r') ||
+		(cValue == '\n'));
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * unsigned p_HexCharValue(char)
+ *  This global function returns the decimal value of the validated hex char.
+ * .KB_C_FN_DEFINITION_END
+ */
+unsigned
+p_HexCharValue(char cValue)
+{
+	if (cValue < ('9' + 1))
+		return (cValue - '0');
+	if (cValue < ('F' + 1))
+		return (cValue - 'A' + 10);
+	return (cValue - 'a' + 10);
+}
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * unsigned p_ASCIIToHex(char *)
+ *  This global function set the unsigned value equal to the converted
+ * hex number passed as a string.  No error checking is performed; the
+ * string must be valid hex value, point at the start of string, and be
+ * NULL-terminated.
+ * .KB_C_FN_DEFINITION_END
+ */
+unsigned
+p_ASCIIToHex(const char *buf)
+{
+	unsigned	lValue = 0;
+
+	if ((*buf == '0') && ((buf[1] == 'x') || (buf[1] == 'X')))
+		buf += 2;
+
+	while (*buf) {
+		lValue <<= 4;
+		lValue += p_HexCharValue(*buf++);
+	}
+	return (lValue);
+}
+
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * unsigned p_ASCIIToDec(char *)
+ *  This global function set the unsigned value equal to the converted
+ * decimal number passed as a string.  No error checking is performed; the
+ * string must be valid decimal value, point at the start of string, and be
+ * NULL-terminated.
+ * .KB_C_FN_DEFINITION_END
+ */
+unsigned
+p_ASCIIToDec(const char *buf)
+{
+	unsigned v = 0;
+
+	while (*buf) {
+		v *= 10;
+		v += (*buf++) - '0';
+	}
+	return (v);
+}
diff --git a/sys/boot/arm/at91/libat91/strlen.c b/sys/boot/arm/at91/libat91/strlen.c
new file mode 100644
index 0000000..227e516
--- /dev/null
+++ b/sys/boot/arm/at91/libat91/strlen.c
@@ -0,0 +1,46 @@
+/*-
+ * 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 "lib.h"
+
+/*
+ * .KB_C_FN_DEFINITION_START
+ * int p_strlen(char *)
+ *  This global function returns the number of bytes starting at the pointer
+ * before (not including) the string termination character ('/0').
+ * .KB_C_FN_DEFINITION_END
+ */
+int
+p_strlen(const char *buffer)
+{
+	const char *ptr = buffer;
+	while (*ptr++)
+		continue;
+	return (ptr - buffer);
+}