path: root/recipes/linux/linux-2.6.34/ts72xx/0015-ts72xx_sdcard.patch

From 3da21c5a4384f9853fd75cb9fed204402072d280 Mon Sep 17 00:00:00 2001
From: Matthieu Crapet <mcrapet@gmail.com>
Date: Sun, 20 Jun 2010 10:46:15 +0200
Subject: [PATCH 15/18] ts72xx_sdcard

SD Card support for TS-7260. Device name is "tssda".
Patch based on work of Breton Saunders:
http://tech.groups.yahoo.com/group/ts-7000/message/15787
http://tech.groups.yahoo.com/group/ts-7000/message/16028
---
 arch/arm/mach-ep93xx/ts72xx.c |   24 +
 drivers/block/Kconfig         |    7 +
 drivers/block/Makefile        |    2 +
 drivers/block/sdcore2.c       | 2391 +++++++++++++++++++++++++++++++++++++++++
 drivers/block/sdcore2.h       |  372 +++++++
 drivers/block/tssdcard.c      |  415 +++++++
 6 files changed, 3211 insertions(+), 0 deletions(-)
 create mode 100644 drivers/block/sdcore2.c
 create mode 100644 drivers/block/sdcore2.h
 create mode 100644 drivers/block/tssdcard.c

diff --git a/arch/arm/mach-ep93xx/ts72xx.c b/arch/arm/mach-ep93xx/ts72xx.c
index ba27d9d..7fd8f80 100644
--- a/arch/arm/mach-ep93xx/ts72xx.c
+++ b/arch/arm/mach-ep93xx/ts72xx.c
@@ -178,6 +178,29 @@ static void __init ts72xx_register_flash(void)
 }
 
 /*************************************************************************
+ * SD Card (TS-7260 only)
+ *************************************************************************/
+
+static struct resource ts72xx_sdcard_resource = {
+	.start		= TS7260_SDCARD_PHYS_BASE,
+	.end		= TS7260_SDCARD_PHYS_BASE + 0x20,
+	.flags		= IORESOURCE_MEM,
+};
+
+static struct platform_device ts72xx_sdcard = {
+	.name		= "ts72xx-sdcard",
+	.id		= 0,
+	.num_resources	= 1,
+	.resource	= &ts72xx_sdcard_resource,
+};
+
+static void __init ts72xx_register_sdcard(void)
+{
+	if (board_is_ts7260())
+		platform_device_register(&ts72xx_sdcard);
+}
+
+/*************************************************************************
  * RTC
  *************************************************************************/
 static unsigned char ts72xx_rtc_readbyte(unsigned long addr)
