Patchwork [Precise,LTS,1/2] add Realtek 5229 card reader staging driver

login
register
mail settings
Submitter Adam Lee
Date Jan. 17, 2013, 2:52 a.m.
Message ID <1358391167-10780-1-git-send-email-adam.lee@canonical.com>
Download mbox | patch
Permalink /patch/213126/
State New
Headers show

Comments

Adam Lee - Jan. 17, 2013, 2:52 a.m.
BugLink: http://launchpad.net/bugs/1057089

Realtek pushed a non-staging version into upstream(
67d16a4686c9b94c8f52a66afe7521909aeb75b4), but it crashes with ThinkPad
E430 and E530, so I ported this official staging driver to our kernel.

Tested with ThinkPad E430, the card reader works well.

Cc: Ming Lei <ming.lei@canonical.com>
Signed-off-by: Adam Lee <adam.lee@canonical.com>
---
 drivers/staging/Kconfig                  |    2 +
 drivers/staging/Makefile                 |    1 +
 drivers/staging/rts5229/Kconfig          |   15 +
 drivers/staging/rts5229/Makefile         |   13 +
 drivers/staging/rts5229/debug.h          |   49 +
 drivers/staging/rts5229/define.debug     |   29 +
 drivers/staging/rts5229/define.release   |   29 +
 drivers/staging/rts5229/general.c        |   38 +
 drivers/staging/rts5229/general.h        |   35 +
 drivers/staging/rts5229/ms.c             | 4600 ++++++++++++++++++++++++
 drivers/staging/rts5229/ms.h             |  226 ++
 drivers/staging/rts5229/rtsx.c           | 1094 ++++++
 drivers/staging/rts5229/rtsx.h           |  230 ++
 drivers/staging/rts5229/rtsx_card.c      |  993 ++++++
 drivers/staging/rts5229/rtsx_card.h      |  779 +++++
 drivers/staging/rts5229/rtsx_chip.c      | 2097 +++++++++++
 drivers/staging/rts5229/rtsx_chip.h      |  973 ++++++
 drivers/staging/rts5229/rtsx_scsi.c      | 3322 ++++++++++++++++++
 drivers/staging/rts5229/rtsx_scsi.h      |  186 +
 drivers/staging/rts5229/rtsx_sys.h       |   57 +
 drivers/staging/rts5229/rtsx_transport.c |  890 +++++
 drivers/staging/rts5229/rtsx_transport.h |   69 +
 drivers/staging/rts5229/sd.c             | 5570 ++++++++++++++++++++++++++++++
 drivers/staging/rts5229/sd.h             |  293 ++
 drivers/staging/rts5229/trace.h          |  159 +
 25 files changed, 21749 insertions(+)
 create mode 100644 drivers/staging/rts5229/Kconfig
 create mode 100644 drivers/staging/rts5229/Makefile
 create mode 100644 drivers/staging/rts5229/debug.h
 create mode 100644 drivers/staging/rts5229/define.debug
 create mode 100644 drivers/staging/rts5229/define.release
 create mode 100644 drivers/staging/rts5229/general.c
 create mode 100644 drivers/staging/rts5229/general.h
 create mode 100644 drivers/staging/rts5229/ms.c
 create mode 100644 drivers/staging/rts5229/ms.h
 create mode 100644 drivers/staging/rts5229/rtsx.c
 create mode 100644 drivers/staging/rts5229/rtsx.h
 create mode 100644 drivers/staging/rts5229/rtsx_card.c
 create mode 100644 drivers/staging/rts5229/rtsx_card.h
 create mode 100644 drivers/staging/rts5229/rtsx_chip.c
 create mode 100644 drivers/staging/rts5229/rtsx_chip.h
 create mode 100644 drivers/staging/rts5229/rtsx_scsi.c
 create mode 100644 drivers/staging/rts5229/rtsx_scsi.h
 create mode 100644 drivers/staging/rts5229/rtsx_sys.h
 create mode 100644 drivers/staging/rts5229/rtsx_transport.c
 create mode 100644 drivers/staging/rts5229/rtsx_transport.h
 create mode 100644 drivers/staging/rts5229/sd.c
 create mode 100644 drivers/staging/rts5229/sd.h
 create mode 100644 drivers/staging/rts5229/trace.h
Tim Gardner - Jan. 17, 2013, 1:23 p.m.
On 01/16/2013 07:52 PM, Adam Lee wrote:
> BugLink: http://launchpad.net/bugs/1057089
>
> Realtek pushed a non-staging version into upstream(
> 67d16a4686c9b94c8f52a66afe7521909aeb75b4), but it crashes with ThinkPad
> E430 and E530, so I ported this official staging driver to our kernel.
>
> Tested with ThinkPad E430, the card reader works well.
>
> Cc: Ming Lei <ming.lei@canonical.com>
> Signed-off-by: Adam Lee <adam.lee@canonical.com>
> ---

Perhaps you should spend a little time figuring out _why_ this driver 
crashes on a Thinkpad. I'm not going to merge a driver that has been 
essentially abandoned, e.g., moved out of staging. I want the real thing 
that comes with upstream stable and security updates. Furthermore, I 
want a proper pull request for a patch of this size.

rtg
Adam Lee - Jan. 17, 2013, 1:40 p.m.
On Thu, Jan 17, 2013 at 06:23:57AM -0700, Tim Gardner wrote:
> Perhaps you should spend a little time figuring out _why_ this
> driver crashes on a Thinkpad. I'm not going to merge a driver that
> has been essentially abandoned, e.g., moved out of staging. I want
> the real thing that comes with upstream stable and security updates.
> Furthermore, I want a proper pull request for a patch of this size.

I am trying to fix the upstream dirver. Thanks for the advice.

Patch

diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index ea57692..c41c2e3 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -58,6 +58,8 @@  source "drivers/staging/rts_pstor/Kconfig"
 
 source "drivers/staging/rts5139/Kconfig"
 
+source "drivers/staging/rts5229/Kconfig"
+
 source "drivers/staging/frontier/Kconfig"
 
 source "drivers/staging/pohmelfs/Kconfig"
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 3ded8cd..3585e26 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -21,6 +21,7 @@  obj-$(CONFIG_RTL8192E)		+= rtl8192e/
 obj-$(CONFIG_R8712U)		+= rtl8712/
 obj-$(CONFIG_RTS_PSTOR)		+= rts_pstor/
 obj-$(CONFIG_RTS5139)		+= rts5139/
+obj-$(CONFIG_RTS5229)		+= rts5229/
 obj-$(CONFIG_SPECTRA)		+= spectra/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
 obj-$(CONFIG_POHMELFS)		+= pohmelfs/
diff --git a/drivers/staging/rts5229/Kconfig b/drivers/staging/rts5229/Kconfig
new file mode 100644
index 0000000..75591b3
--- /dev/null
+++ b/drivers/staging/rts5229/Kconfig
@@ -0,0 +1,15 @@ 
+config RTS5229
+	tristate "Realtek RTS5229 Card Reader support"
+	depends on PCI && SCSI
+	help
+	  Say Y here to include driver code to support the Realtek
+	  PCI-E card readers.
+
+	  If this driver is compiled as a module, it will be named rts_5229.
+
+config RTS5229_DEBUG
+	bool "Realtek RTS5229 Card Reader verbose debug"
+	depends on RTS5229
+	help
+	  Say Y here in order to have the rts_5229 code generate
+	  verbose debugging messages.
diff --git a/drivers/staging/rts5229/Makefile b/drivers/staging/rts5229/Makefile
new file mode 100644
index 0000000..b7440d7
--- /dev/null
+++ b/drivers/staging/rts5229/Makefile
@@ -0,0 +1,13 @@ 
+ccflags := -Idrivers/scsi
+
+obj-$(CONFIG_RTS5229)	:= rts5229.o
+
+rts5229-y :=				\
+		rtsx.o			\
+		rtsx_chip.o		\
+		rtsx_transport.o	\
+		rtsx_scsi.o		\
+		rtsx_card.o		\
+		general.o		\
+		sd.o			\
+		ms.o
diff --git a/drivers/staging/rts5229/debug.h b/drivers/staging/rts5229/debug.h
new file mode 100644
index 0000000..87c22cc
--- /dev/null
+++ b/drivers/staging/rts5229/debug.h
@@ -0,0 +1,49 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_DEBUG_H
+#define __REALTEK_RTSX_DEBUG_H
+
+#include <linux/kernel.h>
+
+#define DBG 0
+
+#define RTSX_STOR "rts5229: "
+
+#if DBG
+#define DEBUGP(x...) printk( KERN_DEBUG RTSX_STOR x )
+#define DEBUGPN(x...) printk( KERN_DEBUG x )
+#define DEBUGPX(x...) printk( x )
+#define DEBUG(x) x
+#else
+#define DEBUGP(x...)
+#define DEBUGPN(x...)
+#define DEBUGPX(x...)
+#define DEBUG(x)
+#endif
+
+#define RTSX_DEBUGP(x) DEBUGP x
+#define RTSX_DEBUGPN(x) DEBUGPN x
+#define RTSX_DEBUG(x) DEBUG(x)
+
+#endif
diff --git a/drivers/staging/rts5229/define.debug b/drivers/staging/rts5229/define.debug
new file mode 100644
index 0000000..3892f7e
--- /dev/null
+++ b/drivers/staging/rts5229/define.debug
@@ -0,0 +1,29 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_DEFINE_H
+#define __REALTEK_RTSX_DEFINE_H
+
+#define DBG	1
+
+#endif  // __REALTEK_RTSX_DEFINE_H
diff --git a/drivers/staging/rts5229/define.release b/drivers/staging/rts5229/define.release
new file mode 100644
index 0000000..8f44aa9
--- /dev/null
+++ b/drivers/staging/rts5229/define.release
@@ -0,0 +1,29 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_DEFINE_H
+#define __REALTEK_RTSX_DEFINE_H
+
+#define DBG	0
+
+#endif  // __REALTEK_RTSX_DEFINE_H
diff --git a/drivers/staging/rts5229/general.c b/drivers/staging/rts5229/general.c
new file mode 100644
index 0000000..81009f6
--- /dev/null
+++ b/drivers/staging/rts5229/general.c
@@ -0,0 +1,38 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include "general.h"
+
+#if DBG
+int trigger_enabled = 1;
+#endif
+
+int bit1cnt_long(u32 data)
+{
+	int i, cnt = 0;
+	for (i = 0; i < 32; i++) {
+		if (data & 0x01)
+			cnt++;
+		data >>= 1;
+	}
+	return cnt;
+}
diff --git a/drivers/staging/rts5229/general.h b/drivers/staging/rts5229/general.h
new file mode 100644
index 0000000..94fba25
--- /dev/null
+++ b/drivers/staging/rts5229/general.h
@@ -0,0 +1,35 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __RTSX_GENERAL_H
+#define __RTSX_GENERAL_H
+
+#include "rtsx.h"
+
+#if DBG
+extern int trigger_enabled;
+#endif
+
+int bit1cnt_long(u32 data);
+
+#endif
diff --git a/drivers/staging/rts5229/ms.c b/drivers/staging/rts5229/ms.c
new file mode 100644
index 0000000..f513d41
--- /dev/null
+++ b/drivers/staging/rts5229/ms.c
@@ -0,0 +1,4600 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "ms.h"
+
+static inline void ms_set_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+
+	ms_card->err_code = err_code;
+
+#if DBG
+	if (err_code != MS_NO_ERROR) {
+		int i;
+		for (i = 0; i < 4; i++) {
+			rtsx_readl(chip, RTSX_HCBAR + i * 4);
+		}
+		CATCH_TRIGGER1(chip);
+	}
+#endif
+}
+
+static inline int ms_check_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+
+	return (ms_card->err_code == err_code);
+}
+
+static int ms_parse_err_code(struct rtsx_chip *chip)
+{
+	TRACE_RET(chip, STATUS_FAIL);
+}
+
+static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode, u8 tpc,
+			   u8 cnt, u8 cfg)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	u8 *ptr;
+
+	RTSX_DEBUGP(("ms_transfer_tpc: tpc = 0x%x\n", tpc));
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+		     MS_TRANSFER_START | trans_mode);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
+		     MS_TRANSFER_END);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval < 0) {
+		rtsx_clear_ms_error(chip);
+		ms_set_err_code(chip, MS_TO_ERROR);
+		TRACE_RET(chip, ms_parse_err_code(chip));
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if (!(tpc & 0x08)) {
+		if (*ptr & MS_CRC16_ERR) {
+			ms_set_err_code(chip, MS_CRC16_ERROR);
+			TRACE_RET(chip, ms_parse_err_code(chip));
+		}
+	} else {
+		if (CHK_MSPRO(ms_card) && !(*ptr & 0x80)) {
+			if (*ptr & (MS_INT_ERR | MS_INT_CMDNK)) {
+				ms_set_err_code(chip, MS_CMD_NK);
+				TRACE_RET(chip, ms_parse_err_code(chip));
+			}
+		}
+	}
+
+	if (*ptr & MS_RDY_TIMEOUT) {
+		rtsx_clear_ms_error(chip);
+		ms_set_err_code(chip, MS_TO_ERROR);
+		TRACE_RET(chip, ms_parse_err_code(chip));
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode, u8 tpc,
+			    u16 sec_cnt, u8 cfg, int mode_2k, int use_sg,
+			    void *buf, int buf_len)
+{
+	int retval;
+	u8 val, err_code = 0;
+	enum dma_data_direction dir;
+
+	if (!buf || !buf_len) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (trans_mode == MS_TM_AUTO_READ) {
+		dir = DMA_FROM_DEVICE;
+		err_code = MS_FLASH_READ_ERROR;
+	} else if (trans_mode == MS_TM_AUTO_WRITE) {
+		dir = DMA_TO_DEVICE;
+		err_code = MS_FLASH_WRITE_ERROR;
+	} else {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF,
+		     (u8) (sec_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
+		     (u8) sec_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+
+	if (mode_2k) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE,
+			     MS_2K_SECTOR_MODE);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE,
+			     0);
+	}
+
+	trans_dma_enable(dir, chip, sec_cnt * 512, DMA_512);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+		     MS_TRANSFER_START | trans_mode);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
+		     MS_TRANSFER_END);
+
+	rtsx_send_cmd_no_wait(chip);
+
+	retval =
+	    rtsx_transfer_data(chip, MS_CARD, buf, buf_len, use_sg, dir,
+			       chip->mspro_timeout);
+	if (retval < 0) {
+		ms_set_err_code(chip, err_code);
+		if (retval == -ETIMEDOUT) {
+			retval = STATUS_TIMEDOUT;
+		} else {
+			retval = STATUS_FAIL;
+		}
+		TRACE_RET(chip, retval);
+	}
+
+	RTSX_READ_REG(chip, MS_TRANS_CFG, &val);
+	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_write_bytes(struct rtsx_chip *chip, u8 tpc, u8 cnt, u8 cfg,
+			  u8 * data, int data_len)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+
+	if (!data || (data_len < cnt)) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < cnt; i++) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF,
+			     data[i]);
+	}
+	if (cnt % 2) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF,
+			     0xFF);
+	}
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+		     MS_TRANSFER_START | MS_TM_WRITE_BYTES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
+		     MS_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval < 0) {
+		u8 val = 0;
+
+		rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		RTSX_DEBUGP(("MS_TRANS_CFG: 0x%02x\n", val));
+
+		rtsx_clear_ms_error(chip);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR) {
+				ms_set_err_code(chip, MS_CRC16_ERROR);
+				TRACE_RET(chip, ms_parse_err_code(chip));
+			}
+		} else {
+			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
+					ms_set_err_code(chip, MS_CMD_NK);
+					TRACE_RET(chip,
+						  ms_parse_err_code(chip));
+				}
+			}
+		}
+
+		if (val & MS_RDY_TIMEOUT) {
+			ms_set_err_code(chip, MS_TO_ERROR);
+			TRACE_RET(chip, ms_parse_err_code(chip));
+		}
+
+		ms_set_err_code(chip, MS_TO_ERROR);
+		TRACE_RET(chip, ms_parse_err_code(chip));
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_bytes(struct rtsx_chip *chip, u8 tpc, u8 cnt, u8 cfg,
+			 u8 * data, int data_len)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 *ptr;
+
+	if (!data) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+		     MS_TRANSFER_START | MS_TM_READ_BYTES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
+		     MS_TRANSFER_END);
+
+	for (i = 0; i < data_len - 1; i++) {
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
+	}
+	if (data_len % 2) {
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0,
+			     0);
+	} else {
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1,
+			     0, 0);
+	}
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval < 0) {
+		u8 val = 0;
+
+		rtsx_read_register(chip, MS_TRANS_CFG, &val);
+
+		rtsx_clear_ms_error(chip);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR) {
+				ms_set_err_code(chip, MS_CRC16_ERROR);
+				TRACE_RET(chip, ms_parse_err_code(chip));
+			}
+		} else {
+			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
+					ms_set_err_code(chip, MS_CMD_NK);
+					TRACE_RET(chip,
+						  ms_parse_err_code(chip));
+				}
+			}
+		}
+
+		if (val & MS_RDY_TIMEOUT) {
+			ms_set_err_code(chip, MS_TO_ERROR);
+			TRACE_RET(chip, ms_parse_err_code(chip));
+		}
+
+		ms_set_err_code(chip, MS_TO_ERROR);
+		TRACE_RET(chip, ms_parse_err_code(chip));
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	for (i = 0; i < data_len; i++) {
+		data[i] = ptr[i];
+	}
+
+	if ((tpc == PRO_READ_SHORT_DATA) && (data_len == 8)) {
+		RTSX_DEBUGP(("Read format progress:\n"));
+		RTSX_DUMP(ptr, cnt);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_set_rw_reg_addr(struct rtsx_chip *chip,
+			      u8 read_start, u8 read_cnt, u8 write_start,
+			      u8 write_cnt)
+{
+	int retval, i;
+	u8 data[4];
+
+	data[0] = read_start;
+	data[1] = read_cnt;
+	data[2] = write_start;
+	data[3] = write_cnt;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, SET_RW_REG_ADRS, 4, NO_WAIT_INT,
+				   data, 4);
+		if (retval == STATUS_SUCCESS) {
+			return STATUS_SUCCESS;
+		}
+		rtsx_clear_ms_error(chip);
+	}
+
+	TRACE_RET(chip, STATUS_FAIL);
+}
+
+static int ms_send_cmd(struct rtsx_chip *chip, u8 cmd, u8 cfg)
+{
+	u8 data[2];
+
+	data[0] = cmd;
+	data[1] = 0;
+
+	return ms_write_bytes(chip, PRO_SET_CMD, 1, cfg, data, 1);
+}
+
+static int ms_set_init_para(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	if (CHK_MSHG(ms_card)) {
+		if (chip->asic_code) {
+			ms_card->ms_clock = chip->asic_ms_hg_clk;
+		} else {
+			ms_card->ms_clock = chip->fpga_ms_hg_clk;
+		}
+	} else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
+		if (chip->asic_code) {
+			ms_card->ms_clock = chip->asic_ms_4bit_clk;
+		} else {
+			ms_card->ms_clock = chip->fpga_ms_4bit_clk;
+		}
+	} else {
+		if (chip->asic_code) {
+			ms_card->ms_clock = chip->asic_ms_1bit_clk;
+		} else {
+			ms_card->ms_clock = chip->fpga_ms_1bit_clk;
+		}
+	}
+
+	retval = switch_clock(chip, ms_card->ms_clock);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = select_card(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_switch_clock(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	retval = select_card(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = switch_clock(chip, ms_card->ms_clock);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int ms_pull_ctl_disable(struct rtsx_chip *chip)
+{
+	RTSX_WRITE_REG(chip, CARD_PULL_CTL5, 0xFF, 0x55);
+	RTSX_WRITE_REG(chip, CARD_PULL_CTL6, 0xFF, 0x15);
+
+	return STATUS_SUCCESS;
+}
+
+int ms_pull_ctl_enable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	/* MS CD: pull up
+	 * others: pull down
+	 */
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x15);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 100);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_prepare_reset(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	u8 oc_mask = 0;
+
+	ms_card->ms_type = 0;
+	ms_card->check_ms_flow = 0;
+	ms_card->switch_8bit_fail = 0;
+	ms_card->delay_write.delay_write_flag = 0;
+
+	ms_card->pro_under_formatting = 0;
+
+	retval = ms_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!chip->ft2_fast_mode) {
+		wait_timeout(250);
+	}
+
+	retval = enable_card_clock(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (chip->asic_code) {
+		retval = ms_pull_ctl_enable(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
+			       FPGA_MS_PULL_CTL_BIT | 0x20, 0);
+	}
+
+	if (!chip->ft2_fast_mode) {
+#ifdef SUPPORT_OCP
+		if (CHECK_VERSION(chip, 0x5227, IC_VER_A))
+			RTSX_WRITE_REG(chip, PWD_SUSPEND_EN, 0x08, 0x00);
+#endif
+
+		retval = card_power_on(chip, MS_CARD);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		wait_timeout(150);
+
+#ifdef SUPPORT_OCP
+		oc_mask = SD_OC_NOW | SD_OC_EVER;
+		if (chip->ocp_stat & oc_mask) {
+			RTSX_DEBUGP(("Over current, OCPSTAT is 0x%x\n",
+				     chip->ocp_stat));
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (CHECK_VERSION(chip, 0x5227, IC_VER_A)) {
+			retval = rtsx_disable_ocp(chip);
+			if (retval != STATUS_SUCCESS)
+				TRACE_RET(chip, STATUS_FAIL);
+
+			RTSX_WRITE_REG(chip, PWD_SUSPEND_EN, 0x08, 0x08);
+		}
+#endif
+	}
+
+	RTSX_WRITE_REG(chip, CARD_OE, MS_OUTPUT_EN, MS_OUTPUT_EN);
+
+	if (chip->asic_code) {
+		RTSX_WRITE_REG(chip, MS_CFG, 0xFF,
+			       SAMPLE_TIME_RISING | PUSH_TIME_DEFAULT |
+			       NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
+	} else {
+		RTSX_WRITE_REG(chip, MS_CFG, 0xFF,
+			       SAMPLE_TIME_FALLING | PUSH_TIME_DEFAULT |
+			       NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
+	}
+	RTSX_WRITE_REG(chip, MS_TRANS_CFG, 0xFF,
+		       NO_WAIT_INT | NO_AUTO_READ_INT_REG);
+	RTSX_WRITE_REG(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
+		       MS_STOP | MS_CLR_ERR);
+
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 val;
+
+	retval = ms_set_rw_reg_addr(chip, Pro_StatusReg, 6, SystemParm, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG, 6,
+				    NO_WAIT_INT);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, PPBUF_BASE2 + 2, &val);
+	RTSX_DEBUGP(("Type register: 0x%x\n", val));
+	if (val != 0x01) {
+		if (val != 0x02) {
+			ms_card->check_ms_flow = 1;
+		}
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, PPBUF_BASE2 + 4, &val);
+	RTSX_DEBUGP(("Category register: 0x%x\n", val));
+	if (val != 0) {
+		ms_card->check_ms_flow = 1;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, PPBUF_BASE2 + 5, &val);
+	RTSX_DEBUGP(("Class register: 0x%x\n", val));
+	if (val == 0) {
+		RTSX_READ_REG(chip, PPBUF_BASE2, &val);
+		if (val & WRT_PRTCT) {
+			chip->card_wp |= MS_CARD;
+		} else {
+			chip->card_wp &= ~MS_CARD;
+		}
+	} else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
+		chip->card_wp |= MS_CARD;
+	} else {
+		ms_card->check_ms_flow = 1;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_card->ms_type |= TYPE_MSPRO;
+
+	RTSX_READ_REG(chip, PPBUF_BASE2 + 3, &val);
+	RTSX_DEBUGP(("IF Mode register: 0x%x\n", val));
+	if (val == 0) {
+		ms_card->ms_type &= 0x0F;
+	} else if (val == 7) {
+		if (switch_8bit_bus) {
+			ms_card->ms_type |= MS_HG;
+		} else {
+			ms_card->ms_type &= 0x0F;
+		}
+	} else {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
+{
+	int retval, i, k;
+	u8 val;
+
+	k = 0;
+	do {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+			retval =
+			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
+					  1);
+			if (retval == STATUS_SUCCESS) {
+				break;
+			}
+		}
+		if (i == MS_MAX_RETRY_COUNT) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (k > 100) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		k++;
+		wait_timeout(100);
+	} while (!(val & INT_REG_CED));
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val & INT_REG_ERR) {
+		if (val & INT_REG_CMDNK) {
+			chip->card_wp |= (MS_CARD);
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_switch_parallel_bus(struct rtsx_chip *chip)
+{
+	int retval, i;
+	u8 data[2];
+
+	data[0] = PARALLEL_4BIT_IF;
+	data[1] = 0;
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_switch_8bit_bus(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 data[2];
+
+	data[0] = PARALLEL_8BIT_IF;
+	data[1] = 0;
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, MS_CFG, 0x98,
+		       MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
+	ms_card->ms_type |= MS_8BIT;
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
+				    NO_WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+
+	for (i = 0; i < 3; i++) {
+		retval = ms_prepare_reset(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_identify_media_type(chip, switch_8bit_bus);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_confirm_cpu_startup(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_switch_parallel_bus(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+				ms_set_err_code(chip, MS_NO_CARD);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			continue;
+		} else {
+			break;
+		}
+	}
+
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
+
+	RTSX_WRITE_REG(chip, MS_CFG, PUSH_TIME_ODD, PUSH_TIME_ODD);
+
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MSHG(ms_card) && chip->support_ms_8bit && switch_8bit_bus) {
+		retval = ms_switch_8bit_bus(chip);
+		if (retval != STATUS_SUCCESS) {
+			ms_card->switch_8bit_fail = 1;
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef XC_POWERCLASS
+static int msxc_change_power(struct rtsx_chip *chip, u8 mode)
+{
+	int retval;
+	u8 buf[6];
+
+	ms_cleanup_work(chip);
+
+	retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf[0] = 0;
+	buf[1] = mode;
+	buf[2] = 0;
+	buf[3] = 0;
+	buf[4] = 0;
+	buf[5] = 0;
+
+	retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, MS_TRANS_CFG, buf);
+	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int ms_read_attribute_info(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 val, *buf, class_code, device_type, sub_class, data[16];
+	u16 total_blk = 0, blk_size = 0;
+#ifdef SUPPORT_MSXC
+	u32 xc_total_blk = 0, xc_blk_size = 0;
+#endif
+	u32 sys_info_addr = 0, sys_info_size;
+#ifdef SUPPORT_PCGL_1P18
+	u32 model_name_addr = 0, model_name_size;
+	int found_sys_info = 0, found_model_name = 0;
+#endif
+
+	retval = ms_set_rw_reg_addr(chip, Pro_IntReg, 2, Pro_SystemParm, 7);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MS8BIT(ms_card)) {
+		data[0] = PARALLEL_8BIT_IF;
+	} else {
+		data[0] = PARALLEL_4BIT_IF;
+	}
+	data[1] = 0;
+
+	data[2] = 0x40;
+	data[3] = 0;
+	data[4] = 0;
+	data[5] = 0;
+	data[6] = 0;
+	data[7] = 0;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT, data,
+				   8);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf = (u8 *) rtsx_alloc_dma_buf(chip, 64 * 512, GFP_KERNEL);
+	if (buf == NULL) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			continue;
+		}
+		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_free_dma_buf(chip, buf);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		if (!(val & MS_INT_BREQ)) {
+			rtsx_free_dma_buf(chip, buf);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		retval =
+		    ms_transfer_data(chip, MS_TM_AUTO_READ,
+				     PRO_READ_LONG_DATA, 0x40, WAIT_INT, 0, 0,
+				     buf, 64 * 512);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		} else {
+			rtsx_clear_ms_error(chip);
+		}
+	}
+	if (retval != STATUS_SUCCESS) {
+		rtsx_free_dma_buf(chip, buf);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	i = 0;
+	do {
+		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_free_dma_buf(chip, buf);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if ((val & MS_INT_CED) || !(val & MS_INT_BREQ)) {
+			break;
+		}
+
+		retval =
+		    ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
+				    PRO_READ_LONG_DATA, 0, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_free_dma_buf(chip, buf);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		i++;
+	} while (i < 1024);
+
+	if (retval != STATUS_SUCCESS) {
+		rtsx_free_dma_buf(chip, buf);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if ((buf[0] != 0xa5) && (buf[1] != 0xc3)) {
+		rtsx_free_dma_buf(chip, buf);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if ((buf[4] < 1) || (buf[4] > 12)) {
+		rtsx_free_dma_buf(chip, buf);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	for (i = 0; i < buf[4]; i++) {
+		int cur_addr_off = 16 + i * 12;
+
+#ifdef SUPPORT_MSXC
+		if ((buf[cur_addr_off + 8] == 0x10)
+		    || (buf[cur_addr_off + 8] == 0x13))
+#else
+		if (buf[cur_addr_off + 8] == 0x10)
+#endif
+		{
+			sys_info_addr = ((u32) buf[cur_addr_off + 0] << 24) |
+			    ((u32) buf[cur_addr_off + 1] << 16) |
+			    ((u32) buf[cur_addr_off + 2] << 8) |
+			    buf[cur_addr_off + 3];
+			sys_info_size =
+			    ((u32) buf[cur_addr_off + 4] << 24) | ((u32)
+								   buf
+								   [cur_addr_off
+								    +
+								    5] << 16)
+			    | ((u32) buf[cur_addr_off + 6] << 8) |
+			    buf[cur_addr_off + 7];
+			RTSX_DEBUGP(("sys_info_addr = 0x%x, sys_info_size = 0x%x\n", sys_info_addr, sys_info_size));
+			if (sys_info_size != 96) {
+				rtsx_free_dma_buf(chip, buf);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (sys_info_addr < 0x1A0) {
+				rtsx_free_dma_buf(chip, buf);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if ((sys_info_size + sys_info_addr) > 0x8000) {
+				rtsx_free_dma_buf(chip, buf);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+#ifdef SUPPORT_MSXC
+			if (buf[cur_addr_off + 8] == 0x13) {
+				ms_card->ms_type |= MS_XC;
+			}
+#endif
+#ifdef SUPPORT_PCGL_1P18
+			found_sys_info = 1;
+#else
+			break;
+#endif
+		}
+#ifdef SUPPORT_PCGL_1P18
+		if (buf[cur_addr_off + 8] == 0x15) {
+			model_name_addr =
+			    ((u32) buf[cur_addr_off + 0] << 24) | ((u32)
+								   buf
+								   [cur_addr_off
+								    +
+								    1] << 16)
+			    | ((u32) buf[cur_addr_off + 2] << 8) |
+			    buf[cur_addr_off + 3];
+			model_name_size =
+			    ((u32) buf[cur_addr_off + 4] << 24) | ((u32)
+								   buf
+								   [cur_addr_off
+								    +
+								    5] << 16)
+			    | ((u32) buf[cur_addr_off + 6] << 8) |
+			    buf[cur_addr_off + 7];
+			RTSX_DEBUGP(("model_name_addr = 0x%x, model_name_size = 0x%x\n", model_name_addr, model_name_size));
+			if (model_name_size != 48) {
+				rtsx_free_dma_buf(chip, buf);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (model_name_addr < 0x1A0) {
+				rtsx_free_dma_buf(chip, buf);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if ((model_name_size + model_name_addr) > 0x8000) {
+				rtsx_free_dma_buf(chip, buf);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			found_model_name = 1;
+		}
+
+		if (found_sys_info && found_model_name) {
+			break;
+		}
+#endif
+	}
+
+	if (i == buf[4]) {
+		rtsx_free_dma_buf(chip, buf);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	class_code = buf[sys_info_addr + 0];
+	device_type = buf[sys_info_addr + 56];
+	sub_class = buf[sys_info_addr + 46];
+#ifdef SUPPORT_MSXC
+	if (CHK_MSXC(ms_card)) {
+		xc_total_blk = ((u32) buf[sys_info_addr + 6] << 24) |
+		    ((u32) buf[sys_info_addr + 7] << 16) |
+		    ((u32) buf[sys_info_addr + 8] << 8) |
+		    buf[sys_info_addr + 9];
+		xc_blk_size = ((u32) buf[sys_info_addr + 32] << 24) |
+		    ((u32) buf[sys_info_addr + 33] << 16) |
+		    ((u32) buf[sys_info_addr + 34] << 8) |
+		    buf[sys_info_addr + 35];
+		RTSX_DEBUGP(("xc_total_blk = 0x%x, xc_blk_size = 0x%x\n",
+			     xc_total_blk, xc_blk_size));
+	} else {
+		total_blk =
+		    ((u16) buf[sys_info_addr + 6] << 8) | buf[sys_info_addr +
+							      7];
+		blk_size =
+		    ((u16) buf[sys_info_addr + 2] << 8) | buf[sys_info_addr +
+							      3];
+		RTSX_DEBUGP(("total_blk = 0x%x, blk_size = 0x%x\n", total_blk,
+			     blk_size));
+	}
+#else
+	total_blk =
+	    ((u16) buf[sys_info_addr + 6] << 8) | buf[sys_info_addr + 7];
+	blk_size =
+	    ((u16) buf[sys_info_addr + 2] << 8) | buf[sys_info_addr + 3];
+	RTSX_DEBUGP(("total_blk = 0x%x, blk_size = 0x%x\n", total_blk,
+		     blk_size));
+#endif
+
+	RTSX_DEBUGP(("class_code = 0x%x, device_type = 0x%x, sub_class = 0x%x\n", class_code, device_type, sub_class));
+
+	memcpy(ms_card->raw_sys_info, buf + sys_info_addr, 96);
+	memcpy(ms_card->raw_ms_id, ms_card->raw_sys_info + 64, 16);
+	RTSX_DEBUGP(("MSPro ID:\n"));
+	RTSX_DUMP(ms_card->raw_ms_id, 16);
+
+#ifdef SUPPORT_PCGL_1P18
+	memcpy(ms_card->raw_model_name, buf + model_name_addr, 48);
+#endif
+
+	rtsx_free_dma_buf(chip, buf);
+
+#ifdef SUPPORT_MSXC
+	if (CHK_MSXC(ms_card)) {
+		if (class_code != 0x03) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		if (class_code != 0x02) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+#else
+	if (class_code != 0x02) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	if (device_type != 0x00) {
+		if ((device_type == 0x01) || (device_type == 0x02)
+		    || (device_type == 0x03)) {
+			chip->card_wp |= MS_CARD;
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if (sub_class & 0xC0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
+		     class_code, device_type, sub_class));
+
+#ifdef SUPPORT_MSXC
+	if (CHK_MSXC(ms_card)) {
+		chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
+		    xc_total_blk * xc_blk_size;
+	} else {
+		chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
+		    total_blk * blk_size;
+	}
+#else
+	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
+	    total_blk * blk_size;
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef SUPPORT_MAGIC_GATE
+static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
+			       u8 mg_entry_num);
+#endif
+
+static int reset_ms_pro(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+#ifdef XC_POWERCLASS
+	u8 change_power_class;
+
+	if (chip->ms_power_class_en & 0x02) {
+		change_power_class = 2;
+	} else if (chip->ms_power_class_en & 0x01) {
+		change_power_class = 1;
+	} else {
+		change_power_class = 0;
+	}
+#endif
+
+#ifdef XC_POWERCLASS
+ Retry:
+#endif
+	retval = ms_pro_reset_flow(chip, 1);
+	if (retval != STATUS_SUCCESS) {
+		if (ms_card->switch_8bit_fail) {
+			retval = ms_pro_reset_flow(chip, 0);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval = ms_read_attribute_info(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#ifdef XC_POWERCLASS
+	if (CHK_HG8BIT(ms_card)) {
+		change_power_class = 0;
+	}
+
+	if (change_power_class && CHK_MSXC(ms_card)) {
+		u8 power_class_en = chip->ms_power_class_en;
+
+		RTSX_DEBUGP(("power_class_en = 0x%x\n", power_class_en));
+		RTSX_DEBUGP(("change_power_class = %d\n",
+			     change_power_class));
+
+		if (change_power_class) {
+			power_class_en &= (1 << (change_power_class - 1));
+		} else {
+			power_class_en = 0;
+		}
+
+		if (power_class_en) {
+			u8 power_class_mode =
+			    (ms_card->raw_sys_info[46] & 0x18) >> 3;
+			RTSX_DEBUGP(("power_class_mode = 0x%x",
+				     power_class_mode));
+			if (change_power_class > power_class_mode) {
+				change_power_class = power_class_mode;
+			}
+			if (change_power_class) {
+				retval =
+				    msxc_change_power(chip,
+						      change_power_class);
+				if (retval != STATUS_SUCCESS) {
+					change_power_class--;
+					goto Retry;
+				}
+			}
+		}
+	}
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+	retval = mg_set_tpc_para_sub(chip, 0, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	if (CHK_HG8BIT(ms_card)) {
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
+	} else {
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_status_reg(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val[2];
+
+	retval = ms_set_rw_reg_addr(chip, StatusReg0, 2, 0, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
+		ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_extra_data(struct rtsx_chip *chip,
+			      u16 block_addr, u8 page_num, u8 * buf,
+			      int buf_len)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 val, data[10];
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (block_addr >> 8);
+	data[3] = (u8) block_addr;
+	data[4] = 0x40;
+	data[5] = page_num;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			retval = ms_read_status_reg(chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			retval =
+			    ms_set_rw_reg_addr(chip, OverwriteFlag,
+					       MS_EXTRA_SIZE, SystemParm, 6);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+	}
+
+	retval =
+	    ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT, data,
+			  MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (buf && buf_len) {
+		if (buf_len > MS_EXTRA_SIZE) {
+			buf_len = MS_EXTRA_SIZE;
+		}
+		memcpy(buf, data, buf_len);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_write_extra_data(struct rtsx_chip *chip,
+			       u16 block_addr, u8 page_num, u8 * buf,
+			       int buf_len)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 val, data[16];
+
+	if (!buf || (buf_len < MS_EXTRA_SIZE)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       6 + MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (block_addr >> 8);
+	data[3] = (u8) block_addr;
+	data[4] = 0x40;
+	data[5] = page_num;
+
+	for (i = 6; i < MS_EXTRA_SIZE + 6; i++) {
+		data[i] = buf[i - 6];
+	}
+
+	retval =
+	    ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE), NO_WAIT_INT,
+			   data, 16);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	u8 val, data[6];
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (block_addr >> 8);
+	data[3] = (u8) block_addr;
+	data[4] = 0x20;
+	data[5] = page_num;
+
+	retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			retval = ms_read_status_reg(chip);
+			if (retval != STATUS_SUCCESS) {
+				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			}
+		} else {
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_BREQ_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+	}
+
+	retval =
+	    ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA, 0,
+			    NO_WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_set_bad_block(struct rtsx_chip *chip, u16 phy_blk)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	u8 val, data[8], extra[MS_EXTRA_SIZE];
+
+	retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       7);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (phy_blk >> 8);
+	data[3] = (u8) phy_blk;
+	data[4] = 0x80;
+	data[5] = 0;
+	data[6] = extra[0] & 0x7F;
+	data[7] = 0xFF;
+
+	retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 7);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i = 0;
+	u8 val, data[6];
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (phy_blk >> 8);
+	data[3] = (u8) phy_blk;
+	data[4] = 0;
+	data[5] = 0;
+
+	retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+ ERASE_RTY:
+	retval = ms_send_cmd(chip, BLOCK_ERASE, WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val & INT_REG_CMDNK) {
+		if (i < 3) {
+			i++;
+			goto ERASE_RTY;
+		}
+
+		ms_set_err_code(chip, MS_CMD_NK);
+		ms_set_bad_block(chip, phy_blk);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void ms_set_page_status(u16 log_blk, u8 type, u8 * extra,
+			       int extra_len)
+{
+	if (!extra || (extra_len < MS_EXTRA_SIZE)) {
+		return;
+	}
+
+	memset(extra, 0xFF, MS_EXTRA_SIZE);
+
+	if (type == setPS_NG) {
+		extra[0] = 0xB8;
+	} else {
+		extra[0] = 0x98;
+	}
+
+	extra[2] = (u8) (log_blk >> 8);
+	extra[3] = (u8) log_blk;
+}
+
+static int ms_init_page(struct rtsx_chip *chip, u16 phy_blk, u16 log_blk,
+			u8 start_page, u8 end_page)
+{
+	int retval;
+	u8 extra[MS_EXTRA_SIZE], i;
+
+	memset(extra, 0xff, MS_EXTRA_SIZE);
+
+	extra[0] = 0xf8;
+	extra[1] = 0xff;
+	extra[2] = (u8) (log_blk >> 8);
+	extra[3] = (u8) log_blk;
+
+	for (i = start_page; i < end_page; i++) {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval =
+		    ms_write_extra_data(chip, phy_blk, i, extra,
+					MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
+			u16 log_blk, u8 start_page, u8 end_page)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, rty_cnt, uncorrect_flag = 0;
+	u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
+
+	RTSX_DEBUGP(("Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
+		     old_blk, new_blk, log_blk));
+	RTSX_DEBUGP(("start_page = %d, end_page = %d\n", start_page,
+		     end_page));
+
+	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_read_status_reg(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, PPBUF_BASE2, &val);
+
+	if (val & BUF_FULL) {
+		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval =
+		    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (!(val & INT_REG_CED)) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	for (i = start_page; i < end_page; i++) {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		DISABLE_TRIGGER();
+		ms_read_extra_data(chip, old_blk, i, extra, MS_EXTRA_SIZE);
+		ENABLE_TRIGGER();
+
+		retval =
+		    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
+				       SystemParm, 6);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (CHK_MS4BIT(ms_card)) {
+			data[0] = 0x88;
+		} else {
+			data[0] = 0x80;
+		}
+		data[1] = 0;
+		data[2] = (u8) (old_blk >> 8);
+		data[3] = (u8) old_blk;
+		data[4] = 0x20;
+		data[5] = i;
+
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+		retval =
+		    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (val & INT_REG_CED) {
+			if (val & INT_REG_ERR) {
+				retval = ms_read_status_reg(chip);
+				if (retval != STATUS_SUCCESS) {
+					uncorrect_flag = 1;
+					RTSX_DEBUGP(("Uncorrectable error\n"));
+				} else {
+					uncorrect_flag = 0;
+				}
+
+				retval =
+				    ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
+						    READ_PAGE_DATA, 0,
+						    NO_WAIT_INT);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+
+				if (uncorrect_flag) {
+					ms_set_page_status(log_blk, setPS_NG,
+							   extra,
+							   MS_EXTRA_SIZE);
+					if (i == 0) {
+						extra[0] &= 0xEF;
+					}
+					ms_write_extra_data(chip, old_blk, i,
+							    extra,
+							    MS_EXTRA_SIZE);
+					RTSX_DEBUGP(("page %d : extra[0] = 0x%x\n", i, extra[0]));
+					MS_SET_BAD_BLOCK_FLG(ms_card);
+
+					ms_set_page_status(log_blk,
+							   setPS_Error, extra,
+							   MS_EXTRA_SIZE);
+					ms_write_extra_data(chip, new_blk, i,
+							    extra,
+							    MS_EXTRA_SIZE);
+					continue;
+				}
+
+				for (rty_cnt = 0;
+				     rty_cnt < MS_MAX_RETRY_COUNT;
+				     rty_cnt++) {
+					retval =
+					    ms_transfer_tpc(chip,
+							    MS_TM_NORMAL_WRITE,
+							    WRITE_PAGE_DATA,
+							    0, NO_WAIT_INT);
+					if (retval == STATUS_SUCCESS) {
+						break;
+					}
+				}
+				if (rty_cnt == MS_MAX_RETRY_COUNT) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_BREQ_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
+					    MS_EXTRA_SIZE, SystemParm,
+					    (6 + MS_EXTRA_SIZE));
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (CHK_MS4BIT(ms_card)) {
+			data[0] = 0x88;
+		} else {
+			data[0] = 0x80;
+		}
+		data[1] = 0;
+		data[2] = (u8) (new_blk >> 8);
+		data[3] = (u8) new_blk;
+		data[4] = 0x20;
+		data[5] = i;
+
+		if ((extra[0] & 0x60) != 0x60) {
+			data[6] = extra[0];
+		} else {
+			data[6] = 0xF8;
+		}
+		data[6 + 1] = 0xFF;
+		data[6 + 2] = (u8) (log_blk >> 8);
+		data[6 + 3] = (u8) log_blk;
+
+		for (j = 4; j <= MS_EXTRA_SIZE; j++) {
+			data[6 + j] = 0xFF;
+		}
+
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
+				   NO_WAIT_INT, data, 16);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+		retval =
+		    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (val & INT_REG_CED) {
+			if (val & INT_REG_ERR) {
+				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		if (i == 0) {
+			retval =
+			    ms_set_rw_reg_addr(chip, OverwriteFlag,
+					       MS_EXTRA_SIZE, SystemParm, 7);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			ms_set_err_code(chip, MS_NO_ERROR);
+
+			if (CHK_MS4BIT(ms_card)) {
+				data[0] = 0x88;
+			} else {
+				data[0] = 0x80;
+			}
+			data[1] = 0;
+			data[2] = (u8) (old_blk >> 8);
+			data[3] = (u8) old_blk;
+			data[4] = 0x80;
+			data[5] = 0;
+			data[6] = 0xEF;
+			data[7] = 0xFF;
+
+			retval =
+			    ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
+					   data, 8);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			ms_set_err_code(chip, MS_NO_ERROR);
+			retval =
+			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
+					  1);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			if (val & INT_REG_CMDNK) {
+				ms_set_err_code(chip, MS_CMD_NK);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			if (val & INT_REG_CED) {
+				if (val & INT_REG_ERR) {
+					ms_set_err_code(chip,
+							MS_FLASH_WRITE_ERROR);
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_ms(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	u16 i, reg_addr, block_size;
+	u8 val, extra[MS_EXTRA_SIZE], j, *ptr;
+#ifndef SUPPORT_MAGIC_GATE
+	u16 eblock_cnt;
+#endif
+
+	retval = ms_prepare_reset(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_card->ms_type |= TYPE_MS;
+
+	retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_read_status_reg(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, PPBUF_BASE2, &val);
+	if (val & WRT_PRTCT) {
+		chip->card_wp |= MS_CARD;
+	} else {
+		chip->card_wp &= ~MS_CARD;
+	}
+
+	i = 0;
+
+ RE_SEARCH:
+	while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_read_extra_data(chip, i, 0, extra, MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS) {
+			i++;
+			continue;
+		}
+
+		if (extra[0] & BLOCK_OK) {
+			if (!(extra[1] & NOT_BOOT_BLOCK)) {
+				ms_card->boot_block = i;
+				break;
+			}
+		}
+		i++;
+	}
+
+	if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
+		RTSX_DEBUGP(("No boot block found!"));
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	for (j = 0; j < 3; j++) {
+		retval = ms_read_page(chip, ms_card->boot_block, j);
+		if (retval != STATUS_SUCCESS) {
+			if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
+				i = ms_card->boot_block + 1;
+				ms_set_err_code(chip, MS_NO_ERROR);
+				goto RE_SEARCH;
+			}
+		}
+	}
+
+	retval = ms_read_page(chip, ms_card->boot_block, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < 96; i++) {
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 0x1A0 + i, 0,
+			     0);
+	}
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 100);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ptr = rtsx_get_cmd_data(chip);
+	memcpy(ms_card->raw_sys_info, ptr, 96);
+	memcpy(ms_card->raw_ms_id, ms_card->raw_sys_info + 0x0C, 16);
+	RTSX_DEBUGP(("MS ID:\n"));
+	RTSX_DUMP(ms_card->raw_ms_id, 16);
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
+
+	for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3;
+	     reg_addr++) {
+		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+	}
+
+	for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++) {
+		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+	}
+
+	rtsx_add_cmd(chip, READ_REG_CMD, MS_Device_Type, 0, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, MS_4bit_Support, 0, 0);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 100);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ptr = rtsx_get_cmd_data(chip);
+
+	RTSX_DEBUGP(("Boot block data:\n"));
+	RTSX_DUMP(ptr, 16);
+
+	if (ptr[0] != 0x00 || ptr[1] != 0x01) {
+		i = ms_card->boot_block + 1;
+		goto RE_SEARCH;
+	}
+
+	if (ptr[12] != 0x02 || ptr[13] != 0x00) {
+		i = ms_card->boot_block + 1;
+		goto RE_SEARCH;
+	}
+
+	if ((ptr[14] == 1) || (ptr[14] == 3)) {
+		chip->card_wp |= MS_CARD;
+	}
+
+	block_size = ((u16) ptr[6] << 8) | ptr[7];
+	if (block_size == 0x0010) {
+		ms_card->block_shift = 5;
+		ms_card->page_off = 0x1F;
+	} else if (block_size == 0x0008) {
+		ms_card->block_shift = 4;
+		ms_card->page_off = 0x0F;
+	}
+
+	ms_card->total_block = ((u16) ptr[8] << 8) | ptr[9];
+
+#ifdef SUPPORT_MAGIC_GATE
+	j = ptr[10];
+
+	if (ms_card->block_shift == 4) {
+		if (j < 2) {
+			ms_card->capacity = 0x1EE0;
+		} else {
+			ms_card->capacity = 0x3DE0;
+		}
+	} else {
+		if (j < 5) {
+			ms_card->capacity = 0x7BC0;
+		} else if (j < 0xA) {
+			ms_card->capacity = 0xF7C0;
+		} else if (j < 0x11) {
+			ms_card->capacity = 0x1EF80;
+		} else {
+			ms_card->capacity = 0x3DF00;
+		}
+	}
+#else
+	eblock_cnt = ((u16) ptr[10] << 8) | ptr[11];
+
+	ms_card->capacity = ((u32) eblock_cnt - 2) << ms_card->block_shift;
+#endif
+
+	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
+
+	if (ptr[15]) {
+		retval = ms_set_rw_reg_addr(chip, 0, 0, SystemParm, 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		RTSX_WRITE_REG(chip, PPBUF_BASE2, 0xFF, 0x88);
+		RTSX_WRITE_REG(chip, PPBUF_BASE2 + 1, 0xFF, 0);
+
+		retval =
+		    ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
+				    NO_WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		RTSX_WRITE_REG(chip, MS_CFG, 0x58 | MS_NO_CHECK_INT,
+			       MS_BUS_WIDTH_4 | PUSH_TIME_ODD |
+			       MS_NO_CHECK_INT);
+
+		ms_card->ms_type |= MS_4BIT;
+	}
+
+	if (CHK_MS4BIT(ms_card)) {
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
+	} else {
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_init_l2p_tbl(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int size, i, seg_no, retval;
+	u16 defect_block, reg_addr;
+	u8 val1, val2;
+
+	ms_card->segment_cnt = ms_card->total_block >> 9;
+	RTSX_DEBUGP(("ms_card->segment_cnt = %d\n", ms_card->segment_cnt));
+
+	size = ms_card->segment_cnt * sizeof(struct zone_entry);
+	ms_card->segment = (struct zone_entry *)vmalloc(size);
+	if (ms_card->segment == NULL) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	memset(ms_card->segment, 0, size);
+
+	retval = ms_read_page(chip, ms_card->boot_block, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_GOTO(chip, INIT_FAIL);
+	}
+
+	reg_addr = PPBUF_BASE2;
+	for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
+		retval = rtsx_read_register(chip, reg_addr++, &val1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_GOTO(chip, INIT_FAIL);
+		}
+		retval = rtsx_read_register(chip, reg_addr++, &val2);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_GOTO(chip, INIT_FAIL);
+		}
+
+		defect_block = ((u16) val1 << 8) | val2;
+		if (defect_block == 0xFFFF) {
+			break;
+		}
+		seg_no = defect_block / 512;
+		ms_card->segment[seg_no].defect_list[ms_card->segment[seg_no].
+						     disable_count++] =
+		    defect_block;
+	}
+
+	for (i = 0; i < ms_card->segment_cnt; i++) {
+		ms_card->segment[i].build_flag = 0;
+		ms_card->segment[i].l2p_table = NULL;
+		ms_card->segment[i].free_table = NULL;
+		ms_card->segment[i].get_index = 0;
+		ms_card->segment[i].set_index = 0;
+		ms_card->segment[i].unused_blk_cnt = 0;
+
+		RTSX_DEBUGP(("defective block count of segment %d is %d\n",
+			     i, ms_card->segment[i].disable_count));
+	}
+
+	return STATUS_SUCCESS;
+
+ INIT_FAIL:
+	if (ms_card->segment) {
+		vfree(ms_card->segment);
+		ms_card->segment = NULL;
+	}
+
+	return STATUS_FAIL;
+}
+
+static u16 ms_get_l2p_tbl(struct rtsx_chip *chip, int seg_no, u16 log_off)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct zone_entry *segment;
+
+	if (ms_card->segment == NULL) {
+		return 0xFFFF;
+	}
+
+	segment = &(ms_card->segment[seg_no]);
+
+	if (segment->l2p_table) {
+		return segment->l2p_table[log_off];
+	}
+
+	return 0xFFFF;
+}
+
+static void ms_set_l2p_tbl(struct rtsx_chip *chip, int seg_no, u16 log_off,
+			   u16 phy_blk)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct zone_entry *segment;
+
+	if (ms_card->segment == NULL) {
+		return;
+	}
+
+	segment = &(ms_card->segment[seg_no]);
+	if (segment->l2p_table) {
+		segment->l2p_table[log_off] = phy_blk;
+	}
+}
+
+static void ms_set_unused_block(struct rtsx_chip *chip, u16 phy_blk)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct zone_entry *segment;
+	int seg_no;
+
+	seg_no = (int)phy_blk >> 9;
+	segment = &(ms_card->segment[seg_no]);
+
+	segment->free_table[segment->set_index++] = phy_blk;
+	if (segment->set_index >= MS_FREE_TABLE_CNT) {
+		segment->set_index = 0;
+	}
+	segment->unused_blk_cnt++;
+}
+
+static u16 ms_get_unused_block(struct rtsx_chip *chip, int seg_no)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct zone_entry *segment;
+	u16 phy_blk;
+
+	segment = &(ms_card->segment[seg_no]);
+
+	if (segment->unused_blk_cnt <= 0) {
+		return 0xFFFF;
+	}
+
+	phy_blk = segment->free_table[segment->get_index];
+	segment->free_table[segment->get_index++] = 0xFFFF;
+	if (segment->get_index >= MS_FREE_TABLE_CNT) {
+		segment->get_index = 0;
+	}
+	segment->unused_blk_cnt--;
+
+	return phy_blk;
+}
+
+static const unsigned short ms_start_idx[] =
+    { 0, 494, 990, 1486, 1982, 2478, 2974, 3470,
+	3966, 4462, 4958, 5454, 5950, 6446, 6942, 7438, 7934
+};
+
+static int ms_arbitrate_l2p(struct rtsx_chip *chip, u16 phy_blk, u16 log_off,
+			    u8 us1, u8 us2)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct zone_entry *segment;
+	int seg_no;
+	u16 tmp_blk;
+
+	seg_no = (int)phy_blk >> 9;
+	segment = &(ms_card->segment[seg_no]);
+	tmp_blk = segment->l2p_table[log_off];
+
+	if (us1 != us2) {
+		if (us1 == 0) {
+			if (!(chip->card_wp & MS_CARD)) {
+				ms_erase_block(chip, tmp_blk);
+			}
+			ms_set_unused_block(chip, tmp_blk);
+			segment->l2p_table[log_off] = phy_blk;
+		} else {
+			if (!(chip->card_wp & MS_CARD)) {
+				ms_erase_block(chip, phy_blk);
+			}
+			ms_set_unused_block(chip, phy_blk);
+		}
+	} else {
+		if (phy_blk < tmp_blk) {
+			if (!(chip->card_wp & MS_CARD)) {
+				ms_erase_block(chip, phy_blk);
+			}
+			ms_set_unused_block(chip, phy_blk);
+		} else {
+			if (!(chip->card_wp & MS_CARD)) {
+				ms_erase_block(chip, tmp_blk);
+			}
+			ms_set_unused_block(chip, tmp_blk);
+			segment->l2p_table[log_off] = phy_blk;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct zone_entry *segment;
+	int retval, table_size, disable_cnt, defect_flag, i;
+	u16 start, end, phy_blk, log_blk, tmp_blk;
+	u8 extra[MS_EXTRA_SIZE], us1, us2;
+
+	RTSX_DEBUGP(("ms_build_l2p_tbl: %d\n", seg_no));
+
+	if (ms_card->segment == NULL) {
+		retval = ms_init_l2p_tbl(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, retval);
+		}
+	}
+
+	if (ms_card->segment[seg_no].build_flag) {
+		RTSX_DEBUGP(("l2p table of segment %d has been built\n",
+			     seg_no));
+		return STATUS_SUCCESS;
+	}
+
+	if (seg_no == 0) {
+		table_size = 494;
+	} else {
+		table_size = 496;
+	}
+
+	segment = &(ms_card->segment[seg_no]);
+
+	if (segment->l2p_table == NULL) {
+		segment->l2p_table = (u16 *) vmalloc(table_size * 2);
+		if (segment->l2p_table == NULL) {
+			TRACE_GOTO(chip, BUILD_FAIL);
+		}
+	}
+	memset((u8 *) (segment->l2p_table), 0xff, table_size * 2);
+
+	if (segment->free_table == NULL) {
+		segment->free_table = (u16 *) vmalloc(MS_FREE_TABLE_CNT * 2);
+		if (segment->free_table == NULL) {
+			TRACE_GOTO(chip, BUILD_FAIL);
+		}
+	}
+	memset((u8 *) (segment->free_table), 0xff, MS_FREE_TABLE_CNT * 2);
+
+	start = (u16) seg_no << 9;
+	end = (u16) (seg_no + 1) << 9;
+
+	disable_cnt = segment->disable_count;
+
+	segment->get_index = segment->set_index = 0;
+	segment->unused_blk_cnt = 0;
+
+	for (phy_blk = start; phy_blk < end; phy_blk++) {
+		if (disable_cnt) {
+			defect_flag = 0;
+			for (i = 0; i < segment->disable_count; i++) {
+				if (phy_blk == segment->defect_list[i]) {
+					defect_flag = 1;
+					break;
+				}
+			}
+			if (defect_flag) {
+				disable_cnt--;
+				continue;
+			}
+		}
+
+		retval =
+		    ms_read_extra_data(chip, phy_blk, 0, extra,
+				       MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS) {
+			RTSX_DEBUGP(("read extra data fail\n"));
+			ms_set_bad_block(chip, phy_blk);
+			continue;
+		}
+
+		if (seg_no == ms_card->segment_cnt - 1) {
+			if (!(extra[1] & NOT_TRANSLATION_TABLE)) {
+				if (!(chip->card_wp & MS_CARD)) {
+					retval =
+					    ms_erase_block(chip, phy_blk);
+					if (retval != STATUS_SUCCESS) {
+						continue;
+					}
+					extra[2] = 0xff;
+					extra[3] = 0xff;
+				}
+			}
+		}
+
+		if (!(extra[0] & BLOCK_OK)) {
+			continue;
+		}
+		if (!(extra[1] & NOT_BOOT_BLOCK)) {
+			continue;
+		}
+		if ((extra[0] & PAGE_OK) != PAGE_OK) {
+			continue;
+		}
+
+		log_blk = ((u16) extra[2] << 8) | extra[3];
+
+		if (log_blk == 0xFFFF) {
+			if (!(chip->card_wp & MS_CARD)) {
+				retval = ms_erase_block(chip, phy_blk);
+				if (retval != STATUS_SUCCESS) {
+					continue;
+				}
+			}
+			ms_set_unused_block(chip, phy_blk);
+			continue;
+		}
+
+		if ((log_blk < ms_start_idx[seg_no]) ||
+		    (log_blk >= ms_start_idx[seg_no + 1])) {
+			if (!(chip->card_wp & MS_CARD)) {
+				retval = ms_erase_block(chip, phy_blk);
+				if (retval != STATUS_SUCCESS) {
+					continue;
+				}
+			}
+			ms_set_unused_block(chip, phy_blk);
+			continue;
+		}
+
+		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] ==
+		    0xFFFF) {
+			segment->l2p_table[log_blk - ms_start_idx[seg_no]] =
+			    phy_blk;
+			continue;
+		}
+
+		us1 = extra[0] & 0x10;
+		tmp_blk = segment->l2p_table[log_blk - ms_start_idx[seg_no]];
+		retval =
+		    ms_read_extra_data(chip, tmp_blk, 0, extra,
+				       MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS) {
+			continue;
+		}
+		us2 = extra[0] & 0x10;
+
+		(void)ms_arbitrate_l2p(chip, phy_blk,
+				       log_blk - ms_start_idx[seg_no], us1,
+				       us2);
+		continue;
+	}
+
+	segment->build_flag = 1;
+
+	RTSX_DEBUGP(("unused block count: %d\n", segment->unused_blk_cnt));
+
+	if (seg_no == ms_card->segment_cnt - 1) {
+		if (segment->unused_blk_cnt < 2) {
+			chip->card_wp |= MS_CARD;
+		}
+	} else {
+		if (segment->unused_blk_cnt < 1) {
+			chip->card_wp |= MS_CARD;
+		}
+	}
+
+	if (chip->card_wp & MS_CARD) {
+		return STATUS_SUCCESS;
+	}
+
+	for (log_blk = ms_start_idx[seg_no];
+	     log_blk < ms_start_idx[seg_no + 1]; log_blk++) {
+		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] ==
+		    0xFFFF) {
+			phy_blk = ms_get_unused_block(chip, seg_no);
+			if (phy_blk == 0xFFFF) {
+				chip->card_wp |= MS_CARD;
+				return STATUS_SUCCESS;
+			}
+			retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_GOTO(chip, BUILD_FAIL);
+			}
+			segment->l2p_table[log_blk - ms_start_idx[seg_no]] =
+			    phy_blk;
+			if (seg_no == ms_card->segment_cnt - 1) {
+				if (segment->unused_blk_cnt < 2) {
+					chip->card_wp |= MS_CARD;
+					return STATUS_SUCCESS;
+				}
+			} else {
+				if (segment->unused_blk_cnt < 1) {
+					chip->card_wp |= MS_CARD;
+					return STATUS_SUCCESS;
+				}
+			}
+		}
+	}
+
+	if (seg_no == 0) {
+		for (log_blk = 0; log_blk < 494; log_blk++) {
+			tmp_blk = segment->l2p_table[log_blk];
+			if (tmp_blk < ms_card->boot_block) {
+				RTSX_DEBUGP(("Boot block is not the first normal block.\n"));
+
+				if (chip->card_wp & MS_CARD) {
+					break;
+				}
+
+				phy_blk = ms_get_unused_block(chip, 0);
+				retval = ms_copy_page(chip, tmp_blk, phy_blk,
+						      log_blk, 0,
+						      ms_card->page_off + 1);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+
+				segment->l2p_table[log_blk] = phy_blk;
+
+				retval = ms_set_bad_block(chip, tmp_blk);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+		}
+	}
+
+	return STATUS_SUCCESS;
+
+ BUILD_FAIL:
+	segment->build_flag = 0;
+	if (segment->l2p_table) {
+		vfree(segment->l2p_table);
+		segment->l2p_table = NULL;
+	}
+	if (segment->free_table) {
+		vfree(segment->free_table);
+		segment->free_table = NULL;
+	}
+
+	return STATUS_FAIL;
+}
+
+int reset_ms_card(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	memset(ms_card, 0, sizeof(struct ms_info));
+
+	retval = enable_card_clock(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = select_card(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_card->ms_type = 0;
+
+	retval = reset_ms_pro(chip);
+	if (retval != STATUS_SUCCESS) {
+		if (ms_card->check_ms_flow) {
+			retval = reset_ms(chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		retval =
+		    ms_build_l2p_tbl(chip, ms_card->total_block / 512 - 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	RTSX_DEBUGP(("ms_card->ms_type = 0x%x\n", ms_card->ms_type));
+
+	return STATUS_SUCCESS;
+}
+
+static int mspro_set_rw_cmd(struct rtsx_chip *chip, u32 start_sec,
+			    u16 sec_cnt, u8 cmd)
+{
+	int retval, i;
+	u8 data[8];
+
+	data[0] = cmd;
+	data[1] = (u8) (sec_cnt >> 8);
+	data[2] = (u8) sec_cnt;
+	data[3] = (u8) (start_sec >> 24);
+	data[4] = (u8) (start_sec >> 16);
+	data[5] = (u8) (start_sec >> 8);
+	data[6] = (u8) start_sec;
+	data[7] = 0;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT, data,
+				   8);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void mspro_stop_seq_mode(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	if (ms_card->seq_mode) {
+		retval = ms_switch_clock(chip);
+		if (retval != STATUS_SUCCESS) {
+			return;
+		}
+
+		ms_card->seq_mode = 0;
+		ms_card->total_sec_cnt = 0;
+		ms_send_cmd(chip, PRO_STOP, WAIT_INT);
+
+		rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+	}
+}
+
+static inline int ms_auto_tune_clock(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	if (chip->asic_code) {
+		if (ms_card->ms_clock > 30) {
+			ms_card->ms_clock -= 20;
+		}
+	} else {
+		if (ms_card->ms_clock == CLK_80) {
+			ms_card->ms_clock = CLK_60;
+		} else if (ms_card->ms_clock == CLK_60) {
+			ms_card->ms_clock = CLK_40;
+		}
+	}
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
+				 struct rtsx_chip *chip, u32 start_sector,
+				 u16 sector_cnt)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, mode_2k = 0;
+	u16 count;
+	u8 val, trans_mode, rw_tpc, rw_cmd;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	ms_card->cleanup_counter = 0;
+
+	if (CHK_MSHG(ms_card)) {
+		if ((start_sector % 4) || (sector_cnt % 4)) {
+			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+				rw_tpc = PRO_READ_LONG_DATA;
+				rw_cmd = PRO_READ_DATA;
+			} else {
+				rw_tpc = PRO_WRITE_LONG_DATA;
+				rw_cmd = PRO_WRITE_DATA;
+			}
+		} else {
+			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+				rw_tpc = PRO_READ_QUAD_DATA;
+				rw_cmd = PRO_READ_2K_DATA;
+			} else {
+				rw_tpc = PRO_WRITE_QUAD_DATA;
+				rw_cmd = PRO_WRITE_2K_DATA;
+			}
+			mode_2k = 1;
+		}
+	} else {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			rw_tpc = PRO_READ_LONG_DATA;
+			rw_cmd = PRO_READ_DATA;
+		} else {
+			rw_tpc = PRO_WRITE_LONG_DATA;
+			rw_cmd = PRO_WRITE_DATA;
+		}
+	}
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+		trans_mode = MS_TM_AUTO_READ;
+	} else {
+		trans_mode = MS_TM_AUTO_WRITE;
+	}
+
+	RTSX_READ_REG(chip, MS_TRANS_CFG, &val);
+
+	if (ms_card->seq_mode) {
+		if ((ms_card->pre_dir != srb->sc_data_direction)
+		    || ((ms_card->pre_sec_addr + ms_card->pre_sec_cnt) !=
+			start_sector)
+		    || (mode_2k && (ms_card->seq_mode & MODE_512_SEQ))
+		    || (!mode_2k && (ms_card->seq_mode & MODE_2K_SEQ))
+		    || !(val & MS_INT_BREQ)
+		    || ((ms_card->total_sec_cnt + sector_cnt) > 0xFE00)) {
+			ms_card->seq_mode = 0;
+			ms_card->total_sec_cnt = 0;
+			if (val & MS_INT_BREQ) {
+				retval =
+				    ms_send_cmd(chip, PRO_STOP, WAIT_INT);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+
+				rtsx_write_register(chip, RBCTL, RB_FLUSH,
+						    RB_FLUSH);
+			}
+		}
+	}
+
+	if (!ms_card->seq_mode) {
+		ms_card->total_sec_cnt = 0;
+		if (sector_cnt >= SEQ_START_CRITERIA) {
+			if ((ms_card->capacity - start_sector) > 0xFE00) {
+				count = 0xFE00;
+			} else {
+				count =
+				    (u16) (ms_card->capacity - start_sector);
+			}
+			if (count > sector_cnt) {
+				if (mode_2k) {
+					ms_card->seq_mode |= MODE_2K_SEQ;
+				} else {
+					ms_card->seq_mode |= MODE_512_SEQ;
+				}
+			}
+		} else {
+			count = sector_cnt;
+		}
+		retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
+		if (retval != STATUS_SUCCESS) {
+			ms_card->seq_mode = 0;
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval =
+	    ms_transfer_data(chip, trans_mode, rw_tpc, sector_cnt, WAIT_INT,
+			     mode_2k, scsi_sg_count(srb), scsi_sglist(srb),
+			     scsi_bufflen(srb));
+	if (retval != STATUS_SUCCESS) {
+		ms_card->seq_mode = 0;
+		rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		rtsx_clear_ms_error(chip);
+
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			chip->rw_need_retry = 0;
+			RTSX_DEBUGP(("No card exist, exit mspro_rw_multi_sector\n"));
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (val & MS_INT_BREQ) {
+			ms_send_cmd(chip, PRO_STOP, WAIT_INT);
+		}
+		if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+			RTSX_DEBUGP(("MSPro CRC error, tune clock!\n"));
+			chip->rw_need_retry = 1;
+			ms_auto_tune_clock(chip);
+		}
+
+		TRACE_RET(chip, retval);
+	}
+
+	if (ms_card->seq_mode) {
+		ms_card->pre_sec_addr = start_sector;
+		ms_card->pre_sec_cnt = sector_cnt;
+		ms_card->pre_dir = srb->sc_data_direction;
+		ms_card->total_sec_cnt += sector_cnt;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mspro_read_format_progress(struct rtsx_chip *chip,
+				      const int short_data_len)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u32 total_progress, cur_progress;
+	u8 cnt, tmp;
+	u8 data[8];
+
+	RTSX_DEBUGP(("mspro_read_format_progress, short_data_len = %d\n",
+		     short_data_len));
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!(tmp & MS_INT_BREQ)) {
+		if ((tmp &
+		     (MS_INT_CED | MS_INT_BREQ | MS_INT_CMDNK | MS_INT_ERR))
+		    == MS_INT_CED) {
+			ms_card->format_status = FORMAT_SUCCESS;
+			return STATUS_SUCCESS;
+		}
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (short_data_len >= 256) {
+		cnt = 0;
+	} else {
+		cnt = (u8) short_data_len;
+	}
+
+	retval =
+	    rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT,
+				MS_NO_CHECK_INT);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval =
+	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT, data, 8);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	total_progress =
+	    (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
+	cur_progress =
+	    (data[4] << 24) | (data[5] << 16) | (data[6] << 8) | data[7];
+
+	RTSX_DEBUGP(("total_progress = %d, cur_progress = %d\n",
+		     total_progress, cur_progress));
+
+	if (total_progress == 0) {
+		ms_card->progress = 0;
+	} else {
+		u64 ulltmp = (u64) cur_progress * (u64) 65535;
+		do_div(ulltmp, total_progress);
+		ms_card->progress = (u16) ulltmp;
+	}
+	RTSX_DEBUGP(("progress = %d\n", ms_card->progress));
+
+	for (i = 0; i < 5000; i++) {
+		retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
+		if (retval != STATUS_SUCCESS) {
+			ms_card->format_status = FORMAT_FAIL;
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		if (tmp &
+		    (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR)) {
+			break;
+		}
+
+		wait_timeout(1);
+	}
+
+	retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT, 0);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (i == 5000) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
+		ms_card->format_status = FORMAT_FAIL;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (tmp & MS_INT_CED) {
+		ms_card->format_status = FORMAT_SUCCESS;
+		ms_card->pro_under_formatting = 0;
+	} else if (tmp & MS_INT_BREQ) {
+		ms_card->format_status = FORMAT_IN_PROGRESS;
+	} else {
+		ms_card->format_status = FORMAT_FAIL;
+		ms_card->pro_under_formatting = 0;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void mspro_polling_format_status(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int i;
+
+	if (ms_card->pro_under_formatting
+	    && (rtsx_get_stat(chip) != RTSX_STAT_SS)) {
+		rtsx_set_run_stat(chip, 1);
+
+		for (i = 0; i < 65535; i++) {
+			mspro_read_format_progress(chip, MS_SHORT_DATA_LEN);
+			if (ms_card->format_status != FORMAT_IN_PROGRESS) {
+				break;
+			}
+		}
+	}
+
+	return;
+}
+
+int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+		 int short_data_len, int quick_format)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 buf[8], tmp;
+	u16 para;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, Pro_TPCParm, 0x01);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	memset(buf, 0, 2);
+	switch (short_data_len) {
+	case 32:
+		buf[0] = 0;
+		break;
+	case 64:
+		buf[0] = 1;
+		break;
+	case 128:
+		buf[0] = 2;
+		break;
+	case 256:
+	default:
+		buf[0] = 3;
+		break;
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, PRO_WRITE_REG, 1, NO_WAIT_INT, buf,
+				   2);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (quick_format) {
+		para = 0x0000;
+	} else {
+		para = 0x0001;
+	}
+	retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, MS_TRANS_CFG, &tmp);
+
+	if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
+		ms_card->pro_under_formatting = 1;
+		ms_card->progress = 0;
+		ms_card->format_status = FORMAT_IN_PROGRESS;
+		return STATUS_SUCCESS;
+	}
+
+	if (tmp & MS_INT_CED) {
+		ms_card->pro_under_formatting = 0;
+		ms_card->progress = 0;
+		ms_card->format_status = FORMAT_SUCCESS;
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_NO_SENSE);
+		return STATUS_SUCCESS;
+	}
+
+	TRACE_RET(chip, STATUS_FAIL);
+}
+
+static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
+				  u16 log_blk, u8 start_page, u8 end_page,
+				  u8 * buf, unsigned int *index,
+				  unsigned int *offset)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 extra[MS_EXTRA_SIZE], page_addr, val, trans_cfg, data[6];
+	u8 *ptr;
+
+	retval =
+	    ms_read_extra_data(chip, phy_blk, start_page, extra,
+			       MS_EXTRA_SIZE);
+	if (retval == STATUS_SUCCESS) {
+		if ((extra[1] & 0x30) != 0x30) {
+			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (phy_blk >> 8);
+	data[3] = (u8) phy_blk;
+	data[4] = 0;
+	data[5] = start_page;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ptr = buf;
+
+	for (page_addr = start_page; page_addr < end_page; page_addr++) {
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval =
+		    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		if (val & INT_REG_ERR) {
+			if (val & INT_REG_BREQ) {
+				retval = ms_read_status_reg(chip);
+				if (retval != STATUS_SUCCESS) {
+					if (!(chip->card_wp & MS_CARD)) {
+						reset_ms(chip);
+						ms_set_page_status(log_blk,
+								   setPS_NG,
+								   extra,
+								   MS_EXTRA_SIZE);
+						ms_write_extra_data(chip,
+								    phy_blk,
+								    page_addr,
+								    extra,
+								    MS_EXTRA_SIZE);
+					}
+					ms_set_err_code(chip,
+							MS_FLASH_READ_ERROR);
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			} else {
+				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_BREQ_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		if (page_addr == (end_page - 1)) {
+			if (!(val & INT_REG_CED)) {
+				retval =
+				    ms_send_cmd(chip, BLOCK_END, WAIT_INT);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+
+			retval =
+			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
+					  1);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (!(val & INT_REG_CED)) {
+				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			trans_cfg = NO_WAIT_INT;
+		} else {
+			trans_cfg = WAIT_INT;
+		}
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
+			     READ_PAGE_DATA);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
+			     trans_cfg);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+			     RING_BUFFER);
+
+		trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+			     MS_TRANSFER_START | MS_TM_NORMAL_READ);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+			     MS_TRANSFER_END, MS_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval =
+		    rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
+					       scsi_sg_count(chip->srb),
+					       index, offset, DMA_FROM_DEVICE,
+					       chip->ms_timeout);
+		if (retval < 0) {
+			if (retval == -ETIMEDOUT) {
+				ms_set_err_code(chip, MS_TO_ERROR);
+				rtsx_clear_ms_error(chip);
+				TRACE_RET(chip, STATUS_TIMEDOUT);
+			}
+
+			retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+			if (retval != STATUS_SUCCESS) {
+				ms_set_err_code(chip, MS_TO_ERROR);
+				rtsx_clear_ms_error(chip);
+				TRACE_RET(chip, STATUS_TIMEDOUT);
+			}
+			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+				ms_set_err_code(chip, MS_CRC16_ERROR);
+				rtsx_clear_ms_error(chip);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		if (scsi_sg_count(chip->srb) == 0)
+			ptr += 512;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
+				   u16 new_blk, u16 log_blk, u8 start_page,
+				   u8 end_page, u8 * buf, unsigned int *index,
+				   unsigned int *offset)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, i;
+	u8 page_addr, val, data[16];
+	u8 *ptr;
+
+	if (!start_page) {
+		retval =
+		    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
+				       SystemParm, 7);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (CHK_MS4BIT(ms_card)) {
+			data[0] = 0x88;
+		} else {
+			data[0] = 0x80;
+		}
+		data[1] = 0;
+		data[2] = (u8) (old_blk >> 8);
+		data[3] = (u8) old_blk;
+		data[4] = 0x80;
+		data[5] = 0;
+		data[6] = 0xEF;
+		data[7] = 0xFF;
+
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 8);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+		retval =
+		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
+				    NO_WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval =
+	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
+			       (6 + MS_EXTRA_SIZE));
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	if (CHK_MS4BIT(ms_card)) {
+		data[0] = 0x88;
+	} else {
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8) (new_blk >> 8);
+	data[3] = (u8) new_blk;
+	if ((end_page - start_page) == 1) {
+		data[4] = 0x20;
+	} else {
+		data[4] = 0;
+	}
+	data[5] = start_page;
+	data[6] = 0xF8;
+	data[7] = 0xFF;
+	data[8] = (u8) (log_blk >> 8);
+	data[9] = (u8) log_blk;
+
+	for (i = 0x0A; i < 0x10; i++) {
+		data[i] = 0xFF;
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE,
+				   NO_WAIT_INT, data, 16);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ptr = buf;
+	for (page_addr = start_page; page_addr < end_page; page_addr++) {
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		if (!(val & INT_REG_BREQ)) {
+			ms_set_err_code(chip, MS_BREQ_ERROR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		udelay(30);
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
+			     WRITE_PAGE_DATA);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
+			     WAIT_INT);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+			     RING_BUFFER);
+
+		trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+			     MS_TRANSFER_START | MS_TM_NORMAL_WRITE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+			     MS_TRANSFER_END, MS_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval =
+		    rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
+					       scsi_sg_count(chip->srb),
+					       index, offset, DMA_TO_DEVICE,
+					       chip->ms_timeout);
+		if (retval < 0) {
+			ms_set_err_code(chip, MS_TO_ERROR);
+			rtsx_clear_ms_error(chip);
+
+			if (retval == -ETIMEDOUT) {
+				TRACE_RET(chip, STATUS_TIMEDOUT);
+			} else {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		retval =
+		    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if ((end_page - start_page) == 1) {
+			if (!(val & INT_REG_CED)) {
+				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			if (page_addr == (end_page - 1)) {
+				if (!(val & INT_REG_CED)) {
+					retval =
+					    ms_send_cmd(chip, BLOCK_END,
+							WAIT_INT);
+					if (retval != STATUS_SUCCESS) {
+						TRACE_RET(chip, STATUS_FAIL);
+					}
+				}
+
+				retval =
+				    ms_read_bytes(chip, GET_INT, 1,
+						  NO_WAIT_INT, &val, 1);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+
+			if ((page_addr == (end_page - 1))
+			    || (page_addr == ms_card->page_off)) {
+				if (!(val & INT_REG_CED)) {
+					ms_set_err_code(chip,
+							MS_FLASH_WRITE_ERROR);
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+		}
+
+		if (scsi_sg_count(chip->srb) == 0)
+			ptr += 512;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_finish_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
+			   u16 log_blk, u8 page_off)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval, seg_no;
+
+	retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
+			      page_off, ms_card->page_off + 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	seg_no = old_blk >> 9;
+
+	if (MS_TST_BAD_BLOCK_FLG(ms_card)) {
+		MS_CLR_BAD_BLOCK_FLG(ms_card);
+		ms_set_bad_block(chip, old_blk);
+	} else {
+		retval = ms_erase_block(chip, old_blk);
+		if (retval == STATUS_SUCCESS) {
+			ms_set_unused_block(chip, old_blk);
+		}
+	}
+
+	ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_prepare_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
+			    u16 log_blk, u8 start_page)
+{
+	int retval;
+
+	if (start_page) {
+		retval =
+		    ms_copy_page(chip, old_blk, new_blk, log_blk, 0,
+				 start_page);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef MS_DELAY_WRITE
+int ms_delay_write(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	struct ms_delay_write_tag *delay_write = &(ms_card->delay_write);
+	int retval;
+
+	if (delay_write->delay_write_flag) {
+		retval = ms_set_init_para(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		delay_write->delay_write_flag = 0;
+		retval = ms_finish_write(chip,
+					 delay_write->old_phyblock,
+					 delay_write->new_phyblock,
+					 delay_write->logblock,
+					 delay_write->pageoff);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static inline void ms_rw_fail(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+	} else {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_WRITE_ERR);
+	}
+}
+
+static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+			      u32 start_sector, u16 sector_cnt)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned int lun = SCSI_LUN(srb);
+	int retval, seg_no;
+	unsigned int index = 0, offset = 0;
+	u16 old_blk = 0, new_blk = 0, log_blk, total_sec_cnt = sector_cnt;
+	u8 start_page, end_page = 0, page_cnt;
+	u8 *ptr;
+#ifdef MS_DELAY_WRITE
+	struct ms_delay_write_tag *delay_write = &(ms_card->delay_write);
+#endif
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	ms_card->cleanup_counter = 0;
+
+	ptr = (u8 *) scsi_sglist(srb);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		ms_rw_fail(srb, chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	log_blk = (u16) (start_sector >> ms_card->block_shift);
+	start_page = (u8) (start_sector & ms_card->page_off);
+
+	for (seg_no = 0; seg_no < sizeof(ms_start_idx) / 2 - 1; seg_no++) {
+		if (log_blk < ms_start_idx[seg_no + 1]) {
+			break;
+		}
+	}
+
+	if (ms_card->segment[seg_no].build_flag == 0) {
+		retval = ms_build_l2p_tbl(chip, seg_no);
+		if (retval != STATUS_SUCCESS) {
+			chip->card_fail |= MS_CARD;
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_NOT_PRESENT);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+#ifdef MS_DELAY_WRITE
+		if (delay_write->delay_write_flag &&
+		    (delay_write->logblock == log_blk) &&
+		    (start_page > delay_write->pageoff)) {
+			delay_write->delay_write_flag = 0;
+			retval = ms_copy_page(chip,
+					      delay_write->old_phyblock,
+					      delay_write->new_phyblock,
+					      log_blk, delay_write->pageoff,
+					      start_page);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			old_blk = delay_write->old_phyblock;
+			new_blk = delay_write->new_phyblock;
+		} else if (delay_write->delay_write_flag &&
+			   (delay_write->logblock == log_blk) &&
+			   (start_page == delay_write->pageoff)) {
+			delay_write->delay_write_flag = 0;
+			old_blk = delay_write->old_phyblock;
+			new_blk = delay_write->new_phyblock;
+		} else {
+			retval = ms_delay_write(chip);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+#endif
+			old_blk =
+			    ms_get_l2p_tbl(chip, seg_no,
+					   log_blk - ms_start_idx[seg_no]);
+			new_blk = ms_get_unused_block(chip, seg_no);
+			if ((old_blk == 0xFFFF) || (new_blk == 0xFFFF)) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			retval =
+			    ms_prepare_write(chip, old_blk, new_blk, log_blk,
+					     start_page);
+			if (retval != STATUS_SUCCESS) {
+				if (detect_card_cd(chip, MS_CARD) !=
+				    STATUS_SUCCESS) {
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MEDIA_NOT_PRESENT);
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+#ifdef MS_DELAY_WRITE
+		}
+#endif
+	} else {
+#ifdef MS_DELAY_WRITE
+		retval = ms_delay_write(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+#endif
+		old_blk =
+		    ms_get_l2p_tbl(chip, seg_no,
+				   log_blk - ms_start_idx[seg_no]);
+		if (old_blk == 0xFFFF) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	RTSX_DEBUGP(("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n", seg_no,
+		     old_blk, new_blk));
+
+	while (total_sec_cnt) {
+		if ((start_page + total_sec_cnt) > (ms_card->page_off + 1)) {
+			end_page = ms_card->page_off + 1;
+		} else {
+			end_page = start_page + (u8) total_sec_cnt;
+		}
+		page_cnt = end_page - start_page;
+
+		RTSX_DEBUGP(("start_page = %d, end_page = %d, page_cnt = %d\n", start_page, end_page, page_cnt));
+
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			retval = ms_read_multiple_pages(chip,
+							old_blk, log_blk,
+							start_page, end_page,
+							ptr, &index, &offset);
+		} else {
+			retval = ms_write_multiple_pages(chip, old_blk,
+							 new_blk, log_blk,
+							 start_page, end_page,
+							 ptr, &index,
+							 &offset);
+		}
+
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			ms_rw_fail(srb, chip);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (srb->sc_data_direction == DMA_TO_DEVICE) {
+			if (end_page == (ms_card->page_off + 1)) {
+				retval = ms_erase_block(chip, old_blk);
+				if (retval == STATUS_SUCCESS) {
+					ms_set_unused_block(chip, old_blk);
+				}
+				ms_set_l2p_tbl(chip, seg_no,
+					       log_blk - ms_start_idx[seg_no],
+					       new_blk);
+			}
+		}
+
+		total_sec_cnt -= page_cnt;
+		if (scsi_sg_count(srb) == 0)
+			ptr += page_cnt * 512;
+
+		if (total_sec_cnt == 0) {
+			break;
+		}
+
+		log_blk++;
+
+		for (seg_no = 0; seg_no < sizeof(ms_start_idx) / 2 - 1;
+		     seg_no++) {
+			if (log_blk < ms_start_idx[seg_no + 1]) {
+				break;
+			}
+		}
+
+		if (ms_card->segment[seg_no].build_flag == 0) {
+			retval = ms_build_l2p_tbl(chip, seg_no);
+			if (retval != STATUS_SUCCESS) {
+				chip->card_fail |= MS_CARD;
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		old_blk =
+		    ms_get_l2p_tbl(chip, seg_no,
+				   log_blk - ms_start_idx[seg_no]);
+		if (old_blk == 0xFFFF) {
+			ms_rw_fail(srb, chip);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (srb->sc_data_direction == DMA_TO_DEVICE) {
+			new_blk = ms_get_unused_block(chip, seg_no);
+			if (new_blk == 0xFFFF) {
+				ms_rw_fail(srb, chip);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		RTSX_DEBUGP(("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
+			     seg_no, old_blk, new_blk));
+
+		start_page = 0;
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+		if (end_page < (ms_card->page_off + 1)) {
+#ifdef MS_DELAY_WRITE
+			delay_write->delay_write_flag = 1;
+			delay_write->old_phyblock = old_blk;
+			delay_write->new_phyblock = new_blk;
+			delay_write->logblock = log_blk;
+			delay_write->pageoff = end_page;
+#else
+			retval =
+			    ms_finish_write(chip, old_blk, new_blk, log_blk,
+					    end_page);
+			if (retval != STATUS_SUCCESS) {
+				if (detect_card_cd(chip, MS_CARD) !=
+				    STATUS_SUCCESS) {
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MEDIA_NOT_PRESENT);
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+
+				ms_rw_fail(srb, chip);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+#endif
+		}
+	}
+
+	scsi_set_resid(srb, 0);
+
+	return STATUS_SUCCESS;
+}
+
+int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
+	  u16 sector_cnt)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	if (CHK_MSPRO(ms_card)) {
+		retval =
+		    mspro_rw_multi_sector(srb, chip, start_sector,
+					  sector_cnt);
+	} else {
+		retval =
+		    ms_rw_multi_sector(srb, chip, start_sector, sector_cnt);
+	}
+
+	return retval;
+}
+
+void ms_free_l2p_tbl(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int i = 0;
+
+	if (ms_card->segment != NULL) {
+		for (i = 0; i < ms_card->segment_cnt; i++) {
+			if (ms_card->segment[i].l2p_table != NULL) {
+				vfree(ms_card->segment[i].l2p_table);
+				ms_card->segment[i].l2p_table = NULL;
+			}
+			if (ms_card->segment[i].free_table != NULL) {
+				vfree(ms_card->segment[i].free_table);
+				ms_card->segment[i].free_table = NULL;
+			}
+		}
+		vfree(ms_card->segment);
+		ms_card->segment = NULL;
+	}
+}
+
+#ifdef SUPPORT_MAGIC_GATE
+
+#ifdef READ_BYTES_WAIT_INT
+int ms_poll_int(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANS_CFG, MS_INT_CED,
+		     MS_INT_CED);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	val = *rtsx_get_cmd_data(chip);
+	if (val & MS_INT_ERR) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+#ifdef MS_SAMPLE_INT_ERR
+static int check_ms_err(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val;
+
+	retval = rtsx_read_register(chip, MS_TRANSFER, &val);
+	if (retval != STATUS_SUCCESS) {
+		return 1;
+	}
+	if (val & MS_TRANSFER_ERR) {
+		return 1;
+	}
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+	if (retval != STATUS_SUCCESS) {
+		return 1;
+	}
+
+	if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
+		return 1;
+	}
+
+	return 0;
+}
+#else
+static int check_ms_err(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val;
+
+	retval = rtsx_read_register(chip, MS_TRANSFER, &val);
+	if (retval != STATUS_SUCCESS) {
+		return 1;
+	}
+	if (val & MS_TRANSFER_ERR) {
+		return 1;
+	}
+
+	return 0;
+}
+#endif
+
+static int mg_send_ex_cmd(struct rtsx_chip *chip, u8 cmd, u8 entry_num)
+{
+	int retval, i;
+	u8 data[8];
+
+	data[0] = cmd;
+	data[1] = 0;
+	data[2] = 0;
+	data[3] = 0;
+	data[4] = 0;
+	data[5] = 0;
+	data[6] = entry_num;
+	data[7] = 0;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval =
+		    ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT, data,
+				   8);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+	if (i == MS_MAX_RETRY_COUNT) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (check_ms_err(chip)) {
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+/**
+  * Set TPC Parameter Register to 00
+
+  * It sets the data size of a READ_SHORT_DATA and a WRITE_SHORT_DATA TPC
+  * as 32 bytes.
+  */
+static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
+			       u8 mg_entry_num)
+{
+	int retval;
+	u8 buf[6];
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	if (type == 0) {
+		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_TPCParm, 1);
+	} else {
+		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
+	}
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf[0] = 0;
+	buf[1] = 0;
+	if (type == 1) {
+		buf[2] = 0;
+		buf[3] = 0;
+		buf[4] = 0;
+		buf[5] = mg_entry_num;
+	}
+	retval =
+	    ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
+			   NO_WAIT_INT, buf, 6);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+/**
+  * Get MagciGate ID and set Leaf ID to medium.
+
+  * After receiving this SCSI command, adapter shall fulfill 2 tasks below in order:
+  * 1. send GET_ID TPC command to get MagicGate ID and hold it till Response&challenge CMD.
+  * 2. send SET_ID TPC command to medium with Leaf ID released by host in this SCSI CMD.
+  */
+int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	int i;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf1[32], buf2[12];
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	if (scsi_bufflen(srb) < 12) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	memset(buf1, 0, 32);
+	rtsx_stor_get_xfer_buf(buf2, min(12, (int)scsi_bufflen(srb)), srb);
+	for (i = 0; i < 8; i++) {
+		buf1[8 + i] = buf2[4 + i];
+	}
+	retval =
+	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf1,
+			   32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+/**
+  * Send Local EKB to host.
+
+  * After receiving this SCSI command, adapter shall read the divided data(1536 bytes totally)
+  * from medium by using READ_LONG_DATA TPC for 3 times, and report data to host with
+  * data-length is 1052 bytes.
+  */
+int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval = STATUS_FAIL;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 *buf = NULL;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf = (u8 *) rtsx_alloc_dma_buf(chip, 1540, GFP_KERNEL);
+	if (!buf) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	buf[0] = 0x04;
+	buf[1] = 0x1A;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+
+	retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		TRACE_GOTO(chip, GetEKBFinish);
+	}
+
+	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
+				  3, WAIT_INT, 0, 0, buf + 4, 1536);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		TRACE_GOTO(chip, GetEKBFinish);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	bufflen = min(1052, (int)scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf, bufflen, srb);
+
+ GetEKBFinish:
+	if (buf) {
+		rtsx_free_dma_buf(chip, buf);
+	}
+	return retval;
+}
+
+/**
+  * Send challenge(host) to medium.
+
+  * After receiving this SCSI command, adapter shall sequentially issues TPC commands
+  * to the medium for writing 8-bytes data as challenge by host within a short data packet.
+  */
+int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	int bufflen;
+	int i;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf[32];
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval =
+	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT, buf, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	memcpy(ms_card->magic_gate_id, buf, 16);
+
+#ifdef READ_BYTES_WAIT_INT
+	retval = ms_poll_int(chip);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	bufflen = min(12, (int)scsi_bufflen(srb));
+	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
+
+	for (i = 0; i < 8; i++) {
+		buf[i] = buf[4 + i];
+	}
+	for (i = 0; i < 24; i++) {
+		buf[8 + i] = 0;
+	}
+	retval =
+	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_card->mg_auth = 0;
+
+	return STATUS_SUCCESS;
+}
+
+/**
+  * Send Response and Challenge data  to host.
+
+  * After receiving this SCSI command, adapter shall communicates with the medium, get
+  * parameters(HRd, Rms, MagicGateID) by using READ_SHORT_DATA TPC and send the
+  * data to host according to certain format required by MG-R specification.
+
+  * The paremeter MagicGateID is the one that adapter has obtained from the medium by TPC
+  * commands in Set Leaf ID command phase previously.
+  */
+int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf1[32], buf2[36];
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval =
+	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT, buf1, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf2[0] = 0x00;
+	buf2[1] = 0x22;
+	buf2[2] = 0x00;
+	buf2[3] = 0x00;
+
+	memcpy(buf2 + 4, ms_card->magic_gate_id, 16);
+	memcpy(buf2 + 20, buf1, 16);
+
+	bufflen = min(36, (int)scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf2, bufflen, srb);
+
+#ifdef READ_BYTES_WAIT_INT
+	retval = ms_poll_int(chip);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+/**
+  * Send response(host) to medium.
+
+  * After receiving this SCSI command, adapter shall sequentially issues TPC commands
+  * to the medium for writing 8-bytes data as challenge by host within a short data packet.
+  */
+int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	int i;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf[32];
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	bufflen = min(12, (int)scsi_bufflen(srb));
+	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
+
+	for (i = 0; i < 8; i++) {
+		buf[i] = buf[4 + i];
+	}
+	for (i = 0; i < 24; i++) {
+		buf[8 + i] = 0;
+	}
+	retval =
+	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ms_card->mg_auth = 1;
+
+	return STATUS_SUCCESS;
+}
+
+/**
+  * Send ICV data to host.
+
+  * After receiving this SCSI command, adapter shall read the divided data(1024 bytes totally)
+  * from medium by using READ_LONG_DATA TPC for 2 times, and report data to host with
+  * data-length is 1028 bytes.
+
+  * Since the extra 4 bytes data is just only a prefix to original data that read from medium, so
+  * that the 4-byte data pushed into Ring buffer precedes data tramsinssion from medium to
+  * Ring buffer by DMA mechanisim in order to get maximum performance and minimum code
+  * size simultaneously.
+  */
+int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 *buf = NULL;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf = (u8 *) rtsx_alloc_dma_buf(chip, 1028, GFP_KERNEL);
+	if (!buf) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	buf[0] = 0x04;
+	buf[1] = 0x02;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+
+	retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun,
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		TRACE_GOTO(chip, GetICVFinish);
+	}
+
+	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
+				  2, WAIT_INT, 0, 0, buf + 4, 1024);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun,
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		rtsx_clear_ms_error(chip);
+		TRACE_GOTO(chip, GetICVFinish);
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun,
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		rtsx_clear_ms_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	bufflen = min(1028, (int)scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf, bufflen, srb);
+
+ GetICVFinish:
+	if (buf) {
+		rtsx_free_dma_buf(chip, buf);
+	}
+	return retval;
+}
+
+/**
+  * Send ICV data to medium.
+
+  * After receiving this SCSI command, adapter shall receive 1028 bytes and write the later 1024
+  * bytes to medium by WRITE_LONG_DATA TPC consecutively.
+
+   * Since the first 4-bytes data is just only a prefix to original data that sent by host, and it
+   * should be skipped by shifting DMA pointer before writing 1024 bytes to medium.
+   */
+int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	int bufflen;
+#ifdef MG_SET_ICV_SLOW
+	int i;
+#endif
+	unsigned int lun = SCSI_LUN(srb);
+	u8 *buf = NULL;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	buf = (u8 *) rtsx_alloc_dma_buf(chip, 1028, GFP_KERNEL);
+	if (!buf) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	bufflen = min(1028, (int)scsi_bufflen(srb));
+	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
+
+	retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
+	if (retval != STATUS_SUCCESS) {
+		if (ms_card->mg_auth == 0) {
+			if ((buf[5] & 0xC0) != 0) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+			} else {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_WRITE_ERR);
+			}
+		} else {
+			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
+		}
+		TRACE_GOTO(chip, SetICVFinish);
+	}
+#ifdef MG_SET_ICV_SLOW
+	for (i = 0; i < 2; i++) {
+		udelay(50);
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
+			     PRO_WRITE_LONG_DATA);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
+			     WAIT_INT);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+			     RING_BUFFER);
+
+		trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+			     MS_TRANSFER_START | MS_TM_NORMAL_WRITE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+			     MS_TRANSFER_END, MS_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval =
+		    rtsx_transfer_data(chip, MS_CARD, buf + 4 + i * 512, 512,
+				       0, DMA_TO_DEVICE, 3000);
+		if ((retval < 0) || check_ms_err(chip)) {
+			rtsx_clear_ms_error(chip);
+			if (ms_card->mg_auth == 0) {
+				if ((buf[5] & 0xC0) != 0) {
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+				} else {
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MG_WRITE_ERR);
+				}
+			} else {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_WRITE_ERR);
+			}
+			retval = STATUS_FAIL;
+			TRACE_GOTO(chip, SetICVFinish);
+		}
+	}
+#else
+	retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
+				  2, WAIT_INT, 0, 0, buf + 4, 1024);
+	if ((retval != STATUS_SUCCESS) || check_ms_err(chip)) {
+		rtsx_clear_ms_error(chip);
+		if (ms_card->mg_auth == 0) {
+			if ((buf[5] & 0xC0) != 0) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+			} else {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_WRITE_ERR);
+			}
+		} else {
+			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
+		}
+		TRACE_GOTO(chip, SetICVFinish);
+	}
+#endif
+
+ SetICVFinish:
+	if (buf) {
+		rtsx_free_dma_buf(chip, buf);
+	}
+	return retval;
+}
+
+#endif
+
+void ms_cleanup_work(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+
+	if (CHK_MSPRO(ms_card)) {
+		if (ms_card->seq_mode) {
+			RTSX_DEBUGP(("MS Pro: stop transmission\n"));
+			mspro_stop_seq_mode(chip);
+			ms_card->cleanup_counter = 0;
+		}
+		if (CHK_MSHG(ms_card)) {
+			rtsx_write_register(chip, MS_CFG, MS_2K_SECTOR_MODE,
+					    0x00);
+		}
+	}
+#ifdef MS_DELAY_WRITE
+	else if ((!CHK_MSPRO(ms_card))
+		 && ms_card->delay_write.delay_write_flag) {
+		RTSX_DEBUGP(("MS: delay write\n"));
+		ms_delay_write(chip);
+		ms_card->cleanup_counter = 0;
+	}
+#endif
+}
+
+int ms_power_off_card3v3(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = disable_card_clock(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (chip->asic_code) {
+		retval = ms_pull_ctl_disable(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
+			       FPGA_MS_PULL_CTL_BIT | 0x20,
+			       FPGA_MS_PULL_CTL_BIT);
+	}
+	RTSX_WRITE_REG(chip, CARD_OE, MS_OUTPUT_EN, 0);
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_off(chip, MS_CARD);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int release_ms_card(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+
+	RTSX_DEBUGP(("release_ms_card\n"));
+
+#ifdef MS_DELAY_WRITE
+	ms_card->delay_write.delay_write_flag = 0;
+#endif
+	ms_card->pro_under_formatting = 0;
+
+	chip->card_ready &= ~MS_CARD;
+	chip->card_fail &= ~MS_CARD;
+	chip->card_wp &= ~MS_CARD;
+
+	ms_free_l2p_tbl(chip);
+
+	memset(ms_card->raw_sys_info, 0, 96);
+	memset(ms_card->raw_ms_id, 0, 16);
+#ifdef SUPPORT_PCGL_1P18
+	memset(ms_card->raw_model_name, 0, 48);
+#endif
+
+	if (CHECK_VERSION(chip, 0x5227, IC_VER_A)) {
+		retval = rtsx_enable_ocp(chip);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+
+		wait_timeout(10);
+		if (chip->ocp_int)
+			rtsx_clear_ocp(chip);
+	}
+
+	retval = ms_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rts5229/ms.h b/drivers/staging/rts5229/ms.h
new file mode 100644
index 0000000..bbe03e0
--- /dev/null
+++ b/drivers/staging/rts5229/ms.h
@@ -0,0 +1,226 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_MS_H
+#define __REALTEK_RTSX_MS_H
+
+#define MS_DELAY_WRITE
+
+#define	MS_MAX_RETRY_COUNT	3
+
+#define	MS_EXTRA_SIZE		0x9
+
+#define	WRT_PRTCT		0x01
+
+#define	MS_NO_ERROR				0x00
+#define	MS_CRC16_ERROR				0x80
+#define	MS_TO_ERROR				0x40
+#define	MS_NO_CARD				0x20
+#define	MS_NO_MEMORY				0x10
+#define	MS_CMD_NK				0x08
+#define	MS_FLASH_READ_ERROR			0x04
+#define	MS_FLASH_WRITE_ERROR			0x02
+#define	MS_BREQ_ERROR				0x01
+#define	MS_NOT_FOUND				0x03
+
+#define READ_PAGE_DATA				0x02
+#define READ_REG				0x04
+#define	GET_INT					0x07
+#define WRITE_PAGE_DATA				0x0D
+#define WRITE_REG				0x0B
+#define SET_RW_REG_ADRS				0x08
+#define SET_CMD					0x0E
+
+#define	PRO_READ_LONG_DATA			0x02
+#define	PRO_READ_SHORT_DATA			0x03
+#define PRO_READ_REG				0x04
+#define	PRO_READ_QUAD_DATA			0x05
+#define PRO_GET_INT				0x07
+#define	PRO_WRITE_LONG_DATA			0x0D
+#define	PRO_WRITE_SHORT_DATA			0x0C
+#define	PRO_WRITE_QUAD_DATA			0x0A
+#define PRO_WRITE_REG				0x0B
+#define PRO_SET_RW_REG_ADRS			0x08
+#define PRO_SET_CMD				0x0E
+#define PRO_EX_SET_CMD				0x09
+
+#ifdef SUPPORT_MAGIC_GATE
+
+#define MG_GET_ID		0x40
+#define MG_SET_LID		0x41
+#define MG_GET_LEKB		0x42
+#define MG_SET_RD		0x43
+#define MG_MAKE_RMS		0x44
+#define MG_MAKE_KSE		0x45
+#define MG_SET_IBD		0x46
+#define MG_GET_IBD		0x47
+
+#endif
+
+#ifdef XC_POWERCLASS
+#define XC_CHG_POWER		0x16
+#endif
+
+#define BLOCK_READ	0xAA
+#define	BLOCK_WRITE	0x55
+#define BLOCK_END	0x33
+#define BLOCK_ERASE	0x99
+#define FLASH_STOP	0xCC
+
+#define SLEEP		0x5A
+#define CLEAR_BUF	0xC3
+#define MS_RESET	0x3C
+
+#define PRO_READ_DATA		0x20
+#define	PRO_WRITE_DATA		0x21
+#define PRO_READ_ATRB		0x24
+#define PRO_STOP		0x25
+#define PRO_ERASE		0x26
+#define	PRO_READ_2K_DATA	0x27
+#define	PRO_WRITE_2K_DATA	0x28
+
+#define PRO_FORMAT		0x10
+#define PRO_SLEEP		0x11
+
+#define	IntReg			0x01
+#define StatusReg0		0x02
+#define StatusReg1		0x03
+
+#define SystemParm		0x10
+#define BlockAdrs		0x11
+#define CMDParm			0x14
+#define PageAdrs		0x15
+
+#define OverwriteFlag		0x16
+#define ManagemenFlag		0x17
+#define LogicalAdrs		0x18
+#define ReserveArea		0x1A
+
+#define	Pro_IntReg		0x01
+#define Pro_StatusReg		0x02
+#define Pro_TypeReg		0x04
+#define	Pro_IFModeReg		0x05
+#define Pro_CatagoryReg		0x06
+#define Pro_ClassReg		0x07
+
+#define Pro_SystemParm		0x10
+#define Pro_DataCount1		0x11
+#define Pro_DataCount0		0x12
+#define Pro_DataAddr3		0x13
+#define Pro_DataAddr2		0x14
+#define Pro_DataAddr1		0x15
+#define Pro_DataAddr0		0x16
+
+#define Pro_TPCParm		0x17
+#define Pro_CMDParm		0x18
+
+#define	INT_REG_CED		0x80
+#define	INT_REG_ERR		0x40
+#define	INT_REG_BREQ		0x20
+#define	INT_REG_CMDNK		0x01
+
+#define	BLOCK_BOOT		0xC0
+#define	BLOCK_OK		0x80
+#define	PAGE_OK			0x60
+#define	DATA_COMPL		0x10
+
+#define	NOT_BOOT_BLOCK		0x4
+#define	NOT_TRANSLATION_TABLE	0x8
+
+#define	HEADER_ID0		PPBUF_BASE2
+#define	HEADER_ID1		PPBUF_BASE2 + 1
+#define	DISABLED_BLOCK0		PPBUF_BASE2 + 0x170 + 4
+#define	DISABLED_BLOCK1		PPBUF_BASE2 + 0x170 + 5
+#define	DISABLED_BLOCK2		PPBUF_BASE2 + 0x170 + 6
+#define	DISABLED_BLOCK3		PPBUF_BASE2 + 0x170 + 7
+#define	BLOCK_SIZE_0		PPBUF_BASE2 + 0x1a0 + 2
+#define	BLOCK_SIZE_1		PPBUF_BASE2 + 0x1a0 + 3
+#define	BLOCK_COUNT_0		PPBUF_BASE2 + 0x1a0 + 4
+#define	BLOCK_COUNT_1		PPBUF_BASE2 + 0x1a0 + 5
+#define	EBLOCK_COUNT_0		PPBUF_BASE2 + 0x1a0 + 6
+#define	EBLOCK_COUNT_1		PPBUF_BASE2 + 0x1a0 + 7
+#define	PAGE_SIZE_0		PPBUF_BASE2 + 0x1a0 + 8
+#define	PAGE_SIZE_1		PPBUF_BASE2 + 0x1a0 + 9
+
+#define MS_Device_Type		PPBUF_BASE2 + 0x1D8
+
+#define	MS_4bit_Support	PPBUF_BASE2 + 0x1D3
+
+#define setPS_NG	1
+#define setPS_Error	0
+
+#define	PARALLEL_8BIT_IF	0x40
+#define	PARALLEL_4BIT_IF	0x00
+#define	SERIAL_IF		0x80
+
+#define BUF_FULL	0x10
+#define BUF_EMPTY	0x20
+
+#define	MEDIA_BUSY	0x80
+#define	FLASH_BUSY	0x40
+#define	DATA_ERROR	0x20
+#define	STS_UCDT	0x10
+#define	EXTRA_ERROR	0x08
+#define	STS_UCEX	0x04
+#define	FLAG_ERROR	0x02
+#define	STS_UCFG	0x01
+
+#define MS_SHORT_DATA_LEN	32
+
+#define FORMAT_SUCCESS		0
+#define FORMAT_FAIL		1
+#define FORMAT_IN_PROGRESS	2
+
+#define	MS_SET_BAD_BLOCK_FLG(ms_card)	(ms_card)->multi_flag |= 0x80
+#define MS_CLR_BAD_BLOCK_FLG(ms_card)	(ms_card)->multi_flag &= 0x7F
+#define MS_TST_BAD_BLOCK_FLG(ms_card)	(ms_card)->multi_flag & 0x80
+
+void mspro_polling_format_status(struct rtsx_chip *chip);
+
+void mspro_stop_seq_mode(struct rtsx_chip *chip);
+int reset_ms_card(struct rtsx_chip *chip);
+int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
+	  u16 sector_cnt);
+int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+		 int short_data_len, int quick_format);
+void ms_free_l2p_tbl(struct rtsx_chip *chip);
+void ms_cleanup_work(struct rtsx_chip *chip);
+int ms_pull_ctl_disable(struct rtsx_chip *chip);
+int ms_pull_ctl_enable(struct rtsx_chip *chip);
+int ms_power_off_card3v3(struct rtsx_chip *chip);
+int release_ms_card(struct rtsx_chip *chip);
+#ifdef MS_DELAY_WRITE
+int ms_delay_write(struct rtsx_chip *chip);
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+#endif
+
+#endif
diff --git a/drivers/staging/rts5229/rtsx.c b/drivers/staging/rts5229/rtsx.c
new file mode 100644
index 0000000..98422b8
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx.c
@@ -0,0 +1,1094 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "general.h"
+
+#include "ms.h"
+#include "sd.h"
+
+#define DRIVER_VERSION 		"v1.07"
+
+MODULE_DESCRIPTION("Realtek PCI-Express card reader driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);
+
+static unsigned int delay_use = 1;
+module_param(delay_use, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(delay_use, "seconds to delay before using a new device");
+
+static int ss_en = 0;
+module_param(ss_en, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(ss_en, "enable selective suspend");
+
+static int ss_interval = 50;
+module_param(ss_interval, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(ss_interval, "Interval to enter ss state in seconds");
+
+static int auto_delink_en = 2;
+module_param(auto_delink_en, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(auto_delink_en, "enable auto delink");
+
+static unsigned char aspm_l0s_l1_en = 0;
+module_param(aspm_l0s_l1_en, byte, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(aspm_l0s_l1_en, "enable device aspm");
+
+static int msi_en = 0;
+module_param(msi_en, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(msi_en, "enable msi");
+
+static irqreturn_t rtsx_interrupt(int irq, void *dev_id);
+
+/***********************************************************************
+ * Host functions
+ ***********************************************************************/
+
+static const char *host_info(struct Scsi_Host *host)
+{
+	return "SCSI emulation for RTS5229";
+}
+
+static int slave_alloc(struct scsi_device *sdev)
+{
+	/*
+	 * Set the INQUIRY transfer length to 36.  We don't use any of
+	 * the extra data and many devices choke if asked for more or
+	 * less than 36 bytes.
+	 */
+	sdev->inquiry_len = 36;
+	return 0;
+}
+
+static int slave_configure(struct scsi_device *sdev)
+{
+	/* Scatter-gather buffers (all but the last) must have a length
+	 * divisible by the bulk maxpacket size.  Otherwise a data packet
+	 * would end up being short, causing a premature end to the data
+	 * transfer.  Since high-speed bulk pipes have a maxpacket size
+	 * of 512, we'll use that as the scsi device queue's DMA alignment
+	 * mask.  Guaranteeing proper alignment of the first buffer will
+	 * have the desired effect because, except at the beginning and
+	 * the end, scatter-gather buffers follow page boundaries. */
+	blk_queue_dma_alignment(sdev->request_queue, (512 - 1));
+
+	/* Set the SCSI level to at least 2.  We'll leave it at 3 if that's
+	 * what is originally reported.  We need this to avoid confusing
+	 * the SCSI layer with devices that report 0 or 1, but need 10-byte
+	 * commands (ala ATAPI devices behind certain bridges, or devices
+	 * which simply have broken INQUIRY data).
+	 *
+	 * NOTE: This means /dev/sg programs (ala cdrecord) will get the
+	 * actual information.  This seems to be the preference for
+	 * programs like that.
+	 *
+	 * NOTE: This also means that /proc/scsi/scsi and sysfs may report
+	 * the actual value or the modified one, depending on where the
+	 * data comes from.
+	 */
+	if (sdev->scsi_level < SCSI_2)
+		sdev->scsi_level = sdev->sdev_target->scsi_level = SCSI_2;
+
+	return 0;
+}
+
+/***********************************************************************
+ * /proc/scsi/ functions
+ ***********************************************************************/
+
+#undef SPRINTF
+#define SPRINTF(args...) \
+	do { if (pos < buffer+length) pos += sprintf(pos, ## args); } while (0)
+
+static int proc_info(struct Scsi_Host *host, char *buffer,
+		     char **start, off_t offset, int length, int inout)
+{
+	char *pos = buffer;
+
+	if (inout)
+		return length;
+
+	SPRINTF("   Host scsi%d: %s\n", host->host_no, CR_DRIVER_NAME);
+
+	SPRINTF("       Vendor: Realtek Corp.\n");
+	SPRINTF("      Product: RTS5229\n");
+	SPRINTF("      Version: %s\n", DRIVER_VERSION);
+	SPRINTF("        Build: %s, %s\n", __DATE__, __TIME__);
+
+	/*
+	 * Calculate start of next buffer, and return value.
+	 */
+	*start = buffer + offset;
+
+	if ((pos - buffer) < offset)
+		return (0);
+	else if ((pos - buffer - offset) < length)
+		return (pos - buffer - offset);
+	else
+		return (length);
+}
+
+static int queuecommand_lck(struct scsi_cmnd *srb,
+			    void (*done) (struct scsi_cmnd *))
+{
+	struct rtsx_dev *dev = host_to_rtsx(srb->device->host);
+	struct rtsx_chip *chip = dev->chip;
+
+	if (chip->srb != NULL) {
+		printk(KERN_ERR "Error in %s: chip->srb = %p\n",
+		       __FUNCTION__, chip->srb);
+		return SCSI_MLQUEUE_HOST_BUSY;
+	}
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+		printk(KERN_INFO "Fail command during disconnect\n");
+		srb->result = DID_NO_CONNECT << 16;
+		done(srb);
+		return 0;
+	}
+
+	srb->scsi_done = done;
+	chip->srb = srb;
+	complete(&dev->cmnd_ready);
+
+	return 0;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37)
+static int queuecommand(struct scsi_cmnd *srb,
+			void (*done) (struct scsi_cmnd *))
+{
+	return queuecommand_lck(srb, done);
+}
+#else
+static DEF_SCSI_QCMD(queuecommand)
+#endif
+/***********************************************************************
+ * Error handling functions
+ ***********************************************************************/
+ static int command_abort(struct scsi_cmnd *srb)
+{
+	struct Scsi_Host *host = srb->device->host;
+	struct rtsx_dev *dev = host_to_rtsx(host);
+	struct rtsx_chip *chip = dev->chip;
+
+	printk(KERN_INFO "%s called\n", __FUNCTION__);
+
+	scsi_lock(host);
+
+	if (chip->srb != srb) {
+		scsi_unlock(host);
+		printk(KERN_INFO "-- nothing to abort\n");
+		return FAILED;
+	}
+
+	rtsx_set_stat(chip, RTSX_STAT_ABORT);
+
+	scsi_unlock(host);
+
+	wait_for_completion(&dev->notify);
+
+	return SUCCESS;
+}
+
+/* This invokes the transport reset mechanism to reset the state of the
+ * device */
+static int device_reset(struct scsi_cmnd *srb)
+{
+	int result = 0;
+
+	printk(KERN_INFO "%s called\n", __FUNCTION__);
+
+	return result < 0 ? FAILED : SUCCESS;
+}
+
+static int bus_reset(struct scsi_cmnd *srb)
+{
+	int result = 0;
+
+	printk(KERN_INFO "%s called\n", __FUNCTION__);
+
+	return result < 0 ? FAILED : SUCCESS;
+}
+
+/*
+ * this defines our host template, with which we'll allocate hosts
+ */
+
+static struct scsi_host_template rtsx_host_template = {
+
+	.name = CR_DRIVER_NAME,
+	.proc_name = CR_DRIVER_NAME,
+	.proc_info = proc_info,
+	.info = host_info,
+
+	.queuecommand = queuecommand,
+
+	.eh_abort_handler = command_abort,
+	.eh_device_reset_handler = device_reset,
+	.eh_bus_reset_handler = bus_reset,
+
+	.can_queue = 1,
+	.cmd_per_lun = 1,
+
+	.this_id = -1,
+
+	.slave_alloc = slave_alloc,
+	.slave_configure = slave_configure,
+
+	.sg_tablesize = SG_ALL,
+
+	.max_sectors = 240,
+
+	/* merge commands... this seems to help performance, but
+	 * periodically someone should test to see which setting is more
+	 * optimal.
+	 */
+	.use_clustering = 1,
+
+	.emulated = 1,
+
+	.skip_settle_delay = 1,
+
+	.module = THIS_MODULE
+};
+
+static int rtsx_acquire_irq(struct rtsx_dev *dev)
+{
+	struct rtsx_chip *chip = dev->chip;
+
+	printk(KERN_INFO "%s: chip->msi_en = %d, pci->irq = %d\n",
+	       __FUNCTION__, chip->msi_en, dev->pci->irq);
+
+	if (request_irq(dev->pci->irq, rtsx_interrupt,
+			chip->msi_en ? 0 : IRQF_SHARED,
+			CR_DRIVER_NAME, dev)) {
+		printk(KERN_ERR "rtsx: unable to grab IRQ %d, "
+		       "disabling device\n", dev->pci->irq);
+		return -1;
+	}
+
+	dev->irq = dev->pci->irq;
+	pci_intx(dev->pci, !chip->msi_en);
+
+	return 0;
+}
+
+int rtsx_read_pci_cfg_byte(struct rtsx_chip *chip, u8 bus, u8 dev, u8 func,
+			   u8 offset, u8 * val)
+{
+	struct pci_dev *pdev;
+	u8 data;
+	u8 devfn = (dev << 3) | func;
+
+	pdev = pci_get_bus_and_slot(bus, devfn);
+	if (!dev) {
+		return -1;
+	}
+
+	pci_read_config_byte(pdev, offset, &data);
+	if (val) {
+		*val = data;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/*
+ * power management
+ */
+static int rtsx_suspend(struct pci_dev *pci, pm_message_t state)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_chip *chip;
+
+	printk(KERN_INFO "Ready to suspend\n");
+
+	if (!dev) {
+		printk(KERN_ERR "Invalid memory\n");
+		return 0;
+	}
+
+	mutex_lock(&(dev->dev_mutex));
+
+	chip = dev->chip;
+
+	rtsx_do_before_power_down(chip, PM_S3);
+
+	if (dev->irq >= 0) {
+		synchronize_irq(dev->irq);
+		free_irq(dev->irq, (void *)dev);
+		dev->irq = -1;
+	}
+
+	if (chip->msi_en) {
+		pci_disable_msi(pci);
+	}
+
+	pci_save_state(pci);
+	pci_enable_wake(pci, pci_choose_state(pci, state), 1);
+	pci_disable_device(pci);
+	pci_set_power_state(pci, pci_choose_state(pci, state));
+
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+}
+
+static int rtsx_resume(struct pci_dev *pci)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_chip *chip;
+
+	printk(KERN_INFO "Ready to resume\n");
+
+	if (!dev) {
+		printk(KERN_ERR "Invalid memory\n");
+		return 0;
+	}
+
+	chip = dev->chip;
+
+	mutex_lock(&(dev->dev_mutex));
+
+	pci_set_power_state(pci, PCI_D0);
+	pci_restore_state(pci);
+	if (pci_enable_device(pci) < 0) {
+		printk(KERN_ERR "%s: pci_enable_device failed, "
+		       "disabling device\n", CR_DRIVER_NAME);
+
+		mutex_unlock(&dev->dev_mutex);
+		return -EIO;
+	}
+	pci_set_master(pci);
+
+	if (chip->msi_en) {
+		if (pci_enable_msi(pci) < 0) {
+			chip->msi_en = 0;
+		}
+	}
+
+	if (rtsx_acquire_irq(dev) < 0) {
+
+		mutex_unlock(&dev->dev_mutex);
+		return -EIO;
+	}
+
+	rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 0x00);
+	rtsx_init_chip(chip);
+
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+}
+#endif
+
+static void rtsx_shutdown(struct pci_dev *pci)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_chip *chip;
+
+	printk(KERN_INFO "Ready to shutdown\n");
+
+	if (!dev) {
+		printk(KERN_ERR "Invalid memory\n");
+		return;
+	}
+
+	chip = dev->chip;
+
+	rtsx_do_before_power_down(chip, PM_S1);
+
+	if (dev->irq >= 0) {
+		synchronize_irq(dev->irq);
+		free_irq(dev->irq, (void *)dev);
+		dev->irq = -1;
+	}
+
+	if (chip->msi_en) {
+		pci_disable_msi(pci);
+	}
+
+	pci_disable_device(pci);
+
+	return;
+}
+
+static int rtsx_control_thread(void *__dev)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)__dev;
+	struct rtsx_chip *chip = dev->chip;
+	struct Scsi_Host *host = rtsx_to_host(dev);
+
+	current->flags |= PF_NOFREEZE;
+
+	for (;;) {
+		if (wait_for_completion_interruptible(&dev->cmnd_ready))
+			break;
+
+		mutex_lock(&(dev->dev_mutex));
+
+		if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+			printk(KERN_INFO "-- rts5229-control exiting\n");
+			mutex_unlock(&dev->dev_mutex);
+			break;
+		}
+
+		scsi_lock(host);
+
+		if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+			chip->srb->result = DID_ABORT << 16;
+			goto SkipForAbort;
+		}
+
+		scsi_unlock(host);
+
+		/* reject the command if the direction indicator
+		 * is UNKNOWN
+		 */
+		if (chip->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
+			printk(KERN_ERR "UNKNOWN data direction\n");
+			chip->srb->result = DID_ERROR << 16;
+		}
+
+		/* reject if target != 0 or if LUN is higher than
+		 * the maximum known LUN
+		 */
+		else if (chip->srb->device->id) {
+			printk(KERN_ERR "Bad target number (%d:%d)\n",
+			       chip->srb->device->id, chip->srb->device->lun);
+			chip->srb->result = DID_BAD_TARGET << 16;
+		}
+
+		else if (chip->srb->device->lun > chip->max_lun) {
+			printk(KERN_ERR "Bad LUN (%d:%d)\n",
+			       chip->srb->device->id, chip->srb->device->lun);
+			chip->srb->result = DID_BAD_TARGET << 16;
+		}
+
+		else {
+			RTSX_DEBUG(scsi_show_command(chip->srb));
+			rtsx_invoke_transport(chip->srb, chip);
+		}
+
+		scsi_lock(host);
+
+		if (!chip->srb) ;
+
+		else if (chip->srb->result != DID_ABORT << 16) {
+			chip->srb->scsi_done(chip->srb);
+		} else {
+ SkipForAbort:
+			printk(KERN_ERR "scsi command aborted\n");
+		}
+
+		if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+			complete(&(dev->notify));
+
+			rtsx_set_stat(chip, RTSX_STAT_IDLE);
+		}
+
+		chip->srb = NULL;
+		scsi_unlock(host);
+
+		mutex_unlock(&dev->dev_mutex);
+	}
+
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	complete_and_exit(&dev->control_exit, 0);
+}
+
+static int rtsx_polling_thread(void *__dev)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)__dev;
+	struct rtsx_chip *chip = dev->chip;
+	struct sd_info *sd_card = &(chip->sd_card);
+	struct ms_info *ms_card = &(chip->ms_card);
+
+	sd_card->cleanup_counter = 0;
+	ms_card->cleanup_counter = 0;
+
+	wait_timeout((delay_use + 5) * 1000);
+
+	for (;;) {
+		wait_timeout(POLLING_INTERVAL);
+
+		mutex_lock(&(dev->dev_mutex));
+
+		if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+			printk(KERN_INFO "-- rtsx-polling exiting\n");
+			mutex_unlock(&dev->dev_mutex);
+			break;
+		}
+
+		mutex_unlock(&dev->dev_mutex);
+
+		mspro_polling_format_status(chip);
+
+		mutex_lock(&(dev->dev_mutex));
+
+		rtsx_polling_func(chip);
+
+		mutex_unlock(&dev->dev_mutex);
+	}
+
+	complete_and_exit(&dev->polling_exit, 0);
+}
+
+/*
+ * interrupt handler
+ */
+static irqreturn_t rtsx_interrupt(int irq, void *dev_id)
+{
+	struct rtsx_dev *dev = dev_id;
+	struct rtsx_chip *chip;
+	int retval;
+	u32 status;
+
+	if (dev) {
+		chip = dev->chip;
+	} else {
+		return IRQ_NONE;
+	}
+
+	if (!chip) {
+		return IRQ_NONE;
+	}
+
+	spin_lock(&dev->reg_lock);
+
+	retval = rtsx_pre_handle_interrupt(chip);
+	if (retval == STATUS_FAIL) {
+		spin_unlock(&dev->reg_lock);
+		if (chip->int_reg == 0xFFFFFFFF) {
+			return IRQ_HANDLED;
+		} else {
+			return IRQ_NONE;
+		}
+	}
+
+	status = chip->int_reg;
+
+	if (dev->check_card_cd) {
+		if (!(dev->check_card_cd & status)) {
+			dev->trans_result = TRANS_RESULT_FAIL;
+			if (dev->done) {
+				complete(dev->done);
+			}
+			goto Exit;
+		}
+	}
+
+	if (status & (NEED_COMPLETE_INT | DELINK_INT)) {
+		if (status & (TRANS_FAIL_INT | DELINK_INT)) {
+			if (status & DELINK_INT) {
+				RTSX_SET_DELINK(chip);
+			}
+			dev->trans_result = TRANS_RESULT_FAIL;
+			if (dev->done) {
+				complete(dev->done);
+			}
+		} else if (status & TRANS_OK_INT) {
+			dev->trans_result = TRANS_RESULT_OK;
+			if (dev->done) {
+				complete(dev->done);
+			}
+		} else if (status & DATA_DONE_INT) {
+			dev->trans_result = TRANS_NOT_READY;
+			if (dev->done && (dev->trans_state == STATE_TRANS_SG)) {
+				complete(dev->done);
+			}
+		}
+	}
+
+ Exit:
+	spin_unlock(&dev->reg_lock);
+	return IRQ_HANDLED;
+}
+
+static void rtsx_release_resources(struct rtsx_dev *dev)
+{
+	printk(KERN_INFO "-- %s\n", __FUNCTION__);
+
+	/* Tell the control thread to exit.  The SCSI host must
+	 * already have been removed so it won't try to queue
+	 * any more commands.
+	 */
+	printk(KERN_INFO "-- sending exit command to thread\n");
+	complete(&dev->cmnd_ready);
+	if (dev->ctl_thread)
+		wait_for_completion(&dev->control_exit);
+	if (dev->polling_thread)
+		wait_for_completion(&dev->polling_exit);
+
+	wait_timeout(200);
+
+	if (dev->rtsx_resv_buf) {
+		dma_free_coherent(&(dev->pci->dev), RTSX_RESV_BUF_LEN,
+				  dev->rtsx_resv_buf,
+				  dev->rtsx_resv_buf_addr);
+		dev->chip->host_cmds_ptr = NULL;
+		dev->chip->host_sg_tbl_ptr = NULL;
+	}
+
+	if (dev->irq > 0)
+		free_irq(dev->irq, (void *)dev);
+	if (dev->chip->msi_en)
+		pci_disable_msi(dev->pci);
+	if (dev->remap_addr)
+		iounmap(dev->remap_addr);
+
+	pci_disable_device(dev->pci);
+	pci_release_regions(dev->pci);
+
+	rtsx_release_chip(dev->chip);
+	kfree(dev->chip);
+}
+
+/* First stage of disconnect processing: stop all commands and remove
+ * the host */
+static void quiesce_and_remove_host(struct rtsx_dev *dev)
+{
+	struct Scsi_Host *host = rtsx_to_host(dev);
+	struct rtsx_chip *chip = dev->chip;
+
+	/* Prevent new transfers, stop the current command, and
+	 * interrupt a SCSI-scan or device-reset delay */
+	mutex_lock(&dev->dev_mutex);
+	scsi_lock(host);
+	rtsx_set_stat(chip, RTSX_STAT_DISCONNECT);
+	scsi_unlock(host);
+	mutex_unlock(&dev->dev_mutex);
+	wake_up(&dev->delay_wait);
+	wait_for_completion(&dev->scanning_done);
+
+	wait_timeout(100);
+
+	/* queuecommand won't accept any new commands and the control
+	 * thread won't execute a previously-queued command.  If there
+	 * is such a command pending, complete it with an error. */
+	mutex_lock(&dev->dev_mutex);
+	if (chip->srb) {
+		chip->srb->result = DID_NO_CONNECT << 16;
+		scsi_lock(host);
+		chip->srb->scsi_done(dev->chip->srb);
+		chip->srb = NULL;
+		scsi_unlock(host);
+	}
+	mutex_unlock(&dev->dev_mutex);
+
+	scsi_remove_host(host);
+}
+
+static void release_everything(struct rtsx_dev *dev)
+{
+	rtsx_release_resources(dev);
+
+	/* Drop our reference to the host; the SCSI core will free it
+	 * when the refcount becomes 0. */
+	scsi_host_put(rtsx_to_host(dev));
+}
+
+static int rtsx_scan_thread(void *__dev)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)__dev;
+	struct rtsx_chip *chip = dev->chip;
+
+	if (delay_use > 0) {
+		printk(KERN_INFO "%s: waiting for device "
+		       "to settle before scanning\n", CR_DRIVER_NAME);
+		wait_event_interruptible_timeout(dev->delay_wait,
+						 rtsx_chk_stat(chip,
+							       RTSX_STAT_DISCONNECT),
+						 delay_use * HZ);
+	}
+
+	if (!rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+		scsi_scan_host(rtsx_to_host(dev));
+		printk(KERN_INFO "%s: device scan complete\n",
+		       CR_DRIVER_NAME);
+
+	}
+
+	complete_and_exit(&dev->scanning_done, 0);
+}
+
+static void rtsx_init_options(struct rtsx_chip *chip)
+{
+	chip->vendor_id = chip->rtsx->pci->vendor;
+	chip->product_id = chip->rtsx->pci->device;
+	chip->ssvid = chip->rtsx->pci->subsystem_vendor;
+	chip->ssdid = chip->rtsx->pci->subsystem_device;
+	chip->adma_mode = 1;
+	chip->lun_mc = 0;
+	chip->driver_first_load = 1;
+
+	chip->use_hw_setting = 1;
+	chip->mspro_formatter_enable = 1;
+	chip->lun_mode = DEFAULT_SINGLE;
+	chip->auto_delink_en = auto_delink_en;
+	chip->ss_en = ss_en;
+	chip->ss_idle_period = ss_interval * 1000;
+	chip->remote_wakeup_en = 0;
+	chip->aspm_l0s_l1_en = aspm_l0s_l1_en;
+	chip->dynamic_aspm = 1;
+	chip->config_host_aspm = 0;
+	chip->force_host_aspm = 0;
+	chip->host_aspm_para = 3;
+	chip->fpga_sd_sdr104_clk = CLK_200;
+	chip->fpga_sd_ddr50_clk = CLK_100;
+	chip->fpga_sd_sdr50_clk = CLK_100;
+	chip->fpga_sd_hs_clk = CLK_100;
+	chip->fpga_mmc_52m_clk = CLK_80;
+	chip->fpga_ms_hg_clk = CLK_80;
+	chip->fpga_ms_4bit_clk = CLK_80;
+	chip->fpga_ms_1bit_clk = CLK_40;
+	chip->asic_sd_sdr104_clk = 203;
+	chip->asic_sd_sdr50_clk = 98;
+	chip->asic_sd_ddr50_clk = 98;
+	chip->asic_sd_hs_clk = 98;
+	chip->asic_mmc_52m_clk = 98;
+	chip->asic_ms_hg_clk = 117;
+	chip->asic_ms_4bit_clk = 78;
+	chip->asic_ms_1bit_clk = 39;
+	chip->ssc_depth_sd_sdr104 = SSC_DEPTH_2M;
+	chip->ssc_depth_sd_sdr50 = SSC_DEPTH_2M;
+	chip->ssc_depth_sd_ddr50 = SSC_DEPTH_1M;
+	chip->ssc_depth_sd_hs = SSC_DEPTH_1M;
+	chip->ssc_depth_mmc_52m = SSC_DEPTH_1M;
+	chip->ssc_depth_ms_hg = SSC_DEPTH_1M;
+	chip->ssc_depth_ms_4bit = SSC_DEPTH_512K;
+	chip->ssc_depth_low_speed = SSC_DEPTH_512K;
+	chip->ssc_en = 1;
+	chip->sd_speed_prior = 0x01040203;
+	chip->sd_current_prior = 0x00010203;
+	chip->sd_ctl =
+	    SD_PUSH_POINT_AUTO | SD_SAMPLE_POINT_AUTO | SUPPORT_MMC_DDR_MODE;
+	chip->sd_ddr_tx_phase = 0;
+	chip->mmc_ddr_tx_phase = 1;
+	chip->sd_sdr104_default_tx_phase = 27;
+	chip->sd_sdr104_default_rx_phase = 24;
+	chip->sd_sdr50_default_tx_phase = 27;
+	chip->sd_sdr50_default_rx_phase = 6;
+	chip->sd_ddr50_default_tx_phase = 16;
+	chip->sd_ddr50_default_rx_phase = 5;
+	chip->pmos_pwr_on_interval = 200;
+	chip->sd_voltage_switch_delay = 1000;
+	chip->ms_power_class_en = 3;
+
+	chip->sd_400mA_ocp_thd = 1;
+	chip->sd_800mA_ocp_thd = 5;
+
+	chip->card_drive_sel = 0x55;
+	chip->sd30_drive_sel_1v8 = 0x03;
+	chip->sd30_drive_sel_3v3 = 0x01;
+	chip->sd30_clk_drive_sel_1v8 = 0xB3;
+	chip->sd30_cmd_drive_sel_1v8 = 0xB3;
+	chip->sd30_dat_drive_sel_1v8 = 0xB3;
+	chip->sd30_clk_drive_sel_3v3 = 0x96;
+	chip->sd30_cmd_drive_sel_3v3 = 0x96;
+	chip->sd30_dat_drive_sel_3v3 = 0x96;
+
+	chip->do_delink_before_power_down = 1;
+	chip->auto_power_down = 1;
+	chip->polling_config = 0;
+
+	chip->force_clkreq_0 = 1;
+	chip->ft2_fast_mode = 0;
+
+	chip->sd_timeout = 10000;
+	chip->ms_timeout = 2000;
+	chip->mspro_timeout = 15000;
+
+	chip->power_down_in_ss = 1;
+
+	chip->sdr104_en = 1;
+	chip->sdr50_en = 1;
+	chip->ddr50_en = 1;
+
+	chip->delink_stage1_step = 100;
+	chip->delink_stage2_step = 40;
+	chip->delink_stage3_step = 20;
+
+	chip->auto_delink_in_L1 = 1;
+	chip->blink_led = 1;
+	chip->msi_en = msi_en;
+	chip->hp_watch_bios_hotplug = 0;
+	chip->phy_voltage = 0xFF;
+
+	chip->support_ms_8bit = 1;
+	chip->s3_pwr_off_delay = 1000;
+
+	chip->pre_read_th = PRE_READ_30M;
+	chip->relink_time = 0x08FFFF;
+
+	chip->phy_pcr = 0xBA42;
+	chip->phy_rcr0 = 0x713F;
+	chip->phy_rcr2 = 0xC56A;
+	chip->phy_optimize = 1;
+
+	chip->ltr_en = 1;
+	chip->obff_en = 1;
+	chip->support_card = SUPPORT_SDMMC | SUPPORT_MS | SUPPORT_XD;
+
+	chip->sdr_tx_tuning_en = 1;
+
+	chip->cd_max_show_cnt = MAX_SHOW_CNT;
+
+	chip->dev_option = TURN_ON_LED_AT_START;
+	chip->sd_option = 0;
+	chip->ms_option = 0;
+}
+
+static int __devinit rtsx_probe(struct pci_dev *pci,
+				const struct pci_device_id *pci_id)
+{
+	struct Scsi_Host *host;
+	struct rtsx_dev *dev;
+	int err = 0;
+	struct task_struct *th;
+
+	printk(KERN_INFO "--- %s, %s ---\n", __DATE__, __TIME__);
+
+	err = pci_enable_device(pci);
+	if (err < 0) {
+		printk(KERN_ERR "PCI enable device failed!\n");
+		return err;
+	}
+
+	err = pci_request_regions(pci, CR_DRIVER_NAME);
+	if (err < 0) {
+		printk(KERN_ERR "PCI request regions for %s failed!\n",
+		       CR_DRIVER_NAME);
+		pci_disable_device(pci);
+		return err;
+	}
+
+	/*
+	 * Ask the SCSI layer to allocate a host structure, with extra
+	 * space at the end for our private rtsx_dev structure.
+	 */
+	host = scsi_host_alloc(&rtsx_host_template, sizeof(*dev));
+	if (!host) {
+		printk(KERN_ERR "Unable to allocate the scsi host\n");
+		pci_release_regions(pci);
+		pci_disable_device(pci);
+		return -ENOMEM;
+	}
+
+	dev = host_to_rtsx(host);
+	memset(dev, 0, sizeof(struct rtsx_dev));
+
+	dev->chip =
+	    (struct rtsx_chip *)kmalloc(sizeof(struct rtsx_chip), GFP_KERNEL);
+	if (dev->chip == NULL) {
+		goto errout;
+	}
+	memset(dev->chip, 0, sizeof(struct rtsx_chip));
+
+	spin_lock_init(&dev->reg_lock);
+	mutex_init(&(dev->dev_mutex));
+	init_completion(&dev->cmnd_ready);
+	init_completion(&dev->control_exit);
+	init_completion(&dev->polling_exit);
+	init_completion(&(dev->notify));
+	init_completion(&dev->scanning_done);
+	init_waitqueue_head(&dev->delay_wait);
+
+	dev->pci = pci;
+	dev->irq = -1;
+
+	printk(KERN_INFO "Resource length: 0x%x\n",
+	       (unsigned int)pci_resource_len(pci, 0));
+	dev->addr = pci_resource_start(pci, 0);
+	dev->remap_addr =
+	    ioremap_nocache(dev->addr, pci_resource_len(pci, 0));
+	if (dev->remap_addr == NULL) {
+		printk(KERN_ERR "ioremap error\n");
+		err = -ENXIO;
+		goto errout;
+	}
+
+	printk(KERN_INFO "Original address: 0x%lx, remapped address: 0x%lx\n",
+	       (unsigned long)(dev->addr), (unsigned long)(dev->remap_addr));
+
+	dev->rtsx_resv_buf =
+	    dma_alloc_coherent(&(pci->dev), RTSX_RESV_BUF_LEN,
+			       &(dev->rtsx_resv_buf_addr), GFP_KERNEL);
+	if (dev->rtsx_resv_buf == NULL) {
+		printk(KERN_ERR "alloc dma buffer fail\n");
+		err = -ENXIO;
+		goto errout;
+	}
+	dev->chip->host_cmds_ptr = dev->rtsx_resv_buf;
+	dev->chip->host_cmds_addr = dev->rtsx_resv_buf_addr;
+	dev->chip->host_sg_tbl_ptr = dev->rtsx_resv_buf + HOST_CMDS_BUF_LEN;
+	dev->chip->host_sg_tbl_addr =
+	    dev->rtsx_resv_buf_addr + HOST_CMDS_BUF_LEN;
+
+	dev->chip->rtsx = dev;
+
+	rtsx_init_options(dev->chip);
+
+	printk(KERN_INFO "pci->irq = %d\n", pci->irq);
+
+	if (dev->chip->msi_en) {
+		if (pci_enable_msi(pci) < 0) {
+			dev->chip->msi_en = 0;
+		}
+	}
+
+	if (rtsx_acquire_irq(dev) < 0) {
+		err = -EBUSY;
+		goto errout;
+	}
+
+	pci_set_master(pci);
+	synchronize_irq(dev->irq);
+
+	if (rtsx_init_chip(dev->chip) != STATUS_SUCCESS) {
+		err = -EIO;
+		printk(KERN_ERR "rtsx_init_chip fail\n");
+		goto errout;
+	}
+
+	err = scsi_add_host(host, &pci->dev);
+	if (err) {
+		printk(KERN_ERR "Unable to add the scsi host\n");
+		goto errout;
+	}
+
+	th = kthread_run(rtsx_control_thread, dev, CR_DRIVER_NAME);
+	if (IS_ERR(th)) {
+		printk(KERN_ERR "Unable to start control thread\n");
+		err = PTR_ERR(th);
+		goto errout;
+	}
+	dev->ctl_thread = th;
+
+	th = kthread_create(rtsx_scan_thread, dev, "rts5229-scan");
+	if (IS_ERR(th)) {
+		printk(KERN_ERR
+		       "Unable to start the device-scanning thread\n");
+		complete(&dev->scanning_done);
+		quiesce_and_remove_host(dev);
+		err = PTR_ERR(th);
+		goto errout;
+	}
+
+	wake_up_process(th);
+
+	th = kthread_run(rtsx_polling_thread, dev, "rts5229-polling");
+	if (IS_ERR(th)) {
+		printk(KERN_ERR
+		       "Unable to start the device-polling thread\n");
+		quiesce_and_remove_host(dev);
+		err = PTR_ERR(th);
+		goto errout;
+	}
+	dev->polling_thread = th;
+
+	pci_set_drvdata(pci, dev);
+
+	return 0;
+
+ errout:
+	printk(KERN_ERR "rtsx_probe() failed\n");
+	release_everything(dev);
+
+	return err;
+}
+
+static void __devexit rtsx_remove(struct pci_dev *pci)
+{
+	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+
+	printk(KERN_INFO "rtsx_remove() called\n");
+
+	quiesce_and_remove_host(dev);
+	release_everything(dev);
+
+	pci_set_drvdata(pci, NULL);
+}
+
+static struct pci_device_id rts5229_ids[] = {
+	{0x10EC, 0x5229, PCI_ANY_ID, PCI_ANY_ID, PCI_CLASS_OTHERS << 16,
+	 0xFF0000},
+	{0x10EC, 0x5227, PCI_ANY_ID, PCI_ANY_ID, PCI_CLASS_OTHERS << 16,
+	 0xFF0000},
+	{0,},
+};
+
+MODULE_DEVICE_TABLE(pci, rts5229_ids);
+
+static struct pci_driver driver = {
+	.name = CR_DRIVER_NAME,
+	.id_table = rts5229_ids,
+	.probe = rtsx_probe,
+	.remove = __devexit_p(rtsx_remove),
+#ifdef CONFIG_PM
+	.suspend = rtsx_suspend,
+	.resume = rtsx_resume,
+#endif
+	.shutdown = rtsx_shutdown,
+};
+
+static int __init rts5229_init(void)
+{
+	printk(KERN_INFO "Initializing Realtek PCIE storage driver...\n");
+
+	return pci_register_driver(&driver);
+}
+
+static void __exit rts5229_exit(void)
+{
+	printk(KERN_INFO "rtsx_exit() called\n");
+
+	pci_unregister_driver(&driver);
+
+	printk(KERN_INFO "%s module exit\n", CR_DRIVER_NAME);
+}
+
+module_init(rts5229_init)
+module_exit(rts5229_exit)
diff --git a/drivers/staging/rts5229/rtsx.h b/drivers/staging/rts5229/rtsx.h
new file mode 100644
index 0000000..9bb8171
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx.h
@@ -0,0 +1,230 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_H
+#define __REALTEK_RTSX_H
+
+#include <asm/io.h>
+#include <asm/bitops.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/pci.h>
+#include <linux/mutex.h>
+#include <linux/cdrom.h>
+#include <linux/workqueue.h>
+#include <linux/timer.h>
+#include <linux/time.h>
+
+#include <scsi/scsi.h>
+#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
+#include <scsi/scsi_devinfo.h>
+#include <scsi/scsi_eh.h>
+#include <scsi/scsi_host.h>
+
+#include "debug.h"
+#include "trace.h"
+#include "general.h"
+
+#define CR_DRIVER_NAME		"rts5229"
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 14)
+#ifdef CONFIG_PCI
+#undef pci_intx
+#define pci_intx(pci,x)
+#endif
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 24)
+#define sg_page(sg)	(sg)->page
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 25)
+#define scsi_set_resid(srb, residue)	((srb)->resid = (residue))
+#define scsi_get_resid(srb)		((srb)->resid)
+
+static inline unsigned scsi_bufflen(struct scsi_cmnd *cmd)
+{
+	return cmd->request_bufflen;
+}
+#endif
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 32)
+#define pci_get_bus_and_slot(bus, devfn)	\
+	pci_get_domain_bus_and_slot(0, (bus), (devfn))
+#endif
+
+/*
+ * macros for easy use
+ */
+#define rtsx_writel(chip, reg, value) \
+	iowrite32(value, (chip)->rtsx->remap_addr + reg)
+#define rtsx_readl(chip, reg) \
+	ioread32((chip)->rtsx->remap_addr + reg)
+#define rtsx_writew(chip, reg, value) \
+	iowrite16(value, (chip)->rtsx->remap_addr + reg)
+#define rtsx_readw(chip, reg) \
+	ioread16((chip)->rtsx->remap_addr + reg)
+#define rtsx_writeb(chip, reg, value) \
+	iowrite8(value, (chip)->rtsx->remap_addr + reg)
+#define rtsx_readb(chip, reg) \
+	ioread8((chip)->rtsx->remap_addr + reg)
+
+#define rtsx_read_config_byte(chip, where, val) \
+	pci_read_config_byte((chip)->rtsx->pci, where, val)
+#define rtsx_read_config_word(chip, where, val) \
+	pci_read_config_word((chip)->rtsx->pci, where, val)
+#define rtsx_read_config_dword(chip, where, val) \
+	pci_read_config_dword((chip)->rtsx->pci, where, val)
+
+#define rtsx_write_config_byte(chip, where, val) \
+	pci_write_config_byte((chip)->rtsx->pci, where, val)
+#define rtsx_write_config_word(chip, where, val) \
+	pci_write_config_word((chip)->rtsx->pci, where, val)
+#define rtsx_write_config_dword(chip, where, val) \
+	pci_write_config_dword((chip)->rtsx->pci, where, val)
+
+#define wait_timeout_x(task_state,msecs)	  set_current_state((task_state)); \
+						schedule_timeout((msecs) * HZ / 1000)
+#define wait_timeout(msecs)		wait_timeout_x(TASK_INTERRUPTIBLE, (msecs))
+
+#define STATE_TRANS_NONE	0
+#define STATE_TRANS_CMD		1
+#define STATE_TRANS_BUF		2
+#define STATE_TRANS_SG		3
+
+#define TRANS_NOT_READY		0
+#define TRANS_RESULT_OK		1
+#define TRANS_RESULT_FAIL	2
+
+#define SCSI_LUN(srb)		(srb)->device->lun
+
+#define rtsx_alloc_dma_buf(chip, size, flag)	kmalloc((size), (flag))
+#define rtsx_free_dma_buf(chip, ptr)		kfree((ptr))
+
+typedef unsigned long DELAY_PARA_T;
+
+struct rtsx_chip;
+
+struct rtsx_dev {
+	struct pci_dev *pci;
+
+	unsigned long addr;
+	void __iomem *remap_addr;
+	int irq;
+
+	spinlock_t reg_lock;
+
+	struct task_struct *ctl_thread;
+	struct task_struct *polling_thread;
+
+	struct completion cmnd_ready;
+	struct completion control_exit;
+	struct completion polling_exit;
+	struct completion notify;
+	struct completion scanning_done;
+
+	wait_queue_head_t delay_wait;
+	struct mutex dev_mutex;
+
+	void *rtsx_resv_buf;
+	dma_addr_t rtsx_resv_buf_addr;
+
+	char trans_result;
+	char trans_state;
+
+	struct completion *done;
+
+	u32 check_card_cd;
+
+	struct rtsx_chip *chip;
+};
+
+typedef struct rtsx_dev rtsx_dev_t;
+
+static inline struct Scsi_Host *rtsx_to_host(struct rtsx_dev *dev)
+{
+	return container_of((void *)dev, struct Scsi_Host, hostdata);
+}
+
+static inline struct rtsx_dev *host_to_rtsx(struct Scsi_Host *host)
+{
+	return (struct rtsx_dev *)host->hostdata;
+}
+
+static inline void get_current_time(u8 * timeval_buf, int buf_len)
+{
+	struct timeval tv;
+
+	if (!timeval_buf || (buf_len < 8)) {
+		return;
+	}
+
+	do_gettimeofday(&tv);
+
+	timeval_buf[0] = (u8) (tv.tv_sec >> 24);
+	timeval_buf[1] = (u8) (tv.tv_sec >> 16);
+	timeval_buf[2] = (u8) (tv.tv_sec >> 8);
+	timeval_buf[3] = (u8) (tv.tv_sec);
+	timeval_buf[4] = (u8) (tv.tv_usec >> 24);
+	timeval_buf[5] = (u8) (tv.tv_usec >> 16);
+	timeval_buf[6] = (u8) (tv.tv_usec >> 8);
+	timeval_buf[7] = (u8) (tv.tv_usec);
+}
+
+/* The scsi_lock() and scsi_unlock() macros protect the sm_state and the
+ * single queue element srb for write access */
+#define scsi_unlock(host)	spin_unlock_irq(host->host_lock)
+#define scsi_lock(host)		spin_lock_irq(host->host_lock)
+
+#define lock_state(chip)	spin_lock_irq(&((chip)->rtsx->reg_lock))
+#define unlock_state(chip)	spin_unlock_irq(&((chip)->rtsx->reg_lock))
+
+enum xfer_buf_dir { TO_XFER_BUF, FROM_XFER_BUF };
+
+static inline void EnableHostASPM(struct rtsx_chip *chip)
+{
+}
+
+static inline void DisableHostASPM(struct rtsx_chip *chip)
+{
+}
+
+static inline void SetHostASPM(struct rtsx_chip *chip, u8 val)
+{
+}
+
+static inline void GetHostASPM(struct rtsx_chip *chip, u8 * val)
+{
+}
+
+int rtsx_read_pci_cfg_byte(struct rtsx_chip *chip, u8 bus, u8 dev, u8 func,
+			   u8 offset, u8 * val);
+
+#endif
diff --git a/drivers/staging/rts5229/rtsx_card.c b/drivers/staging/rts5229/rtsx_card.c
new file mode 100644
index 0000000..f8c547b
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_card.c
@@ -0,0 +1,993 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+#include <linux/kernel.h>
+
+#include "rtsx.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+
+#include "rtsx_sys.h"
+#include "general.h"
+
+#include "sd.h"
+#include "ms.h"
+
+void do_remaining_work(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	struct ms_info *ms_card = &(chip->ms_card);
+
+	if (chip->card_ready & SD_CARD) {
+		if (sd_card->seq_mode) {
+			rtsx_set_run_stat(chip, 1);
+			sd_card->cleanup_counter++;
+		} else {
+			sd_card->cleanup_counter = 0;
+		}
+	}
+
+	if (chip->card_ready & MS_CARD) {
+		if (CHK_MSPRO(ms_card)) {
+			if (ms_card->seq_mode) {
+				rtsx_set_run_stat(chip, 1);
+				ms_card->cleanup_counter++;
+			} else {
+				ms_card->cleanup_counter = 0;
+			}
+		} else {
+#ifdef MS_DELAY_WRITE
+			if (ms_card->delay_write.delay_write_flag) {
+				rtsx_set_run_stat(chip, 1);
+				ms_card->cleanup_counter++;
+			} else {
+				ms_card->cleanup_counter = 0;
+			}
+#endif
+		}
+	}
+
+	if (sd_card->cleanup_counter > POLLING_WAIT_CNT) {
+		sd_cleanup_work(chip);
+	}
+
+	if (ms_card->cleanup_counter > POLLING_WAIT_CNT) {
+		ms_cleanup_work(chip);
+	}
+}
+
+void do_reset_sd_card(struct rtsx_chip *chip)
+{
+	int retval;
+
+	RTSX_DEBUGP(("%s: %d, card2lun = 0x%x\n", __FUNCTION__,
+		     chip->sd_reset_counter, chip->card2lun[SD_CARD]));
+
+	if (chip->card2lun[SD_CARD] >= MAX_ALLOWED_LUN_CNT) {
+		clear_bit(SD_NR, &(chip->need_reset));
+		chip->sd_reset_counter = 0;
+		chip->sd_show_cnt = 0;
+		return;
+	}
+
+	chip->rw_fail_cnt[chip->card2lun[SD_CARD]] = 0;
+
+	rtsx_set_run_stat(chip, 1);
+
+	retval = reset_sd_card(chip);
+	if (chip->need_release & SD_CARD) {
+		return;
+	}
+	if (retval == STATUS_SUCCESS) {
+		clear_bit(SD_NR, &(chip->need_reset));
+		chip->sd_reset_counter = 0;
+		chip->sd_show_cnt = 0;
+		chip->card_ready |= SD_CARD;
+		chip->card_fail &= ~SD_CARD;
+		chip->rw_card[chip->card2lun[SD_CARD]] = sd_rw;
+	} else {
+		if (chip->sd_reset_counter >= MAX_RESET_CNT) {
+			clear_bit(SD_NR, &(chip->need_reset));
+			chip->sd_reset_counter = 0;
+			chip->sd_show_cnt = 0;
+		} else {
+			chip->sd_reset_counter++;
+		}
+		chip->card_ready &= ~SD_CARD;
+		chip->card_fail |= SD_CARD;
+		chip->capacity[chip->card2lun[SD_CARD]] = 0;
+		chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
+
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		if (!chip->ft2_fast_mode) {
+			card_power_off(chip, SD_CARD);
+		}
+		disable_card_clock(chip, SD_CARD);
+	}
+}
+
+void do_reset_ms_card(struct rtsx_chip *chip)
+{
+	int retval;
+
+	RTSX_DEBUGP(("%s: %d, card2lun = 0x%x\n", __FUNCTION__,
+		     chip->ms_reset_counter, chip->card2lun[MS_CARD]));
+
+	if (chip->card2lun[MS_CARD] >= MAX_ALLOWED_LUN_CNT) {
+		clear_bit(MS_NR, &(chip->need_reset));
+		chip->ms_reset_counter = 0;
+		chip->ms_show_cnt = 0;
+		return;
+	}
+
+	chip->rw_fail_cnt[chip->card2lun[MS_CARD]] = 0;
+
+	rtsx_set_run_stat(chip, 1);
+
+	retval = reset_ms_card(chip);
+	if (chip->need_release & MS_CARD) {
+		return;
+	}
+	if (retval == STATUS_SUCCESS) {
+		clear_bit(MS_NR, &(chip->need_reset));
+		chip->ms_reset_counter = 0;
+		chip->card_ready |= MS_CARD;
+		chip->card_fail &= ~MS_CARD;
+		chip->rw_card[chip->card2lun[MS_CARD]] = ms_rw;
+	} else {
+		if (chip->ms_reset_counter >= MAX_RESET_CNT) {
+			clear_bit(MS_NR, &(chip->need_reset));
+			chip->ms_reset_counter = 0;
+			chip->ms_show_cnt = 0;
+		} else {
+			chip->ms_reset_counter++;
+		}
+		chip->card_ready &= ~MS_CARD;
+		chip->card_fail |= MS_CARD;
+		chip->capacity[chip->card2lun[MS_CARD]] = 0;
+		chip->rw_card[chip->card2lun[MS_CARD]] = NULL;
+
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		if (!chip->ft2_fast_mode) {
+			card_power_off(chip, MS_CARD);
+		}
+		disable_card_clock(chip, MS_CARD);
+	}
+}
+
+void rtsx_power_off_card(struct rtsx_chip *chip)
+{
+	if (chip->card_ready & SD_CARD) {
+		sd_cleanup_work(chip);
+		sd_power_off_card3v3(chip);
+	}
+
+	if (chip->card_ready & MS_CARD) {
+		ms_cleanup_work(chip);
+		ms_power_off_card3v3(chip);
+	}
+}
+
+void rtsx_release_cards(struct rtsx_chip *chip)
+{
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if (chip->card_ready & SD_CARD) {
+		if (chip->int_reg & SD_EXIST) {
+			sd_cleanup_work(chip);
+		}
+		release_sd_card(chip);
+	}
+
+	if (chip->card_ready & MS_CARD) {
+		if (chip->int_reg & MS_EXIST) {
+			ms_cleanup_work(chip);
+		}
+		release_ms_card(chip);
+	}
+}
+
+void rtsx_reset_cards(struct rtsx_chip *chip)
+{
+	if (!chip->need_reset) {
+		return;
+	}
+
+	rtsx_set_run_stat(chip, 1);
+
+	rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->need_reset & SD_CARD) {
+		chip->card_exist |= SD_CARD;
+
+		if (chip->sd_show_cnt >= chip->cd_max_show_cnt) {
+			rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+			do_reset_sd_card(chip);
+		} else {
+			chip->sd_show_cnt++;
+		}
+	}
+	if (chip->need_reset & MS_CARD) {
+		chip->card_exist |= MS_CARD;
+
+		if (chip->ms_show_cnt >= chip->cd_max_show_cnt) {
+			do_reset_ms_card(chip);
+		} else {
+			chip->ms_show_cnt++;
+		}
+	}
+}
+
+void rtsx_reinit_cards(struct rtsx_chip *chip, int reset_chip)
+{
+	rtsx_set_run_stat(chip, 1);
+
+	rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+
+	if (reset_chip)
+		rtsx_reset_chip(chip);
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if ((chip->int_reg & SD_EXIST) && (chip->need_reinit & SD_CARD)) {
+		release_sd_card(chip);
+
+		wait_timeout(100);
+
+		chip->card_exist |= SD_CARD;
+		do_reset_sd_card(chip);
+	}
+
+	if ((chip->int_reg & MS_EXIST) && (chip->need_reinit & MS_CARD)) {
+		release_ms_card(chip);
+
+		wait_timeout(100);
+
+		chip->card_exist |= MS_CARD;
+		do_reset_ms_card(chip);
+	}
+
+	chip->need_reinit = 0;
+}
+
+#ifdef DISABLE_CARD_INT
+void card_cd_debounce(struct rtsx_chip *chip, unsigned long *need_reset,
+		      unsigned long *need_release)
+{
+	u8 release_map = 0, reset_map = 0;
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if (chip->card_exist) {
+		if (chip->card_exist & SD_CARD) {
+			if (!(chip->int_reg & SD_EXIST)) {
+				release_map |= SD_CARD;
+			}
+		} else if (chip->card_exist & MS_CARD) {
+			if (!(chip->int_reg & MS_EXIST)) {
+				release_map |= MS_CARD;
+			}
+		}
+	} else {
+		if (chip->int_reg & SD_EXIST) {
+			reset_map |= SD_CARD;
+		} else if (chip->int_reg & MS_EXIST) {
+			reset_map |= MS_CARD;
+		}
+	}
+
+	if (reset_map) {
+		int sd_cnt = 0, ms_cnt = 0;
+		int i;
+
+		for (i = 0; i < (DEBOUNCE_CNT); i++) {
+			chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+			if (chip->int_reg & SD_EXIST) {
+				sd_cnt++;
+			} else {
+				sd_cnt = 0;
+			}
+			if (chip->int_reg & MS_EXIST) {
+				ms_cnt++;
+			} else {
+				ms_cnt = 0;
+			}
+			wait_timeout(30);
+		}
+
+		reset_map = 0;
+		if (!(chip->card_exist & SD_CARD)
+		    && (sd_cnt > (DEBOUNCE_CNT - 1))) {
+			reset_map |= SD_CARD;
+		}
+		if (!(chip->card_exist & MS_CARD)
+		    && (ms_cnt > (DEBOUNCE_CNT - 1))) {
+			reset_map |= MS_CARD;
+		}
+	}
+
+	if (need_reset) {
+		*need_reset = reset_map;
+	}
+	if (need_release) {
+		*need_release = release_map;
+	}
+}
+#endif
+
+void rtsx_init_cards(struct rtsx_chip *chip)
+{
+	if (RTSX_TST_DELINK(chip) && (rtsx_get_stat(chip) != RTSX_STAT_SS)) {
+		RTSX_DEBUGP(("Reset chip in polling thread!\n"));
+		rtsx_init_chip(chip);
+		RTSX_CLR_DELINK(chip);
+	}
+#ifdef DISABLE_CARD_INT
+	card_cd_debounce(chip, &(chip->need_reset), &(chip->need_release));
+#endif
+
+	if (chip->need_release) {
+		if (!(chip->card_exist & SD_CARD)) {
+			clear_bit(SD_NR, &(chip->need_release));
+		}
+		if (!(chip->card_exist & MS_CARD)) {
+			clear_bit(MS_NR, &(chip->need_release));
+		}
+
+		RTSX_DEBUGP(("chip->need_release = 0x%x\n",
+			     (unsigned int)(chip->need_release)));
+
+#ifdef SUPPORT_OCP
+		if (chip->need_release) {
+			if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER))
+				rtsx_clear_ocp(chip);
+
+			chip->ocp_stat = 0;
+		}
+#endif
+		if (chip->need_release) {
+			rtsx_set_run_stat(chip, 0);
+
+			rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+		}
+
+		if (chip->need_release & SD_CARD) {
+			clear_bit(SD_NR, &(chip->need_release));
+			chip->card_exist &= ~SD_CARD;
+			chip->card_ejected &= ~SD_CARD;
+			chip->card_fail &= ~SD_CARD;
+			CLR_BIT(chip->lun_mc, chip->card2lun[SD_CARD]);
+			chip->rw_fail_cnt[chip->card2lun[SD_CARD]] = 0;
+			rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+
+			release_sd_card(chip);
+		}
+
+		if (chip->need_release & MS_CARD) {
+			clear_bit(MS_NR, &(chip->need_release));
+			chip->card_exist &= ~MS_CARD;
+			chip->card_ejected &= ~MS_CARD;
+			chip->card_fail &= ~MS_CARD;
+			CLR_BIT(chip->lun_mc, chip->card2lun[MS_CARD]);
+			chip->rw_fail_cnt[chip->card2lun[MS_CARD]] = 0;
+
+			release_ms_card(chip);
+		}
+
+		RTSX_DEBUGP(("chip->card_exist = 0x%x\n", chip->card_exist));
+
+		if (!chip->card_exist && chip->blink_led) {
+			turn_off_led(chip);
+		}
+	}
+
+	if (chip->need_reset) {
+		RTSX_DEBUGP(("chip->need_reset = 0x%x\n",
+			     (unsigned int)(chip->need_reset)));
+
+		rtsx_reset_cards(chip);
+	}
+
+	if (chip->need_reinit) {
+		RTSX_DEBUGP(("chip->need_reinit = 0x%x\n",
+			     (unsigned int)(chip->need_reinit)));
+
+		rtsx_reinit_cards(chip, 0);
+	}
+}
+
+static inline u8 double_depth(u8 depth)
+{
+	return ((depth > 1) ? (depth - 1) : depth);
+}
+
+int switch_ssc_clock(struct rtsx_chip *chip, int clk)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	u8 N = (u8) (clk - 2), min_N, max_N;
+	u8 mcu_cnt, div, max_div, ssc_depth;
+	int sd_vpclk_phase_reset = 0;
+
+	if (chip->cur_clk == clk) {
+		return STATUS_SUCCESS;
+	}
+
+	min_N = 80;
+	max_N = 208;
+	max_div = CLK_DIV_8;
+
+	if (chip->cur_card == SD_CARD) {
+		struct sd_info *sd_card = &(chip->sd_card);
+		if (CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card)) {
+			sd_vpclk_phase_reset = 1;
+		}
+	}
+
+	RTSX_DEBUGP(("Switch SSC clock to %dMHz (cur_clk = %d)\n", clk,
+		     chip->cur_clk));
+
+	if ((clk <= 2) || (N > max_N)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	mcu_cnt = (u8) (125 / clk + 3);
+	if (mcu_cnt > 15) {
+		mcu_cnt = 15;
+	}
+
+	div = CLK_DIV_1;
+	while ((N < min_N) && (div < max_div)) {
+		N = (N + 2) * 2 - 2;
+		div++;
+	}
+	RTSX_DEBUGP(("N = %d, div = %d\n", N, div));
+
+	if (chip->ssc_en) {
+		if (chip->cur_card == SD_CARD) {
+			if (CHK_SD_SDR104(sd_card)) {
+				ssc_depth = chip->ssc_depth_sd_sdr104;
+			} else if (CHK_SD_SDR50(sd_card)) {
+				ssc_depth = chip->ssc_depth_sd_sdr50;
+			} else if (CHK_SD_DDR50(sd_card)) {
+				ssc_depth =
+				    double_depth(chip->ssc_depth_sd_ddr50);
+			} else if (CHK_SD_HS(sd_card)) {
+				ssc_depth =
+				    double_depth(chip->ssc_depth_sd_hs);
+			} else if (CHK_MMC_52M(sd_card)
+				   || CHK_MMC_DDR52(sd_card)) {
+				ssc_depth =
+				    double_depth(chip->ssc_depth_mmc_52m);
+			} else {
+				ssc_depth =
+				    double_depth(chip->ssc_depth_low_speed);
+			}
+		} else if (chip->cur_card == MS_CARD) {
+			if (CHK_MSPRO(ms_card)) {
+				if (CHK_HG8BIT(ms_card)) {
+					ssc_depth =
+					    double_depth(chip->
+							 ssc_depth_ms_hg);
+				} else {
+					ssc_depth =
+					    double_depth(chip->
+							 ssc_depth_ms_4bit);
+				}
+			} else {
+				if (CHK_MS4BIT(ms_card)) {
+					ssc_depth =
+					    double_depth(chip->
+							 ssc_depth_ms_4bit);
+				} else {
+					ssc_depth =
+					    double_depth(chip->
+							 ssc_depth_low_speed);
+				}
+			}
+		} else {
+			ssc_depth = double_depth(chip->ssc_depth_low_speed);
+		}
+
+		if (ssc_depth) {
+			if (div == CLK_DIV_2) {
+				if (ssc_depth > 1) {
+					ssc_depth -= 1;
+				} else {
+					ssc_depth = SSC_DEPTH_4M;
+				}
+			} else if (div == CLK_DIV_4) {
+				if (ssc_depth > 2) {
+					ssc_depth -= 2;
+				} else {
+					ssc_depth = SSC_DEPTH_4M;
+				}
+			} else if (div == CLK_DIV_8) {
+				if (ssc_depth > 3) {
+					ssc_depth -= 3;
+				} else {
+					ssc_depth = SSC_DEPTH_4M;
+				}
+			}
+		}
+	} else {
+		ssc_depth = 0;
+	}
+
+	RTSX_DEBUGP(("ssc_depth = %d\n", ssc_depth));
+
+	rtsx_init_cmd(chip);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CLK_CTL, CLK_LOW_FREQ,
+		     CLK_LOW_FREQ);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0xFF,
+		     (div << 4) | mcu_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_CTL2, SSC_DEPTH_MASK,
+		     ssc_depth);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_DIV_N_0, 0xFF, N);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, SSC_RSTB);
+	if (sd_vpclk_phase_reset) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
+			     PHASE_NOT_RESET, 0);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
+			     PHASE_NOT_RESET, PHASE_NOT_RESET);
+	}
+
+	retval = rtsx_send_cmd(chip, 0, WAIT_TIME);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	udelay(10);
+	RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+
+	chip->cur_clk = clk;
+
+	return STATUS_SUCCESS;
+}
+
+int switch_normal_clock(struct rtsx_chip *chip, int clk)
+{
+	u8 sel, div, mcu_cnt;
+
+	if (chip->cur_clk == clk) {
+		return STATUS_SUCCESS;
+	}
+
+	switch (clk) {
+	case CLK_20:
+		RTSX_DEBUGP(("Switch clock to 20MHz\n"));
+		sel = SSC_80;
+		div = CLK_DIV_4;
+		mcu_cnt = 7;
+		break;
+
+	case CLK_30:
+		RTSX_DEBUGP(("Switch clock to 30MHz\n"));
+		sel = SSC_120;
+		div = CLK_DIV_4;
+		mcu_cnt = 7;
+		break;
+
+	case CLK_40:
+		RTSX_DEBUGP(("Switch clock to 40MHz\n"));
+		sel = SSC_80;
+		div = CLK_DIV_2;
+		mcu_cnt = 7;
+		break;
+
+	case CLK_50:
+		RTSX_DEBUGP(("Switch clock to 50MHz\n"));
+		sel = SSC_100;
+		div = CLK_DIV_2;
+		mcu_cnt = 6;
+		break;
+
+	case CLK_60:
+		RTSX_DEBUGP(("Switch clock to 60MHz\n"));
+		sel = SSC_120;
+		div = CLK_DIV_2;
+		mcu_cnt = 6;
+		break;
+
+	case CLK_80:
+		RTSX_DEBUGP(("Switch clock to 80MHz\n"));
+		sel = SSC_80;
+		div = CLK_DIV_1;
+		mcu_cnt = 5;
+		break;
+
+	case CLK_100:
+		RTSX_DEBUGP(("Switch clock to 100MHz\n"));
+		sel = SSC_100;
+		div = CLK_DIV_1;
+		mcu_cnt = 5;
+		break;
+
+	case CLK_120:
+		RTSX_DEBUGP(("Switch clock to 120MHz\n"));
+		sel = SSC_120;
+		div = CLK_DIV_1;
+		mcu_cnt = 5;
+		break;
+
+	case CLK_150:
+		RTSX_DEBUGP(("Switch clock to 150MHz\n"));
+		sel = SSC_150;
+		div = CLK_DIV_1;
+		mcu_cnt = 4;
+		break;
+
+	case CLK_200:
+		RTSX_DEBUGP(("Switch clock to 200MHz\n"));
+		sel = SSC_200;
+		div = CLK_DIV_1;
+		mcu_cnt = 4;
+		break;
+
+	default:
+		RTSX_DEBUGP(("Try to switch to an illegal clock (%d)\n",
+			     clk));
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, CLK_CTL, 0xFF, CLK_LOW_FREQ);
+	RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
+	RTSX_WRITE_REG(chip, SD_VPCLK1_CTL, PHASE_NOT_RESET, 0);
+	RTSX_WRITE_REG(chip, CLK_DIV, 0xFF, (div << 4) | mcu_cnt);
+	RTSX_WRITE_REG(chip, CLK_SEL, 0xFF, sel);
+
+	udelay(200);
+	RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, PHASE_NOT_RESET);
+	RTSX_WRITE_REG(chip, SD_VPCLK1_CTL, PHASE_NOT_RESET, PHASE_NOT_RESET);
+	udelay(200);
+	RTSX_WRITE_REG(chip, CLK_CTL, 0xFF, 0);
+
+	chip->cur_clk = clk;
+
+	return STATUS_SUCCESS;
+}
+
+void trans_dma_enable(enum dma_data_direction dir, struct rtsx_chip *chip,
+		      u32 byte_cnt, u8 pack_size)
+{
+	if (pack_size > DMA_1024) {
+		pack_size = DMA_512;
+	}
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, IRQSTAT0, DMA_DONE_INT,
+		     DMA_DONE_INT);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC3, 0xFF,
+		     (u8) (byte_cnt >> 24));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC2, 0xFF,
+		     (u8) (byte_cnt >> 16));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC1, 0xFF, (u8) (byte_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC0, 0xFF, (u8) byte_cnt);
+
+	if (dir == DMA_FROM_DEVICE) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, DMACTL,
+			     0x03 | DMA_PACK_SIZE_MASK,
+			     DMA_DIR_FROM_CARD | DMA_EN | pack_size);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, DMACTL,
+			     0x03 | DMA_PACK_SIZE_MASK,
+			     DMA_DIR_TO_CARD | DMA_EN | pack_size);
+	}
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     RING_BUFFER);
+}
+
+int enable_card_clock(struct rtsx_chip *chip, u8 card)
+{
+	u8 clk_en = 0;
+
+	if (card & SD_CARD) {
+		clk_en |= SD_CLK_EN;
+	}
+	if (card & MS_CARD) {
+		clk_en |= MS_CLK_EN;
+	}
+
+	RTSX_WRITE_REG(chip, CARD_CLK_EN, clk_en, clk_en);
+
+	return STATUS_SUCCESS;
+}
+
+int disable_card_clock(struct rtsx_chip *chip, u8 card)
+{
+	u8 clk_en = 0;
+
+	if (card & SD_CARD) {
+		clk_en |= SD_CLK_EN;
+	}
+	if (card & MS_CARD) {
+		clk_en |= MS_CLK_EN;
+	}
+
+	RTSX_WRITE_REG(chip, CARD_CLK_EN, clk_en, 0);
+
+	return STATUS_SUCCESS;
+}
+
+int card_power_on(struct rtsx_chip *chip, u8 card)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, SD_POWER_MASK,
+		     SD_PARTIAL_POWER_ON);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, PWR_GATE_CTRL, LDO3318_PWR_MASK,
+		     LDO_SUSPEND);
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	udelay(chip->pmos_pwr_on_interval);
+
+	rtsx_init_cmd(chip);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, SD_POWER_MASK,
+		     SD_POWER_ON);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, PWR_GATE_CTRL, LDO3318_PWR_MASK,
+		     LDO_ON);
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int card_power_off(struct rtsx_chip *chip, u8 card)
+{
+	RTSX_WRITE_REG(chip, CARD_PWR_CTL, SD_POWER_MASK | PMOS_STRG_MASK,
+		       SD_POWER_OFF | PMOS_STRG_400mA);
+	RTSX_WRITE_REG(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_OFF);
+
+	return STATUS_SUCCESS;
+}
+
+int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 sec_addr,
+	    u16 sec_cnt)
+{
+	int retval;
+	unsigned int lun = SCSI_LUN(srb);
+	int i;
+
+	if (chip->rw_card[lun] == NULL) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	for (i = 0; i < 4; i++) {
+		chip->rw_need_retry = 0;
+		chip->rw_retry_cnt = i;
+
+		retval = chip->rw_card[lun] (srb, chip, sec_addr, sec_cnt);
+		if (retval != STATUS_SUCCESS) {
+			if (rtsx_check_chip_exist(chip) != STATUS_SUCCESS) {
+				rtsx_release_chip(chip);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (detect_card_cd(chip, chip->cur_card) !=
+			    STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (!chip->rw_need_retry) {
+				RTSX_DEBUGP(("RW fail, but no need to retry\n"));
+				break;
+			}
+		} else {
+			chip->rw_need_retry = 0;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+int card_share_mode(struct rtsx_chip *chip, int card)
+{
+	u8 value;
+
+	if (card == SD_CARD) {
+		value = CARD_SHARE_SD;
+	} else if (card == MS_CARD) {
+		value = CARD_SHARE_MS;
+	} else {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, CARD_SHARE_MODE, CARD_SHARE_MASK, value);
+
+	return STATUS_SUCCESS;
+}
+
+int select_card(struct rtsx_chip *chip, int card)
+{
+	int retval;
+
+	if (chip->cur_card != card) {
+		u8 mod;
+
+		if (card == SD_CARD) {
+			mod = SD_MOD_SEL;
+		} else if (card == MS_CARD) {
+			mod = MS_MOD_SEL;
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		RTSX_WRITE_REG(chip, CARD_SELECT, 0x07, mod);
+		chip->cur_card = card;
+
+		retval = card_share_mode(chip, card);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void toggle_gpio(struct rtsx_chip *chip)
+{
+	u8 temp_reg;
+
+	rtsx_read_register(chip, GPIO_CTL, &temp_reg);
+	temp_reg ^= 0x01;
+	rtsx_write_register(chip, GPIO_CTL, 0x01, temp_reg);
+}
+
+void toggle_led(struct rtsx_chip *chip)
+{
+	u8 temp_reg;
+
+	rtsx_read_register(chip, GPIO_CTL, &temp_reg);
+	temp_reg ^= 0x02;
+	rtsx_write_register(chip, GPIO_CTL, 0x02, temp_reg);
+}
+
+void turn_on_led(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, GPIO_CTL, 0x02, 0x02);
+}
+
+void turn_off_led(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, GPIO_CTL, 0x02, 0x00);
+}
+
+#ifdef LED_AUTO_BLINK
+void enable_auto_blink(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, OLT_LED_CTL, LED_SHINE_EN, LED_SHINE_EN);
+}
+
+void disable_auto_blink(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, OLT_LED_CTL, LED_SHINE_EN, 0x00);
+}
+#endif
+
+int detect_card_cd(struct rtsx_chip *chip, int card)
+{
+	u32 card_cd, status;
+
+	if (card == SD_CARD) {
+		card_cd = SD_EXIST;
+	} else if (card == MS_CARD) {
+		card_cd = MS_EXIST;
+	} else {
+		RTSX_DEBUGP(("Wrong card type: 0x%x\n", card));
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	status = rtsx_readl(chip, RTSX_BIPR);
+	if (!(status & card_cd)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int check_card_exist(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_exist & chip->lun2card[lun]) {
+		return 1;
+	}
+
+	return 0;
+}
+
+int check_card_ready(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_ready & chip->lun2card[lun]) {
+		return 1;
+	}
+
+	return 0;
+}
+
+int check_card_wp(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_wp & chip->lun2card[lun]) {
+		return 1;
+	}
+
+	return 0;
+}
+
+int check_card_fail(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_fail & chip->lun2card[lun]) {
+		return 1;
+	}
+
+	return 0;
+}
+
+int check_card_ejected(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_ejected & chip->lun2card[lun]) {
+		return 1;
+	}
+
+	return 0;
+}
+
+u8 get_lun_card(struct rtsx_chip * chip, unsigned int lun)
+{
+	if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD) {
+		return (u8) SD_CARD;
+	} else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD) {
+		return (u8) MS_CARD;
+	}
+
+	return 0;
+}
+
+void eject_card(struct rtsx_chip *chip, unsigned int lun)
+{
+	if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD) {
+		sd_cleanup_work(chip);
+		release_sd_card(chip);
+		chip->card_ejected |= SD_CARD;
+		chip->card_ready &= ~SD_CARD;
+		chip->capacity[lun] = 0;
+	} else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD) {
+		ms_cleanup_work(chip);
+		release_ms_card(chip);
+		chip->card_ejected |= MS_CARD;
+		chip->card_ready &= ~MS_CARD;
+		chip->capacity[lun] = 0;
+	}
+}
diff --git a/drivers/staging/rts5229/rtsx_card.h b/drivers/staging/rts5229/rtsx_card.h
new file mode 100644
index 0000000..727bb15
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_card.h
@@ -0,0 +1,779 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_CARD_H
+#define __REALTEK_RTSX_CARD_H
+
+#include "debug.h"
+#include "rtsx.h"
+#include "rtsx_chip.h"
+#include "rtsx_transport.h"
+#include "sd.h"
+
+#define SSC_POWER_DOWN		0x01
+#define SD_OC_POWER_DOWN	0x02
+#define ALL_POWER_DOWN		0x03
+#define OC_POWER_DOWN		0x02
+
+#define PMOS_STRG_MASK		0x10
+#define PMOS_STRG_800mA		0x10
+#define PMOS_STRG_400mA		0x00
+
+#define POWER_OFF		0x03
+#define PARTIAL_POWER_ON	0x01
+#define POWER_ON		0x00
+
+#define MS_POWER_OFF		0x0C
+#define MS_PARTIAL_POWER_ON	0x04
+#define MS_POWER_ON		0x00
+#define MS_POWER_MASK		0x0C
+
+#define SD_POWER_OFF		0x03
+#define SD_PARTIAL_POWER_ON	0x01
+#define SD_POWER_ON		0x00
+#define SD_POWER_MASK		0x03
+
+#define SD_OUTPUT_EN		0x04
+#define MS_OUTPUT_EN		0x08
+
+#define CLK_LOW_FREQ		0x01
+
+#define CLK_DIV_1		0x01
+#define CLK_DIV_2		0x02
+#define CLK_DIV_4		0x03
+#define CLK_DIV_8		0x04
+
+#define SSC_80			0
+#define SSC_100			1
+#define SSC_120			2
+#define SSC_150			3
+#define SSC_200			4
+
+#define SD_CLK_EN		0x04
+#define MS_CLK_EN		0x08
+
+#define SD_MOD_SEL		2
+#define MS_MOD_SEL		3
+
+#define CHANGE_CLK		0x01
+
+#define	SD_CRC7_ERR			0x80
+#define	SD_CRC16_ERR			0x40
+#define	SD_CRC_WRITE_ERR		0x20
+#define	SD_CRC_WRITE_ERR_MASK	    	0x1C
+#define	GET_CRC_TIME_OUT		0x02
+#define	SD_TUNING_COMPARE_ERR		0x01
+
+#define	SD_RSP_80CLK_TIMEOUT		0x01
+
+#define	SD_CLK_TOGGLE_EN		0x80
+#define	SD_CLK_FORCE_STOP	        0x40
+#define	SD_DAT3_STATUS		        0x10
+#define	SD_DAT2_STATUS		        0x08
+#define	SD_DAT1_STATUS		        0x04
+#define	SD_DAT0_STATUS		        0x02
+#define	SD_CMD_STATUS			0x01
+
+#define	SD_IO_USING_1V8		        0x80
+#define	SD_IO_USING_3V3		        0x7F
+#define	TYPE_A_DRIVING		        0x00
+#define	TYPE_B_DRIVING			0x01
+#define	TYPE_C_DRIVING			0x02
+#define	TYPE_D_DRIVING		        0x03
+
+#define	DDR_FIX_RX_DAT			0x00
+#define	DDR_VAR_RX_DAT			0x80
+#define	DDR_FIX_RX_DAT_EDGE		0x00
+#define	DDR_FIX_RX_DAT_14_DELAY		0x40
+#define	DDR_FIX_RX_CMD			0x00
+#define	DDR_VAR_RX_CMD			0x20
+#define	DDR_FIX_RX_CMD_POS_EDGE		0x00
+#define	DDR_FIX_RX_CMD_14_DELAY		0x10
+#define	SD20_RX_POS_EDGE		0x00
+#define	SD20_RX_14_DELAY		0x08
+#define SD20_RX_SEL_MASK		0x08
+
+#define	DDR_FIX_TX_CMD_DAT		0x00
+#define	DDR_VAR_TX_CMD_DAT		0x80
+#define	DDR_FIX_TX_DAT_14_TSU		0x00
+#define	DDR_FIX_TX_DAT_12_TSU		0x40
+#define	DDR_FIX_TX_CMD_NEG_EDGE		0x00
+#define	DDR_FIX_TX_CMD_14_AHEAD		0x20
+#define	SD20_TX_NEG_EDGE		0x00
+#define	SD20_TX_14_AHEAD		0x10
+#define SD20_TX_SEL_MASK		0x10
+#define	DDR_VAR_SDCLK_POL_SWAP		0x01
+
+#define	SD_TRANSFER_START		0x80
+#define	SD_TRANSFER_END			0x40
+#define SD_STAT_IDLE			0x20
+#define	SD_TRANSFER_ERR			0x10
+#define	SD_TM_NORMAL_WRITE		0x00
+#define	SD_TM_AUTO_WRITE_3		0x01
+#define	SD_TM_AUTO_WRITE_4		0x02
+#define	SD_TM_AUTO_READ_3		0x05
+#define	SD_TM_AUTO_READ_4		0x06
+#define	SD_TM_CMD_RSP			0x08
+#define	SD_TM_AUTO_WRITE_1		0x09
+#define	SD_TM_AUTO_WRITE_2		0x0A
+#define	SD_TM_NORMAL_READ		0x0C
+#define	SD_TM_AUTO_READ_1		0x0D
+#define	SD_TM_AUTO_READ_2		0x0E
+#define	SD_TM_AUTO_TUNING		0x0F
+
+#define PHASE_CHANGE			0x80
+#define PHASE_NOT_RESET			0x40
+
+#define DCMPS_CHANGE			0x80
+#define DCMPS_CHANGE_DONE	   	0x40
+#define DCMPS_ERROR			0x20
+#define DCMPS_CURRENT_PHASE     	0x1F
+
+#define SD_CLK_DIVIDE_0			0x00
+#define	SD_CLK_DIVIDE_256		0xC0
+#define	SD_CLK_DIVIDE_128		0x80
+#define	SD_BUS_WIDTH_1			0x00
+#define	SD_BUS_WIDTH_4			0x01
+#define	SD_BUS_WIDTH_8			0x02
+#define	SD_ASYNC_FIFO_NOT_RST		0x10
+#define	SD_20_MODE			0x00
+#define	SD_DDR_MODE			0x04
+#define	SD_30_MODE			0x08
+
+#define SD_CLK_DIVIDE_MASK		0xC0
+
+#define SD_CMD_IDLE			0x80
+
+#define SD_DATA_IDLE			0x80
+
+#define DCM_RESET			0x08
+#define DCM_LOCKED			0x04
+#define DCM_208M			0x00
+#define DCM_TX			        0x01
+#define DCM_RX			        0x02
+
+#define DRP_START			0x80
+#define DRP_DONE			0x40
+
+#define DRP_WRITE			0x80
+#define DRP_READ			0x00
+#define DCM_WRITE_ADDRESS_50		0x50
+#define DCM_WRITE_ADDRESS_51		0x51
+#define DCM_READ_ADDRESS_00		0x00
+#define DCM_READ_ADDRESS_51		0x51
+
+#define	SD_CALCULATE_CRC7		0x00
+#define	SD_NO_CALCULATE_CRC7		0x80
+#define	SD_CHECK_CRC16			0x00
+#define	SD_NO_CHECK_CRC16		0x40
+#define SD_NO_CHECK_WAIT_CRC_TO		0x20
+#define	SD_WAIT_BUSY_END		0x08
+#define	SD_NO_WAIT_BUSY_END		0x00
+#define	SD_CHECK_CRC7			0x00
+#define	SD_NO_CHECK_CRC7		0x04
+#define	SD_RSP_LEN_0			0x00
+#define	SD_RSP_LEN_6			0x01
+#define	SD_RSP_LEN_17			0x02
+#define	SD_RSP_TYPE_R0		0x04
+#define	SD_RSP_TYPE_R1		0x01
+#define	SD_RSP_TYPE_R1b		0x09
+#define	SD_RSP_TYPE_R2		0x02
+#define	SD_RSP_TYPE_R3		0x05
+#define	SD_RSP_TYPE_R4		0x05
+#define	SD_RSP_TYPE_R5		0x01
+#define	SD_RSP_TYPE_R6		0x01
+#define	SD_RSP_TYPE_R7		0x01
+
+#define	SD_RSP_80CLK_TIMEOUT_EN	    0x01
+
+#define	SAMPLE_TIME_RISING		0x00
+#define	SAMPLE_TIME_FALLING		0x80
+#define	PUSH_TIME_DEFAULT		0x00
+#define	PUSH_TIME_ODD			0x40
+#define	NO_EXTEND_TOGGLE		0x00
+#define	EXTEND_TOGGLE_CHK		0x20
+#define	MS_BUS_WIDTH_1			0x00
+#define	MS_BUS_WIDTH_4			0x10
+#define	MS_BUS_WIDTH_8			0x18
+#define	MS_2K_SECTOR_MODE		0x04
+#define	MS_512_SECTOR_MODE		0x00
+#define	MS_TOGGLE_TIMEOUT_EN		0x00
+#define	MS_TOGGLE_TIMEOUT_DISEN		0x01
+#define MS_NO_CHECK_INT			0x02
+
+#define	WAIT_INT			0x80
+#define	NO_WAIT_INT			0x00
+#define	NO_AUTO_READ_INT_REG		0x00
+#define	AUTO_READ_INT_REG		0x40
+#define	MS_CRC16_ERR			0x20
+#define	MS_RDY_TIMEOUT			0x10
+#define	MS_INT_CMDNK			0x08
+#define	MS_INT_BREQ			0x04
+#define	MS_INT_ERR			0x02
+#define	MS_INT_CED			0x01
+
+#define	MS_TRANSFER_START		0x80
+#define	MS_TRANSFER_END			0x40
+#define	MS_TRANSFER_ERR			0x20
+#define	MS_BS_STATE			0x10
+#define	MS_TM_READ_BYTES		0x00
+#define	MS_TM_NORMAL_READ		0x01
+#define	MS_TM_WRITE_BYTES		0x04
+#define	MS_TM_NORMAL_WRITE		0x05
+#define	MS_TM_AUTO_READ			0x08
+#define	MS_TM_AUTO_WRITE		0x0C
+
+#define CARD_SHARE_MASK			0x0F
+#define	CARD_SHARE_NORMAL		0x00
+#define	CARD_SHARE_SD			0x04
+#define	CARD_SHARE_MS			0x08
+
+#define SD_STOP			0x04
+#define MS_STOP			0x08
+#define SD_CLR_ERR		0x40
+#define MS_CLR_ERR		0x80
+
+#define CRC_FIX_CLK		(0x00 << 0)
+#define CRC_VAR_CLK0		(0x01 << 0)
+#define CRC_VAR_CLK1		(0x02 << 0)
+#define SD30_FIX_CLK		(0x00 << 2)
+#define SD30_VAR_CLK0		(0x01 << 2)
+#define SD30_VAR_CLK1		(0x02 << 2)
+#define SAMPLE_FIX_CLK		(0x00 << 4)
+#define SAMPLE_VAR_CLK0		(0x01 << 4)
+#define SAMPLE_VAR_CLK1		(0x02 << 4)
+
+#define PINGPONG_BUFFER		0x01
+#define RING_BUFFER		0x00
+
+#define RB_FLUSH		0x80
+#define RB_FULL			0x02
+
+#define DMA_DONE_INT_EN			0x80
+#define SUSPEND_INT_EN			0x40
+#define LINK_RDY_INT_EN			0x20
+#define LINK_DOWN_INT_EN		0x10
+
+#define DMA_DONE_INT			0x80
+#define SUSPEND_INT			0x40
+#define LINK_RDY_INT			0x20
+#define LINK_DOWN_INT			0x10
+
+#define MRD_ERR_INT_EN			0x40
+#define MWR_ERR_INT_EN			0x20
+#define SCSI_CMD_INT_EN			0x10
+#define TLP_RCV_INT_EN			0x08
+#define TLP_TRSMT_INT_EN		0x04
+#define MRD_COMPLETE_INT_EN		0x02
+#define MWR_COMPLETE_INT_EN		0x01
+
+#define MRD_ERR_INT			0x40
+#define MWR_ERR_INT			0x20
+#define SCSI_CMD_INT			0x10
+#define TLP_RX_INT			0x08
+#define TLP_TX_INT			0x04
+#define MRD_COMPLETE_INT		0x02
+#define MWR_COMPLETE_INT		0x01
+
+#define MSG_RX_INT_EN			0x08
+#define MRD_RX_INT_EN			0x04
+#define MWR_RX_INT_EN			0x02
+#define CPLD_RX_INT_EN			0x01
+
+#define MSG_RX_INT			0x08
+#define MRD_RX_INT			0x04
+#define MWR_RX_INT			0x02
+#define CPLD_RX_INT			0x01
+
+#define MSG_TX_INT_EN			0x08
+#define MRD_TX_INT_EN			0x04
+#define MWR_TX_INT_EN			0x02
+#define CPLD_TX_INT_EN			0x01
+
+#define MSG_TX_INT			0x08
+#define MRD_TX_INT			0x04
+#define MWR_TX_INT			0x02
+#define CPLD_TX_INT			0x01
+
+#define DMA_RST				0x80
+#define DMA_BUSY			0x04
+#define DMA_DIR_TO_CARD			0x00
+#define DMA_DIR_FROM_CARD		0x02
+#define DMA_EN				0x01
+#define DMA_128				(0 << 4)
+#define DMA_256				(1 << 4)
+#define DMA_512				(2 << 4)
+#define DMA_1024			(3 << 4)
+#define DMA_PACK_SIZE_MASK		0x30
+
+#define	RSTB_MODE_DETECT		0x80
+#define	MODE_OUT_VLD			0x40
+#define	MODE_OUT_0_NONE			0x00
+#define	MODE_OUT_10_NONE		0x04
+#define	MODE_OUT_10_47			0x05
+#define	MODE_OUT_10_180			0x06
+#define	MODE_OUT_10_680			0x07
+#define	MODE_OUT_16_NONE		0x08
+#define	MODE_OUT_16_47			0x09
+#define	MODE_OUT_16_180			0x0A
+#define	MODE_OUT_16_680			0x0B
+#define	MODE_OUT_NONE_NONE		0x0C
+#define	MODE_OUT_NONE_47		0x0D
+#define	MODE_OUT_NONE_180		0x0E
+#define	MODE_OUT_NONE_680		0x0F
+
+#define SD_DETECT_EN			0x08
+#define SD_OCP_INT_EN			0x04
+#define SD_OCP_INT_CLR			0x02
+#define SD_OC_CLR			0x01
+
+#define SD_OCP_DETECT			0x08
+#define SD_OC_NOW			0x04
+#define SD_OC_EVER			0x02
+
+#define SD_OCP_GLITCH_MASK		0x07
+#define SD_OCP_GLITCH_6_4		0x00
+#define SD_OCP_GLITCH_64		0x01
+#define SD_OCP_GLITCH_640		0x02
+#define SD_OCP_GLITCH_1000		0x03
+#define SD_OCP_GLITCH_2000		0x04
+#define SD_OCP_GLITCH_4000		0x05
+#define SD_OCP_GLITCH_8000		0x06
+#define SD_OCP_GLITCH_10000		0x07
+
+#define SD_OCP_TIME_60			0x00
+#define SD_OCP_TIME_100			0x01
+#define SD_OCP_TIME_200			0x02
+#define SD_OCP_TIME_400			0x03
+#define SD_OCP_TIME_600			0x04
+#define SD_OCP_TIME_800			0x05
+#define SD_OCP_TIME_1100		0x06
+#define SD_OCP_TIME_MASK		0x07
+
+#define SD_OCP_THD_450			0x00
+#define SD_OCP_THD_550			0x01
+#define SD_OCP_THD_650			0x02
+#define SD_OCP_THD_750			0x03
+#define SD_OCP_THD_850			0x04
+#define SD_OCP_THD_950			0x05
+#define SD_OCP_THD_1050			0x06
+#define SD_OCP_THD_1150			0x07
+#define SD_OCP_THD_MASK			0x07
+
+#define FPGA_MS_PULL_CTL_EN		0xEF
+#define FPGA_SD_PULL_CTL_EN		0xF7
+#define FPGA_MS_PULL_CTL_BIT		0x10
+#define FPGA_SD_PULL_CTL_BIT		0x08
+
+#define LED_SHINE_EN			0x08
+#define LED_SPEED_MASK			0x07
+
+#define SSC_RSTB		0x80
+#define SSC_8X_EN		0x40
+#define SSC_FIX_FRAC		0x20
+#define SSC_SEL_1M		0x00
+#define SSC_SEL_2M		0x08
+#define SSC_SEL_4M		0x10
+#define SSC_SEL_8M		0x18
+
+#define SSC_DEPTH_MASK		0x07
+#define SSC_DEPTH_DISALBE	0x00
+#define SSC_DEPTH_4M		0x01
+#define SSC_DEPTH_2M		0x02
+#define SSC_DEPTH_1M		0x03
+#define SSC_DEPTH_512K		0x04
+#define SSC_DEPTH_256K		0x05
+#define SSC_DEPTH_128K		0x06
+#define SSC_DEPTH_64K		0x07
+
+#define SD_D7_NP		0x00
+#define SD_D7_PD		(0x01 << 4)
+#define SD_DAT7_PU		(0x02 << 4)
+#define SD_CLK_NP		0x00
+#define SD_CLK_PD		(0x01 << 2)
+#define SD_CLK_PU		(0x02 << 2)
+#define SD_D5_NP		0x00
+#define SD_D5_PD		0x01
+#define SD_D5_PU		0x02
+
+#define MS_D1_NP		0x00
+#define MS_D1_PD		(0x01 << 6)
+#define MS_D1_PU		(0x02 << 6)
+#define MS_D2_NP		0x00
+#define MS_D2_PD		(0x01 << 4)
+#define MS_D2_PU		(0x02 << 4)
+#define MS_CLK_NP		0x00
+#define MS_CLK_PD		(0x01 << 2)
+#define MS_CLK_PU		(0x02 << 2)
+#define MS_D6_NP		0x00
+#define MS_D6_PD		0x01
+#define MS_D6_PU		0x02
+
+#define SD_D6_NP		0x00
+#define SD_D6_PD		(0x01 << 6)
+#define SD_D6_PU		(0x02 << 6)
+#define SD_D0_NP		0x00
+#define SD_D0_PD		(0x01 << 4)
+#define SD_D0_PU		(0x02 << 4)
+#define SD_D1_NP		0x00
+#define SD_D1_PD		0x01
+#define SD_D1_PU		0x02
+
+#define MS_D3_NP		0x00
+#define MS_D3_PD		(0x01 << 6)
+#define MS_D3_PU		(0x02 << 6)
+#define MS_D0_NP		0x00
+#define MS_D0_PD		(0x01 << 4)
+#define MS_D0_PU		(0x02 << 4)
+#define MS_BS_NP		0x00
+#define MS_BS_PD		(0x01 << 2)
+#define MS_BS_PU		(0x02 << 2)
+
+#define SD_D4_NP		0x00
+#define SD_D4_PD		(0x01 << 6)
+#define SD_D4_PU		(0x02 << 6)
+
+#define MS_D7_NP		0x00
+#define MS_D7_PD		(0x01 << 6)
+#define MS_D7_PU		(0x02 << 6)
+
+#define SD_D3_NP		0x00
+#define SD_D3_PD		(0x01 << 4)
+#define SD_D3_PU		(0x02 << 4)
+#define SD_D2_NP		0x00
+#define SD_D2_PD		(0x01 << 2)
+#define SD_D2_PU		(0x02 << 2)
+
+#define MS_INS_PD		0x00
+#define MS_INS_PU		(0x01 << 7)
+#define SD_WP_NP		0x00
+#define SD_WP_PD		(0x01 << 5)
+#define SD_WP_PU		(0x02 << 5)
+#define SD_CD_PD		0x00
+#define SD_CD_PU		(0x01 << 4)
+#define SD_CMD_NP		0x00
+#define SD_CMD_PD		(0x01 << 2)
+#define SD_CMD_PU		(0x02 << 2)
+
+#define MS_D5_NP		0x00
+#define MS_D5_PD		(0x01 << 2)
+#define MS_D5_PU		(0x02 << 2)
+#define MS_D4_NP		0x00
+#define MS_D4_PD		0x01
+#define MS_D4_PU		0x02
+
+#define FORCE_PM_CLOCK		0x10
+#define EN_CLOCK_PM		0x01
+
+#define HOST_ENTER_S3		0x02
+#define HOST_ENTER_S1		0x01
+
+#define AUX_PWR_DETECTED	0x01
+
+#define PHY_DEBUG_MODE		0x01
+
+#define PWR_GATE_EN		0x01
+#define LDO3318_PWR_MASK	0x06
+#define LDO_ON			0x06
+#define LDO_SUSPEND		0x02
+#define LDO_OFF			0x00
+
+#define SD_CFG1			0xFDA0
+#define SD_CFG2			0xFDA1
+#define SD_CFG3			0xFDA2
+#define SD_STAT1		0xFDA3
+#define SD_STAT2		0xFDA4
+#define SD_BUS_STAT		0xFDA5
+#define SD_PAD_CTL		0xFDA6
+#define SD_SAMPLE_POINT_CTL	0xFDA7
+#define SD_PUSH_POINT_CTL	0xFDA8
+#define SD_CMD0			0xFDA9
+#define SD_CMD1			0xFDAA
+#define SD_CMD2			0xFDAB
+#define SD_CMD3			0xFDAC
+#define SD_CMD4			0xFDAD
+#define SD_CMD5			0xFDAE
+#define SD_BYTE_CNT_L		0xFDAF
+#define SD_BYTE_CNT_H		0xFDB0
+#define SD_BLOCK_CNT_L		0xFDB1
+#define SD_BLOCK_CNT_H		0xFDB2
+#define SD_TRANSFER		0xFDB3
+#define SD_CMD_STATE		0xFDB5
+#define SD_DATA_STATE		0xFDB6
+
+#define	DCM_DRP_CTL         	0xFC23
+#define	DCM_DRP_TRIG		0xFC24
+#define	DCM_DRP_CFG         	0xFC25
+#define	DCM_DRP_WR_DATA_L   	0xFC26
+#define	DCM_DRP_WR_DATA_H   	0xFC27
+#define	DCM_DRP_RD_DATA_L   	0xFC28
+#define	DCM_DRP_RD_DATA_H   	0xFC29
+#define SD_VPCLK0_CTL		0xFC2A
+#define SD_VPCLK1_CTL		0xFC2B
+#define SD_DCMPS0_CTL		0xFC2C
+#define SD_DCMPS1_CTL		0xFC2D
+#define SD_VPTX_CTL		SD_VPCLK0_CTL
+#define SD_VPRX_CTL		SD_VPCLK1_CTL
+#define SD_DCMPS_TX_CTL		SD_DCMPS0_CTL
+#define SD_DCMPS_RX_CTL		SD_DCMPS1_CTL
+
+#define CARD_CLK_SOURCE		0xFC2E
+
+#define CARD_PWR_CTL		0xFD50
+#define CARD_CLK_SWITCH		0xFD51
+#define CARD_SHARE_MODE		0xFD52
+#define CARD_DRIVE_SEL		0xFD53
+#define CARD_STOP		0xFD54
+#define CARD_OE			0xFD55
+
+#define OLT_LED_CTL		0xFC1E
+#define GPIO_CTL		0xFC1F
+
+#define CARD_DATA_SOURCE	0xFD5B
+#define CARD_SELECT		0xFD5C
+#define SD30_DRIVE_SEL		0xFD5E
+#define SD30_CMD_DRIVE_SEL	0xFD5E
+#define SD30_CLK_DRIVE_SEL	0xFD5A
+#define SD30_DAT_DRIVE_SEL	0xFD5F
+
+#define CARD_CLK_EN		0xFD69
+
+#define FPDCTL			0xFC00
+#define PDINFO			0xFC01
+
+#define CLK_CTL			0xFC02
+#define CLK_DIV			0xFC03
+#define CLK_SEL			0xFC04
+
+#define SSC_DIV_N_0		0xFC0F
+#define SSC_DIV_N_1		0xFC10
+
+#define RCCTL			0xFC14
+
+#define FPGA_PULL_CTL		0xFC1D
+
+#define CARD_PULL_CTL1		0xFD60
+#define CARD_PULL_CTL2		0xFD61
+#define CARD_PULL_CTL3		0xFD62
+#define CARD_PULL_CTL4		0xFD63
+#define CARD_PULL_CTL5		0xFD64
+#define CARD_PULL_CTL6		0xFD65
+
+#define IRQEN0				0xFE20
+#define IRQSTAT0			0xFE21
+#define IRQEN1				0xFE22
+#define IRQSTAT1			0xFE23
+#define TLPRIEN				0xFE24
+#define TLPRISTAT			0xFE25
+#define TLPTIEN				0xFE26
+#define TLPTISTAT			0xFE27
+#define DMATC0				0xFE28
+#define DMATC1				0xFE29
+#define DMATC2				0xFE2A
+#define DMATC3				0xFE2B
+#define DMACTL				0xFE2C
+#define BCTL				0xFE2D
+#define RBBC0				0xFE2E
+#define RBBC1				0xFE2F
+#define RBDAT				0xFE30
+#define RBCTL				0xFE34
+#define CFGADDR0			0xFE35
+#define CFGADDR1			0xFE36
+#define CFGDATA0			0xFE37
+#define CFGDATA1			0xFE38
+#define CFGDATA2			0xFE39
+#define CFGDATA3			0xFE3A
+#define CFGRWCTL			0xFE3B
+#define PHYRWCTL			0xFE3C
+#define PHYDATA0			0xFE3D
+#define PHYDATA1			0xFE3E
+#define PHYADDR				0xFE3F
+#define MSGRXDATA0			0xFE40
+#define MSGRXDATA1			0xFE41
+#define MSGRXDATA2			0xFE42
+#define MSGRXDATA3			0xFE43
+#define MSGTXDATA0			0xFE44
+#define MSGTXDATA1			0xFE45
+#define MSGTXDATA2			0xFE46
+#define MSGTXDATA3			0xFE47
+#define MSGTXCTL			0xFE48
+#define PETXCFG				0xFE49
+#define LTR_CTL				0xFE4A
+
+#define CDRESUMECTL			0xFE52
+#define WAKE_SEL_CTL			0xFE54
+#define PME_FORCE_CTL			0xFE56
+#define ASPM_FORCE_CTL			0xFE57
+#define PM_CLK_FORCE_CTL		0xFE58
+#define PERST_GLITCH_WIDTH		0xFE5C
+#define CHANGE_LINK_STATE		0xFE5B
+#define RESET_LOAD_REG			0xFE5E
+#define HOST_SLEEP_STATE		0xFE60
+
+#define NFTS_TX_CTRL			0xFE72
+
+#define PWR_GATE_CTRL			0xFE75
+#define PWD_SUSPEND_EN			0xFE76
+
+#define AUTOLOAD_CFG			0xFF46
+
+#define MS_CFG				0xFD40
+#define MS_TPC				0xFD41
+#define MS_TRANS_CFG			0xFD42
+#define MS_TRANSFER			0xFD43
+#define MS_INT_REG			0xFD44
+#define MS_BYTE_CNT			0xFD45
+#define MS_SECTOR_CNT_L			0xFD46
+#define MS_SECTOR_CNT_H			0xFD47
+#define MS_DBUS_H			0xFD48
+
+#define SSC_CTL1			0xFC11
+#define SSC_CTL2			0xFC12
+
+#define OCPCTL				0xFC15
+#define OCPSTAT				0xFC16
+#define OCPGLITCH			0xFC17
+#define OCPPARA1			0xFC18
+#define OCPPARA2			0xFC19
+
+#define RTS5227_OCPCTL			0xFD6A
+#define RTS5227_OCPSTAT			0xFD6E
+#define RTS5227_OCPGLITCH		0xFD6C
+#define RTS5227_OCPPARA1		0xFD6B
+#define RTS5227_OCPPARA2		0xFD6D
+
+#define SRAM_BASE			0xE600
+#define RBUF_BASE			0xF400
+#define PPBUF_BASE1			0xF800
+#ifdef USING_PPBUF
+#define PPBUF_BASE2			0xFA00
+#else
+#define PPBUF_BASE2			0xF800
+#endif
+#define IMAGE_FLAG_ADDR0		0xCE80
+#define IMAGE_FLAG_ADDR1		0xCE81
+
+#ifdef USING_PPBUF
+#define PPBUF_LEN			512
+#else
+#define PPBUF_LEN			64
+#endif
+
+#define READ_OP			1
+#define WRITE_OP		2
+
+#define LCTLR		0x80
+
+#define POLLING_WAIT_CNT	1
+#define IDLE_MAX_COUNT		10
+
+#define DEBOUNCE_CNT			5
+
+void do_remaining_work(struct rtsx_chip *chip);
+void do_reset_sd_card(struct rtsx_chip *chip);
+void do_reset_ms_card(struct rtsx_chip *chip);
+void rtsx_power_off_card(struct rtsx_chip *chip);
+void rtsx_release_cards(struct rtsx_chip *chip);
+void rtsx_reset_cards(struct rtsx_chip *chip);
+void rtsx_reinit_cards(struct rtsx_chip *chip, int reset_chip);
+void rtsx_init_cards(struct rtsx_chip *chip);
+int switch_ssc_clock(struct rtsx_chip *chip, int clk);
+int switch_normal_clock(struct rtsx_chip *chip, int clk);
+int enable_card_clock(struct rtsx_chip *chip, u8 card);
+int disable_card_clock(struct rtsx_chip *chip, u8 card);
+int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 sec_addr,
+	    u16 sec_cnt);
+void trans_dma_enable(enum dma_data_direction dir, struct rtsx_chip *chip,
+		      u32 byte_cnt, u8 pack_size);
+void toggle_gpio(struct rtsx_chip *chip);
+void toggle_led(struct rtsx_chip *chip);
+void turn_on_led(struct rtsx_chip *chip);
+void turn_off_led(struct rtsx_chip *chip);
+#ifdef LED_AUTO_BLINK
+void enable_auto_blink(struct rtsx_chip *chip);
+void disable_auto_blink(struct rtsx_chip *chip);
+#endif
+
+int card_share_mode(struct rtsx_chip *chip, int card);
+int select_card(struct rtsx_chip *chip, int card);
+int detect_card_cd(struct rtsx_chip *chip, int card);
+int check_card_exist(struct rtsx_chip *chip, unsigned int lun);
+int check_card_ready(struct rtsx_chip *chip, unsigned int lun);
+int check_card_wp(struct rtsx_chip *chip, unsigned int lun);
+int check_card_fail(struct rtsx_chip *chip, unsigned int lun);
+int check_card_ejected(struct rtsx_chip *chip, unsigned int lun);
+void eject_card(struct rtsx_chip *chip, unsigned int lun);
+u8 get_lun_card(struct rtsx_chip *chip, unsigned int lun);
+
+static inline u32 get_card_size(struct rtsx_chip *chip, unsigned int lun)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &(chip->sd_card);
+
+	if ((get_lun_card(chip, lun) == SD_CARD)
+	    && (sd_card->sd_lock_status & SD_LOCKED)) {
+		return 0;
+	} else {
+		return chip->capacity[lun];
+	}
+#else
+	return chip->capacity[lun];
+#endif
+}
+
+static inline int switch_clock(struct rtsx_chip *chip, int clk)
+{
+	int retval = 0;
+
+	if (chip->asic_code) {
+		retval = switch_ssc_clock(chip, clk);
+	} else {
+		retval = switch_normal_clock(chip, clk);
+	}
+
+	return retval;
+}
+
+int card_power_on(struct rtsx_chip *chip, u8 card);
+int card_power_off(struct rtsx_chip *chip, u8 card);
+
+static inline int card_power_off_all(struct rtsx_chip *chip)
+{
+	RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0x0F, 0x0F);
+
+	return STATUS_SUCCESS;
+}
+
+static inline void rtsx_clear_sd_error(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
+			    SD_STOP | SD_CLR_ERR);
+}
+
+static inline void rtsx_clear_ms_error(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
+			    MS_STOP | MS_CLR_ERR);
+}
+
+#endif
diff --git a/drivers/staging/rts5229/rtsx_chip.c b/drivers/staging/rts5229/rtsx_chip.c
new file mode 100644
index 0000000..7b2b2237
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_chip.c
@@ -0,0 +1,2097 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+
+#include "rtsx.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "rtsx_chip.h"
+#include "rtsx_sys.h"
+#include "general.h"
+
+#include "sd.h"
+#include "ms.h"
+
+void rtsx_disable_card_int(struct rtsx_chip *chip)
+{
+	u32 reg = rtsx_readl(chip, RTSX_BIER);
+
+	reg &= ~(SD_INT_EN | MS_INT_EN);
+	rtsx_writel(chip, RTSX_BIER, reg);
+}
+
+void rtsx_enable_card_int(struct rtsx_chip *chip)
+{
+	u32 reg = rtsx_readl(chip, RTSX_BIER);
+	int i;
+
+	for (i = 0; i <= chip->max_lun; i++) {
+
+		if (chip->lun2card[i] & SD_CARD) {
+			reg |= SD_INT_EN;
+		}
+		if (chip->lun2card[i] & MS_CARD) {
+			reg |= MS_INT_EN;
+		}
+	}
+
+	rtsx_writel(chip, RTSX_BIER, reg);
+}
+
+void rtsx_enable_bus_int(struct rtsx_chip *chip)
+{
+	u32 reg = 0;
+#ifndef DISABLE_CARD_INT
+	int i;
+#endif
+
+	reg = TRANS_OK_INT_EN | TRANS_FAIL_INT_EN | DELINK_INT_EN;
+
+#ifndef DISABLE_CARD_INT
+	for (i = 0; i <= chip->max_lun; i++) {
+		RTSX_DEBUGP(("lun2card[%d] = 0x%02x\n", i,
+			     chip->lun2card[i]));
+
+		if (chip->lun2card[i] & SD_CARD) {
+			reg |= SD_INT_EN;
+		}
+		if (chip->lun2card[i] & MS_CARD) {
+			reg |= MS_INT_EN;
+		}
+	}
+#endif
+
+#ifdef SUPPORT_OCP
+	reg |= SD_OC_INT_EN;
+#endif
+	if (!chip->adma_mode) {
+		reg |= DATA_DONE_INT_EN;
+	}
+
+	rtsx_writel(chip, RTSX_BIER, reg);
+
+	RTSX_DEBUGP(("RTSX_BIER: 0x%08x\n", reg));
+}
+
+void rtsx_disable_bus_int(struct rtsx_chip *chip)
+{
+	rtsx_writel(chip, RTSX_BIER, 0);
+}
+
+static int rtsx_init_pull_ctl(struct rtsx_chip *chip)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	if (CHECK_IC_VER(chip, IC_VER_C))
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xE5);
+	else
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xD5);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x15);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	return STATUS_SUCCESS;
+}
+
+static int rtsx_init_ocp(struct rtsx_chip *chip)
+{
+	u16 ocp_para1, ocp_para2, ocp_glitch, ocp_ctl;
+	u8 ocp_glitch_mask, ocp_glitch_time;
+
+	if (CHECK_PID(chip, 0x5229)) {
+		ocp_para1 = OCPPARA1;
+		ocp_para2 = OCPPARA2;
+		ocp_glitch = OCPGLITCH;
+		ocp_ctl = OCPCTL;
+		ocp_glitch_mask = 0x07;
+		ocp_glitch_time = 3;
+	} else {
+		ocp_para1 = RTS5227_OCPPARA1;
+		ocp_para2 = RTS5227_OCPPARA2;
+		ocp_glitch = RTS5227_OCPGLITCH;
+		ocp_ctl = RTS5227_OCPCTL;
+		ocp_glitch_mask = 0x0F;
+		ocp_glitch_time = 6;
+	}
+
+	RTSX_WRITE_REG(chip, ocp_para1, SD_OCP_TIME_MASK, SD_OCP_TIME_800);
+	RTSX_WRITE_REG(chip, ocp_para2, SD_OCP_THD_MASK,
+		       chip->sd_400mA_ocp_thd);
+	RTSX_WRITE_REG(chip, ocp_glitch, ocp_glitch_mask, ocp_glitch_time);
+	RTSX_WRITE_REG(chip, ocp_ctl, 0xFF, SD_OCP_INT_EN | SD_DETECT_EN);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_enable_ocp(struct rtsx_chip *chip)
+{
+	u16 ocp_ctl;
+
+	if (CHECK_PID(chip, 0x5229))
+		ocp_ctl = OCPCTL;
+	else
+		ocp_ctl = RTS5227_OCPCTL;
+
+	RTSX_WRITE_REG(chip, ocp_ctl, 0xFF, SD_OCP_INT_EN | SD_DETECT_EN);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_disable_ocp(struct rtsx_chip *chip)
+{
+	u16 ocp_ctl;
+
+	if (CHECK_PID(chip, 0x5229))
+		ocp_ctl = OCPCTL;
+	else
+		ocp_ctl = RTS5227_OCPCTL;
+
+	RTSX_WRITE_REG(chip, ocp_ctl, SD_OCP_INT_EN | SD_DETECT_EN, 0);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_clear_ocp(struct rtsx_chip *chip)
+{
+	u16 ocp_ctl;
+
+	if (CHECK_PID(chip, 0x5229))
+		ocp_ctl = OCPCTL;
+	else
+		ocp_ctl = RTS5227_OCPCTL;
+
+	RTSX_WRITE_REG(chip, ocp_ctl, SD_OCP_INT_CLR | SD_OC_CLR,
+		       SD_OCP_INT_CLR | SD_OC_CLR);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_reset_chip(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 lun2card = 0;
+	int i;
+
+	for (i = 0; i <= chip->max_lun; i++)
+		lun2card |= chip->lun2card[i];
+
+	rtsx_writel(chip, RTSX_HCBAR, chip->host_cmds_addr);
+
+	rtsx_init_aspm(chip);
+
+	if (chip->asic_code) {
+		u16 val;
+
+		retval = rtsx_write_phy_register(chip, 0x00, chip->phy_pcr);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (chip->phy_optimize) {
+			retval =
+			    rtsx_write_phy_register(chip, 0x03,
+						    chip->phy_rcr2);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		retval = rtsx_write_phy_register(chip, 0x19, 0xFE6C);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		wait_timeout(1);
+		retval = rtsx_write_phy_register(chip, 0x0A, 0x05C0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = rtsx_read_phy_register(chip, 0x08, &val);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		RTSX_DEBUGP(("Read from phy 0x08: 0x%04x\n", val));
+
+		if (chip->phy_voltage <= 0x3F) {
+			chip->phy_voltage &= 0x3F;
+			RTSX_DEBUGP(("chip->phy_voltage = 0x%x\n",
+				     chip->phy_voltage));
+			val &= ~0x3F;
+			val |= chip->phy_voltage;
+			RTSX_DEBUGP(("Write to phy 0x08: 0x%04x\n", val));
+			retval = rtsx_write_phy_register(chip, 0x08, val);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			chip->phy_voltage = (u8) (val & 0x3F);
+			RTSX_DEBUGP(("Default, chip->phy_voltage = 0x%x\n",
+				     chip->phy_voltage));
+		}
+	}
+
+	RTSX_WRITE_REG(chip, HOST_SLEEP_STATE, 0x03, 0x00);
+
+	RTSX_WRITE_REG(chip, CARD_CLK_EN, 0x1E, 0);
+
+	RTSX_WRITE_REG(chip, ASPM_FORCE_CTL, 0x13, 0);
+
+#ifdef SUPPORT_OCP
+	RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, 0);
+
+	retval = rtsx_init_ocp(chip);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+#else
+	RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, OC_POWER_DOWN);
+#endif
+
+	if (chip->dev_option & TURN_ON_LED_AT_START)
+		RTSX_WRITE_REG(chip, GPIO_CTL, 0x02, 0x02);
+	else
+		RTSX_WRITE_REG(chip, GPIO_CTL, 0x02, 0x00);
+
+#ifdef LED_AUTO_BLINK
+	RTSX_WRITE_REG(chip, OLT_LED_CTL, LED_SHINE_EN | LED_SPEED_MASK,
+		       0x02);
+#endif
+
+	RTSX_WRITE_REG(chip, CHANGE_LINK_STATE, 0x0A, 0);
+
+	RTSX_WRITE_REG(chip, CARD_DRIVE_SEL, 0xFF, chip->card_drive_sel);
+	retval = sd_set_sd30_drive(chip, SD_IO_3V3);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	if (chip->asic_code) {
+		RTSX_WRITE_REG(chip, SSC_CTL1, 0xFF, SSC_8X_EN | SSC_SEL_4M);
+		RTSX_WRITE_REG(chip, SSC_CTL2, 0xFF, 0x12);
+	}
+
+	RTSX_WRITE_REG(chip, CHANGE_LINK_STATE, 0x16, 0x10);
+
+	if (chip->aspm_l0s_l1_en) {
+		if (!chip->dynamic_aspm) {
+			retval =
+			    rtsx_write_config_byte(chip, LCTLR,
+						   chip->aspm_l0s_l1_en);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (chip->config_host_aspm) {
+				rtsx_enable_host_aspm(chip);
+			}
+			chip->aspm_level[0] = chip->aspm_l0s_l1_en;
+			chip->aspm_enabled = 1;
+
+			if (chip->config_host_aspm)
+				rtsx_set_host_aspm(chip, chip->host_aspm_val);
+		}
+	} else {
+		retval =
+		    rtsx_write_config_byte(chip, LCTLR, chip->aspm_l0s_l1_en);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval = rtsx_write_config_byte(chip, 0x81, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = rtsx_write_cfg_dw(chip, 0, 0x70C, 0xFF000000, 0x5B000000);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, IRQSTAT0, LINK_RDY_INT, LINK_RDY_INT);
+
+	RTSX_WRITE_REG(chip, PERST_GLITCH_WIDTH, 0xFF, 0x80);
+
+	RTSX_WRITE_REG(chip, PWD_SUSPEND_EN, 0xFF, 0xFF);
+	RTSX_WRITE_REG(chip, PWR_GATE_CTRL, PWR_GATE_EN, PWR_GATE_EN);
+
+	rtsx_enable_bus_int(chip);
+
+#ifdef HW_INT_WRITE_CLR
+	RTSX_WRITE_REG(chip, NFTS_TX_CTRL, 0x02, 0);
+#endif
+
+	chip->need_reset = 0;
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+#ifdef HW_INT_WRITE_CLR
+	rtsx_writel(chip, RTSX_BIPR, chip->int_reg);
+#endif
+	if (lun2card & SD_CARD) {
+		if (chip->int_reg & SD_EXIST) {
+			chip->need_reset |= SD_CARD;
+		}
+	}
+	if (lun2card & MS_CARD) {
+		if (chip->int_reg & MS_EXIST) {
+			chip->need_reset |= MS_CARD;
+		}
+	}
+	if (chip->int_reg & CARD_EXIST) {
+		RTSX_WRITE_REG(chip, SSC_CTL1, SSC_RSTB, SSC_RSTB);
+	}
+
+	RTSX_DEBUGP(("In rtsx_init_chip, chip->need_reset = 0x%x\n",
+		     (unsigned int)(chip->need_reset)));
+
+	RTSX_WRITE_REG(chip, RCCTL, 0x01, 0x00);
+
+	if (CHECK_PID(chip, 0x5227)) {
+		chip->aux_pwr_exist = 1;
+		RTSX_DEBUGP(("chip->aux_pwr_exist = %d\n",
+			     chip->aux_pwr_exist));
+	}
+
+	if (chip->remote_wakeup_en && !CHK_AUTODELINK_EN(chip)) {
+		if (CHECK_PID(chip, 0x5229))
+			RTSX_WRITE_REG(chip, WAKE_SEL_CTL, 0x07, 0x07);
+		else
+			RTSX_WRITE_REG(chip, AUTOLOAD_CFG, 0x01, 0x01);
+		if (chip->aux_pwr_exist) {
+			RTSX_WRITE_REG(chip, PME_FORCE_CTL, 0xFF, 0x33);
+		}
+	} else {
+		if (CHECK_PID(chip, 0x5229))
+			RTSX_WRITE_REG(chip, WAKE_SEL_CTL, 0x07, 0x04);
+		else
+			RTSX_WRITE_REG(chip, AUTOLOAD_CFG, 0x01, 0x00);
+		RTSX_WRITE_REG(chip, PME_FORCE_CTL, 0xFF, 0x30);
+	}
+
+	if (chip->force_clkreq_0) {
+		if (CHECK_PID(chip, 0x5229))
+			RTSX_WRITE_REG(chip, PETXCFG, 0x08, 0x08);
+		else
+			RTSX_WRITE_REG(chip, 0xFF03, 0x80, 0x80);
+	} else {
+		if (CHECK_PID(chip, 0x5229))
+			RTSX_WRITE_REG(chip, PETXCFG, 0x08, 0x00);
+		else
+			RTSX_WRITE_REG(chip, 0xFF03, 0x80, 0x00);
+	}
+
+	if (CHECK_PID(chip, 0x5227)) {
+		if (chip->ltr_en) {
+			u16 val;
+
+			if (rtsx_read_config_word(chip, 0x98, &val) < 0)
+				TRACE_RET(chip, STATUS_FAIL);
+			RTSX_DEBUGP(("Config byte 0x98: 0x%04x\n", val));
+			if (val & 0x400) {
+				chip->ltr_enabled = 1;
+				RTSX_DEBUGP(("Set LTR_CTL to 0xA3\n"));
+				RTSX_WRITE_REG(chip, LTR_CTL, 0xFF, 0xA3);
+				chip->ltr_active = 1;
+			} else {
+				chip->ltr_enabled = 0;
+			}
+		}
+
+		if (chip->obff_en)
+			rtsx_write_register(chip, 0xFE4C, 0x03, 0x03);
+		else
+			rtsx_write_register(chip, 0xFE4C, 0x03, 0x00);
+
+		RTSX_WRITE_REG(chip, AUTOLOAD_CFG, 0x50, 0x40);
+	}
+
+	if (chip->ft2_fast_mode) {
+		RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0xFF,
+			       MS_PARTIAL_POWER_ON | SD_PARTIAL_POWER_ON);
+		udelay(chip->pmos_pwr_on_interval);
+		RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0xFF,
+			       MS_POWER_ON | SD_POWER_ON);
+
+		wait_timeout(200);
+	}
+
+	if (chip->asic_code) {
+		RTSX_WRITE_REG(chip, 0xFE78, 0x03, 0x00);
+		RTSX_WRITE_REG(chip, 0xFE78, 0x03, 0x01);
+	} else {
+		RTSX_WRITE_REG(chip, 0xFE78, 0x03, 0x01);
+	}
+
+	retval = rtsx_init_pull_ctl(chip);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	rtsx_reset_detected_cards(chip, 0);
+
+	chip->driver_first_load = 0;
+
+	return STATUS_SUCCESS;
+}
+
+static inline int check_sd_speed_prior(u32 sd_speed_prior)
+{
+	int i, fake_para = 0;
+
+	for (i = 0; i < 4; i++) {
+		u8 tmp = (u8) (sd_speed_prior >> (i * 8));
+		if ((tmp < 0x01) || (tmp > 0x04)) {
+			fake_para = 1;
+			break;
+		}
+	}
+
+	return !fake_para;
+}
+
+static inline int check_sd_current_prior(u32 sd_current_prior)
+{
+	int i, fake_para = 0;
+
+	for (i = 0; i < 4; i++) {
+		u8 tmp = (u8) (sd_current_prior >> (i * 8));
+		if (tmp > 0x03) {
+			fake_para = 1;
+			break;
+		}
+	}
+
+	return !fake_para;
+}
+
+static inline int rts5229_set_cfg_epcore(struct rtsx_chip *chip,
+					 u16 autoload_addr, u16 cfg_addr,
+					 u8 func, u8 mode, u8 data)
+{
+	RTSX_WRITE_REG(chip, autoload_addr, 0xFF, data);
+	RTSX_WRITE_REG(chip, autoload_addr + 1, 0xFF,
+		       (mode & 0x0F) | ((func & 0x03) << 4) |
+		       (((u8) cfg_addr & 0x03) << 6));
+	RTSX_WRITE_REG(chip, autoload_addr + 2, 0xFF, (u8) (cfg_addr >> 2));
+	RTSX_WRITE_REG(chip, autoload_addr + 3, 0xFF,
+		       ((u8) (cfg_addr >> 10) & 0x3F) | 0x80);
+
+	return STATUS_SUCCESS;
+}
+
+static inline int rts5229_set_cfg_phy(struct rtsx_chip *chip,
+				      u16 autoload_addr, u8 phy_addr,
+				      u16 data)
+{
+	RTSX_WRITE_REG(chip, autoload_addr, 0xFF, (u8) data);
+	RTSX_WRITE_REG(chip, autoload_addr + 1, 0xFF, (u8) (data >> 8));
+	RTSX_WRITE_REG(chip, autoload_addr + 2, 0xFF, phy_addr);
+	RTSX_WRITE_REG(chip, autoload_addr + 3, 0xC0, 0x40);
+
+	return STATUS_SUCCESS;
+}
+
+static inline int rts5229_auto_load(struct rtsx_chip *chip, u32 relink_time,
+				    struct rts5229_auto_load_map *map_item,
+				    u8 cnt)
+{
+	int retval;
+	u16 autoload_addr;
+	u8 i, mask;
+
+	if (CHECK_PID(chip, 0x5229)) {
+		mask = 0x3F;
+	} else {
+		mask = 0x01;
+		relink_time >>= 4;
+
+		/* RTS5227 uses a bit (bit 7 of cfg address 0x817)
+		 * to lock/unlock BIOS config function, which must be
+		 * enabled before setting autoload structure
+		 */
+		retval =
+		    rtsx_write_cfg_dw(chip, 0, 0x814, 0xFF000000, 0x80000000);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, 0xFF00, 0xFF, (cnt & 0x7F) | 0x80);
+	RTSX_WRITE_REG(chip, 0xFF01, 0xFF, (u8) relink_time);
+	RTSX_WRITE_REG(chip, 0xFF02, 0xFF, (u8) (relink_time >> 8));
+	RTSX_WRITE_REG(chip, 0xFF03, mask, (u8) (relink_time >> 16));
+
+	RTSX_WRITE_REG(chip, 0xFF04, 0xFF, (u8) chip->ssvid);
+	RTSX_WRITE_REG(chip, 0xFF05, 0xFF, (u8) (chip->ssvid >> 8));
+	RTSX_WRITE_REG(chip, 0xFF06, 0xFF, (u8) chip->ssdid);
+	RTSX_WRITE_REG(chip, 0xFF07, 0xFF, (u8) (chip->ssdid >> 8));
+
+	autoload_addr = 0xFF08;
+	for (i = 0; i < cnt; i++) {
+		if (map_item[i].type == CFG_EPCORE) {
+			RTSX_DEBUGP(("CFG_EPCORE: cfg_addr = 0x%04x, "
+				     "func = %d, mode = 0x%x, data = 0x%02x\n",
+				     map_item[i].item.cfg_epcore.cfg_addr,
+				     map_item[i].item.cfg_epcore.func,
+				     map_item[i].item.cfg_epcore.mode,
+				     map_item[i].item.cfg_epcore.data));
+
+			retval = rts5229_set_cfg_epcore(chip, autoload_addr,
+							map_item[i].item.
+							cfg_epcore.cfg_addr,
+							map_item[i].item.
+							cfg_epcore.func,
+							map_item[i].item.
+							cfg_epcore.mode,
+							map_item[i].item.
+							cfg_epcore.data);
+			if (retval != STATUS_SUCCESS)
+				TRACE_RET(chip, STATUS_FAIL);
+
+			autoload_addr += 4;
+		} else if (map_item[i].type == CFG_PHY) {
+			RTSX_DEBUGP(("CFG_PHY: phy_addr = 0x%02x, data = 0x%04x\n", map_item[i].item.cfg_phy.phy_addr, map_item[i].item.cfg_phy.data));
+
+			retval = rts5229_set_cfg_phy(chip, autoload_addr,
+						     map_item[i].item.cfg_phy.
+						     phy_addr,
+						     map_item[i].item.cfg_phy.
+						     data);
+			if (retval != STATUS_SUCCESS)
+				TRACE_RET(chip, STATUS_FAIL);
+
+			autoload_addr += 4;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static inline void rts5229_init_cfg_epcore(struct rtsx_chip *chip,
+					   u16 cfg_addr, u8 func, u8 data)
+{
+	int i;
+
+	if (chip->al_map_cnt >= AL_MAP_MAX_CNT)
+		return;
+	i = chip->al_map_cnt++;
+
+	chip->al_map[i].type = CFG_EPCORE;
+	chip->al_map[i].item.cfg_epcore.cfg_addr = cfg_addr & 0xFFFC;
+	chip->al_map[i].item.cfg_epcore.func = func;
+	chip->al_map[i].item.cfg_epcore.mode = (u8) 0x01 << (cfg_addr & 0x03);
+	chip->al_map[i].item.cfg_epcore.data = data;
+}
+
+static inline void rts5229_init_cfg_phy(struct rtsx_chip *chip, u8 phy_addr,
+					u16 data)
+{
+	int i;
+
+	if (chip->al_map_cnt >= AL_MAP_MAX_CNT)
+		return;
+	i = chip->al_map_cnt++;
+
+	chip->al_map[i].type = CFG_PHY;
+	chip->al_map[i].item.cfg_phy.phy_addr = phy_addr;
+	chip->al_map[i].item.cfg_phy.data = data;
+}
+
+static int rtsx_init_from_hw(struct rtsx_chip *chip)
+{
+	int retval;
+	u32 lval;
+	u8 val;
+
+	val = rtsx_readb(chip, 0x1C);
+	if ((val & 0x10) == 0) {
+		chip->asic_code = 1;
+	} else {
+		chip->asic_code = 0;
+	}
+
+	retval = rtsx_read_register(chip, 0xFE90, &val);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+	RTSX_DEBUGP(("0xFE90: 0x%x\n", val));
+	chip->ic_version = val & 0x0F;
+
+	if (!chip->use_hw_setting)
+		return STATUS_SUCCESS;
+
+	retval = rtsx_read_cfg_dw(chip, 0, 0x724, &lval);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+	RTSX_DEBUGP(("dw in 0x724: 0x%x\n", lval));
+
+	val = (u8) (lval >> 24);
+	if ((val & 0x01) == 0) {
+		u8 lun_mode[4] = {
+			0xFF,
+			MS_LUN,
+			SD_LUN,
+			DEFAULT_SINGLE,
+		};
+		u8 sd_drive[4] = {
+			0x01,
+			0x02,
+			0x05,
+			0x03
+		};
+		u8 ssc_depth1[4] = {
+			SSC_DEPTH_512K,
+			SSC_DEPTH_1M,
+			SSC_DEPTH_2M,
+			SSC_DEPTH_4M,
+		};
+		u8 ssc_depth2[4] = {
+			SSC_DEPTH_256K,
+			SSC_DEPTH_512K,
+			SSC_DEPTH_1M,
+			SSC_DEPTH_2M,
+		};
+
+		rts5229_init_cfg_epcore(chip, 0x727, 0, val);
+
+		chip->lun_mode = lun_mode[(val >> 6) & 0x03];
+		chip->aspm_l0s_l1_en = (val >> 4) & 0x03;
+		chip->sd30_drive_sel_1v8 = sd_drive[(val >> 2) & 0x03];
+		chip->card_drive_sel &= 0x3F;
+		chip->card_drive_sel |= ((val >> 1) & 0x01) << 6;
+
+		val = (u8) (lval >> 16);
+		rts5229_init_cfg_epcore(chip, 0x726, 0, val);
+
+		if ((val & 0xC0) != 0xC0) {
+			chip->asic_sd_hs_clk =
+			    (49 - ((val >> 6) & 0x03) * 2) * 2;
+			chip->asic_mmc_52m_clk = chip->asic_sd_hs_clk;
+		}
+		chip->sdr50_en = (val >> 5) & 0x01;
+		chip->ddr50_en = (val >> 4) & 0x01;
+		chip->sdr104_en = (val >> 3) & 0x01;
+		if ((val & 0x07) != 0x07)
+			chip->asic_ms_hg_clk = (59 - (val & 0x07)) * 2;
+
+		val = (u8) (lval >> 8);
+		rts5229_init_cfg_epcore(chip, 0x725, 0, val);
+
+		if ((val & 0xE0) != 0xE0)
+			chip->asic_sd_sdr104_clk =
+			    206 - ((val >> 5) & 0x07) * 3;
+		if ((val & 0x1C) != 0x1C)
+			chip->asic_sd_sdr50_clk =
+			    98 - ((val >> 2) & 0x07) * 2;
+		if ((val & 0x03) != 0x03)
+			chip->asic_sd_ddr50_clk = (48 - (val & 0x03) * 2) * 2;
+
+		val = (u8) lval;
+		rts5229_init_cfg_epcore(chip, 0x724, 0, val);
+
+		chip->ssc_depth_sd_sdr104 = ssc_depth1[(val >> 6) & 0x03];
+		chip->ssc_depth_sd_sdr50 = chip->ssc_depth_sd_sdr104;
+		chip->ssc_depth_sd_ddr50 = ssc_depth1[(val >> 4) & 0x03];
+		chip->ssc_depth_sd_hs = ssc_depth2[(val >> 2) & 0x03];
+		chip->ssc_depth_mmc_52m = chip->ssc_depth_sd_hs;
+		chip->ssc_depth_ms_hg = ssc_depth2[val & 0x03];
+
+		retval = rtsx_read_cfg_dw(chip, 0, 0x814, &lval);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+		RTSX_DEBUGP(("dw in 0x814: 0x%x\n", lval));
+
+		val = (u8) lval;
+		rts5229_init_cfg_epcore(chip, 0x814, 0, val);
+
+		if (chip->auto_delink_en != 2)
+			chip->auto_delink_en = ! !(val & 0x80);
+		chip->sd30_drive_sel_3v3 = sd_drive[(val >> 5) & 0x03];
+		if (val & 0x10)
+			chip->support_card &= ~SUPPORT_MMC;
+		else
+			chip->support_card |= SUPPORT_MMC;
+		chip->led_always_on = ! !(val & 0x08);
+		chip->obff_en = ! !(val & 0x04);
+	}
+
+	if (chip->hp_watch_bios_hotplug && CHK_AUTODELINK_EN(chip)) {
+		u8 reg58, reg5b;
+
+		if (rtsx_read_pci_cfg_byte
+		    (chip, 0x00, 0x1C, 0x02, 0x58, &reg58) < 0) {
+			return STATUS_SUCCESS;
+		}
+		if (rtsx_read_pci_cfg_byte
+		    (chip, 0x00, 0x1C, 0x02, 0x5B, &reg5b) < 0) {
+			return STATUS_SUCCESS;
+		}
+
+		RTSX_DEBUGP(("reg58 = 0x%x, reg5b = 0x%x\n", reg58, reg5b));
+
+		if ((reg58 == 0x00) && (reg5b == 0x01)) {
+			chip->auto_delink_en = 0;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_init_chip(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	struct ms_info *ms_card = &(chip->ms_card);
+	int retval;
+	unsigned int i;
+
+	RTSX_DEBUGP(("VID: 0x%04x, PID: 0x%04x, SSVID: 0x%04x, SSDID: 0x%04x\n", chip->vendor_id, chip->product_id, chip->ssvid, chip->ssdid));
+
+	chip->ic_version = 0;
+
+#ifdef _MSG_TRACE
+	chip->msg_idx = 0;
+#endif
+
+	memset(sd_card, 0, sizeof(struct sd_info));
+	memset(ms_card, 0, sizeof(struct ms_info));
+
+	chip->sd_reset_counter = 0;
+	chip->ms_reset_counter = 0;
+
+	chip->sd_show_cnt = chip->cd_max_show_cnt;
+	chip->ms_show_cnt = chip->cd_max_show_cnt;
+
+	chip->auto_delink_cnt = 0;
+	chip->auto_delink_allowed = 1;
+	rtsx_set_stat(chip, RTSX_STAT_INIT);
+
+	chip->ltr_enabled = 0;
+	chip->ltr_active = 0;
+	chip->aspm_enabled = 0;
+	chip->cur_card = 0;
+	chip->phy_debug_mode = 0;
+	chip->sd20_mode = 0;
+	chip->sd_retune_clock = 0;
+	chip->led_test_mode = 0;
+	chip->al_map_cnt = 0;
+
+	for (i = 0; i < MAX_ALLOWED_LUN_CNT; i++) {
+		set_sense_type(chip, i, SENSE_TYPE_NO_SENSE);
+		chip->rw_fail_cnt[i] = 0;
+	}
+
+	if (!check_sd_speed_prior(chip->sd_speed_prior)) {
+		chip->sd_speed_prior = 0x01040203;
+	}
+	RTSX_DEBUGP(("sd_speed_prior = 0x%08x\n", chip->sd_speed_prior));
+
+	if (!check_sd_current_prior(chip->sd_current_prior)) {
+		chip->sd_current_prior = 0x00010203;
+	}
+	RTSX_DEBUGP(("sd_current_prior = 0x%08x\n", chip->sd_current_prior));
+
+	if ((chip->sd_ddr_tx_phase > 31) || (chip->sd_ddr_tx_phase < 0)) {
+		chip->sd_ddr_tx_phase = 0;
+	}
+	if ((chip->mmc_ddr_tx_phase > 31) || (chip->mmc_ddr_tx_phase < 0)) {
+		chip->mmc_ddr_tx_phase = 0;
+	}
+
+	RTSX_WRITE_REG(chip, FPDCTL, SSC_POWER_DOWN, 0);
+	udelay(200);
+
+	RTSX_WRITE_REG(chip, CLK_DIV, 0x07, 0x07);
+
+	retval = rtsx_init_from_hw(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	chip->ss_en = 0;
+
+	RTSX_DEBUGP(("chip->asic_code = %d\n", chip->asic_code));
+	RTSX_DEBUGP(("chip->use_hw_setting = %d\n", chip->use_hw_setting));
+	RTSX_DEBUGP(("chip->ic_version = 0x%x\n", chip->ic_version));
+	RTSX_DEBUGP(("chip->aux_pwr_exist = %d\n", chip->aux_pwr_exist));
+	RTSX_DEBUGP(("chip->aspm_l0s_l1_en = %d\n", chip->aspm_l0s_l1_en));
+	RTSX_DEBUGP(("chip->dynamic_aspm = %d\n", chip->dynamic_aspm));
+	RTSX_DEBUGP(("chip->auto_delink_en = %d\n", chip->auto_delink_en));
+	RTSX_DEBUGP(("chip->ss_en = %d\n", chip->ss_en));
+	RTSX_DEBUGP(("chip->lun_mode = %d\n", chip->lun_mode));
+	RTSX_DEBUGP(("chip->card_drive_sel = 0x%x\n", chip->card_drive_sel));
+	if (CHECK_PID(chip, 0x5229)) {
+		RTSX_DEBUGP(("chip->sd30_drive_sel_1v8 = 0x%x\n",
+			     chip->sd30_drive_sel_1v8));
+		RTSX_DEBUGP(("chip->sd30_drive_sel_3v3 = 0x%x\n",
+			     chip->sd30_drive_sel_3v3));
+	} else {
+		RTSX_DEBUGP(("chip->sd30_clk_drive_sel_1v8 = 0x%x\n",
+			     chip->sd30_clk_drive_sel_1v8));
+		RTSX_DEBUGP(("chip->sd30_cmd_drive_sel_1v8 = 0x%x\n",
+			     chip->sd30_cmd_drive_sel_1v8));
+		RTSX_DEBUGP(("chip->sd30_dat_drive_sel_1v8 = 0x%x\n",
+			     chip->sd30_dat_drive_sel_1v8));
+		RTSX_DEBUGP(("chip->sd30_clk_drive_sel_3v3 = 0x%x\n",
+			     chip->sd30_clk_drive_sel_3v3));
+		RTSX_DEBUGP(("chip->sd30_cmd_drive_sel_3v3 = 0x%x\n",
+			     chip->sd30_cmd_drive_sel_3v3));
+		RTSX_DEBUGP(("chip->sd30_dat_drive_sel_3v3 = 0x%x\n",
+			     chip->sd30_dat_drive_sel_3v3));
+	}
+	RTSX_DEBUGP(("chip->sdr50_en = %d\n", chip->sdr50_en));
+	RTSX_DEBUGP(("chip->ddr50_en = %d\n", chip->ddr50_en));
+	RTSX_DEBUGP(("chip->sdr104_en = %d\n", chip->sdr104_en));
+	RTSX_DEBUGP(("chip->asic_sd_hs_clk = %d\n", chip->asic_sd_hs_clk));
+	RTSX_DEBUGP(("chip->asic_mmc_52m_clk = %d\n",
+		     chip->asic_mmc_52m_clk));
+	RTSX_DEBUGP(("chip->asic_ms_hg_clk = %d\n", chip->asic_ms_hg_clk));
+	RTSX_DEBUGP(("chip->asic_sd_sdr104_clk = %d\n",
+		     chip->asic_sd_sdr104_clk));
+	RTSX_DEBUGP(("chip->asic_sd_sdr50_clk = %d\n",
+		     chip->asic_sd_sdr50_clk));
+	RTSX_DEBUGP(("chip->asic_sd_ddr50_clk = %d\n",
+		     chip->asic_sd_ddr50_clk));
+	RTSX_DEBUGP(("chip->ssc_depth_sd_sdr104 = %d\n",
+		     chip->ssc_depth_sd_sdr104));
+	RTSX_DEBUGP(("chip->ssc_depth_sd_sdr50 = %d\n",
+		     chip->ssc_depth_sd_sdr50));
+	RTSX_DEBUGP(("chip->ssc_depth_sd_ddr50 = %d\n",
+		     chip->ssc_depth_sd_ddr50));
+	RTSX_DEBUGP(("chip->ssc_depth_sd_hs = %d\n", chip->ssc_depth_sd_hs));
+	RTSX_DEBUGP(("chip->ssc_depth_mmc_52m = %d\n",
+		     chip->ssc_depth_mmc_52m));
+	RTSX_DEBUGP(("chip->ssc_depth_ms_hg = %d\n", chip->ssc_depth_ms_hg));
+
+	chip->card2lun[SD_CARD] = 0;
+	chip->card2lun[MS_CARD] = 0;
+
+	if (CHECK_LUN_MODE(chip, DEFAULT_SINGLE))
+		chip->lun2card[0] = SD_CARD | MS_CARD;
+	else if (CHECK_LUN_MODE(chip, SD_LUN))
+		chip->lun2card[0] = SD_CARD;
+	else if (CHECK_LUN_MODE(chip, MS_LUN))
+		chip->lun2card[0] = MS_CARD;
+	else
+		TRACE_RET(chip, STATUS_FAIL);
+
+	chip->max_lun = 0;
+
+	if (chip->support_card == 0)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	if (!(chip->support_card & SUPPORT_SDMMC)) {
+		chip->card2lun[SD_CARD] = 0xFF;
+		chip->lun2card[0] &= ~SD_CARD;
+	}
+
+	if (!(chip->support_card & SUPPORT_MS)) {
+		chip->card2lun[MS_CARD] = 0xFF;
+		chip->lun2card[0] &= ~MS_CARD;
+	}
+
+	if (chip->asic_code) {
+		rts5229_init_cfg_phy(chip, 0x00, chip->phy_pcr);
+		rts5229_init_cfg_phy(chip, 0x03, chip->phy_rcr2);
+		rts5229_init_cfg_phy(chip, 0x19, 0xFE6C);
+		rts5229_init_cfg_phy(chip, 0x0A, 0x05C0);
+	}
+
+	retval = rts5229_auto_load(chip, chip->relink_time,
+				   chip->al_map, (u8) chip->al_map_cnt);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	retval = rtsx_reset_chip(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void rtsx_release_chip(struct rtsx_chip *chip)
+{
+	ms_free_l2p_tbl(chip);
+	chip->card_exist = 0;
+	chip->card_ready = 0;
+}
+
+#ifndef LED_AUTO_BLINK
+static inline void rtsx_blink_led(struct rtsx_chip *chip)
+{
+	if (chip->card_exist && chip->blink_led) {
+		if (chip->led_toggle_counter < LED_TOGGLE_INTERVAL) {
+			chip->led_toggle_counter++;
+		} else {
+			chip->led_toggle_counter = 0;
+			toggle_led(chip);
+		}
+	}
+}
+#endif
+
+void rtsx_polling_func(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &(chip->sd_card);
+#endif
+	int ss_allowed;
+	u32 bar0 = 0;
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND)) {
+		return;
+	}
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK)) {
+		goto Delink_Stage;
+	}
+
+	if (chip->polling_config) {
+		u8 val;
+		rtsx_read_config_byte(chip, 0, &val);
+	}
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_SS)) {
+		return;
+	}
+#ifdef SUPPORT_OCP
+	if (chip->ocp_int) {
+		u16 ocp_stat;
+
+		if (CHECK_PID(chip, 0x5229))
+			ocp_stat = OCPSTAT;
+		else
+			ocp_stat = RTS5227_OCPSTAT;
+
+		rtsx_read_register(chip, ocp_stat, &(chip->ocp_stat));
+
+		if (chip->card_exist & SD_CARD) {
+			sd_power_off_card3v3(chip);
+		} else if (chip->card_exist & MS_CARD) {
+			ms_power_off_card3v3(chip);
+		}
+
+		chip->ocp_int = 0;
+	}
+#endif
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_erase_status) {
+		if (chip->card_exist & SD_CARD) {
+			u8 val;
+			rtsx_read_register(chip, SD_BUS_STAT, &val);
+			if (val & SD_DAT0_STATUS) {
+				sd_card->sd_erase_status = SD_NOT_ERASE;
+				sd_card->sd_lock_notify = 1;
+				chip->need_reinit |= SD_CARD;
+			}
+		} else {
+			sd_card->sd_erase_status = SD_NOT_ERASE;
+		}
+	}
+#endif
+
+	rtsx_init_cards(chip);
+
+	if (chip->ss_en) {
+		ss_allowed = 1;
+	} else {
+		ss_allowed = 0;
+	}
+
+	if (ss_allowed) {
+		if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
+			chip->ss_counter = 0;
+		} else {
+			if (chip->ss_counter <
+			    (chip->ss_idle_period / POLLING_INTERVAL)) {
+				chip->ss_counter++;
+			} else {
+				rtsx_exclusive_enter_ss(chip);
+				return;
+			}
+		}
+	}
+
+	rtsx_read_config_dword(chip, 0x10, &bar0);
+	if (!bar0) {
+		RTSX_DEBUGP(("Bar0 not valid\n"));
+		rtsx_set_stat(chip, RTSX_STAT_DISCONNECT);
+		notify_refresh_driver(chip);
+		return;
+	}
+
+	if (chip->idle_counter < IDLE_MAX_COUNT) {
+		chip->idle_counter++;
+	} else {
+		if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
+			RTSX_DEBUGP(("Idle state!\n"));
+			rtsx_set_stat(chip, RTSX_STAT_IDLE);
+
+#ifdef LED_AUTO_BLINK
+			if (!chip->led_test_mode)
+				disable_auto_blink(chip);
+#else
+			chip->led_toggle_counter = 0;
+#endif
+			rtsx_force_power_on(chip, SSC_PDCTL);
+
+			if (!chip->led_test_mode) {
+				if (chip->led_always_on && chip->card_ready
+				    && chip->blink_led) {
+					turn_on_led(chip);
+				} else {
+					turn_off_led(chip);
+				}
+			}
+
+			if (chip->ltr_enabled) {
+				RTSX_DEBUGP(("Set LTR_CTL to 0x83\n"));
+				rtsx_write_register(chip, LTR_CTL, 0xFF,
+						    0x83);
+				chip->ltr_active = 0;
+			}
+
+			if (chip->auto_power_down && !chip->card_ready) {
+				rtsx_force_power_down(chip,
+						      SSC_PDCTL | OC_PDCTL);
+			}
+		}
+	}
+
+	switch (rtsx_get_stat(chip)) {
+	case RTSX_STAT_RUN:
+#ifndef LED_AUTO_BLINK
+		if (!chip->led_test_mode)
+			rtsx_blink_led(chip);
+#endif
+		do_remaining_work(chip);
+		break;
+
+	case RTSX_STAT_IDLE:
+		rtsx_enable_aspm(chip);
+		break;
+
+	default:
+		break;
+	}
+
+#ifdef SUPPORT_OCP
+	if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
+		RTSX_DEBUGP(("Over current, OCPSTAT is 0x%x\n",
+			     chip->ocp_stat));
+		if (chip->card_exist & SD_CARD) {
+			rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+			chip->card_fail |= SD_CARD;
+		} else if (chip->card_exist & MS_CARD) {
+			rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+			chip->card_fail |= MS_CARD;
+		}
+		card_power_off(chip, SD_CARD);
+	}
+#endif
+
+ Delink_Stage:
+	if (CHK_AUTODELINK_EN(chip) && chip->auto_delink_allowed &&
+	    !chip->card_ready && !chip->card_ejected) {
+		int enter_L1 = chip->auto_delink_in_L1
+		    && (chip->aspm_l0s_l1_en || chip->ss_en);
+		int delink_stage1_cnt = chip->delink_stage1_step;
+		int delink_stage2_cnt =
+		    delink_stage1_cnt + chip->delink_stage2_step;
+		int delink_stage3_cnt =
+		    delink_stage2_cnt + chip->delink_stage3_step;
+
+		if (chip->auto_delink_cnt <= delink_stage3_cnt) {
+			if (chip->auto_delink_cnt == delink_stage1_cnt) {
+				rtsx_set_stat(chip, RTSX_STAT_DELINK);
+
+				clear_first_install_mark(chip);
+
+				if (chip->card_exist) {
+					RTSX_DEBUGP(("False card inserted, do force delink\n"));
+
+					if (enter_L1) {
+						rtsx_write_register(chip,
+								    HOST_SLEEP_STATE,
+								    0x03, 1);
+					}
+					rtsx_write_register(chip,
+							    CHANGE_LINK_STATE,
+							    0x0A, 0x0A);
+					if (CHECK_PID(chip, 0x5227))
+						rtsx_write_register(chip,
+								    AUTOLOAD_CFG,
+								    0x40,
+								    0x00);
+
+					if (enter_L1) {
+						rtsx_enter_L1(chip);
+					}
+
+					chip->auto_delink_cnt =
+					    delink_stage3_cnt + 1;
+				} else {
+					RTSX_DEBUGP(("No card inserted, do delink\n"));
+
+					if (enter_L1) {
+						rtsx_write_register(chip,
+								    HOST_SLEEP_STATE,
+								    0x03, 1);
+					}
+#ifdef HW_INT_WRITE_CLR
+					rtsx_writel(chip, RTSX_BIPR,
+						    0xFFFFFFFF);
+					RTSX_DEBUGP(("RTSX_BIPR: 0x%x\n",
+						     rtsx_readl(chip,
+								RTSX_BIPR)));
+#endif
+					rtsx_write_register(chip,
+							    CHANGE_LINK_STATE,
+							    0x02, 0x02);
+					if (CHECK_PID(chip, 0x5227))
+						rtsx_write_register(chip,
+								    AUTOLOAD_CFG,
+								    0x40,
+								    0x00);
+
+					if (enter_L1) {
+						rtsx_enter_L1(chip);
+					}
+				}
+			}
+
+			if (chip->auto_delink_cnt == delink_stage2_cnt) {
+				RTSX_DEBUGP(("Try to do force delink\n"));
+
+				if (enter_L1) {
+					rtsx_exit_L1(chip);
+				}
+
+				rtsx_write_register(chip, CHANGE_LINK_STATE,
+						    0x0A, 0x0A);
+			}
+
+			if (chip->auto_delink_cnt == delink_stage3_cnt) {
+				RTSX_DEBUGP(("Notify user space to refresh driver\n"));
+				notify_refresh_driver(chip);
+			}
+
+			chip->auto_delink_cnt++;
+		}
+	} else {
+		chip->auto_delink_cnt = 0;
+	}
+}
+
+void rtsx_undo_delink(struct rtsx_chip *chip)
+{
+	chip->auto_delink_allowed = 0;
+	rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0x00);
+}
+
+/**
+ * rtsx_stop_cmd - stop command transfer and DMA transfer
+ * @chip: Realtek's card reader chip
+ * @card: flash card type
+ *
+ * Stop command transfer and DMA transfer.
+ * This function is called in error handler.
+ */
+void rtsx_stop_cmd(struct rtsx_chip *chip, int card)
+{
+	int i;
+
+	for (i = 0; i <= 8; i++) {
+		int addr = RTSX_HCBAR + i * 4;
+		u32 reg;
+		reg = rtsx_readl(chip, addr);
+		RTSX_DEBUGP(("BAR (0x%02x): 0x%08x\n", addr, reg));
+	}
+	rtsx_writel(chip, RTSX_HCBCTLR, STOP_CMD);
+	rtsx_writel(chip, RTSX_HDBCTLR, STOP_DMA);
+
+	for (i = 0; i < 16; i++) {
+		u16 addr = 0xFE20 + (u16) i;
+		u8 val;
+		rtsx_read_register(chip, addr, &val);
+		RTSX_DEBUGP(("0x%04X: 0x%02x\n", addr, val));
+	}
+
+	rtsx_write_register(chip, DMACTL, 0x80, 0x80);
+	rtsx_write_register(chip, RBCTL, 0x80, 0x80);
+}
+
+#define MAX_RW_REG_CNT		1024
+
+int rtsx_write_register(struct rtsx_chip *chip, u16 addr, u8 mask, u8 data)
+{
+	int i;
+	u32 val = 3 << 30;
+
+	val |= (u32) (addr & 0x3FFF) << 16;
+	val |= (u32) mask << 8;
+	val |= (u32) data;
+
+	rtsx_writel(chip, RTSX_HAIMR, val);
+
+	for (i = 0; i < MAX_RW_REG_CNT; i++) {
+		val = rtsx_readl(chip, RTSX_HAIMR);
+		if ((val & (1 << 31)) == 0) {
+			if (data != (u8) val) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			return STATUS_SUCCESS;
+		}
+	}
+
+	TRACE_RET(chip, STATUS_TIMEDOUT);
+}
+
+int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 * data)
+{
+	u32 val = 2 << 30;
+	int i;
+
+	if (data) {
+		*data = 0;
+	}
+
+	val |= (u32) (addr & 0x3FFF) << 16;
+
+	rtsx_writel(chip, RTSX_HAIMR, val);
+
+	for (i = 0; i < MAX_RW_REG_CNT; i++) {
+		val = rtsx_readl(chip, RTSX_HAIMR);
+		if ((val & (1 << 31)) == 0) {
+			break;
+		}
+	}
+
+	if (i >= MAX_RW_REG_CNT) {
+		TRACE_RET(chip, STATUS_TIMEDOUT);
+	}
+
+	if (data) {
+		*data = (u8) (val & 0xFF);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
+		      u32 val)
+{
+	u8 mode = 0, tmp;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		if (mask & 0xFF) {
+			RTSX_WRITE_REG(chip, CFGDATA0 + i, 0xFF,
+				       (u8) (val & mask & 0xFF));
+			mode |= (1 << i);
+		}
+		mask >>= 8;
+		val >>= 8;
+	}
+
+	if (mode) {
+		RTSX_WRITE_REG(chip, CFGADDR0, 0xFF, (u8) addr);
+		RTSX_WRITE_REG(chip, CFGADDR1, 0xFF, (u8) (addr >> 8));
+
+		RTSX_WRITE_REG(chip, CFGRWCTL, 0xFF,
+			       0x80 | mode | ((func_no & 0x03) << 4));
+
+		for (i = 0; i < MAX_RW_REG_CNT; i++) {
+			RTSX_READ_REG(chip, CFGRWCTL, &tmp);
+			if ((tmp & 0x80) == 0) {
+				break;
+			}
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 * val)
+{
+	int i;
+	u8 tmp;
+	u32 data = 0;
+
+	RTSX_WRITE_REG(chip, CFGADDR0, 0xFF, (u8) addr);
+	RTSX_WRITE_REG(chip, CFGADDR1, 0xFF, (u8) (addr >> 8));
+	RTSX_WRITE_REG(chip, CFGRWCTL, 0xFF, 0x80 | ((func_no & 0x03) << 4));
+
+	for (i = 0; i < MAX_RW_REG_CNT; i++) {
+		RTSX_READ_REG(chip, CFGRWCTL, &tmp);
+		if ((tmp & 0x80) == 0) {
+			break;
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		RTSX_READ_REG(chip, CFGDATA0 + i, &tmp);
+		data |= (u32) tmp << (i * 8);
+	}
+
+	if (val) {
+		*val = data;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 * buf,
+		       int len)
+{
+	u32 *data, *mask;
+	u16 offset = addr % 4;
+	u16 aligned_addr = addr - offset;
+	int dw_len, i, j;
+	int retval;
+
+	RTSX_DEBUGP(("%s\n", __FUNCTION__));
+
+	if (!buf) {
+		TRACE_RET(chip, STATUS_NOMEM);
+	}
+
+	if ((len + offset) % 4) {
+		dw_len = (len + offset) / 4 + 1;
+	} else {
+		dw_len = (len + offset) / 4;
+	}
+	RTSX_DEBUGP(("dw_len = %d\n", dw_len));
+
+	data = (u32 *) vmalloc(dw_len * 4);
+	if (!data) {
+		TRACE_RET(chip, STATUS_NOMEM);
+	}
+	memset(data, 0, dw_len * 4);
+
+	mask = (u32 *) vmalloc(dw_len * 4);
+	if (!mask) {
+		vfree(data);
+		TRACE_RET(chip, STATUS_NOMEM);
+	}
+	memset(mask, 0, dw_len * 4);
+
+	j = 0;
+	for (i = 0; i < len; i++) {
+		mask[j] |= 0xFF << (offset * 8);
+		data[j] |= buf[i] << (offset * 8);
+		if (++offset == 4) {
+			j++;
+			offset = 0;
+		}
+	}
+
+	RTSX_DUMP(mask, dw_len * 4);
+	RTSX_DUMP(data, dw_len * 4);
+
+	for (i = 0; i < dw_len; i++) {
+		retval =
+		    rtsx_write_cfg_dw(chip, func, aligned_addr + i * 4,
+				      mask[i], data[i]);
+		if (retval != STATUS_SUCCESS) {
+			vfree(data);
+			vfree(mask);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	vfree(data);
+	vfree(mask);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 * buf,
+		      int len)
+{
+	u32 *data;
+	u16 offset = addr % 4;
+	u16 aligned_addr = addr - offset;
+	int dw_len, i, j;
+	int retval;
+
+	RTSX_DEBUGP(("%s\n", __FUNCTION__));
+
+	if ((len + offset) % 4) {
+		dw_len = (len + offset) / 4 + 1;
+	} else {
+		dw_len = (len + offset) / 4;
+	}
+	RTSX_DEBUGP(("dw_len = %d\n", dw_len));
+
+	data = (u32 *) vmalloc(dw_len * 4);
+	if (!data) {
+		TRACE_RET(chip, STATUS_NOMEM);
+	}
+
+	for (i = 0; i < dw_len; i++) {
+		retval =
+		    rtsx_read_cfg_dw(chip, func, aligned_addr + i * 4,
+				     data + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(data);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if (buf) {
+		j = 0;
+
+		for (i = 0; i < len; i++) {
+			buf[i] = (u8) (data[j] >> (offset * 8));
+			if (++offset == 4) {
+				j++;
+				offset = 0;
+			}
+		}
+	}
+
+	vfree(data);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_phy_register(struct rtsx_chip *chip, u8 addr, u16 val)
+{
+	int i, finished = 0;
+	u8 tmp;
+
+	RTSX_WRITE_REG(chip, PHYDATA0, 0xFF, (u8) val);
+	RTSX_WRITE_REG(chip, PHYDATA1, 0xFF, (u8) (val >> 8));
+	RTSX_WRITE_REG(chip, PHYADDR, 0xFF, addr);
+
+	RTSX_WRITE_REG(chip, PHYRWCTL, 0xFF, 0x81);
+
+	for (i = 0; i < 100000; i++) {
+		RTSX_READ_REG(chip, PHYRWCTL, &tmp);
+		if (!(tmp & 0x80)) {
+			finished = 1;
+			break;
+		}
+	}
+
+	if (!finished) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 * val)
+{
+	int i, finished = 0;
+	u16 data = 0;
+	u8 tmp;
+
+	RTSX_WRITE_REG(chip, PHYADDR, 0xFF, addr);
+	RTSX_WRITE_REG(chip, PHYRWCTL, 0xFF, 0x80);
+
+	for (i = 0; i < 100000; i++) {
+		RTSX_READ_REG(chip, PHYRWCTL, &tmp);
+		if (!(tmp & 0x80)) {
+			finished = 1;
+			break;
+		}
+	}
+
+	if (!finished) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_READ_REG(chip, PHYDATA0, &tmp);
+	data = tmp;
+	RTSX_READ_REG(chip, PHYDATA1, &tmp);
+	data |= (u16) tmp << 8;
+
+	if (val) {
+		*val = data;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_clr_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
+{
+	int retval;
+	u16 value;
+
+	retval = rtsx_read_phy_register(chip, reg, &value);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (value & (1 << bit)) {
+		value &= ~(1 << bit);
+		retval = rtsx_write_phy_register(chip, reg, value);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
+{
+	int retval;
+	u16 value;
+
+	retval = rtsx_read_phy_register(chip, reg, &value);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	if (0 == (value & (1 << bit))) {
+		value |= (1 << bit);
+		retval = rtsx_write_phy_register(chip, reg, value);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_check_link_ready(struct rtsx_chip *chip)
+{
+	u8 val;
+
+	RTSX_READ_REG(chip, IRQSTAT0, &val);
+
+	RTSX_DEBUGP(("IRQSTAT0: 0x%x\n", val));
+	if (val & LINK_RDY_INT) {
+		RTSX_DEBUGP(("Delinked!\n"));
+
+		rtsx_write_register(chip, IRQSTAT0, LINK_RDY_INT,
+				    LINK_RDY_INT);
+
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void rtsx_handle_pm_dstate(struct rtsx_chip *chip, u8 dstate)
+{
+	RTSX_DEBUGP(("%04x set pm_dstate to %d\n", chip->product_id, dstate));
+
+	rtsx_write_config_byte(chip, 0x44, dstate);
+	rtsx_write_config_byte(chip, 0x45, 0);
+}
+
+void rtsx_enter_L1(struct rtsx_chip *chip)
+{
+	rtsx_handle_pm_dstate(chip, 2);
+}
+
+void rtsx_exit_L1(struct rtsx_chip *chip)
+{
+	rtsx_write_config_byte(chip, 0x44, 0);
+	rtsx_write_config_byte(chip, 0x45, 0);
+}
+
+void rtsx_enter_ss(struct rtsx_chip *chip)
+{
+	RTSX_DEBUGP(("Enter Selective Suspend State!\n"));
+
+	rtsx_write_register(chip, IRQSTAT0, LINK_RDY_INT, LINK_RDY_INT);
+
+	if (chip->power_down_in_ss) {
+		rtsx_power_off_card(chip);
+
+		rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+	}
+
+	if (CHK_AUTODELINK_EN(chip)) {
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x01, 0x01);
+	} else {
+		if (!chip->phy_debug_mode) {
+			u32 tmp;
+			tmp = rtsx_readl(chip, RTSX_BIER);
+			tmp |= CARD_INT;
+			rtsx_writel(chip, RTSX_BIER, tmp);
+		}
+
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 0);
+	}
+
+	rtsx_enter_L1(chip);
+
+	RTSX_CLR_DELINK(chip);
+
+	rtsx_set_stat(chip, RTSX_STAT_SS);
+}
+
+void rtsx_exit_ss(struct rtsx_chip *chip)
+{
+	RTSX_DEBUGP(("Exit Selective Suspend State!\n"));
+
+	rtsx_exit_L1(chip);
+
+	if (chip->power_down_in_ss) {
+		rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+
+		udelay(1000);
+	}
+
+	if (RTSX_TST_DELINK(chip)) {
+		chip->need_reinit = SD_CARD | MS_CARD;
+		rtsx_reinit_cards(chip, 1);
+		RTSX_CLR_DELINK(chip);
+	} else if (chip->power_down_in_ss) {
+		chip->need_reinit = SD_CARD | MS_CARD;
+		rtsx_reinit_cards(chip, 0);
+	}
+}
+
+int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
+{
+	u32 status, int_enable;
+	int exit_ss = 0;
+#ifdef SUPPORT_OCP
+	u32 ocp_int = 0;
+
+	ocp_int = SD_OC_INT;
+#endif
+
+	if (chip->ss_en) {
+		chip->ss_counter = 0;
+		if (rtsx_get_stat(chip) == RTSX_STAT_SS) {
+			exit_ss = 1;
+			rtsx_exit_L1(chip);
+
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+		}
+	}
+
+	int_enable = rtsx_readl(chip, RTSX_BIER);
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+#ifdef HW_INT_WRITE_CLR
+	rtsx_writel(chip, RTSX_BIPR, chip->int_reg);
+#endif
+
+	if (((chip->int_reg & int_enable) == 0)
+	    || (chip->int_reg == 0xFFFFFFFF)) {
+		return STATUS_FAIL;
+	}
+
+	status = chip->int_reg &= (int_enable | 0x7FFFFF);
+
+	if (status & CARD_INT) {
+		chip->auto_delink_cnt = 0;
+
+		if (status & SD_INT) {
+			if (status & SD_EXIST) {
+				RTSX_MSG_IN_INT(("Insert SD\n"));
+				set_bit(SD_NR, &(chip->need_reset));
+			} else {
+				RTSX_MSG_IN_INT(("Remove SD\n"));
+				set_bit(SD_NR, &(chip->need_release));
+				chip->sd_reset_counter = 0;
+				chip->sd_show_cnt = 0;
+				clear_bit(SD_NR, &(chip->need_reset));
+			}
+		} else {
+			if (exit_ss && (status & SD_EXIST)) {
+				set_bit(SD_NR, &(chip->need_reinit));
+			}
+		}
+
+		if (status & MS_INT) {
+			if (status & MS_EXIST) {
+				RTSX_MSG_IN_INT(("Insert MS\n"));
+				set_bit(MS_NR, &(chip->need_reset));
+			} else {
+				RTSX_MSG_IN_INT(("Remove MS\n"));
+				set_bit(MS_NR, &(chip->need_release));
+				chip->ms_reset_counter = 0;
+				chip->ms_show_cnt = 0;
+				clear_bit(MS_NR, &(chip->need_reset));
+			}
+		} else {
+			if (exit_ss && (status & MS_EXIST)) {
+				set_bit(MS_NR, &(chip->need_reinit));
+			}
+		}
+	}
+#ifdef SUPPORT_OCP
+	chip->ocp_int = ocp_int & status;
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat)
+{
+	int retval;
+
+	RTSX_DEBUGP(("rtsx_do_before_power_down, pm_stat = %d\n", pm_stat));
+
+	rtsx_set_stat(chip, RTSX_STAT_SUSPEND);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		return;
+	}
+
+	rtsx_release_cards(chip);
+	rtsx_disable_bus_int(chip);
+	if (chip->blink_led)
+		turn_off_led(chip);
+
+	rtsx_write_register(chip, PETXCFG, 0x08, 0x08);
+
+	if (pm_stat == PM_S1) {
+		RTSX_DEBUGP(("Host enter S1\n"));
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03,
+				    HOST_ENTER_S1);
+	} else if (pm_stat == PM_S3) {
+		if (chip->s3_pwr_off_delay > 0) {
+			wait_timeout(chip->s3_pwr_off_delay);
+		}
+		RTSX_DEBUGP(("Host enter S3\n"));
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03,
+				    HOST_ENTER_S3);
+
+		if (CHECK_PID(chip, 0x5227)) {
+			rtsx_write_register(chip, 0xFF01, 0xFF, 0);
+			rtsx_write_register(chip, 0xFF02, 0xFF, 0);
+			rtsx_write_register(chip, 0xFF03, 0x01, 0);
+
+			rtsx_write_register(chip, AUTOLOAD_CFG, 0x50, 0x10);
+		}
+	}
+
+	if (chip->do_delink_before_power_down && CHK_AUTODELINK_EN(chip)) {
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 2);
+	}
+
+	rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+
+	chip->cur_clk = 0;
+	chip->cur_card = 0;
+
+	chip->card_exist = 0;
+}
+
+void rtsx_enable_aspm(struct rtsx_chip *chip)
+{
+	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
+		if (!chip->aspm_enabled) {
+			RTSX_DEBUGP(("Try to enable ASPM\n"));
+			chip->aspm_enabled = 1;
+
+			if (chip->asic_code) {
+				rtsx_write_phy_register(chip, 0x07, 0);
+			}
+			rtsx_write_config_byte(chip, LCTLR,
+					       chip->aspm_l0s_l1_en);
+
+			if (chip->config_host_aspm) {
+				rtsx_set_host_aspm(chip, chip->host_aspm_val);
+			}
+		}
+	}
+
+	return;
+}
+
+void rtsx_disable_aspm(struct rtsx_chip *chip)
+{
+	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
+		if (chip->aspm_enabled) {
+			RTSX_DEBUGP(("Try to disable ASPM\n"));
+			chip->aspm_enabled = 0;
+
+			if (chip->config_host_aspm) {
+				rtsx_disable_host_aspm(chip);
+			}
+
+			rtsx_write_config_byte(chip, LCTLR, 0x00);
+			wait_timeout(10);
+		}
+	}
+
+	return;
+}
+
+void rtsx_enter_work_state(struct rtsx_chip *chip)
+{
+	rtsx_disable_aspm(chip);
+
+	if (chip->ltr_enabled) {
+		if (!chip->ltr_active) {
+			RTSX_DEBUGP(("Set LTR_CTL to 0xA3\n"));
+			rtsx_write_register(chip, LTR_CTL, 0xFF, 0xA3);
+			chip->ltr_active = 1;
+		}
+	}
+}
+
+void rtsx_init_aspm(struct rtsx_chip *chip)
+{
+	if (chip->force_host_aspm) {
+		chip->host_aspm_val = chip->host_aspm_para;
+	} else {
+		rtsx_get_host_aspm(chip, &chip->host_aspm_val);
+		chip->host_aspm_val &= 0xCF;
+	}
+	RTSX_DEBUGP(("chip->host_aspm_val = 0x%x\n", chip->host_aspm_val));
+
+	if (chip->config_host_aspm)
+		rtsx_disable_host_aspm(chip);
+
+	rtsx_write_config_byte(chip, LCTLR, 0x00);
+}
+
+int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 * buf, int buf_len)
+{
+	int retval;
+	int i;
+	u16 reg_addr;
+	u8 *ptr;
+
+	if (!buf) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	ptr = buf;
+	reg_addr = PPBUF_BASE2;
+#ifdef USING_PPBUF
+	for (i = 0; i < buf_len / 256; i++) {
+		int j;
+
+		rtsx_init_cmd(chip);
+
+		for (j = 0; j < 256; j++) {
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
+		}
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		memcpy(ptr, rtsx_get_cmd_data(chip), 256);
+		ptr += 256;
+	}
+
+	if (buf_len % 256) {
+		rtsx_init_cmd(chip);
+
+		for (i = 0; i < buf_len % 256; i++) {
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
+		}
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+#else
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < buf_len % 256; i++) {
+		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
+	}
+
+	retval = rtsx_send_cmd(chip, 0, 250);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	memcpy(ptr, rtsx_get_cmd_data(chip), buf_len % 256);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 * buf, int buf_len)
+{
+	int retval;
+	int i;
+	u16 reg_addr;
+	u8 *ptr;
+
+	if (!buf) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+
+	ptr = buf;
+	reg_addr = PPBUF_BASE2;
+#ifdef USING_PPBUF
+	for (i = 0; i < buf_len / 256; i++) {
+		int j;
+
+		rtsx_init_cmd(chip);
+
+		for (j = 0; j < 256; j++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF,
+				     *ptr);
+			ptr++;
+		}
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if (buf_len % 256) {
+		rtsx_init_cmd(chip);
+
+		for (i = 0; i < buf_len % 256; i++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF,
+				     *ptr);
+			ptr++;
+		}
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+#else
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < buf_len % 256; i++) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF, *ptr);
+		ptr++;
+	}
+
+	retval = rtsx_send_cmd(chip, 0, 250);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_check_chip_exist(struct rtsx_chip *chip)
+{
+	if (rtsx_readl(chip, 0) == 0xFFFFFFFF) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_force_power_on(struct rtsx_chip *chip, u8 ctl)
+{
+	int retval;
+	u8 mask = 0;
+
+	if (ctl & SSC_PDCTL) {
+		mask |= SSC_POWER_DOWN;
+	}
+#ifdef SUPPORT_OCP
+	if (ctl & OC_PDCTL) {
+		mask |= SD_OC_POWER_DOWN;
+	}
+#endif
+
+	if (mask) {
+		retval = rtsx_write_register(chip, FPDCTL, mask, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_force_power_down(struct rtsx_chip *chip, u8 ctl)
+{
+	int retval;
+	u8 mask = 0, val = 0;
+
+	if (ctl & SSC_PDCTL) {
+		mask |= SSC_POWER_DOWN;
+	}
+#ifdef SUPPORT_OCP
+	if (ctl & OC_PDCTL) {
+		mask |= SD_OC_POWER_DOWN;
+	}
+#endif
+
+	if (mask) {
+		val = mask;
+		retval = rtsx_write_register(chip, FPDCTL, mask, val);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void rtsx_wait_rb_full(struct rtsx_chip *chip)
+{
+	int i;
+	u8 val;
+
+	for (i = 0; i < 1000; i++) {
+		if (rtsx_read_register(chip, RBCTL, &val)) {
+			break;
+		}
+		if (val & RB_FULL) {
+			break;
+		}
+
+		mdelay(1);
+	}
+}
+
+void rtsx_set_stat(struct rtsx_chip *chip, enum RTSX_STAT stat)
+{
+	if (stat != RTSX_STAT_IDLE)
+		chip->idle_counter = 0;
+
+	if (chip->rtsx_stat != stat)
+		chip->rtsx_stat = stat;
+}
+
+void rtsx_set_run_stat(struct rtsx_chip *chip, int enable_blink)
+{
+	chip->idle_counter = 0;
+
+	if (chip->rtsx_stat != RTSX_STAT_RUN) {
+		chip->rtsx_stat = RTSX_STAT_RUN;
+#ifdef LED_AUTO_BLINK
+		if (!chip->led_test_mode && chip->blink_led && enable_blink)
+			enable_auto_blink(chip);
+#endif
+	}
+
+}
diff --git a/drivers/staging/rts5229/rtsx_chip.h b/drivers/staging/rts5229/rtsx_chip.h
new file mode 100644
index 0000000..f9c3254
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_chip.h
@@ -0,0 +1,973 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_CHIP_H
+#define __REALTEK_RTSX_CHIP_H
+
+#include "rtsx.h"
+
+#define SUPPORT_CPRM
+#define SUPPORT_OCP
+#define SUPPORT_MAGIC_GATE
+#define SUPPORT_MSXC
+#define SUPPORT_SD_LOCK
+#define HW_INT_WRITE_CLR
+#define LED_AUTO_BLINK
+#define USING_PPBUF
+
+#ifdef SUPPORT_MAGIC_GATE
+#define MG_SET_ICV_SLOW
+#define MS_SAMPLE_INT_ERR
+#define READ_BYTES_WAIT_INT
+#endif
+
+#ifdef SUPPORT_MSXC
+#define XC_POWERCLASS
+#define SUPPORT_PCGL_1P18
+#endif
+
+#define AL_MAP_MAX_CNT		16
+
+#define LED_BLINK_SPEED		5
+#define LED_TOGGLE_INTERVAL	6
+#define	GPIO_TOGGLE_THRESHOLD   1024
+
+#define POLLING_INTERVAL	30
+
+#define TRACE_ITEM_CNT		64
+
+#ifndef STATUS_SUCCESS
+#define STATUS_SUCCESS		0
+#endif
+#ifndef STATUS_FAIL
+#define STATUS_FAIL		1
+#endif
+#ifndef STATUS_TIMEDOUT
+#define STATUS_TIMEDOUT		2
+#endif
+#ifndef STATUS_NOMEM
+#define STATUS_NOMEM		3
+#endif
+#ifndef STATUS_READ_FAIL
+#define STATUS_READ_FAIL	4
+#endif
+#ifndef STATUS_WRITE_FAIL
+#define STATUS_WRITE_FAIL	5
+#endif
+#ifndef STATUS_ERROR
+#define STATUS_ERROR		10
+#endif
+
+#define PM_S1			1
+#define PM_S3			3
+
+/*
+ * Transport return codes
+ */
+
+#define TRANSPORT_GOOD	   	0
+#define TRANSPORT_FAILED  	1
+#define TRANSPORT_NO_SENSE 	2
+#define TRANSPORT_ERROR   	3
+
+/*-----------------------------------
+    Start-Stop-Unit
+-----------------------------------*/
+#define STOP_MEDIUM			0x00
+#define MAKE_MEDIUM_READY		0x01
+#define UNLOAD_MEDIUM			0x02
+#define LOAD_MEDIUM			0x03
+
+/*-----------------------------------
+    STANDARD_INQUIRY
+-----------------------------------*/
+#define QULIFIRE                0x00
+#define AENC_FNC                0x00
+#define TRML_IOP                0x00
+#define REL_ADR                 0x00
+#define WBUS_32                 0x00
+#define WBUS_16                 0x00
+#define SYNC                    0x00
+#define LINKED                  0x00
+#define CMD_QUE                 0x00
+#define SFT_RE                  0x00
+
+#define VEN_ID_LEN              8
+#define PRDCT_ID_LEN            16
+#define PRDCT_REV_LEN           4
+
+#define RTSX_FLIDX_TRANS_ACTIVE		18
+#define RTSX_FLIDX_ABORTING		20
+#define RTSX_FLIDX_DISCONNECTING	21
+#define ABORTING_OR_DISCONNECTING	((1UL << US_FLIDX_ABORTING) | \
+					 (1UL << US_FLIDX_DISCONNECTING))
+#define RTSX_FLIDX_RESETTING		22
+#define RTSX_FLIDX_TIMED_OUT		23
+
+#define DRCT_ACCESS_DEV         0x00
+#define RMB_DISC                0x80
+#define ANSI_SCSI2              0x02
+
+#define SCSI                    0x00
+
+#define	WRITE_PROTECTED_MEDIA 0x07
+
+#define ILI                     0x20
+
+#define NO_SENSE                0x00
+#define RECOVER_ERR             0x01
+#define NOT_READY               0x02
+#define MEDIA_ERR               0x03
+#define HARDWARE_ERR            0x04
+#define ILGAL_REQ               0x05
+#define UNIT_ATTENTION          0x06
+#define DAT_PRTCT               0x07
+#define BLNC_CHK                0x08
+
+#define CPY_ABRT                0x0a
+#define ABRT_CMD                0x0b
+#define EQUAL                   0x0c
+#define VLM_OVRFLW              0x0d
+#define MISCMP                  0x0e
+
+#define READ_ERR                -1
+#define WRITE_ERR               -2
+
+#define	FIRST_RESET				0x01
+#define	USED_EXIST				0x02
+
+/*-----------------------------------
+    SENSE_DATA
+-----------------------------------*/
+
+#define SENSE_VALID             0x80
+#define SENSE_INVALID           0x00
+
+#define CUR_ERR                 0x70
+#define DEF_ERR                 0x71
+
+#define SNSKEYINFO_LEN          3
+
+#define SKSV                    0x80
+#define CDB_ILLEGAL             0x40
+#define DAT_ILLEGAL             0x00
+#define BPV                     0x08
+#define BIT_ILLEGAL0            0
+#define BIT_ILLEGAL1            1
+#define BIT_ILLEGAL2            2
+#define BIT_ILLEGAL3            3
+#define BIT_ILLEGAL4            4
+#define BIT_ILLEGAL5            5
+#define BIT_ILLEGAL6            6
+#define BIT_ILLEGAL7            7
+
+#define ASC_NO_INFO             0x00
+#define ASC_MISCMP              0x1d
+#define ASC_INVLD_CDB           0x24
+#define ASC_INVLD_PARA          0x26
+#define ASC_LU_NOT_READY	0x04
+#define ASC_WRITE_ERR           0x0c
+#define ASC_READ_ERR            0x11
+#define ASC_LOAD_EJCT_ERR       0x53
+#define	ASC_MEDIA_NOT_PRESENT	0x3A
+#define	ASC_MEDIA_CHANGED	0x28
+#define	ASC_MEDIA_IN_PROCESS	0x04
+#define	ASC_WRITE_PROTECT	0x27
+#define ASC_LUN_NOT_SUPPORTED	0x25
+
+#define ASCQ_NO_INFO            0x00
+#define	ASCQ_MEDIA_IN_PROCESS	0x01
+#define ASCQ_MISCMP             0x00
+#define ASCQ_INVLD_CDB          0x00
+#define ASCQ_INVLD_PARA         0x02
+#define ASCQ_LU_NOT_READY	0x02
+#define ASCQ_WRITE_ERR          0x02
+#define ASCQ_READ_ERR           0x00
+#define ASCQ_LOAD_EJCT_ERR      0x00
+#define	ASCQ_WRITE_PROTECT	0x00
+
+struct sense_data_t {
+	unsigned char err_code;
+
+	unsigned char seg_no;
+	unsigned char sense_key;
+
+	unsigned char info[4];
+	unsigned char ad_sense_len;
+	unsigned char cmd_info[4];
+	unsigned char asc;
+	unsigned char ascq;
+	unsigned char rfu;
+	unsigned char sns_key_info[3];
+};
+
+#define RTSX_HCBAR		0x00
+#define RTSX_HCBCTLR		0x04
+#define RTSX_HDBAR		0x08
+#define RTSX_HDBCTLR		0x0C
+#define RTSX_HAIMR		0x10
+#define RTSX_BIPR		0x14
+#define RTSX_BIER		0x18
+
+#define STOP_CMD		(0x01 << 28)
+
+#define SDMA_MODE		0x00
+#define ADMA_MODE		(0x02 << 26)
+#define STOP_DMA		(0x01 << 28)
+#define TRIG_DMA		(0x01 << 31)
+
+#define CMD_DONE_INT		(1 << 31)
+#define DATA_DONE_INT		(1 << 30)
+#define TRANS_OK_INT		(1 << 29)
+#define TRANS_FAIL_INT		(1 << 28)
+#define MS_INT			(1 << 26)
+#define SD_INT			(1 << 25)
+#define GPIO0_INT		(1 << 24)
+#define OC_INT			(1 << 23)
+#define SD_WRITE_PROTECT	(1 << 19)
+#define MS_EXIST		(1 << 17)
+#define SD_EXIST		(1 << 16)
+#define DELINK_INT		GPIO0_INT
+#define SD_OC_INT		(1 << 22)
+
+#define CARD_INT		(MS_INT | SD_INT)
+#define NEED_COMPLETE_INT	(DATA_DONE_INT | TRANS_OK_INT | TRANS_FAIL_INT)
+#define RTSX_INT		(CMD_DONE_INT | NEED_COMPLETE_INT | CARD_INT | GPIO0_INT | OC_INT)
+
+#define CARD_EXIST		(MS_EXIST | SD_EXIST)
+
+#define CMD_DONE_INT_EN		(1 << 31)
+#define DATA_DONE_INT_EN	(1 << 30)
+#define TRANS_OK_INT_EN		(1 << 29)
+#define TRANS_FAIL_INT_EN	(1 << 28)
+#define MS_INT_EN		(1 << 26)
+#define SD_INT_EN		(1 << 25)
+#define GPIO0_INT_EN		(1 << 24)
+#define OC_INT_EN		(1 << 23)
+#define DELINK_INT_EN		GPIO0_INT_EN
+#define SD_OC_INT_EN		(1 << 22)
+
+#define READ_REG_CMD		0
+#define WRITE_REG_CMD		1
+#define CHECK_REG_CMD		2
+
+#define HOST_TO_DEVICE		0
+#define DEVICE_TO_HOST		1
+
+#define RTSX_RESV_BUF_LEN	4096
+#define HOST_CMDS_BUF_LEN	1024
+#define HOST_SG_TBL_BUF_LEN	(RTSX_RESV_BUF_LEN - HOST_CMDS_BUF_LEN)
+
+#define SD_NR		2
+#define MS_NR		3
+#define SD_CARD		(1 << SD_NR)
+#define MS_CARD		(1 << MS_NR)
+
+#define MAX_ALLOWED_LUN_CNT	8
+
+#define MS_FREE_TABLE_CNT	512
+
+#define SET_BIT(data, idx)	(data) |= 1 << (idx)
+#define CLR_BIT(data, idx)	(data) &= ~(1 << (idx))
+#define CHK_BIT(data, idx)	((data) & (1 << (idx)))
+
+#define SG_INT			0x04
+#define SG_END			0x02
+#define SG_VALID		0x01
+
+#define SG_NO_OP		0x00
+#define SG_TRANS_DATA		(0x02 << 4)
+#define SG_LINK_DESC		(0x03 << 4)
+
+struct rtsx_chip;
+
+typedef int (*card_rw_func) (struct scsi_cmnd * srb, struct rtsx_chip * chip,
+			     u32 sec_addr, u16 sec_cnt);
+
+enum card_clock { CLK_20 =
+	    1, CLK_30, CLK_40, CLK_50, CLK_60, CLK_80, CLK_100, CLK_120,
+	    CLK_150, CLK_200 };
+
+enum RTSX_STAT { RTSX_STAT_INIT, RTSX_STAT_IDLE, RTSX_STAT_RUN, RTSX_STAT_SS,
+	RTSX_STAT_DELINK, RTSX_STAT_SUSPEND, RTSX_STAT_ABORT,
+	    RTSX_STAT_DISCONNECT
+};
+enum IC_VER { IC_VER_A, IC_VER_B, IC_VER_C };
+
+#define MAX_RESET_CNT		3
+
+#define MAX_DEFECTIVE_BLOCK     10
+
+struct zone_entry {
+	u16 *l2p_table;
+	u16 *free_table;
+	u16 defect_list[MAX_DEFECTIVE_BLOCK];
+	int set_index;
+	int get_index;
+	int unused_blk_cnt;
+	int disable_count;
+	int build_flag;
+};
+
+#define TYPE_SD			0x0000
+#define TYPE_MMC		0x0001
+
+#define SD_HS			0x0100
+#define SD_SDR50		0x0200
+#define SD_DDR50		0x0400
+#define SD_SDR104		0x0800
+#define SD_HCXC			0x1000
+
+#define MMC_26M			0x0100
+#define MMC_52M			0x0200
+#define MMC_4BIT		0x0400
+#define MMC_8BIT		0x0800
+#define MMC_SECTOR_MODE		0x1000
+#define MMC_DDR52		0x2000
+
+#define CHK_SD(sd_card)			(((sd_card)->sd_type & 0xFF) == TYPE_SD)
+#define CHK_SD_HS(sd_card)		(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_HS))
+#define CHK_SD_SDR50(sd_card)		(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_SDR50))
+#define CHK_SD_DDR50(sd_card)		(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_DDR50))
+#define CHK_SD_SDR104(sd_card)		(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_SDR104))
+#define CHK_SD_HCXC(sd_card)		(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_HCXC))
+#define CHK_SD_HC(sd_card)		(CHK_SD_HCXC(sd_card) && ((sd_card)->capacity <= 0x4000000))
+#define CHK_SD_XC(sd_card)		(CHK_SD_HCXC(sd_card) && ((sd_card)->capacity > 0x4000000))
+#define CHK_SD30_SPEED(sd_card)		(CHK_SD_SDR50(sd_card) || CHK_SD_DDR50(sd_card) || CHK_SD_SDR104(sd_card))
+
+#define SET_SD(sd_card)			((sd_card)->sd_type = TYPE_SD)
+#define SET_SD_HS(sd_card)		((sd_card)->sd_type |= SD_HS)
+#define SET_SD_SDR50(sd_card)		((sd_card)->sd_type |= SD_SDR50)
+#define SET_SD_DDR50(sd_card)		((sd_card)->sd_type |= SD_DDR50)
+#define SET_SD_SDR104(sd_card)		((sd_card)->sd_type |= SD_SDR104)
+#define SET_SD_HCXC(sd_card)		((sd_card)->sd_type |= SD_HCXC)
+
+#define CLR_SD_HS(sd_card)		((sd_card)->sd_type &= ~SD_HS)
+#define CLR_SD_SDR50(sd_card)		((sd_card)->sd_type &= ~SD_SDR50)
+#define CLR_SD_DDR50(sd_card)		((sd_card)->sd_type &= ~SD_DDR50)
+#define CLR_SD_SDR104(sd_card)		((sd_card)->sd_type &= ~SD_SDR104)
+#define CLR_SD_HCXC(sd_card)		((sd_card)->sd_type &= ~SD_HCXC)
+
+#define CHK_MMC(sd_card)		(((sd_card)->sd_type & 0xFF) == TYPE_MMC)
+#define CHK_MMC_26M(sd_card)		(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_26M))
+#define CHK_MMC_52M(sd_card)		(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_52M))
+#define CHK_MMC_4BIT(sd_card)		(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_4BIT))
+#define CHK_MMC_8BIT(sd_card)		(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_8BIT))
+#define CHK_MMC_SECTOR_MODE(sd_card)	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_SECTOR_MODE))
+#define CHK_MMC_DDR52(sd_card)		(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_DDR52))
+
+#define SET_MMC(sd_card)		((sd_card)->sd_type = TYPE_MMC)
+#define SET_MMC_26M(sd_card)		((sd_card)->sd_type |= MMC_26M)
+#define SET_MMC_52M(sd_card)		((sd_card)->sd_type |= MMC_52M)
+#define SET_MMC_4BIT(sd_card)		((sd_card)->sd_type |= MMC_4BIT)
+#define SET_MMC_8BIT(sd_card)		((sd_card)->sd_type |= MMC_8BIT)
+#define SET_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type |= MMC_SECTOR_MODE)
+#define SET_MMC_DDR52(sd_card)		((sd_card)->sd_type |= MMC_DDR52)
+
+#define CLR_MMC_26M(sd_card)		((sd_card)->sd_type &= ~MMC_26M)
+#define CLR_MMC_52M(sd_card)		((sd_card)->sd_type &= ~MMC_52M)
+#define CLR_MMC_4BIT(sd_card)		((sd_card)->sd_type &= ~MMC_4BIT)
+#define CLR_MMC_8BIT(sd_card)		((sd_card)->sd_type &= ~MMC_8BIT)
+#define CLR_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type &= ~MMC_SECTOR_MODE)
+#define CLR_MMC_DDR52(sd_card)		((sd_card)->sd_type &= ~MMC_DDR52)
+
+#define CHK_MMC_HS(sd_card)		(CHK_MMC_52M(sd_card) || CHK_MMC_26M(sd_card))
+#define CLR_MMC_HS(sd_card)			\
+do {						\
+	CLR_MMC_DDR52(sd_card);			\
+	CLR_MMC_52M(sd_card);			\
+	CLR_MMC_26M(sd_card);			\
+} while(0)
+
+#define SD_SUPPORT_CLASS_TEN		0x01
+#define SD_SUPPORT_1V8			0x02
+
+#define SD_SET_CLASS_TEN(sd_card)	((sd_card)->sd_setting |= SD_SUPPORT_CLASS_TEN)
+#define SD_CHK_CLASS_TEN(sd_card)	((sd_card)->sd_setting & SD_SUPPORT_CLASS_TEN)
+#define SD_CLR_CLASS_TEN(sd_card)	((sd_card)->sd_setting &= ~SD_SUPPORT_CLASS_TEN)
+#define SD_SET_1V8(sd_card)		((sd_card)->sd_setting |= SD_SUPPORT_1V8)
+#define SD_CHK_1V8(sd_card)		((sd_card)->sd_setting & SD_SUPPORT_1V8)
+#define SD_CLR_1V8(sd_card)		((sd_card)->sd_setting &= ~SD_SUPPORT_1V8)
+
+struct sd_info {
+	u16 sd_type;
+	u8 err_code;
+	u8 sd_data_buf_ready;
+	u32 sd_addr;
+	u32 capacity;
+
+	u8 raw_csd[16];
+	u8 raw_cid[16];
+	u8 raw_scr[8];
+
+	int seq_mode;
+	enum dma_data_direction pre_dir;
+	u32 pre_sec_addr;
+	u16 pre_sec_cnt;
+#ifdef PRE_READ_EN
+	u16 total_sec_cnt;
+#endif
+
+	int cleanup_counter;
+
+	int sd_clock;
+
+	int mmc_dont_switch_bus;
+
+#ifdef SUPPORT_CPRM
+	int sd_pass_thru_en;
+	int pre_cmd_err;
+	u8 last_rsp_type;
+	u8 rsp[17];
+#endif
+
+	u8 func_group1_mask;
+	u8 func_group2_mask;
+	u8 func_group3_mask;
+	u8 func_group4_mask;
+
+	u8 current_limit;
+
+	u8 sd_switch_fail;
+	u8 sd_read_phase;
+
+#ifdef SUPPORT_SD_LOCK
+	u8 sd_lock_status;
+	u8 sd_erase_status;
+	u8 sd_lock_notify;
+#endif
+	int need_retune;
+};
+
+#define MODE_512_SEQ		0x01
+#define MODE_2K_SEQ		0x02
+
+#define TYPE_MS			0x0000
+#define TYPE_MSPRO		0x0001
+
+#define MS_4BIT			0x0100
+#define MS_8BIT			0x0200
+#define MS_HG			0x0400
+#define MS_XC			0x0800
+
+#define HG8BIT			(MS_HG | MS_8BIT)
+
+#define CHK_MSPRO(ms_card)	(((ms_card)->ms_type & 0xFF) == TYPE_MSPRO)
+#define CHK_HG8BIT(ms_card)	(CHK_MSPRO(ms_card) && (((ms_card)->ms_type & HG8BIT) == HG8BIT))
+#define CHK_MSXC(ms_card)	(CHK_MSPRO(ms_card) && ((ms_card)->ms_type & MS_XC))
+#define CHK_MSHG(ms_card)	(CHK_MSPRO(ms_card) && ((ms_card)->ms_type & MS_HG))
+
+#define CHK_MS8BIT(ms_card)	(((ms_card)->ms_type & MS_8BIT))
+#define CHK_MS4BIT(ms_card)	(((ms_card)->ms_type & MS_4BIT))
+
+struct ms_delay_write_tag {
+	u16 old_phyblock;
+	u16 new_phyblock;
+	u16 logblock;
+	u8 pageoff;
+	u8 delay_write_flag;
+};
+
+struct ms_info {
+	u16 ms_type;
+	u8 block_shift;
+	u8 page_off;
+	u16 total_block;
+	u16 boot_block;
+	u32 capacity;
+
+	u8 check_ms_flow;
+	u8 switch_8bit_fail;
+	u8 err_code;
+
+	struct zone_entry *segment;
+	int segment_cnt;
+
+	int pro_under_formatting;
+	int format_status;
+	u16 progress;
+	u8 raw_sys_info[96];
+	u8 raw_ms_id[16];
+#ifdef SUPPORT_PCGL_1P18
+	u8 raw_model_name[48];
+#endif
+
+	u8 multi_flag;
+
+	u8 seq_mode;
+	enum dma_data_direction pre_dir;
+	u32 pre_sec_addr;
+	u16 pre_sec_cnt;
+	u32 total_sec_cnt;
+
+	struct ms_delay_write_tag delay_write;
+
+	int cleanup_counter;
+
+	int ms_clock;
+
+#ifdef SUPPORT_MAGIC_GATE
+	u8 magic_gate_id[16];
+	u8 mg_entry_num;
+	int mg_auth;
+#endif
+};
+
+#ifdef _MSG_TRACE
+struct trace_msg_t {
+	u16 line;
+#define MSG_FUNC_LEN 64
+	char func[MSG_FUNC_LEN];
+#define MSG_FILE_LEN 32
+	char file[MSG_FILE_LEN];
+#define TIME_VAL_LEN 16
+	u8 timeval_buf[TIME_VAL_LEN];
+	u8 valid;
+};
+#endif
+
+#define DEFAULT_SINGLE		0
+#define SD_LUN			1
+#define MS_LUN			2
+
+#define QFN		0
+#define LQFP		1
+
+#define SD_PUSH_POINT_CTL_MASK		0x03
+#define SD_PUSH_POINT_DELAY		0x01
+#define SD_PUSH_POINT_AUTO		0x02
+#define SD_SAMPLE_POINT_CTL_MASK	0x0C
+#define SD_SAMPLE_POINT_DELAY		0x04
+#define SD_SAMPLE_POINT_AUTO		0x08
+#define SD_DDR_TX_PHASE_SET_BY_USER	0x10
+#define MMC_DDR_TX_PHASE_SET_BY_USER	0x20
+#define SUPPORT_MMC_DDR_MODE		0x40
+#define RESET_MMC_FIRST			0x80
+
+#define SEQ_START_CRITERIA		0x20
+
+#define POWER_CLASS_2_EN		0x02
+#define POWER_CLASS_1_EN		0x01
+
+#define MAX_SHOW_CNT			10
+#define MAX_RESET_CNT			3
+
+#define PRE_READ_30M			0xF000
+
+#define PRE_READ_TH			64
+
+struct rts5229_cfg_epcore {
+	u16 cfg_addr;
+	u8 func;
+	u8 mode;
+	u8 data;
+};
+
+struct rts5229_cfg_phy {
+	u8 phy_addr;
+	u16 data;
+};
+
+struct rts5229_auto_load_map {
+	enum { CFG_PHY, CFG_EPCORE, CFG_OTHER } type;
+
+	union {
+		struct rts5229_cfg_epcore cfg_epcore;
+		struct rts5229_cfg_phy cfg_phy;
+	} item;
+};
+
+struct rtsx_chip {
+	rtsx_dev_t *rtsx;
+
+	u32 int_reg;
+	char max_lun;
+	void *context;
+
+	void *host_cmds_ptr;
+	dma_addr_t host_cmds_addr;
+	int ci;
+
+	void *host_sg_tbl_ptr;
+	dma_addr_t host_sg_tbl_addr;
+	int sgi;
+
+	struct scsi_cmnd *srb;
+	struct sense_data_t sense_buffer[MAX_ALLOWED_LUN_CNT];
+
+	int cur_clk;
+
+	int cur_card;
+
+	unsigned long need_release;
+	unsigned long need_reset;
+	unsigned long need_reinit;
+
+	int rw_need_retry;
+	int rw_retry_cnt;
+
+#ifdef SUPPORT_OCP
+	u32 ocp_int;
+	u8 ocp_stat;
+#endif
+
+	u8 card_exist;
+	u8 card_ready;
+	u8 card_fail;
+	u8 card_ejected;
+	u8 card_wp;
+
+	u8 lun_mc;
+
+#ifndef LED_AUTO_BLINK
+	int led_toggle_counter;
+#endif
+
+	int sd_reset_counter;
+	int ms_reset_counter;
+
+	u8 card_bus_width[MAX_ALLOWED_LUN_CNT];
+	u32 capacity[MAX_ALLOWED_LUN_CNT];
+	card_rw_func rw_card[MAX_ALLOWED_LUN_CNT];
+	u8 card2lun[32];
+	u8 lun2card[MAX_ALLOWED_LUN_CNT];
+
+	int rw_fail_cnt[MAX_ALLOWED_LUN_CNT];
+
+	int sd_show_cnt;
+	int ms_show_cnt;
+
+	struct sd_info sd_card;
+	struct ms_info ms_card;
+
+#ifdef _MSG_TRACE
+	struct trace_msg_t trace_msg[TRACE_ITEM_CNT];
+	int msg_idx;
+#endif
+
+	int auto_delink_cnt;
+	int auto_delink_allowed;
+
+	int ltr_enabled;
+	int ltr_active;
+	int aspm_enabled;
+	u8 host_aspm_val;
+
+	u8 rtsx_flag;
+
+	int sd20_mode;
+	int sd_retune_clock;
+	int sd_default_tx_phase;
+	int sd_default_rx_phase;
+
+	int ss_counter;
+	int idle_counter;
+	enum RTSX_STAT rtsx_stat;
+
+	u16 vendor_id;
+	u16 product_id;
+	u16 ssvid;
+	u16 ssdid;
+	u8 ic_version;
+
+	int driver_first_load;
+
+	u8 aspm_level[2];
+
+	struct rts5229_auto_load_map al_map[AL_MAP_MAX_CNT];
+	int al_map_cnt;
+
+	int led_test_mode;
+
+	int adma_mode;
+
+	int auto_delink_en;
+	int ss_en;
+	u8 lun_mode;
+	u8 aspm_l0s_l1_en;
+
+	int power_down_in_ss;
+
+	int sdr104_en;
+	int ddr50_en;
+	int sdr50_en;
+
+	int baro_pkg;
+
+	int asic_code;
+	int phy_debug_mode;
+	int aux_pwr_exist;
+	u8 ms_power_class_en;
+
+	int mspro_formatter_enable;
+
+	int use_hw_setting;
+
+	int remote_wakeup_en;
+
+	int ss_idle_period;
+
+	int dynamic_aspm;
+	int config_host_aspm;
+	int force_host_aspm;
+	u8 host_aspm_para;
+
+	int fpga_sd_sdr104_clk;
+	int fpga_sd_ddr50_clk;
+	int fpga_sd_sdr50_clk;
+	int fpga_sd_hs_clk;
+	int fpga_mmc_52m_clk;
+	int fpga_ms_hg_clk;
+	int fpga_ms_4bit_clk;
+	int fpga_ms_1bit_clk;
+
+	int asic_sd_sdr104_clk;
+	int asic_sd_ddr50_clk;
+	int asic_sd_sdr50_clk;
+	int asic_sd_hs_clk;
+	int asic_mmc_52m_clk;
+	int asic_ms_hg_clk;
+	int asic_ms_4bit_clk;
+	int asic_ms_1bit_clk;
+
+	u8 ssc_depth_sd_sdr104;
+	u8 ssc_depth_sd_ddr50;
+	u8 ssc_depth_sd_sdr50;
+	u8 ssc_depth_sd_hs;
+	u8 ssc_depth_mmc_52m;
+	u8 ssc_depth_ms_hg;
+	u8 ssc_depth_ms_4bit;
+	u8 ssc_depth_low_speed;
+
+	u8 card_drive_sel;
+	u8 sd30_drive_sel_1v8;
+	u8 sd30_drive_sel_3v3;
+
+	u8 sd30_clk_drive_sel_1v8;
+	u8 sd30_cmd_drive_sel_1v8;
+	u8 sd30_dat_drive_sel_1v8;
+	u8 sd30_clk_drive_sel_3v3;
+	u8 sd30_cmd_drive_sel_3v3;
+	u8 sd30_dat_drive_sel_3v3;
+
+	u8 sd_400mA_ocp_thd;
+	u8 sd_800mA_ocp_thd;
+
+	int ssc_en;
+
+	int msi_en;
+
+	int sdr_tx_tuning_en;
+
+	int sd_timeout;
+	int ms_timeout;
+	int mspro_timeout;
+
+	int auto_power_down;
+
+	int sd_ddr_tx_phase;
+	int mmc_ddr_tx_phase;
+	int sd_sdr104_default_tx_phase;
+	int sd_sdr104_default_rx_phase;
+	int sd_sdr50_default_tx_phase;
+	int sd_sdr50_default_rx_phase;
+	int sd_ddr50_default_tx_phase;
+	int sd_ddr50_default_rx_phase;
+
+	int pmos_pwr_on_interval;
+	int sd_voltage_switch_delay;
+	int s3_pwr_off_delay;
+
+	int force_clkreq_0;
+
+	int ft2_fast_mode;
+
+	int do_delink_before_power_down;
+
+	int polling_config;
+
+	int delink_stage1_step;
+	int delink_stage2_step;
+	int delink_stage3_step;
+
+	int auto_delink_in_L1;
+
+	int hp_watch_bios_hotplug;
+
+	int support_ms_8bit;
+
+#define SUPPORT_SD	0x01
+#define SUPPORT_MMC	0x02
+#define SUPPORT_MS	0x04
+#define SUPPORT_XD	0x08
+#define SUPPORT_SDMMC	(SUPPORT_SD | SUPPORT_MMC)
+	u8 support_card;
+
+	u8 blink_led;
+	u8 led_always_on;
+
+	u8 phy_voltage;
+
+	u32 sd_speed_prior;
+	u32 sd_current_prior;
+
+	u32 sd_ctl;
+
+	u32 relink_time;
+
+	int cd_max_show_cnt;
+
+	int pre_read_en;
+
+	int pre_read_th;
+
+	int ltr_en;
+
+	int obff_en;
+
+	u16 phy_pcr;
+	u16 phy_rcr0;
+	u16 phy_rcr2;
+
+	int phy_optimize;
+
+#define TURN_ON_LED_AT_START		0x00000001
+	u32 dev_option;
+
+	u32 sd_option;
+
+	u32 ms_option;
+};
+
+#define rtsx_get_stat(chip)		(chip)->rtsx_stat
+#define rtsx_chk_stat(chip, stat)	((chip)->rtsx_stat == (stat))
+
+#define RTSX_SET_DELINK(chip)	(chip)->rtsx_flag |= 0x01
+#define RTSX_CLR_DELINK(chip)	(chip)->rtsx_flag &= 0xFE
+#define RTSX_TST_DELINK(chip)	(chip)->rtsx_flag & 0x01
+
+#define CHECK_PID(chip, pid)		((chip)->product_id == (pid))
+#define CHECK_BARO_PKG(chip, pkg)	((chip)->baro_pkg == (pkg))
+#define CHECK_LUN_MODE(chip, mode)	((chip)->lun_mode == (mode))
+#define GET_LUN_MODE(chip)		((chip)->lun_mode)
+#define CHECK_IC_VER(chip, ver)		((chip)->ic_version == (ver))
+#define CHECK_VERSION(chip, pid, ver)	\
+	(CHECK_PID(chip, pid) && CHECK_IC_VER(chip, ver))
+
+/* auto_delink_en == 0: disable auto delink (may refer to hw setting)
+ * auto_delink_en == 1: enable auto delink (may refer to hw setting)
+ * auto_delink_en == 2: disable auto delink (don't refer to hardware setting any more)
+ */
+#define CHK_AUTODELINK_EN(chip)		((chip)->auto_delink_en == 1)
+
+#define SSC_PDCTL		0x01
+#define OC_PDCTL		0x02
+
+int rtsx_force_power_on(struct rtsx_chip *chip, u8 ctl);
+int rtsx_force_power_down(struct rtsx_chip *chip, u8 ctl);
+
+void rtsx_disable_card_int(struct rtsx_chip *chip);
+void rtsx_enable_card_int(struct rtsx_chip *chip);
+void rtsx_enable_bus_int(struct rtsx_chip *chip);
+void rtsx_disable_bus_int(struct rtsx_chip *chip);
+int rtsx_enable_ocp(struct rtsx_chip *chip);
+int rtsx_disable_ocp(struct rtsx_chip *chip);
+int rtsx_clear_ocp(struct rtsx_chip *chip);
+int rtsx_reset_chip(struct rtsx_chip *chip);
+int rtsx_init_chip(struct rtsx_chip *chip);
+void rtsx_release_chip(struct rtsx_chip *chip);
+void rtsx_polling_func(struct rtsx_chip *chip);
+void rtsx_undo_delink(struct rtsx_chip *chip);
+void rtsx_stop_cmd(struct rtsx_chip *chip, int card);
+int rtsx_write_register(struct rtsx_chip *chip, u16 addr, u8 mask, u8 data);
+int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 * data);
+int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
+		      u32 val);
+int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 * val);
+int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 * buf,
+		       int len);
+int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 * buf,
+		      int len);
+int rtsx_write_phy_register(struct rtsx_chip *chip, u8 addr, u16 val);
+int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 * val);
+int rtsx_clr_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit);
+int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit);
+int rtsx_check_link_ready(struct rtsx_chip *chip);
+void rtsx_enter_ss(struct rtsx_chip *chip);
+void rtsx_exit_ss(struct rtsx_chip *chip);
+int rtsx_pre_handle_interrupt(struct rtsx_chip *chip);
+void rtsx_enter_L1(struct rtsx_chip *chip);
+void rtsx_exit_L1(struct rtsx_chip *chip);
+void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat);
+void rtsx_enable_aspm(struct rtsx_chip *chip);
+void rtsx_disable_aspm(struct rtsx_chip *chip);
+void rtsx_enable_stable_aspm(struct rtsx_chip *chip, u8 val);
+void rtsx_disable_stable_aspm(struct rtsx_chip *chip, u8 * val);
+void rtsx_enter_work_state(struct rtsx_chip *chip);
+void rtsx_init_aspm(struct rtsx_chip *chip);
+int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 * buf, int buf_len);
+int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 * buf, int buf_len);
+int rtsx_check_chip_exist(struct rtsx_chip *chip);
+void rtsx_wait_rb_full(struct rtsx_chip *chip);
+void rtsx_set_stat(struct rtsx_chip *chip, enum RTSX_STAT stat);
+void rtsx_set_run_stat(struct rtsx_chip *chip, int enable_blink);
+
+#define RTSX_WRITE_REG(chip, addr, mask, data)					\
+do {										\
+	int retval = rtsx_write_register((chip), (addr), (mask), (data));	\
+	if (retval != STATUS_SUCCESS) {						\
+		TRACE_RET((chip), retval);					\
+	}									\
+} while (0)
+
+#define RTSX_READ_REG(chip, addr, data)						\
+do {										\
+	int retval = rtsx_read_register((chip), (addr), (data));		\
+	if (retval != STATUS_SUCCESS) {						\
+		TRACE_RET((chip), retval);					\
+	}									\
+} while (0)
+
+static inline void rtsx_enable_host_aspm(struct rtsx_chip *chip)
+{
+	RTSX_DEBUGP(("Enable host ASPM!\n"));
+	EnableHostASPM(chip);
+}
+
+static inline void rtsx_disable_host_aspm(struct rtsx_chip *chip)
+{
+	RTSX_DEBUGP(("Disable host ASPM!\n"));
+	DisableHostASPM(chip);
+}
+
+static inline void rtsx_set_host_aspm(struct rtsx_chip *chip, u8 val)
+{
+	SetHostASPM(chip, val);
+}
+
+static inline void rtsx_get_host_aspm(struct rtsx_chip *chip, u8 * val)
+{
+	GetHostASPM(chip, val);
+}
+
+#endif
diff --git a/drivers/staging/rts5229/rtsx_scsi.c b/drivers/staging/rts5229/rtsx_scsi.c
new file mode 100644
index 0000000..95293f0
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_scsi.c
@@ -0,0 +1,3322 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+#include "rtsx_transport.h"
+#include "rtsx_sys.h"
+#include "rtsx_card.h"
+#include "rtsx_chip.h"
+#include "rtsx_scsi.h"
+#include "sd.h"
+#include "ms.h"
+
+void scsi_show_command(struct scsi_cmnd *srb)
+{
+	char *what = NULL;
+	int i, unknown_cmd = 0;
+
+	switch (srb->cmnd[0]) {
+	case TEST_UNIT_READY:
+		what = "TEST_UNIT_READY";
+		break;
+	case REZERO_UNIT:
+		what = "REZERO_UNIT";
+		break;
+	case REQUEST_SENSE:
+		what = "REQUEST_SENSE";
+		break;
+	case FORMAT_UNIT:
+		what = "FORMAT_UNIT";
+		break;
+	case READ_BLOCK_LIMITS:
+		what = "READ_BLOCK_LIMITS";
+		break;
+	case REASSIGN_BLOCKS:
+		what = "REASSIGN_BLOCKS";
+		break;
+	case READ_6:
+		what = "READ_6";
+		break;
+	case WRITE_6:
+		what = "WRITE_6";
+		break;
+	case SEEK_6:
+		what = "SEEK_6";
+		break;
+	case READ_REVERSE:
+		what = "READ_REVERSE";
+		break;
+	case WRITE_FILEMARKS:
+		what = "WRITE_FILEMARKS";
+		break;
+	case SPACE:
+		what = "SPACE";
+		break;
+	case INQUIRY:
+		what = "INQUIRY";
+		break;
+	case RECOVER_BUFFERED_DATA:
+		what = "RECOVER_BUFFERED_DATA";
+		break;
+	case MODE_SELECT:
+		what = "MODE_SELECT";
+		break;
+	case RESERVE:
+		what = "RESERVE";
+		break;
+	case RELEASE:
+		what = "RELEASE";
+		break;
+	case COPY:
+		what = "COPY";
+		break;
+	case ERASE:
+		what = "ERASE";
+		break;
+	case MODE_SENSE:
+		what = "MODE_SENSE";
+		break;
+	case START_STOP:
+		what = "START_STOP";
+		break;
+	case RECEIVE_DIAGNOSTIC:
+		what = "RECEIVE_DIAGNOSTIC";
+		break;
+	case SEND_DIAGNOSTIC:
+		what = "SEND_DIAGNOSTIC";
+		break;
+	case ALLOW_MEDIUM_REMOVAL:
+		what = "ALLOW_MEDIUM_REMOVAL";
+		break;
+	case SET_WINDOW:
+		what = "SET_WINDOW";
+		break;
+	case READ_CAPACITY:
+		what = "READ_CAPACITY";
+		break;
+	case READ_10:
+		what = "READ_10";
+		break;
+	case WRITE_10:
+		what = "WRITE_10";
+		break;
+	case SEEK_10:
+		what = "SEEK_10";
+		break;
+	case WRITE_VERIFY:
+		what = "WRITE_VERIFY";
+		break;
+	case VERIFY:
+		what = "VERIFY";
+		break;
+	case SEARCH_HIGH:
+		what = "SEARCH_HIGH";
+		break;
+	case SEARCH_EQUAL:
+		what = "SEARCH_EQUAL";
+		break;
+	case SEARCH_LOW:
+		what = "SEARCH_LOW";
+		break;
+	case SET_LIMITS:
+		what = "SET_LIMITS";
+		break;
+	case READ_POSITION:
+		what = "READ_POSITION";
+		break;
+	case SYNCHRONIZE_CACHE:
+		what = "SYNCHRONIZE_CACHE";
+		break;
+	case LOCK_UNLOCK_CACHE:
+		what = "LOCK_UNLOCK_CACHE";
+		break;
+	case READ_DEFECT_DATA:
+		what = "READ_DEFECT_DATA";
+		break;
+	case MEDIUM_SCAN:
+		what = "MEDIUM_SCAN";
+		break;
+	case COMPARE:
+		what = "COMPARE";
+		break;
+	case COPY_VERIFY:
+		what = "COPY_VERIFY";
+		break;
+	case WRITE_BUFFER:
+		what = "WRITE_BUFFER";
+		break;
+	case READ_BUFFER:
+		what = "READ_BUFFER";
+		break;
+	case UPDATE_BLOCK:
+		what = "UPDATE_BLOCK";
+		break;
+	case READ_LONG:
+		what = "READ_LONG";
+		break;
+	case WRITE_LONG:
+		what = "WRITE_LONG";
+		break;
+	case CHANGE_DEFINITION:
+		what = "CHANGE_DEFINITION";
+		break;
+	case WRITE_SAME:
+		what = "WRITE_SAME";
+		break;
+	case GPCMD_READ_SUBCHANNEL:
+		what = "READ SUBCHANNEL";
+		break;
+	case READ_TOC:
+		what = "READ_TOC";
+		break;
+	case GPCMD_READ_HEADER:
+		what = "READ HEADER";
+		break;
+	case GPCMD_PLAY_AUDIO_10:
+		what = "PLAY AUDIO (10)";
+		break;
+	case GPCMD_PLAY_AUDIO_MSF:
+		what = "PLAY AUDIO MSF";
+		break;
+	case GPCMD_GET_EVENT_STATUS_NOTIFICATION:
+		what = "GET EVENT/STATUS NOTIFICATION";
+		break;
+	case GPCMD_PAUSE_RESUME:
+		what = "PAUSE/RESUME";
+		break;
+	case LOG_SELECT:
+		what = "LOG_SELECT";
+		break;
+	case LOG_SENSE:
+		what = "LOG_SENSE";
+		break;
+	case GPCMD_STOP_PLAY_SCAN:
+		what = "STOP PLAY/SCAN";
+		break;
+	case GPCMD_READ_DISC_INFO:
+		what = "READ DISC INFORMATION";
+		break;
+	case GPCMD_READ_TRACK_RZONE_INFO:
+		what = "READ TRACK INFORMATION";
+		break;
+	case GPCMD_RESERVE_RZONE_TRACK:
+		what = "RESERVE TRACK";
+		break;
+	case GPCMD_SEND_OPC:
+		what = "SEND OPC";
+		break;
+	case MODE_SELECT_10:
+		what = "MODE_SELECT_10";
+		break;
+	case GPCMD_REPAIR_RZONE_TRACK:
+		what = "REPAIR TRACK";
+		break;
+	case 0x59:
+		what = "READ MASTER CUE";
+		break;
+	case MODE_SENSE_10:
+		what = "MODE_SENSE_10";
+		break;
+	case GPCMD_CLOSE_TRACK:
+		what = "CLOSE TRACK/SESSION";
+		break;
+	case 0x5C:
+		what = "READ BUFFER CAPACITY";
+		break;
+	case 0x5D:
+		what = "SEND CUE SHEET";
+		break;
+	case GPCMD_BLANK:
+		what = "BLANK";
+		break;
+	case REPORT_LUNS:
+		what = "REPORT LUNS";
+		break;
+	case MOVE_MEDIUM:
+		what = "MOVE_MEDIUM or PLAY AUDIO (12)";
+		break;
+	case READ_12:
+		what = "READ_12";
+		break;
+	case WRITE_12:
+		what = "WRITE_12";
+		break;
+	case WRITE_VERIFY_12:
+		what = "WRITE_VERIFY_12";
+		break;
+	case SEARCH_HIGH_12:
+		what = "SEARCH_HIGH_12";
+		break;
+	case SEARCH_EQUAL_12:
+		what = "SEARCH_EQUAL_12";
+		break;
+	case SEARCH_LOW_12:
+		what = "SEARCH_LOW_12";
+		break;
+	case SEND_VOLUME_TAG:
+		what = "SEND_VOLUME_TAG";
+		break;
+	case READ_ELEMENT_STATUS:
+		what = "READ_ELEMENT_STATUS";
+		break;
+	case GPCMD_READ_CD_MSF:
+		what = "READ CD MSF";
+		break;
+	case GPCMD_SCAN:
+		what = "SCAN";
+		break;
+	case GPCMD_SET_SPEED:
+		what = "SET CD SPEED";
+		break;
+	case GPCMD_MECHANISM_STATUS:
+		what = "MECHANISM STATUS";
+		break;
+	case GPCMD_READ_CD:
+		what = "READ CD";
+		break;
+	case 0xE1:
+		what = "WRITE CONTINUE";
+		break;
+	case WRITE_LONG_2:
+		what = "WRITE_LONG_2";
+		break;
+	case VENDOR_CMND:
+		what = "Realtek's vendor command";
+		break;
+	default:
+		what = "(unknown command)";
+		unknown_cmd = 1;
+		break;
+	}
+
+	if (srb->cmnd[0] != TEST_UNIT_READY) {
+		RTSX_DEBUGP(("Command %s (%d bytes)\n", what, srb->cmd_len));
+	}
+	if (unknown_cmd) {
+		RTSX_DEBUGP((""));
+		for (i = 0; i < srb->cmd_len && i < 16; i++)
+			RTSX_DEBUGPN((" %02x", srb->cmnd[i]));
+		RTSX_DEBUGPN(("\n"));
+	}
+}
+
+void set_sense_type(struct rtsx_chip *chip, unsigned int lun, int sense_type)
+{
+	switch (sense_type) {
+	case SENSE_TYPE_MEDIA_CHANGE:
+		set_sense_data(chip, lun, CUR_ERR, 0x06, 0, 0x28, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_NOT_PRESENT:
+		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x3A, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_LBA_OVER_RANGE:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x21, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x25, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_WRITE_PROTECT:
+		set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x27, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x11, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_WRITE_ERR:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x02, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_INVALID_CMD_FIELD:
+		set_sense_data(chip, lun, CUR_ERR, ILGAL_REQ, 0,
+			       ASC_INVLD_CDB, ASCQ_INVLD_CDB, CDB_ILLEGAL, 1);
+		break;
+
+	case SENSE_TYPE_FORMAT_IN_PROGRESS:
+		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04, 0, 0);
+		break;
+
+	case SENSE_TYPE_FORMAT_CMD_FAILED:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x31, 0x01, 0, 0);
+		break;
+
+#ifdef SUPPORT_MAGIC_GATE
+	case SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x02, 0, 0);
+		break;
+
+	case SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x00, 0, 0);
+		break;
+
+	case SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM:
+		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x30, 0x00, 0, 0);
+		break;
+
+	case SENSE_TYPE_MG_WRITE_ERR:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x00, 0, 0);
+		break;
+#endif
+
+#ifdef SUPPORT_SD_LOCK
+	case SENSE_TYPE_MEDIA_READ_FORBIDDEN:
+		set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x11, 0x13, 0, 0);
+		break;
+#endif
+
+	case SENSE_TYPE_NO_SENSE:
+	default:
+		set_sense_data(chip, lun, CUR_ERR, 0, 0, 0, 0, 0, 0);
+		break;
+	}
+}
+
+void set_sense_data(struct rtsx_chip *chip, unsigned int lun, u8 err_code,
+		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
+		    u16 sns_key_info1)
+{
+	struct sense_data_t *sense = &(chip->sense_buffer[lun]);
+
+	sense->err_code = err_code;
+	sense->sense_key = sense_key;
+	sense->info[0] = (u8) (info >> 24);
+	sense->info[1] = (u8) (info >> 16);
+	sense->info[2] = (u8) (info >> 8);
+	sense->info[3] = (u8) info;
+
+	sense->ad_sense_len = sizeof(struct sense_data_t) - 8;
+	sense->asc = asc;
+	sense->ascq = ascq;
+	if (sns_key_info0 != 0) {
+		sense->sns_key_info[0] = SKSV | sns_key_info0;
+		sense->sns_key_info[1] = (sns_key_info1 & 0xf0) >> 8;
+		sense->sns_key_info[2] = sns_key_info1 & 0x0f;
+	}
+}
+
+static int test_unit_ready(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+#ifdef SUPPORT_SD_LOCK
+	if (get_lun_card(chip, SCSI_LUN(srb)) == SD_CARD) {
+		struct sd_info *sd_card = &(chip->sd_card);
+		if (sd_card->sd_lock_notify) {
+			sd_card->sd_lock_notify = 0;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+			return TRANSPORT_FAILED;
+		} else if (sd_card->sd_lock_status & SD_LOCKED) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_READ_FORBIDDEN);
+			return TRANSPORT_FAILED;
+		}
+	}
+#endif
+
+	return TRANSPORT_GOOD;
+}
+
+static unsigned char formatter_inquiry_str[20] = {
+	'M', 'E', 'M', 'O', 'R', 'Y', 'S', 'T', 'I', 'C', 'K',
+#ifdef SUPPORT_MAGIC_GATE
+	'-', 'M', 'G',
+#else
+	0x20, 0x20, 0x20,
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+	0x0B,
+#else
+	0x09,
+#endif
+	0x00,
+	0x00,
+	0x20, 0x20, 0x20,
+};
+
+static unsigned char inquiry_caching_mode[] = {
+	0x00 | 0x08,
+	0x12,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static unsigned char inquiry_control_mode[] = {
+	0x00 | 0x0A,
+	0x0A,
+	0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static unsigned char inquiry_info_exception_mode[] = {
+	0x00 | 0x1C,
+	0x0A,
+	0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static unsigned char inquiry_standard_header[] = {
+	QULIFIRE | DRCT_ACCESS_DEV,
+
+	RMB_DISC | 0x0D,
+
+	0x06,
+
+	0x02,
+
+	0x5B,
+
+	0x02,
+
+	0,
+	REL_ADR | WBUS_32 | WBUS_16 | SYNC | LINKED | CMD_QUE | SFT_RE
+};
+
+static unsigned char inquiry_unit_serial_num[] = {
+	QULIFIRE | DRCT_ACCESS_DEV,
+	0x80,
+	0x00,
+	0x10,
+	'2', '0', '1', '2', '0', '6', '2', '9', '1', '4', '3', '4', '5', '3',
+	    '0', '0'
+};
+
+static unsigned char inquiry_supported_vpd_pages[] = {
+	QULIFIRE | DRCT_ACCESS_DEV,
+	0x00,
+	0x00,
+	0x03,
+	0x00,
+	0x80,
+	0x83
+};
+
+static unsigned char inquiry_device_identification[] = {
+	QULIFIRE | DRCT_ACCESS_DEV,
+	0x83,
+	0x00,
+	0x34,
+
+	0x00 | 0x02,
+	0x00 | 0x00 | 0x01,
+	0x00,
+	0x18,
+	0x72, 0x65, 0x61, 0x6c, 0x74, 0x65, 0x6b, 0x20,
+	'2', '0', '1', '2', '0', '6', '2', '9', '1', '4', '3', '4', '5', '3',
+	    '0', '0',
+
+	0x00 | 0x01,
+	0x00 | 0x00 | 0x02,
+	0x00,
+	0x08,
+	0x00, 0xE0, 0x4C, 0xFF, 0xFF, 0xFF, 0xFF, 0x00,
+
+	0x00 | 0x03,
+	0x00 | 0x00 | 0x08,
+	0x00,
+	0x08,
+	0x72, 0x65, 0x61, 0x6c, 0x74, 0x65, 0x6b, 0x20,
+
+};
+
+static unsigned char inquiry_extended[] = {
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0,
+	0,
+	0x04, 0x60,
+	0x04, 0xC0,
+	0x00, 0x00,
+	0x00, 0x00,
+	0x00, 0x00,
+	0x00, 0x00,
+	0x00, 0x00,
+	0x00, 0x00,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	const char *inquiry_string = (char *)"Generic-FlashCard       1.00 ";
+	unsigned char sendbytes;
+	unsigned char *buf, *ptr, *vendor_str;
+	int pro_formatter_flag = 0;
+	u8 id = get_lun_card(chip, lun);
+	u8 page_code, evpd;
+	int finish_inquiry = 0;
+
+	evpd = srb->cmnd[1] & 0x01;
+	page_code = srb->cmnd[2];
+
+	/* SPC4, 6.4.1, page 308
+	 * If the page code field is not set to zero when the EVPD bit is set to zero,
+	 * the command shall be terminated with CHECK CONDITION STATUS, with the
+	 * sense key set to ILLEGAL REQUEST, and the addtional sense code set to
+	 * INVALID FIELD IN CDB
+	 */
+
+	if ((0 == evpd) && (0 != page_code)) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (1 == evpd) {
+		if ((page_code != 0x80) && (page_code != 0x83)
+		    && (page_code != 0x00)) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	buf = vmalloc(scsi_bufflen(srb));
+	if (buf == NULL)
+		TRACE_RET(chip, TRANSPORT_ERROR);
+
+	if (chip->mspro_formatter_enable && (chip->lun2card[lun] & MS_CARD)) {
+		if (MS_CARD == id || !id)
+			pro_formatter_flag = 1;
+	}
+
+	switch (page_code) {
+	case 0x83:
+		if (scsi_bufflen(srb) < 56)
+			sendbytes = (unsigned char)(scsi_bufflen(srb));
+		else
+			sendbytes = 56;
+		memcpy(buf, inquiry_device_identification, sendbytes);
+		finish_inquiry = 1;
+		break;
+
+	case 0x80:
+		if (scsi_bufflen(srb) < 20)
+			sendbytes = (unsigned char)(scsi_bufflen(srb));
+		else
+			sendbytes = 20;
+		memcpy(buf, inquiry_unit_serial_num, sendbytes);
+		finish_inquiry = 1;
+		break;
+
+	case 0x00:
+		if (evpd) {
+			if (scsi_bufflen(srb) < 7)
+				sendbytes =
+				    (unsigned char)(scsi_bufflen(srb));
+			else
+				sendbytes = 7;
+			memcpy(buf, inquiry_supported_vpd_pages, sendbytes);
+			finish_inquiry = 1;
+		} else {
+			RTSX_DEBUGP(("Standard inquiry flow\n"));
+		}
+		break;
+
+	default:
+		RTSX_DEBUGP(("Standard inquiry flow\n"));
+	}
+
+	if (finish_inquiry)
+		goto finish;
+
+	if (scsi_bufflen(srb) < 96)
+		sendbytes = (unsigned char)(scsi_bufflen(srb));
+	else
+		sendbytes = 96;
+	ptr = buf;
+
+	if (sendbytes > 8) {
+		memcpy(ptr, inquiry_standard_header, 8);
+		ptr += 8;
+	} else {
+		memcpy(ptr, inquiry_standard_header, sendbytes);
+		goto finish;
+	}
+
+	if (sendbytes > 36) {
+		memcpy(ptr, inquiry_string, 28);
+		ptr += 28;
+	} else {
+		memcpy(ptr, inquiry_string, sendbytes - 8);
+		goto finish;
+	}
+
+	if (pro_formatter_flag)
+		vendor_str = formatter_inquiry_str;
+	else
+		vendor_str = inquiry_extended;
+	if (sendbytes > 56) {
+		memcpy(ptr, vendor_str, 20);
+		ptr += 20;
+	} else {
+		memcpy(ptr, vendor_str, sendbytes - 36);
+		goto finish;
+	}
+
+	memcpy(ptr, inquiry_extended + 20, sendbytes - 56);
+
+ finish:
+	scsi_set_resid(srb, 0);
+
+	rtsx_stor_set_xfer_buf(buf, sendbytes, srb);
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int start_stop_unit(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	scsi_set_resid(srb, scsi_bufflen(srb));
+
+	if (srb->cmnd[1] == 1) {
+		return TRANSPORT_GOOD;
+	}
+
+	switch (srb->cmnd[0x4]) {
+	case STOP_MEDIUM:
+		return TRANSPORT_GOOD;
+
+	case UNLOAD_MEDIUM:
+		if (check_card_ready(chip, lun)) {
+			eject_card(chip, lun);
+		}
+		return TRANSPORT_GOOD;
+
+	case MAKE_MEDIUM_READY:
+	case LOAD_MEDIUM:
+		if (check_card_ready(chip, lun)) {
+			return TRANSPORT_GOOD;
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_NOT_PRESENT);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+
+		break;
+	}
+
+	TRACE_RET(chip, TRANSPORT_ERROR);
+}
+
+static int allow_medium_removal(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int prevent;
+
+	prevent = srb->cmnd[4] & 0x1;
+
+	scsi_set_resid(srb, 0);
+
+	if (prevent) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int request_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sense_data_t *sense;
+	unsigned int lun = SCSI_LUN(srb);
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned char *tmp, *buf;
+
+	sense = &(chip->sense_buffer[lun]);
+
+	if ((get_lun_card(chip, lun) == MS_CARD)
+	    && ms_card->pro_under_formatting) {
+		if (ms_card->format_status == FORMAT_SUCCESS) {
+			set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+			ms_card->pro_under_formatting = 0;
+			ms_card->progress = 0;
+		} else if (ms_card->format_status == FORMAT_IN_PROGRESS) {
+			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04,
+				       0x04, 0, (u16) (ms_card->progress));
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_FORMAT_CMD_FAILED);
+			ms_card->pro_under_formatting = 0;
+			ms_card->progress = 0;
+		}
+
+		rtsx_set_stat(chip, RTSX_STAT_RUN);
+	}
+
+	buf = vmalloc(scsi_bufflen(srb));
+	if (buf == NULL) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	tmp = (unsigned char *)sense;
+	memcpy(buf, tmp, scsi_bufflen(srb));
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	vfree(buf);
+
+	scsi_set_resid(srb, 0);
+	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+	return TRANSPORT_GOOD;
+}
+
+static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
+			  int lun, u8 * buf, int buf_len)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	int sys_info_offset;
+	int data_size = buf_len;
+	int support_format = 0;
+	int i = 0;
+
+	if (cmd == MODE_SENSE) {
+		sys_info_offset = 8;
+		if (data_size > 0x68) {
+			data_size = 0x68;
+		}
+		buf[i++] = 0x67;
+	} else {
+		sys_info_offset = 12;
+		if (data_size > 0x6C) {
+			data_size = 0x6C;
+		}
+		buf[i++] = 0x00;
+		buf[i++] = 0x6A;
+	}
+
+	if (check_card_ready(chip, lun)) {
+		if (CHK_MSXC(ms_card)) {
+			support_format = 1;
+			buf[i++] = 0x40;
+		} else if (CHK_MSPRO(ms_card)) {
+			support_format = 1;
+			buf[i++] = 0x20;
+		} else {
+			buf[i++] = 0x10;
+		}
+
+		if (check_card_wp(chip, lun)) {
+			buf[i++] = 0x80;
+		} else {
+			buf[i++] = 0x00;
+		}
+	} else {
+		buf[i++] = 0x00;
+		buf[i++] = 0x00;
+	}
+
+	buf[i++] = 0x00;
+
+	if (cmd == MODE_SENSE_10) {
+		buf[i++] = 0x00;
+		buf[i++] = 0x00;
+		buf[i++] = 0x00;
+
+		if (data_size >= 9) {
+			buf[i++] = 0x20;
+		}
+		if (data_size >= 10) {
+			buf[i++] = 0x62;
+		}
+		if (data_size >= 11) {
+			buf[i++] = 0x00;
+		}
+		if (data_size >= 12) {
+			if (support_format) {
+				buf[i++] = 0xC0;
+			} else {
+				buf[i++] = 0x00;
+			}
+		}
+	} else {
+		if (data_size >= 5) {
+			buf[i++] = 0x20;
+		}
+		if (data_size >= 6) {
+			buf[i++] = 0x62;
+		}
+		if (data_size >= 7) {
+			buf[i++] = 0x00;
+		}
+		if (data_size >= 8) {
+			if (support_format) {
+				buf[i++] = 0xC0;
+			} else {
+				buf[i++] = 0x00;
+			}
+		}
+	}
+
+	if (data_size > sys_info_offset) {
+		int len = data_size - sys_info_offset;
+		len = (len < 96) ? len : 96;
+
+		memcpy(buf + sys_info_offset, ms_card->raw_sys_info, len);
+	}
+}
+
+static int fill_mode_sense(struct rtsx_chip *chip, unsigned int lun,
+			   unsigned char *buf, int buf_len,
+			   unsigned char page_code)
+{
+	int size = 0;
+	unsigned char *ptr;
+
+	switch (page_code) {
+	case 0x3F:
+	case 0x00:
+		size = 48;
+		ptr = buf;
+		*ptr++ = 0x2F;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size) {
+			memcpy(ptr, inquiry_caching_mode, 20);
+			ptr += 20;
+			memcpy(ptr, inquiry_control_mode, 12);
+			ptr += 12;
+			memcpy(ptr, inquiry_info_exception_mode, 12);
+		} else {
+			size = 4;
+		}
+		break;
+
+	case 0x08:
+		size = 24;
+		ptr = buf;
+		*ptr++ = 0x17;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size)
+			memcpy(ptr, inquiry_caching_mode, 20);
+		else
+			size = 4;
+		break;
+
+	case 0x0A:
+		size = 16;
+		ptr = buf;
+		*ptr++ = 0x0F;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size)
+			memcpy(ptr, inquiry_control_mode, 12);
+		else
+			size = 4;
+		break;
+
+	case 0x1C:
+		size = 16;
+		ptr = buf;
+		*ptr++ = 0x0F;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size)
+			memcpy(ptr, inquiry_info_exception_mode, 12);
+		else
+			size = 4;
+		break;
+
+	default:
+		size = 4;
+		buf[0] = 0x03;
+		buf[1] = 0x00;
+		if (check_card_wp(chip, lun))
+			buf[2] = 0x80;
+		else
+			buf[2] = 0x00;
+		buf[3] = 0x00;
+	}
+
+	return size;
+}
+
+static int fill_mode_sense_10(struct rtsx_chip *chip, unsigned int lun,
+			      unsigned char *buf, int buf_len,
+			      unsigned char page_code)
+{
+	int size = 0;
+	unsigned char *ptr;
+
+	switch (page_code) {
+	case 0x3F:
+	case 0x00:
+		size = 52;
+		ptr = buf;
+		*ptr++ = 0x00;
+		*ptr++ = 0x32;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size) {
+			memcpy(ptr, inquiry_caching_mode, 20);
+			ptr += 20;
+			memcpy(ptr, inquiry_control_mode, 12);
+			ptr += 12;
+			memcpy(ptr, inquiry_info_exception_mode, 12);
+		} else {
+			size = 8;
+		}
+		break;
+
+	case 0x08:
+		size = 28;
+		ptr = buf;
+		*ptr++ = 0x00;
+		*ptr++ = 0x17;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size)
+			memcpy(ptr, inquiry_caching_mode, 20);
+		else
+			size = 8;
+		break;
+
+	case 0x0A:
+		size = 20;
+		ptr = buf;
+		*ptr++ = 0x00;
+		*ptr++ = 0x0F;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size)
+			memcpy(ptr, inquiry_control_mode, 12);
+		else
+			size = 8;
+		break;
+
+	case 0x1C:
+		size = 20;
+		ptr = buf;
+		*ptr++ = 0x00;
+		*ptr++ = 0x0F;
+		*ptr++ = 0x00;
+		if (check_card_wp(chip, lun))
+			*ptr++ = 0x80;
+		else
+			*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+		*ptr++ = 0x00;
+
+		if (buf_len >= size)
+			memcpy(ptr, inquiry_info_exception_mode, 12);
+		else
+			size = 8;
+		break;
+
+	default:
+		size = 8;
+		buf[0] = 0x00;
+		buf[1] = 0x03;
+		buf[2] = 0x00;
+		if (check_card_wp(chip, lun))
+			buf[3] = 0x80;
+		else
+			buf[3] = 0x00;
+		buf[4] = 0x00;
+		buf[5] = 0x00;
+		buf[6] = 0x00;
+		buf[7] = 0x00;
+	}
+
+	return size;
+}
+
+static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	unsigned int dataSize;
+	int status;
+	int pro_formatter_flag;
+	unsigned char pageCode, *buf;
+	u8 card = get_lun_card(chip, lun);
+
+#ifndef SUPPORT_MAGIC_GATE
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		scsi_set_resid(srb, scsi_bufflen(srb));
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+#endif
+
+	pro_formatter_flag = 0;
+	dataSize = 108;
+#ifdef SUPPORT_MAGIC_GATE
+	if ((chip->lun2card[lun] & MS_CARD)) {
+		if (!card || (card == MS_CARD)) {
+			if (chip->mspro_formatter_enable) {
+				pro_formatter_flag = 1;
+			}
+		}
+	}
+#else
+	if (card == MS_CARD) {
+		if (chip->mspro_formatter_enable) {
+			pro_formatter_flag = 1;
+		}
+	}
+#endif
+
+	buf = kmalloc(dataSize, GFP_KERNEL);
+	if (buf == NULL) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	pageCode = srb->cmnd[2] & 0x3f;
+
+	if ((pageCode == 0x3F) || (pageCode == 0x1C) ||
+	    (pageCode == 0x0A) || (pageCode == 0x08) ||
+	    (pageCode == 0x00) ||
+	    (pro_formatter_flag && (pageCode == 0x20))) {
+		if (srb->cmnd[0] == MODE_SENSE) {
+			if (pro_formatter_flag &&
+			    ((pageCode == 0x3F) || (pageCode == 0x20))) {
+				ms_mode_sense(chip, srb->cmnd[0], lun, buf,
+					      dataSize);
+			} else {
+				dataSize =
+				    fill_mode_sense(chip, lun, buf, dataSize,
+						    pageCode);
+			}
+		} else {
+			if (pro_formatter_flag &&
+			    ((pageCode == 0x3F) || (pageCode == 0x20))) {
+				ms_mode_sense(chip, srb->cmnd[0], lun, buf,
+					      dataSize);
+			} else {
+				dataSize =
+				    fill_mode_sense_10(chip, lun, buf,
+						       dataSize, pageCode);
+			}
+		}
+		status = TRANSPORT_GOOD;
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		scsi_set_resid(srb, scsi_bufflen(srb));
+		status = TRANSPORT_FAILED;
+	}
+
+	if (status == TRANSPORT_GOOD) {
+		unsigned int len = min(scsi_bufflen(srb), dataSize);
+		rtsx_stor_set_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+	}
+	kfree(buf);
+
+	return status;
+}
+
+static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &(chip->sd_card);
+#endif
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+	u32 start_sec;
+	u16 sec_cnt;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_run_stat(chip, 1);
+
+	if (!check_card_ready(chip, lun) || (get_card_size(chip, lun) == 0)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_erase_status) {
+		RTSX_DEBUGP(("SD card being erased!\n"));
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_READ_FORBIDDEN);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (get_lun_card(chip, lun) == SD_CARD) {
+		if (sd_card->sd_lock_status & SD_LOCKED) {
+			RTSX_DEBUGP(("SD card locked!\n"));
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_READ_FORBIDDEN);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+#endif
+
+	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10)) {
+		start_sec =
+		    ((u32) srb->cmnd[2] << 24) | ((u32) srb->
+						  cmnd[3] << 16) | ((u32)
+								    srb->
+								    cmnd[4] <<
+								    8) |
+		    ((u32) srb->cmnd[5]);
+		sec_cnt = ((u16) (srb->cmnd[7]) << 8) | srb->cmnd[8];
+	} else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
+		start_sec = ((u32) (srb->cmnd[1] & 0x1F) << 16) |
+		    ((u32) srb->cmnd[2] << 8) | ((u32) srb->cmnd[3]);
+		sec_cnt = srb->cmnd[4];
+	} else if ((srb->cmnd[0] == VENDOR_CMND)
+		   && (srb->cmnd[1] == SCSI_APP_CMD)
+		   && ((srb->cmnd[2] == PP_READ10)
+		       || (srb->cmnd[2] == PP_WRITE10))) {
+		start_sec =
+		    ((u32) srb->cmnd[4] << 24) | ((u32) srb->
+						  cmnd[5] << 16) | ((u32)
+								    srb->
+								    cmnd[6] <<
+								    8) |
+		    ((u32) srb->cmnd[7]);
+		sec_cnt = ((u16) (srb->cmnd[9]) << 8) | srb->cmnd[10];
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if ((start_sec > get_card_size(chip, lun)) ||
+	    ((start_sec + sec_cnt) > get_card_size(chip, lun))) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LBA_OVER_RANGE);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (sec_cnt == 0) {
+		scsi_set_resid(srb, 0);
+		return TRANSPORT_GOOD;
+	}
+
+	if (chip->rw_fail_cnt[lun] == 3) {
+		RTSX_DEBUGP(("read/write fail three times in succession\n"));
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		} else {
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+		}
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+		if (check_card_wp(chip, lun)) {
+			RTSX_DEBUGP(("Write protected card!\n"));
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_WRITE_PROTECT);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	retval = card_rw(srb, chip, start_sec, sec_cnt);
+	if (retval != STATUS_SUCCESS) {
+		if (chip->need_release & chip->lun2card[lun]) {
+			chip->rw_fail_cnt[lun] = 0;
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_NOT_PRESENT);
+		} else {
+			chip->rw_fail_cnt[lun]++;
+			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			} else {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+			}
+		}
+		retval = TRANSPORT_FAILED;
+		TRACE_GOTO(chip, Exit);
+	} else {
+		chip->rw_fail_cnt[lun] = 0;
+		retval = TRANSPORT_GOOD;
+	}
+
+	scsi_set_resid(srb, 0);
+
+ Exit:
+
+	return retval;
+}
+
+static int read_format_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned char *buf;
+	unsigned int lun = SCSI_LUN(srb);
+	unsigned int buf_len;
+	u8 card = get_lun_card(chip, lun);
+	u32 card_size;
+	int desc_cnt;
+	int i = 0;
+
+	if (!check_card_ready(chip, lun)) {
+		if (!chip->mspro_formatter_enable) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_NOT_PRESENT);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	buf_len = (scsi_bufflen(srb) > 12) ? 0x14 : 12;
+
+	buf = kmalloc(buf_len, GFP_KERNEL);
+	if (buf == NULL) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	buf[i++] = 0;
+	buf[i++] = 0;
+	buf[i++] = 0;
+
+	if ((buf_len > 12) && chip->mspro_formatter_enable &&
+	    (chip->lun2card[lun] & MS_CARD) && (!card || (card == MS_CARD))) {
+		buf[i++] = 0x10;
+		desc_cnt = 2;
+	} else {
+		buf[i++] = 0x08;
+		desc_cnt = 1;
+	}
+
+	while (desc_cnt) {
+		if (check_card_ready(chip, lun)) {
+			card_size = get_card_size(chip, lun);
+			buf[i++] = (unsigned char)(card_size >> 24);
+			buf[i++] = (unsigned char)(card_size >> 16);
+			buf[i++] = (unsigned char)(card_size >> 8);
+			buf[i++] = (unsigned char)card_size;
+
+			if (desc_cnt == 2) {
+				buf[i++] = 2;
+			} else {
+				buf[i++] = 0;
+			}
+		} else {
+			buf[i++] = 0xFF;
+			buf[i++] = 0xFF;
+			buf[i++] = 0xFF;
+			buf[i++] = 0xFF;
+
+			if (desc_cnt == 2) {
+				buf[i++] = 3;
+			} else {
+				buf[i++] = 0;
+			}
+		}
+
+		buf[i++] = 0x00;
+		buf[i++] = 0x02;
+		buf[i++] = 0x00;
+
+		desc_cnt--;
+	}
+
+	buf_len = min(scsi_bufflen(srb), buf_len);
+	rtsx_stor_set_xfer_buf(buf, buf_len, srb);
+	kfree(buf);
+
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned char *buf;
+	unsigned int lun = SCSI_LUN(srb);
+	u32 card_size;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	buf = kmalloc(8, GFP_KERNEL);
+	if (buf == NULL) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	card_size = get_card_size(chip, lun);
+	buf[0] = (unsigned char)((card_size - 1) >> 24);
+	buf[1] = (unsigned char)((card_size - 1) >> 16);
+	buf[2] = (unsigned char)((card_size - 1) >> 8);
+	buf[3] = (unsigned char)(card_size - 1);
+
+	buf[4] = 0x00;
+	buf[5] = 0x00;
+	buf[6] = 0x02;
+	buf[7] = 0x00;
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	kfree(buf);
+
+	scsi_set_resid(srb, 0);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_capacity16(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned char *buf;
+	unsigned int lun = SCSI_LUN(srb);
+	u32 card_size;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	buf = kmalloc(32, GFP_KERNEL);
+	if (buf == NULL)
+		TRACE_RET(chip, TRANSPORT_ERROR);
+
+	card_size = get_card_size(chip, lun);
+	buf[0] = 0x00;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+	buf[4] = (unsigned char)((card_size - 1) >> 24);
+	buf[5] = (unsigned char)((card_size - 1) >> 16);
+	buf[6] = (unsigned char)((card_size - 1) >> 8);
+	buf[7] = (unsigned char)(card_size - 1);
+	buf[8] = 0x00;
+	buf[9] = 0x00;
+	buf[10] = 0x02;
+	buf[11] = 0x00;
+	buf[12] = 0x00;
+
+	scsi_set_resid(srb, 0);
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	kfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16) srb->cmnd[2] << 8) | srb->cmnd[3];
+	len = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	if (addr < 0xC000) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	buf = (u8 *) vmalloc(len);
+	if (!buf) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_WRITE_ERR);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = rtsx_read_register(chip, addr + i, buf + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16) srb->cmnd[2] << 8) | srb->cmnd[3];
+	len = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	if (addr < 0xC000) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
+	buf = (u8 *) vmalloc(len);
+	if (buf == NULL) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_WRITE_ERR);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = rtsx_write_register(chip, addr + i, 0xFF, buf[i]);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_sd_csd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (get_lun_card(chip, lun) != SD_CARD) {
+		set_sense_type(chip, lun,
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	scsi_set_resid(srb, 0);
+	rtsx_stor_set_xfer_buf(sd_card->raw_csd, scsi_bufflen(srb), srb);
+
+	return TRANSPORT_GOOD;
+}
+
+static int toggle_gpio_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	toggle_gpio(chip);
+
+	return TRANSPORT_GOOD;
+}
+
+#ifdef _MSG_TRACE
+static int trace_msg_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned char *ptr, *buf = NULL;
+	int i, msg_cnt;
+	u8 clear;
+	unsigned int buf_len;
+
+	buf_len =
+	    4 +
+	    ((2 + MSG_FUNC_LEN + MSG_FILE_LEN +
+	      TIME_VAL_LEN) * TRACE_ITEM_CNT);
+
+	if ((scsi_bufflen(srb) < buf_len) || (scsi_sglist(srb) == NULL)) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	clear = srb->cmnd[2];
+
+	buf = (unsigned char *)vmalloc(scsi_bufflen(srb));
+	if (buf == NULL) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+	ptr = buf;
+
+	if (chip->trace_msg[chip->msg_idx].valid) {
+		msg_cnt = TRACE_ITEM_CNT;
+	} else {
+		msg_cnt = chip->msg_idx;
+	}
+	*(ptr++) = (u8) (msg_cnt >> 24);
+	*(ptr++) = (u8) (msg_cnt >> 16);
+	*(ptr++) = (u8) (msg_cnt >> 8);
+	*(ptr++) = (u8) msg_cnt;
+	RTSX_DEBUGP(("Trace message count is %d\n", msg_cnt));
+
+	for (i = 1; i <= msg_cnt; i++) {
+		int j, idx;
+
+		idx = chip->msg_idx - i;
+		if (idx < 0) {
+			idx += TRACE_ITEM_CNT;
+		}
+
+		*(ptr++) = (u8) (chip->trace_msg[idx].line >> 8);
+		*(ptr++) = (u8) (chip->trace_msg[idx].line);
+		for (j = 0; j < MSG_FUNC_LEN; j++) {
+			*(ptr++) = chip->trace_msg[idx].func[j];
+		}
+		for (j = 0; j < MSG_FILE_LEN; j++) {
+			*(ptr++) = chip->trace_msg[idx].file[j];
+		}
+		for (j = 0; j < TIME_VAL_LEN; j++) {
+			*(ptr++) = chip->trace_msg[idx].timeval_buf[j];
+		}
+	}
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	vfree(buf);
+
+	if (clear) {
+		chip->msg_idx = 0;
+		for (i = 0; i < TRACE_ITEM_CNT; i++) {
+			chip->trace_msg[i].valid = 0;
+		}
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+#endif
+
+static int read_host_reg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 addr, buf[4];
+	u32 val;
+	unsigned int len;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = srb->cmnd[4];
+
+	val = rtsx_readl(chip, addr);
+	RTSX_DEBUGP(("Host register (0x%x): 0x%x\n", addr, val));
+
+	buf[0] = (u8) (val >> 24);
+	buf[1] = (u8) (val >> 16);
+	buf[2] = (u8) (val >> 8);
+	buf[3] = (u8) val;
+
+	len = min(scsi_bufflen(srb), (unsigned int)4);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_host_reg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 addr, buf[4];
+	u32 val;
+	unsigned int len;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = srb->cmnd[4];
+
+	len = min(scsi_bufflen(srb), (unsigned int)4);
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	val =
+	    ((u32) buf[0] << 24) | ((u32) buf[1] << 16) | ((u32) buf[2] << 8)
+	    | buf[3];
+
+	rtsx_writel(chip, addr, val);
+
+	return TRANSPORT_GOOD;
+}
+
+static int set_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned lun = SCSI_LUN(srb);
+
+	if (srb->cmnd[3] == 1) {
+		struct sd_info *sd_card = &(chip->sd_card);
+		struct ms_info *ms_card = &(chip->ms_card);
+
+		switch (srb->cmnd[4]) {
+		case SD_CARD:
+			sd_card->sd_clock = srb->cmnd[5];
+			break;
+
+		case MS_CARD:
+			ms_card->ms_clock = srb->cmnd[5];
+			break;
+
+		default:
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	} else if (srb->cmnd[3] == 2) {
+		if (srb->cmnd[4]) {
+			chip->blink_led = 1;
+		} else {
+			int retval;
+
+			chip->blink_led = 0;
+
+			rtsx_enter_work_state(chip);
+
+			if (chip->ss_en
+			    && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+				rtsx_exit_ss(chip);
+				wait_timeout(100);
+			}
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+			retval = rtsx_force_power_on(chip, SSC_PDCTL);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+
+			turn_off_led(chip);
+		}
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (srb->cmnd[3] == 1) {
+		struct sd_info *sd_card = &(chip->sd_card);
+		struct ms_info *ms_card = &(chip->ms_card);
+		u8 tmp;
+
+		switch (srb->cmnd[4]) {
+		case SD_CARD:
+			tmp = (u8) (sd_card->sd_clock);
+			break;
+
+		case MS_CARD:
+			tmp = (u8) (ms_card->ms_clock);
+			break;
+
+		default:
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+
+		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
+	} else if (srb->cmnd[3] == 2) {
+		u8 tmp = chip->blink_led;
+
+		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int dma_access_ring_buffer(struct scsi_cmnd *srb,
+				  struct rtsx_chip *chip)
+{
+	int retval;
+	unsigned int lun = SCSI_LUN(srb);
+	u16 len;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	len = ((u16) (srb->cmnd[4]) << 8) | srb->cmnd[5];
+	len = min(len, (u16) scsi_bufflen(srb));
+
+	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+		RTSX_DEBUGP(("Read from device\n"));
+	} else {
+		RTSX_DEBUGP(("Write to device\n"));
+	}
+
+	retval = rtsx_transfer_data(chip, 0, scsi_sglist(srb), len,
+				    scsi_sg_count(srb),
+				    srb->sc_data_direction, 1000);
+	if (retval < 0) {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		} else {
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+		}
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+	scsi_set_resid(srb, 0);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_dev_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	struct ms_info *ms_card = &(chip->ms_card);
+	int buf_len;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 card = get_lun_card(chip, lun);
+	u8 status[32];
+#ifdef SUPPORT_OCP
+	u8 oc_now_mask = 0, oc_ever_mask = 0;
+#endif
+
+	memset(status, 0, 32);
+
+	status[0] = (u8) (chip->product_id);
+	status[1] = chip->ic_version;
+
+	if (CHK_AUTODELINK_EN(chip)) {
+		status[2] = 0x10;
+	} else {
+		status[2] = 0x00;
+	}
+
+	status[3] = 20;
+	status[4] = 10;
+	status[5] = 5;
+	status[6] = 21;
+
+	if (chip->card_wp) {
+		status[7] = 0x20;
+	} else {
+		status[7] = 0x00;
+	}
+
+#ifdef SUPPORT_OCP
+	status[8] = 0;
+	oc_now_mask = SD_OC_NOW;
+	oc_ever_mask = SD_OC_EVER;
+
+	if (chip->ocp_stat & oc_now_mask) {
+		status[8] |= 0x02;
+	}
+	if (chip->ocp_stat & oc_ever_mask) {
+		status[8] |= 0x01;
+	}
+#endif
+
+	if (card == SD_CARD) {
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_HCXC(sd_card)) {
+				if (sd_card->capacity > 0x4000000) {
+					status[0x0E] = 0x02;
+				} else {
+					status[0x0E] = 0x01;
+				}
+			} else {
+				status[0x0E] = 0x00;
+			}
+
+			if (CHK_SD_SDR104(sd_card)) {
+				status[0x0F] = 0x03;
+			} else if (CHK_SD_DDR50(sd_card)) {
+				status[0x0F] = 0x04;
+			} else if (CHK_SD_SDR50(sd_card)) {
+				status[0x0F] = 0x02;
+			} else if (CHK_SD_HS(sd_card)) {
+				status[0x0F] = 0x01;
+			} else {
+				status[0x0F] = 0x00;
+			}
+		} else {
+			if (CHK_MMC_SECTOR_MODE(sd_card)) {
+				status[0x0E] = 0x01;
+			} else {
+				status[0x0E] = 0x00;
+			}
+
+			if (CHK_MMC_DDR52(sd_card)) {
+				status[0x0F] = 0x03;
+			} else if (CHK_MMC_52M(sd_card)) {
+				status[0x0F] = 0x02;
+			} else if (CHK_MMC_26M(sd_card)) {
+				status[0x0F] = 0x01;
+			} else {
+				status[0x0F] = 0x00;
+			}
+		}
+	} else if (card == MS_CARD) {
+		if (CHK_MSPRO(ms_card)) {
+			if (CHK_MSXC(ms_card)) {
+				status[0x0E] = 0x01;
+			} else {
+				status[0x0E] = 0x00;
+			}
+
+			if (CHK_HG8BIT(ms_card)) {
+				status[0x0F] = 0x01;
+			} else {
+				status[0x0F] = 0x00;
+			}
+		}
+	}
+#ifdef SUPPORT_SD_LOCK
+	if (card == SD_CARD) {
+		status[0x17] = 0x80;
+		if (sd_card->sd_erase_status) {
+			status[0x17] |= 0x01;
+		}
+		if (sd_card->sd_lock_status & SD_LOCKED) {
+			status[0x17] |= 0x02;
+			status[0x07] |= 0x40;
+		}
+		if (sd_card->sd_lock_status & SD_PWD_EXIST) {
+			status[0x17] |= 0x04;
+		}
+	} else {
+		status[0x17] = 0x00;
+	}
+
+	RTSX_DEBUGP(("status[0x17] = 0x%x\n", status[0x17]));
+#endif
+
+	status[0x18] = 0x8A;
+
+	status[0x1A] = 0x28;
+
+#ifdef SUPPORT_SD_LOCK
+	status[0x1F] = 0x01;
+#endif
+
+	buf_len = min(scsi_bufflen(srb), (unsigned int)sizeof(status));
+	rtsx_stor_set_xfer_buf(status, buf_len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_card_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int buf_len;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 card = get_lun_card(chip, lun);
+	u8 status[32] = { 0 };
+	u8 current_limit[4] = { 0x01, 0x02, 0x04, 0x08 };
+
+	if (card == SD_CARD) {
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_XC(sd_card))
+				status[0] = PP_SD_XC;
+			else if (CHK_SD_HC(sd_card))
+				status[0] = PP_SD_HC;
+			else
+				status[0] = PP_SD_SC;
+
+			status[4] = sd_card->func_group1_mask;
+			status[5] = sd_card->func_group2_mask;
+			status[6] = sd_card->func_group3_mask;
+			status[7] = sd_card->func_group4_mask;
+
+			if (CHK_SD_XC(sd_card))
+				status[16] = PP_SD_XC;
+			else if (CHK_SD_HC(sd_card))
+				status[16] = PP_SD_HC;
+			else
+				status[16] = PP_SD_SC;
+
+			if (CHK_SD_DDR50(sd_card))
+				status[20] = 0x10;
+			else if (CHK_SD_SDR104(sd_card))
+				status[20] = 0x08;
+			else if (CHK_SD_SDR50(sd_card))
+				status[20] = 0x04;
+			else if (CHK_SD_HS(sd_card))
+				status[20] = 0x02;
+			else
+				status[20] = 0x01;
+
+			status[21] = 0x00;
+
+			status[22] = 0x00;
+
+			if (sd_card->current_limit < 4)
+				status[23] =
+				    current_limit[sd_card->current_limit];
+			else
+				status[23] = 0;
+
+		} else {
+			if (CHK_MMC_SECTOR_MODE(sd_card))
+				status[0] = PP_MMC_HC;
+			else
+				status[0] = 0;
+
+			if (CHK_MMC_SECTOR_MODE(sd_card))
+				status[16] = PP_MMC_HC;
+			else
+				status[16] = 0;
+
+		}
+	}
+
+	buf_len = min(scsi_bufflen(srb), (unsigned int)sizeof(status));
+	rtsx_stor_set_xfer_buf(status, buf_len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int set_chip_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	set_sense_type(chip, SCSI_LUN(srb),
+		       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+	TRACE_RET(chip, TRANSPORT_FAILED);
+}
+
+static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval = STATUS_SUCCESS;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 cmd_type, mask, value, idx;
+	u16 addr;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	switch (srb->cmnd[3]) {
+	case INIT_BATCHCMD:
+		rtsx_init_cmd(chip);
+		break;
+
+	case ADD_BATCHCMD:
+		cmd_type = srb->cmnd[4];
+		if (cmd_type > 2) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		addr = (srb->cmnd[5] << 8) | srb->cmnd[6];
+		mask = srb->cmnd[7];
+		value = srb->cmnd[8];
+		rtsx_add_cmd(chip, cmd_type, addr, mask, value);
+		break;
+
+	case SEND_BATCHCMD:
+		retval = rtsx_send_cmd(chip, 0, 1000);
+		break;
+
+	case GET_BATCHRSP:
+		idx = srb->cmnd[4];
+		value = *(rtsx_get_cmd_data(chip) + idx);
+		if (scsi_bufflen(srb) < 1) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		rtsx_stor_set_xfer_buf(&value, 1, srb);
+		scsi_set_resid(srb, 0);
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int suit_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	switch (srb->cmnd[3]) {
+	case INIT_BATCHCMD:
+	case ADD_BATCHCMD:
+	case SEND_BATCHCMD:
+	case GET_BATCHRSP:
+		result = rw_mem_cmd_buf(srb, chip);
+		break;
+	default:
+		result = TRANSPORT_ERROR;
+	}
+
+	return result;
+}
+
+static int read_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+	u16 val;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
+	len = ((u16) srb->cmnd[6] << 8) | srb->cmnd[7];
+
+	if (len % 2) {
+		len -= len % 2;
+	}
+
+	if (len) {
+		buf = (u8 *) vmalloc(len);
+		if (!buf) {
+			TRACE_RET(chip, TRANSPORT_ERROR);
+		}
+
+		retval = rtsx_force_power_on(chip, SSC_PDCTL);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+
+		for (i = 0; i < len / 2; i++) {
+			retval = rtsx_read_phy_register(chip, addr + i, &val);
+			if (retval != STATUS_SUCCESS) {
+				vfree(buf);
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+
+			buf[2 * i] = (u8) (val >> 8);
+			buf[2 * i + 1] = (u8) val;
+		}
+
+		len =
+		    (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
+		rtsx_stor_set_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+		vfree(buf);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+	u16 val;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
+	len = ((u16) srb->cmnd[6] << 8) | srb->cmnd[7];
+
+	if (len % 2) {
+		len -= len % 2;
+	}
+
+	if (len) {
+		len =
+		    (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
+
+		buf = (u8 *) vmalloc(len);
+		if (buf == NULL) {
+			TRACE_RET(chip, TRANSPORT_ERROR);
+		}
+
+		rtsx_stor_get_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+		retval = rtsx_force_power_on(chip, SSC_PDCTL);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+
+		for (i = 0; i < len / 2; i++) {
+			val = ((u16) buf[2 * i] << 8) | buf[2 * i + 1];
+			retval = rtsx_write_phy_register(chip, addr + i, val);
+			if (retval != STATUS_SUCCESS) {
+				vfree(buf);
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		}
+
+		vfree(buf);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u8 func, cfg_mode;
+	u16 addr, len;
+	u8 *buf;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	/* bit [2-1]: func number
+	 * bit [3]: 1 -> cfg mode, 0 -> back door mode
+	 * bit [7-4]: RFU
+	 */
+	func = srb->cmnd[3] & 0x07;
+	if (srb->cmnd[3] & 0x08) {
+		cfg_mode = 1;
+	} else {
+		cfg_mode = 0;
+	}
+	addr = ((u16) (srb->cmnd[4]) << 8) | srb->cmnd[5];
+	len = ((u16) (srb->cmnd[6]) << 8) | srb->cmnd[7];
+
+	RTSX_DEBUGP(("%s: func = %d, addr = 0x%x, len = %d\n", __FUNCTION__,
+		     func, addr, len));
+
+	if (func > 0) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	buf = (u8 *) vmalloc(len);
+	if (!buf) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	if (cfg_mode) {
+		int i;
+		for (i = 0; i < len; i++) {
+			retval =
+			    rtsx_read_config_byte(chip, addr + i, &buf[i]);
+			if (retval < 0) {
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+				vfree(buf);
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		}
+	} else {
+		retval = rtsx_read_cfg_seq(chip, func, addr, buf, len);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			vfree(buf);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	len = (u16) min(scsi_bufflen(srb), (unsigned int)len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u8 func, cfg_mode;
+	u16 addr, len;
+	u8 *buf;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	/* bit [2-1]: func number
+	 * bit [3]: 1 -> cfg mode, 0 -> back door mode
+	 * bit [7-4]: RFU
+	 */
+	func = srb->cmnd[3] & 0x07;
+	if (srb->cmnd[3] & 0x08) {
+		cfg_mode = 1;
+	} else {
+		cfg_mode = 0;
+	}
+	addr = ((u16) (srb->cmnd[4]) << 8) | srb->cmnd[5];
+	len = ((u16) (srb->cmnd[6]) << 8) | srb->cmnd[7];
+
+	RTSX_DEBUGP(("%s: func = %d, addr = 0x%x\n", __FUNCTION__, func,
+		     addr));
+
+	if (func > 0) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
+	buf = (u8 *) vmalloc(len);
+	if (!buf) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	if (cfg_mode) {
+		int i;
+		for (i = 0; i < len; i++) {
+			retval =
+			    rtsx_write_config_byte(chip, addr + i, buf[i]);
+			if (retval < 0) {
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				vfree(buf);
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		}
+	} else {
+		retval = rtsx_write_cfg_seq(chip, func, addr, buf, len);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			vfree(buf);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int app_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	switch (srb->cmnd[2]) {
+	case PP_READ10:
+	case PP_WRITE10:
+		result = read_write(srb, chip);
+		break;
+
+	case READ_HOST_REG:
+		result = read_host_reg(srb, chip);
+		break;
+
+	case WRITE_HOST_REG:
+		result = write_host_reg(srb, chip);
+		break;
+
+	case GET_VAR:
+		result = get_variable(srb, chip);
+		break;
+
+	case SET_VAR:
+		result = set_variable(srb, chip);
+		break;
+
+	case DMA_READ:
+	case DMA_WRITE:
+		result = dma_access_ring_buffer(srb, chip);
+		break;
+
+	case READ_PHY:
+		result = read_phy_register(srb, chip);
+		break;
+
+	case WRITE_PHY:
+		result = write_phy_register(srb, chip);
+		break;
+
+	case READ_CFG:
+		result = read_cfg_byte(srb, chip);
+		break;
+
+	case WRITE_CFG:
+		result = write_cfg_byte(srb, chip);
+		break;
+
+	case SET_CHIP_MODE:
+		result = set_chip_mode(srb, chip);
+		break;
+
+	case SUIT_CMD:
+		result = suit_cmd(srb, chip);
+		break;
+
+	case GET_DEV_STATUS:
+		result = get_dev_status(srb, chip);
+		break;
+
+	case GET_CARD_STATUS:
+		result = get_card_status(srb, chip);
+		break;
+
+	default:
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return result;
+}
+
+static int read_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 rtsx_status[16];
+	int buf_len;
+	unsigned int lun = SCSI_LUN(srb);
+
+	rtsx_status[0] = (u8) (chip->vendor_id >> 8);
+	rtsx_status[1] = (u8) (chip->vendor_id);
+
+	rtsx_status[2] = (u8) (chip->product_id >> 8);
+	rtsx_status[3] = (u8) (chip->product_id);
+
+	rtsx_status[4] = (u8) lun;
+
+	if (chip->card_exist) {
+		if (chip->card_exist & SD_CARD) {
+			rtsx_status[5] = 2;
+		} else if (chip->card_exist & MS_CARD) {
+			rtsx_status[5] = 3;
+		} else {
+			rtsx_status[5] = 7;
+		}
+	} else {
+		rtsx_status[5] = 7;
+	}
+
+	rtsx_status[6] = 1;
+
+	rtsx_status[7] = (u8) (chip->product_id);
+	rtsx_status[8] = chip->ic_version;
+
+	if (check_card_exist(chip, lun)) {
+		rtsx_status[9] = 1;
+	} else {
+		rtsx_status[9] = 0;
+	}
+
+	rtsx_status[10] = 1;
+
+	if (CHECK_LUN_MODE(chip, DEFAULT_SINGLE))
+		rtsx_status[11] = SD_CARD | MS_CARD;
+	else if (CHECK_LUN_MODE(chip, SD_LUN))
+		rtsx_status[11] = SD_CARD;
+	else if (CHECK_LUN_MODE(chip, MS_LUN))
+		rtsx_status[11] = MS_CARD;
+	else
+		rtsx_status[11] = 0;
+
+	if (check_card_ready(chip, lun)) {
+		rtsx_status[12] = 1;
+	} else {
+		rtsx_status[12] = 0;
+	}
+
+	if (get_lun_card(chip, lun) == SD_CARD) {
+		struct sd_info *sd_card = &(chip->sd_card);
+
+		rtsx_status[13] = 0x20;
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_HCXC(sd_card)) {
+				rtsx_status[13] |= 0x04;
+			}
+			if (CHK_SD_HS(sd_card)) {
+				rtsx_status[13] |= 0x02;
+			}
+		} else {
+			rtsx_status[13] |= 0x08;
+			if (CHK_MMC_52M(sd_card)) {
+				rtsx_status[13] |= 0x02;
+			}
+			if (CHK_MMC_SECTOR_MODE(sd_card)) {
+				rtsx_status[13] |= 0x04;
+			}
+		}
+	} else if (get_lun_card(chip, lun) == MS_CARD) {
+		struct ms_info *ms_card = &(chip->ms_card);
+
+		if (CHK_MSPRO(ms_card)) {
+			rtsx_status[13] = 0x38;
+			if (CHK_HG8BIT(ms_card)) {
+				rtsx_status[13] |= 0x04;
+			}
+#ifdef SUPPORT_MSXC
+			if (CHK_MSXC(ms_card)) {
+				rtsx_status[13] |= 0x01;
+			}
+#endif
+		} else {
+			rtsx_status[13] = 0x30;
+		}
+	} else {
+		rtsx_status[13] = 0x70;
+	}
+
+	rtsx_status[14] = 0x78;
+	rtsx_status[15] = 0x82;
+
+	buf_len = min(scsi_bufflen(srb), (unsigned int)sizeof(rtsx_status));
+	rtsx_stor_set_xfer_buf(rtsx_status, buf_len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_card_bus_width(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	u8 card, bus_width;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	card = get_lun_card(chip, lun);
+	if ((card == SD_CARD) || (card == MS_CARD)) {
+		bus_width = chip->card_bus_width[lun];
+	} else {
+		set_sense_type(chip, lun,
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	scsi_set_resid(srb, 0);
+	rtsx_stor_set_xfer_buf(&bus_width, scsi_bufflen(srb), srb);
+
+	return TRANSPORT_GOOD;
+}
+
+static int set_led(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 buf[2] = { 0x55, 0x55 };
+	u8 led_mode, led_val;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+
+	rtsx_force_power_on(chip, SSC_PDCTL);
+
+	led_mode = srb->cmnd[4];
+	led_val = srb->cmnd[5];
+	RTSX_DEBUGP(("led_mode = 0x%x, led_val = 0x%x\n", led_mode, led_val));
+
+	if (led_mode) {
+		chip->led_test_mode = 1;
+		disable_auto_blink(chip);
+
+		if (led_val)
+			turn_on_led(chip);
+		else
+			turn_off_led(chip);
+	} else {
+		chip->led_test_mode = 0;
+	}
+
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	scsi_set_resid(srb, 0);
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+
+	return TRANSPORT_GOOD;
+}
+
+static int vendor_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	switch (srb->cmnd[1]) {
+	case READ_STATUS:
+		result = read_status(srb, chip);
+		break;
+
+	case READ_MEM:
+		result = read_mem(srb, chip);
+		break;
+
+	case WRITE_MEM:
+		result = write_mem(srb, chip);
+		break;
+
+	case TOGGLE_GPIO:
+		result = toggle_gpio_cmd(srb, chip);
+		break;
+
+	case GET_SD_CSD:
+		result = get_sd_csd(srb, chip);
+		break;
+
+	case GET_BUS_WIDTH:
+		result = get_card_bus_width(srb, chip);
+		break;
+
+#ifdef _MSG_TRACE
+	case TRACE_MSG:
+		result = trace_msg_cmd(srb, chip);
+		break;
+#endif
+
+	case SCSI_APP_CMD:
+		result = app_cmd(srb, chip);
+		break;
+
+	case SET_LED:
+		result = set_led(srb, chip);
+		break;
+
+	default:
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return result;
+}
+
+static int ms_format_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned int lun = SCSI_LUN(srb);
+	int retval, quick_format;
+
+	if (get_lun_card(chip, lun) != MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if ((srb->cmnd[3] != 0x4D) || (srb->cmnd[4] != 0x47)
+	    || (srb->cmnd[5] != 0x66) || (srb->cmnd[6] != 0x6D)
+	    || (srb->cmnd[7] != 0x74)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+
+		if (!check_card_ready(chip, lun)
+		    || (get_card_size(chip, lun) == 0)) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_NOT_PRESENT);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	if (srb->cmnd[8] & 0x01) {
+		quick_format = 0;
+	} else {
+		quick_format = 1;
+	}
+
+	if (!(chip->card_ready & MS_CARD)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (chip->card_wp & MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	retval = mspro_format(srb, chip, MS_SHORT_DATA_LEN, quick_format);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+
+#ifdef SUPPORT_PCGL_1P18
+int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned int lun = SCSI_LUN(srb);
+	u8 dev_info_id, data_len;
+	u8 *buf;
+	unsigned int buf_len;
+	int i;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+	if ((get_lun_card(chip, lun) != MS_CARD)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if ((srb->cmnd[2] != 0xB0) || (srb->cmnd[4] != 0x4D) ||
+	    (srb->cmnd[5] != 0x53) || (srb->cmnd[6] != 0x49) ||
+	    (srb->cmnd[7] != 0x44)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	dev_info_id = srb->cmnd[3];
+	if ((CHK_MSXC(ms_card) && (dev_info_id == 0x10)) ||
+	    (!CHK_MSXC(ms_card) && (dev_info_id == 0x13)) ||
+	    !CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (dev_info_id == 0x15) {
+		buf_len = data_len = 0x3A;
+	} else {
+		buf_len = data_len = 0x6A;
+	}
+
+	buf = (u8 *) kmalloc(buf_len, GFP_KERNEL);
+	if (!buf) {
+		TRACE_RET(chip, TRANSPORT_ERROR);
+	}
+
+	i = 0;
+	buf[i++] = 0x00;
+	buf[i++] = data_len;
+	if (CHK_MSXC(ms_card)) {
+		buf[i++] = 0x03;
+	} else {
+		buf[i++] = 0x02;
+	}
+	buf[i++] = 0x01;
+	buf[i++] = 0x00;
+	buf[i++] = 0x00;
+	buf[i++] = 0x00;
+	buf[i++] = 0x01;
+
+	buf[i++] = dev_info_id;
+	if (dev_info_id == 0x15) {
+		data_len = 0x31;
+	} else {
+		data_len = 0x61;
+	}
+	buf[i++] = 0x00;
+	buf[i++] = data_len;
+	buf[i++] = 0x80;
+	if ((dev_info_id == 0x10) || (dev_info_id == 0x13)) {
+		memcpy(buf + i, ms_card->raw_sys_info, 96);
+	} else {
+		memcpy(buf + i, ms_card->raw_model_name, 48);
+	}
+
+	rtsx_stor_set_xfer_buf(buf, buf_len, srb);
+
+	if (dev_info_id == 0x15) {
+		scsi_set_resid(srb, scsi_bufflen(srb) - 0x3C);
+	} else {
+		scsi_set_resid(srb, scsi_bufflen(srb) - 0x6C);
+	}
+
+	kfree(buf);
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int ms_sp_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval = TRANSPORT_ERROR;
+
+	if (srb->cmnd[2] == MS_FORMAT) {
+		retval = ms_format_cmnd(srb, chip);
+	}
+#ifdef SUPPORT_PCGL_1P18
+	else if (srb->cmnd[2] == GET_MS_INFORMATION) {
+		retval = get_ms_information(srb, chip);
+	}
+#endif
+
+	return retval;
+}
+
+#ifdef SUPPORT_CPRM
+static int sd_extention_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	int result;
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	sd_cleanup_work(chip);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+	if ((get_lun_card(chip, lun) != SD_CARD)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	switch (srb->cmnd[0]) {
+	case SD_PASS_THRU_MODE:
+		result = sd_pass_thru_mode(srb, chip);
+		break;
+
+	case SD_EXECUTE_NO_DATA:
+		result = sd_execute_no_data(srb, chip);
+		break;
+
+	case SD_EXECUTE_READ:
+		result = sd_execute_read_data(srb, chip);
+		break;
+
+	case SD_EXECUTE_WRITE:
+		result = sd_execute_write_data(srb, chip);
+		break;
+
+	case SD_GET_RSP:
+		result = sd_get_cmd_rsp(srb, chip);
+		break;
+
+	case SD_HW_RST:
+		result = sd_hw_rst(srb, chip);
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	return result;
+}
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+int mg_report_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+	u8 key_format;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	ms_cleanup_work(chip);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+	if ((get_lun_card(chip, lun) != MS_CARD)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (srb->cmnd[7] != KC_MG_R_PRO) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	key_format = srb->cmnd[10] & 0x3F;
+	RTSX_DEBUGP(("key_format = 0x%x\n", key_format));
+
+	switch (key_format) {
+	case KF_GET_LOC_EKB:
+		if ((scsi_bufflen(srb) == 0x41C) &&
+		    (srb->cmnd[8] == 0x04) && (srb->cmnd[9] == 0x1C)) {
+			retval = mg_get_local_EKB(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	case KF_RSP_CHG:
+		if ((scsi_bufflen(srb) == 0x24) &&
+		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x24)) {
+			retval = mg_get_rsp_chg(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	case KF_GET_ICV:
+		ms_card->mg_entry_num = srb->cmnd[5];
+		if ((scsi_bufflen(srb) == 0x404) &&
+		    (srb->cmnd[8] == 0x04) &&
+		    (srb->cmnd[9] == 0x04) &&
+		    (srb->cmnd[2] == 0x00) &&
+		    (srb->cmnd[3] == 0x00) &&
+		    (srb->cmnd[4] == 0x00) && (srb->cmnd[5] < 32)) {
+			retval = mg_get_ICV(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+
+int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+	u8 key_format;
+
+	RTSX_DEBUGP(("--%s--\n", __FUNCTION__));
+
+	rtsx_enter_work_state(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	ms_cleanup_work(chip);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+	if (check_card_wp(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+	if ((get_lun_card(chip, lun) != MS_CARD)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (srb->cmnd[7] != KC_MG_R_PRO) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	key_format = srb->cmnd[10] & 0x3F;
+	RTSX_DEBUGP(("key_format = 0x%x\n", key_format));
+
+	switch (key_format) {
+	case KF_SET_LEAF_ID:
+		if ((scsi_bufflen(srb) == 0x0C) &&
+		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
+			retval = mg_set_leaf_id(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	case KF_CHG_HOST:
+		if ((scsi_bufflen(srb) == 0x0C) &&
+		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
+			retval = mg_chg(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	case KF_RSP_HOST:
+		if ((scsi_bufflen(srb) == 0x0C) &&
+		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
+			retval = mg_rsp(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	case KF_SET_ICV:
+		ms_card->mg_entry_num = srb->cmnd[5];
+		if ((scsi_bufflen(srb) == 0x404) &&
+		    (srb->cmnd[8] == 0x04) &&
+		    (srb->cmnd[9] == 0x04) &&
+		    (srb->cmnd[2] == 0x00) &&
+		    (srb->cmnd[3] == 0x00) &&
+		    (srb->cmnd[4] == 0x00) && (srb->cmnd[5] < 32)) {
+			retval = mg_set_ICV(srb, chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, TRANSPORT_FAILED);
+			}
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		TRACE_RET(chip, TRANSPORT_FAILED);
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+#endif
+
+int rtsx_scsi_handler(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &(chip->sd_card);
+#endif
+	struct ms_info *ms_card = &(chip->ms_card);
+	unsigned int lun = SCSI_LUN(srb);
+	int result;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_erase_status) {
+		if (!
+		    ((srb->cmnd[0] == VENDOR_CMND)
+		     && (srb->cmnd[1] == SCSI_APP_CMD)
+		     && (srb->cmnd[2] == GET_DEV_STATUS))
+		    && (srb->cmnd[0] != REQUEST_SENSE)) {
+			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04,
+				       0x04, 0, 0);
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+#endif
+
+	if ((get_lun_card(chip, lun) == MS_CARD) &&
+	    (ms_card->format_status == FORMAT_IN_PROGRESS)) {
+		if ((srb->cmnd[0] != REQUEST_SENSE)
+		    && (srb->cmnd[0] != INQUIRY)) {
+			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04,
+				       0x04, 0, (u16) (ms_card->progress));
+			TRACE_RET(chip, TRANSPORT_FAILED);
+		}
+	}
+
+	switch (srb->cmnd[0]) {
+	case READ_10:
+	case WRITE_10:
+	case READ_6:
+	case WRITE_6:
+		result = read_write(srb, chip);
+		break;
+
+	case TEST_UNIT_READY:
+		result = test_unit_ready(srb, chip);
+		break;
+
+	case INQUIRY:
+		result = inquiry(srb, chip);
+		break;
+
+	case READ_CAPACITY:
+		result = read_capacity(srb, chip);
+		break;
+
+	case SERVICE_ACTION_IN:
+		result = read_capacity16(srb, chip);
+		break;
+
+	case START_STOP:
+		result = start_stop_unit(srb, chip);
+		break;
+
+	case ALLOW_MEDIUM_REMOVAL:
+		result = allow_medium_removal(srb, chip);
+		break;
+
+	case REQUEST_SENSE:
+		result = request_sense(srb, chip);
+		break;
+
+	case MODE_SENSE:
+	case MODE_SENSE_10:
+		result = mode_sense(srb, chip);
+		break;
+
+	case 0x23:
+		result = read_format_capacity(srb, chip);
+		break;
+
+	case VENDOR_CMND:
+		result = vendor_cmnd(srb, chip);
+		break;
+
+	case MS_SP_CMND:
+		result = ms_sp_cmnd(srb, chip);
+		break;
+
+#ifdef SUPPORT_CPRM
+	case SD_PASS_THRU_MODE:
+	case SD_EXECUTE_NO_DATA:
+	case SD_EXECUTE_READ:
+	case SD_EXECUTE_WRITE:
+	case SD_GET_RSP:
+	case SD_HW_RST:
+		result = sd_extention_cmnd(srb, chip);
+		break;
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+	case CMD_MSPRO_MG_RKEY:
+		result = mg_report_key(srb, chip);
+		break;
+
+	case CMD_MSPRO_MG_SKEY:
+		result = mg_send_key(srb, chip);
+		break;
+#endif
+
+	case FORMAT_UNIT:
+	case MODE_SELECT:
+	case VERIFY:
+	case MODE_SELECT_10:
+		result = TRANSPORT_GOOD;
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		result = TRANSPORT_FAILED;
+	}
+
+	return result;
+}
diff --git a/drivers/staging/rts5229/rtsx_scsi.h b/drivers/staging/rts5229/rtsx_scsi.h
new file mode 100644
index 0000000..2bae583
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_scsi.h
@@ -0,0 +1,186 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_SCSI_H
+#define __REALTEK_RTSX_SCSI_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+
+#define MS_SP_CMND		0xFA
+#define MS_FORMAT		0xA0
+#define GET_MS_INFORMATION	0xB0
+
+#define VENDOR_CMND		0xF0
+
+#define READ_STATUS		0x09
+#define SET_LED			0x0A
+#define READ_MEM		0x0D
+#define WRITE_MEM		0x0E
+#define GET_BUS_WIDTH		0x13
+#define GET_SD_CSD		0x14
+#define TOGGLE_GPIO		0x15
+#define TRACE_MSG		0x18
+
+#define SCSI_APP_CMD		0x10
+
+#define PP_READ10		0x1A
+#define PP_WRITE10		0x0A
+#define READ_HOST_REG		0x1D
+#define WRITE_HOST_REG		0x0D
+#define SET_VAR			0x05
+#define GET_VAR			0x15
+#define DMA_READ		0x16
+#define DMA_WRITE		0x06
+#define GET_DEV_STATUS		0x10
+#define GET_CARD_STATUS		0x12
+#define SET_CHIP_MODE		0x27
+#define SUIT_CMD		0xE0
+#define WRITE_PHY		0x07
+#define READ_PHY		0x17
+#define WRITE_CFG		0x0E
+#define READ_CFG		0x1E
+
+#define INIT_BATCHCMD		0x41
+#define ADD_BATCHCMD		0x42
+#define SEND_BATCHCMD		0x43
+#define GET_BATCHRSP		0x44
+
+#define CHIP_NORMALMODE		0x00
+#define CHIP_DEBUGMODE		0x01
+
+#define SD_PASS_THRU_MODE	0xD0
+#define SD_EXECUTE_NO_DATA	0xD1
+#define SD_EXECUTE_READ		0xD2
+#define SD_EXECUTE_WRITE	0xD3
+#define SD_GET_RSP		0xD4
+#define SD_HW_RST		0xD6
+
+#ifdef SUPPORT_MAGIC_GATE
+#define CMD_MSPRO_MG_RKEY	0xA4
+#define CMD_MSPRO_MG_SKEY	0xA3
+
+#define KC_MG_R_PRO		0xBE
+
+#define KF_SET_LEAF_ID		0x31
+#define KF_GET_LOC_EKB		0x32
+#define KF_CHG_HOST		0x33
+#define KF_RSP_CHG		0x34
+#define KF_RSP_HOST		0x35
+#define KF_GET_ICV		0x36
+#define KF_SET_ICV		0x37
+#endif
+
+#define	SENSE_TYPE_NO_SENSE				0
+#define	SENSE_TYPE_MEDIA_CHANGE				1
+#define	SENSE_TYPE_MEDIA_NOT_PRESENT			2
+#define	SENSE_TYPE_MEDIA_LBA_OVER_RANGE			3
+#define	SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT		4
+#define	SENSE_TYPE_MEDIA_WRITE_PROTECT			5
+#define	SENSE_TYPE_MEDIA_INVALID_CMD_FIELD		6
+#define	SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR		7
+#define	SENSE_TYPE_MEDIA_WRITE_ERR			8
+#define SENSE_TYPE_FORMAT_IN_PROGRESS			9
+#define SENSE_TYPE_FORMAT_CMD_FAILED			10
+#ifdef SUPPORT_MAGIC_GATE
+#define SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB		0x0b
+#define SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN		0x0c
+#define SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM		0x0d
+#define SENSE_TYPE_MG_WRITE_ERR				0x0e
+#endif
+#ifdef SUPPORT_SD_LOCK
+#define SENSE_TYPE_MEDIA_READ_FORBIDDEN			0x10
+#endif
+
+#define	PIO_MODE_0			0x01
+#define	PIO_MODE_1			0x02
+#define	PIO_MODE_2			0x04
+#define	PIO_MODE_3			0x08
+#define	PIO_MODE_4			0x10
+#define	PIO_MODE_5			0x20
+#define	PIO_MODE_6			0x40
+#define	PIO_MULTI_SETCTORS		0x80
+#define	UDMA_MODE_0			0x01
+#define	UDMA_MODE_1			0x02
+#define	UDMA_MODE_2			0x04
+#define	UDMA_MODE_3			0x08
+#define	UDMA_MODE_4			0x10
+#define	UDMA_MODE_5			0x20
+#define	UDMA_MODE_6			0x40
+#define	UDMA_MODE_7			0x80
+
+#define PP_OC_LUN_STAT_SPRT		0x08
+#define PP_WP_LUN_STAT_SPRT		0x20
+#define PP_VBUS_INFO_SPRT		0x02
+#define PP_CARD_VOLTAGE			0x08
+#define pp_CARD_VOLTAGE_1V8		0x08
+
+#define PP_SD_CPRM			0x08
+#ifdef SUPPORT_MAGIC_GATE
+#define PP_MAGIC_GATE			0x02
+#else
+#define PP_MAGIC_GATE			0x00
+#endif
+
+#define PP_AUTO_DELINK_EN_DEF		0x10
+
+#define PP_VBUS_TOO_LOW			0x02
+
+#define PP_LUN_WRITE_PROTECT		0x20
+#define PP_LUN_READ_PROTECT		0x40
+
+#define PP_FLASH_CODE			0x40
+#define PP_CODE_MODE_FUNC		0x40
+#define PP_FLASH_OP_VER2		0x02
+#define	PP_USB_SPEED_FUNCTION_SUPPORT	0x08
+
+#define PP_SD_LOCK_FUNC			0x01
+#define PP_SD_LOCK_SUPPORT		0x80
+#define PP_SD_ERASING			0x01
+#define PP_SD_LOCKED			0x02
+#define PP_SD_PWD_EXIST			0x04
+
+#define PP_SD_SC			0x00
+#define PP_SD_HC			0x01
+#define PP_SD_XC			0x02
+#define PP_SD_STD			0x00
+#define PP_SD_HIGH			0x01
+#define PP_SD_SDR50			0x02
+#define PP_SD_SDR104			0x03
+#define PP_SD_DDR50			0x04
+
+#define PP_MMC_SC			0x00
+#define PP_MMC_HC			0x01
+#define PP_MMC_XC			0x02
+#define PP_MMC_STD			0x00
+#define PP_MMC_HIGH			0x01
+#define PP_MMC_DDR50			0x04
+
+void scsi_show_command(struct scsi_cmnd *srb);
+void set_sense_type(struct rtsx_chip *chip, unsigned int lun, int sense_type);
+void set_sense_data(struct rtsx_chip *chip, unsigned int lun, u8 err_code,
+		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
+		    u16 sns_key_info1);
+int rtsx_scsi_handler(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+
+#endif
diff --git a/drivers/staging/rts5229/rtsx_sys.h b/drivers/staging/rts5229/rtsx_sys.h
new file mode 100644
index 0000000..2856417
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_sys.h
@@ -0,0 +1,57 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __RTSX_SYS_H
+#define __RTSX_SYS_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+#include "rtsx_card.h"
+
+typedef dma_addr_t ULONG_PTR;
+
+static inline void rtsx_exclusive_enter_ss(struct rtsx_chip *chip)
+{
+	struct rtsx_dev *dev = chip->rtsx;
+
+	spin_lock(&(dev->reg_lock));
+	rtsx_enter_ss(chip);
+	spin_unlock(&(dev->reg_lock));
+}
+
+static inline void rtsx_reset_detected_cards(struct rtsx_chip *chip, int flag)
+{
+	rtsx_reset_cards(chip);
+}
+
+static inline void clear_first_install_mark(struct rtsx_chip *chip)
+{
+}
+
+static inline void notify_refresh_driver(struct rtsx_chip *chip)
+{
+}
+
+#define RTSX_MSG_IN_INT(x)
+
+#endif
diff --git a/drivers/staging/rts5229/rtsx_transport.c b/drivers/staging/rts5229/rtsx_transport.c
new file mode 100644
index 0000000..dc74fb1
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_transport.c
@@ -0,0 +1,890 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+#include "rtsx_scsi.h"
+#include "rtsx_transport.h"
+#include "rtsx_chip.h"
+#include "rtsx_card.h"
+#include "debug.h"
+
+/***********************************************************************
+ * Scatter-gather transfer buffer access routines
+ ***********************************************************************/
+
+/* Copy a buffer of length buflen to/from the srb's transfer buffer.
+ * (Note: for scatter-gather transfers (srb->use_sg > 0), srb->request_buffer
+ * points to a list of s-g entries and we ignore srb->request_bufflen.
+ * For non-scatter-gather transfers, srb->request_buffer points to the
+ * transfer buffer itself and srb->request_bufflen is the buffer's length.)
+ * Update the *index and *offset variables so that the next copy will
+ * pick up from where this one left off. */
+
+unsigned int rtsx_stor_access_xfer_buf(unsigned char *buffer,
+				       unsigned int buflen,
+				       struct scsi_cmnd *srb,
+				       unsigned int *index,
+				       unsigned int *offset,
+				       enum xfer_buf_dir dir)
+{
+	unsigned int cnt;
+
+	/* If not using scatter-gather, just transfer the data directly.
+	 * Make certain it will fit in the available buffer space. */
+	if (scsi_sg_count(srb) == 0) {
+		if (*offset >= scsi_bufflen(srb))
+			return 0;
+		cnt = min(buflen, scsi_bufflen(srb) - *offset);
+		if (dir == TO_XFER_BUF)
+			memcpy((unsigned char *)scsi_sglist(srb) + *offset,
+			       buffer, cnt);
+		else
+			memcpy(buffer, (unsigned char *)scsi_sglist(srb) +
+			       *offset, cnt);
+		*offset += cnt;
+
+		/* Using scatter-gather.  We have to go through the list one entry
+		 * at a time.  Each s-g entry contains some number of pages, and
+		 * each page has to be kmap()'ed separately.  If the page is already
+		 * in kernel-addressable memory then kmap() will return its address.
+		 * If the page is not directly accessible -- such as a user buffer
+		 * located in high memory -- then kmap() will map it to a temporary
+		 * position in the kernel's virtual address space. */
+	} else {
+		struct scatterlist *sg =
+		    (struct scatterlist *)scsi_sglist(srb)
+		    + *index;
+
+		/* This loop handles a single s-g list entry, which may
+		 * include multiple pages.  Find the initial page structure
+		 * and the starting offset within the page, and update
+		 * the *offset and *index values for the next loop. */
+		cnt = 0;
+		while (cnt < buflen && *index < scsi_sg_count(srb)) {
+			struct page *page = sg_page(sg) +
+			    ((sg->offset + *offset) >> PAGE_SHIFT);
+			unsigned int poff =
+			    (sg->offset + *offset) & (PAGE_SIZE - 1);
+			unsigned int sglen = sg->length - *offset;
+
+			if (sglen > buflen - cnt) {
+
+				sglen = buflen - cnt;
+				*offset += sglen;
+			} else {
+
+				*offset = 0;
+				++*index;
+				++sg;
+			}
+
+			/* Transfer the data for all the pages in this
+			 * s-g entry.  For each page: call kmap(), do the
+			 * transfer, and call kunmap() immediately after. */
+			while (sglen > 0) {
+				unsigned int plen = min(sglen, (unsigned int)
+							PAGE_SIZE - poff);
+				unsigned char *ptr = kmap(page);
+
+				if (dir == TO_XFER_BUF)
+					memcpy(ptr + poff, buffer + cnt,
+					       plen);
+				else
+					memcpy(buffer + cnt, ptr + poff,
+					       plen);
+				kunmap(page);
+
+				poff = 0;
+				++page;
+				cnt += plen;
+				sglen -= plen;
+			}
+		}
+	}
+
+	return cnt;
+}
+
+/* Store the contents of buffer into srb's transfer buffer and set the
+* SCSI residue. */
+void rtsx_stor_set_xfer_buf(unsigned char *buffer,
+			    unsigned int buflen, struct scsi_cmnd *srb)
+{
+	unsigned int index = 0, offset = 0;
+
+	rtsx_stor_access_xfer_buf(buffer, buflen, srb, &index, &offset,
+				  TO_XFER_BUF);
+	if (buflen < scsi_bufflen(srb))
+		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
+}
+
+void rtsx_stor_get_xfer_buf(unsigned char *buffer,
+			    unsigned int buflen, struct scsi_cmnd *srb)
+{
+	unsigned int index = 0, offset = 0;
+
+	rtsx_stor_access_xfer_buf(buffer, buflen, srb, &index, &offset,
+				  FROM_XFER_BUF);
+	if (buflen < scsi_bufflen(srb))
+		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
+}
+
+/***********************************************************************
+ * Transport routines
+ ***********************************************************************/
+
+/* Invoke the transport and basic error-handling/recovery methods
+ *
+ * This is used to send the message to the device and receive the response.
+ */
+void rtsx_invoke_transport(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	result = rtsx_scsi_handler(srb, chip);
+
+	/* if the command gets aborted by the higher layers, we need to
+	 * short-circuit all other processing
+	 */
+	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+		RTSX_DEBUGP(("-- command was aborted\n"));
+		srb->result = DID_ABORT << 16;
+		goto Handle_Errors;
+	}
+
+	if (result == TRANSPORT_ERROR) {
+		RTSX_DEBUGP(("-- transport indicates error, resetting\n"));
+		srb->result = DID_ERROR << 16;
+		goto Handle_Errors;
+	}
+
+	srb->result = SAM_STAT_GOOD;
+
+	/*
+	 * If we have a failure, we're going to do a REQUEST_SENSE
+	 * automatically.  Note that we differentiate between a command
+	 * "failure" and an "error" in the transport mechanism.
+	 */
+	if (result == TRANSPORT_FAILED) {
+
+		srb->result = SAM_STAT_CHECK_CONDITION;
+		memcpy(srb->sense_buffer,
+		       (unsigned char *)&(chip->sense_buffer[SCSI_LUN(srb)]),
+		       sizeof(struct sense_data_t));
+	}
+
+	return;
+
+	/* Error and abort processing: try to resynchronize with the device
+	 * by issuing a port reset.  If that fails, try a class-specific
+	 * device reset. */
+ Handle_Errors:
+	return;
+}
+
+/**
+ * rtsx_add_cmd - add a command to command buffer.
+ * @chip: Realtek's card reader chip
+ * @cmd_type: command type, including read/write/check register
+ * @reg_addr: internal card controller register address
+ * @mask: bit mask
+ * @data: register data
+ *
+ * Add a command to command buffer.
+ *
+ * Usually, this function is called after rtsx_init_cmd, which
+ * intializes the command index to zero. After all commands are added,
+ * rtsx_send_cmd or rtsx_send_cmd_no_wait should be called to send those
+ * commands to card reader chip.
+ */
+void rtsx_add_cmd(struct rtsx_chip *chip,
+		  u8 cmd_type, u16 reg_addr, u8 mask, u8 data)
+{
+	u32 *cb = (u32 *) (chip->host_cmds_ptr);
+	u32 val = 0;
+
+	val |= (u32) (cmd_type & 0x03) << 30;
+	val |= (u32) (reg_addr & 0x3FFF) << 16;
+	val |= (u32) mask << 8;
+	val |= (u32) data;
+
+	spin_lock_irq(&chip->rtsx->reg_lock);
+	if (chip->ci < (HOST_CMDS_BUF_LEN / 4)) {
+		cb[(chip->ci)++] = cpu_to_le32(val);
+	}
+	spin_unlock_irq(&chip->rtsx->reg_lock);
+}
+
+/**
+ * rtsx_send_cmd_no_wait - send commands to chip.
+ * @chip: Realtek's card reader chip
+ *
+ * Trigger card reader chip to fetch commands from command buffer.
+ * This funtion returns immediately.
+ */
+void rtsx_send_cmd_no_wait(struct rtsx_chip *chip)
+{
+	u32 val = 1 << 31;
+
+	rtsx_writel(chip, RTSX_HCBAR, chip->host_cmds_addr);
+
+	val |= (u32) (chip->ci * 4) & 0x00FFFFFF;
+	val |= 0x40000000;
+	rtsx_writel(chip, RTSX_HCBCTLR, val);
+}
+
+/**
+ * rtsx_send_cmd - send commands to chip.
+ * @chip: Realtek's card reader chip
+ * @card: this command is relevant to card or not
+ * @timeout: time out in millisecond
+ *
+ * Trigger card reader chip to fetch commands from command buffer.
+ * This funtion will wait for transfer-finished interrupt.
+ *
+ * Returns zero if successful, or a negative error code on failure.
+ */
+int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	u32 val = 1 << 31;
+	long timeleft;
+	int err = 0;
+
+	if (card == SD_CARD) {
+		rtsx->check_card_cd = SD_EXIST;
+	} else if (card == MS_CARD) {
+		rtsx->check_card_cd = MS_EXIST;
+	} else {
+		rtsx->check_card_cd = 0;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	rtsx->done = &trans_done;
+	rtsx->trans_result = TRANS_NOT_READY;
+	init_completion(&trans_done);
+	rtsx->trans_state = STATE_TRANS_CMD;
+
+	rtsx_writel(chip, RTSX_HCBAR, chip->host_cmds_addr);
+
+	val |= (u32) (chip->ci * 4) & 0x00FFFFFF;
+	val |= 0x40000000;
+	rtsx_writel(chip, RTSX_HCBCTLR, val);
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	timeleft =
+	    wait_for_completion_interruptible_timeout(&trans_done,
+						      timeout * HZ / 1000);
+	if (timeleft <= 0) {
+		RTSX_DEBUGP(("chip->int_reg = 0x%x\n", chip->int_reg));
+		err = -ETIMEDOUT;
+		TRACE_GOTO(chip, finish_send_cmd);
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+		err = -EIO;
+	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+		err = 0;
+	}
+	spin_unlock_irq(&rtsx->reg_lock);
+
+ finish_send_cmd:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+
+	if (err < 0) {
+		rtsx_stop_cmd(chip, card);
+	}
+
+	return err;
+}
+
+/**
+ * rtsx_add_sg_tbl - add a sg entry to sg table.
+ * @chip: Realtek's card reader chip
+ * @addr: address of host DMA buffer to transfer data
+ * @len: buffer length in bytes
+ * @option: option
+ *
+ * Add a sg entry to sg table.
+ *
+ * Note: The length field is 20-bit long. So if the buffer length is
+ * longer than 0x80000, this function will divide the buffer into
+ * several small buffers to ensure the length field won't overflow.
+ */
+static inline void rtsx_add_sg_tbl(struct rtsx_chip *chip, u32 addr, u32 len,
+				   u8 option)
+{
+	u64 *sgb = (u64 *) (chip->host_sg_tbl_ptr);
+	u64 val = 0;
+	u32 temp_len = 0;
+	u8 temp_opt = 0;
+
+	do {
+		if (len > 0x80000) {
+			temp_len = 0x80000;
+			temp_opt = option & (~SG_END);
+		} else {
+			temp_len = len;
+			temp_opt = option;
+		}
+		val = ((u64) addr << 32) | ((u64) temp_len << 12) | temp_opt;
+
+		if (chip->sgi < (HOST_SG_TBL_BUF_LEN / 8)) {
+			sgb[(chip->sgi)++] = cpu_to_le64(val);
+		}
+
+		len -= temp_len;
+		addr += temp_len;
+	} while (len);
+}
+
+/**
+ * rtsx_transfer_sglist_adma_partial - transfer sg list partially in adma mode
+ * @chip: Realtek's card reader chip
+ * @card: this command is relevant to card or not
+ * @sg: scatter-gather list
+ * @num_sg: entry count of sg list
+ * @index: next transfer will pick up from which sg entry
+ * @offset: next transfer will pick up from the offset in the sg entry
+ * @size: transfer size in bytes
+ * @dma_dir: transfer direction (DMA_FROM_DEVICE or DMA_TO_DEVICE)
+ * @timeout: time out in millisecond
+ *
+ * Transfer partial data in scatter-gather mode. In this mode,
+ * ADMA option will be turned on.
+ *
+ * This function is usually called in MS card flow. In MS
+ * read/write function, one transfer stage will be divided to several stages.
+ * The *index and *offset variables are used to record the postion in
+ * scatter-gather list that the next transfer will pick up.
+ */
+static int rtsx_transfer_sglist_adma_partial(struct rtsx_chip *chip, u8 card,
+					     struct scatterlist *sg,
+					     int num_sg, unsigned int *index,
+					     unsigned int *offset, int size,
+					     enum dma_data_direction dma_dir,
+					     int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	u8 dir;
+	int sg_cnt, i, resid;
+	int err = 0;
+	long timeleft;
+	struct scatterlist *sg_ptr;
+	u32 val = TRIG_DMA;
+
+	if ((sg == NULL) || (num_sg <= 0) || !offset || !index) {
+		return -EIO;
+	}
+
+	if (dma_dir == DMA_TO_DEVICE) {
+		dir = HOST_TO_DEVICE;
+	} else if (dma_dir == DMA_FROM_DEVICE) {
+		dir = DEVICE_TO_HOST;
+	} else {
+		return -ENXIO;
+	}
+
+	if (card == SD_CARD) {
+		rtsx->check_card_cd = SD_EXIST;
+	} else if (card == MS_CARD) {
+		rtsx->check_card_cd = MS_EXIST;
+	} else {
+		rtsx->check_card_cd = 0;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	rtsx->done = &trans_done;
+
+	rtsx->trans_state = STATE_TRANS_SG;
+	rtsx->trans_result = TRANS_NOT_READY;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	sg_cnt = dma_map_sg(&(rtsx->pci->dev), sg, num_sg, dma_dir);
+
+	resid = size;
+	sg_ptr = sg;
+	chip->sgi = 0;
+	for (i = 0; i < *index; i++) {
+		sg_ptr = sg_next(sg_ptr);
+	}
+	for (i = *index; i < sg_cnt; i++) {
+		dma_addr_t addr;
+		unsigned int len;
+		u8 option;
+
+		addr = sg_dma_address(sg_ptr);
+		len = sg_dma_len(sg_ptr);
+
+		RTSX_DEBUGP(("DMA addr: 0x%x, Len: 0x%x\n",
+			     (unsigned int)addr, len));
+		RTSX_DEBUGP(("*index = %d, *offset = %d\n", *index, *offset));
+
+		addr += *offset;
+
+		if ((len - *offset) > resid) {
+			*offset += resid;
+			len = resid;
+			resid = 0;
+		} else {
+			resid -= (len - *offset);
+			len -= *offset;
+			*offset = 0;
+			*index = *index + 1;
+		}
+		if ((i == (sg_cnt - 1)) || !resid) {
+			option = SG_VALID | SG_END | SG_TRANS_DATA;
+		} else {
+			option = SG_VALID | SG_TRANS_DATA;
+		}
+
+		rtsx_add_sg_tbl(chip, (u32) addr, (u32) len, option);
+
+		if (!resid) {
+			break;
+		}
+
+		sg_ptr = sg_next(sg_ptr);
+	}
+
+	RTSX_DEBUGP(("SG table count = %d\n", chip->sgi));
+
+	val |= (u32) (dir & 0x01) << 29;
+	val |= ADMA_MODE;
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	init_completion(&trans_done);
+
+	rtsx_writel(chip, RTSX_HDBAR, chip->host_sg_tbl_addr);
+	rtsx_writel(chip, RTSX_HDBCTLR, val);
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	timeleft =
+	    wait_for_completion_interruptible_timeout(&trans_done,
+						      timeout * HZ / 1000);
+	if (timeleft <= 0) {
+		RTSX_DEBUGP(("Timeout (%s %d)\n", __FUNCTION__, __LINE__));
+		RTSX_DEBUGP(("chip->int_reg = 0x%x\n", chip->int_reg));
+		err = -ETIMEDOUT;
+		goto out;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+		err = -EIO;
+		spin_unlock_irq(&rtsx->reg_lock);
+		goto out;
+	}
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_NOT_READY) {
+		init_completion(&trans_done);
+		spin_unlock_irq(&rtsx->reg_lock);
+		timeleft =
+		    wait_for_completion_interruptible_timeout(&trans_done,
+							      timeout * HZ /
+							      1000);
+		if (timeleft <= 0) {
+			RTSX_DEBUGP(("Timeout (%s %d)\n", __FUNCTION__,
+				     __LINE__));
+			RTSX_DEBUGP(("chip->int_reg = 0x%x\n",
+				     chip->int_reg));
+			err = -ETIMEDOUT;
+			goto out;
+		}
+	} else {
+		spin_unlock_irq(&rtsx->reg_lock);
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+		err = -EIO;
+	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+		err = 0;
+	}
+	spin_unlock_irq(&rtsx->reg_lock);
+
+ out:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+	dma_unmap_sg(&(rtsx->pci->dev), sg, num_sg, dma_dir);
+
+	if (err < 0) {
+		rtsx_stop_cmd(chip, card);
+	}
+
+	if (err == -ETIMEDOUT) {
+		CATCH_TRIGGER1(chip);
+	}
+
+	return err;
+}
+
+/**
+ * rtsx_transfer_sglist_adma - transfer sg list in adma mode
+ * @chip: Realtek's card reader chip
+ * @card: this command is relevant to card or not
+ * @sg: scatter-gather list
+ * @num_sg: entry count of sg list
+ * @dma_dir: transfer direction (DMA_FROM_DEVICE or DMA_TO_DEVICE)
+ * @timeout: time out in millisecond
+ *
+ * Transfer data in scatter-gather mode. In this mode, ADMA option will be turned on.
+ */
+static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
+				     struct scatterlist *sg, int num_sg,
+				     enum dma_data_direction dma_dir,
+				     int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	u8 dir;
+	int buf_cnt, i;
+	int err = 0;
+	long timeleft;
+	struct scatterlist *sg_ptr;
+
+	if ((sg == NULL) || (num_sg <= 0)) {
+		return -EIO;
+	}
+
+	if (dma_dir == DMA_TO_DEVICE) {
+		dir = HOST_TO_DEVICE;
+	} else if (dma_dir == DMA_FROM_DEVICE) {
+		dir = DEVICE_TO_HOST;
+	} else {
+		return -ENXIO;
+	}
+
+	if (card == SD_CARD) {
+		rtsx->check_card_cd = SD_EXIST;
+	} else if (card == MS_CARD) {
+		rtsx->check_card_cd = MS_EXIST;
+	} else {
+		rtsx->check_card_cd = 0;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	rtsx->done = &trans_done;
+
+	rtsx->trans_state = STATE_TRANS_SG;
+	rtsx->trans_result = TRANS_NOT_READY;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	buf_cnt = dma_map_sg(&(rtsx->pci->dev), sg, num_sg, dma_dir);
+
+	sg_ptr = sg;
+
+	for (i = 0; i <= buf_cnt / (HOST_SG_TBL_BUF_LEN / 8); i++) {
+		u32 val = TRIG_DMA;
+		int sg_cnt, j;
+
+		if (i == buf_cnt / (HOST_SG_TBL_BUF_LEN / 8)) {
+			sg_cnt = buf_cnt % (HOST_SG_TBL_BUF_LEN / 8);
+		} else {
+			sg_cnt = (HOST_SG_TBL_BUF_LEN / 8);
+		}
+
+		chip->sgi = 0;
+		for (j = 0; j < sg_cnt; j++) {
+			dma_addr_t addr = sg_dma_address(sg_ptr);
+			unsigned int len = sg_dma_len(sg_ptr);
+			u8 option;
+
+			RTSX_DEBUGP(("DMA addr: 0x%x, Len: 0x%x\n",
+				     (unsigned int)addr, len));
+
+			if (j == (sg_cnt - 1)) {
+				option = SG_VALID | SG_END | SG_TRANS_DATA;
+			} else {
+				option = SG_VALID | SG_TRANS_DATA;
+			}
+
+			rtsx_add_sg_tbl(chip, (u32) addr, (u32) len, option);
+
+			sg_ptr = sg_next(sg_ptr);
+		}
+
+		RTSX_DEBUGP(("SG table count = %d\n", chip->sgi));
+
+		val |= (u32) (dir & 0x01) << 29;
+		val |= ADMA_MODE;
+
+		spin_lock_irq(&rtsx->reg_lock);
+
+		init_completion(&trans_done);
+
+		rtsx_writel(chip, RTSX_HDBAR, chip->host_sg_tbl_addr);
+		rtsx_writel(chip, RTSX_HDBCTLR, val);
+
+		spin_unlock_irq(&rtsx->reg_lock);
+
+		timeleft =
+		    wait_for_completion_interruptible_timeout(&trans_done,
+							      timeout * HZ /
+							      1000);
+		if (timeleft <= 0) {
+			RTSX_DEBUGP(("Timeout (%s %d)\n", __FUNCTION__,
+				     __LINE__));
+			RTSX_DEBUGP(("chip->int_reg = 0x%x\n",
+				     chip->int_reg));
+			err = -ETIMEDOUT;
+			goto out;
+		}
+
+		spin_lock_irq(&rtsx->reg_lock);
+		if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+			err = -EIO;
+			spin_unlock_irq(&rtsx->reg_lock);
+			goto out;
+		}
+		spin_unlock_irq(&rtsx->reg_lock);
+
+		sg_ptr += sg_cnt;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_NOT_READY) {
+		init_completion(&trans_done);
+		spin_unlock_irq(&rtsx->reg_lock);
+		timeleft =
+		    wait_for_completion_interruptible_timeout(&trans_done,
+							      timeout * HZ /
+							      1000);
+		if (timeleft <= 0) {
+			RTSX_DEBUGP(("Timeout (%s %d)\n", __FUNCTION__,
+				     __LINE__));
+			RTSX_DEBUGP(("chip->int_reg = 0x%x\n",
+				     chip->int_reg));
+			err = -ETIMEDOUT;
+			goto out;
+		}
+	} else {
+		spin_unlock_irq(&rtsx->reg_lock);
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+		err = -EIO;
+	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+		err = 0;
+	}
+	spin_unlock_irq(&rtsx->reg_lock);
+
+ out:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+	dma_unmap_sg(&(rtsx->pci->dev), sg, num_sg, dma_dir);
+
+	if (err < 0) {
+		rtsx_stop_cmd(chip, card);
+	}
+
+	if (err == -ETIMEDOUT) {
+		CATCH_TRIGGER1(chip);
+	}
+
+	return err;
+}
+
+/**
+ * rtsx_transfer_buf - transfer data in linear buffer.
+ * @chip: Realtek's card reader chip
+ * @card: this command is relevant to card or not
+ * @buf: data buffer
+ * @len: buffer length
+ * @dma_dir: transfer direction (DMA_FROM_DEVICE or DMA_TO_DEVICE)
+ * @timeout: time out in millisecond
+ *
+ * Transfer data in linear buffer.
+ */
+static int rtsx_transfer_buf(struct rtsx_chip *chip, u8 card, void *buf,
+			     size_t len, enum dma_data_direction dma_dir,
+			     int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	dma_addr_t addr;
+	u8 dir;
+	int err = 0;
+	u32 val = (1 << 31);
+	long timeleft;
+
+	if ((buf == NULL) || (len <= 0)) {
+		return -EIO;
+	}
+
+	if (dma_dir == DMA_TO_DEVICE) {
+		dir = HOST_TO_DEVICE;
+	} else if (dma_dir == DMA_FROM_DEVICE) {
+		dir = DEVICE_TO_HOST;
+	} else {
+		return -ENXIO;
+	}
+
+	addr = dma_map_single(&(rtsx->pci->dev), buf, len, dma_dir);
+	if (!addr) {
+		return -ENOMEM;
+	}
+
+	if (card == SD_CARD) {
+		rtsx->check_card_cd = SD_EXIST;
+	} else if (card == MS_CARD) {
+		rtsx->check_card_cd = MS_EXIST;
+	} else {
+		rtsx->check_card_cd = 0;
+	}
+
+	val |= (u32) (dir & 0x01) << 29;
+	val |= (u32) (len & 0x00FFFFFF);
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	rtsx->done = &trans_done;
+
+	init_completion(&trans_done);
+
+	rtsx->trans_state = STATE_TRANS_BUF;
+	rtsx->trans_result = TRANS_NOT_READY;
+
+	rtsx_writel(chip, RTSX_HDBAR, addr);
+	rtsx_writel(chip, RTSX_HDBCTLR, val);
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	timeleft =
+	    wait_for_completion_interruptible_timeout(&trans_done,
+						      timeout * HZ / 1000);
+	if (timeleft <= 0) {
+		RTSX_DEBUGP(("Timeout (%s %d)\n", __FUNCTION__, __LINE__));
+		RTSX_DEBUGP(("chip->int_reg = 0x%x\n", chip->int_reg));
+		err = -ETIMEDOUT;
+		goto out;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+		err = -EIO;
+	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+		err = 0;
+	}
+	spin_unlock_irq(&rtsx->reg_lock);
+
+ out:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+	dma_unmap_single(&(rtsx->pci->dev), addr, len, dma_dir);
+
+	if (err < 0) {
+		rtsx_stop_cmd(chip, card);
+	}
+
+	if (err == -ETIMEDOUT) {
+		CATCH_TRIGGER1(chip);
+	}
+
+	return err;
+}
+
+int rtsx_transfer_data_partial(struct rtsx_chip *chip, u8 card, void *buf,
+			       size_t len, int use_sg, unsigned int *index,
+			       unsigned int *offset,
+			       enum dma_data_direction dma_dir, int timeout)
+{
+	int err = 0;
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+		return -EIO;
+	}
+
+	if (use_sg) {
+		err =
+		    rtsx_transfer_sglist_adma_partial(chip, card,
+						      (struct scatterlist *)
+						      buf, use_sg, index,
+						      offset, (int)len,
+						      dma_dir, timeout);
+	} else {
+		err =
+		    rtsx_transfer_buf(chip, card, buf, len, dma_dir, timeout);
+	}
+
+	if (err < 0) {
+		if (RTSX_TST_DELINK(chip)) {
+			RTSX_CLR_DELINK(chip);
+			chip->need_reinit = SD_CARD | MS_CARD;
+			rtsx_reinit_cards(chip, 1);
+		}
+	}
+
+	return err;
+}
+
+int rtsx_transfer_data(struct rtsx_chip *chip, u8 card, void *buf, size_t len,
+		       int use_sg, enum dma_data_direction dma_dir,
+		       int timeout)
+{
+	int err = 0;
+
+	RTSX_DEBUGP(("use_sg = %d\n", use_sg));
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+		return -EIO;
+	}
+
+	if (use_sg) {
+		err =
+		    rtsx_transfer_sglist_adma(chip, card,
+					      (struct scatterlist *)buf,
+					      use_sg, dma_dir, timeout);
+	} else {
+		err =
+		    rtsx_transfer_buf(chip, card, buf, len, dma_dir, timeout);
+	}
+
+	if (err < 0) {
+		if (RTSX_TST_DELINK(chip)) {
+			RTSX_CLR_DELINK(chip);
+			chip->need_reinit = SD_CARD | MS_CARD;
+			rtsx_reinit_cards(chip, 1);
+		}
+	}
+
+	return err;
+}
diff --git a/drivers/staging/rts5229/rtsx_transport.h b/drivers/staging/rts5229/rtsx_transport.h
new file mode 100644
index 0000000..beb4ac3
--- /dev/null
+++ b/drivers/staging/rts5229/rtsx_transport.h
@@ -0,0 +1,69 @@ 
+/* Driver for Realtek PCI-Express card reader
+ * Header file
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#ifndef __REALTEK_RTSX_TRANSPORT_H
+#define __REALTEK_RTSX_TRANSPORT_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+
+#define WAIT_TIME	2000
+
+unsigned int rtsx_stor_access_xfer_buf(unsigned char *buffer,
+				       unsigned int buflen,
+				       struct scsi_cmnd *srb,
+				       unsigned int *index,
+				       unsigned int *offset,
+				       enum xfer_buf_dir dir);
+void rtsx_stor_set_xfer_buf(unsigned char *buffer, unsigned int buflen,
+			    struct scsi_cmnd *srb);
+void rtsx_stor_get_xfer_buf(unsigned char *buffer, unsigned int buflen,
+			    struct scsi_cmnd *srb);
+void rtsx_invoke_transport(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+
+#define rtsx_init_cmd(chip)			((chip)->ci = 0)
+
+void rtsx_add_cmd(struct rtsx_chip *chip,
+		  u8 cmd_type, u16 reg_addr, u8 mask, u8 data);
+void rtsx_send_cmd_no_wait(struct rtsx_chip *chip);
+int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout);
+
+extern inline u8 *rtsx_get_cmd_data(struct rtsx_chip *chip)
+{
+#ifdef CMD_USING_SG
+	return (u8 *) (chip->host_sg_tbl_ptr);
+#else
+	return (u8 *) (chip->host_cmds_ptr);
+#endif
+}
+
+int rtsx_transfer_data(struct rtsx_chip *chip, u8 card, void *buf, size_t len,
+		       int use_sg, enum dma_data_direction dma_dir,
+		       int timeout);
+
+int rtsx_transfer_data_partial(struct rtsx_chip *chip, u8 card, void *buf,
+			       size_t len, int use_sg, unsigned int *index,
+			       unsigned int *offset,
+			       enum dma_data_direction dma_dir, int timeout);
+
+#endif
diff --git a/drivers/staging/rts5229/sd.c b/drivers/staging/rts5229/sd.c
new file mode 100644
index 0000000..ae79bb0
--- /dev/null
+++ b/drivers/staging/rts5229/sd.c
@@ -0,0 +1,5570 @@ 
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * 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, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http:
+ *
+ * Author:
+ *   wwang (wei_wang@realsil.com.cn)
+ *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "sd.h"
+
+#define SD_MAX_RETRY_COUNT	3
+#define RX_TUNING_CNT		3
+#define TX_TUNING_CNT		3
+
+static inline void sd_set_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+
+	sd_card->err_code |= err_code;
+}
+
+static inline void sd_clr_err_code(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+
+	sd_card->err_code = 0;
+}
+
+static inline int sd_check_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+
+	return (sd_card->err_code & err_code);
+}
+
+static int sd_check_data0_status(struct rtsx_chip *chip)
+{
+	u8 stat;
+
+	RTSX_READ_REG(chip, SD_BUS_STAT, &stat);
+
+	if (!(stat & SD_DAT0_STATUS)) {
+		sd_set_err_code(chip, SD_BUSY);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx,
+			       u32 arg, u8 rsp_type, u8 * rsp, int rsp_len)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int timeout = 100;
+	u16 reg_addr;
+	u8 *ptr;
+	int stat_idx = 0;
+	int rty_cnt = 0;
+
+	sd_clr_err_code(chip);
+
+	RTSX_DEBUGP(("SD/MMC CMD %d, arg = 0x%08x\n", cmd_idx, arg));
+
+	if (rsp_type == SD_RSP_TYPE_R1b) {
+		timeout = 3000;
+	}
+
+ RTY_SEND_CMD:
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | cmd_idx);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, (u8) (arg >> 24));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, (u8) (arg >> 16));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, (u8) (arg >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER,
+		     0xFF, SD_TM_CMD_RSP | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
+		     SD_TRANSFER_END | SD_STAT_IDLE,
+		     SD_TRANSFER_END | SD_STAT_IDLE);
+
+	if (rsp_type == SD_RSP_TYPE_R2) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16;
+		     reg_addr++) {
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+		}
+		stat_idx = 16;
+	} else if (rsp_type != SD_RSP_TYPE_R0) {
+		for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4; reg_addr++) {
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+		}
+		stat_idx = 5;
+	}
+
+	rtsx_add_cmd(chip, READ_REG_CMD, SD_STAT1, 0, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		u8 val;
+
+		rtsx_read_register(chip, SD_STAT1, &val);
+		RTSX_DEBUGP(("SD_STAT1: 0x%x\n", val));
+
+		rtsx_read_register(chip, SD_STAT2, &val);
+		RTSX_DEBUGP(("SD_STAT2: 0x%x\n", val));
+
+		if (val & SD_RSP_80CLK_TIMEOUT) {
+			rtsx_clear_sd_error(chip);
+			sd_set_err_code(chip, SD_RSP_TIMEOUT);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		rtsx_read_register(chip, SD_BUS_STAT, &val);
+		RTSX_DEBUGP(("SD_BUS_STAT: 0x%x\n", val));
+
+		if (retval == -ETIMEDOUT) {
+			if (rsp_type & SD_WAIT_BUSY_END) {
+				retval = sd_check_data0_status(chip);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_clear_sd_error(chip);
+					TRACE_RET(chip, retval);
+				}
+			} else {
+				sd_set_err_code(chip, SD_TO_ERR);
+			}
+			retval = STATUS_TIMEDOUT;
+		} else {
+			retval = STATUS_FAIL;
+		}
+		rtsx_clear_sd_error(chip);
+
+		TRACE_RET(chip, retval);
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R0) {
+		return STATUS_SUCCESS;
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if ((ptr[0] & 0xC0) != 0) {
+		sd_set_err_code(chip, SD_STS_ERR);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
+		if (ptr[stat_idx] & SD_CRC7_ERR) {
+			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
+				sd_set_err_code(chip, SD_CRC_ERR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (rty_cnt < SD_MAX_RETRY_COUNT) {
+				wait_timeout(20);
+				rty_cnt++;
+				goto RTY_SEND_CMD;
+			} else {
+				sd_set_err_code(chip, SD_CRC_ERR);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+	}
+
+	if ((rsp_type == SD_RSP_TYPE_R1) || (rsp_type == SD_RSP_TYPE_R1b)) {
+		if ((cmd_idx != SEND_RELATIVE_ADDR)
+		    && (cmd_idx != SEND_IF_COND)) {
+			if (cmd_idx != STOP_TRANSMISSION) {
+				if (ptr[1] & 0x80) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+#ifdef SUPPORT_SD_LOCK
+			if (ptr[1] & 0x7D)
+#else
+			if (ptr[1] & 0x7F)
+#endif
+			{
+				RTSX_DEBUGP(("ptr[1]: 0x%02x\n", ptr[1]));
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (ptr[2] & 0xFF) {
+				RTSX_DEBUGP(("ptr[2]: 0x%02x\n", ptr[2]));
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (ptr[3] & 0x80) {
+				RTSX_DEBUGP(("ptr[3]: 0x%02x\n", ptr[3]));
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+			if (ptr[3] & 0x01) {
+				sd_card->sd_data_buf_ready = 1;
+			} else {
+				sd_card->sd_data_buf_ready = 0;
+			}
+		}
+	}
+
+	if (rsp && rsp_len) {
+		memcpy(rsp, ptr, rsp_len);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static inline void sd_print_debug_reg(struct rtsx_chip *chip)
+{
+#if DBG
+	u8 val;
+
+	rtsx_read_register(chip, SD_STAT1, &val);
+	RTSX_DEBUGP(("SD_STAT1: 0x%x\n", val));
+
+	rtsx_read_register(chip, SD_STAT2, &val);
+	RTSX_DEBUGP(("SD_STAT2: 0x%x\n", val));
+
+	rtsx_read_register(chip, SD_BUS_STAT, &val);
+	RTSX_DEBUGP(("SD_BUS_STAT: 0x%x\n", val));
+
+	rtsx_read_register(chip, SD_CMD0, &val);
+	RTSX_DEBUGP(("SD_CMD0: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_CMD1, &val);
+	RTSX_DEBUGP(("SD_CMD1: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_CMD2, &val);
+	RTSX_DEBUGP(("SD_CMD2: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_CMD3, &val);
+	RTSX_DEBUGP(("SD_CMD3: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_CMD4, &val);
+	RTSX_DEBUGP(("SD_CMD4: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_CMD5, &val);
+	RTSX_DEBUGP(("SD_CMD5: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_TRANSFER, &val);
+	RTSX_DEBUGP(("SD_TRANSFER: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_BLOCK_CNT_L, &val);
+	RTSX_DEBUGP(("SD_BLOCK_CNT_L: 0x%02x\n", val));
+	rtsx_read_register(chip, SD_BLOCK_CNT_H, &val);
+	RTSX_DEBUGP(("SD_BLOCK_CNT_H: 0x%02x\n", val));
+
+#endif
+}
+
+static int sd_read_data(struct rtsx_chip *chip, u8 trans_mode, u8 * cmd,
+			int cmd_len, u16 byte_cnt, u16 blk_cnt, u8 bus_width,
+			u8 * buf, int buf_len, int timeout)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i;
+
+	sd_clr_err_code(chip);
+
+	if (!buf) {
+		buf_len = 0;
+	}
+
+	if (buf_len && (buf_len > PPBUF_LEN)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	rtsx_init_cmd(chip);
+
+	if (cmd_len) {
+		RTSX_DEBUGP(("SD/MMC CMD %d\n", cmd[0] - 0x40));
+		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0 + i, 0xFF,
+				     cmd[i]);
+		}
+	}
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8) byte_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
+		     (u8) (byte_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, (u8) blk_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
+		     (u8) (blk_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
+		     | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	if (trans_mode != SD_TM_AUTO_TUNING) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+			     PINGPONG_BUFFER);
+	}
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+		     trans_mode | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		sd_print_debug_reg(chip);
+
+		if (retval == -ETIMEDOUT) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					    sd_card->sd_addr, SD_RSP_TYPE_R1,
+					    NULL, 0);
+		}
+
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (buf && buf_len) {
+		retval = rtsx_read_ppbuf(chip, buf, buf_len);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_write_data(struct rtsx_chip *chip, u8 trans_mode,
+			 u8 * cmd, int cmd_len, u16 byte_cnt, u16 blk_cnt,
+			 u8 bus_width, u8 * buf, int buf_len, int timeout)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i;
+
+	sd_clr_err_code(chip);
+
+	if (!buf) {
+		buf_len = 0;
+	}
+
+	if (buf_len > PPBUF_LEN) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (buf && buf_len) {
+		retval = rtsx_write_ppbuf(chip, buf, buf_len);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	rtsx_init_cmd(chip);
+
+	if (cmd_len) {
+		RTSX_DEBUGP(("SD/MMC CMD %d\n", cmd[0] - 0x40));
+		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0 + i, 0xFF,
+				     cmd[i]);
+		}
+	}
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8) byte_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
+		     (u8) (byte_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, (u8) blk_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
+		     (u8) (blk_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
+		     | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+		     trans_mode | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		sd_print_debug_reg(chip);
+
+		if (retval == -ETIMEDOUT) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					    sd_card->sd_addr, SD_RSP_TYPE_R1,
+					    NULL, 0);
+		}
+
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i;
+	u8 csd_ver, trans_speed;
+	u8 rsp[16];
+
+	for (i = 0; i < 6; i++) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval =
+		    sd_send_cmd_get_rsp(chip, SEND_CSD, sd_card->sd_addr,
+					SD_RSP_TYPE_R2, rsp, 16);
+		if (retval == STATUS_SUCCESS) {
+			break;
+		}
+	}
+
+	if (i == 6) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	memcpy(sd_card->raw_csd, rsp + 1, 15);
+
+	RTSX_READ_REG(chip, SD_CMD5, sd_card->raw_csd + 15);
+
+	RTSX_DEBUGP(("CSD Response:\n"));
+	RTSX_DUMP(sd_card->raw_csd, 16);
+
+	csd_ver = (rsp[1] & 0xc0) >> 6;
+	RTSX_DEBUGP(("csd_ver = %d\n", csd_ver));
+
+	trans_speed = rsp[4];
+	if ((trans_speed & 0x07) == 0x02) {
+		if ((trans_speed & 0xf8) >= 0x30) {
+			if (chip->asic_code) {
+				sd_card->sd_clock = 47;
+			} else {
+				sd_card->sd_clock = CLK_50;
+			}
+		} else if ((trans_speed & 0xf8) == 0x28) {
+			if (chip->asic_code) {
+				sd_card->sd_clock = 39;
+			} else {
+				sd_card->sd_clock = CLK_40;
+			}
+		} else if ((trans_speed & 0xf8) == 0x20) {
+			if (chip->asic_code) {
+				sd_card->sd_clock = 29;
+			} else {
+				sd_card->sd_clock = CLK_30;
+			}
+		} else if ((trans_speed & 0xf8) >= 0x10) {
+			if (chip->asic_code) {
+				sd_card->sd_clock = 23;
+			} else {
+				sd_card->sd_clock = CLK_20;
+			}
+		} else if ((trans_speed & 0x08) >= 0x08) {
+			if (chip->asic_code) {
+				sd_card->sd_clock = 19;
+			} else {
+				sd_card->sd_clock = CLK_20;
+			}
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MMC_SECTOR_MODE(sd_card)) {
+		sd_card->capacity = 0;
+	} else {
+		if ((!CHK_SD_HCXC(sd_card)) || (csd_ver == 0)) {
+			u8 blk_size, c_size_mult;
+			u16 c_size;
+			blk_size = rsp[6] & 0x0F;
+			c_size = ((u16) (rsp[7] & 0x03) << 10)
+			    + ((u16) rsp[8] << 2)
+			    + ((u16) (rsp[9] & 0xC0) >> 6);
+			c_size_mult = (u8) ((rsp[10] & 0x03) << 1);
+			c_size_mult += (rsp[11] & 0x80) >> 7;
+			sd_card->capacity =
+			    (((u32) (c_size + 1)) *
+			     (1 << (c_size_mult + 2))) << (blk_size - 9);
+		} else {
+			u32 total_sector = 0;
+			total_sector = (((u32) rsp[8] & 0x3f) << 16) |
+			    ((u32) rsp[9] << 8) | (u32) rsp[10];
+			sd_card->capacity = (total_sector + 1) << 10;
+		}
+	}
+
+	if (check_wp) {
+		if (rsp[15] & 0x30) {
+			chip->card_wp |= SD_CARD;
+		}
+		RTSX_DEBUGP(("CSD WP Status: 0x%x\n", rsp[15]));
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_set_sample_push_timing(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+
+	if (CHK_SD_SDR104(sd_card) || CHK_SD_SDR50(sd_card)) {
+		RTSX_WRITE_REG(chip, SD_CFG1, 0x0C | SD_ASYNC_FIFO_NOT_RST,
+			       SD_30_MODE | SD_ASYNC_FIFO_NOT_RST);
+		RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, CLK_LOW_FREQ);
+		RTSX_WRITE_REG(chip, CARD_CLK_SOURCE, 0xFF,
+			       CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
+		RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+	} else if (CHK_SD_DDR50(sd_card) || CHK_MMC_DDR52(sd_card)) {
+		RTSX_WRITE_REG(chip, SD_CFG1, 0x0C | SD_ASYNC_FIFO_NOT_RST,
+			       SD_DDR_MODE | SD_ASYNC_FIFO_NOT_RST);
+		RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, CLK_LOW_FREQ);
+		RTSX_WRITE_REG(chip, CARD_CLK_SOURCE, 0xFF,
+			       CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
+		RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, DDR_VAR_TX_CMD_DAT,
+			       DDR_VAR_TX_CMD_DAT);
+		RTSX_WRITE_REG(chip, SD_SAMPLE_POINT_CTL,
+			       DDR_VAR_RX_DAT | DDR_VAR_RX_CMD,
+			       DDR_VAR_RX_DAT | DDR_VAR_RX_CMD);
+	} else {
+		u8 val = 0;
+
+		RTSX_WRITE_REG(chip, SD_CFG1, 0x0C, SD_20_MODE);
+		RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, CLK_LOW_FREQ);
+		RTSX_WRITE_REG(chip, CARD_CLK_SOURCE, 0xFF,
+			       CRC_FIX_CLK | SD30_VAR_CLK0 | SAMPLE_VAR_CLK1);
+		RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+
+		if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) ==
+		    SD_PUSH_POINT_AUTO) {
+			val = SD20_TX_NEG_EDGE;
+		} else if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) ==
+			   SD_PUSH_POINT_DELAY) {
+			val = SD20_TX_14_AHEAD;
+		} else {
+			val = SD20_TX_NEG_EDGE;
+		}
+		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, SD20_TX_SEL_MASK,
+			       val);
+
+		if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
+		    SD_SAMPLE_POINT_AUTO) {
+			if (chip->asic_code) {
+				if (CHK_SD_HS(sd_card)
+				    || CHK_MMC_52M(sd_card)) {
+					val = SD20_RX_14_DELAY;
+				} else {
+					val = SD20_RX_POS_EDGE;
+				}
+			} else {
+				val = SD20_RX_14_DELAY;
+			}
+		} else if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
+			   SD_SAMPLE_POINT_DELAY) {
+			val = SD20_RX_14_DELAY;
+		} else {
+			val = SD20_RX_POS_EDGE;
+		}
+		RTSX_WRITE_REG(chip, SD_SAMPLE_POINT_CTL, SD20_RX_SEL_MASK,
+			       val);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void sd_choose_proper_clock(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+
+	if (CHK_SD_SDR104(sd_card)) {
+		if (chip->asic_code) {
+			sd_card->sd_clock = chip->asic_sd_sdr104_clk;
+		} else {
+			sd_card->sd_clock = chip->fpga_sd_sdr104_clk;
+		}
+	} else if (CHK_SD_DDR50(sd_card)) {
+		if (chip->asic_code) {
+			sd_card->sd_clock = chip->asic_sd_ddr50_clk;
+		} else {
+			sd_card->sd_clock = chip->fpga_sd_ddr50_clk;
+		}
+	} else if (CHK_SD_SDR50(sd_card)) {
+		if (chip->asic_code) {
+			sd_card->sd_clock = chip->asic_sd_sdr50_clk;
+		} else {
+			sd_card->sd_clock = chip->fpga_sd_sdr50_clk;
+		}
+	} else if (CHK_SD_HS(sd_card)) {
+		if (chip->asic_code) {
+			sd_card->sd_clock = chip->asic_sd_hs_clk;
+		} else {
+			sd_card->sd_clock = chip->fpga_sd_hs_clk;
+		}
+	} else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card)) {
+		if (chip->asic_code) {
+			sd_card->sd_clock = chip->asic_mmc_52m_clk;
+		} else {
+			sd_card->sd_clock = chip->fpga_mmc_52m_clk;
+		}
+	} else if (CHK_MMC_26M(sd_card)) {
+		if (chip->asic_code) {
+			sd_card->sd_clock = 48;
+		} else {
+			sd_card->sd_clock = CLK_50;
+		}
+	}
+}
+
+static int sd_set_clock_divider(struct rtsx_chip *chip, u8 clk_div)
+{
+	RTSX_WRITE_REG(chip, SD_CFG1, SD_CLK_DIVIDE_MASK, clk_div);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_set_init_para(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	if (sd_card->sd_type) {
+		if (chip->sd_retune_clock)
+			sd_card->sd_clock = chip->sd_retune_clock;
+		else
+			sd_choose_proper_clock(chip);
+	}
+
+	retval = sd_set_sample_push_timing(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = switch_clock(chip, sd_card->sd_clock);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_select_card(struct rtsx_chip *chip, int select)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 cmd_idx, cmd_type;
+	u32 addr;
+
+	if (select) {
+		cmd_idx = SELECT_CARD;
+		cmd_type = SD_RSP_TYPE_R1;
+		addr = sd_card->sd_addr;
+	} else {
+		cmd_idx = DESELECT_CARD;
+		cmd_type = SD_RSP_TYPE_R0;
+		addr = 0;
+	}
+
+	retval = sd_send_cmd_get_rsp(chip, cmd_idx, addr, cmd_type, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef SUPPORT_SD_LOCK
+static int sd_update_lock_status(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 rsp[5];
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				SD_RSP_TYPE_R1, rsp, 5);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (rsp[1] & 0x02) {
+		sd_card->sd_lock_status |= SD_LOCKED;
+	} else {
+		sd_card->sd_lock_status &= ~SD_LOCKED;
+	}
+
+	RTSX_DEBUGP(("sd_card->sd_lock_status = 0x%x\n",
+		     sd_card->sd_lock_status));
+
+	if (rsp[1] & 0x01) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int sd_wait_state_data_ready(struct rtsx_chip *chip, u8 state,
+				    u8 data_ready, int polling_cnt)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval, i;
+	u8 rsp[5];
+
+	for (i = 0; i < polling_cnt; i++) {
+		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					     sd_card->sd_addr, SD_RSP_TYPE_R1,
+					     rsp, 5);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (((rsp[3] & 0x1E) == state)
+		    && ((rsp[3] & 0x01) == data_ready)) {
+			return STATUS_SUCCESS;
+		}
+	}
+
+	TRACE_RET(chip, STATUS_FAIL);
+}
+
+static int sd_change_bank_voltage(struct rtsx_chip *chip, u8 voltage)
+{
+	int retval;
+
+	if (voltage == SD_IO_3V3) {
+		if (chip->asic_code) {
+			retval =
+			    rtsx_write_phy_register(chip, 0x08,
+						    0x4FC0 | chip->
+						    phy_voltage);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			RTSX_WRITE_REG(chip, SD_PAD_CTL, SD_IO_USING_1V8, 0);
+		}
+	} else if (voltage == SD_IO_1V8) {
+		if (chip->asic_code) {
+			u16 phy;
+			if (CHECK_PID(chip, 0x5227)) {
+				retval =
+				    rtsx_write_phy_register(chip, 0x11,
+							    0x3C02);
+				if (retval != STATUS_SUCCESS)
+					TRACE_RET(chip, STATUS_FAIL);
+				phy = 0x4C80;
+			} else {
+				phy = 0x4C40;
+			}
+			retval =
+			    rtsx_write_phy_register(chip, 0x08,
+						    phy | chip->phy_voltage);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			RTSX_WRITE_REG(chip, SD_PAD_CTL, SD_IO_USING_1V8,
+				       SD_IO_USING_1V8);
+		}
+	} else {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_voltage_switch(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 stat;
+
+	RTSX_WRITE_REG(chip, SD_BUS_STAT,
+		       SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
+		       SD_CLK_TOGGLE_EN);
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, VOLTAGE_SWITCH, 0, SD_RSP_TYPE_R1, NULL,
+				0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	udelay(chip->sd_voltage_switch_delay);
+
+	RTSX_READ_REG(chip, SD_BUS_STAT, &stat);
+	if (stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
+		    SD_DAT1_STATUS | SD_DAT0_STATUS)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_FORCE_STOP);
+	retval = sd_change_bank_voltage(chip, SD_IO_1V8);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	wait_timeout(50);
+
+	RTSX_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_TOGGLE_EN);
+	wait_timeout(10);
+
+	RTSX_READ_REG(chip, SD_BUS_STAT, &stat);
+	if ((stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
+		     SD_DAT1_STATUS | SD_DAT0_STATUS)) !=
+	    (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
+	     SD_DAT1_STATUS | SD_DAT0_STATUS)) {
+		RTSX_DEBUGP(("SD_BUS_STAT: 0x%x\n", stat));
+		rtsx_write_register(chip, SD_BUS_STAT,
+				    SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
+		rtsx_write_register(chip, CARD_CLK_EN, 0xFF, 0);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, SD_BUS_STAT,
+		       SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_reset_dcm(struct rtsx_chip *chip, u8 tune_dir)
+{
+	if (tune_dir == TUNE_RX) {
+		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_RESET | DCM_RX);
+		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_RX);
+	} else {
+		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_RESET | DCM_TX);
+		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_TX);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point,
+			   u8 tune_dir)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	u16 SD_VP_CTL, SD_DCMPS_CTL;
+	u8 val;
+	int retval;
+	int ddr_rx = 0;
+
+	RTSX_DEBUGP(("sd_change_phase (sample_point = %d, tune_dir = %d)\n",
+		     sample_point, tune_dir));
+
+	if (tune_dir == TUNE_RX) {
+		SD_VP_CTL = SD_VPRX_CTL;
+		SD_DCMPS_CTL = SD_DCMPS_RX_CTL;
+		if (CHK_SD_DDR50(sd_card)) {
+			ddr_rx = 1;
+		}
+	} else {
+		SD_VP_CTL = SD_VPTX_CTL;
+		SD_DCMPS_CTL = SD_DCMPS_TX_CTL;
+	}
+
+	if (chip->asic_code) {
+		RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, CHANGE_CLK);
+		RTSX_WRITE_REG(chip, SD_VP_CTL, 0x1F, sample_point);
+		RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
+		RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET,
+			       PHASE_NOT_RESET);
+		RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, 0);
+	} else {
+#if DBG
+		rtsx_read_register(chip, SD_VP_CTL, &val);
+		RTSX_DEBUGP(("SD_VP_CTL: 0x%x\n", val));
+		rtsx_read_register(chip, SD_DCMPS_CTL, &val);
+		RTSX_DEBUGP(("SD_DCMPS_CTL: 0x%x\n", val));
+#endif
+
+		if (ddr_rx) {
+			RTSX_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE,
+				       PHASE_CHANGE);
+			udelay(50);
+			RTSX_WRITE_REG(chip, SD_VP_CTL, 0xFF,
+				       PHASE_CHANGE | PHASE_NOT_RESET |
+				       sample_point);
+		} else {
+			RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, CHANGE_CLK);
+			udelay(50);
+			RTSX_WRITE_REG(chip, SD_VP_CTL, 0xFF,
+				       PHASE_NOT_RESET | sample_point);
+		}
+		udelay(100);
+
+		rtsx_init_cmd(chip);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_DCMPS_CTL, DCMPS_CHANGE,
+			     DCMPS_CHANGE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, SD_DCMPS_CTL,
+			     DCMPS_CHANGE_DONE, DCMPS_CHANGE_DONE);
+		retval = rtsx_send_cmd(chip, SD_CARD, 100);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_GOTO(chip, Fail);
+		}
+
+		val = *rtsx_get_cmd_data(chip);
+		if (val & DCMPS_ERROR) {
+			TRACE_GOTO(chip, Fail);
+		}
+		if ((val & DCMPS_CURRENT_PHASE) != sample_point) {
+			TRACE_GOTO(chip, Fail);
+		}
+		RTSX_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
+		if (ddr_rx) {
+			RTSX_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE, 0);
+		} else {
+			RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, 0);
+		}
+		udelay(50);
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG1, SD_ASYNC_FIFO_NOT_RST, 0);
+
+	return STATUS_SUCCESS;
+
+ Fail:
+#if DBG
+	rtsx_read_register(chip, SD_VP_CTL, &val);
+	RTSX_DEBUGP(("SD_VP_CTL: 0x%x\n", val));
+	rtsx_read_register(chip, SD_DCMPS_CTL, &val);
+	RTSX_DEBUGP(("SD_DCMPS_CTL: 0x%x\n", val));
+#endif
+
+	rtsx_write_register(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
+	rtsx_write_register(chip, SD_VP_CTL, PHASE_CHANGE, 0);
+	wait_timeout(10);
+	sd_reset_dcm(chip, tune_dir);
+	return STATUS_FAIL;
+}
+
+static int sd_check_spec(struct rtsx_chip *chip, u8 bus_width)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 cmd[5], buf[8];
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	cmd[0] = 0x40 | SEND_SCR;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval =
+	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 8, 1, bus_width,
+			 buf, 8, 250);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	memcpy(sd_card->raw_scr, buf, 8);
+
+	if ((buf[0] & 0x0F) == 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_query_switch_result(struct rtsx_chip *chip, u8 func_group,
+				  u8 func_to_switch, u8 * buf, int buf_len)
+{
+	u8 support_mask = 0, query_switch = 0, switch_busy = 0;
+	int support_offset = 0, query_switch_offset = 0, check_busy_offset =
+	    0;
+
+	if (func_group == SD_FUNC_GROUP_1) {
+		support_offset = FUNCTION_GROUP1_SUPPORT_OFFSET;
+		query_switch_offset = FUNCTION_GROUP1_QUERY_SWITCH_OFFSET;
+		check_busy_offset = FUNCTION_GROUP1_CHECK_BUSY_OFFSET;
+
+		switch (func_to_switch) {
+		case HS_SUPPORT:
+			support_mask = HS_SUPPORT_MASK;
+			query_switch = HS_QUERY_SWITCH_OK;
+			switch_busy = HS_SWITCH_BUSY;
+			break;
+
+		case SDR50_SUPPORT:
+			support_mask = SDR50_SUPPORT_MASK;
+			query_switch = SDR50_QUERY_SWITCH_OK;
+			switch_busy = SDR50_SWITCH_BUSY;
+			break;
+
+		case SDR104_SUPPORT:
+			support_mask = SDR104_SUPPORT_MASK;
+			query_switch = SDR104_QUERY_SWITCH_OK;
+			switch_busy = SDR104_SWITCH_BUSY;
+			break;
+
+		case DDR50_SUPPORT:
+			support_mask = DDR50_SUPPORT_MASK;
+			query_switch = DDR50_QUERY_SWITCH_OK;
+			switch_busy = DDR50_SWITCH_BUSY;
+			break;
+
+		default:
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else if (func_group == SD_FUNC_GROUP_3) {
+		support_offset = FUNCTION_GROUP3_SUPPORT_OFFSET;
+		query_switch_offset = FUNCTION_GROUP3_QUERY_SWITCH_OFFSET;
+		check_busy_offset = FUNCTION_GROUP3_CHECK_BUSY_OFFSET;
+
+		switch (func_to_switch) {
+		case DRIVING_TYPE_A:
+			support_mask = DRIVING_TYPE_A_MASK;
+			query_switch = TYPE_A_QUERY_SWITCH_OK;
+			switch_busy = TYPE_A_SWITCH_BUSY;
+			break;
+
+		case DRIVING_TYPE_C:
+			support_mask = DRIVING_TYPE_C_MASK;
+			query_switch = TYPE_C_QUERY_SWITCH_OK;
+			switch_busy = TYPE_C_SWITCH_BUSY;
+			break;
+
+		case DRIVING_TYPE_D:
+			support_mask = DRIVING_TYPE_D_MASK;
+			query_switch = TYPE_D_QUERY_SWITCH_OK;
+			switch_busy = TYPE_D_SWITCH_BUSY;
+			break;
+
+		default:
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else if (func_group == SD_FUNC_GROUP_4) {
+		support_offset = FUNCTION_GROUP4_SUPPORT_OFFSET;
+		query_switch_offset = FUNCTION_GROUP4_QUERY_SWITCH_OFFSET;
+		check_busy_offset = FUNCTION_GROUP4_CHECK_BUSY_OFFSET;
+
+		switch (func_to_switch) {
+		case CURRENT_LIMIT_400:
+			support_mask = CURRENT_LIMIT_400_MASK;
+			query_switch = CURRENT_LIMIT_400_QUERY_SWITCH_OK;
+			switch_busy = CURRENT_LIMIT_400_SWITCH_BUSY;
+			break;
+
+		case CURRENT_LIMIT_600:
+			support_mask = CURRENT_LIMIT_600_MASK;
+			query_switch = CURRENT_LIMIT_600_QUERY_SWITCH_OK;
+			switch_busy = CURRENT_LIMIT_600_SWITCH_BUSY;
+			break;
+
+		case CURRENT_LIMIT_800:
+			support_mask = CURRENT_LIMIT_800_MASK;
+			query_switch = CURRENT_LIMIT_800_QUERY_SWITCH_OK;
+			switch_busy = CURRENT_LIMIT_800_SWITCH_BUSY;
+			break;
+
+		default:
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (func_group == SD_FUNC_GROUP_1) {
+		if (!(buf[support_offset] & support_mask) ||
+		    ((buf[query_switch_offset] & 0x0F) != query_switch)) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if ((buf[DATA_STRUCTURE_VER_OFFSET] == 0x01) &&
+	    ((buf[check_busy_offset] & switch_busy) == switch_busy)) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_set_ocp_thd(struct rtsx_chip *chip, u8 thd)
+{
+	u16 ocp_para2;
+
+	if (CHECK_PID(chip, 0x5229))
+		ocp_para2 = OCPPARA2;
+	else
+		ocp_para2 = RTS5227_OCPPARA2;
+
+	RTSX_WRITE_REG(chip, ocp_para2, SD_OCP_THD_MASK, thd);
+
+	return 0;
+}
+
+static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
+				u8 func_group, u8 func_to_switch,
+				u8 bus_width)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 cmd[5], buf[64];
+
+	RTSX_DEBUGP(("sd_check_switch_mode (mode = %d, func_group = %d, func_to_switch = %d)\n", mode, func_group, func_to_switch));
+
+	cmd[0] = 0x40 | SWITCH;
+	cmd[1] = mode;
+
+	if (func_group == SD_FUNC_GROUP_1) {
+		cmd[2] = 0xFF;
+		cmd[3] = 0xFF;
+		cmd[4] = 0xF0 + func_to_switch;
+	} else if (func_group == SD_FUNC_GROUP_3) {
+		cmd[2] = 0xFF;
+		cmd[3] = 0xF0 + func_to_switch;
+		cmd[4] = 0xFF;
+	} else if (func_group == SD_FUNC_GROUP_4) {
+		cmd[2] = 0xFF;
+		cmd[3] = 0x0F + (func_to_switch << 4);
+		cmd[4] = 0xFF;
+	} else {
+		cmd[1] = SD_CHECK_MODE;
+		cmd[2] = 0xFF;
+		cmd[3] = 0xFF;
+		cmd[4] = 0xFF;
+	}
+
+	retval =
+	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1, bus_width,
+			 buf, 64, 250);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DUMP(buf, 64);
+
+	if (func_group == NO_ARGUMENT) {
+		sd_card->func_group1_mask = buf[0x0D];
+		sd_card->func_group2_mask = buf[0x0B];
+		sd_card->func_group3_mask = buf[0x09];
+		sd_card->func_group4_mask = buf[0x07];
+
+		RTSX_DEBUGP(("func_group1_mask = 0x%02x\n", buf[0x0D]));
+		RTSX_DEBUGP(("func_group2_mask = 0x%02x\n", buf[0x0B]));
+		RTSX_DEBUGP(("func_group3_mask = 0x%02x\n", buf[0x09]));
+		RTSX_DEBUGP(("func_group4_mask = 0x%02x\n", buf[0x07]));
+	} else {
+		u16 cc = ((u16) buf[0] << 8) | buf[1];
+		RTSX_DEBUGP(("Maximum current consumption: %dmA\n", cc));
+		if ((cc == 0) || (cc > 800)) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		retval =
+		    sd_query_switch_result(chip, func_group, func_to_switch,
+					   buf, 64);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if ((cc > 400) || (func_to_switch > CURRENT_LIMIT_400)) {
+			retval = sd_set_ocp_thd(chip, chip->sd_800mA_ocp_thd);
+			if (retval != STATUS_SUCCESS)
+				TRACE_RET(chip, STATUS_FAIL);
+
+			RTSX_WRITE_REG(chip, CARD_PWR_CTL, PMOS_STRG_MASK,
+				       PMOS_STRG_800mA);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static u8 downgrade_switch_mode(u8 func_group, u8 func_to_switch)
+{
+	if (func_group == SD_FUNC_GROUP_1) {
+		if (func_to_switch > HS_SUPPORT) {
+			func_to_switch--;
+		}
+	} else if (func_group == SD_FUNC_GROUP_4) {
+		if (func_to_switch > CURRENT_LIMIT_200) {
+			func_to_switch--;
+		}
+	}
+
+	return func_to_switch;
+}
+
+static int sd_check_switch(struct rtsx_chip *chip,
+			   u8 func_group, u8 func_to_switch, u8 bus_width)
+{
+	int retval;
+	int i;
+	int switch_good = 0;
+
+	for (i = 0; i < 3; i++) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = sd_check_switch_mode(chip, SD_CHECK_MODE, func_group,
+					      func_to_switch, bus_width);
+		if (retval == STATUS_SUCCESS) {
+			u8 stat;
+
+			retval = sd_check_switch_mode(chip, SD_SWITCH_MODE,
+						      func_group,
+						      func_to_switch,
+						      bus_width);
+			if (retval == STATUS_SUCCESS) {
+				switch_good = 1;
+				break;
+			}
+
+			RTSX_READ_REG(chip, SD_STAT1, &stat);
+			if (stat & SD_CRC16_ERR) {
+				RTSX_DEBUGP(("SD CRC16 error when switching mode\n"));
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		func_to_switch =
+		    downgrade_switch_mode(func_group, func_to_switch);
+
+		wait_timeout(20);
+	}
+
+	if (!switch_good) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i;
+	u8 func_to_switch = 0;
+
+	retval = sd_check_switch_mode(chip, SD_CHECK_MODE,
+				      NO_ARGUMENT, NO_ARGUMENT, bus_width);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	sd_card->func_group1_mask &= ~(sd_card->sd_switch_fail);
+	RTSX_DEBUGP(("After masked with sd_switch_fail, func_group1_mask = 0x%x\n", sd_card->func_group1_mask));
+
+	for (i = 0; i < 4; i++) {
+		switch ((u8) (chip->sd_speed_prior >> (i * 8))) {
+		case SDR104_SUPPORT:
+			if ((sd_card->func_group1_mask & SDR104_SUPPORT_MASK)
+			    && chip->sdr104_en) {
+				func_to_switch = SDR104_SUPPORT;
+			}
+			break;
+
+		case DDR50_SUPPORT:
+			if ((sd_card->func_group1_mask & DDR50_SUPPORT_MASK)
+			    && chip->ddr50_en) {
+				func_to_switch = DDR50_SUPPORT;
+			}
+			break;
+
+		case SDR50_SUPPORT:
+			if ((sd_card->func_group1_mask & SDR50_SUPPORT_MASK)
+			    && chip->sdr50_en) {
+				func_to_switch = SDR50_SUPPORT;
+			}
+			break;
+
+		case HS_SUPPORT:
+			if (sd_card->func_group1_mask & HS_SUPPORT_MASK) {
+				func_to_switch = HS_SUPPORT;
+			}
+			break;
+
+		default:
+			continue;
+		}
+
+		if (func_to_switch) {
+			break;
+		}
+	}
+	RTSX_DEBUGP(("SD_FUNC_GROUP_1: func_to_switch = 0x%02x",
+		     func_to_switch));
+
+#ifdef SUPPORT_SD_LOCK
+	if ((sd_card->sd_lock_status & SD_SDR_RST)
+	    && (DDR50_SUPPORT == func_to_switch)
+	    && (sd_card->func_group1_mask & SDR50_SUPPORT_MASK)) {
+		func_to_switch = SDR50_SUPPORT;
+		RTSX_DEBUGP(("Using SDR50 instead of DDR50 for SD Lock\n"));
+	}
+#endif
+
+	if (func_to_switch) {
+		retval =
+		    sd_check_switch(chip, SD_FUNC_GROUP_1, func_to_switch,
+				    bus_width);
+		if (retval != STATUS_SUCCESS) {
+			if (func_to_switch == SDR104_SUPPORT) {
+				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK;
+			} else if (func_to_switch == DDR50_SUPPORT) {
+				sd_card->sd_switch_fail =
+				    SDR104_SUPPORT_MASK | DDR50_SUPPORT_MASK;
+			} else if (func_to_switch == SDR50_SUPPORT) {
+				sd_card->sd_switch_fail =
+				    SDR104_SUPPORT_MASK | DDR50_SUPPORT_MASK |
+				    SDR50_SUPPORT_MASK;
+			}
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (func_to_switch == SDR104_SUPPORT) {
+			SET_SD_SDR104(sd_card);
+			chip->sd_default_tx_phase =
+			    chip->sd_sdr104_default_tx_phase;
+			chip->sd_default_rx_phase =
+			    chip->sd_sdr104_default_rx_phase;
+		} else if (func_to_switch == DDR50_SUPPORT) {
+			SET_SD_DDR50(sd_card);
+			chip->sd_default_tx_phase =
+			    chip->sd_ddr50_default_tx_phase;
+			chip->sd_default_rx_phase =
+			    chip->sd_ddr50_default_rx_phase;
+		} else if (func_to_switch == SDR50_SUPPORT) {
+			SET_SD_SDR50(sd_card);
+			chip->sd_default_tx_phase =
+			    chip->sd_sdr50_default_tx_phase;
+			chip->sd_default_rx_phase =
+			    chip->sd_sdr50_default_rx_phase;
+		} else {
+			SET_SD_HS(sd_card);
+		}
+	}
+
+	if (CHK_SD_DDR50(sd_card)) {
+		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0x06, 0x04);
+		retval = sd_set_sample_push_timing(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if (!func_to_switch || (func_to_switch == HS_SUPPORT)) {
+		return STATUS_SUCCESS;
+	}
+
+	func_to_switch = 0xFF;
+
+	for (i = 0; i < 4; i++) {
+		switch ((u8) (chip->sd_current_prior >> (i * 8))) {
+		case CURRENT_LIMIT_800:
+			if (sd_card->
+			    func_group4_mask & CURRENT_LIMIT_800_MASK) {
+				func_to_switch = CURRENT_LIMIT_800;
+			}
+			break;
+
+		case CURRENT_LIMIT_600:
+			if (sd_card->
+			    func_group4_mask & CURRENT_LIMIT_600_MASK) {
+				func_to_switch = CURRENT_LIMIT_600;
+			}
+			break;
+
+		case CURRENT_LIMIT_400:
+			if (sd_card->
+			    func_group4_mask & CURRENT_LIMIT_400_MASK) {
+				func_to_switch = CURRENT_LIMIT_400;
+			}
+			break;
+
+		case CURRENT_LIMIT_200:
+			if (sd_card->
+			    func_group4_mask & CURRENT_LIMIT_200_MASK) {
+				func_to_switch = CURRENT_LIMIT_200;
+			}
+			break;
+
+		default:
+			continue;
+		}
+
+		if (func_to_switch != 0xFF) {
+			break;
+		}
+	}
+
+	RTSX_DEBUGP(("SD_FUNC_GROUP_4: func_to_switch = 0x%02x",
+		     func_to_switch));
+
+	if (func_to_switch <= CURRENT_LIMIT_800) {
+		sd_card->current_limit = CURRENT_LIMIT_200;
+
+		retval =
+		    sd_check_switch(chip, SD_FUNC_GROUP_4, func_to_switch,
+				    bus_width);
+		if (retval != STATUS_SUCCESS) {
+			if (sd_check_err_code(chip, SD_NO_CARD)) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		} else {
+			sd_card->current_limit = func_to_switch;
+		}
+		RTSX_DEBUGP(("sd_card->current_limit = 0x%x\n",
+			     sd_card->current_limit));
+		RTSX_DEBUGP(("Switch current limit finished! (%d)\n",
+			     retval));
+	}
+
+	if (CHK_SD_DDR50(sd_card)) {
+		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0x06, 0);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_wait_data_idle(struct rtsx_chip *chip)
+{
+	int retval = STATUS_TIMEDOUT;
+	int i;
+	u8 val = 0;
+
+	for (i = 0; i < 100; i++) {
+		RTSX_READ_REG(chip, SD_DATA_STATE, &val);
+		if (val & SD_DATA_IDLE) {
+			retval = STATUS_SUCCESS;
+			break;
+		}
+		udelay(100);
+	}
+	RTSX_DEBUGP(("SD_DATA_STATE: 0x%02x\n", val));
+
+	return retval;
+}
+
+static int sd_sdr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	int retval;
+	u8 cmd[5];
+
+	retval = sd_change_phase(chip, sample_point, TUNE_RX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	cmd[0] = 0x40 | SEND_TUNING_PATTERN;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_AUTO_TUNING,
+			      cmd, 5, 0x40, 1, SD_BUS_WIDTH_4, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		(void)sd_wait_data_idle(chip);
+
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 cmd[5];
+
+	retval = sd_change_phase(chip, sample_point, TUNE_RX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("sd ddr tuning rx\n"));
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	cmd[0] = 0x40 | SD_STATUS;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ,
+			      cmd, 5, 64, 1, SD_BUS_WIDTH_4, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		(void)sd_wait_data_idle(chip);
+
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mmc_ddr_tunning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 cmd[5], bus_width;
+
+	if (CHK_MMC_8BIT(sd_card)) {
+		bus_width = SD_BUS_WIDTH_8;
+	} else if (CHK_MMC_4BIT(sd_card)) {
+		bus_width = SD_BUS_WIDTH_4;
+	} else {
+		bus_width = SD_BUS_WIDTH_1;
+	}
+
+	retval = sd_change_phase(chip, sample_point, TUNE_RX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("mmc ddr tuning rx\n"));
+
+	cmd[0] = 0x40 | SEND_EXT_CSD;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ,
+			      cmd, 5, 0x200, 1, bus_width, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		(void)sd_wait_data_idle(chip);
+
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_sdr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	retval = sd_change_phase(chip, sample_point, TUNE_TX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+		       SD_RSP_80CLK_TIMEOUT_EN);
+
+	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		if (sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
+			rtsx_write_register(chip, SD_CFG3,
+					    SD_RSP_80CLK_TIMEOUT_EN, 0);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 cmd[5], bus_width;
+
+	retval = sd_change_phase(chip, sample_point, TUNE_TX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_SD(sd_card)) {
+		bus_width = SD_BUS_WIDTH_4;
+	} else {
+		if (CHK_MMC_8BIT(sd_card)) {
+			bus_width = SD_BUS_WIDTH_8;
+		} else if (CHK_MMC_4BIT(sd_card)) {
+			bus_width = SD_BUS_WIDTH_4;
+		} else {
+			bus_width = SD_BUS_WIDTH_1;
+		}
+	}
+
+	retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+		       SD_RSP_80CLK_TIMEOUT_EN);
+
+	cmd[0] = 0x40 | PROGRAM_CSD;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_2,
+			       cmd, 5, 16, 1, bus_width, sd_card->raw_csd, 16,
+			       100);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				    0);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+
+	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+			    SD_RSP_TYPE_R1, NULL, 0);
+
+	return STATUS_SUCCESS;
+}
+
+static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
+				u8 tune_dir)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	struct timing_phase_path path[MAX_PHASE + 1];
+	int i, j, cont_path_cnt;
+	int new_block, max_len, final_path_idx;
+	u8 final_phase = 0xFF;
+
+	if (phase_map == 0xFFFFFFFF) {
+		if (tune_dir == TUNE_RX) {
+			final_phase = (u8) chip->sd_default_rx_phase;
+		} else {
+			final_phase = (u8) chip->sd_default_tx_phase;
+		}
+
+		goto Search_Finish;
+	}
+
+	cont_path_cnt = 0;
+	new_block = 1;
+	j = 0;
+	for (i = 0; i < MAX_PHASE + 1; i++) {
+		if (phase_map & (1 << i)) {
+			if (new_block) {
+				new_block = 0;
+				j = cont_path_cnt++;
+				path[j].start = i;
+				path[j].end = i;
+			} else {
+				path[j].end = i;
+			}
+		} else {
+			new_block = 1;
+			if (cont_path_cnt) {
+				int idx = cont_path_cnt - 1;
+				path[idx].len =
+				    path[idx].end - path[idx].start + 1;
+				path[idx].mid =
+				    path[idx].start + path[idx].len / 2;
+			}
+		}
+	}
+
+	if (cont_path_cnt == 0) {
+		RTSX_DEBUGP(("No continuous phase path\n"));
+		goto Search_Finish;
+	} else {
+		int idx = cont_path_cnt - 1;
+		path[idx].len = path[idx].end - path[idx].start + 1;
+		path[idx].mid = path[idx].start + path[idx].len / 2;
+	}
+
+	if ((path[0].start == 0)
+	    && (path[cont_path_cnt - 1].end == MAX_PHASE)) {
+		path[0].start = path[cont_path_cnt - 1].start - MAX_PHASE - 1;
+		path[0].len += path[cont_path_cnt - 1].len;
+		path[0].mid = path[0].start + path[0].len / 2;
+		if (path[0].mid < 0) {
+			path[0].mid += MAX_PHASE + 1;
+		}
+		cont_path_cnt--;
+	}
+
+	max_len = 0;
+	final_phase = 0;
+	final_path_idx = 0;
+	for (i = 0; i < cont_path_cnt; i++) {
+		if (path[i].len > max_len) {
+			max_len = path[i].len;
+			final_phase = (u8) path[i].mid;
+			final_path_idx = i;
+		}
+
+		RTSX_DEBUGP(("path[%d].start = %d\n", i, path[i].start));
+		RTSX_DEBUGP(("path[%d].end = %d\n", i, path[i].end));
+		RTSX_DEBUGP(("path[%d].len = %d\n", i, path[i].len));
+		RTSX_DEBUGP(("path[%d].mid = %d\n", i, path[i].mid));
+		RTSX_DEBUGP(("\n"));
+	}
+
+	if (tune_dir == TUNE_TX) {
+		if (CHK_SD_SDR104(sd_card)) {
+			if (max_len > 15) {
+				int temp_mid = (max_len - 16) / 2;
+				int temp_final_phase =
+				    path[final_path_idx].end - (max_len -
+								(6 +
+								 temp_mid));
+
+				if (temp_final_phase < 0) {
+					final_phase =
+					    (u8) (temp_final_phase +
+						  MAX_PHASE + 1);
+				} else {
+					final_phase = (u8) temp_final_phase;
+				}
+			}
+		} else if (CHK_SD_SDR50(sd_card)) {
+			if (max_len > 12) {
+				int temp_mid = (max_len - 13) / 2;
+				int temp_final_phase =
+				    path[final_path_idx].end - (max_len -
+								(3 +
+								 temp_mid));
+
+				if (temp_final_phase < 0) {
+					final_phase =
+					    (u8) (temp_final_phase +
+						  MAX_PHASE + 1);
+				} else {
+					final_phase = (u8) temp_final_phase;
+				}
+			}
+		}
+	}
+
+ Search_Finish:
+	RTSX_DEBUGP(("Final choosen phase: %d\n", final_phase));
+	return final_phase;
+}
+
+static int sd_tuning_rx(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i, j;
+	u32 raw_phase_map[RX_TUNING_CNT] = { 0 }, phase_map;
+	u8 final_phase;
+	int (*tuning_cmd) (struct rtsx_chip * chip, u8 sample_point);
+
+	if (CHK_SD(sd_card)) {
+		if (CHK_SD_DDR50(sd_card)) {
+			tuning_cmd = sd_ddr_tuning_rx_cmd;
+		} else {
+			tuning_cmd = sd_sdr_tuning_rx_cmd;
+		}
+	} else {
+		if (CHK_MMC_DDR52(sd_card)) {
+			tuning_cmd = mmc_ddr_tunning_rx_cmd;
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	for (i = 0; i < RX_TUNING_CNT; i++) {
+		raw_phase_map[i] = 0;
+		for (j = MAX_PHASE; j >= 0; j--) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			retval = tuning_cmd(chip, (u8) j);
+			if (retval == STATUS_SUCCESS) {
+				raw_phase_map[i] |= 1 << j;
+			}
+		}
+
+		if (raw_phase_map[i] == 0)
+			break;
+	}
+
+	phase_map = 0xFFFFFFFF;
+	for (i = 0; i < RX_TUNING_CNT; i++) {
+		RTSX_DEBUGP(("RX raw_phase_map[%d] = 0x%08x\n", i,
+			     raw_phase_map[i]));
+		phase_map &= raw_phase_map[i];
+	}
+	RTSX_DEBUGP(("RX phase_map = 0x%08x\n", phase_map));
+
+	final_phase = sd_search_final_phase(chip, phase_map, TUNE_RX);
+	if (final_phase == 0xFF) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_change_phase(chip, final_phase, TUNE_RX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i;
+	u32 phase_map;
+	u8 final_phase;
+
+	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+		       SD_RSP_80CLK_TIMEOUT_EN);
+
+	phase_map = 0;
+	for (i = MAX_PHASE; i >= 0; i--) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			rtsx_write_register(chip, SD_CFG3,
+					    SD_RSP_80CLK_TIMEOUT_EN, 0);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = sd_change_phase(chip, (u8) i, TUNE_TX);
+		if (retval != STATUS_SUCCESS) {
+			continue;
+		}
+
+		retval =
+		    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+					SD_RSP_TYPE_R1, NULL, 0);
+		if ((retval == STATUS_SUCCESS)
+		    || !sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
+			phase_map |= 1 << i;
+		}
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+
+	RTSX_DEBUGP(("DDR TX pre tune phase_map = 0x%08x\n", phase_map));
+
+	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
+	if (final_phase == 0xFF) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_change_phase(chip, final_phase, TUNE_TX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("DDR TX pre tune phase: %d\n", (int)final_phase));
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_tuning_tx(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int i, j;
+	u32 raw_phase_map[TX_TUNING_CNT] = { 0 }, phase_map;
+	u8 final_phase;
+	int (*tuning_cmd) (struct rtsx_chip * chip, u8 sample_point);
+
+	if (CHK_SD(sd_card)) {
+		if (CHK_SD_DDR50(sd_card)) {
+			tuning_cmd = sd_ddr_tuning_tx_cmd;
+		} else {
+			tuning_cmd = sd_sdr_tuning_tx_cmd;
+		}
+	} else {
+		if (CHK_MMC_DDR52(sd_card)) {
+			tuning_cmd = sd_ddr_tuning_tx_cmd;
+		} else {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	for (i = 0; i < TX_TUNING_CNT; i++) {
+		raw_phase_map[i] = 0;
+		for (j = MAX_PHASE; j >= 0; j--) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				rtsx_write_register(chip, SD_CFG3,
+						    SD_RSP_80CLK_TIMEOUT_EN,
+						    0);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			retval = tuning_cmd(chip, (u8) j);
+			if (retval == STATUS_SUCCESS) {
+				raw_phase_map[i] |= 1 << j;
+			}
+		}
+
+		if (raw_phase_map[i] == 0)
+			break;
+	}
+
+	phase_map = 0xFFFFFFFF;
+	for (i = 0; i < TX_TUNING_CNT; i++) {
+		RTSX_DEBUGP(("TX raw_phase_map[%d] = 0x%08x\n", i,
+			     raw_phase_map[i]));
+		phase_map &= raw_phase_map[i];
+	}
+	RTSX_DEBUGP(("TX phase_map = 0x%08x\n", phase_map));
+
+	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
+	if (final_phase == 0xFF) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_change_phase(chip, final_phase, TUNE_TX);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_sdr_tuning(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (chip->sdr_tx_tuning_en) {
+		retval = sd_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+	} else {
+		retval =
+		    sd_change_phase(chip, (u8) chip->sd_default_tx_phase,
+				    TUNE_TX);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_tuning_rx(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_tuning(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_ddr_pre_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		retval =
+		    sd_change_phase(chip, (u8) chip->sd_ddr_tx_phase,
+				    TUNE_TX);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval = sd_tuning_rx(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mmc_ddr_tuning(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_ddr_pre_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		retval =
+		    sd_change_phase(chip, (u8) chip->mmc_ddr_tx_phase,
+				    TUNE_TX);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	retval = sd_tuning_rx(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_set_sd30_drive(struct rtsx_chip *chip, u8 voltage)
+{
+	u8 sd30_drive_sel;
+	u8 sd30_clk_drive_sel;
+	u8 sd30_cmd_drive_sel;
+	u8 sd30_dat_drive_sel;
+
+	if (voltage == SD_IO_3V3) {
+		sd30_drive_sel = chip->sd30_drive_sel_3v3;
+		sd30_clk_drive_sel = chip->sd30_clk_drive_sel_3v3;
+		sd30_cmd_drive_sel = chip->sd30_cmd_drive_sel_3v3;
+		sd30_dat_drive_sel = chip->sd30_dat_drive_sel_3v3;
+	} else {
+		sd30_drive_sel = chip->sd30_drive_sel_1v8;
+		sd30_clk_drive_sel = chip->sd30_clk_drive_sel_1v8;
+		sd30_cmd_drive_sel = chip->sd30_cmd_drive_sel_1v8;
+		sd30_dat_drive_sel = chip->sd30_dat_drive_sel_1v8;
+	}
+
+	if (CHECK_PID(chip, 0x5229)) {
+		RTSX_WRITE_REG(chip, SD30_DRIVE_SEL, 0xFF, sd30_drive_sel);
+	} else {
+		RTSX_WRITE_REG(chip, SD30_CLK_DRIVE_SEL, 0xFF,
+			       sd30_clk_drive_sel);
+		RTSX_WRITE_REG(chip, SD30_CMD_DRIVE_SEL, 0xFF,
+			       sd30_cmd_drive_sel);
+		RTSX_WRITE_REG(chip, SD30_DAT_DRIVE_SEL, 0xFF,
+			       sd30_dat_drive_sel);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_switch_clock(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	int re_tuning = 0;
+
+	retval = select_card(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if ((CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card))) {
+		if (sd_card->need_retune
+		    && (sd_card->sd_clock != chip->cur_clk)) {
+			re_tuning = 1;
+			sd_card->need_retune = 0;
+		}
+	}
+
+	retval = switch_clock(chip, sd_card->sd_clock);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (re_tuning) {
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_DDR50(sd_card)) {
+				retval = sd_ddr_tuning(chip);
+			} else {
+				retval = sd_sdr_tuning(chip);
+			}
+		} else {
+			if (CHK_MMC_DDR52(sd_card)) {
+				retval = mmc_ddr_tuning(chip);
+			}
+		}
+
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_prepare_reset(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	if (chip->asic_code) {
+		sd_card->sd_clock = 29;
+	} else {
+		sd_card->sd_clock = CLK_30;
+	}
+
+	sd_card->sd_type = 0;
+	sd_card->seq_mode = 0;
+	sd_card->sd_data_buf_ready = 0;
+	sd_card->capacity = 0;
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status = 0;
+	sd_card->sd_erase_status = 0;
+#endif
+
+	chip->capacity[chip->card2lun[SD_CARD]] = 0;
+
+	retval = sd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, retval);
+	}
+
+	RTSX_WRITE_REG(chip, SD_CFG1, 0xFF,
+		       SD_CLK_DIVIDE_128 | SD_20_MODE | SD_BUS_WIDTH_1);
+	RTSX_WRITE_REG(chip, SD_SAMPLE_POINT_CTL, 0xFF, SD20_RX_POS_EDGE);
+	RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0xFF, 0);
+
+	RTSX_WRITE_REG(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
+		       SD_STOP | SD_CLR_ERR);
+
+	retval = select_card(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_pull_ctl_disable(struct rtsx_chip *chip)
+{
+	RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF, 0x55);
+	if (CHECK_IC_VER(chip, IC_VER_C))
+		RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF, 0xE5);
+	else
+		RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF, 0xD5);
+
+	return STATUS_SUCCESS;
+}
+
+int sd_pull_ctl_enable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	/* SD Data0~3: pull up
+	 * SD CD: pull up
+	 * SD WP: pull up
+	 * SD CMD: pull up
+	 * SD CLK: pull down
+	 */
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0xAA);
+	if (CHECK_IC_VER(chip, IC_VER_C))
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xD9);
+	else
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xE9);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 100);
+	if (retval < 0) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_init_power(struct rtsx_chip *chip)
+{
+	int retval;
+
+	RTSX_WRITE_REG(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_OFF);
+
+	retval = sd_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_change_bank_voltage(chip, SD_IO_3V3);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	retval = sd_set_sd30_drive(chip, SD_IO_3V3);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	if (!chip->ft2_fast_mode) {
+		wait_timeout(250);
+	}
+
+	retval = enable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (chip->asic_code) {
+		retval = sd_pull_ctl_enable(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	} else {
+		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
+			       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
+	}
+
+	if (chip->ft2_fast_mode) {
+		RTSX_WRITE_REG(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_ON);
+	} else {
+#ifdef SUPPORT_OCP
+		if (CHECK_VERSION(chip, 0x5227, IC_VER_A))
+			RTSX_WRITE_REG(chip, PWD_SUSPEND_EN, 0x08, 0x00);
+#endif
+
+		retval = card_power_on(chip, SD_CARD);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		wait_timeout(260);
+
+#ifdef SUPPORT_OCP
+		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
+			RTSX_DEBUGP(("Over current, OCPSTAT is 0x%x\n",
+				     chip->ocp_stat));
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (CHECK_VERSION(chip, 0x5227, IC_VER_A)) {
+			retval = rtsx_disable_ocp(chip);
+			if (retval != STATUS_SUCCESS)
+				TRACE_RET(chip, STATUS_FAIL);
+
+			RTSX_WRITE_REG(chip, PWD_SUSPEND_EN, 0x08, 0x08);
+		}
+#endif
+	}
+
+	RTSX_WRITE_REG(chip, CARD_OE, SD_OUTPUT_EN, SD_OUTPUT_EN);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_dummy_clock(struct rtsx_chip *chip)
+{
+	RTSX_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN, SD_CLK_TOGGLE_EN);
+	wait_timeout(5);
+	RTSX_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN, 0x00);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_read_lba0(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 bus_width;
+#ifdef USING_PPBUF
+	u8 cmd[5];
+#else
+	u8 *buf;
+
+	buf = (u8 *) rtsx_alloc_dma_buf(chip, 512, GFP_KERNEL);
+	if (buf == NULL) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+#endif
+
+	if (CHK_SD(sd_card)) {
+		bus_width = SD_BUS_WIDTH_4;
+	} else {
+		if (CHK_MMC_8BIT(sd_card)) {
+			bus_width = SD_BUS_WIDTH_8;
+		} else if (CHK_MMC_4BIT(sd_card)) {
+			bus_width = SD_BUS_WIDTH_4;
+		} else {
+			bus_width = SD_BUS_WIDTH_1;
+		}
+	}
+
+#ifdef USING_PPBUF
+	cmd[0] = 0x40 | READ_SINGLE_BLOCK;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval =
+	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 512, 1, bus_width,
+			 NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#else
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
+		     0x40 | READ_SINGLE_BLOCK);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 2);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
+		     | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+
+	trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+		     SD_TM_NORMAL_READ | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	rtsx_send_cmd_no_wait(chip);
+
+	retval =
+	    rtsx_transfer_data(chip, SD_CARD, buf, 512, 0, DMA_FROM_DEVICE,
+			       100);
+	if (retval < 0) {
+		sd_print_debug_reg(chip);
+		rtsx_free_dma_buf(chip, buf);
+		rtsx_clear_sd_error(chip);
+
+		if (retval == -ETIMEDOUT) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					    sd_card->sd_addr, SD_RSP_TYPE_R1,
+					    NULL, 0);
+		}
+
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	rtsx_free_dma_buf(chip, buf);
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_check_wp_state(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u32 val;
+	u16 sd_card_type;
+	u8 cmd[5], buf[64];
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	cmd[0] = 0x40 | SD_STATUS;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval =
+	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1,
+			 SD_BUS_WIDTH_4, buf, 64, 250);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+
+		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				    SD_RSP_TYPE_R1, NULL, 0);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("ACMD13:\n"));
+	RTSX_DUMP(buf, 64);
+
+	sd_card_type = ((u16) buf[2] << 8) | buf[3];
+	RTSX_DEBUGP(("sd_card_type = 0x%04x\n", sd_card_type));
+	if ((sd_card_type == 0x0001) || (sd_card_type == 0x0002)) {
+		chip->card_wp |= SD_CARD;
+	}
+
+	val = rtsx_readl(chip, RTSX_BIPR);
+	if (val & SD_WRITE_PROTECT) {
+		chip->card_wp |= SD_CARD;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_sd(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval, i = 0, j = 0, k = 0, hi_cap_flow = 0;
+	int sd_dont_switch = 0;
+	int support_1v8 = 0;
+	u8 rsp[16];
+	u8 switch_bus_width;
+	u32 voltage = 0;
+	int sd20_mode = chip->sd20_mode;
+	int read_lba0 = 1;
+
+	RTSX_DEBUGP(("chip->sd20_mode = %d\n", chip->sd20_mode));
+
+	SET_SD(sd_card);
+
+ Switch_Fail:
+
+	i = 0;
+	j = 0;
+	k = 0;
+	hi_cap_flow = 0;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
+		goto SD_UNLOCK_ENTRY;
+	}
+#endif
+
+	retval = sd_prepare_reset(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_dummy_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+ RTY_SD_RST:
+	retval =
+	    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL,
+				0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	wait_timeout(20);
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, SEND_IF_COND, 0x000001AA,
+				SD_RSP_TYPE_R7, rsp, 5);
+	if (retval == STATUS_SUCCESS) {
+		if ((rsp[4] == 0xAA) && ((rsp[3] & 0x0f) == 0x01)) {
+			hi_cap_flow = 1;
+			if (sd20_mode) {
+				voltage = SUPPORT_VOLTAGE |
+				    SUPPORT_HIGH_AND_EXTENDED_CAPACITY;
+			} else {
+				voltage = SUPPORT_VOLTAGE |
+				    SUPPORT_HIGH_AND_EXTENDED_CAPACITY |
+				    SUPPORT_MAX_POWER_PERMANCE | SUPPORT_1V8;
+			}
+		}
+	}
+
+	if (!hi_cap_flow) {
+		voltage = SUPPORT_VOLTAGE;
+
+		retval =
+		    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0,
+					SD_RSP_TYPE_R0, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		wait_timeout(20);
+	}
+
+	do {
+		retval =
+		    sd_send_cmd_get_rsp(chip, APP_CMD, 0, SD_RSP_TYPE_R1,
+					NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			j++;
+			if (j < 3) {
+				goto RTY_SD_RST;
+			} else {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		retval =
+		    sd_send_cmd_get_rsp(chip, SD_APP_OP_COND, voltage,
+					SD_RSP_TYPE_R3, rsp, 5);
+		if (retval != STATUS_SUCCESS) {
+			k++;
+			if (k < 3) {
+				goto RTY_SD_RST;
+			} else {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+		}
+
+		i++;
+		wait_timeout(20);
+	} while (!(rsp[1] & 0x80) && (i < 255));
+
+	if (i == 255) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (hi_cap_flow) {
+		if (rsp[1] & 0x40) {
+			SET_SD_HCXC(sd_card);
+		} else {
+			CLR_SD_HCXC(sd_card);
+		}
+		if (CHK_SD_HCXC(sd_card) && !sd20_mode) {
+			support_1v8 = (rsp[1] & 0x01) ? 1 : 0;
+		} else {
+			support_1v8 = 0;
+		}
+	} else {
+		CLR_SD_HCXC(sd_card);
+		support_1v8 = 0;
+	}
+	RTSX_DEBUGP(("support_1v8 = %d\n", support_1v8));
+
+	if (support_1v8) {
+		retval = sd_voltage_switch(chip);
+		if (retval != STATUS_SUCCESS)
+			goto SD20_MODE;
+	}
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2,
+				sd_card->raw_cid, 16);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	RTSX_DEBUGP(("CID Response:\n"));
+	RTSX_DUMP(sd_card->raw_cid, 16);
+
+	for (i = 0; i < 3; i++) {
+		retval =
+		    sd_send_cmd_get_rsp(chip, SEND_RELATIVE_ADDR, 0,
+					SD_RSP_TYPE_R6, rsp, 5);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		sd_card->sd_addr = (u32) rsp[1] << 24;
+		sd_card->sd_addr += (u32) rsp[2] << 16;
+
+		if (sd_card->sd_addr) {
+			break;
+		}
+	}
+
+	retval = sd_check_csd(chip, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_select_card(chip, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#ifdef SUPPORT_SD_LOCK
+ SD_UNLOCK_ENTRY:
+	retval = sd_update_lock_status(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (sd_card->sd_lock_status & SD_LOCKED) {
+		sd_card->sd_lock_status |= (SD_LOCK_1BIT_MODE | SD_PWD_EXIST);
+		return STATUS_SUCCESS;
+	} else if (!(sd_card->sd_lock_status & SD_UNLOCK_POW_ON)) {
+		sd_card->sd_lock_status &= ~SD_PWD_EXIST;
+	}
+#endif
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	retval =
+	    sd_send_cmd_get_rsp(chip, SET_CLR_CARD_DETECT, 0, SD_RSP_TYPE_R1,
+				NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (support_1v8) {
+		retval =
+		    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+					SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		retval =
+		    sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2,
+					SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		switch_bus_width = SD_BUS_WIDTH_4;
+	} else {
+		switch_bus_width = SD_BUS_WIDTH_1;
+	}
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1,
+				NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (!(sd_card->raw_csd[4] & 0x40)) {
+		sd_dont_switch = 1;
+	}
+
+	if (!sd_dont_switch) {
+
+		if (sd20_mode) {
+			sd_card->sd_switch_fail = SDR104_SUPPORT_MASK |
+			    DDR50_SUPPORT_MASK | SDR50_SUPPORT_MASK;
+		}
+
+		retval = sd_check_spec(chip, switch_bus_width);
+		if (retval == STATUS_SUCCESS) {
+			retval = sd_switch_function(chip, switch_bus_width);
+			if (retval != STATUS_SUCCESS) {
+				RTSX_DEBUGP(("Switch bus fail, reset again\n"));
+				goto SD_NOT_SWITCH;
+			}
+		} else {
+			if (support_1v8) {
+				RTSX_DEBUGP(("Switch bus fail, reset again\n"));
+				goto SD_NOT_SWITCH;
+			}
+		}
+	}
+
+	if (support_1v8) {
+		retval = sd_set_sd30_drive(chip, SD_IO_1V8);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+	} else {
+		retval =
+		    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+					SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+		retval =
+		    sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2,
+					SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
+#endif
+
+	if (!sd20_mode && CHK_SD30_SPEED(sd_card)) {
+		retval = sd_set_init_para(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (CHK_SD_DDR50(sd_card)) {
+			retval = sd_ddr_tuning(chip);
+		} else {
+			retval = sd_sdr_tuning(chip);
+		}
+
+		if (retval != STATUS_SUCCESS)
+			goto SD20_MODE;
+
+		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				    SD_RSP_TYPE_R1, NULL, 0);
+
+		if (CHK_SD_DDR50(sd_card)) {
+			retval =
+			    sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+			if (retval != STATUS_SUCCESS) {
+				read_lba0 = 0;
+			}
+		}
+
+		if (read_lba0) {
+			retval = sd_read_lba0(chip);
+			if (retval != STATUS_SUCCESS) {
+				RTSX_DEBUGP(("Reading LBA0 fail,"
+					     "try to reset card in sd 2.0 mode\n"));
+				goto SD20_MODE;
+			}
+		}
+	}
+
+	retval = sd_check_wp_state(chip);
+	if (retval != STATUS_SUCCESS) {
+		if (CHK_SD30_SPEED(sd_card))
+			goto SD20_MODE;
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
+		RTSX_WRITE_REG(chip, SD_BLOCK_CNT_H, 0xFF, 0x02);
+		RTSX_WRITE_REG(chip, SD_BLOCK_CNT_L, 0xFF, 0x00);
+	}
+#endif
+
+	return STATUS_SUCCESS;
+
+ SD_NOT_SWITCH:
+	sd_dont_switch = 1;
+	goto POWER_CYCLE;
+
+ SD20_MODE:
+	sd20_mode = 1;
+	goto POWER_CYCLE;
+
+ POWER_CYCLE:
+	retval = sd_init_power(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	goto Switch_Fail;
+}
+
+static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 buf[8] = { 0 }, bus_width, *ptr;
+	u16 byte_cnt;
+	int len;
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, BUSTEST_W, 0, SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (width == MMC_8BIT_BUS) {
+		buf[0] = 0x55;
+		buf[1] = 0xAA;
+		len = 8;
+		byte_cnt = 8;
+		bus_width = SD_BUS_WIDTH_8;
+	} else {
+		buf[0] = 0x5A;
+		len = 4;
+		byte_cnt = 4;
+		bus_width = SD_BUS_WIDTH_4;
+	}
+
+	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_3,
+			       NULL, 0, byte_cnt, 1, bus_width, buf, len,
+			       100);
+	if (retval != STATUS_SUCCESS) {
+		u8 val1 = 0, val2 = 0;
+		rtsx_read_register(chip, SD_STAT1, &val1);
+		rtsx_read_register(chip, SD_STAT2, &val2);
+		rtsx_clear_sd_error(chip);
+		if ((val1 & 0xE0) || val2) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	RTSX_DEBUGP(("SD/MMC CMD %d\n", BUSTEST_R));
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | BUSTEST_R);
+
+	if (width == MMC_8BIT_BUS) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x08);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x04);
+	}
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_NO_CHECK_CRC16 |
+		     SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+		     SD_TM_NORMAL_READ | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2, 0, 0);
+	if (width == MMC_8BIT_BUS) {
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 1, 0, 0);
+	}
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 100);
+	if (retval < 0) {
+		rtsx_clear_sd_error(chip);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if (width == MMC_8BIT_BUS) {
+		RTSX_DEBUGP(("BUSTEST_R [8bits]: 0x%02x 0x%02x\n", ptr[0],
+			     ptr[1]));
+		if ((ptr[0] == 0xAA) && (ptr[1] == 0x55)) {
+			u8 rsp[5];
+			u32 arg;
+
+			if (CHK_MMC_DDR52(sd_card)) {
+				arg = 0x03B70600;
+			} else {
+				arg = 0x03B70200;
+			}
+			retval =
+			    sd_send_cmd_get_rsp(chip, SWITCH, arg,
+						SD_RSP_TYPE_R1b, rsp, 5);
+			if ((retval == STATUS_SUCCESS)
+			    && !(rsp[4] & MMC_SWITCH_ERR)) {
+				return STATUS_SUCCESS;
+			}
+		}
+	} else {
+		RTSX_DEBUGP(("BUSTEST_R [4bits]: 0x%02x\n", ptr[0]));
+		if (ptr[0] == 0xA5) {
+			u8 rsp[5];
+			u32 arg;
+
+			if (CHK_MMC_DDR52(sd_card)) {
+				arg = 0x03B70500;
+			} else {
+				arg = 0x03B70100;
+			}
+			retval =
+			    sd_send_cmd_get_rsp(chip, SWITCH, arg,
+						SD_RSP_TYPE_R1b, rsp, 5);
+			if ((retval == STATUS_SUCCESS)
+			    && !(rsp[4] & MMC_SWITCH_ERR)) {
+				return STATUS_SUCCESS;
+			}
+		}
+	}
+
+	TRACE_RET(chip, STATUS_FAIL);
+}
+
+static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+	u8 *ptr, card_type, card_type_mask = 0;
+#ifndef USING_PPBUF
+	u8 *buf;
+
+	buf = (u8 *) rtsx_alloc_dma_buf(chip, 512, GFP_KERNEL);
+	if (buf == NULL) {
+		TRACE_RET(chip, STATUS_ERROR);
+	}
+#endif
+
+	CLR_MMC_HS(sd_card);
+
+	RTSX_DEBUGP(("SD/MMC CMD %d\n", SEND_EXT_CSD));
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | SEND_EXT_CSD);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 2);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
+		     | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+
+#ifdef USING_PPBUF
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+#else
+	trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
+#endif
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+		     SD_TM_NORMAL_READ | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+#ifdef USING_PPBUF
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 196, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 212, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 213, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 214, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 215, 0xFF, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 1000);
+	if (retval < 0) {
+		if (retval == -ETIMEDOUT) {
+			rtsx_clear_sd_error(chip);
+
+			sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					    sd_card->sd_addr, SD_RSP_TYPE_R1,
+					    NULL, 0);
+		}
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#else
+	rtsx_send_cmd_no_wait(chip);
+
+	retval =
+	    rtsx_transfer_data(chip, SD_CARD, buf, 512, 0, DMA_FROM_DEVICE,
+			       100);
+	if (retval < 0) {
+		rtsx_free_dma_buf(chip, buf);
+		if (retval == -ETIMEDOUT) {
+			rtsx_clear_sd_error(chip);
+
+			sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					    sd_card->sd_addr, SD_RSP_TYPE_R1,
+					    NULL, 0);
+		}
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	ptr = rtsx_get_cmd_data(chip);
+	if (ptr[0] & SD_TRANSFER_ERR) {
+		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				    SD_RSP_TYPE_R1, NULL, 0);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (CHK_MMC_SECTOR_MODE(sd_card)) {
+#ifdef USING_PPBUF
+		sd_card->capacity =
+		    ((u32) ptr[5] << 24) | ((u32) ptr[4] << 16) | ((u32)
+								   ptr[3] <<
+								   8) | ((u32)
+									 ptr
+									 [2]);
+
+#else
+		sd_card->capacity =
+		    ((u32) buf[215] << 24) | ((u32) buf[214] << 16) | ((u32)
+								       buf
+								       [213]
+								       << 8) |
+		    ((u32) buf[212]);
+#endif
+	}
+#ifdef SUPPORT_SD_LOCK
+	if (!(sd_card->sd_lock_status & SD_SDR_RST) &&
+	    (chip->sd_ctl & SUPPORT_MMC_DDR_MODE)) {
+		card_type_mask = 0x07;
+	} else {
+		card_type_mask = 0x03;
+	}
+#else
+	if (chip->sd_ctl & SUPPORT_MMC_DDR_MODE) {
+		card_type_mask = 0x07;
+	} else {
+		card_type_mask = 0x03;
+	}
+#endif
+#ifdef USING_PPBUF
+	card_type = ptr[1] & card_type_mask;
+#else
+	card_type = buf[196] & card_type_mask;
+#endif
+	if (card_type) {
+		u8 rsp[5];
+
+		if (card_type & 0x04) {
+			if (switch_ddr) {
+				SET_MMC_DDR52(sd_card);
+			} else {
+				SET_MMC_52M(sd_card);
+			}
+		} else if (card_type & 0x02) {
+			SET_MMC_52M(sd_card);
+		} else {
+			SET_MMC_26M(sd_card);
+		}
+
+		retval =
+		    sd_send_cmd_get_rsp(chip, SWITCH, 0x03B90100,
+					SD_RSP_TYPE_R1b, rsp, 5);
+		if ((retval != STATUS_SUCCESS) || (rsp[4] & MMC_SWITCH_ERR)) {
+			CLR_MMC_HS(sd_card);
+		}
+	}
+#ifndef USING_PPBUF
+	rtsx_free_dma_buf(chip, buf);
+#endif
+
+	sd_choose_proper_clock(chip);
+	retval = switch_clock(chip, sd_card->sd_clock);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (mmc_test_switch_bus(chip, MMC_8BIT_BUS) == STATUS_SUCCESS) {
+		SET_MMC_8BIT(sd_card);
+		chip->card_bus_width[chip->card2lun[SD_CARD]] = 8;
+#ifdef SUPPORT_SD_LOCK
+		sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
+#endif
+	} else if (mmc_test_switch_bus(chip, MMC_4BIT_BUS) == STATUS_SUCCESS) {
+		SET_MMC_4BIT(sd_card);
+		chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
+#ifdef SUPPORT_SD_LOCK
+		sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
+#endif
+	} else {
+		CLR_MMC_8BIT(sd_card);
+		CLR_MMC_4BIT(sd_card);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_mmc(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval, i = 0, j = 0, k = 0;
+	int switch_ddr = 1;
+	u8 rsp[16];
+	u8 spec_ver = 0;
+	u32 temp;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
+		goto MMC_UNLOCK_ENTRY;
+	}
+#endif
+
+ DDR_TUNING_FAIL:
+
+	retval = sd_prepare_reset(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, retval);
+	}
+
+	SET_MMC(sd_card);
+
+ RTY_MMC_RST:
+	retval =
+	    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL,
+				0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	do {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, SEND_OP_COND,
+					     (SUPPORT_VOLTAGE | 0x40000000),
+					     SD_RSP_TYPE_R3, rsp, 5);
+		if (retval != STATUS_SUCCESS) {
+			if (sd_check_err_code(chip, SD_BUSY)
+			    || sd_check_err_code(chip, SD_TO_ERR)) {
+				k++;
+				if (k < 20) {
+					sd_clr_err_code(chip);
+					goto RTY_MMC_RST;
+				} else {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			} else {
+				j++;
+				if (j < 100) {
+					sd_clr_err_code(chip);
+					goto RTY_MMC_RST;
+				} else {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+			}
+		}
+
+		wait_timeout(20);
+		i++;
+	} while (!(rsp[1] & 0x80) && (i < 255));
+
+	if (i == 255) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if ((rsp[1] & 0x60) == 0x40) {
+		SET_MMC_SECTOR_MODE(sd_card);
+	} else {
+		CLR_MMC_SECTOR_MODE(sd_card);
+	}
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2,
+				sd_card->raw_cid, 16);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	RTSX_DEBUGP(("CID Response:\n"));
+	RTSX_DUMP(sd_card->raw_cid, 16);
+
+	sd_card->sd_addr = 0x00100000;
+	retval =
+	    sd_send_cmd_get_rsp(chip, SET_RELATIVE_ADDR, sd_card->sd_addr,
+				SD_RSP_TYPE_R6, rsp, 5);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_check_csd(chip, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	spec_ver = (sd_card->raw_csd[0] & 0x3C) >> 2;
+
+	retval = sd_select_card(chip, 1);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1,
+				NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#ifdef SUPPORT_SD_LOCK
+ MMC_UNLOCK_ENTRY:
+	retval = sd_update_lock_status(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+#endif
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	chip->card_bus_width[chip->card2lun[SD_CARD]] = 1;
+
+	if (!sd_card->mmc_dont_switch_bus) {
+
+		if (spec_ver == 4) {
+			(void)mmc_switch_timing_bus(chip, switch_ddr);
+		}
+
+		if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0)) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (switch_ddr && CHK_MMC_DDR52(sd_card)) {
+			retval = sd_set_init_para(chip);
+			if (retval != STATUS_SUCCESS) {
+				TRACE_RET(chip, STATUS_FAIL);
+			}
+
+			retval = mmc_ddr_tuning(chip);
+			if (retval != STATUS_SUCCESS) {
+				retval = sd_init_power(chip);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+				switch_ddr = 0;
+				goto DDR_TUNING_FAIL;
+			}
+
+			retval =
+			    sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+			if (retval == STATUS_SUCCESS) {
+				retval = sd_read_lba0(chip);
+				if (retval != STATUS_SUCCESS) {
+					retval = sd_init_power(chip);
+					if (retval != STATUS_SUCCESS) {
+						TRACE_RET(chip, STATUS_FAIL);
+					}
+					switch_ddr = 0;
+					goto DDR_TUNING_FAIL;
+				}
+			}
+		}
+	}
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
+		RTSX_WRITE_REG(chip, SD_BLOCK_CNT_H, 0xFF, 0x02);
+		RTSX_WRITE_REG(chip, SD_BLOCK_CNT_L, 0xFF, 0x00);
+	}
+#endif
+
+	temp = rtsx_readl(chip, RTSX_BIPR);
+	if (temp & SD_WRITE_PROTECT) {
+		chip->card_wp |= SD_CARD;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int reset_sd_card(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	memset(sd_card, 0, sizeof(struct sd_info));
+	chip->capacity[chip->card2lun[SD_CARD]] = 0;
+
+	retval = enable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_init_power(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (chip->support_card & SUPPORT_MMC) {
+		if (chip->sd_ctl & RESET_MMC_FIRST) {
+			retval = reset_mmc(chip);
+			if ((retval != STATUS_SUCCESS)
+			    && !sd_check_err_code(chip, SD_NO_CARD)) {
+				retval = reset_sd(chip);
+				if (retval != STATUS_SUCCESS) {
+					retval =
+					    sd_change_bank_voltage(chip,
+								   SD_IO_3V3);
+					if (retval != STATUS_SUCCESS) {
+						TRACE_RET(chip, STATUS_FAIL);
+					}
+				}
+			}
+		} else {
+			retval = reset_sd(chip);
+			if (retval != STATUS_SUCCESS) {
+				if (sd_check_err_code(chip, SD_NO_CARD)) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+
+				retval =
+				    sd_change_bank_voltage(chip, SD_IO_3V3);
+				if (retval != STATUS_SUCCESS) {
+					TRACE_RET(chip, STATUS_FAIL);
+				}
+
+				retval = reset_mmc(chip);
+			}
+		}
+	} else {
+		retval = reset_sd(chip);
+		if (retval != STATUS_SUCCESS) {
+			sd_change_bank_voltage(chip, SD_IO_3V3);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+	}
+
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	RTSX_WRITE_REG(chip, SD_BYTE_CNT_L, 0xFF, 0);
+	RTSX_WRITE_REG(chip, SD_BYTE_CNT_H, 0xFF, 2);
+
+	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
+
+	retval = sd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("sd_card->sd_type = 0x%x\n", sd_card->sd_type));
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_mmc_only(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	sd_card->sd_type = 0;
+	sd_card->seq_mode = 0;
+	sd_card->sd_data_buf_ready = 0;
+	sd_card->capacity = 0;
+	sd_card->sd_switch_fail = 0;
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status = 0;
+	sd_card->sd_erase_status = 0;
+#endif
+
+	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity = 0;
+
+	retval = enable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_init_power(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = reset_mmc(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+	RTSX_WRITE_REG(chip, SD_BYTE_CNT_L, 0xFF, 0);
+	RTSX_WRITE_REG(chip, SD_BYTE_CNT_H, 0xFF, 2);
+
+	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
+
+	retval = sd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	RTSX_DEBUGP(("In reset_mmc_only, sd_card->sd_type = 0x%x\n",
+		     sd_card->sd_type));
+
+	return STATUS_SUCCESS;
+}
+
+#define WAIT_DATA_READY_RTY_CNT		255
+
+static int wait_data_buf_ready(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int i, retval;
+
+	for (i = 0; i < WAIT_DATA_READY_RTY_CNT; i++) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		sd_card->sd_data_buf_ready = 0;
+
+		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					     sd_card->sd_addr, SD_RSP_TYPE_R1,
+					     NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		if (sd_card->sd_data_buf_ready) {
+			return sd_send_cmd_get_rsp(chip, SEND_STATUS,
+						   sd_card->sd_addr,
+						   SD_RSP_TYPE_R1, NULL, 0);
+		}
+	}
+
+	sd_set_err_code(chip, SD_TO_ERR);
+
+	TRACE_RET(chip, STATUS_FAIL);
+}
+
+void sd_stop_seq_mode(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	if (sd_card->seq_mode) {
+		retval = sd_switch_clock(chip);
+		if (retval != STATUS_SUCCESS) {
+			return;
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
+					     SD_RSP_TYPE_R1b, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_STS_ERR);
+		}
+		retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+		if (retval != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_STS_ERR);
+		}
+		sd_card->seq_mode = 0;
+
+		rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+	}
+}
+
+static inline int sd_auto_tune_clock(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	if (chip->asic_code) {
+		if (sd_card->sd_clock > 30) {
+			sd_card->sd_clock -= 20;
+		}
+	} else {
+		switch (sd_card->sd_clock) {
+		case CLK_200:
+			sd_card->sd_clock = CLK_150;
+			break;
+
+		case CLK_150:
+			sd_card->sd_clock = CLK_120;
+			break;
+
+		case CLK_120:
+			sd_card->sd_clock = CLK_100;
+			break;
+
+		case CLK_100:
+			sd_card->sd_clock = CLK_80;
+			break;
+
+		case CLK_80:
+			sd_card->sd_clock = CLK_60;
+			break;
+
+		case CLK_60:
+			sd_card->sd_clock = CLK_50;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	chip->sd_retune_clock = sd_card->sd_clock;
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef PRE_READ_EN
+
+static int sd_stop_sequential(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	int retval;
+
+	if ((sd_card->pre_sec_cnt < 0x80)
+	    && (sd_card->pre_dir == DMA_FROM_DEVICE)
+	    && !CHK_SD30_SPEED(sd_card)
+	    && !CHK_SD_HS(sd_card)
+	    && !CHK_MMC_HS(sd_card)) {
+		sd_send_cmd_get_rsp(chip, SEND_STATUS,
+				    sd_card->sd_addr, SD_RSP_TYPE_R1, NULL,
+				    0);
+	}
+
+	retval =
+	    sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0, SD_RSP_TYPE_R1b,
+				NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		chip->rw_need_retry = 1;
+		sd_set_err_code(chip, SD_STS_ERR);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	retval = rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+	if (retval != STATUS_SUCCESS) {
+		sd_set_err_code(chip, SD_IO_ERR);
+		TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if ((sd_card->pre_sec_cnt < 0x80)
+	    && !CHK_SD30_SPEED(sd_card)
+	    && !CHK_SD_HS(sd_card)
+	    && !CHK_MMC_HS(sd_card)) {
+		sd_send_cmd_get_rsp(chip, SEND_STATUS,
+				    sd_card->sd_addr, SD_RSP_TYPE_R1, NULL,
+				    0);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static inline int sd_in_seq(struct sd_info *sd_card,
+			    u32 start_sector,
+			    enum dma_data_direction data_dir)
+{
+	if (sd_card->pre_dir != data_dir)
+		return 0;
+	if ((sd_card->pre_sec_addr + sd_card->pre_sec_cnt) != start_sector)
+		return 0;
+
+	return 1;
+}
+
+static inline u32 sd_calc_data_addr(struct sd_info *sd_card, u32 start_sector)
+{
+	u32 data_addr;
+
+	if (!CHK_SD_HCXC(sd_card) && !CHK_MMC_SECTOR_MODE(sd_card)) {
+		data_addr = start_sector << 9;
+	} else {
+		data_addr = start_sector;
+	}
+
+	return data_addr;
+}
+
+static void sd_auto_read2(struct rtsx_chip *chip, u32 data_addr,
+			  u16 sector_cnt)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	u8 cfg2;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF,
+		     (u8) sector_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
+		     (u8) (sector_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     RING_BUFFER);
+
+	if (CHK_MMC_8BIT(sd_card)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
+			     SD_BUS_WIDTH_8);
+	} else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
+			     SD_BUS_WIDTH_4);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
+			     SD_BUS_WIDTH_1);
+	}
+
+	RTSX_DEBUGP(("SD/MMC CMD %d\n", READ_MULTIPLE_BLOCK));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
+		     0x40 | READ_MULTIPLE_BLOCK);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF,
+		     (u8) (data_addr >> 24));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF,
+		     (u8) (data_addr >> 16));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF,
+		     (u8) (data_addr >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) data_addr);
+
+	cfg2 = SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
+	    SD_CHECK_CRC7 | SD_RSP_LEN_6;
+	if (!CHK_SD30_SPEED(sd_card)) {
+		cfg2 |= SD_NO_CHECK_WAIT_CRC_TO;
+	}
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, cfg2);
+
+	trans_dma_enable(DMA_FROM_DEVICE, chip, sector_cnt * 512, DMA_512);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+		     SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	rtsx_send_cmd_no_wait(chip);
+}
+
+static int sd_write(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+		    u32 start_sector, u16 sector_cnt)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	u32 data_addr;
+	u8 cfg2;
+	int retval;
+
+	data_addr = sd_calc_data_addr(sd_card, start_sector);
+
+	if (sd_card->seq_mode
+	    && !sd_in_seq(sd_card, start_sector, srb->sc_data_direction)) {
+		retval = sd_stop_sequential(chip);
+		if (retval != STATUS_SUCCESS) {
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		sd_card->seq_mode = 0;
+	}
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF,
+		     (u8) sector_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
+		     (u8) (sector_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     RING_BUFFER);
+
+	if (CHK_MMC_8BIT(sd_card)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
+			     SD_BUS_WIDTH_8);
+	} else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
+			     SD_BUS_WIDTH_4);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
+			     SD_BUS_WIDTH_1);
+	}
+
+	if (sd_card->seq_mode) {
+		cfg2 =
+		    SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+		    SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 | SD_RSP_LEN_0;
+		if (!CHK_SD30_SPEED(sd_card)) {
+			cfg2 |= SD_NO_CHECK_WAIT_CRC_TO;
+		}
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, cfg2);
+
+		trans_dma_enable(srb->sc_data_direction, chip,
+				 sector_cnt * 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+			     SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
+
+		rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
+			     SD_TRANSFER_END, SD_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+	} else {
+		retval = rtsx_send_cmd(chip, SD_CARD, 50);
+		if (retval < 0) {
+			rtsx_clear_sd_error(chip);
+
+			chip->rw_need_retry = 1;
+			sd_set_err_code(chip, SD_TO_ERR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = wait_data_buf_ready(chip);
+		if (retval != STATUS_SUCCESS) {
+			chip->rw_need_retry = 1;
+			sd_set_err_code(chip, SD_TO_ERR);
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, WRITE_MULTIPLE_BLOCK,
+					     data_addr, SD_RSP_TYPE_R1, NULL,
+					     0);
+		if (retval != STATUS_SUCCESS) {
+			chip->rw_need_retry = 1;
+			TRACE_RET(chip, STATUS_FAIL);
+		}
+
+		rtsx_init_cmd(chip);
+
+		cfg2 =
+		    SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+		    SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 | SD_RSP_LEN_0;
+		if (!CHK_SD30_SPEED(sd_card)) {
+			cfg2 |= SD_NO_CHECK_WAIT_CRC_TO;
+		}
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, cfg2);
+
+		trans_dma_enable(srb->sc_data_direction, chip,
+				 sector_cnt * 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+			     SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
+			     SD_TRANSFER_END, SD_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		sd_card->seq_mode = 1;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_read(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+		   u32 start_sector, u16 sector_cnt)
+{
+	struct sd_info *sd_card = &(chip->sd_card);
+	u32 data_addr;<