@@ -278,6 +301,7 @@ static void __init ts72xx_init_machine(void)
 {
 	ep93xx_init_devices();
 	ts72xx_register_flash();
+	ts72xx_register_sdcard();
 	platform_device_register(&ts72xx_rtc_device);
 	platform_device_register(&ts72xx_wdt_device);
 
diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
index 77bfce5..fc8fae5 100644
--- a/drivers/block/Kconfig
+++ b/drivers/block/Kconfig
@@ -488,4 +488,11 @@ config BLK_DEV_HD
 
 	  If unsure, say N.
 
+config BLK_DEV_TS72XX_SDCARD
+	tristate "TS-72XX SD Card support"
+	depends on ARCH_EP93XX && MACH_TS72XX
+	help
+	  This option enables support SD Card control on Technologic Systems
+	  TS-7260 SBC.
+
 endif # BLK_DEV
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
index aff5ac9..a4d0579 100644
--- a/drivers/block/Makefile
+++ b/drivers/block/Makefile
@@ -34,8 +34,10 @@ obj-$(CONFIG_VIODASD)		+= viodasd.o
 obj-$(CONFIG_BLK_DEV_SX8)	+= sx8.o
 obj-$(CONFIG_BLK_DEV_UB)	+= ub.o
 obj-$(CONFIG_BLK_DEV_HD)	+= hd.o
+obj-$(CONFIG_BLK_DEV_TS72XX_SDCARD)	+= ts72xx_sdcard.o
 
 obj-$(CONFIG_XEN_BLKDEV_FRONTEND)	+= xen-blkfront.o
 obj-$(CONFIG_BLK_DEV_DRBD)     += drbd/
 
 swim_mod-objs	:= swim.o swim_asm.o
+ts72xx_sdcard-objs	:= tssdcard.o sdcore2.o
diff --git a/drivers/block/sdcore2.c b/drivers/block/sdcore2.c
new file mode 100644
index 0000000..6dadee1
--- /dev/null
+++ b/drivers/block/sdcore2.c
@@ -0,0 +1,2391 @@
+/*
+ * Copyright (c) 2006-2009, Technologic Systems
+ * All rights reserved.
+ */
+
+/*
+ * This code is 100% operating system/CPU independent-- not a single global
+ * reference, external symbol, or #include is required.  Centric upon one data
+ * structure "struct sdcore".  OS-specific callbacks for things like DMA
+ * acceleration and sleeping are defined by function pointers to OS-specific
+ * code in the struct sdcore.  Minimally requires the os_sleep() callback to be
+ * implemented for proper SD card initialization and a pointer to start
+ * of SD card registers.  Auto-determines TS SD core version.  All other
+ * callback functions may be left NULL-- they are only to allow speed/CPU
+ * utilization improvements.
+ *
+ * 3 main public functions - sdreset(), sdread() and sdwrite().  sdreset()
+ * returns card size.  OS specific shim is required to turn this
+ * simple API into the complicated, constantly changing, hacker "designed"
+ * horrible excuses for abstraction, block driver APIs required in other
+ * "modern" operating systems.
+ *
+ * Not all SD cards over the years have followed spec perfectly -- many
+ * don't even check CRC's on the CMD or DAT busses and some have problems
+ * (lock up) when reading/writing the last sectors with SD read/write multiple
+ * commands.
+ *
+ * The TS SD hardware cores are not much more than GPIO bit-bang cores with
+ * a few well-placed hardware optimizations to achieve reasonable
+ * performance goals.  In the roughly 2000 lines of code that follow, there
+ * is support for all distinct TS hardware SD cores on PPC and ARM platforms,
+ * a generic (private) SD  command layer, sdcmd(), and SD flash card
+ * (public) routines for initialization + read/write + some SD security
+ * features.
+ *
+ */
+
+/* Register offset definitions.  TS-SDCORE is 4 regs total. */
+#define SDCMD		0
+#define SDGPIO		0	/* version 2 register */
+#define SDDAT		1
+#define SDSTAT2		1
+#define SDSTATE		2
+#define SDCTRL		3
+#define SDDAT2		4
+#define SDCMD2		8
+#define SDCTRL2		12
+#define SDLUN2		2
+
+struct sdcore {
+	/* virtual address of SD block register start, to be filled in
+	 * by client code before calling any sdcore functions.
+	 */
+	unsigned int sd_regstart;
+
+	/* public bits for sd_state bitfield, can be read from client code.
+	 * Do not write!  Other bits are used internally.
+	 */
+	#define SDDAT_RX	(1<<0)
+	#define SDDAT_TX	(1<<1)
+	#define SDCMD_RX	(1<<2)
+	#define SDCMD_TX	(1<<3)
+	unsigned int sd_state;
+
+	/* Erase hint for subsequent sdwrite() call, used to optimize
+	 * write throughput on multi-sector writes by pre-erasing this
+	 * many sectors. XXX: this doesn't have much benefit on most SDs
+	 */
+	unsigned int sd_erasehint;
+
+	/* Following this comment are 3 function pointer declarations to
+	 * OS helper functions.  The 'os_arg' member is passed as the
+	 * first argument to the helpers and should be set by
+	 * client code before issueing sdreset()
+	 *
+	 * os_dmastream(os_arg, buf, buflen)
+	 * This function should look at sd_state and set up and run an
+	 * appropriate DMA transfer.  If buf is NULL, callee doesn't care
+	 * about the actual data sent/received and helper function
+	 * can do whatever it wants.  Should return 0 when DMA transfer was
+	 * run and completed successfully.  If this function pointer is
+	 * NULL, PIO methods of transfer will be used instead of DMA.
+	 *
+	 * os_dmaprep(os_arg, buf, buflen)
+	 * This function is used to prepare an area of memory for a possible
+	 * DMA transfer.  This function is called once per distinct buffer
+	 * passed in.  After this function is called, os_dmastream() may be
+	 * called one or more times (for sequential addresses) on subregions
+	 * of the address range passed here.  Should write-back or invalidate
+	 * L1 cache lines and possibly look up physical addresses for buf
+	 * passed in if I/O buffers.  If 'os_dmaprep' is set to NULL, function
+	 * call will not happen. (though os_dmastream() calls may still)
+	 *
+	 * os_delay(os_arg, microseconds)
+	 * This function is supposed to delay or stall the processor for
+	 * the passed in value number of microseconds.
+	 */
+	void *os_arg;
+	int (*os_dmastream)(void *, unsigned char *, unsigned int);
+	void (*os_dmaprep)(void *, unsigned char *, unsigned int);
+	void (*os_delay)(void *, unsigned int);
+	void (*os_irqwait)(void *, unsigned int);
+	int (*os_powerok)(void *);
+	int (*os_timeout)(void *);
+	int (*os_reset_timeout)(void *);
+
+	/* If the SD card last successfully reset is write protected, this
+	 * member will be non-zero.
+	 */
+	unsigned int sd_wprot;
+
+	/* If this card may have been already initialized by TS-SDBOOT, place
+	 * the magic token it placed in the EP93xx SYSCON ScratchReg1 here
+	 * to avoid re-initialization.
+	 */
+	unsigned int sdboot_token;
+
+	/* CRC hint for subsequent sdwrite() call, used to optimize
+	 * write throughput while using DMA by pre-calculating CRC's for
+	 * next write
+	 */
+	unsigned char *sd_crchint;
+
+	/* The block size of the memory device.  Normally 512, but can be 1024
+	 * for larger cards
+	 */
+	unsigned int sd_blocksize;
+
+	/* Password for auto-unlocking in sdreset()
+	 */
+	unsigned char *sd_pwd;
+
+	/* If the SD card was password locked, this will be non-zero.
+	 */
+	unsigned int sd_locked;
+
+	/* Whether or not writes can be parked.
+	 */
+	unsigned int sd_writeparking;
+
+	/* Logical unit number.  Some SD cores will have multiple card slots.
+	 */
+	unsigned int sd_lun;
+
+	/* The rest of these members are for private internal use and should
+	 * not be of interest to client code.
+	 */
+	unsigned int sd_rcaarg;
+	unsigned int sd_csd[17];
+	unsigned int sd_crcseq;
+	unsigned short sd_crcs[4];
+	unsigned int sd_crctmp[4];
+	unsigned int sd_timeout;
+	unsigned int parked_sector;
+	unsigned int hw_version;
+	unsigned char sd_scr[8];
+	unsigned int sd_sz;
+};
+
+/* For sdreadv() / sdwritev() */
+struct sdiov {
+	unsigned char *sdiov_base;
+	unsigned int sdiov_nsect;
+};
+
+int sdreset(struct sdcore *);
+int sdread(struct sdcore *, unsigned int, unsigned char *, int);
+int sdwrite(struct sdcore *, unsigned int, unsigned char *, int);
+int sdreadv(struct sdcore *, unsigned int, struct sdiov *, int);
+int sdwritev(struct sdcore *, unsigned int, struct sdiov *, int);
+int sdsetwprot(struct sdcore *, unsigned int);
+#define SDLOCK_UNLOCK	0
+#define SDLOCK_SETPWD	1
+#define SDLOCK_CLRPWD	2
+#define SDLOCK_ERASE	8
+#ifndef SD_NOLOCKSUPPORT
+int sdlockctl(struct sdcore *, unsigned int, unsigned char *, unsigned char *);
+#endif
+
+/*
+ * Everything below here is secret!  This code shouldn't have to change
+ * even for different OS.
+ */
+
+const static unsigned short crc16tbl[256] = {
+	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+	0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+	0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+	0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+	0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+	0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+	0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+	0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+	0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+	0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+	0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+	0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+	0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+	0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+	0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+	0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+	0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+	0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+	0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+	0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+	0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+	0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+	0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+	0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+	0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+	0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+	0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+	0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+	0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+	0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+	0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+	0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0,
+};
+
+const static unsigned char destagger[256] = {
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+	0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+	2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3, 2, 3,
+};
+
+#ifndef MAX_SDCORES
+#define MAX_SDCORES 64
+#endif
+static struct sdcore *sdcores[MAX_SDCORES];
+
+static unsigned int crc7(unsigned int, const unsigned int *, unsigned int);
+static int sdreset2(struct sdcore *);
+static int version(struct sdcore *);
+static int sdfastinit(struct sdcore *sd);
+static int sdcmd2(struct sdcore *, unsigned short, unsigned int,
+  unsigned int *, unsigned char **);
+static int sdcmd(struct sdcore *, unsigned short, unsigned int,
+  unsigned int *, unsigned char **);
+static void mkcommand(unsigned int, unsigned int, unsigned int *);
+static int stop(struct sdcore *);
+static int stop2(struct sdcore *);
+static int sdread2(struct sdcore *, unsigned int, unsigned char *, int)
+  __attribute__ ((unused));
+static int do_read2(struct sdcore *, unsigned int, struct sdiov *,
+  unsigned int);
+static int do_read(struct sdcore *, unsigned int, struct sdiov *,
+  unsigned int);
+static int do_write(struct sdcore *, unsigned int, struct sdiov *,
+  unsigned int);
+static int do_write2(struct sdcore *, unsigned int, struct sdiov *,
+  unsigned int);
+static int sdsetwprot2(struct sdcore *, unsigned int);
+#ifndef SD_NOLOCKSUPPORT
+static int sdlockctl2(struct sdcore *, unsigned int, unsigned char *,
+  unsigned char *);
+#endif
+
+#ifndef SDPOKE8
+# define SDPOKE8(sd, x, y)	\
+  *(volatile unsigned char *)((sd)->sd_regstart + (x)) = (y)
+#endif
+#ifndef SDPOKE32
+# define SDPOKE32(sd, x, y)	\
+  *(volatile unsigned int *)((sd)->sd_regstart + (x)) = (y)
+#endif
+#ifndef SDPOKE16
+# define SDPOKE16(sd, x, y)	\
+  *(volatile unsigned short *)((sd)->sd_regstart + (x)) = (y)
+#endif
+#ifndef SDPEEK8
+# define SDPEEK8(sd, x)	*(volatile unsigned char *)((sd)->sd_regstart + (x))
+#endif
+#ifndef SDPEEK32
+# define SDPEEK32(sd, x)	*(volatile unsigned int *)((sd)->sd_regstart + (x))
+#endif
+#ifndef SDPEEK16
+# define SDPEEK16(sd, x)	*(volatile unsigned short *)((sd)->sd_regstart + (x))
+#endif
+
+#define S_DUMMY_CLK	0
+#define S_SEND_CMD	1
+#define S_WAIT_RESP	2
+#define S_RX_RESP	3
+#define S_WAIT_BUSY	4
+#define S_TX_WRITE	5
+#define S_CRC_CHECK	6
+#define S_OFF		7
+
+#define TYPE_SHORTRESP	2
+#define TYPE_LONGRESP	3
+#define TYPE_BSYRESP	4
+#define TYPE_NORESP	1
+#define TYPE_RXDAT	0
+#define TYPE_TXDAT	5
+#define TYPE_ABORT	6
+#define TYPE_RXDAT_IGNRESP	7
+
+#define CMD(idx, type)	(0x40 | (idx) | ((type)<<8))
+
+#define CMD_GO_IDLE_STATE		CMD(0, TYPE_NORESP)
+#define CMD_ALL_SEND_CID		CMD(2, TYPE_LONGRESP)
+#define CMD_SEND_RELATIVE_ADDR		CMD(3, TYPE_SHORTRESP)
+#define CMD_SWITCH_FUNC			CMD(6, TYPE_RXDAT)
+#define CMD_SWITCH_FUNC2		CMD(6, TYPE_RXDAT_IGNRESP)
+#define CMD_SELECT_CARD			CMD(7, TYPE_BSYRESP)
+#define CMD_DESELECT_CARD		CMD(7, TYPE_NORESP)
+#define CMD_SEND_IF_COND		CMD(8, TYPE_SHORTRESP)
+#define CMD_SEND_CSD			CMD(9, TYPE_LONGRESP)
+#define CMD_PROGRAM_CSD			CMD(27, TYPE_TXDAT)
+#define CMD_SET_BLOCKLEN		CMD(16, TYPE_SHORTRESP)
+#define CMD_LOCK_UNLOCK			CMD(42, TYPE_TXDAT)
+#define CMD_APP_CMD			CMD(55, TYPE_SHORTRESP)
+#define CMD_READ_SINGLE_BLOCK		CMD(17, TYPE_RXDAT)
+#define CMD_READ_MULTIPLE_BLOCK		CMD(18, TYPE_RXDAT)
+#define CMD_READ_MULTIPLE_BLOCK2	CMD(18, TYPE_RXDAT_IGNRESP)
+#define CMD_STOP_TRANSMISSION		CMD(12, TYPE_ABORT)
+#define CMD_SEND_STATUS			CMD(13, TYPE_SHORTRESP)
+#define CMD_WRITE_BLOCK			CMD(24, TYPE_TXDAT)
+#define CMD_WRITE_MULTIPLE_BLOCK	CMD(25, TYPE_TXDAT)
+
+#define ACMD_SD_SEND_OP_COND		CMD(41, TYPE_SHORTRESP)
+#define ACMD_SET_CLR_CARD_DETECT	CMD(42, TYPE_SHORTRESP)
+#define ACMD_SET_BUS_WIDTH		CMD(6, TYPE_SHORTRESP)
+#define ACMD_SET_WR_BLK_ERASE_COUNT	CMD(23, TYPE_SHORTRESP)
+#define ACMD_SEND_NUM_WR_BLOCKS		CMD(22, TYPE_RXDAT)
+#define ACMD_SEND_SCR			CMD(51, TYPE_RXDAT)
+#define ACMD_SEND_SCR2			CMD(51, TYPE_RXDAT_IGNRESP)
+
+/* Private bits for struct sdcore, sd_state member */
+#define DATSSP_NOCRC		(1<<4)
+#define DATSSP_4BIT		(1<<5)
+#define SD_HC			(1<<6)
+#define SD_HISPEED		(1<<7)
+#define SD_LOSPEED		(1<<8)
+#define SD_SELECTED		(1<<9)
+#define SD_RESET		(1<<10)
+
+#define	NULL			((void *)0)
+
+static void remember_sdcore(struct sdcore *sd) {
+	int i, newlun = 0;
+
+	for (i = 0; i < sizeof(sdcores); i++) {
+		if (sdcores[i] == NULL) {
+			/* new core, first reset */
+			sdcores[i] = sd;
+			/* core was almost definitely power-cycled on prev lun
+			 * sdreset2(), so we don't need to have the sdreset2()
+			 * do it again.
+			 */
+			if (newlun) sd->sd_state = SD_RESET;
+			break;
+		} else if (sdcores[i]->sd_regstart == sd->sd_regstart) {
+			newlun = 1;
+			if (sdcores[i]->sd_lun == sd->sd_lun) {
+				sdcores[i] = sd;
+				break;
+			}
+		}
+	}
+}
+
+static int activate(struct sdcore *sd) {
+	int i;
+
+	/* Are we already selected? */
+	if ((sd->sd_state & (SD_SELECTED|SD_RESET)) == SD_SELECTED)
+	  return 0;
+
+	/* Find currently activated SD slot for this HW core */
+	for (i = 0; i < sizeof(sdcores); i++) {
+		if (sdcores[i] == NULL) break;
+		if (sdcores[i]->sd_regstart == sd->sd_regstart &&
+		  sdcores[i]->sd_state & SD_SELECTED) break;
+	}
+
+	/* Stop whatever parked transfer it has going on. */
+	if (sdcores[i]) {
+		stop2(sdcores[i]);
+		sdcores[i]->sd_state &= ~SD_SELECTED;
+	}
+
+	/* Change clock routing, mark us as selected */
+#ifdef BIGENDIAN
+	SDPOKE16(sd, SDLUN2, sd->sd_lun << 8);
+#else
+	SDPOKE16(sd, SDLUN2, sd->sd_lun);
+#endif
+
+	/* Change clock frequency */
+	if (sd->sd_state & SD_HISPEED) SDPOKE8(sd, SDSTAT2, 0x38);
+	else SDPOKE8(sd, SDSTAT2, 0x18);
+
+	sd->sd_state |= SD_SELECTED;
+	if (sd->sd_state & SD_RESET) return 1;
+	else return 0;
+
+}
+
+inline static unsigned short
+crc16_acc(unsigned short crc, unsigned int b)
+{
+	return (crc << 8) ^ crc16tbl[(crc >> 8) ^ b];
+}
+
+static void sd_initcrc(struct sdcore *sd)
+{
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		sd->sd_crctmp[i] = 0;
+		sd->sd_crcs[i] = 0;
+	}
+	sd->sd_crcseq = 6;
+}
+
+static void sd_1bit_feedcrc(struct sdcore *sd, unsigned int dat)
+{
+	sd->sd_crcs[0] = crc16_acc(sd->sd_crcs[0], dat);
+}
+
+static void sd_4bit_feedcrc(struct sdcore *sd, unsigned int dat)
+{
+	unsigned int a = 0, b = 0, c = 0, d = 0;
+	unsigned int shift = (sd->sd_crcseq & 0x7);
+
+	a = sd->sd_crctmp[0];
+	b = sd->sd_crctmp[1];
+	c = sd->sd_crctmp[2];
+	d = sd->sd_crctmp[3];
+
+	a |= destagger[dat] << shift;
+	dat >>= 1;
+	b |= destagger[dat] << shift;
+	dat >>= 1;
+	c |= destagger[dat] << shift;
+	dat >>= 1;
+	d |= destagger[dat] << shift;
+
+	if (shift == 0) {
+		sd->sd_crcs[0] = crc16_acc(sd->sd_crcs[0], a);
+		sd->sd_crcs[1] = crc16_acc(sd->sd_crcs[1], b);
+		sd->sd_crcs[2] = crc16_acc(sd->sd_crcs[2], c);
+		sd->sd_crcs[3] = crc16_acc(sd->sd_crcs[3], d);
+		a = b = c = d = 0;
+	}
+
+	sd->sd_crcseq -= 2;
+	sd->sd_crctmp[0] = a;
+	sd->sd_crctmp[1] = b;
+	sd->sd_crctmp[2] = c;
+	sd->sd_crctmp[3] = d;
+}
+
+/* This should be called 8 times to get the full 8 bytes of CRC generated */
+static unsigned int sd_4bit_getcrc(struct sdcore *sd)
+{
+	static const unsigned char restaggertbl[4] = { 0x0, 0x1, 0x10, 0x11 };
+	static const unsigned char restaggertbl_lsl1[4] =
+		{ 0x0, 0x2, 0x20, 0x22 };
+	static const unsigned char restaggertbl_lsl2[4] =
+		{ 0x0, 0x4, 0x40, 0x44 };
+	static const unsigned char restaggertbl_lsl3[4] =
+		{ 0x0, 0x8, 0x80, 0x88 };
+	unsigned int ret;
+
+	ret = restaggertbl[sd->sd_crcs[0] >> 14];
+	sd->sd_crcs[0] <<= 2;
+	ret |= restaggertbl_lsl1[sd->sd_crcs[1] >> 14];
+	sd->sd_crcs[1] <<= 2;
+	ret |= restaggertbl_lsl2[sd->sd_crcs[2] >> 14];
+	sd->sd_crcs[2] <<= 2;
+	ret |= restaggertbl_lsl3[sd->sd_crcs[3] >> 14];
+	sd->sd_crcs[3] <<= 2;
+
+	return ret;
+}
+
+/* This should be called 2 times to get the full 2 bytes of CRC generated */
+static unsigned int sd_1bit_getcrc(struct sdcore *sd)
+{
+	unsigned int ret;
+
+	ret = sd->sd_crcs[0] >> 8;
+	sd->sd_crcs[0] = (sd->sd_crcs[0] & 0xff) << 8;
+	return ret;
+}
+
+static inline void datssp_feedcrc(struct sdcore *sd, unsigned int dat)
+{
+	if (!(sd->sd_state & DATSSP_NOCRC)) {
+		if (sd->sd_state & DATSSP_4BIT) sd_4bit_feedcrc(sd, dat);
+		else sd_1bit_feedcrc(sd, dat);
+	}
+}
+
+static inline unsigned int datssp_getcrc(struct sdcore *sd)
+{
+	unsigned int ret = 0;
+
+	if (!(sd->sd_state & DATSSP_NOCRC)) {
+		if (sd->sd_state & DATSSP_4BIT) ret = sd_4bit_getcrc(sd);
+		else ret = sd_1bit_getcrc(sd);
+	}
+	return ret;
+}
+
+static inline unsigned int
+crc7(unsigned int crc, const unsigned int *pc, unsigned int len)
+{
+	unsigned int i;
+	unsigned char ibit;
+	unsigned char c;
+
+	for (i = 0; i < len; i++, pc++) {
+		c = *pc;
+		for (ibit = 0; ibit < 8; ibit++) {
+			crc <<= 1;
+			if ((c ^ crc) & 0x80) crc ^= 0x09;
+
+			c <<= 1;
+		}
+
+		crc &= 0x7F;
+	}
+
+	return crc;
+}
+
+static inline void
+mkcommand(unsigned int cmdidx, unsigned int arg, unsigned int *retcmd)
+{
+	retcmd[0] = cmdidx;
+	retcmd[1] = arg >> 24;
+	retcmd[2] = arg >> 16;
+	retcmd[3] = arg >> 8;
+	retcmd[4] = arg;
+	retcmd[5] = (0x1 | (crc7(0, retcmd, 5) << 1));
+}
+
+static inline void reset_timeout(struct sdcore *sd) {
+	sd->sd_timeout = 0;
+	if (sd->os_reset_timeout) sd->os_reset_timeout(sd);
+}
+
+static inline int timeout(struct sdcore *sd) {
+	if (sd->sd_timeout > 1000000) return 1;
+	else if (sd->os_timeout) return sd->os_timeout(sd);
+	else sd->sd_timeout++;
+	return 0;
+}
+
+static
+unsigned int sdsize(struct sdcore *sd)
+{
+	unsigned int csize, csize_mult, rd_bl_len;
+
+	if (sd->sd_sz != 0) return sd->sd_sz;
+
+	if (sd->sd_csd[1] & 0xc0) {
+		csize = (sd->sd_csd[10] | (sd->sd_csd[9] << 8));
+		sd->sd_sz = (csize + 1) * 1024;
+	} else {
+	        rd_bl_len = 1 << ((sd->sd_csd[6] & 0xf) - 9);
+		csize = ((sd->sd_csd[7] & 0x03) << 10) |
+		  ((sd->sd_csd[8] << 2) | ((sd->sd_csd[9] & 0xc0) >> 6));
+		csize_mult = ((sd->sd_csd[10] & 0x03) << 1) |
+		  ((sd->sd_csd[11] & 0x80) >> 7);
+		sd->sd_sz = (csize + 1) * (1 << (csize_mult + 2)) * rd_bl_len;
+	}
+	return sd->sd_sz;
+}
+
+static unsigned int tend_ssp(struct sdcore *sd, unsigned int **cmdresp,
+  unsigned char **dat) {
+	unsigned int d;
+	unsigned int s = SDPEEK8(sd, SDSTATE);
+
+	if (s & 0x8) {
+		if (sd->sd_state & SDCMD_RX) {
+			d = SDPEEK8(sd, SDCMD);
+			if (cmdresp) {
+				**cmdresp = d;
+				*cmdresp = *cmdresp + 1;
+				reset_timeout(sd);
+			}
+		} else if (sd->sd_state & SDCMD_TX) {
+			SDPOKE8(sd, SDCMD, **cmdresp);
+			*cmdresp = *cmdresp + 1;
+			reset_timeout(sd);
+		}
+	}
+
+	if (s & 0x10) {
+		if (sd->sd_state & SDDAT_RX) {
+			d = SDPEEK8(sd, SDDAT);
+			if (dat) {
+				**dat = d;
+				*dat = *dat + 1;
+				reset_timeout(sd);
+			}
+		} else if (sd->sd_state & SDDAT_TX) {
+			reset_timeout(sd);
+			if (dat) {
+				d = **dat;
+				*dat = *dat + 1;
+				SDPOKE8(sd, SDDAT, d);
+				datssp_feedcrc(sd, d);
+			} else {
+				d = datssp_getcrc(sd);
+				SDPOKE8(sd, SDDAT, d);
+			}
+		}
+	}
+
+	return s;
+}
+
+static int
+error(unsigned int *resp, unsigned short req)
+{
+	unsigned int crc, status;
+
+	if ((req & 0x3f) != resp[0]) return 1;
+
+	crc = (0x1 | (crc7(0, resp, 5) << 1));
+	if (crc != resp[5]) return 1;
+
+	status = resp[1] << 24;
+	status |= resp[2] << 16;
+	status |= resp[3] << 8;
+	status |= resp[4];
+
+	return status & 0xfdf90008;
+}
+
+static int
+sdcmd2(struct sdcore *sd, unsigned short req, unsigned int arg,
+  unsigned int *resp, unsigned char **dat)
+{
+	unsigned int i, j, s, cmdresp[17];
+	unsigned int resplen;
+	unsigned int type = (req >> 8);
+	unsigned int cmdidx = req;
+	unsigned int *cmdptr = cmdresp;
+	unsigned int *respptr;
+	unsigned int dly;
+	int ok32 = (sd->hw_version == 2);
+	int ok16 = (ok32 || (sd->hw_version == 3));
+	int sddat2_8;
+
+	// If no space for response provided by caller, use local buffer
+	if (resp == NULL) resp = cmdresp;
+	respptr = resp;
+
+	if (activate(sd)) return 1;
+
+	dly = sd->sd_state & SD_LOSPEED;
+
+	if (!dly) {
+		unsigned int x;
+		SDPOKE8(sd, SDGPIO, 0xbf);
+#ifdef BIGENDIAN
+		x = (cmdidx & 0xff);
+		x |= ((arg >> 24) & 0xff) << 8;
+		x |= ((arg >> 16) & 0xff) << 16;
+		x |= ((arg >> 8) & 0xff) << 24;
+		if (ok32) SDPOKE32(sd, SDCMD2, x);
+		else if (ok16) {
+			SDPOKE16(sd, SDCMD2, x);
+			SDPOKE16(sd, SDCMD2, x >> 16);
+		} else {
+			SDPOKE8(sd, SDCMD2, x);
+			SDPOKE8(sd, SDCMD2, x >> 8);
+			SDPOKE8(sd, SDCMD2, x >> 16);
+			SDPOKE8(sd, SDCMD2, x >> 24);
+		}
+#else
+		x = (cmdidx & 0xff) << 24;
+		x |= ((arg >> 24) & 0xff) << 16;
+		x |= ((arg >> 16) & 0xff) << 8;
+		x |= ((arg >> 8) & 0xff);
+		if (ok32) SDPOKE32(sd, SDCMD2, x);
+		else if (ok16) {
+			SDPOKE16(sd, SDCMD2, x >> 16);
+			SDPOKE16(sd, SDCMD2, x);
+		} else {
+			SDPOKE8(sd, SDCMD2, x >> 24);
+			SDPOKE8(sd, SDCMD2, x >> 16);
+			SDPOKE8(sd, SDCMD2, x >> 8);
+			SDPOKE8(sd, SDCMD2, x);
+		}
+#endif
+		SDPOKE8(sd, SDCMD2, arg);
+	} else {
+		// Build command packet
+		mkcommand(cmdidx, arg, cmdptr);
+
+		// Send command
+		for (i = 0; i < 6; i++) {
+			unsigned int b = *cmdptr++;
+			unsigned int x;
+
+			if (timeout(sd)) break;
+			for (j = 0; j < 8; j++) {
+				x = 0x8f | ((b & 0x80) >> 3);
+				b = b << 1;
+				SDPOKE8(sd, SDGPIO, x); // clk negedge
+				SDPEEK8(sd, SDGPIO);    // delay
+				SDPEEK8(sd, SDGPIO);    // delay
+				x |= 0x20;
+				SDPOKE8(sd, SDGPIO, x); // clk posedge
+				SDPEEK8(sd, SDGPIO);    // delay
+				SDPEEK8(sd, SDGPIO);    // delay
+			}
+		}
+	}
+
+	if (type == TYPE_NORESP) goto done;
+	else if (type == TYPE_RXDAT_IGNRESP) goto ignresp;
+	else if (type == TYPE_LONGRESP) resplen = 17;
+	else resplen = 6;
+
+	// clock until start bit on CMD pin
+	while(1) {
+		if (timeout(sd)) {
+			goto done;
+		}
+		if (req == CMD_SEND_IF_COND) sd->sd_timeout += 100000;
+		SDPOKE8(sd, SDGPIO, 0xdf); // clk negedge
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		s = SDPEEK8(sd, SDGPIO);   // sample
+		if ((s & 0x10) == 0x0) break;
+		SDPOKE8(sd, SDGPIO, 0xff); // clk posedge
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+	}
+	reset_timeout(sd);
+
+	// Next we receive the response.
+	if (ok16 && !ok32) sddat2_8 = SDDAT2 + 1;
+	else sddat2_8 = SDDAT2;
+	if (dly) for (i = 0; i < resplen; i++) {
+		unsigned int r = 0;
+
+		for (j = 0; j < 8; j++) {
+			SDPOKE8(sd, SDGPIO, 0xdf); // clk negedge
+			SDPEEK8(sd, SDGPIO);       // delay
+			s = SDPEEK8(sd, SDGPIO);   // sample
+			SDPOKE8(sd, SDGPIO, 0xff); // clk posedge
+			SDPEEK8(sd, SDGPIO);       // delay
+			SDPEEK8(sd, SDGPIO);       // delay
+			r = r << 1;
+			r |= ((s & 0x10) >> 4);
+		}
+
+		*respptr++ = r;
+	} else while (resplen > 0) {
+		unsigned int r;
+
+#ifdef BIGENDIAN
+		if (ok32 && resplen >= 4) {
+			r = SDPEEK32(sd, SDCMD2);
+			*respptr++ = r & 0xff;
+			*respptr++ = (r >> 8) & 0xff;
+			*respptr++ = (r >> 16) & 0xff;
+			*respptr++ = (r >> 24);
+			resplen -= 4;
+		} else if (ok16 && resplen >= 2) {
+			r = SDPEEK16(sd, SDCMD2);
+			*respptr++ = r & 0xff;
+			*respptr++ = (r >> 8) & 0xff;
+
+			resplen -= 2;
+		} else {
+			*respptr++ = SDPEEK8(sd, sddat2_8);
+			resplen--;
+		}
+#else
+		if (ok32 && resplen >= 4) {
+			r = SDPEEK32(sd, SDCMD2);
+			*respptr++ = (r >> 24);
+			*respptr++ = (r >> 16) & 0xff;
+			*respptr++ = (r >> 8) & 0xff;
+			*respptr++ = r & 0xff;
+			resplen -= 4;
+		} else if (ok16 && resplen >= 2) {
+			r = SDPEEK16(sd, SDCMD2);
+			*respptr++ = (r >> 8) & 0xff;
+			*respptr++ = r & 0xff;
+			resplen -= 2;
+		} else {
+			*respptr++ = SDPEEK8(sd, sddat2_8);
+			resplen--;
+		}
+#endif
+	}
+	if (type == TYPE_BSYRESP) {
+		s = 0;
+		while ((s & 0x7) != 0x7) {
+			if (timeout(sd)) break;
+			SDPOKE8(sd, SDGPIO, 0x9f);  // clk negedge
+			if (dly) SDPEEK8(sd, SDGPIO);        // delay
+			s = s << 1;
+			s |= SDPEEK8(sd, SDGPIO) & 0x1;
+			SDPOKE8(sd, SDGPIO, 0xbf);
+			if (dly) SDPEEK8(sd, SDGPIO);
+		}
+	}
+
+ignresp:
+
+	if (type == TYPE_ABORT)
+		sd->sd_state &= ~(SDDAT_RX|SDDAT_TX);
+
+#ifndef SD_READONLYDMA
+	if (type == TYPE_TXDAT) {
+		sd->sd_state |= SDDAT_TX;
+		/* 2 clocks for nWR */
+		SDPOKE8(sd, SDGPIO, 0xdf); // clk negedge
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		SDPOKE8(sd, SDGPIO, 0xff); // clk posedge
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		SDPOKE8(sd, SDGPIO, 0xdf); // clk negedge
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		SDPOKE8(sd, SDGPIO, 0xff); // clk posedge
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (sd->sd_state & DATSSP_4BIT)
+			SDPOKE8(sd, SDGPIO, 0x10); // assert start, clk negedge
+		else
+			SDPOKE8(sd, SDGPIO, 0x1e);
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (sd->sd_state & DATSSP_4BIT)
+			SDPOKE8(sd, SDGPIO, 0x30); // clk posedge
+		else
+			SDPOKE8(sd, SDGPIO, 0x3e);
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+		if (dly) SDPEEK8(sd, SDGPIO);       // delay
+	}
+#endif
+
+	if (type == TYPE_RXDAT || type == TYPE_RXDAT_IGNRESP)
+	  sd->sd_state |= SDDAT_RX;
+
+done:
+	// 8 clocks before stopping
+	if (!(sd->sd_state & (SDDAT_TX|SDDAT_RX))) {
+		if (dly) for (i = 0; i < 8; i++) {
+			SDPOKE8(sd, SDGPIO, 0xdf);
+			SDPEEK8(sd, SDGPIO);       // delay
+			SDPEEK8(sd, SDGPIO);       // delay
+			SDPOKE8(sd, SDGPIO, 0xff);
+			SDPEEK8(sd, SDGPIO);       // delay
+			SDPEEK8(sd, SDGPIO);       // delay
+		} else {
+			SDPOKE8(sd, SDGPIO, 0xff);
+			SDPOKE8(sd, SDCMD2, 0xff);
+		}
+	}
+	if (timeout(sd)) return 1;
+	else return 0;
+
+}
+
+
+static int
+sdcmd(struct sdcore *sd, unsigned short req, unsigned int arg,
+  unsigned int *resp, unsigned char **dat)
+{
+	unsigned int s, cmdresp[17];
+	unsigned int resplen;
+	unsigned int type = (req >> 8);
+	unsigned int cmdidx = req;
+	unsigned int *cmdptr = cmdresp;
+	unsigned int *cmd = cmdresp;
+	unsigned int *respptr;
+	unsigned int ndat;
+
+	if (sd->hw_version != 0) return sdcmd2(sd, req, arg, resp, dat);
+
+	// If no space for response provided by caller, use local buffer
+	if (resp == NULL) resp = cmdresp;
+	respptr = resp;
+
+	// Before continuing, we must wait for the FSM to get to the
+	// S_SEND_CMD state.  After a previous command, we may still be
+	// in S_DUMMY_CLK or in case of an ABORT, we may be in the middle of
+	// clocking a byte for TX or RX.
+	s = SDPEEK8(sd, SDSTATE);
+	while ((s & 0x7) != S_SEND_CMD) {
+		if (timeout(sd)) break;
+		s = SDPEEK8(sd, SDSTATE);
+	}
+
+	// We know we're in S_SEND_CMD, but we may need to change the
+	// command type.  This won't cause a state change.
+	if ((s & 0xe7) != (S_SEND_CMD | (type << 5)))
+		SDPOKE8(sd, SDSTATE, S_SEND_CMD | (type << 5));
+
+	// Build command packet
+	mkcommand(cmdidx, arg, cmdptr);
+
+	// Next, we loop while tending the SSPs until we get our last
+	// byte of command data out.  We may get a few bytes from the DAT
+	// SSP if we are aborting a previous data transfer command.  If we do
+	// those get placed in a buffer or thrown away based on the callers
+	// "dat" parameter.
+	sd->sd_state |= SDCMD_TX;
+	while ((cmdptr - cmd) != 6) {
+		if (timeout(sd)) break;
+		s = tend_ssp(sd, &cmdptr, dat);
+	}
+	sd->sd_state &= ~SDCMD_TX;
+
+	// If we got out of sync with the hardware, that would be bad.
+	// The hardware should still be in S_SEND_CMD for the last CMDSSP
+	// byte.
+	if ((s & 0x7) != S_SEND_CMD) {
+		SDPOKE8(sd, SDSTATE, S_OFF);
+		return 1;
+	}
+
+	if (type == TYPE_NORESP) goto done;
+	else if (type == TYPE_LONGRESP) resplen = 17;
+	else resplen = 6;
+
+	// Next state should be S_WAIT_RESP or S_RX_RESP.  We may get
+	// more bytes from the DATSSP while shifting out our last bits of cmd
+	while (((s & 0x7) != S_WAIT_RESP) && ((s & 0x7) != S_RX_RESP)) {
+		if (timeout(sd)) break;
+		if (req == CMD_SEND_IF_COND) sd->sd_timeout += 1000;
+		s = tend_ssp(sd, NULL, dat);
+	}
+
+	// Once we're in S_WAIT_RESP or S_RX_RESP though, the DATSSP is only
+	// active for 2 more clocks at the beginning of the S_WAIT_RESP state.
+	// This is enough for one more byte in 4-bit mode, though we may have
+	// 2 bytes already in our DATSSP.
+	if (sd->sd_state & (SDDAT_RX|SDDAT_TX)) {
+		do {
+			if (timeout(sd)) break;
+			s = tend_ssp(sd, NULL, dat);
+		} while (!(s & 0x18));
+
+		// We've now read/wrote one more byte to the DATSSP
+		// which should allow our FSM to advance to the RX_RESP state.
+		// If we pick up more than 2 more DATSSP bytes, something is
+		// wrong.
+		ndat = 0;
+		while ((s & 0x7) != S_RX_RESP) {
+			if (timeout(sd) || ndat > 2) break;
+			s = tend_ssp(sd, NULL, dat);
+			if (s & 0x10) ndat++;
+		}
+
+		if (ndat > 2) {
+			SDPOKE8(sd, SDSTATE, S_OFF);
+			return 1;
+		}
+	}
+
+	// We're now done with whatever business we had remaining with the
+	// previous command's DATSSP transfer since we've either just got our
+	// first byte of response or our last byte of data
+	sd->sd_state &= ~(SDDAT_RX|SDDAT_TX);
+	if (type == TYPE_RXDAT) sd->sd_state |= SDDAT_RX;
+
+	// Next we receive the response.  If this is TYPE_RXDAT command,
+	// or an abortion of a previous TYPE_RXDAT command, we may get a
+	// few bytes from the DAT SSP also.
+	sd->sd_state |= SDCMD_RX;
+	while ((respptr - resp) != resplen) {
+		if (timeout(sd)) break;
+		s = tend_ssp(sd, &respptr, dat);
+		if ((s & 0x10) && (resp == respptr)) {
+			SDPOKE8(sd, SDSTATE, S_OFF);
+			sd->sd_state &= ~(SDCMD_RX|SDDAT_RX);
+			return 1;
+		}
+	}
+	sd->sd_state &= ~SDCMD_RX;
+
+	if (type == TYPE_ABORT)
+		sd->sd_state &= ~(SDDAT_RX|SDDAT_TX);
+
+	if (type == TYPE_TXDAT) sd->sd_state |= SDDAT_TX;
+
+done:
+	if (timeout(sd)) return 1;
+	else return 0;
+
+}
+
+static int datssp_stream2(struct sdcore *sd, unsigned char **dat,
+  unsigned int buflen)
+{
+	unsigned char *d;
+	int ret;
+#ifndef SD_READONLYDMA
+	int ok32;
+	int ok16;
+	int sddat2_8;
+	unsigned int x;
+#endif
+
+	if (sd->os_dmastream /* && (sd->sd_state & SDDAT_RX) */) {
+		d = dat ? *dat : NULL;
+		ret = sd->os_dmastream(sd->os_arg, d, buflen);
+		if (!ret && d) *dat += buflen;
+		return ret;
+	}
+
+#ifndef SD_READONLYDMA
+	d = *dat;
+
+	while (buflen > 512) {
+		datssp_stream2(sd, dat, 512);
+		if (sd->os_irqwait) sd->os_irqwait(sd->os_arg, 1);
+		buflen -= 512;
+		d = *dat;
+	}
+
+	ok32 = (sd->hw_version == 2);
+	ok16 = (ok32 || (sd->hw_version == 3));
+	if (ok16 && !ok32) sddat2_8 = SDDAT2 + 1;
+	else sddat2_8 = SDDAT2;
+
+	if (sd->sd_state & SDDAT_RX) {
+
+		while (((int)d & 0x1) || buflen == 1) {
+			*d++ = SDPEEK8(sd, sddat2_8);
+			buflen--;
+		}
+
+		if (((int)d & 0x2) && buflen >= 2) {
+			if (ok16) *(unsigned short *)(d) = SDPEEK16(sd, SDDAT2);
+			else {
+#ifdef BIGENDIAN
+				x = SDPEEK8(sd, sddat2_8) << 8;
+				x |= SDPEEK8(sd, sddat2_8);
+#else
+				x = SDPEEK8(sd, sddat2_8);
+				x |= SDPEEK8(sd, sddat2_8) << 8;
+#endif
+				*(unsigned short *)(d) = x;
+			}
+			buflen -= 2;
+			d += 2;
+		}
+
+		if (ok32) while (buflen >= 4) {
+			*(unsigned int *)(d) = SDPEEK32(sd, SDDAT2);
+			buflen -= 4;
+			d += 4;
+		} else if (ok16) while (buflen >= 4) {
+#ifdef BIGENDIAN
+			x = SDPEEK16(sd, SDDAT2) << 16;
+			x |= SDPEEK16(sd, SDDAT2);
+#else
+			x = SDPEEK16(sd, SDDAT2);
+			x |= SDPEEK16(sd, SDDAT2) << 16;
+#endif
+			buflen -= 4;
+			*(unsigned int *)(d) = x;
+			d += 4;
+		} else while (buflen >= 4) {
+#ifdef BIGENDIAN
+			x = SDPEEK8(sd, sddat2_8) << 24;
+			x |= SDPEEK8(sd, sddat2_8) << 16;
+			x |= SDPEEK8(sd, sddat2_8) << 8;
+			x |= SDPEEK8(sd, sddat2_8);
+#else
+			x = SDPEEK8(sd, sddat2_8);
+			x |= SDPEEK8(sd, sddat2_8) << 8;
+			x |= SDPEEK8(sd, sddat2_8) << 16;
+			x |= SDPEEK8(sd, sddat2_8) << 24;
+#endif
+			buflen -= 4;
+			*(unsigned int *)(d) = x;
+			d += 4;
+		}
+	} else {
+		while (((int)d & 0x1) || buflen == 1) {
+			SDPOKE8(sd, SDDAT2, *d++);
+			buflen--;
+		}
+
+		if (((int)d & 0x2) && buflen >= 2) {
+			if (ok16) SDPOKE16(sd, SDDAT2, *(unsigned short *)(d));
+			else {
+				x = *(unsigned short *)(d);
+#ifdef BIGENDIAN
+				SDPOKE8(sd, SDDAT2, x >> 8);
+				SDPOKE8(sd, SDDAT2, x);
+#else
+				SDPOKE8(sd, SDDAT2, x);
+				SDPOKE8(sd, SDDAT2, x >> 8);
+#endif
+			}
+			buflen -= 2;
+			d += 2;
+		}
+
+		if (ok32) while (buflen >= 4) {
+			SDPOKE32(sd, SDDAT2, *(unsigned int *)(d));
+			buflen -= 4;
+			d += 4;
+		} else if (ok16) while (buflen >= 4) {
+			x = *(unsigned int *)(d);
+			buflen -= 4;
+			d += 4;
+#ifdef BIGENDIAN
+			SDPOKE16(sd, SDDAT2, x >> 16);
+			SDPOKE16(sd, SDDAT2, x);
+#else
+			SDPOKE16(sd, SDDAT2, x);
+			SDPOKE16(sd, SDDAT2, x >> 16);
+#endif
+		} else while (buflen >= 4) {
+			x = *(unsigned int *)(d);
+			buflen -= 4;
+			d += 4;
+#ifdef BIGENDIAN
+			SDPOKE8(sd, SDDAT2, x >> 24);
+			SDPOKE8(sd, SDDAT2, x >> 16);
+			SDPOKE8(sd, SDDAT2, x >> 8);
+			SDPOKE8(sd, SDDAT2, x);
+#else
+			SDPOKE8(sd, SDDAT2, x);
+			SDPOKE8(sd, SDDAT2, x >> 8);
+			SDPOKE8(sd, SDDAT2, x >> 16);
+			SDPOKE8(sd, SDDAT2, x >> 24);
+#endif
+		}
+	}
+
+	*dat = d;
+
+	if (buflen > 0) return datssp_stream2(sd, dat, buflen);
+	else return 0;
+#else
+	return 0;
+#endif
+}
+
+static int datssp_stream(struct sdcore *sd, unsigned char **dat,
+  unsigned int buflen)
+{
+	unsigned int s, t, byte = 0;
+	unsigned char *d;
+
+	if (((sd->sd_state & SDDAT_RX) && sd->os_dmastream) /* ||
+	  ((sd->sd_state & SDDAT_TX) && sd->os_dmastream && dat) */ ) {
+		unsigned char *d = dat ? *dat : NULL;
+		int ret = sd->os_dmastream(sd->os_arg, d, buflen);
+		if (!ret && d) *dat += buflen;
+		return ret;
+	}
+
+	if (sd->hw_version > 0) return datssp_stream2(sd, dat, buflen);
+
+	while (buflen) {
+		if (timeout(sd)) return 1;
+		s = tend_ssp(sd, NULL, dat);
+		if (s & 0x10) {
+			buflen--;
+			if (byte++ > 7) {
+				if (sd->sd_state & SDDAT_RX)
+				  goto fastrx;
+				else goto fasttx;
+			}
+		}
+	}
+
+	// Now we can go faster (PIO)
+fastrx:
+	if (dat) {
+		d = *dat;
+		while (buflen) {
+			s = SDPEEK8(sd, SDDAT);
+			*d = s;
+			buflen--;
+			d++;
+		}
+		*dat = d;
+	} else {
+		while (buflen--) SDPEEK8(sd, SDDAT);
+	}
+	return 0;
+
+fasttx:
+	if (dat) {
+		d = *dat;
+		while (buflen) {
+			t = *d;
+			SDPOKE8(sd, SDDAT, t);
+			buflen--;
+			d++;
+			datssp_feedcrc(sd, t);
+		}
+		*dat = d;
+	} else {
+		while (buflen--) SDPOKE8(sd, SDDAT, datssp_getcrc(sd));
+	}
+	return 0;
+}
+
+static int stop(struct sdcore *sd)
+{
+	int ret;
+	unsigned int resp[6];
+
+	if (sd->hw_version) return stop2(sd);
+
+	if (sd->parked_sector) {
+		if (sd->sd_state & SDDAT_TX) {
+			/* wait to get out of S_WAIT_BUSY */
+			while ((SDPEEK8(sd, SDSTATE) & 0x7) != S_TX_WRITE)
+			  if (timeout(sd)) break;
+
+			/* abort parked write */
+			SDPOKE8(sd, SDSTATE, S_SEND_CMD | (TYPE_ABORT << 5));
+			sd->sd_state &= ~SDDAT_TX;
+			sd->sd_state |= SDDAT_RX;
+			ret = sdcmd(sd, CMD_STOP_TRANSMISSION, 0, resp, NULL);
+			sd->sd_state &= ~SDDAT_RX;
+			SDPOKE8(sd, SDSTATE, S_WAIT_BUSY | (TYPE_BSYRESP << 5));
+		} else {
+			/* abort parked read */
+			SDPOKE8(sd, SDSTATE, S_SEND_CMD | (TYPE_ABORT << 5));
+			ret = sdcmd(sd, CMD_STOP_TRANSMISSION, 0, resp, NULL);
+		}
+		sd->parked_sector = 0;
+		if (ret || error(resp, CMD_STOP_TRANSMISSION) || timeout(sd))
+		  return 1;
+	}
+	return 0;
+}
+
+static int stop2(struct sdcore *sd)
+{
+	int ret;
+	unsigned int resp[6];
+
+	if (sd->parked_sector) {
+		if (sd->os_irqwait) sd->os_irqwait(sd->os_arg, 0);
+		if (sd->sd_state & SDDAT_TX) {
+			/* abort parked write */
+		 	ret = sdcmd2(sd, CMD_STOP_TRANSMISSION, 0, resp, NULL);
+			SDPOKE8(sd, SDCTRL2, 0x0);
+			if (sd->os_irqwait) sd->os_irqwait(sd->os_arg, 5);
+			SDPOKE8(sd, SDGPIO, 0xff);
+
+			/*
+			while ((SDPEEK8(sd, SDGPIO) & 0xf) != 0xf) {
+				sd->os_delay(sd->os_arg, 1);
+				SDPOKE8(sd, SDGPIO, 0xdf);
+				SDPOKE8(sd, SDGPIO, 0xff);
+				if (timeout(sd)) return 1;
+			}
+			*/
+			reset_timeout(sd);
+		} else {
+			/* abort parked read */
+			ret = sdcmd2(sd, CMD_STOP_TRANSMISSION, 0, resp, NULL);
+		}
+		sd->parked_sector = 0;
+		if (ret || error(resp, CMD_STOP_TRANSMISSION) || timeout(sd)) {
+			return 1;
+		}
+	}
+	return 0;
+}
+
+static int do_read2(struct sdcore *sd, unsigned int sector, struct sdiov *iov,
+  unsigned int iovcnt)
+{
+	unsigned int ret, n, s, sz;
+	unsigned char *datptr, *dat;
+
+	if (iovcnt == 0) return 0;
+
+	if (activate(sd)) return 1;
+
+	n = iov->sdiov_nsect;
+	datptr = dat = iov->sdiov_base;
+	sz = sdsize(sd);
+	if (sector >= sz) return 0;
+
+	if (sd->parked_sector) {
+		if (!(sd->sd_state & SDDAT_TX) && sd->parked_sector == sector) {
+			if (sd->os_irqwait && !sd->os_dmastream)
+			  sd->os_irqwait(sd->os_arg, 3);
+			goto receive;
+		}
+
+		stop2(sd);
+	}
+
+	if (sd->sd_state & SD_HC)
+	  ret = sdcmd2(sd, CMD_READ_MULTIPLE_BLOCK2, sector, NULL, NULL);
+	else
+	  ret = sdcmd2(sd, CMD_READ_MULTIPLE_BLOCK2, sector * 512, NULL, NULL);
+
+	do {
+		if (timeout(sd)) return 1;;
+		SDPOKE8(sd, SDGPIO, 0xdf);
+		s = SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xff);
+	} while ((s & 0xf) != 0x0);
+	reset_timeout(sd);
+
+receive:
+	if (sd->os_dmaprep && sd->os_dmastream)
+	  sd->os_dmaprep(sd->os_arg, datptr, n * 512);
+
+	SDPOKE8(sd, SDGPIO, 0xdf);
+	sd->parked_sector = sector + n;
+
+nextiov:
+	if (sd->parked_sector > sz) {
+		n -= sd->parked_sector - sz;
+		sd->parked_sector = sz;
+	}
+	datssp_stream2(sd, &datptr, n * 512);
+
+	if (--iovcnt) {
+		++iov;
+		n = iov->sdiov_nsect;
+		datptr = iov->sdiov_base;
+		sd->parked_sector += n;
+		if (sd->os_dmaprep && sd->os_dmastream)
+		  sd->os_dmaprep(sd->os_arg, datptr, n * 512);
+		goto nextiov;
+	}
+
+	/* s = SDPEEK8(sd, SDSTAT2);
+	if (s & 0x44) {
+		sd->sd_timeout = 1000001;
+		return 1;
+	}
+	else */ return 0;
+}
+
+static int do_read(struct sdcore *sd, unsigned int sector, struct sdiov *iov,
+  unsigned int iovcnt)
+{
+	unsigned int resp[6], ret, n, sz;
+	unsigned char *datptr, *dat;
+
+	if (iovcnt == 0) return 0;
+
+	n = iov->sdiov_nsect;
+	datptr = dat = iov->sdiov_base;
+	sz = sdsize(sd);
+	if (sector >= sz) return 0;
+
+	if (sd->parked_sector) {
+		if (!(sd->sd_state & SDDAT_TX) && sd->parked_sector == sector)
+		  goto receive;
+
+		stop(sd);
+	}
+
+	if (sd->sd_state & SD_HC)
+	  ret = sdcmd(sd, CMD_READ_MULTIPLE_BLOCK, sector, resp, &datptr);
+	else
+	  ret = sdcmd(sd, CMD_READ_MULTIPLE_BLOCK, sector * 512, resp, &datptr);
+	if (ret || error(resp, CMD_READ_MULTIPLE_BLOCK)) return 1;
+
+receive:
+	if (sd->os_dmaprep && sd->os_dmastream)
+	  sd->os_dmaprep(sd->os_arg, datptr, n * 512 - (datptr - dat));
+
+	datssp_stream(sd, &datptr, 512 - (datptr - dat));
+	datssp_stream(sd, NULL, 6);
+
+	sd->parked_sector = sector + n;
+	if (sd->parked_sector > sz) {
+		n -= sd->parked_sector - sz;
+		sd->parked_sector = sz;
+	}
+	n--;
+
+nextiov:
+	while (n--) {
+		SDPOKE8(sd, SDSTATE, S_WAIT_RESP | (TYPE_RXDAT << 5));
+		datssp_stream(sd, NULL, 2); // last part of prev CRC
+		datssp_stream(sd, &datptr, 512);
+		datssp_stream(sd, NULL, 6); // first part of CRC
+	}
+
+	if (--iovcnt) {
+		++iov;
+		n = iov->sdiov_nsect;
+		datptr = iov->sdiov_base;
+		sd->parked_sector += n;
+		if (sd->parked_sector > sz) {
+			n -= sd->parked_sector - sz;
+			sd->parked_sector = sz;
+		}
+		if (sd->os_dmaprep && sd->os_dmastream && n > 0)
+		  sd->os_dmaprep(sd->os_arg, datptr, n * 512);
+		goto nextiov;
+	}
+
+	SDPOKE8(sd, SDSTATE, S_WAIT_RESP | (TYPE_RXDAT << 5));
+	datssp_stream(sd, NULL, 2); // last part of prev CRC
+	return 0;
+}
+
+static int do_write2(struct sdcore *sd, unsigned int sector, struct sdiov *iov,
+  unsigned int iovcnt)
+{
+	unsigned char *datptr;
+	unsigned int resp[6], ret, n, s, sz, ss;
+
+	if (sd->sd_wprot) return 1;
+
+	if (iovcnt == 0) return 0;
+
+	if (activate(sd)) return 1;
+
+	sz = sdsize(sd);
+	if (sector >= sz) return 0;
+
+	if (sd->os_powerok) {
+		int ok = sd->os_powerok(sd);
+		if (!ok && sd->parked_sector) {
+			stop2(sd);
+			return 1;
+		} else if (!ok) return 1;
+	}
+
+	if (sd->parked_sector) {
+		if ((sd->sd_state & SDDAT_TX) && sd->parked_sector == sector)
+		  goto transmit;
+
+		stop2(sd);
+	}
+
+	if (sd->sd_erasehint) {
+		sdcmd2(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+		sdcmd2(sd, ACMD_SET_WR_BLK_ERASE_COUNT, sd->sd_erasehint,
+		  NULL, NULL);
+		sd->sd_erasehint = 0;
+	}
+
+	if (sd->sd_state & SD_HC)
+	  ret = sdcmd2(sd, CMD_WRITE_MULTIPLE_BLOCK, sector, resp, NULL);
+	else
+	  ret = sdcmd2(sd, CMD_WRITE_MULTIPLE_BLOCK, sector * 512, resp, NULL);
+	if (ret || error(resp, CMD_WRITE_MULTIPLE_BLOCK)) {
+		return 1;
+	}
+	sd->parked_sector = sector;
+	ss = SDPEEK8(sd, SDSTAT2);
+
+transmit:
+	while (iovcnt--) {
+		datptr = iov->sdiov_base;
+		n = iov->sdiov_nsect;
+		sd->parked_sector += n;
+		if (sd->parked_sector > sz) {
+			n -= sd->parked_sector - sz;
+			sd->parked_sector = sz;
+		}
+		datssp_stream2(sd, &datptr, n * 512);
+		iov++;
+	}
+
+	if (!sd->sd_writeparking) {
+		ret = stop2(sd);
+		if (ret) return ret;
+	}
+
+	if (sd->os_irqwait) sd->os_irqwait(sd->os_arg, 2);
+
+	s = SDPEEK8(sd, SDSTAT2);
+	if (s & 0x44) {
+		sd->sd_timeout = 1000001;
+		return 1;
+	} else {
+		reset_timeout(sd);
+		return 0;
+	}
+}
+
+static int do_write(struct sdcore *sd, unsigned int sector, struct sdiov *iov,
+  unsigned int iovcnt)
+{
+	unsigned char *datptr, *crcptr, **crcptrptr;
+	unsigned int resp[6], ret, n, sz;
+
+	if (sd->sd_wprot) return 1;
+
+	if (iovcnt == 0) return 0;
+
+	sz = sdsize(sd);
+	if (sector >= sz) return 0;
+
+	if (0 /* sd->sd_crchint */) {
+		// CRC is pre-calculated so don't recalculate
+		crcptr = sd->sd_crchint;
+		crcptrptr = &crcptr;
+		sd->sd_state |= DATSSP_NOCRC;
+		sd->sd_crchint = NULL;
+	} else {
+		crcptrptr = NULL;
+		sd->sd_state &= ~DATSSP_NOCRC;
+	}
+
+	if (sd->parked_sector) {
+		if ((sd->sd_state & SDDAT_TX) && sd->parked_sector == sector)
+		  goto transmit;
+
+		stop(sd);
+	}
+
+	if (sd->sd_erasehint) {
+		sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+		sdcmd(sd, ACMD_SET_WR_BLK_ERASE_COUNT, sd->sd_erasehint,
+		  NULL, NULL);
+		sd->sd_erasehint = 0;
+	}
+
+	if (sd->sd_state & SD_HC)
+	  ret = sdcmd(sd, CMD_WRITE_MULTIPLE_BLOCK, sector, resp, NULL);
+	else
+	  ret = sdcmd(sd, CMD_WRITE_MULTIPLE_BLOCK, sector * 512, resp, NULL);
+	if (ret || error(resp, CMD_WRITE_MULTIPLE_BLOCK)) {
+		return 1;
+	}
+	sd->parked_sector = sector;
+
+transmit:
+	while (iovcnt--) {
+		datptr = iov->sdiov_base;
+		n = iov->sdiov_nsect;
+		sd->parked_sector += n;
+		if (sd->parked_sector > sz) {
+			n -= sd->parked_sector - sz;
+			sd->parked_sector = sz;
+		}
+		while (n--) {
+			datssp_stream(sd, &datptr, 512);
+			datssp_stream(sd, crcptrptr, 8); // CRC bytes
+			SDPOKE8(sd, SDSTATE, S_CRC_CHECK | (TYPE_TXDAT << 5));
+		}
+		iov++;
+	}
+
+	if (!sd->sd_writeparking) {
+		stop(sd);
+	}
+
+	return 0;
+}
+
+static
+int sdfastinit(struct sdcore *sd)
+{
+	SDPOKE8(sd, SDCTRL, 0x40);
+	sd->sd_state = DATSSP_4BIT;
+
+	sd->sd_rcaarg = ~sd->sdboot_token;
+	sdcmd(sd, CMD_DESELECT_CARD, ~sd->sd_rcaarg, NULL, NULL);
+	sdcmd(sd, CMD_SEND_CSD, sd->sd_rcaarg, sd->sd_csd, NULL);
+	sdcmd(sd, CMD_SELECT_CARD, sd->sd_rcaarg, NULL, NULL);
+
+	if (sd->os_dmastream) SDPOKE8(sd, SDCTRL, 0x42);
+	if ((SDPEEK8(sd, SDCTRL) & 0x80) || (sd->sd_csd[15] & 0x30))
+	  sd->sd_wprot = 1;
+	sd->sd_blocksize = 1 << ((sd->sd_csd[6] & 0xf));
+	if (timeout(sd)) return 0;
+	else return sdsize(sd);
+}
+
+static
+int sdreset2(struct sdcore *sd)
+{
+	unsigned int rca, s, i, x;
+	unsigned int resp[17];
+
+	reset_timeout(sd);
+	sd_initcrc(sd);
+	sd->parked_sector = 0;
+	sd->sd_wprot = 0;
+	sd->sd_blocksize = 0;
+	sd->sd_sz = 0;
+	if (sd->hw_version == 0) sd->hw_version = version(sd);
+	if (sd->hw_version == 0) return 0;
+	sd->sd_state &= SD_RESET;
+	remember_sdcore(sd);
+	activate(sd);
+	sd->sd_state |= SD_LOSPEED;
+
+	if (!(sd->sd_state & SD_RESET) && (SDPEEK8(sd, SDGPIO) != 0x0)) {
+		SDPOKE8(sd, SDGPIO, 0x0);
+#ifdef BIGENDIAN
+		for (i = 0; i < 8; i++) SDPOKE16(sd, SDLUN2, i << 8);
+#else
+		for (i = 0; i < 8; i++) SDPOKE16(sd, SDLUN2, i);
+#endif
+		sd->os_delay(sd->os_arg, 100000);
+
+		/* this was a global reset, so let the other luns know */
+		for (i = 0; i < sizeof(sdcores); i++) {
+			if (sdcores[i] == NULL) break;
+			if (sdcores[i]->sd_regstart == sd->sd_regstart)
+			  sdcores[i]->sd_state |= SD_RESET;
+		}
+#ifdef BIGENDIAN
+		SDPOKE16(sd, SDLUN2, sd->sd_lun << 8);
+#else
+		SDPOKE16(sd, SDLUN2, sd->sd_lun);
+#endif
+	}
+	sd->sd_state &= ~SD_RESET;
+
+	// gratuitous clocks
+	SDPOKE8(sd, SDGPIO, 0xff);
+	sd->os_delay(sd->os_arg, 5000);
+	for (i = 0; i < 750; i++) {
+		SDPOKE8(sd, SDGPIO, 0xff);
+		SDPEEK8(sd, SDGPIO); /* delay */
+		SDPEEK8(sd, SDGPIO); /* delay */
+		SDPOKE8(sd, SDGPIO, 0xdf);
+		SDPEEK8(sd, SDGPIO); /* delay */
+		SDPEEK8(sd, SDGPIO); /* delay */
+	}
+
+	SDPEEK8(sd, SDSTAT2); /* reset any timeout/crc conditions */
+	SDPOKE8(sd, SDSTAT2, 0x18);
+	s = sdcmd2(sd, CMD_SEND_IF_COND, 0x1aa, resp, NULL);
+	if (s) {
+		reset_timeout(sd);
+		x = 0x00ff0000;
+	} else {
+		x = 0x40ff0000;
+	}
+
+	do {
+		sdcmd2(sd, CMD_APP_CMD, 0, NULL, NULL);
+		sdcmd2(sd, ACMD_SD_SEND_OP_COND, x, resp, NULL);
+		if (timeout(sd)) break;
+		// TODO: check for valid result or limit # of loops,
+		// otherwise we may loop forever on malfunctioning cards.
+	} while (((resp[1] & 0x80) == 0x0));
+
+	if ((x & 0x40000000) && (resp[1] & 0x40)) {
+		sd->sd_state |= SD_HC;
+	}
+
+	sdcmd2(sd, CMD_ALL_SEND_CID, 0, resp, NULL);
+	sdcmd2(sd, CMD_SEND_RELATIVE_ADDR, 0, resp, NULL);
+	rca = resp[1] << 8 | resp[2];
+	sd->sd_rcaarg = (rca & 0xff00) << 16 | (rca & 0xff) << 16;
+	sd->sdboot_token = ~sd->sd_rcaarg;
+
+	sdcmd2(sd, CMD_SEND_CSD, sd->sd_rcaarg, sd->sd_csd, NULL);
+	sdcmd2(sd, CMD_SELECT_CARD, sd->sd_rcaarg, resp, NULL);
+
+	if ((resp[1] & 0x2)) {
+		unsigned int ret = 1;
+		sd->sd_locked = 1;
+#ifndef SD_NOLOCKSUPPORT
+		if (sd->sd_pwd)
+	 	  ret = sdlockctl2(sd, SDLOCK_UNLOCK, sd->sd_pwd, NULL);
+#endif
+		if (ret != 0) return 0;
+	} else sd->sd_locked = 0;
+
+	sdcmd2(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd2(sd, ACMD_SET_CLR_CARD_DETECT, 0, NULL, NULL);
+	sdcmd2(sd, CMD_SET_BLOCKLEN, 512, NULL, NULL);
+	sdcmd2(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd2(sd, ACMD_SET_BUS_WIDTH, 2, resp, NULL);
+	sd->sd_state |= DATSSP_4BIT;
+	sd->sd_state &= ~SD_LOSPEED;
+
+	sdcmd2(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd2(sd, ACMD_SEND_SCR2, 0, NULL, NULL);
+	do {
+		if (timeout(sd)) break;
+		SDPOKE8(sd, SDGPIO, 0xdf);
+		SDPEEK8(sd, SDGPIO);
+		s = SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xff);
+		SDPEEK8(sd, SDGPIO);
+	} while ((s & 0xf) != 0x0);
+	for (i = 0; i < 16; i++) {
+		SDPOKE8(sd, SDGPIO, 0xdf);
+		SDPEEK8(sd, SDGPIO);
+		s = (SDPEEK8(sd, SDGPIO) & 0xf) << 4;
+		SDPOKE8(sd, SDGPIO, 0xff);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xdf);
+		SDPEEK8(sd, SDGPIO);
+		s |= (SDPEEK8(sd, SDGPIO) & 0xf);
+		SDPOKE8(sd, SDGPIO, 0xff);
+		SDPEEK8(sd, SDGPIO);
+		if (i < 8) sd->sd_scr[i] = s;
+	}
+	for (i = 0; i < 8; i++) {
+		SDPOKE8(sd, SDGPIO, 0xdf);
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xff);
+		SDPEEK8(sd, SDGPIO);
+	}
+	sd->sd_state &= ~SDDAT_RX;
+
+#ifndef SD_NOHIGHSPEED
+	if ((sd->sd_scr[0] & 0xf) >= 1) { // SD version >= 1.10
+		unsigned char dat[64];
+		sdcmd2(sd, CMD_SWITCH_FUNC2, 0x80fffff1, NULL, NULL);
+		do {
+			if (timeout(sd)) break;
+			SDPOKE8(sd, SDGPIO, 0xdf);
+			SDPEEK8(sd, SDGPIO);
+			s = SDPEEK8(sd, SDGPIO);
+			SDPOKE8(sd, SDGPIO, 0xff);
+			SDPEEK8(sd, SDGPIO);
+		} while ((s & 0xf) != 0x0);
+		for (i = 0; i < 72; i++) {
+			SDPOKE8(sd, SDGPIO, 0xdf);
+			SDPEEK8(sd, SDGPIO);
+			s = (SDPEEK8(sd, SDGPIO) & 0xf) << 4;
+			SDPOKE8(sd, SDGPIO, 0xff);
+			SDPEEK8(sd, SDGPIO);
+			SDPOKE8(sd, SDGPIO, 0xdf);
+			SDPEEK8(sd, SDGPIO);
+			s |= (SDPEEK8(sd, SDGPIO) & 0xf);
+			SDPOKE8(sd, SDGPIO, 0xff);
+			SDPEEK8(sd, SDGPIO);
+			if (i < 64) dat[i] = s;
+		}
+		for (i = 0; i < 8; i++) {
+			SDPOKE8(sd, SDGPIO, 0xdf);
+			SDPEEK8(sd, SDGPIO);
+			SDPEEK8(sd, SDGPIO);
+			SDPOKE8(sd, SDGPIO, 0xff);
+			SDPEEK8(sd, SDGPIO);
+		}
+		sd->sd_state &= ~SDDAT_RX;
+		if (dat[0] | dat[1]) {
+			SDPOKE8(sd, SDSTAT2, 0x38);
+			sd->sd_state |= SD_HISPEED;
+		}
+	}
+#endif
+
+#ifdef BIGENDIAN
+	if ((sd->sd_csd[15] & 0x30) || (SDPEEK16(sd, SDGPIO) & 0x2))
+#else
+	if ((sd->sd_csd[15] & 0x30) || (SDPEEK16(sd, SDGPIO) & 0x200))
+#endif
+		sd->sd_wprot = 1;
+	sd->sd_blocksize = 1 << ((sd->sd_csd[6] & 0xf));
+	if (timeout(sd)) return 0;
+	else {
+		reset_timeout(sd);
+		return sdsize(sd);
+	}
+}
+
+/*
+ * return 0 : 8 bit TS-SDCORE v1
+ * return 1 : 8 bit 4x8 TS-SDCORE v2
+ * return 2 : 32 bit 4x32 TS-SDCORE v2
+ * return 3 : 16 bit 4x32 TS-SDCORE v2
+ * return 4 : 8 bit 4x32 TS-SDCORE v2
+ */
+static int version(struct sdcore *sd)
+{
+	int a, b, i;
+
+
+#ifdef SD_FORCEVERSION
+	return SD_FORCEVERSION;
+#endif
+	for (i = 0; i < sizeof(sdcores); i++) {
+		if (sdcores[i] == NULL) break;
+		if (sdcores[i]->sd_regstart == sd->sd_regstart)
+		  return sdcores[i]->hw_version;
+	}
+
+	a = SDPEEK8(sd, 3);
+	SDPOKE8(sd, 3, (a ^ 0x40));
+	b = SDPEEK8(sd, 3);
+	SDPOKE8(sd, 3, a);
+	if ((a & 0x40) ^ (b & 0x40)) return 0;
+	else if (a & 0x40) return 1;
+	/* either 2, 3, or 4 */
+	a = SDPEEK32(sd, 12);
+	b = SDPEEK16(sd, 12);
+#ifdef BIGENDIAN
+	if ((a & 0x40000000) && (b & 0x4000)) return 2;
+#else
+	if ((a & 0x40) && (b & 0x40)) return 2;
+#endif
+	a = SDPEEK8(sd, 12);
+	if (a & 0x40) return 3;
+	else return 4;
+}
+
+int sdreset(struct sdcore *sd)
+{
+	unsigned int rca, s, x;
+	unsigned int resp[17];
+
+	reset_timeout(sd);
+	sd_initcrc(sd);
+	sd->parked_sector = 0;
+	sd->sd_wprot = 0;
+	sd->sd_blocksize = 0;
+	sd->sd_sz = 0;
+
+	sd->hw_version = version(sd);
+	if (sd->hw_version >= 2) return sdreset2(sd);
+
+	// check for no SD card present
+	if (SDPEEK8(sd, SDCTRL) & 0x8) return 0;
+
+	if (sd->sdboot_token) {
+		int ret = sdfastinit(sd);
+		sd->sdboot_token = 0;
+		if (ret) return ret;
+	}
+
+	// set controller for 1-bit mode, slow clock
+	SDPOKE8(sd, SDCTRL, 0x20);
+
+	SDPOKE8(sd, SDSTATE, S_DUMMY_CLK);
+	sd->sd_state = SDCMD_RX|SDDAT_RX;
+	s = SDPEEK8(sd, SDSTATE);
+	while ((s & 0x7) != S_SEND_CMD) {
+		// If we timeout here, it would be VERY BAD as we have no
+		// further recourse to set things right if we can't turn
+		// the SD off.
+		if (timeout(sd)) return 0;
+		sd->os_delay(sd->os_arg, 10000);
+		sd->sd_timeout += 10000;
+
+		// We won't be able to change state until both SSPs are empty
+		s = tend_ssp(sd, NULL, NULL);
+	}
+	SDPOKE8(sd, SDSTATE, S_OFF);
+	sd->sd_state = 0;
+
+	sd->os_delay(sd->os_arg, 50000);
+
+	SDPOKE8(sd, SDSTATE, S_DUMMY_CLK);
+	sd->os_delay(sd->os_arg, 100000);
+	if ((SDPEEK8(sd, SDSTATE) & 0x7) == S_OFF) {
+		// No card present
+		return 0;
+	}
+
+	SDPOKE8(sd, SDSTATE, S_WAIT_RESP);
+	// clock will freerun waiting for a response that will never come
+	sd->os_delay(sd->os_arg, 50000);
+
+	SDPOKE8(sd, SDSTATE, S_DUMMY_CLK);
+
+	s = sdcmd(sd, CMD_SEND_IF_COND, 0x1aa, resp, NULL);
+	if (s) {
+		reset_timeout(sd);
+		SDPOKE8(sd, SDSTATE, S_DUMMY_CLK);
+		x = 0x00ff0000;
+	} else {
+		x = 0x40ff0000;
+	}
+
+	do {
+		sdcmd(sd, CMD_APP_CMD, 0, NULL, NULL);
+		sdcmd(sd, ACMD_SD_SEND_OP_COND, x, resp, NULL);
+		if (timeout(sd)) break;
+	} while (((resp[1] & 0x80) == 0x0));
+
+	if ((x & 0x40000000) && (resp[1] & 0x40)) sd->sd_state |= SD_HC;
+
+	sdcmd(sd, CMD_ALL_SEND_CID, 0, resp, NULL);
+	sdcmd(sd, CMD_SEND_RELATIVE_ADDR, 0, resp, NULL);
+	rca = resp[1] << 8 | resp[2];
+	sd->sd_rcaarg = (rca & 0xff00) << 16 | (rca & 0xff) << 16;
+	sd->sdboot_token = ~sd->sd_rcaarg;
+
+	sdcmd(sd, CMD_SEND_CSD, sd->sd_rcaarg, sd->sd_csd, NULL);
+	sdcmd(sd, CMD_SELECT_CARD, sd->sd_rcaarg, resp, NULL);
+
+	if ((resp[1] & 0x2)) {
+		unsigned int ret = 1;
+		sd->sd_locked = 1;
+#ifndef SD_NOLOCKSUPPORT
+		if (sd->sd_pwd)
+	 	  ret = sdlockctl(sd, SDLOCK_UNLOCK, sd->sd_pwd, NULL);
+#endif
+		if (ret != 0) return 0;
+	} else sd->sd_locked = 0;
+
+	sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd(sd, ACMD_SET_CLR_CARD_DETECT, 0, NULL, NULL);
+	/*
+	sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd(sd, ACMD_SEND_SCR, 0, NULL, &datptr);
+	while ((datptr - sd->sd_scr) != 8) {
+		if (timeout(sd)) return 1;
+		tend_ssp(sd, NULL, &datptr);
+	}
+	datssp_stream(sd, NULL, 3);
+	SDPOKE8(sd, SDSTATE, (TYPE_ABORT << 5) | S_SEND_CMD);
+	sd->sd_state |= SDCMD_RX|SDDAT_RX;
+	while ((SDPEEK8(sd, SDSTATE) & 0x17) != S_SEND_CMD) {
+		if (timeout(sd)) break;
+		tend_ssp(sd, NULL, NULL);
+	}
+	sd->sd_state &= ~(SDCMD_RX|SDDAT_RX);
+	if ((sd->sd_scr[0] & 0xf) >= 1) { // SD version >= 1.10
+		unsigned char dat[64];
+		datptr = dat;
+		sdcmd(sd, CMD_SWITCH_FUNC, 0x80fffff1, NULL, &datptr);
+		while ((datptr - dat) != 64) {
+			if (timeout(sd)) break;
+			tend_ssp(sd, NULL, &datptr);
+		}
+		datssp_stream(sd, NULL, 3);
+		SDPOKE8(sd, SDSTATE, (TYPE_ABORT << 5) | S_SEND_CMD);
+		sd->sd_state |= SDCMD_RX|SDDAT_RX;
+		while ((SDPEEK8(sd, SDSTATE) & 0x7) != S_SEND_CMD) {
+			if (timeout(sd)) break;
+			tend_ssp(sd, NULL, NULL);
+		}
+		sd->sd_state &= ~(SDCMD_RX|SDDAT_RX);
+	}
+	*/
+
+	sdcmd(sd, CMD_SET_BLOCKLEN, 512, NULL, NULL);
+	sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd(sd, ACMD_SET_BUS_WIDTH, 2, resp, NULL);
+
+	// set controller for 4-bit mode, fast clock
+	SDPOKE8(sd, SDCTRL, (0x40 | (sd->os_dmastream ? 0x2 : 0x0)));
+	sd->sd_state |= DATSSP_4BIT;
+
+	/*
+	sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+	sdcmd(sd, ACMD_SEND_SCR, 0, NULL, &datptr);
+	while ((datptr - sd->sd_scr) != 8) {
+		if (timeout(sd)) break;
+		tend_ssp(sd, NULL, &datptr);
+	}
+	datssp_stream(sd, NULL, 6);
+	SDPOKE8(sd, SDSTATE, S_DUMMY_CLK | (TYPE_SHORTRESP << 5));
+	bzero(resp, 6 * 4);
+	sdcmd(sd, CMD_SEND_STATUS, 0, resp, NULL);
+	*/
+
+	if ((SDPEEK8(sd, SDCTRL) & 0x80) || (sd->sd_csd[15] & 0x30))
+		sd->sd_wprot = 1;
+	sd->sd_blocksize = 1 << ((sd->sd_csd[6] & 0xf));
+	if (timeout(sd) || error(resp, ACMD_SET_BUS_WIDTH)) return 0;
+	else return sdsize(sd);
+}
+
+static
+int sdread2(struct sdcore *sd, unsigned int sector, unsigned char *dat,
+  int nsectors)
+{
+	struct sdiov iov;
+	int ret;
+
+	iov.sdiov_base = dat;
+	iov.sdiov_nsect = nsectors;
+	ret = do_read2(sd, sector, &iov, 1);
+	return ret;
+}
+
+int sdread(struct sdcore *sd, unsigned int sector, unsigned char *dat,
+  int nsectors)
+{
+	struct sdiov iov;
+	int ret;
+
+	iov.sdiov_base = dat;
+	iov.sdiov_nsect = nsectors;
+	if (sd->hw_version == 0) ret = do_read(sd, sector, &iov, 1);
+	else ret = do_read2(sd, sector, &iov, 1);
+	return ret;
+}
+
+int sdwrite(struct sdcore *sd, unsigned int sector, unsigned char *dat,
+  int nsectors)
+{
+	struct sdiov iov;
+	unsigned int ret;
+
+	iov.sdiov_base = dat;
+	iov.sdiov_nsect = nsectors;
+	if (sd->hw_version == 0) ret = do_write(sd, sector, &iov, 1);
+	else ret = do_write2(sd, sector, &iov, 1);
+	return ret;
+
+}
+
+int sdreadv(struct sdcore *sd, unsigned int sector, struct sdiov *iov,
+  int niov)
+{
+	if (sd->hw_version == 0) return do_read(sd, sector, iov, niov);
+	else return do_read2(sd, sector, iov, niov);
+}
+
+int sdwritev(struct sdcore *sd, unsigned int sector, struct sdiov *iov,
+  int niov)
+{
+	if (sd->hw_version == 0) return do_write(sd, sector, iov, niov);
+	else return do_write2(sd, sector, iov, niov);
+}
+
+static
+int sdsetwprot2(struct sdcore *sd, unsigned int perm)
+{
+	int i, ret, s;
+	unsigned int csd[16], resp[6];
+	unsigned char csdchars[16];
+	unsigned char *csdptr = csdchars;
+
+	stop2(sd);
+
+	perm = perm ? 0x3 : 0x1;
+	for (i = 0; i < 16; i++) csd[i] = sd->sd_csd[i + 1];
+	csd[14] &= ~(0x3 << 4);
+	csd[14] |= (perm << 4);
+	csd[15] = 0x1 | crc7(0, csd, 15) << 1;
+	for (i = 0; i < 16; i++) csdchars[i] = csd[i];
+
+	ret = sdcmd2(sd, CMD_PROGRAM_CSD, 0, resp, NULL);
+	if (ret || error(resp, CMD_PROGRAM_CSD)) return 1;
+	for (i = 0; i < 16; i++) {
+		s = *csdptr++;
+		sd_4bit_feedcrc(sd, s);
+		SDPOKE8(sd, SDGPIO, (0x10|((s & 0xf0) >> 4)));
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, (0x30|((s & 0xf0) >> 4)));
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, (0x10|(s & 0xf)));
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, (0x30|(s & 0xf)));
+		SDPEEK8(sd, SDGPIO);
+	}
+	for (i = 0; i < 8; i++) {
+		s = sd_4bit_getcrc(sd);
+		SDPOKE8(sd, SDGPIO, (0x10|((s & 0xf0) >> 4)));
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, (0x30|((s & 0xf0) >> 4)));
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, (0x10|(s & 0xf)));
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, (0x30|(s & 0xf)));
+		SDPEEK8(sd, SDGPIO);
+	}
+	// End bit
+	SDPOKE8(sd, SDGPIO, 0x1f);
+	SDPEEK8(sd, SDGPIO);
+	SDPEEK8(sd, SDGPIO);
+	SDPOKE8(sd, SDGPIO, 0x3f);
+	SDPEEK8(sd, SDGPIO);
+	SDPOKE8(sd, SDGPIO, 0xbf);  //  tristate dat
+	// CRC ack
+	s = 0;
+	for (i = 0; i < 7; i++) {
+		SDPOKE8(sd, SDGPIO, 0x9f);  // clk negedge
+		SDPEEK8(sd, SDGPIO);        // delay
+		s = s << 1;
+		s |= (SDPEEK8(sd, SDGPIO) & 0x1);
+		SDPOKE8(sd, SDGPIO, 0xbf);  // clk posedge
+	}
+	if ((s & 0xf) != 0x5) return 1;
+	// wait for unbusy
+	s = 0;
+	while ((s & 0x7) != 0x7) {
+		if (timeout(sd)) break;
+		SDPOKE8(sd, SDGPIO, 0x9f);  // clk negedge
+		SDPEEK8(sd, SDGPIO);        // delay
+		s = s << 1;
+		s |= SDPEEK8(sd, SDGPIO) & 0x1;
+		SDPOKE8(sd, SDGPIO, 0xbf);
+	}
+	for (i = 0; i < 8; i++) {
+		SDPOKE8(sd, SDGPIO, 0x9f);
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xbf);
+		SDPEEK8(sd, SDGPIO);
+	}
+	sd->sd_state &= ~SDDAT_TX;
+
+	sdcmd2(sd, CMD_DESELECT_CARD, ~sd->sd_rcaarg, NULL, NULL);
+	ret = sdcmd2(sd, CMD_SEND_CSD, sd->sd_rcaarg, sd->sd_csd, NULL);
+	if (ret || sd->sd_csd[15] != csd[14]) {
+		return 1;
+	}
+	sdcmd2(sd, CMD_SELECT_CARD, sd->sd_rcaarg, resp, NULL);
+
+	sd->sd_wprot = 1;
+	return 0;
+}
+
+int sdsetwprot(struct sdcore *sd, unsigned int perm)
+{
+	int i, ret;
+	unsigned int csd[16], resp[6];
+	unsigned char csdchars[16];
+	unsigned char *csdptr = csdchars;
+
+	if (sd->hw_version) return sdsetwprot2(sd, perm);
+
+	if (stop(sd)) return 1;
+
+	perm = perm ? 0x3 : 0x1;
+	for (i = 0; i < 16; i++) csd[i] = sd->sd_csd[i + 1];
+	csd[14] &= ~(0x3 << 4);
+	csd[14] |= (perm << 4);
+	csd[15] = 0x1 | crc7(0, csd, 15) << 1;
+	for (i = 0; i < 16; i++) csdchars[i] = csd[i];
+
+	ret = sdcmd(sd, CMD_PROGRAM_CSD, 0, resp, NULL);
+	if (ret || error(resp, CMD_PROGRAM_CSD)) return 1;
+	datssp_stream(sd, &csdptr, 16);
+	datssp_stream(sd, NULL, 8);
+	SDPOKE8(sd, SDSTATE, S_CRC_CHECK | (TYPE_BSYRESP << 5));
+	sd->sd_state &= ~SDDAT_TX;
+
+	sdcmd(sd, CMD_DESELECT_CARD, ~sd->sd_rcaarg, NULL, NULL);
+	ret = sdcmd(sd, CMD_SEND_CSD, sd->sd_rcaarg, sd->sd_csd, NULL);
+	if (ret || sd->sd_csd[15] != csd[14]) {
+		return 1;
+	}
+	sdcmd(sd, CMD_SELECT_CARD, sd->sd_rcaarg, resp, NULL);
+
+	sd->sd_wprot = 1;
+	return 0;
+}
+
+#ifndef SD_NOLOCKSUPPORT
+int sdlockctl(struct sdcore *sd, unsigned int cmd, unsigned char *pwd,
+  unsigned char *sdbootdat)
+{
+	unsigned char pwddat[18];
+	unsigned char *pwdptr = pwddat;
+	unsigned int resp[6];
+	int ret, i, len;
+	int ccc = (sd->sd_csd[5] << 4) | (sd->sd_csd[6] >> 4);
+
+	if (sd->hw_version) return sdlockctl2(sd, cmd, pwd, sdbootdat);
+
+	if (!(ccc & 0x80)) return 1; // Class 7 is lock-unlock commands
+
+	if (pwd == NULL && cmd != SDLOCK_ERASE) return 1;
+
+	if (stop(sd)) return 1;
+
+	if (sd->sd_state & DATSSP_4BIT) {
+		int oldctrl = SDPEEK8(sd, SDCTRL);
+		int ret;
+
+		sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+		sdcmd(sd, ACMD_SET_BUS_WIDTH, 0, NULL, NULL);
+		SDPOKE8(sd, SDCTRL, 0x20);
+		sd->sd_state &= ~DATSSP_4BIT;
+		ret = sdlockctl(sd, cmd, pwd, sdbootdat);
+		sdcmd(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+		sdcmd(sd, ACMD_SET_BUS_WIDTH, 2, NULL, NULL);
+		sd->sd_state |= DATSSP_4BIT;
+		SDPOKE8(sd, SDCTRL, oldctrl);
+		return ret;
+	}
+
+	pwddat[0] = cmd;
+	if (cmd != SDLOCK_ERASE) {
+		pwddat[1] = 16; // length
+		for (i = 0; i < 16; i++) {
+			pwddat[2 + i] = pwd[i];
+		}
+	}
+
+	if (cmd == SDLOCK_ERASE) len = 1; else len = 18;
+	ret = sdcmd(sd, CMD_SET_BLOCKLEN, len, resp, NULL);
+	if (ret || error(resp, CMD_SET_BLOCKLEN)) return 1;
+	ret = sdcmd(sd, CMD_LOCK_UNLOCK, 0, resp, NULL);
+	if (ret || error(resp, CMD_LOCK_UNLOCK)) return 1;
+
+	while ((pwdptr - pwddat) != len) {
+		if (timeout(sd)) return 1;
+		tend_ssp(sd, NULL, &pwdptr);
+	}
+
+	if (sd->sd_state & DATSSP_4BIT) datssp_stream(sd, NULL, 8);
+	else datssp_stream(sd, NULL, 2);
+
+	SDPOKE8(sd, SDSTATE, S_CRC_CHECK | (TYPE_BSYRESP << 5));
+	sd->sd_state &= ~SDDAT_TX;
+	ret = sdcmd(sd, CMD_SET_BLOCKLEN, 512, resp, NULL);
+	if (ret || error(resp, CMD_SET_BLOCKLEN)) return 1;
+	ret = sdcmd(sd, CMD_SEND_STATUS, sd->sd_rcaarg, resp, NULL);
+	if (ret || error(resp, CMD_SEND_STATUS)) return 1;
+
+	if ((cmd == SDLOCK_ERASE || cmd == SDLOCK_UNLOCK ||
+	  cmd == SDLOCK_CLRPWD) && (resp[1] & 0x2)) {
+		return 1;
+	}
+
+	if (sdbootdat) {
+		sdbootdat[0] = SDLOCK_UNLOCK;
+		for (i = 1; i < 18; i++) {
+			sdbootdat[i] = pwddat[i];
+			sd_1bit_feedcrc(sd, pwddat[i]);
+		}
+		sdbootdat[18] = sd_1bit_getcrc(sd);
+		sdbootdat[19] = sd_1bit_getcrc(sd);
+	}
+
+	return 0;
+}
+
+static
+int sdlockctl2(struct sdcore *sd, unsigned int cmd, unsigned char *pwd,
+  unsigned char *sdbootdat)
+{
+	unsigned char pwddat[18];
+	unsigned char *pwdptr = pwddat;
+	unsigned int resp[6];
+	int ret, i, j, len, s;
+	int ccc = (sd->sd_csd[5] << 4) | (sd->sd_csd[6] >> 4);
+
+	if (!(ccc & 0x80)) return 1; // Class 7 is lock-unlock commands
+
+	if (pwd == NULL && cmd != SDLOCK_ERASE) return 1;
+
+	stop2(sd);
+
+	if (sd->sd_state & DATSSP_4BIT) {
+		int ret;
+
+		sdcmd2(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+		sdcmd2(sd, ACMD_SET_BUS_WIDTH, 0, NULL, NULL);
+		sd->sd_state &= ~DATSSP_4BIT;
+		ret = sdlockctl2(sd, cmd, pwd, sdbootdat);
+		sdcmd2(sd, CMD_APP_CMD, sd->sd_rcaarg, NULL, NULL);
+		sdcmd2(sd, ACMD_SET_BUS_WIDTH, 2, NULL, NULL);
+		sd->sd_state |= DATSSP_4BIT;
+		return ret;
+	}
+
+	pwddat[0] = cmd;
+	if (cmd != SDLOCK_ERASE) {
+		pwddat[1] = 16; // length
+		for (i = 0; i < 16; i++) {
+			pwddat[2 + i] = pwd[i];
+		}
+	}
+
+	if (cmd == SDLOCK_ERASE) len = 1; else len = 18;
+	ret = sdcmd2(sd, CMD_SET_BLOCKLEN, len, resp, NULL);
+	if (ret || error(resp, CMD_SET_BLOCKLEN)) return 1;
+	ret = sdcmd2(sd, CMD_LOCK_UNLOCK, 0, resp, NULL);
+	if (ret || error(resp, CMD_LOCK_UNLOCK)) return 1;
+
+	for (i = 0; i < len; i++) {
+		unsigned int b = *pwdptr++;
+		unsigned int x;
+
+		sd_1bit_feedcrc(sd, b);
+		for (j = 0; j < 8; j++) {
+			x = 0x1e | ((b >> 7) & 0x1);
+			b = b << 1;
+			SDPOKE8(sd, SDGPIO, x);  // clk negedge
+			SDPEEK8(sd, SDGPIO);
+			SDPEEK8(sd, SDGPIO);
+			x |= 0x20;
+			SDPOKE8(sd, SDGPIO, x);  // clk posedge
+			SDPEEK8(sd, SDGPIO);
+		}
+	}
+	for (i = 0; i < 2; i++) {
+		unsigned int b = sd_1bit_getcrc(sd);
+		unsigned int x;
+
+		for (j = 0; j < 8; j++) {
+			x = 0x1e | ((b >> 7) & 0x1);
+			b = b << 1;
+			SDPOKE8(sd, SDGPIO, x);  // clk negedge
+			SDPEEK8(sd, SDGPIO);
+			SDPEEK8(sd, SDGPIO);
+			x |= 0x20;
+			SDPOKE8(sd, SDGPIO, x);  // clk posedge
+			SDPEEK8(sd, SDGPIO);
+		}
+	}
+	// End bit
+	SDPOKE8(sd, SDGPIO, 0x1f);  // clk negedge
+	SDPEEK8(sd, SDGPIO);
+	SDPOKE8(sd, SDGPIO, 0xbf);  // clk posedge, tristate dat
+	// CRC ack
+	s = 0;
+	for (i = 0; i < 7; i++) {
+		SDPOKE8(sd, SDGPIO, 0x9f);  // clk negedge
+		SDPEEK8(sd, SDGPIO);        // delay
+		s = s << 1;
+		s |= SDPEEK8(sd, SDGPIO) & 0x1;
+		SDPOKE8(sd, SDGPIO, 0xbf);  // clk posedge
+		SDPEEK8(sd, SDGPIO);
+	}
+	if ((s & 0xf) != 0x5) return 1;
+
+	// wait for unbusy
+	s = 0;
+	while ((s & 0x7) != 0x7) {
+		if (timeout(sd)) break;
+		SDPOKE8(sd, SDGPIO, 0x9f);  // clk negedge
+		SDPEEK8(sd, SDGPIO);        // delay
+		s = s << 1;
+		s |= SDPEEK8(sd, SDGPIO) & 0x1;
+		SDPOKE8(sd, SDGPIO, 0xbf);
+		SDPEEK8(sd, SDGPIO);
+	}
+	for (i = 0; i < 8; i++) {
+		SDPOKE8(sd, SDGPIO, 0x9f);
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xbf);
+		SDPEEK8(sd, SDGPIO);
+	}
+
+	sd->sd_state &= ~SDDAT_TX;
+	ret = sdcmd2(sd, CMD_SET_BLOCKLEN, 512, resp, NULL);
+	if (ret || error(resp, CMD_SET_BLOCKLEN)) {
+		return 1;
+	}
+	ret = sdcmd2(sd, CMD_SEND_STATUS, sd->sd_rcaarg, resp, NULL);
+	if (ret || error(resp, CMD_SEND_STATUS)) {
+		return 1;
+	}
+
+	if ((cmd == SDLOCK_ERASE || cmd == SDLOCK_UNLOCK ||
+	  cmd == SDLOCK_CLRPWD) && (resp[1] & 0x2)) {
+		return 1;
+	}
+
+	if (sdbootdat) {
+		sdbootdat[0] = SDLOCK_UNLOCK;
+		for (i = 1; i < 18; i++) {
+			sdbootdat[i] = pwddat[i];
+			sd_1bit_feedcrc(sd, pwddat[i]);
+		}
+		sdbootdat[18] = sd_1bit_getcrc(sd);
+		sdbootdat[19] = sd_1bit_getcrc(sd);
+	}
+
+	for (i = 0; i < 8; i++) {
+		SDPOKE8(sd, SDGPIO, 0x9f);
+		SDPEEK8(sd, SDGPIO);
+		SDPEEK8(sd, SDGPIO);
+		SDPOKE8(sd, SDGPIO, 0xbf);
+		SDPEEK8(sd, SDGPIO);
+	}
+	return 0;
+}
+#endif
diff --git a/drivers/block/sdcore2.h b/drivers/block/sdcore2.h
new file mode 100644
index 0000000..38d5b96
--- /dev/null
+++ b/drivers/block/sdcore2.h
@@ -0,0 +1,372 @@
+/*
+ * Copyright (c) 2006-2008, Technologic Systems
+ * All rights reserved.
+ */
+
+#ifndef _SDCORE_H_
+#define _SDCORE_H_
+
+// Additional missing defs
+#define SDCMD 0                // cmd register
+#define SDDAT 1                // data register
+#define SDSTATE 2              // state register
+#define SDCTRL 3               // ctrl register
+
+
+// this bit is set when no card inserted
+#define SDCTRL_CARD_ABSENT 0x08
+
+
+
+/* public bits for sd_state bitfield, can be read from client code.
+ * Do not write!  Other bits are used internally.
+ */
+#define SDDAT_RX	(1<<0)
+#define SDDAT_TX	(1<<1)
+#define SDCMD_RX	(1<<2)
+#define SDCMD_TX	(1<<3)
+
+// used to disable CRC calculations in write mode
+#define SDCRC_DISABLE   (1 << 4)
+
+
+
+// used to choose between 4 bit crc mode and 1 bit crc mode
+
+// note - likely set in sdreset when configuring interface bit width
+#define SDSSP_4BIT_MODE (1 << 5)
+
+// SD_ADDRESSING_DIRECT means that sd card addresses
+// will be communicated in read/write mode using
+// full offsets, not 512 byte block offsets.
+// the core will mulitply the address by 512 if this
+// bit is cleared
+#define SD_ADDRESSING_DIRECT (1 << 6)
+
+
+/* These structs should start intialized to all 0's (bzero()'ed).  Proper
+ * operation can be assured by setting sd_regstart, and the os_delay
+ * callback.  sdreset() should be called to initialize the core, then
+ * sdread() and sdwrite() can be used.
+ */
+struct sdcore {
+	/* virtual address of SD block register start, to be filled in
+	 * by client code before calling any sdcore functions.
+	 */
+  // 0-3
+	unsigned char* sd_regstart;
+
+
+
+  // 4-7
+	unsigned int sd_state;
+
+	/* Erase hint for subsequent sdwrite() call, used to optimize
+	 * write throughput on multi-sector writes by pre-erasing this
+	 * many sectors.
+	 */
+  // 8-11
+	unsigned int sd_erasehint;
+
+	/* Following this comment are 5 function pointer declarations to
+	 * OS helper functions.  The 'os_arg' member is passed as the
+	 * first argument to the helpers and should be set by
+	 * client code before issueing sdreset()
+	 *
+	 * os_dmastream(os_arg, buf, buflen)
+	 * This function should look at sd_state and set up and run an
+	 * appropriate DMA transfer.  If buf is NULL, callee doesn't care
+	 * about the actual data sent/received and helper function
+	 * can do whatever it wants.  Should return 0 when DMA transfer was
+	 * run and completed successfully.  If this function pointer is
+	 * NULL, PIO methods of transfer will be used instead of DMA.
+	 *
+	 * os_dmaprep(os_arg, buf, buflen)
+	 * This function is used to prepare an area of memory for a possible
+	 * DMA transfer.  This function is called once per distinct buffer
+	 * passed in.  After this function is called, os_dmastream() may be
+	 * called one or more times (for sequential addresses) on subregions
+	 * of the address range passed here.  Should write-back or invalidate
+	 * L1 cache lines and possibly look up physical addresses for buf
+	 * passed in if I/O buffers.  If 'os_dmaprep' is set to NULL, function
+	 * call will not happen. (though os_dmastream() calls may still)
+	 *
+	 * os_delay(os_arg, microseconds)
+	 * This function is supposed to delay or stall the processor for
+	 * the passed in value number of microseconds.
+	 *
+	 * os_irqwait(os_arg, type)
+	 * Called at certain times to request to be put to sleep/block until
+	 * an SD interrupt occurs.  It is not critical to set this function.
+	 * When NULL, the sdcore routines simply busy-wait.
+	 *
+	 * os_powerok(os_arg)
+	 * Experimental callback function -- set to NULL for now.
+	 */
+  // 12-15
+	void *os_arg;
+  // 16-19
+	int (*os_dmastream)(void *, unsigned char *, unsigned int);
+  // 20-23
+	void (*os_dmaprep)(void *, unsigned char *, unsigned int);
+  // 24-27
+	void (*os_delay)(void *, unsigned int);
+  // 28-31
+	void (*os_irqwait)(void *, unsigned int);
+  // 32-35
+	int (*os_powerok)(void *);
+
+	int (*os_timeout)(void *);
+	int (*os_reset_timeout)(void *);
+
+	/* If the SD card last successfully reset is write protected, this
+	 * member will be non-zero.
+	 */
+  // 36-39
+	unsigned int sd_wprot;
+
+	/* If this card may have been already initialized by TS-SDBOOT, place
+	 * the magic token it placed in the EP93xx SYSCON ScratchReg1 here
+	 * before calling sdreset() to avoid re-initialization.
+	 */
+  // 40-43
+	unsigned int sdboot_token;
+
+	/* CRC hint for subsequent sdwrite() call, used to optimize
+	 * write throughput while using DMA by pre-calculating CRC's for
+	 * next write.  NULL means no hint supplied.
+	 */
+  // 44-47
+	unsigned char *sd_crchint;
+
+	/* The block size of the memory device.  Normally 512, but can be 1024
+	 * for larger cards.  Read-only member and actually not very useful.
+	 */
+  // 48-51
+	unsigned int sd_blocksize;
+
+	/* Password for auto-unlocking in sdreset()
+	 */
+  // 52-55
+	unsigned char *sd_pwd;
+
+	/* If the SD card was password locked, this will be non-zero after
+	 * unsuccessful sdreset().
+	 */
+  // 56-59
+	unsigned int sd_locked;
+
+	/* Whether or not writes can be parked.  Definitely should be set to 1
+	 * as writes are very slow without it.
+	 */
+  // 60-63
+	unsigned int sd_writeparking;
+
+	/* Logical unit number.  Some SD cores will have multiple card slots.
+	 * LUN #0 is the first.
+	 */
+  // 64-67
+	unsigned int sd_lun;
+
+	/* The rest of these members are for private internal use and should
+	 * not be of interest to client code.
+	 */
+
+
+  // 68-71
+  unsigned int rca;  // relative card address
+
+
+  unsigned int sd_csd[17];
+  /*
+
+
+  // 72 -75    0
+  unsigned int unknown72;      // one of the csds?
+
+  // 76 -79    1
+  unsigned int unknown76;      // csd 0x00
+  // 80 -83    2
+  unsigned int unknown04;      // csd 0x01
+  // 84 - 87   3
+  unsigned int unknown05;      // csd 0x02
+  // 88 - 91   4
+  unsigned int unknown06;      // csd 0x03
+  // 92 - 95   5
+  unsigned int unknown92;      // csd 0x04
+  // 96 - 100  6
+  unsigned int unknown96;      // csd 0x05
+  // 100 - 103 7
+  unsigned int unknown100;     // csd 0x06
+  // 104 - 107 8
+  unsigned int unknown104;     // csd 0x07
+  // 108 - 111 9
+  unsigned int unknown108;     // csd 0x08
+  // 112 - 115 10
+  unsigned int unknown112;     // csd 0x09
+  // 116 - 119 11
+  unsigned int unknown116;     // csd 0x0a
+  // 120       12
+  unsigned int unknown24;      // csd 0x0b
+  // 124       13
+  unsigned int unknown25;      // csd 0x0c
+  // 128       14
+  unsigned int unknown26;      // csd 0x0d
+  // 132       15
+  unsigned int unknown132;      // csd 0x0e
+  // 136       16
+  unsigned int unknown28;      // csd 0x0f
+  */
+
+
+  // 140
+  unsigned int sd_crc_shift;
+
+  // 144 + 4 + 4
+  unsigned short s_crc_table[4]; // 4 shorts
+  // 152 + 4 + 4 + 4 + 4
+  unsigned int   l_crc_table[4]; // 4 longs
+
+  // 168
+  unsigned int sd_timeout;       // used to busy wait
+
+  // 172
+  unsigned int sd_cur_sector;    // stop indicator - if zero , then stop procedure will be skipped
+
+  // 176
+  unsigned int sdcore_version;   // hardware version
+  // 180
+  unsigned int unknown39;
+  // 184
+  unsigned int unknown40;
+  // 188
+  unsigned int sdcore_sdsize;
+
+
+
+  unsigned int unknown42;
+  unsigned int unknown43;
+  unsigned int unknown44;
+  unsigned int unknown45;
+  unsigned int unknown46;
+  unsigned int unknown47;
+
+
+
+
+
+
+
+
+
+
+
+
+
+};
+
+/* I believe sdcores is a table mapping
+   id -> sdcore struct.  The table is
+   64 long, meaning that one could build a ts device with
+   64 sdcores on it.
+*/
+//extern unsigned char sdcores[256];
+
+
+
+
+/* For sdreadv() / sdwritev() */
+struct sdiov {
+  unsigned char *sdiov_base;
+  unsigned int sdiov_nsect;
+};
+
+
+
+
+int sdreset(struct sdcore *);
+
+int sdsize(struct sdcore* sdcore);
+
+
+int sdread(struct sdcore* sdcore,
+	   unsigned int sector,
+	   unsigned char* buffer,
+	   int nsect);
+
+int sdwrite(struct sdcore *, unsigned int, unsigned char *, int);
+
+
+// same signature as do_read
+int do_read(struct sdcore*, unsigned int, struct sdiov*, int);
+int sdreadv(struct sdcore * sdcore,
+	    unsigned int sector,
+	    struct sdiov * sdiov,
+	    int nsdiov);
+
+// same signature as do_write
+int do_write(struct sdcore* sdcore,
+	     unsigned int sector,
+	     struct sdiov* sdiov,
+	     int nsdiov);
+
+int sdwritev(struct sdcore *, unsigned int, struct sdiov *, int);
+
+
+void sd_1bit_feedcrc(struct sdcore*, unsigned int);
+void sd_4bit_feedcrc(struct sdcore*, unsigned int);
+
+unsigned char sd_1bit_getcrc(struct sdcore*);
+unsigned char sd_4bit_getcrc(struct sdcore*);
+
+/** stop takes only sdcore parameters */
+int stop(struct sdcore*);
+
+
+int tend_ssp(struct sdcore* sdcore,
+	     unsigned int** unknown_r1,      // r1
+	     unsigned char** unknown_r2);
+
+
+int datssp_stream(struct sdcore* sdcore,
+		  unsigned char** data,
+		  int count);
+
+/*
+ * @param cmd is the command - I believe that the lower byte is the command, and
+ *       the upper one is the crc
+ *
+ * @param data is a character buffer for data received in the ssp dat register, as
+ * a result of a command execution.
+ */
+
+int sdcmd(struct sdcore* sdcore,
+	  unsigned short cmd,
+	  unsigned int sdargs,
+	  unsigned int* response,
+	  unsigned char** data); // command response buffer?
+
+
+/**
+ * Error tests if a sdcommand error has been received.
+ * It does this by checking that the command was
+ * correctly returned by the card (the first byte in buffer),
+ * and that a CRC error has not occurred.  IF one has occurred
+ * it will attempt a 1bit fix. (suspected)
+ */
+int error(unsigned int* buffer, unsigned int cmd);
+
+
+
+int sdsetwprot(struct sdcore *, unsigned int);
+#define SDLOCK_UNLOCK	0
+#define SDLOCK_SETPWD	1
+#define SDLOCK_CLRPWD	2
+#define SDLOCK_ERASE	8
+
+int sdlockctl(struct sdcore *,
+	      unsigned int,      // op code
+	      unsigned char *,
+	      unsigned char *);
+
+#endif
diff --git a/drivers/block/tssdcard.c b/drivers/block/tssdcard.c
new file mode 100644
index 0000000..c76d9a7
--- /dev/null
+++ b/drivers/block/tssdcard.c
@@ -0,0 +1,415 @@
+/*
+ * TS SD Card device driver
+ *
+ * (c) Copyright 2010  Matthieu Crapet <mcrapet@gmail.com>
+ * Based on Technologic Systems & Breton M. Saunders work
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ * Notes:
+ *   - request processing method is: no request queue
+ *   - no M2M DMA is used
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/major.h>
+#include <linux/blkdev.h>
+#include <linux/bio.h>
+#include <linux/delay.h>
+#include <linux/hdreg.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include "sdcore2.h"
+
+#define SDCARD_DEV_NAME          "tssd" /* will appear in /proc/partitions & /sys/class/block */
+#define SD_SHIFT                 4      /* max 16 partitions = 1 << 4 */
+
+#define KERN_SECTOR_SIZE         512    /* in bytes */
+#define HARD_SECTOR_SIZE         512    /* in bytes */
+#define HARD_2_KERN_SECTOR_RATIO 1      /* 1 kernel sector = 1 hardware sector */
+
+
+struct ts72xx_sdcard_device {
+	struct sdcore tssdcore;         /* Physical core layer */
+	void __iomem *mmio_base;
+	long size;                      /* Device size in (hardware) sectors */
+	int id;
+	int media_change;
+	int users;
+
+	spinlock_t lock;
+	struct device *dev;
+	struct request_queue *queue;
+	struct gendisk *disk;
+};
+
+
+/*
+ * Low level function to handle an I/O request
+ */
+static inline int sdcard_ll_transfer(struct ts72xx_sdcard_device *dev,
+		unsigned long sector, unsigned long nsect, char *buffer, int rw)
+{
+	int ret;
+
+	//spin_unlock(&dev->lock); // ???
+
+	if ((sector + nsect) > (dev->size * HARD_2_KERN_SECTOR_RATIO)) {
+		dev_err(dev->dev, "tranfer: beyond-end write (%ld %ld)\n", sector, nsect);
+		//spin_lock(&dev->lock); // ???
+		return -1;
+	}
+
+	switch (rw) {
+		case WRITE:
+			ret = sdwrite(&dev->tssdcore, sector, buffer, nsect);
+			if (ret && !dev->tssdcore.sd_wprot) {
+				sdreset(&dev->tssdcore);
+				ret = sdwrite(&dev->tssdcore, sector, buffer, nsect);
+			}
+			break;
+
+		case READ:
+		case READA:
+			ret = sdread(&dev->tssdcore, sector, buffer, nsect);
+			if (ret) {
+				// SDCARD RESET may be printed when the core determines that the SD card has
+				// f*ed up.this is not handled correctly yet; and should likely be inside a while loop
+				dev_err(dev->dev, "transfer: SDCARD RESET\n");
+				sdreset(&dev->tssdcore);
+				ret = sdread(&dev->tssdcore, sector, buffer, nsect);
+			}
+			break;
+	}
+
+	//spin_lock(&dev->lock); // ???
+	return 0;
+}
+
+/*
+ * The direct make request version.
+ */
+static int sdcard_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct ts72xx_sdcard_device *dev = q->queuedata;
+
+	struct bio_vec *bvec;
+	sector_t sector;
+	int i, rw;
+	int err = -EIO;
+
+	/* handle bio */
+	sector = bio->bi_sector;
+	rw = bio_rw(bio);
+
+	bio_for_each_segment(bvec, bio, i) {
+		char *buffer = __bio_kmap_atomic(bio, i, KM_USER0);
+		unsigned int len = bvec->bv_len / HARD_SECTOR_SIZE;
+
+		//printk("bvec: len=%d offt=%d page=%p\n", bvec->bv_len, bvec->bv_offset, bvec->bv_page);
+
+		err = sdcard_ll_transfer(dev, sector, len, buffer, rw);
+		if (err)
+			break;
+
+		sector += len;
+		__bio_kunmap_atomic(bio, KM_USER0);
+	}
+	bio_endio(bio, err);
+
+	return 0;
+}
+
+static void sdcard_delay(void *arg, unsigned int us)
+{
+	udelay(us);
+}
+
+static int sdcard_open(struct block_device *bdev, fmode_t mode)
+{
+	struct ts72xx_sdcard_device *dev = bdev->bd_disk->private_data;
+	unsigned long flags;
+
+	dev_dbg(dev->dev, "open() users=%i\n", dev->users + 1);
+
+	spin_lock_irqsave(&dev->lock, flags);
+	dev->users++;
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	check_disk_change(bdev);
+	return 0;
+};
+
+static int sdcard_release(struct gendisk *disk, fmode_t mode)
+{
+	struct ts72xx_sdcard_device *dev = disk->private_data;
+	unsigned long flags;
+
+	dev_dbg(dev->dev, "release() users=%i\n", dev->users - 1);
+
+	spin_lock_irqsave(&dev->lock, flags);
+	dev->users--;
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	return 0;
+}
+
+static int sdcard_media_changed(struct gendisk *disk)
+{
+	struct ts72xx_sdcard_device *dev = disk->private_data;
+
+	char buf[HARD_SECTOR_SIZE];
+	dev->media_change = sdread(&dev->tssdcore, 1, buf, 1);
+
+	dev_dbg(dev->dev, "media_changed() %i\n", dev->media_change);
+	return dev->media_change;
+}
+
+static int sdcard_revalidate(struct gendisk *disk)
+{
+	struct ts72xx_sdcard_device *dev = disk->private_data;
+	int ret = 0;
+
+	dev_dbg(dev->dev, "revalidate() %i\n", dev->media_change);
+	if (dev->media_change) {
+		dev->size = sdreset(&dev->tssdcore);
+		set_disk_ro(dev->disk, !!(dev->tssdcore.sd_wprot));
+		if (dev->size > 0) {
+			set_capacity(dev->disk, dev->size * HARD_2_KERN_SECTOR_RATIO);
+			dev->media_change = 0;
+		} else {
+			dev_err(dev->dev, "revalidate() no card found\n");
+			ret = -1;
+		}
+	}
+	return ret;
+}
+
+static int sdcard_getgeo(struct block_device *bdev, struct hd_geometry *geo)
+{
+	struct gendisk *disk = bdev->bd_disk;
+	struct ts72xx_sdcard_device *dev = disk->private_data;
+
+	/* We don't have real geometry info, but let's at least return
+	 * values consistent with the size of the device */
+	geo->heads = 16;
+	geo->sectors = 32;
+	geo->cylinders = get_capacity(disk) / (16 * 32);
+
+	dev_dbg(dev->dev, "getgeo() %d heads, %d sectors, %d cylinders\n",
+			geo->heads, geo->sectors, geo->cylinders);
+	return 0;
+}
+
+/*
+ * The device operations structure.
+ */
+static struct block_device_operations ts72xx_sdcard_ops = {
+	.owner			= THIS_MODULE,
+	.open			= sdcard_open,
+	.release		= sdcard_release,
+	.media_changed		= sdcard_media_changed,
+	.revalidate_disk	= sdcard_revalidate,
+	.getgeo			= sdcard_getgeo
+};
+
+static int sdcard_major;
+
+/* ---------------------------------------------------------------------
+ * Device setup
+ */
+
+static int ts72xx_sdcard_setup(const char *name, struct ts72xx_sdcard_device *dev)
+{
+	int rc;
+
+	spin_lock_init(&dev->lock);
+
+	/*
+	 * Initialize the request queue
+	 */
+	dev->queue = blk_alloc_queue(GFP_KERNEL);
+	if (!dev->queue)
+		goto err_alloc_queue;
+
+	dev->queue->queuedata = dev;
+	blk_queue_make_request(dev->queue, sdcard_make_request);
+	blk_queue_logical_block_size(dev->queue, HARD_SECTOR_SIZE);
+
+      	dev->tssdcore.sd_regstart = (unsigned char *)dev->mmio_base;
+	dev->tssdcore.os_arg       = dev;
+	dev->tssdcore.os_delay     = sdcard_delay;
+	dev->tssdcore.os_dmastream = NULL;
+	dev->tssdcore.os_dmaprep   = NULL;
+
+	// don't want to write park
+	dev->tssdcore.sd_writeparking = 1;
+	// I do want to pre-erase blocks - 8 blocks pre-erase
+	dev->tssdcore.sd_erasehint = 8;
+	dev->tssdcore.sdboot_token = 0;
+
+	dev->disk = alloc_disk(1 << SD_SHIFT);
+	if (!dev->disk) {
+		goto err_alloc_disk;
+	}
+
+	dev->disk->major = sdcard_major;
+	dev->disk->first_minor = dev->id << SD_SHIFT;
+	dev->disk->flags = GENHD_FL_REMOVABLE;
+	dev->disk->fops = &ts72xx_sdcard_ops;
+	dev->disk->queue = dev->queue;
+	dev->disk->private_data = dev;
+	snprintf(dev->disk->disk_name, 32, SDCARD_DEV_NAME "%c", dev->id + 'a');
+
+	/* SD Card size and Reset
+	 * (set_disk_ro, set_capacity will be called) */
+	dev->media_change = 1;
+	rc = sdcard_revalidate(dev->disk);
+	if (rc) {
+		dev_info(dev->dev, "No SD card detected!\n");
+		goto err_alloc_disk;
+	}
+
+	dev_info(dev->dev, "SD card hardware revision: %08x\n",
+			dev->tssdcore.sdcore_version);
+	dev_info(dev->dev, "block device major number = %d\n",
+			sdcard_major);
+	dev_info(dev->dev, "New SD card detected, name=%s size=%ld (sectors)\n",
+			dev->disk->disk_name, dev->size);
+
+	/* Make the sysace device 'live' */
+	add_disk(dev->disk);
+
+	return 0;
+
+err_alloc_disk:
+	blk_cleanup_queue(dev->queue);
+err_alloc_queue:
+	return -ENOMEM;
+}
+
+
+/* ---------------------------------------------------------------------
+ * Platform drivers functons
+ */
+
+static int __init ts72xx_sdcard_probe(struct platform_device *pdev)
+{
+	struct ts72xx_sdcard_device *dev;
+	struct resource *res;
+	int rc;
+
+	dev = kzalloc(sizeof(struct ts72xx_sdcard_device), GFP_KERNEL);
+	if (!dev) {
+		rc = -ENOMEM;
+		goto fail_no_mem;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		rc = -ENXIO;
+		goto fail_no_mem_resource;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		rc = -EBUSY;
+		goto fail_no_mem_resource;
+	}
+
+	dev->mmio_base = ioremap(res->start, resource_size(res));
+	if (dev->mmio_base == NULL) {
+		rc = -ENXIO;
+		goto fail_no_ioremap;
+	}
+
+	dev->dev = &pdev->dev;
+	dev->id = pdev->id;
+	platform_set_drvdata(pdev, dev);
+
+	rc = ts72xx_sdcard_setup(SDCARD_DEV_NAME, dev);
+	if (rc) {
+		dev_err(dev->dev, "ts72xx_sdcard_setup failed\n");
+		goto fail_sdcard_setup;
+	}
+
+	return 0;
+
+fail_sdcard_setup:
+	iounmap(dev->mmio_base);
+fail_no_ioremap:
+	release_mem_region(res->start, resource_size(res));
+fail_no_mem_resource:
+	kfree(dev);
+fail_no_mem:
+	return rc;
+}
+
+static int __exit ts72xx_sdcard_remove(struct platform_device *pdev)
+{
+	struct ts72xx_sdcard_device *dev = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	platform_set_drvdata(pdev, NULL);
+	iounmap(dev->mmio_base);
+	release_mem_region(res->start, resource_size(res));
+	blk_cleanup_queue(dev->queue);
+	del_gendisk(dev->disk);
+	put_disk(dev->disk);
+	kfree(dev);
+
+	return 0;
+}
+
+static struct platform_driver ts72xx_sdcard_driver = {
+	.driver		= {
+		.name	= "ts72xx-sdcard",
+		.owner	= THIS_MODULE,
+	},
+	.remove		= __exit_p(ts72xx_sdcard_remove),
+};
+
+
+/* ---------------------------------------------------------------------
+ * Module init/exit routines
+ */
+
+static int __init ts72xx_sdcard_init(void)
+{
+	int rc;
+
+	sdcard_major = rc = register_blkdev(sdcard_major, SDCARD_DEV_NAME);
+	if (rc <= 0) {
+		printk(KERN_ERR "%s:%u: register_blkdev failed %d\n", __func__,
+				__LINE__, rc);
+		return rc;
+	}
+
+	rc = platform_driver_probe(&ts72xx_sdcard_driver, ts72xx_sdcard_probe);
+	if (rc)
+		unregister_blkdev(sdcard_major, SDCARD_DEV_NAME);
+
+	return rc;
+}
+
+static void __exit ts72xx_sdcard_exit(void)
+{
+	unregister_blkdev(sdcard_major, SDCARD_DEV_NAME);
+	platform_driver_unregister(&ts72xx_sdcard_driver);
+}
+
+module_init(ts72xx_sdcard_init);
+module_exit(ts72xx_sdcard_exit);
+
+MODULE_AUTHOR("Matthieu Crapet <mcrapet@gmail.com>");
+MODULE_DESCRIPTION("TS72xx SD Card block driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_BLOCKDEV_MAJOR(SCSI_DISK0_MAJOR);
+MODULE_ALIAS("tssd");
-- 
1.7.1