Re: [merged] memstick-add-support-for-legacy-memorysticks.patch removed from -mm tree

Re: [merged] memstick-add-support-for-legacy-memorysticks.patch removed from -mm tree

[Maxim Levitsky]

[-- Attachment #1: Type: text/plain, Size: 661 bytes --]

Hi!

It my fault, because I am very busy lately and almost do no kernel
development, and so this patch even though I had tried to merge it few
times, somewhat each time fell through the cracks.
Could you merge it now, that I see that merge window is open?

I attach the driver, its a standalone driver, so it can't add bugs to
the kernel.
I just had put so much work into this, that its sad to see this not
merged.
It should not cause any maintenance issues, just like the rest of
memstick subsystem that is hardly maintained now (Alex doesn't respond
to the mails for long time.

I do use this subsystem here quite a lot.

Best regards,
        Maxim Levitsky

[-- Attachment #2: 0001-memstick-add-support-for-legacy-memorysticks.patch --]
[-- Type: text/x-patch, Size: 71609 bytes --]

>From 2bad0b84c55907dbbba0f2a91ca99e4ef6050bea Mon Sep 17 00:00:00 2001
From: Maxim Levitsky <maximlevitsky@gmail.com>
Date: Fri, 5 Apr 2013 16:37:31 +0300
Subject: [PATCH] memstick: add support for legacy memorysticks

Based partially on MS standard spec quotes from Alex Dubov.

As any code that works with user data this driver isn't
recommended to use to write cards that contain valuable data.

It tries its best though to avoid data corruption
and possible damage to the card.

Tested on MS DUO 64 MB card on Ricoh R592 card reader.

Signed-off-by: Maxim Levitsky <maximlevitsky@gmail.com>
---
 MAINTAINERS                      |    5 +
 drivers/memstick/core/Kconfig    |   12 +
 drivers/memstick/core/Makefile   |    2 +-
 drivers/memstick/core/ms_block.c | 2395 ++++++++++++++++++++++++++++++++++++++
 drivers/memstick/core/ms_block.h |  290 +++++
 5 files changed, 2703 insertions(+), 1 deletion(-)
 create mode 100644 drivers/memstick/core/ms_block.c
 create mode 100644 drivers/memstick/core/ms_block.h

diff --git a/MAINTAINERS b/MAINTAINERS
index 836a618..5479d2a 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -7359,6 +7359,11 @@ W:	http://tifmxx.berlios.de/
 S:	Maintained
 F:	drivers/memstick/host/tifm_ms.c
 
+SONY MEMORYSTICK STANDARD SUPPORT
+M:	Maxim Levitsky <maximlevitsky@gmail.com>
+S:	Maintained
+F:	drivers/memstick/core/ms_block.*
+
 SOUND
 M:	Jaroslav Kysela <perex@perex.cz>
 M:	Takashi Iwai <tiwai@suse.de>
diff --git a/drivers/memstick/core/Kconfig b/drivers/memstick/core/Kconfig
index 95f1814..1d38949 100644
--- a/drivers/memstick/core/Kconfig
+++ b/drivers/memstick/core/Kconfig
@@ -24,3 +24,15 @@ config MSPRO_BLOCK
 	  support. This provides a block device driver, which you can use
 	  to mount the filesystem. Almost everyone wishing MemoryStick
 	  support should say Y or M here.
+
+config MS_BLOCK
+	tristate "MemoryStick Standard device driver"
+	depends on BLOCK
+	help
+	  Say Y here to enable the MemoryStick Standard device driver
+	  support. This provides a block device driver, which you can use
+	  to mount the filesystem.
+	  This driver works with old (bulky) MemoryStick and MemoryStick Duo
+	  but not PRO. Say Y if you have such card.
+	  Driver is new and not yet well tested, thus it can damage your card
+	  (even permanently)
diff --git a/drivers/memstick/core/Makefile b/drivers/memstick/core/Makefile
index ecd0299..0d7f90c 100644
--- a/drivers/memstick/core/Makefile
+++ b/drivers/memstick/core/Makefile
@@ -3,5 +3,5 @@
 #
 
 obj-$(CONFIG_MEMSTICK)		+= memstick.o
-
+obj-$(CONFIG_MS_BLOCK)		+= ms_block.o
 obj-$(CONFIG_MSPRO_BLOCK)	+= mspro_block.o
diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
new file mode 100644
index 0000000..71e5937
--- /dev/null
+++ b/drivers/memstick/core/ms_block.c
@@ -0,0 +1,2395 @@
+/*
+ *  ms_block.c - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2012 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver were copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ */
+#define DRIVER_NAME "ms_block"
+#define pr_fmt(fmt) DRIVER_NAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/memstick.h>
+#include <linux/idr.h>
+#include <linux/hdreg.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/bitmap.h>
+#include <linux/scatterlist.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include "ms_block.h"
+
+static int debug;
+static int cache_flush_timeout = 1000;
+static bool verify_writes;
+
+/*
+ * Copies section of 'sg_from' starting from offset 'offset' and with length
+ * 'len' To another scatterlist of to_nents enties
+ */
+static size_t msb_sg_copy(struct scatterlist *sg_from, struct scatterlist *sg_to,
+					int to_nents, size_t offset, size_t len)
+{
+	size_t copied = 0;
+
+	while (offset > 0) {
+		if (offset >= sg_from->length) {
+			if (sg_is_last(sg_from))
+				return 0;
+
+			offset -= sg_from->length;
+			sg_from = sg_next(sg_from);
+			continue;
+		}
+
+		copied = min(len, sg_from->length - offset);
+		sg_set_page(sg_to, sg_page(sg_from),
+			copied, sg_from->offset + offset);
+
+		len -= copied;
+		offset = 0;
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_to = sg_next(sg_to);
+		to_nents--;
+		sg_from = sg_next(sg_from);
+	}
+
+	while (len > sg_from->length && to_nents--) {
+		len -= sg_from->length;
+		copied += sg_from->length;
+
+		sg_set_page(sg_to, sg_page(sg_from),
+				sg_from->length, sg_from->offset);
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_from = sg_next(sg_from);
+		sg_to = sg_next(sg_to);
+	}
+
+	if (len && to_nents) {
+		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
+		copied += len;
+	}
+out:
+	sg_mark_end(sg_to);
+	return copied;
+}
+
+/*
+ * Compares section of 'sg' starting from offset 'offset' and with length 'len'
+ * to linear buffer of length 'len' at address 'buffer'
+ * Returns 0 if equal and  -1 otherwice
+ */
+static int msb_sg_compare_to_buffer(struct scatterlist *sg,
+					size_t offset, u8 *buffer, size_t len)
+{
+	int retval = 0, cmplen;
+	struct sg_mapping_iter miter;
+
+	sg_miter_start(&miter, sg, sg_nents(sg),
+					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
+
+	while (sg_miter_next(&miter) && len > 0) {
+		if (offset >= miter.length) {
+			offset -= miter.length;
+			continue;
+		}
+
+		cmplen = min(miter.length - offset, len);
+		retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
+		if (retval)
+			break;
+
+		buffer += cmplen;
+		len -= cmplen;
+		offset = 0;
+	}
+
+	if (!retval && len)
+		retval = -1;
+
+	sg_miter_stop(&miter);
+	return retval;
+}
+
+
+/* Get zone at which block with logical address 'lba' lives
+ * Flash is broken into zones.
+ * Each zone consists of 512 eraseblocks, out of which in first
+ * zone 494 are used and 496 are for all following zones.
+ * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
+*/
+static int msb_get_zone_from_lba(int lba)
+{
+	if (lba < 494)
+		return 0;
+	return ((lba - 494) / 496) + 1;
+}
+
+/* Get zone of physical block. Trivial */
+static int msb_get_zone_from_pba(int pba)
+{
+	return pba / MS_BLOCKS_IN_ZONE;
+}
+
+/* Debug test to validate free block counts */
+#ifdef DEBUG
+static int msb_validate_used_block_bitmap(struct msb_data *msb)
+{
+	int total_free_blocks = 0;
+	int i;
+
+	for (i = 0 ; i < msb->zone_count ; i++)
+		total_free_blocks += msb->free_block_count[i];
+
+	if (msb->block_count - bitmap_weight(msb->used_blocks_bitmap,
+					msb->block_count) == total_free_blocks)
+		return 0;
+
+	pr_err("BUG: free block counts don't match the bitmap");
+	msb->read_only = true;
+	return -EINVAL;
+}
+#endif
+
+/* Mark physical block as used */
+static void msb_mark_block_used(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (test_bit(pba, msb->used_blocks_bitmap)) {
+		pr_err(
+		"BUG: attempt to mark already used pba %d as used", pba);
+		msb->read_only = true;
+		return;
+	}
+
+#ifdef DEBUG
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+#endif
+	/* No races because all IO is single threaded */
+	__set_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]--;
+}
+
+/* Mark physical block as free */
+static void msb_mark_block_unused(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (!test_bit(pba, msb->used_blocks_bitmap)) {
+		pr_err("BUG: attempt to mark "
+				"already unused pba %d as unused" , pba);
+		msb->read_only = true;
+		return;
+	}
+
+#ifdef DEBUG
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+#endif
+	/* No races because all IO is single threaded */
+	__clear_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]++;
+}
+
+/* Invalidate current register window */
+static void msb_invalidate_reg_window(struct msb_data *msb)
+{
+	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.w_length = sizeof(struct ms_id_register);
+	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.r_length = sizeof(struct ms_id_register);
+	msb->addr_valid = false;
+}
+
+/* Start a state machine */
+static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
+		(struct memstick_dev *card, struct memstick_request **req))
+{
+	struct memstick_dev *card = msb->card;
+
+	WARN_ON(msb->state != -1);
+	msb->int_polling = false;
+	msb->state = 0;
+	msb->exit_error = 0;
+
+	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
+
+	card->next_request = state_func;
+	memstick_new_req(card->host);
+	wait_for_completion(&card->mrq_complete);
+
+	WARN_ON(msb->state != -1);
+	return msb->exit_error;
+}
+
+/* State machines call that to exit */
+static int msb_exit_state_machine(struct msb_data *msb, int error)
+{
+	WARN_ON(msb->state == -1);
+
+	msb->state = -1;
+	msb->exit_error = error;
+	msb->card->next_request = h_msb_default_bad;
+
+	/* Invalidate reg window on errors */
+	if (error)
+		msb_invalidate_reg_window(msb);
+
+	complete(&msb->card->mrq_complete);
+	return -ENXIO;
+}
+
+/* read INT register */
+static int msb_read_int_reg(struct msb_data *msb, long timeout)
+{
+	struct memstick_request *mrq = &msb->card->current_mrq;
+
+	WARN_ON(msb->state == -1);
+
+	if (!msb->int_polling) {
+		msb->int_timeout = jiffies +
+			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
+		msb->int_polling = true;
+	} else if (time_after(jiffies, msb->int_timeout)) {
+		mrq->data[0] = MEMSTICK_INT_CMDNAK;
+		return 0;
+	}
+
+	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
+				mrq->need_card_int && !mrq->error) {
+		mrq->data[0] = mrq->int_reg;
+		mrq->need_card_int = false;
+		return 0;
+	} else {
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		return 1;
+	}
+}
+
+/* Read a register */
+static int msb_read_regs(struct msb_data *msb, int offset, int len)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.r_offset != offset ||
+	    msb->reg_addr.r_length != len || !msb->addr_valid) {
+
+		msb->reg_addr.r_offset = offset;
+		msb->reg_addr.r_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
+	return 1;
+}
+
+/* Write a card register */
+static int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.w_offset != offset ||
+		msb->reg_addr.w_length != len  || !msb->addr_valid) {
+
+		msb->reg_addr.w_offset = offset;
+		msb->reg_addr.w_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
+	return 1;
+}
+
+/* Handler for absence of IO */
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq)
+{
+	return -ENXIO;
+}
+
+/*
+ * This function is a handler for reads of one page from device.
+ * Writes output to msb->current_sg, takes sector address from msb->reg.param
+ * Can also be used to read extra data only. Set params accordintly.
+ */
+static int h_msb_read_page(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 command, intreg;
+
+	if (mrq->error) {
+		dbg("read_page, unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+	case MSB_RP_SEND_BLOCK_ADDRESS:
+		/* msb_write_regs sometimes "fails" because it needs to update
+			the reg window, and thus it returns request for that.
+			Then we stay in this state and retry */
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			(unsigned char *)&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_RP_SEND_READ_COMMAND;
+		return 0;
+
+	case MSB_RP_SEND_READ_COMMAND:
+		command = MS_CMD_BLOCK_READ;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		msb->state = MSB_RP_SEND_INT_REQ;
+		return 0;
+
+	case MSB_RP_SEND_INT_REQ:
+		msb->state = MSB_RP_RECEIVE_INT_REQ_RESULT;
+		/* If dont actually need to send the int read request (only in
+			serial mode), then just fall through */
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_RP_RECEIVE_INT_REQ_RESULT:
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state = MSB_RP_SEND_INT_REQ;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state = (intreg & MEMSTICK_INT_ERR) ?
+			MSB_RP_SEND_READ_STATUS_REG : MSB_RP_SEND_OOB_READ;
+		goto again;
+
+	case MSB_RP_SEND_READ_STATUS_REG:
+		 /* read the status register to understand source of the INT_ERR */
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, status),
+			sizeof(struct ms_status_register)))
+			return 0;
+
+		msb->state = MSB_RP_RECEIVE_OOB_READ;
+		return 0;
+
+	case MSB_RP_RECIVE_STATUS_REG:
+		msb->regs.status = *(struct ms_status_register *)mrq->data;
+		msb->state = MSB_RP_SEND_OOB_READ;
+		/* fallthrough */
+
+	case MSB_RP_SEND_OOB_READ:
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register)))
+			return 0;
+
+		msb->state = MSB_RP_RECEIVE_OOB_READ;
+		return 0;
+
+	case MSB_RP_RECEIVE_OOB_READ:
+		msb->regs.extra_data =
+			*(struct ms_extra_data_register *) mrq->data;
+		msb->state = MSB_RP_SEND_READ_DATA;
+		/* fallthrough */
+
+	case MSB_RP_SEND_READ_DATA:
+		/* Skip that state if we only read the oob */
+		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
+			msb->state = MSB_RP_RECEIVE_READ_DATA;
+			goto again;
+		}
+
+		sg_init_table(sg, ARRAY_SIZE(sg));
+		msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size);
+
+		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
+		msb->state = MSB_RP_RECEIVE_READ_DATA;
+		return 0;
+
+	case MSB_RP_RECEIVE_READ_DATA:
+		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, 0);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
+			dbg("read_page: uncorrectable error");
+			return msb_exit_state_machine(msb, -EBADMSG);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
+			dbg("read_page: correctable error");
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, -EUCLEAN);
+		} else {
+			dbg("read_page: INT error, but no status error bits");
+			return msb_exit_state_machine(msb, -EIO);
+		}
+	}
+
+	BUG();
+}
+
+/*
+ * Handler of writes of exactly one block.
+ * Takes address from msb->regs.param.
+ * Writes same extra data to blocks, also taken
+ * from msb->regs.extra
+ * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
+ * device refuses to take the command or something else
+ */
+static int h_msb_write_block(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 intreg, command;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+again:
+	switch (msb->state) {
+
+	/* HACK: Jmicon handling of TPCs between 8 and
+	 *	sizeof(memstick_request.data) is broken due to hardware
+	 *	bug in PIO mode that is used for these TPCs
+	 *	Therefore split the write
+	 */
+
+	case MSB_WB_SEND_WRITE_PARAMS:
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_WB_SEND_WRITE_OOB;
+		return 0;
+
+	case MSB_WB_SEND_WRITE_OOB:
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+		msb->state = MSB_WB_SEND_WRITE_COMMAND;
+		return 0;
+
+
+	case MSB_WB_SEND_WRITE_COMMAND:
+		command = MS_CMD_BLOCK_WRITE;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		msb->state = MSB_WB_SEND_INT_REQ;
+		return 0;
+
+	case MSB_WB_SEND_INT_REQ:
+		msb->state = MSB_WB_RECEIVE_INT_REQ;
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_WB_RECEIVE_INT_REQ:
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		/* errors mean out of here, and fast... */
+		if (intreg & (MEMSTICK_INT_CMDNAK))
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+
+		/* for last page we need to poll CED */
+		if (msb->current_page == msb->pages_in_block) {
+			if (intreg & MEMSTICK_INT_CED)
+				return msb_exit_state_machine(msb, 0);
+			msb->state = MSB_WB_SEND_INT_REQ;
+			goto again;
+
+		}
+
+		/* for non-last page we need BREQ before writing next chunk */
+		if (!(intreg & MEMSTICK_INT_BREQ)) {
+			msb->state = MSB_WB_SEND_INT_REQ;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state = MSB_WB_SEND_WRITE_DATA;
+		/* fallthrough */
+
+	case MSB_WB_SEND_WRITE_DATA:
+		sg_init_table(sg, ARRAY_SIZE(sg));
+
+		if (msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size) < msb->page_size)
+			return msb_exit_state_machine(msb, -EIO);
+
+		memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
+		mrq->need_card_int = 1;
+		msb->state = MSB_WB_RECEIVE_WRITE_CONFIRMATION;
+		return 0;
+
+	case MSB_WB_RECEIVE_WRITE_CONFIRMATION:
+		msb->current_page++;
+		msb->current_sg_offset += msb->page_size;
+		msb->state = MSB_WB_SEND_INT_REQ;
+		goto again;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
+/*
+ * This function is used to send simple IO requests to device that consist
+ * of register write + command
+ */
+static int h_msb_send_command(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	u8 intreg;
+
+	if (mrq->error) {
+		dbg("send_command: unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+
+	/* HACK: see h_msb_write_block */
+	case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+		msb->state = MSB_SC_SEND_WRITE_OOB;
+		return 0;
+
+	case MSB_SC_SEND_WRITE_OOB:
+		if (!msb->command_need_oob) {
+			msb->state = MSB_SC_SEND_COMMAND;
+			goto again;
+		}
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+
+		msb->state = MSB_SC_SEND_COMMAND;
+		return 0;
+
+	case MSB_SC_SEND_COMMAND:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
+		msb->state = MSB_SC_SEND_INT_REQ;
+		return 0;
+
+	case MSB_SC_SEND_INT_REQ:
+		msb->state = MSB_SC_RECEIVE_INT_REQ;
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_SC_RECEIVE_INT_REQ:
+		intreg = mrq->data[0];
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state = MSB_SC_SEND_INT_REQ;
+			goto again;
+		}
+
+		return msb_exit_state_machine(msb, 0);
+	}
+
+	BUG();
+}
+
+/* Small handler for card reset */
+static int h_msb_reset(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	u8 command = MS_CMD_RESET;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+	switch (msb->state) {
+	case MSB_RS_SEND:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		mrq->need_card_int = 0;
+		msb->state = MSB_RS_CONFIRM;
+		return 0;
+	case MSB_RS_CONFIRM:
+		return msb_exit_state_machine(msb, 0);
+	}
+	BUG();
+}
+
+/* This handler is used to do serial->parallel switch */
+static int h_msb_parallel_switch(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct memstick_host *host = card->host;
+
+	if (mrq->error) {
+		dbg("parallel_switch: error");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+
+	switch (msb->state) {
+	case MSB_PS_SEND_SWITCH_COMMAND:
+		/* Set the parallel interface on memstick side */
+		msb->regs.param.system |= MEMSTICK_SYS_PAM;
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			1,
+			(unsigned char *)&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_PS_SWICH_HOST;
+		return 0;
+
+	case MSB_PS_SWICH_HOST:
+		 /* Set parallel interface on our side + send a dummy request
+			to see if card responds */
+		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		msb->state = MSB_PS_CONFIRM;
+		return 0;
+
+	case MSB_PS_CONFIRM:
+		return msb_exit_state_machine(msb, 0);
+	}
+
+	BUG();
+}
+
+static int msb_switch_to_parallel(struct msb_data *msb);
+
+/* Reset the card, to guard against hw errors beeing treated as bad blocks */
+static int msb_reset(struct msb_data *msb, bool full)
+{
+
+	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
+	struct memstick_dev *card = msb->card;
+	struct memstick_host *host = card->host;
+	int error;
+
+	/* Reset the card */
+	msb->regs.param.system = MEMSTICK_SYS_BAMD;
+
+	if (full) {
+		error =  host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+		if (error)
+			goto out_error;
+
+		msb_invalidate_reg_window(msb);
+
+		error = host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_ON);
+		if (error)
+			goto out_error;
+
+		error = host->set_param(host,
+					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+		if (error) {
+out_error:
+			dbg("Failed to reset the host controller");
+			msb->read_only = true;
+			return -EFAULT;
+		}
+	}
+
+	error = msb_run_state_machine(msb, h_msb_reset);
+	if (error) {
+		dbg("Failed to reset the card");
+		msb->read_only = true;
+		return -ENODEV;
+	}
+
+	/* Set parallel mode */
+	if (was_parallel)
+		msb_switch_to_parallel(msb);
+	return 0;
+}
+
+/* Attempts to switch interface to parallel mode */
+static int msb_switch_to_parallel(struct msb_data *msb)
+{
+	int error;
+
+	error = msb_run_state_machine(msb, h_msb_parallel_switch);
+	if (error) {
+		pr_err("Switch to parallel failed");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		msb_reset(msb, true);
+		return -EFAULT;
+	}
+
+	msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
+	return 0;
+}
+
+/* Changes overwrite flag on a page */
+static int msb_set_overwrite_flag(struct msb_data *msb,
+						u16 pba, u8 page, u8 flag)
+{
+	if (msb->read_only)
+		return -EROFS;
+
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_OVERWRITE;
+	msb->regs.extra_data.overwrite_flag = flag;
+	msb->command_value = MS_CMD_BLOCK_WRITE;
+	msb->command_need_oob = true;
+
+	dbg_verbose("changing overwrite flag to %02x for sector %d, page %d",
+							flag, pba, page);
+	return msb_run_state_machine(msb, h_msb_send_command);
+}
+
+static int msb_mark_bad(struct msb_data *msb, int pba)
+{
+	pr_notice("marking pba %d as bad", pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(
+			msb, pba, 0, 0xFF & ~MEMSTICK_OVERWRITE_BKST);
+}
+
+static int msb_mark_page_bad(struct msb_data *msb, int pba, int page)
+{
+	dbg("marking page %d of pba %d as bad", page, pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(msb,
+		pba, page, ~MEMSTICK_OVERWRITE_PGST0);
+}
+
+/* Erases one physical block */
+static int msb_erase_block(struct msb_data *msb, u16 pba)
+{
+	int error, try;
+	if (msb->read_only)
+		return -EROFS;
+
+	dbg_verbose("erasing pba %d", pba);
+
+	for (try = 1 ; try < 3 ; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = 0;
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->command_value = MS_CMD_BLOCK_ERASE;
+		msb->command_need_oob = false;
+
+
+		error = msb_run_state_machine(msb, h_msb_send_command);
+		if (!error || msb_reset(msb, true))
+			break;
+	}
+
+	if (error) {
+		pr_err("erase failed, marking pba %d as bad", pba);
+		msb_mark_bad(msb, pba);
+	}
+
+	dbg_verbose("erase success, marking pba %d as unused", pba);
+	msb_mark_block_unused(msb, pba);
+	__set_bit(pba, msb->erased_blocks_bitmap);
+	return error;
+}
+
+/* Reads one page from device */
+static int msb_read_page(struct msb_data *msb,
+	u16 pba, u8 page, struct ms_extra_data_register *extra,
+					struct scatterlist *sg,  int offset)
+{
+	int try, error;
+
+	if (pba == MS_BLOCK_INVALID) {
+		unsigned long flags;
+		struct sg_mapping_iter miter;
+		size_t len = msb->page_size;
+
+		dbg_verbose("read unmapped sector. returning 0xFF");
+
+		local_irq_save(flags);
+		sg_miter_start(&miter, sg, sg_nents(sg),
+				SG_MITER_ATOMIC | SG_MITER_TO_SG);
+
+		while (sg_miter_next(&miter) && len > 0) {
+
+			int chunklen;
+
+			if (offset && offset >= miter.length) {
+				offset -= miter.length;
+				continue;
+			}
+
+			chunklen = min(miter.length - offset, len);
+			memset(miter.addr + offset, 0xFF, chunklen);
+			len -= chunklen;
+			offset = 0;
+		}
+
+		sg_miter_stop(&miter);
+		local_irq_restore(flags);
+
+		if (offset)
+			return -EFAULT;
+
+		if (extra)
+			memset(extra, 0xFF, sizeof(*extra));
+		return 0;
+	}
+
+	if (pba >= msb->block_count) {
+		pr_err("BUG: attempt to read beyond"
+					" the end of the card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	for (try = 1 ; try < 3 ; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = page;
+		msb->regs.param.cp = MEMSTICK_CP_PAGE;
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		error = msb_run_state_machine(msb, h_msb_read_page);
+
+
+		if (error == -EUCLEAN) {
+			pr_notice("correctable error on pba %d, page %d",
+				pba, page);
+			error = 0;
+		}
+
+		if (!error && extra)
+			*extra = msb->regs.extra_data;
+
+		if (!error || msb_reset(msb, true))
+			break;
+
+	}
+
+	/* Mark bad pages */
+	if (error == -EBADMSG) {
+		pr_err("uncorrectable error on read of pba %d, page %d",
+			pba, page);
+
+		if (msb->regs.extra_data.overwrite_flag &
+					MEMSTICK_OVERWRITE_PGST0)
+			msb_mark_page_bad(msb, pba, page);
+		return -EBADMSG;
+	}
+
+	if (error)
+		pr_err("read of pba %d, page %d failed with error %d",
+			pba, page, error);
+	return error;
+}
+
+/* Reads oob of page only */
+static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
+	struct ms_extra_data_register *extra)
+{
+	int error;
+
+	BUG_ON(!extra);
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
+
+	if (pba > msb->block_count) {
+		pr_err("BUG: attempt to read beyond"
+					" the end of card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	error = msb_run_state_machine(msb, h_msb_read_page);
+	*extra = msb->regs.extra_data;
+
+	if (error == -EUCLEAN) {
+		pr_notice("correctable error on pba %d, page %d",
+			pba, page);
+		return 0;
+	}
+
+	return error;
+}
+
+
+/* Reads a block and compares it with data contained in scatterlist orig_sg */
+static int msb_verify_block(struct msb_data *msb, u16 pba,
+				struct scatterlist *orig_sg,  int offset)
+{
+	struct scatterlist sg;
+	int page = 0, error;
+
+	sg_init_one(&sg, msb->block_buffer, msb->block_size);
+
+	while (page < msb->pages_in_block) {
+
+		error = msb_read_page(msb, pba, page,
+				NULL, &sg, page * msb->page_size);
+		if (error)
+			return error;
+		page++;
+	}
+
+	if (msb_sg_compare_to_buffer(orig_sg, offset,
+				msb->block_buffer, msb->block_size))
+		return -EIO;
+	return 0;
+}
+
+/* Writes exectly one block + oob */
+static int msb_write_block(struct msb_data *msb,
+			u16 pba, u32 lba, struct scatterlist *sg, int offset)
+{
+	int error, current_try = 1;
+	BUG_ON(sg->length < msb->page_size);
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (pba == MS_BLOCK_INVALID) {
+		pr_err(
+			"BUG: write: attempt to write MS_BLOCK_INVALID block");
+		return -EINVAL;
+	}
+
+	if (pba >= msb->block_count || lba >= msb->logical_block_count) {
+		pr_err(
+		"BUG: write: attempt to write beyond the end of device");
+		return -EINVAL;
+	}
+
+	if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+		pr_err("BUG: write: lba zone mismatch");
+		return -EINVAL;
+	}
+
+	if (pba == msb->boot_block_locations[0] ||
+		pba == msb->boot_block_locations[1]) {
+		pr_err("BUG: write: attempt to write to boot blocks!");
+		return -EINVAL;
+	}
+
+	while (1) {
+
+		if (msb->read_only)
+			return -EROFS;
+
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->regs.param.page_address = 0;
+		msb->regs.param.block_address = cpu_to_be16(pba);
+
+		msb->regs.extra_data.management_flag = 0xFF;
+		msb->regs.extra_data.overwrite_flag = 0xF8;
+		msb->regs.extra_data.logical_address = cpu_to_be16(lba);
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		msb->current_page = 0;
+
+		error = msb_run_state_machine(msb, h_msb_write_block);
+
+		/* Sector we just wrote to is assumed erased since its pba
+			was erased. If it wasn't erased, write will succeed
+			and will just clear the bits that were set in the block
+			thus test that what we have written,
+			matches what we expect.
+			We do trust the blocks that we erased */
+		if (!error && (verify_writes ||
+				!test_bit(pba, msb->erased_blocks_bitmap)))
+			error = msb_verify_block(msb, pba, sg, offset);
+
+		if (!error)
+			break;
+
+		if (current_try > 1 || msb_reset(msb, true))
+			break;
+
+		pr_err("write failed, trying to erase the pba %d", pba);
+		error = msb_erase_block(msb, pba);
+		if (error)
+			break;
+
+		current_try++;
+	}
+	return error;
+}
+
+/* Finds a free block for write replacement */
+static u16 msb_get_free_block(struct msb_data *msb, int zone)
+{
+	u16 pos;
+	int pba = zone * MS_BLOCKS_IN_ZONE;
+	int i;
+
+	get_random_bytes(&pos, sizeof(pos));
+
+	if (!msb->free_block_count[zone]) {
+		pr_err("NO free blocks in the zone %d, to use for a write, "
+			"(media is WORN out) switching to RO mode", zone);
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	pos %= msb->free_block_count[zone];
+
+	dbg_verbose("have %d choices for a free block, selected randomally: %d",
+		msb->free_block_count[zone], pos);
+
+	pba = find_next_zero_bit(msb->used_blocks_bitmap,
+							msb->block_count, pba);
+	for (i = 0 ; i < pos ; ++i)
+		pba = find_next_zero_bit(msb->used_blocks_bitmap,
+						msb->block_count, pba + 1);
+
+	dbg_verbose("result of the free blocks scan: pba %d", pba);
+
+	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
+		pr_err("BUG: cant get a free block");
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	msb_mark_block_used(msb, pba);
+	return pba;
+}
+
+static int msb_update_block(struct msb_data *msb, u16 lba,
+	struct scatterlist *sg, int offset)
+{
+	u16 pba, new_pba;
+	int error, try;
+
+	pba = msb->lba_to_pba_table[lba];
+	dbg_verbose("start of a block update at lba  %d, pba %d", lba, pba);
+
+	if (pba != MS_BLOCK_INVALID) {
+		dbg_verbose("setting the update flag on the block");
+		msb_set_overwrite_flag(msb, pba, 0,
+				0xFF & ~MEMSTICK_OVERWRITE_UDST);
+	}
+
+	for (try = 0 ; try < 3 ; try++) {
+		new_pba = msb_get_free_block(msb,
+			msb_get_zone_from_lba(lba));
+
+		if (new_pba == MS_BLOCK_INVALID) {
+			error = -EIO;
+			goto out;
+		}
+
+		dbg_verbose("block update: writing updated block to the pba %d",
+								new_pba);
+		error = msb_write_block(msb, new_pba, lba, sg, offset);
+		if (error == -EBADMSG) {
+			msb_mark_bad(msb, new_pba);
+			continue;
+		}
+
+		if (error)
+			goto out;
+
+		dbg_verbose("block update: erasing the old block");
+		msb_erase_block(msb, pba);
+		msb->lba_to_pba_table[lba] = new_pba;
+		return 0;
+	}
+out:
+	if (error) {
+		pr_err("block update error after %d tries, "
+						"switching to r/o mode", try);
+		msb->read_only = true;
+	}
+	return error;
+}
+
+/* Converts endiannes in the boot block for easy use */
+static void msb_fix_boot_page_endianness(struct ms_boot_page *p)
+{
+	p->header.block_id = be16_to_cpu(p->header.block_id);
+	p->header.format_reserved = be16_to_cpu(p->header.format_reserved);
+	p->entry.disabled_block.start_addr
+		= be32_to_cpu(p->entry.disabled_block.start_addr);
+	p->entry.disabled_block.data_size
+		= be32_to_cpu(p->entry.disabled_block.data_size);
+	p->entry.cis_idi.start_addr
+		= be32_to_cpu(p->entry.cis_idi.start_addr);
+	p->entry.cis_idi.data_size
+		= be32_to_cpu(p->entry.cis_idi.data_size);
+	p->attr.block_size = be16_to_cpu(p->attr.block_size);
+	p->attr.number_of_blocks = be16_to_cpu(p->attr.number_of_blocks);
+	p->attr.number_of_effective_blocks
+		= be16_to_cpu(p->attr.number_of_effective_blocks);
+	p->attr.page_size = be16_to_cpu(p->attr.page_size);
+	p->attr.memory_manufacturer_code
+		= be16_to_cpu(p->attr.memory_manufacturer_code);
+	p->attr.memory_device_code = be16_to_cpu(p->attr.memory_device_code);
+	p->attr.implemented_capacity
+		= be16_to_cpu(p->attr.implemented_capacity);
+	p->attr.controller_number = be16_to_cpu(p->attr.controller_number);
+	p->attr.controller_function = be16_to_cpu(p->attr.controller_function);
+}
+
+static int msb_read_boot_blocks(struct msb_data *msb)
+{
+	int pba = 0;
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	struct ms_boot_page *page;
+
+	msb->boot_block_locations[0] = MS_BLOCK_INVALID;
+	msb->boot_block_locations[1] = MS_BLOCK_INVALID;
+	msb->boot_block_count = 0;
+
+	dbg_verbose("Start of a scan for the boot blocks");
+
+	if (!msb->boot_page) {
+		page = kmalloc(sizeof(struct ms_boot_page)*2, GFP_KERNEL);
+		if (!page)
+			return -ENOMEM;
+
+		msb->boot_page = page;
+	} else
+		page = msb->boot_page;
+
+	msb->block_count = MS_BLOCK_MAX_BOOT_ADDR;
+
+	for (pba = 0 ; pba < MS_BLOCK_MAX_BOOT_ADDR ; pba++) {
+
+		sg_init_one(&sg, page, sizeof(*page));
+		if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) {
+			dbg("boot scan: can't read pba %d", pba);
+			continue;
+		}
+
+		if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) {
+			dbg("managment flag doesn't indicate boot block %d",
+									pba);
+			continue;
+		}
+
+		if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) {
+			dbg("the pba at %d doesn' contain boot block ID", pba);
+			continue;
+		}
+
+		msb_fix_boot_page_endianness(page);
+		msb->boot_block_locations[msb->boot_block_count] = pba;
+
+		page++;
+		msb->boot_block_count++;
+
+		if (msb->boot_block_count == 2)
+			break;
+	}
+
+	if (!msb->boot_block_count) {
+		pr_err("media doesn't contain master page, aborting");
+		return -EIO;
+	}
+
+	dbg_verbose("End of scan for boot blocks");
+	return 0;
+}
+
+static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
+{
+	struct ms_boot_page *boot_block;
+	struct scatterlist sg;
+	u16 *buffer = NULL;
+	int offset = 0;
+	int i, error = 0;
+	int data_size, data_offset, page, page_offset, size_to_read;
+	u16 pba;
+
+	BUG_ON(block_nr > 1);
+	boot_block = &msb->boot_page[block_nr];
+	pba = msb->boot_block_locations[block_nr];
+
+	if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID)
+		return -EINVAL;
+
+	data_size = boot_block->entry.disabled_block.data_size;
+	data_offset = sizeof(struct ms_boot_page) +
+			boot_block->entry.disabled_block.start_addr;
+	if (!data_size)
+		return 0;
+
+	page = data_offset / msb->page_size;
+	page_offset = data_offset % msb->page_size;
+	size_to_read =
+		DIV_ROUND_UP(data_size + page_offset, msb->page_size) *
+			msb->page_size;
+
+	dbg("reading bad block of boot block at pba %d, offset %d len %d",
+		pba, data_offset, data_size);
+
+	buffer = kzalloc(size_to_read, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	/* Read the buffer */
+	sg_init_one(&sg, buffer, size_to_read);
+
+	while (offset < size_to_read) {
+		error = msb_read_page(msb, pba, page, NULL, &sg, offset);
+		if (error)
+			goto out;
+
+		page++;
+		offset += msb->page_size;
+
+		if (page == msb->pages_in_block) {
+			pr_err(
+			"bad block table extends beyond the boot block");
+			break;
+		}
+	}
+
+	/* Process the bad block table */
+	for (i = page_offset ; i < data_size / sizeof(u16) ; i++) {
+
+		u16 bad_block = be16_to_cpu(buffer[i]);
+
+		if (bad_block >= msb->block_count) {
+			dbg("bad block table contains invalid block %d",
+								bad_block);
+			continue;
+		}
+
+		if (test_bit(bad_block, msb->used_blocks_bitmap))  {
+			dbg("duplicate bad block %d in the table",
+				bad_block);
+			continue;
+		}
+
+		dbg("block %d is marked as factory bad", bad_block);
+		msb_mark_block_used(msb, bad_block);
+	}
+out:
+	kfree(buffer);
+	return error;
+}
+
+static int msb_ftl_initialize(struct msb_data *msb)
+{
+	int i;
+
+	if (msb->ftl_initialized)
+		return 0;
+
+	msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE;
+	msb->logical_block_count = msb->zone_count * 496 - 2;
+
+	msb->used_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->erased_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->lba_to_pba_table =
+		kmalloc(msb->logical_block_count * sizeof(u16), GFP_KERNEL);
+
+	if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
+						!msb->erased_blocks_bitmap) {
+		kfree(msb->used_blocks_bitmap);
+		kfree(msb->lba_to_pba_table);
+		kfree(msb->erased_blocks_bitmap);
+		return -ENOMEM;
+	}
+
+	for (i = 0 ; i < msb->zone_count ; i++)
+		msb->free_block_count[i] = MS_BLOCKS_IN_ZONE;
+
+	memset(msb->lba_to_pba_table, MS_BLOCK_INVALID,
+			msb->logical_block_count * sizeof(u16));
+
+	dbg("initial FTL tables created. Zone count = %d, "
+					"Logical block count = %d",
+		msb->zone_count, msb->logical_block_count);
+
+	msb->ftl_initialized = true;
+	return 0;
+}
+
+static int msb_ftl_scan(struct msb_data *msb)
+{
+	u16 pba, lba, other_block;
+	u8 overwrite_flag, managment_flag, other_overwrite_flag;
+	int error;
+	struct ms_extra_data_register extra;
+	u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL);
+
+	if (!overwrite_flags)
+		return -ENOMEM;
+
+	dbg("Start of media scanning");
+	for (pba = 0 ; pba < msb->block_count ; pba++) {
+
+		if (pba == msb->boot_block_locations[0] ||
+			pba == msb->boot_block_locations[1]) {
+			dbg_verbose("pba %05d -> [boot block]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		if (test_bit(pba, msb->used_blocks_bitmap)) {
+			dbg_verbose("pba %05d -> [factory bad]", pba);
+			continue;
+		}
+
+		memset(&extra, 0, sizeof(extra));
+		error = msb_read_oob(msb, pba, 0, &extra);
+
+		/* can't trust the page if we can't read the oob */
+		if (error == -EBADMSG) {
+			pr_notice(
+			"oob of pba %d damaged, will try to erase it", pba);
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		} else if (error)
+			return error;
+
+		lba = be16_to_cpu(extra.logical_address);
+		managment_flag = extra.management_flag;
+		overwrite_flag = extra.overwrite_flag;
+		overwrite_flags[pba] = overwrite_flag;
+
+		/* Skip bad blocks */
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
+			dbg("pba %05d -> [BAD]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Skip system/drm blocks */
+		if ((managment_flag & MEMSTICK_MANAGMENT_FLAG_NORMAL) !=
+			MEMSTICK_MANAGMENT_FLAG_NORMAL) {
+			dbg("pba %05d -> [reserved managment flag %02x]",
+							pba, managment_flag);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Erase temporary tables */
+		if (!(managment_flag & MEMSTICK_MANAGEMENT_ATFLG)) {
+			dbg("pba %05d -> [temp table] - will erase", pba);
+
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		if (lba == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [free]", pba);
+			continue;
+		}
+
+		msb_mark_block_used(msb, pba);
+
+		/* Block has LBA not according to zoning*/
+		if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+			pr_notice("pba %05d -> [bad lba %05d] - will erase",
+								pba, lba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		/* No collisions - great */
+		if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [lba %05d]", pba, lba);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		other_block = msb->lba_to_pba_table[lba];
+		other_overwrite_flag = overwrite_flags[other_block];
+
+		pr_notice("Collision between pba %d and pba %d",
+			pba, other_block);
+
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			pr_notice("pba %d is marked as stable, use it", pba);
+			msb_erase_block(msb, other_block);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			pr_notice("pba %d is marked as stable, use it",
+								other_block);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		pr_notice("collision between blocks %d and %d,"
+		" without stable flag set on both, erasing pba %d",
+				pba, other_block, other_block);
+
+		msb_erase_block(msb, other_block);
+		msb->lba_to_pba_table[lba] = pba;
+	}
+
+	dbg("End of media scanning");
+	kfree(overwrite_flags);
+	return 0;
+}
+
+static void msb_cache_flush_timer(unsigned long data)
+{
+	struct msb_data *msb = (struct msb_data *)data;
+	msb->need_flush_cache = true;
+	queue_work(msb->io_queue, &msb->io_work);
+}
+
+
+static void msb_cache_discard(struct msb_data *msb)
+{
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return;
+
+	del_timer_sync(&msb->cache_flush_timer);
+
+	dbg_verbose("Discarding the write cache");
+	msb->cache_block_lba = MS_BLOCK_INVALID;
+	bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block);
+}
+
+static int msb_cache_init(struct msb_data *msb)
+{
+	setup_timer(&msb->cache_flush_timer, msb_cache_flush_timer,
+		(unsigned long)msb);
+
+	if (!msb->cache)
+		msb->cache = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->cache)
+		return -ENOMEM;
+
+	msb_cache_discard(msb);
+	return 0;
+}
+
+static int msb_cache_flush(struct msb_data *msb)
+{
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	int page, offset, error;
+	u16 pba, lba;
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return 0;
+
+	lba = msb->cache_block_lba;
+	pba = msb->lba_to_pba_table[lba];
+
+	dbg_verbose("Flushing the write cache of pba %d (LBA %d)",
+						pba, msb->cache_block_lba);
+
+	sg_init_one(&sg, msb->cache , msb->block_size);
+
+	/* Read all missing pages in cache */
+	for (page = 0 ; page < msb->pages_in_block ; page++) {
+
+		if (test_bit(page, &msb->valid_cache_bitmap))
+			continue;
+
+		offset = page * msb->page_size;
+
+		dbg_verbose("reading non-present sector %d of cache block %d",
+			page, lba);
+		error = msb_read_page(msb, pba, page, &extra, &sg, offset);
+
+		/* Bad pages are copied with 00 page status */
+		if (error == -EBADMSG) {
+			pr_err("read error on sector %d, contents probably"
+				" damaged", page);
+			continue;
+		}
+
+		if (error)
+			return error;
+
+		if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) !=
+							MEMSTICK_OV_PG_NORMAL) {
+			dbg("page %d is marked as bad", page);
+			continue;
+		}
+
+		set_bit(page, &msb->valid_cache_bitmap);
+	}
+
+	/* Write the cache now */
+	error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
+	pba = msb->lba_to_pba_table[msb->cache_block_lba];
+
+	/* Mark invalid pages */
+	if (!error) {
+		for (page = 0 ; page < msb->pages_in_block ; page++) {
+
+			if (test_bit(page, &msb->valid_cache_bitmap))
+				continue;
+
+			dbg("marking page %d as containing damaged data",
+				page);
+			msb_set_overwrite_flag(msb,
+				pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL);
+		}
+	}
+
+	msb_cache_discard(msb);
+	return error;
+}
+
+static int msb_cache_write(struct msb_data *msb, int lba,
+	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
+{
+	int error;
+	struct scatterlist sg_tmp[10];
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
+						lba != msb->cache_block_lba)
+		if (add_to_cache_only)
+			return 0;
+
+	/* If we need to write different block */
+	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
+						lba != msb->cache_block_lba) {
+		dbg_verbose("first flush the cache");
+		error = msb_cache_flush(msb);
+		if (error)
+			return error;
+	}
+
+	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
+		msb->cache_block_lba  = lba;
+		mod_timer(&msb->cache_flush_timer,
+			jiffies + msecs_to_jiffies(cache_flush_timeout));
+	}
+
+	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
+
+	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+	msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+
+	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
+		msb->cache + page * msb->page_size, msb->page_size);
+
+	set_bit(page, &msb->valid_cache_bitmap);
+	return 0;
+}
+
+static int msb_cache_read(struct msb_data *msb, int lba,
+				int page, struct scatterlist *sg, int offset)
+{
+	int pba = msb->lba_to_pba_table[lba];
+	struct scatterlist sg_tmp[10];
+	int error = 0;
+
+	if (lba == msb->cache_block_lba &&
+			test_bit(page, &msb->valid_cache_bitmap)) {
+
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from cache",
+							lba, pba, page);
+
+		sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+		msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+		sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp),
+			msb->cache + msb->page_size * page,
+							msb->page_size);
+	} else {
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from device",
+							lba, pba, page);
+
+		error = msb_read_page(msb, pba, page, NULL, sg, offset);
+		if (error)
+			return error;
+
+		msb_cache_write(msb, lba, page, true, sg, offset);
+	}
+	return error;
+}
+
+/* Emulated geometry table
+ * This table content isn't that importaint,
+ * One could put here different values, providing that they still
+ * cover whole disk.
+ * 64 MB entry is what windows reports for my 64M memstick */
+
+static const struct chs_entry chs_table[] = {
+/*        size sectors cylynders  heads */
+	{ 4,    16,    247,       2  },
+	{ 8,    16,    495,       2  },
+	{ 16,   16,    495,       4  },
+	{ 32,   16,    991,       4  },
+	{ 64,   16,    991,       8  },
+	{128,   16,    991,       16 },
+	{ 0 }
+};
+
+/* Load information about the card */
+static int msb_init_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	struct ms_boot_page *boot_block;
+	int error = 0, i, raw_size_in_megs;
+
+	msb->caps = 0;
+
+	if (card->id.class >= MEMSTICK_CLASS_ROM &&
+				card->id.class <= MEMSTICK_CLASS_ROM)
+		msb->read_only = true;
+
+	msb->state = -1;
+	error = msb_reset(msb, false);
+	if (error)
+		return error;
+
+	/* Due to a bug in Jmicron driver written by Alex Dubov,
+	 its serial mode barely works,
+	 so we switch to parallel mode right away */
+	if (host->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+
+	msb->page_size = sizeof(struct ms_boot_page);
+
+	/* Read the boot page */
+	error = msb_read_boot_blocks(msb);
+	if (error)
+		return -EIO;
+
+	boot_block = &msb->boot_page[0];
+
+	/* Save intersting attributes from boot page */
+	msb->block_count = boot_block->attr.number_of_blocks;
+	msb->page_size = boot_block->attr.page_size;
+
+	msb->pages_in_block = boot_block->attr.block_size * 2;
+	msb->block_size = msb->page_size * msb->pages_in_block;
+
+	if (msb->page_size > PAGE_SIZE) {
+		/* this isn't supported by linux at all, anyway*/
+		dbg("device page %d size isn't supported", msb->page_size);
+		return -EINVAL;
+	}
+
+	msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->block_buffer)
+		return -ENOMEM;
+
+	raw_size_in_megs = (msb->block_size * msb->block_count) >> 20;
+
+	for (i = 0 ; chs_table[i].size ; i++) {
+
+		if (chs_table[i].size != raw_size_in_megs)
+			continue;
+
+		msb->geometry.cylinders = chs_table[i].cyl;
+		msb->geometry.heads = chs_table[i].head;
+		msb->geometry.sectors = chs_table[i].sec;
+		break;
+	}
+
+	if (boot_block->attr.transfer_supporting == 1)
+		msb->caps |= MEMSTICK_CAP_PAR4;
+
+	if (boot_block->attr.device_type & 0x03)
+		msb->read_only = true;
+
+	dbg("Total block count = %d", msb->block_count);
+	dbg("Each block consists of %d pages", msb->pages_in_block);
+	dbg("Page size = %d bytes", msb->page_size);
+	dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4));
+	dbg("Read only: %d", msb->read_only);
+
+#if 0
+	/* Now we can switch the interface */
+	if (host->caps & msb->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+#endif
+
+	error = msb_cache_init(msb);
+	if (error)
+		return error;
+
+	error = msb_ftl_initialize(msb);
+	if (error)
+		return error;
+
+
+	/* Read the bad block table */
+	error = msb_read_bad_block_table(msb, 0);
+
+	if (error && error != -ENOMEM) {
+		dbg("failed to read bad block table from primary boot block,"
+							" trying from backup");
+		error = msb_read_bad_block_table(msb, 1);
+	}
+
+	if (error)
+		return error;
+
+	/* *drum roll* Scan the media */
+	error = msb_ftl_scan(msb);
+	if (error) {
+		pr_err("Scan of media failed");
+		return error;
+	}
+
+	return 0;
+
+}
+
+static int msb_do_write_request(struct msb_data *msb, int lba,
+	int page, struct scatterlist *sg, size_t len, int *sucessfuly_written)
+{
+	int error = 0;
+	off_t offset = 0;
+	*sucessfuly_written = 0;
+
+	while (offset < len) {
+		if (page == 0 && len - offset >= msb->block_size) {
+
+			if (msb->cache_block_lba == lba)
+				msb_cache_discard(msb);
+
+			dbg_verbose("Writing whole lba %d", lba);
+			error = msb_update_block(msb, lba, sg, offset);
+			if (error)
+				return error;
+
+			offset += msb->block_size;
+			*sucessfuly_written += msb->block_size;
+			lba++;
+			continue;
+		}
+
+		error = msb_cache_write(msb, lba, page, false, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_written += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static int msb_do_read_request(struct msb_data *msb, int lba,
+		int page, struct scatterlist *sg, int len, int *sucessfuly_read)
+{
+	int error = 0;
+	int offset = 0;
+	*sucessfuly_read = 0;
+
+	while (offset < len) {
+
+		error = msb_cache_read(msb, lba, page, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_read += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static void msb_io_work(struct work_struct *work)
+{
+	struct msb_data *msb = container_of(work, struct msb_data, io_work);
+	int page, error, len;
+	sector_t lba;
+	unsigned long flags;
+	struct scatterlist *sg = msb->prealloc_sg;
+
+	dbg_verbose("IO: work started");
+
+	while (1) {
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (msb->need_flush_cache) {
+			msb->need_flush_cache = false;
+			spin_unlock_irqrestore(&msb->q_lock, flags);
+			msb_cache_flush(msb);
+			continue;
+		}
+
+		if (!msb->req) {
+			msb->req = blk_fetch_request(msb->queue);
+			if (!msb->req) {
+				dbg_verbose("IO: no more requests exiting");
+				spin_unlock_irqrestore(&msb->q_lock, flags);
+				return;
+			}
+		}
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+
+		/* If card was removed meanwhile */
+		if (!msb->req)
+			return;
+
+		/* process the request */
+		dbg_verbose("IO: processing new request");
+		blk_rq_map_sg(msb->queue, msb->req, sg);
+
+		lba = blk_rq_pos(msb->req);
+
+		sector_div(lba, msb->page_size / 512);
+		page = do_div(lba, msb->pages_in_block);
+
+		if (rq_data_dir(msb->req) == READ)
+			error = msb_do_read_request(msb, lba, page, sg,
+				blk_rq_bytes(msb->req), &len);
+		else
+			error = msb_do_write_request(msb, lba, page, sg,
+				blk_rq_bytes(msb->req), &len);
+
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (len)
+			if (!__blk_end_request(msb->req, 0, len))
+				msb->req = NULL;
+
+		if (error && msb->req) {
+			dbg_verbose("IO: ending one sector "
+					"of the request with error");
+			if (!__blk_end_request(msb->req, error, msb->page_size))
+				msb->req = NULL;
+		}
+
+		if (msb->req)
+			dbg_verbose("IO: request still pending");
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+}
+
+static DEFINE_IDR(msb_disk_idr); /*set of used disk numbers */
+static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */
+
+static int msb_bd_open(struct block_device *bdev, fmode_t mode)
+{
+	struct gendisk *disk = bdev->bd_disk;
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device open");
+
+	mutex_lock(&msb_disk_lock);
+
+	if (msb && msb->card)
+		msb->usage_count++;
+
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static void msb_data_clear(struct msb_data *msb)
+{
+	kfree(msb->boot_page);
+	kfree(msb->used_blocks_bitmap);
+	kfree(msb->lba_to_pba_table);
+	kfree(msb->cache);
+	msb->card = NULL;
+}
+
+static int msb_disk_release(struct gendisk *disk)
+{
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device release");
+	mutex_lock(&msb_disk_lock);
+
+	if (msb) {
+		if (msb->usage_count)
+			msb->usage_count--;
+
+		if (!msb->usage_count) {
+			kfree(msb);
+			disk->private_data = NULL;
+			idr_remove(&msb_disk_idr, msb->disk_id);
+			put_disk(disk);
+		}
+	}
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static int msb_bd_release(struct gendisk *disk, fmode_t mode)
+{
+	return msb_disk_release(disk);
+}
+
+static int msb_bd_getgeo(struct block_device *bdev,
+				 struct hd_geometry *geo)
+{
+	struct msb_data *msb = bdev->bd_disk->private_data;
+	*geo = msb->geometry;
+	return 0;
+}
+
+static int msb_prepare_req(struct request_queue *q, struct request *req)
+{
+	if (req->cmd_type != REQ_TYPE_FS &&
+				req->cmd_type != REQ_TYPE_BLOCK_PC) {
+		blk_dump_rq_flags(req, "MS unsupported request");
+		return BLKPREP_KILL;
+	}
+	req->cmd_flags |= REQ_DONTPREP;
+	return BLKPREP_OK;
+}
+
+static void msb_submit_req(struct request_queue *q)
+{
+	struct memstick_dev *card = q->queuedata;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct request *req = NULL;
+
+	dbg_verbose("Submit request");
+
+	if (msb->card_dead) {
+		dbg("Refusing requests on removed card");
+
+		WARN_ON(!msb->io_queue_stopped);
+
+		while ((req = blk_fetch_request(q)) != NULL)
+			__blk_end_request_all(req, -ENODEV);
+		return;
+	}
+
+	if (msb->req)
+		return;
+
+	if (!msb->io_queue_stopped)
+		queue_work(msb->io_queue, &msb->io_work);
+}
+
+static int msb_check_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	return (msb->card_dead == 0);
+}
+
+static void msb_stop(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	dbg("Stopping all msblock IO");
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_stop_queue(msb->queue);
+	msb->io_queue_stopped = true;
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	del_timer_sync(&msb->cache_flush_timer);
+	flush_workqueue(msb->io_queue);
+
+	if (msb->req) {
+		spin_lock_irqsave(&msb->q_lock, flags);
+		blk_requeue_request(msb->queue, msb->req);
+		msb->req = NULL;
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+
+}
+
+static void msb_start(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	dbg("Resuming IO from msblock");
+
+	msb_invalidate_reg_window(msb);
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	if (!msb->io_queue_stopped || msb->card_dead) {
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Kick cache flush anyway, its harmless */
+	msb->need_flush_cache = true;
+	msb->io_queue_stopped = false;
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	queue_work(msb->io_queue, &msb->io_work);
+
+}
+
+static const struct block_device_operations msb_bdops = {
+	.open    = msb_bd_open,
+	.release = msb_bd_release,
+	.getgeo  = msb_bd_getgeo,
+	.owner   = THIS_MODULE
+};
+
+/* Registers the block device */
+static int msb_init_disk(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	int rc;
+	u64 limit = BLK_BOUNCE_HIGH;
+	unsigned long capacity;
+
+	if (host->dev.dma_mask && *(host->dev.dma_mask))
+		limit = *(host->dev.dma_mask);
+
+	mutex_lock(&msb_disk_lock);
+	if (!idr_pre_get(&msb_disk_idr, GFP_KERNEL)) {
+		mutex_unlock(&msb_disk_lock);
+		return -ENOMEM;
+	}
+	rc = idr_get_new(&msb_disk_idr, card, &msb->disk_id);
+	mutex_unlock(&msb_disk_lock);
+
+	if (rc)
+		return rc;
+
+	msb->disk = alloc_disk(0);
+	if (!msb->disk) {
+		rc = -ENOMEM;
+		goto out_release_id;
+	}
+
+	msb->queue = blk_init_queue(msb_submit_req, &msb->q_lock);
+	if (!msb->queue) {
+		rc = -ENOMEM;
+		goto out_put_disk;
+	}
+
+	msb->queue->queuedata = card;
+	blk_queue_prep_rq(msb->queue, msb_prepare_req);
+
+	blk_queue_bounce_limit(msb->queue, limit);
+	blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
+	blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
+	blk_queue_max_segment_size(msb->queue,
+				   MS_BLOCK_MAX_PAGES * msb->page_size);
+	blk_queue_logical_block_size(msb->queue, msb->page_size);
+
+	sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
+	msb->disk->fops = &msb_bdops;
+	msb->disk->private_data = msb;
+	msb->disk->queue = msb->queue;
+	msb->disk->driverfs_dev = &card->dev;
+	msb->disk->flags |= GENHD_FL_EXT_DEVT;
+
+	capacity = msb->pages_in_block * msb->logical_block_count;
+	capacity *= (msb->page_size / 512);
+	set_capacity(msb->disk, capacity);
+	dbg("Set total disk size to %lu sectors", capacity);
+
+	msb->usage_count = 1;
+	msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
+	INIT_WORK(&msb->io_work, msb_io_work);
+	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
+
+	if (msb->read_only)
+		set_disk_ro(msb->disk, 1);
+
+	msb_start(card);
+	add_disk(msb->disk);
+	dbg("Disk added");
+	return 0;
+
+out_put_disk:
+	put_disk(msb->disk);
+out_release_id:
+	mutex_lock(&msb_disk_lock);
+	idr_remove(&msb_disk_idr, msb->disk_id);
+	mutex_unlock(&msb_disk_lock);
+	return rc;
+}
+
+static int msb_probe(struct memstick_dev *card)
+{
+	struct msb_data *msb;
+	int rc = 0;
+
+	msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!msb)
+		return -ENOMEM;
+	memstick_set_drvdata(card, msb);
+	msb->card = card;
+	spin_lock_init(&msb->q_lock);
+
+	rc = msb_init_card(card);
+	if (rc)
+		goto out_free;
+
+	rc = msb_init_disk(card);
+	if (!rc) {
+		card->check = msb_check_card;
+		card->stop = msb_stop;
+		card->start = msb_start;
+		return 0;
+	}
+out_free:
+	memstick_set_drvdata(card, NULL);
+	msb_data_clear(msb);
+	kfree(msb);
+	return rc;
+}
+
+static void msb_remove(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	if (!msb->io_queue_stopped)
+		msb_stop(card);
+
+	dbg("Removing the disk device");
+
+	/* Take care of unhandled + new requests from now on */
+	spin_lock_irqsave(&msb->q_lock, flags);
+	msb->card_dead = true;
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Remove the disk */
+	del_gendisk(msb->disk);
+	blk_cleanup_queue(msb->queue);
+	msb->queue = NULL;
+
+	mutex_lock(&msb_disk_lock);
+	msb_data_clear(msb);
+	mutex_unlock(&msb_disk_lock);
+
+	msb_disk_release(msb->disk);
+	memstick_set_drvdata(card, NULL);
+}
+
+#ifdef CONFIG_PM
+
+static int msb_suspend(struct memstick_dev *card, pm_message_t state)
+{
+	msb_stop(card);
+	return 0;
+}
+
+static int msb_resume(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct msb_data *new_msb = NULL;
+	bool card_dead = true;
+
+#ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
+	msb->card_dead = true;
+	return 0;
+#endif
+	mutex_lock(&card->host->lock);
+
+	new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!new_msb)
+		goto out;
+
+	new_msb->card = card;
+	memstick_set_drvdata(card, new_msb);
+	spin_lock_init(&new_msb->q_lock);
+	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
+
+	if (msb_init_card(card))
+		goto out;
+
+	if (msb->block_size != new_msb->block_size)
+		goto out;
+
+	if (memcmp(msb->boot_page, new_msb->boot_page,
+					sizeof(struct ms_boot_page)))
+		goto out;
+
+	if (msb->logical_block_count != new_msb->logical_block_count ||
+		memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
+						msb->logical_block_count))
+		goto out;
+
+	if (msb->block_count != new_msb->block_count ||
+		memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
+							msb->block_count / 8))
+		goto out;
+
+	card_dead = false;
+out:
+	if (card_dead)
+		dbg("Card was removed/replaced during suspend");
+
+	msb->card_dead = card_dead;
+	memstick_set_drvdata(card, msb);
+
+	if (new_msb) {
+		msb_data_clear(new_msb);
+		kfree(new_msb);
+	}
+
+	msb_start(card);
+	mutex_unlock(&card->host->lock);
+	return 0;
+}
+#else
+
+#define msb_suspend NULL
+#define msb_resume NULL
+
+#endif /* CONFIG_PM */
+
+static struct memstick_device_id msb_id_tbl[] = {
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_FLASH},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_ROM},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_RO},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_WP},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
+	 MEMSTICK_CLASS_DUO},
+	{}
+};
+MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
+
+
+static struct memstick_driver msb_driver = {
+	.driver = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE
+	},
+	.id_table = msb_id_tbl,
+	.probe    = msb_probe,
+	.remove   = msb_remove,
+	.suspend  = msb_suspend,
+	.resume   = msb_resume
+};
+
+static int major;
+
+static int __init msb_init(void)
+{
+	int rc = register_blkdev(0, DRIVER_NAME);
+
+	if (rc < 0) {
+		pr_err("failed to register major (error %d)\n", rc);
+		return rc;
+	}
+
+	major = rc;
+	rc = memstick_register_driver(&msb_driver);
+	if (rc) {
+		unregister_blkdev(major, DRIVER_NAME);
+		pr_err("failed to register memstick driver (error %d)\n", rc);
+	}
+
+	return rc;
+}
+
+static void __exit msb_exit(void)
+{
+	memstick_unregister_driver(&msb_driver);
+	unregister_blkdev(major, DRIVER_NAME);
+	idr_destroy(&msb_disk_idr);
+}
+
+module_init(msb_init);
+module_exit(msb_exit);
+
+module_param(cache_flush_timeout, int, S_IRUGO);
+MODULE_PARM_DESC(cache_flush_timeout,
+				"Cache flush timeout in msec (1000 default)");
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+module_param(verify_writes, bool, S_IRUGO);
+MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Maxim Levitsky");
+MODULE_DESCRIPTION("Sony MemoryStick block device driver");
diff --git a/drivers/memstick/core/ms_block.h b/drivers/memstick/core/ms_block.h
new file mode 100644
index 0000000..546b55e
--- /dev/null
+++ b/drivers/memstick/core/ms_block.h
@@ -0,0 +1,290 @@
+/*
+ *  ms_block.c - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2010 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver are copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ * Also ms structures were copied from old broken driver by same author
+ * These probably come from MS spec
+ *
+ */
+
+#ifndef MS_BLOCK_NEW_H
+#define MS_BLOCK_NEW_H
+
+#define MS_BLOCK_MAX_SEGS      32
+#define MS_BLOCK_MAX_PAGES     ((2 << 16) - 1)
+
+#define MS_BLOCK_MAX_BOOT_ADDR 0x000c
+#define MS_BLOCK_BOOT_ID       0x0001
+#define MS_BLOCK_INVALID       0xffff
+#define MS_MAX_ZONES           16
+#define MS_BLOCKS_IN_ZONE      512
+
+#define MS_BLOCK_MAP_LINE_SZ   16
+#define MS_BLOCK_PART_SHIFT    3
+
+
+#define MEMSTICK_UNCORR_ERROR (MEMSTICK_STATUS1_UCFG | \
+		MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCDT)
+
+#define MEMSTICK_CORR_ERROR (MEMSTICK_STATUS1_FGER | MEMSTICK_STATUS1_EXER | \
+	MEMSTICK_STATUS1_DTER)
+
+#define MEMSTICK_INT_ERROR (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)
+
+#define MEMSTICK_OVERWRITE_FLAG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | \
+	MEMSTICK_OVERWRITE_PGST0  | \
+	MEMSTICK_OVERWRITE_BKST)
+
+#define MEMSTICK_OV_PG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | MEMSTICK_OVERWRITE_PGST0)
+
+#define MEMSTICK_MANAGMENT_FLAG_NORMAL \
+	(MEMSTICK_MANAGEMENT_SYSFLG |  \
+	MEMSTICK_MANAGEMENT_SCMS1   |  \
+	MEMSTICK_MANAGEMENT_SCMS0)     \
+
+struct ms_boot_header {
+	unsigned short block_id;
+	unsigned short format_reserved;
+	unsigned char  reserved0[184];
+	unsigned char  data_entry;
+	unsigned char  reserved1[179];
+} __packed;
+
+
+struct ms_system_item {
+	unsigned int  start_addr;
+	unsigned int  data_size;
+	unsigned char data_type_id;
+	unsigned char reserved[3];
+} __packed;
+
+struct ms_system_entry {
+	struct ms_system_item disabled_block;
+	struct ms_system_item cis_idi;
+	unsigned char         reserved[24];
+} __packed;
+
+struct ms_boot_attr_info {
+	unsigned char      memorystick_class;
+	unsigned char      format_unique_value1;
+	unsigned short     block_size;
+	unsigned short     number_of_blocks;
+	unsigned short     number_of_effective_blocks;
+	unsigned short     page_size;
+	unsigned char      extra_data_size;
+	unsigned char      format_unique_value2;
+	unsigned char      assembly_time[8];
+	unsigned char      format_unique_value3;
+	unsigned char      serial_number[3];
+	unsigned char      assembly_manufacturer_code;
+	unsigned char      assembly_model_code[3];
+	unsigned short     memory_manufacturer_code;
+	unsigned short     memory_device_code;
+	unsigned short     implemented_capacity;
+	unsigned char      format_unique_value4[2];
+	unsigned char      vcc;
+	unsigned char      vpp;
+	unsigned short     controller_number;
+	unsigned short     controller_function;
+	unsigned char      reserved0[9];
+	unsigned char      transfer_supporting;
+	unsigned short     format_unique_value5;
+	unsigned char      format_type;
+	unsigned char      memorystick_application;
+	unsigned char      device_type;
+	unsigned char      reserved1[22];
+	unsigned char      format_uniqure_value6[2];
+	unsigned char      reserved2[15];
+} __packed;
+
+struct ms_cis_idi {
+	unsigned short general_config;
+	unsigned short logical_cylinders;
+	unsigned short reserved0;
+	unsigned short logical_heads;
+	unsigned short track_size;
+	unsigned short page_size;
+	unsigned short pages_per_track;
+	unsigned short msw;
+	unsigned short lsw;
+	unsigned short reserved1;
+	unsigned char  serial_number[20];
+	unsigned short buffer_type;
+	unsigned short buffer_size_increments;
+	unsigned short long_command_ecc;
+	unsigned char  firmware_version[28];
+	unsigned char  model_name[18];
+	unsigned short reserved2[5];
+	unsigned short pio_mode_number;
+	unsigned short dma_mode_number;
+	unsigned short field_validity;
+	unsigned short current_logical_cylinders;
+	unsigned short current_logical_heads;
+	unsigned short current_pages_per_track;
+	unsigned int   current_page_capacity;
+	unsigned short mutiple_page_setting;
+	unsigned int   addressable_pages;
+	unsigned short single_word_dma;
+	unsigned short multi_word_dma;
+	unsigned char  reserved3[128];
+} __packed;
+
+
+struct ms_boot_page {
+	struct ms_boot_header    header;
+	struct ms_system_entry   entry;
+	struct ms_boot_attr_info attr;
+} __packed;
+
+struct msb_data {
+	unsigned int			usage_count;
+	struct memstick_dev		*card;
+	struct gendisk			*disk;
+	struct request_queue		*queue;
+	spinlock_t			q_lock;
+	struct hd_geometry		geometry;
+	struct attribute_group		attr_group;
+	struct request			*req;
+	int				caps;
+	int				disk_id;
+
+	/* IO */
+	struct workqueue_struct		*io_queue;
+	bool				io_queue_stopped;
+	struct work_struct		io_work;
+	bool				card_dead;
+
+	/* Media properties */
+	struct ms_boot_page		*boot_page;
+	u16				boot_block_locations[2];
+	int				boot_block_count;
+
+	bool				read_only;
+	unsigned short			page_size;
+	int				block_size;
+	int				pages_in_block;
+	int				zone_count;
+	int				block_count;
+	int				logical_block_count;
+
+	/* FTL tables */
+	unsigned long			*used_blocks_bitmap;
+	unsigned long			*erased_blocks_bitmap;
+	u16				*lba_to_pba_table;
+	int				free_block_count[MS_MAX_ZONES];
+	bool				ftl_initialized;
+
+	/* Cache */
+	unsigned char			*cache;
+	unsigned long			valid_cache_bitmap;
+	int				cache_block_lba;
+	bool				need_flush_cache;
+	struct timer_list		cache_flush_timer;
+
+	/* Preallocated buffers */
+	unsigned char			*block_buffer;
+	struct scatterlist		prealloc_sg[MS_BLOCK_MAX_SEGS+1];
+
+
+	/* handler's local data */
+	struct ms_register_addr		reg_addr;
+	bool				addr_valid;
+
+	u8				command_value;
+	bool				command_need_oob;
+	struct scatterlist		*current_sg;
+	int				current_sg_offset;
+
+	struct ms_register		regs;
+	int				current_page;
+
+	int				state;
+	int				exit_error;
+	bool				int_polling;
+	unsigned long			int_timeout;
+
+};
+
+enum msb_readpage_states {
+	MSB_RP_SEND_BLOCK_ADDRESS = 0,
+	MSB_RP_SEND_READ_COMMAND,
+
+	MSB_RP_SEND_INT_REQ,
+	MSB_RP_RECEIVE_INT_REQ_RESULT,
+
+	MSB_RP_SEND_READ_STATUS_REG,
+	MSB_RP_RECIVE_STATUS_REG,
+
+	MSB_RP_SEND_OOB_READ,
+	MSB_RP_RECEIVE_OOB_READ,
+
+	MSB_RP_SEND_READ_DATA,
+	MSB_RP_RECEIVE_READ_DATA,
+};
+
+enum msb_write_block_states {
+	MSB_WB_SEND_WRITE_PARAMS = 0,
+	MSB_WB_SEND_WRITE_OOB,
+	MSB_WB_SEND_WRITE_COMMAND,
+
+	MSB_WB_SEND_INT_REQ,
+	MSB_WB_RECEIVE_INT_REQ,
+
+	MSB_WB_SEND_WRITE_DATA,
+	MSB_WB_RECEIVE_WRITE_CONFIRMATION,
+};
+
+enum msb_send_command_states {
+	MSB_SC_SEND_WRITE_PARAMS,
+	MSB_SC_SEND_WRITE_OOB,
+	MSB_SC_SEND_COMMAND,
+
+	MSB_SC_SEND_INT_REQ,
+	MSB_SC_RECEIVE_INT_REQ,
+
+};
+
+enum msb_reset_states {
+	MSB_RS_SEND,
+	MSB_RS_CONFIRM,
+};
+
+enum msb_par_switch_states {
+	MSB_PS_SEND_SWITCH_COMMAND,
+	MSB_PS_SWICH_HOST,
+	MSB_PS_CONFIRM,
+};
+
+struct chs_entry {
+	unsigned long size;
+	unsigned char sec;
+	unsigned short cyl;
+	unsigned char head;
+};
+
+static int msb_reset(struct msb_data *msb, bool full);
+
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq);
+
+#define __dbg(level, format, ...) \
+	do { \
+		if (debug >= level) \
+			pr_err(format "\n", ## __VA_ARGS__); \
+	} while (0)
+
+
+#define dbg(format, ...)		__dbg(1, format, ## __VA_ARGS__)
+#define dbg_verbose(format, ...)	__dbg(2, format, ## __VA_ARGS__)
+
+#endif
-- 
1.7.9.5

Re: [merged] memstick-add-support-for-legacy-memorysticks.patch removed from -mm tree

[Valdis.Kletnieks]

[-- Attachment #1: Type: text/plain, Size: 392 bytes --]

On Mon, 08 Jul 2013 03:41:42 +0300, Maxim Levitsky said:

> +/* Debug test to validate free block counts */
> +#ifdef DEBUG
> +static int msb_validate_used_block_bitmap(struct msb_data *msb)
> +{

Is there a reason this should be inside a #ifdef DEBUG with no
easy way to select DEBUG?  Maybe CONFIG_MS_BLOCK_DEBUG similar to
the way other subsystems allow configuring additional debug code?

[-- Attachment #2: Type: application/pgp-signature, Size: 865 bytes --]

Re: [merged] memstick-add-support-for-legacy-memorysticks.patch removed from -mm tree

[Maxim Levitsky]

[-- Attachment #1: Type: text/plain, Size: 825 bytes --]

On Sun, 2013-07-07 at 22:11 -0400, Valdis.Kletnieks@vt.edu wrote: 
> On Mon, 08 Jul 2013 03:41:42 +0300, Maxim Levitsky said:
> 
> > +/* Debug test to validate free block counts */
> > +#ifdef DEBUG
> > +static int msb_validate_used_block_bitmap(struct msb_data *msb)
> > +{
> 
> Is there a reason this should be inside a #ifdef DEBUG with no
> easy way to select DEBUG?  Maybe CONFIG_MS_BLOCK_DEBUG similar to
> the way other subsystems allow configuring additional debug code?

Unless there is a bug in the driver, the code shouldn't really be
needed.
I have a module param 'debug' and I don't mind hooking this code to it.

But I don't mind hooking this code to 'debug' module param I have.
I updated this, and fixed deprecated usage of idr_pre_get.

Updated version attached.


-- 
Best regards,
        Maxim Levitsky



[-- Attachment #2: Type: text/x-patch, Size: 71509 bytes --]

>From 9974616d623266281aba5292c7b6d81710a435f6 Mon Sep 17 00:00:00 2001
From: Maxim Levitsky <maximlevitsky@gmail.com>
Date: Fri, 5 Apr 2013 16:37:31 +0300
Subject: [PATCH] memstick: add support for legacy memorysticks

Based partially on MS standard spec quotes from Alex Dubov.

As any code that works with user data this driver isn't
recommended to use to write cards that contain valuable data.

It tries its best though to avoid data corruption
and possible damage to the card.

Tested on MS DUO 64 MB card on Ricoh R592 card reader.

Signed-off-by: Maxim Levitsky <maximlevitsky@gmail.com>
---
 MAINTAINERS                      |    5 +
 drivers/memstick/core/Kconfig    |   12 +
 drivers/memstick/core/Makefile   |    2 +-
 drivers/memstick/core/ms_block.c | 2390 ++++++++++++++++++++++++++++++++++++++
 drivers/memstick/core/ms_block.h |  290 +++++
 5 files changed, 2698 insertions(+), 1 deletion(-)
 create mode 100644 drivers/memstick/core/ms_block.c
 create mode 100644 drivers/memstick/core/ms_block.h

diff --git a/MAINTAINERS b/MAINTAINERS
index 836a618..5479d2a 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -7359,6 +7359,11 @@ W:	http://tifmxx.berlios.de/
 S:	Maintained
 F:	drivers/memstick/host/tifm_ms.c
 
+SONY MEMORYSTICK STANDARD SUPPORT
+M:	Maxim Levitsky <maximlevitsky@gmail.com>
+S:	Maintained
+F:	drivers/memstick/core/ms_block.*
+
 SOUND
 M:	Jaroslav Kysela <perex@perex.cz>
 M:	Takashi Iwai <tiwai@suse.de>
diff --git a/drivers/memstick/core/Kconfig b/drivers/memstick/core/Kconfig
index 95f1814..1d38949 100644
--- a/drivers/memstick/core/Kconfig
+++ b/drivers/memstick/core/Kconfig
@@ -24,3 +24,15 @@ config MSPRO_BLOCK
 	  support. This provides a block device driver, which you can use
 	  to mount the filesystem. Almost everyone wishing MemoryStick
 	  support should say Y or M here.
+
+config MS_BLOCK
+	tristate "MemoryStick Standard device driver"
+	depends on BLOCK
+	help
+	  Say Y here to enable the MemoryStick Standard device driver
+	  support. This provides a block device driver, which you can use
+	  to mount the filesystem.
+	  This driver works with old (bulky) MemoryStick and MemoryStick Duo
+	  but not PRO. Say Y if you have such card.
+	  Driver is new and not yet well tested, thus it can damage your card
+	  (even permanently)
diff --git a/drivers/memstick/core/Makefile b/drivers/memstick/core/Makefile
index ecd0299..0d7f90c 100644
--- a/drivers/memstick/core/Makefile
+++ b/drivers/memstick/core/Makefile
@@ -3,5 +3,5 @@
 #
 
 obj-$(CONFIG_MEMSTICK)		+= memstick.o
-
+obj-$(CONFIG_MS_BLOCK)		+= ms_block.o
 obj-$(CONFIG_MSPRO_BLOCK)	+= mspro_block.o
diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
new file mode 100644
index 0000000..5de7bb2
--- /dev/null
+++ b/drivers/memstick/core/ms_block.c
@@ -0,0 +1,2390 @@
+/*
+ *  ms_block.c - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2012 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver were copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ */
+#define DRIVER_NAME "ms_block"
+#define pr_fmt(fmt) DRIVER_NAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/memstick.h>
+#include <linux/idr.h>
+#include <linux/hdreg.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/bitmap.h>
+#include <linux/scatterlist.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include "ms_block.h"
+
+static int debug;
+static int cache_flush_timeout = 1000;
+static bool verify_writes;
+
+/*
+ * Copies section of 'sg_from' starting from offset 'offset' and with length
+ * 'len' To another scatterlist of to_nents enties
+ */
+static size_t msb_sg_copy(struct scatterlist *sg_from, struct scatterlist *sg_to,
+					int to_nents, size_t offset, size_t len)
+{
+	size_t copied = 0;
+
+	while (offset > 0) {
+		if (offset >= sg_from->length) {
+			if (sg_is_last(sg_from))
+				return 0;
+
+			offset -= sg_from->length;
+			sg_from = sg_next(sg_from);
+			continue;
+		}
+
+		copied = min(len, sg_from->length - offset);
+		sg_set_page(sg_to, sg_page(sg_from),
+			copied, sg_from->offset + offset);
+
+		len -= copied;
+		offset = 0;
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_to = sg_next(sg_to);
+		to_nents--;
+		sg_from = sg_next(sg_from);
+	}
+
+	while (len > sg_from->length && to_nents--) {
+		len -= sg_from->length;
+		copied += sg_from->length;
+
+		sg_set_page(sg_to, sg_page(sg_from),
+				sg_from->length, sg_from->offset);
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_from = sg_next(sg_from);
+		sg_to = sg_next(sg_to);
+	}
+
+	if (len && to_nents) {
+		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
+		copied += len;
+	}
+out:
+	sg_mark_end(sg_to);
+	return copied;
+}
+
+/*
+ * Compares section of 'sg' starting from offset 'offset' and with length 'len'
+ * to linear buffer of length 'len' at address 'buffer'
+ * Returns 0 if equal and  -1 otherwice
+ */
+static int msb_sg_compare_to_buffer(struct scatterlist *sg,
+					size_t offset, u8 *buffer, size_t len)
+{
+	int retval = 0, cmplen;
+	struct sg_mapping_iter miter;
+
+	sg_miter_start(&miter, sg, sg_nents(sg),
+					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
+
+	while (sg_miter_next(&miter) && len > 0) {
+		if (offset >= miter.length) {
+			offset -= miter.length;
+			continue;
+		}
+
+		cmplen = min(miter.length - offset, len);
+		retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
+		if (retval)
+			break;
+
+		buffer += cmplen;
+		len -= cmplen;
+		offset = 0;
+	}
+
+	if (!retval && len)
+		retval = -1;
+
+	sg_miter_stop(&miter);
+	return retval;
+}
+
+
+/* Get zone at which block with logical address 'lba' lives
+ * Flash is broken into zones.
+ * Each zone consists of 512 eraseblocks, out of which in first
+ * zone 494 are used and 496 are for all following zones.
+ * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
+*/
+static int msb_get_zone_from_lba(int lba)
+{
+	if (lba < 494)
+		return 0;
+	return ((lba - 494) / 496) + 1;
+}
+
+/* Get zone of physical block. Trivial */
+static int msb_get_zone_from_pba(int pba)
+{
+	return pba / MS_BLOCKS_IN_ZONE;
+}
+
+/* Debug test to validate free block counts */
+static int msb_validate_used_block_bitmap(struct msb_data *msb)
+{
+	int total_free_blocks = 0;
+	int i;
+
+	if (!debug)
+		return 0;
+
+	for (i = 0 ; i < msb->zone_count ; i++)
+		total_free_blocks += msb->free_block_count[i];
+
+	if (msb->block_count - bitmap_weight(msb->used_blocks_bitmap,
+					msb->block_count) == total_free_blocks)
+		return 0;
+
+	pr_err("BUG: free block counts don't match the bitmap");
+	msb->read_only = true;
+	return -EINVAL;
+}
+
+/* Mark physical block as used */
+static void msb_mark_block_used(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (test_bit(pba, msb->used_blocks_bitmap)) {
+		pr_err(
+		"BUG: attempt to mark already used pba %d as used", pba);
+		msb->read_only = true;
+		return;
+	}
+
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+
+	/* No races because all IO is single threaded */
+	__set_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]--;
+}
+
+/* Mark physical block as free */
+static void msb_mark_block_unused(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (!test_bit(pba, msb->used_blocks_bitmap)) {
+		pr_err("BUG: attempt to mark "
+				"already unused pba %d as unused" , pba);
+		msb->read_only = true;
+		return;
+	}
+
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+
+	/* No races because all IO is single threaded */
+	__clear_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]++;
+}
+
+/* Invalidate current register window */
+static void msb_invalidate_reg_window(struct msb_data *msb)
+{
+	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.w_length = sizeof(struct ms_id_register);
+	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.r_length = sizeof(struct ms_id_register);
+	msb->addr_valid = false;
+}
+
+/* Start a state machine */
+static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
+		(struct memstick_dev *card, struct memstick_request **req))
+{
+	struct memstick_dev *card = msb->card;
+
+	WARN_ON(msb->state != -1);
+	msb->int_polling = false;
+	msb->state = 0;
+	msb->exit_error = 0;
+
+	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
+
+	card->next_request = state_func;
+	memstick_new_req(card->host);
+	wait_for_completion(&card->mrq_complete);
+
+	WARN_ON(msb->state != -1);
+	return msb->exit_error;
+}
+
+/* State machines call that to exit */
+static int msb_exit_state_machine(struct msb_data *msb, int error)
+{
+	WARN_ON(msb->state == -1);
+
+	msb->state = -1;
+	msb->exit_error = error;
+	msb->card->next_request = h_msb_default_bad;
+
+	/* Invalidate reg window on errors */
+	if (error)
+		msb_invalidate_reg_window(msb);
+
+	complete(&msb->card->mrq_complete);
+	return -ENXIO;
+}
+
+/* read INT register */
+static int msb_read_int_reg(struct msb_data *msb, long timeout)
+{
+	struct memstick_request *mrq = &msb->card->current_mrq;
+
+	WARN_ON(msb->state == -1);
+
+	if (!msb->int_polling) {
+		msb->int_timeout = jiffies +
+			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
+		msb->int_polling = true;
+	} else if (time_after(jiffies, msb->int_timeout)) {
+		mrq->data[0] = MEMSTICK_INT_CMDNAK;
+		return 0;
+	}
+
+	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
+				mrq->need_card_int && !mrq->error) {
+		mrq->data[0] = mrq->int_reg;
+		mrq->need_card_int = false;
+		return 0;
+	} else {
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		return 1;
+	}
+}
+
+/* Read a register */
+static int msb_read_regs(struct msb_data *msb, int offset, int len)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.r_offset != offset ||
+	    msb->reg_addr.r_length != len || !msb->addr_valid) {
+
+		msb->reg_addr.r_offset = offset;
+		msb->reg_addr.r_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
+	return 1;
+}
+
+/* Write a card register */
+static int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.w_offset != offset ||
+		msb->reg_addr.w_length != len  || !msb->addr_valid) {
+
+		msb->reg_addr.w_offset = offset;
+		msb->reg_addr.w_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
+	return 1;
+}
+
+/* Handler for absence of IO */
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq)
+{
+	return -ENXIO;
+}
+
+/*
+ * This function is a handler for reads of one page from device.
+ * Writes output to msb->current_sg, takes sector address from msb->reg.param
+ * Can also be used to read extra data only. Set params accordintly.
+ */
+static int h_msb_read_page(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 command, intreg;
+
+	if (mrq->error) {
+		dbg("read_page, unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+	case MSB_RP_SEND_BLOCK_ADDRESS:
+		/* msb_write_regs sometimes "fails" because it needs to update
+			the reg window, and thus it returns request for that.
+			Then we stay in this state and retry */
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			(unsigned char *)&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_RP_SEND_READ_COMMAND;
+		return 0;
+
+	case MSB_RP_SEND_READ_COMMAND:
+		command = MS_CMD_BLOCK_READ;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		msb->state = MSB_RP_SEND_INT_REQ;
+		return 0;
+
+	case MSB_RP_SEND_INT_REQ:
+		msb->state = MSB_RP_RECEIVE_INT_REQ_RESULT;
+		/* If dont actually need to send the int read request (only in
+			serial mode), then just fall through */
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_RP_RECEIVE_INT_REQ_RESULT:
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state = MSB_RP_SEND_INT_REQ;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state = (intreg & MEMSTICK_INT_ERR) ?
+			MSB_RP_SEND_READ_STATUS_REG : MSB_RP_SEND_OOB_READ;
+		goto again;
+
+	case MSB_RP_SEND_READ_STATUS_REG:
+		 /* read the status register to understand source of the INT_ERR */
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, status),
+			sizeof(struct ms_status_register)))
+			return 0;
+
+		msb->state = MSB_RP_RECEIVE_OOB_READ;
+		return 0;
+
+	case MSB_RP_RECIVE_STATUS_REG:
+		msb->regs.status = *(struct ms_status_register *)mrq->data;
+		msb->state = MSB_RP_SEND_OOB_READ;
+		/* fallthrough */
+
+	case MSB_RP_SEND_OOB_READ:
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register)))
+			return 0;
+
+		msb->state = MSB_RP_RECEIVE_OOB_READ;
+		return 0;
+
+	case MSB_RP_RECEIVE_OOB_READ:
+		msb->regs.extra_data =
+			*(struct ms_extra_data_register *) mrq->data;
+		msb->state = MSB_RP_SEND_READ_DATA;
+		/* fallthrough */
+
+	case MSB_RP_SEND_READ_DATA:
+		/* Skip that state if we only read the oob */
+		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
+			msb->state = MSB_RP_RECEIVE_READ_DATA;
+			goto again;
+		}
+
+		sg_init_table(sg, ARRAY_SIZE(sg));
+		msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size);
+
+		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
+		msb->state = MSB_RP_RECEIVE_READ_DATA;
+		return 0;
+
+	case MSB_RP_RECEIVE_READ_DATA:
+		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, 0);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
+			dbg("read_page: uncorrectable error");
+			return msb_exit_state_machine(msb, -EBADMSG);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
+			dbg("read_page: correctable error");
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, -EUCLEAN);
+		} else {
+			dbg("read_page: INT error, but no status error bits");
+			return msb_exit_state_machine(msb, -EIO);
+		}
+	}
+
+	BUG();
+}
+
+/*
+ * Handler of writes of exactly one block.
+ * Takes address from msb->regs.param.
+ * Writes same extra data to blocks, also taken
+ * from msb->regs.extra
+ * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
+ * device refuses to take the command or something else
+ */
+static int h_msb_write_block(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 intreg, command;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+again:
+	switch (msb->state) {
+
+	/* HACK: Jmicon handling of TPCs between 8 and
+	 *	sizeof(memstick_request.data) is broken due to hardware
+	 *	bug in PIO mode that is used for these TPCs
+	 *	Therefore split the write
+	 */
+
+	case MSB_WB_SEND_WRITE_PARAMS:
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_WB_SEND_WRITE_OOB;
+		return 0;
+
+	case MSB_WB_SEND_WRITE_OOB:
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+		msb->state = MSB_WB_SEND_WRITE_COMMAND;
+		return 0;
+
+
+	case MSB_WB_SEND_WRITE_COMMAND:
+		command = MS_CMD_BLOCK_WRITE;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		msb->state = MSB_WB_SEND_INT_REQ;
+		return 0;
+
+	case MSB_WB_SEND_INT_REQ:
+		msb->state = MSB_WB_RECEIVE_INT_REQ;
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_WB_RECEIVE_INT_REQ:
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		/* errors mean out of here, and fast... */
+		if (intreg & (MEMSTICK_INT_CMDNAK))
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+
+		/* for last page we need to poll CED */
+		if (msb->current_page == msb->pages_in_block) {
+			if (intreg & MEMSTICK_INT_CED)
+				return msb_exit_state_machine(msb, 0);
+			msb->state = MSB_WB_SEND_INT_REQ;
+			goto again;
+
+		}
+
+		/* for non-last page we need BREQ before writing next chunk */
+		if (!(intreg & MEMSTICK_INT_BREQ)) {
+			msb->state = MSB_WB_SEND_INT_REQ;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state = MSB_WB_SEND_WRITE_DATA;
+		/* fallthrough */
+
+	case MSB_WB_SEND_WRITE_DATA:
+		sg_init_table(sg, ARRAY_SIZE(sg));
+
+		if (msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size) < msb->page_size)
+			return msb_exit_state_machine(msb, -EIO);
+
+		memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
+		mrq->need_card_int = 1;
+		msb->state = MSB_WB_RECEIVE_WRITE_CONFIRMATION;
+		return 0;
+
+	case MSB_WB_RECEIVE_WRITE_CONFIRMATION:
+		msb->current_page++;
+		msb->current_sg_offset += msb->page_size;
+		msb->state = MSB_WB_SEND_INT_REQ;
+		goto again;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
+/*
+ * This function is used to send simple IO requests to device that consist
+ * of register write + command
+ */
+static int h_msb_send_command(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	u8 intreg;
+
+	if (mrq->error) {
+		dbg("send_command: unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+
+	/* HACK: see h_msb_write_block */
+	case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+		msb->state = MSB_SC_SEND_WRITE_OOB;
+		return 0;
+
+	case MSB_SC_SEND_WRITE_OOB:
+		if (!msb->command_need_oob) {
+			msb->state = MSB_SC_SEND_COMMAND;
+			goto again;
+		}
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+
+		msb->state = MSB_SC_SEND_COMMAND;
+		return 0;
+
+	case MSB_SC_SEND_COMMAND:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
+		msb->state = MSB_SC_SEND_INT_REQ;
+		return 0;
+
+	case MSB_SC_SEND_INT_REQ:
+		msb->state = MSB_SC_RECEIVE_INT_REQ;
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_SC_RECEIVE_INT_REQ:
+		intreg = mrq->data[0];
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state = MSB_SC_SEND_INT_REQ;
+			goto again;
+		}
+
+		return msb_exit_state_machine(msb, 0);
+	}
+
+	BUG();
+}
+
+/* Small handler for card reset */
+static int h_msb_reset(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	u8 command = MS_CMD_RESET;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+	switch (msb->state) {
+	case MSB_RS_SEND:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		mrq->need_card_int = 0;
+		msb->state = MSB_RS_CONFIRM;
+		return 0;
+	case MSB_RS_CONFIRM:
+		return msb_exit_state_machine(msb, 0);
+	}
+	BUG();
+}
+
+/* This handler is used to do serial->parallel switch */
+static int h_msb_parallel_switch(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct memstick_host *host = card->host;
+
+	if (mrq->error) {
+		dbg("parallel_switch: error");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+
+	switch (msb->state) {
+	case MSB_PS_SEND_SWITCH_COMMAND:
+		/* Set the parallel interface on memstick side */
+		msb->regs.param.system |= MEMSTICK_SYS_PAM;
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			1,
+			(unsigned char *)&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_PS_SWICH_HOST;
+		return 0;
+
+	case MSB_PS_SWICH_HOST:
+		 /* Set parallel interface on our side + send a dummy request
+			to see if card responds */
+		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		msb->state = MSB_PS_CONFIRM;
+		return 0;
+
+	case MSB_PS_CONFIRM:
+		return msb_exit_state_machine(msb, 0);
+	}
+
+	BUG();
+}
+
+static int msb_switch_to_parallel(struct msb_data *msb);
+
+/* Reset the card, to guard against hw errors beeing treated as bad blocks */
+static int msb_reset(struct msb_data *msb, bool full)
+{
+
+	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
+	struct memstick_dev *card = msb->card;
+	struct memstick_host *host = card->host;
+	int error;
+
+	/* Reset the card */
+	msb->regs.param.system = MEMSTICK_SYS_BAMD;
+
+	if (full) {
+		error =  host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+		if (error)
+			goto out_error;
+
+		msb_invalidate_reg_window(msb);
+
+		error = host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_ON);
+		if (error)
+			goto out_error;
+
+		error = host->set_param(host,
+					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+		if (error) {
+out_error:
+			dbg("Failed to reset the host controller");
+			msb->read_only = true;
+			return -EFAULT;
+		}
+	}
+
+	error = msb_run_state_machine(msb, h_msb_reset);
+	if (error) {
+		dbg("Failed to reset the card");
+		msb->read_only = true;
+		return -ENODEV;
+	}
+
+	/* Set parallel mode */
+	if (was_parallel)
+		msb_switch_to_parallel(msb);
+	return 0;
+}
+
+/* Attempts to switch interface to parallel mode */
+static int msb_switch_to_parallel(struct msb_data *msb)
+{
+	int error;
+
+	error = msb_run_state_machine(msb, h_msb_parallel_switch);
+	if (error) {
+		pr_err("Switch to parallel failed");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		msb_reset(msb, true);
+		return -EFAULT;
+	}
+
+	msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
+	return 0;
+}
+
+/* Changes overwrite flag on a page */
+static int msb_set_overwrite_flag(struct msb_data *msb,
+						u16 pba, u8 page, u8 flag)
+{
+	if (msb->read_only)
+		return -EROFS;
+
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_OVERWRITE;
+	msb->regs.extra_data.overwrite_flag = flag;
+	msb->command_value = MS_CMD_BLOCK_WRITE;
+	msb->command_need_oob = true;
+
+	dbg_verbose("changing overwrite flag to %02x for sector %d, page %d",
+							flag, pba, page);
+	return msb_run_state_machine(msb, h_msb_send_command);
+}
+
+static int msb_mark_bad(struct msb_data *msb, int pba)
+{
+	pr_notice("marking pba %d as bad", pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(
+			msb, pba, 0, 0xFF & ~MEMSTICK_OVERWRITE_BKST);
+}
+
+static int msb_mark_page_bad(struct msb_data *msb, int pba, int page)
+{
+	dbg("marking page %d of pba %d as bad", page, pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(msb,
+		pba, page, ~MEMSTICK_OVERWRITE_PGST0);
+}
+
+/* Erases one physical block */
+static int msb_erase_block(struct msb_data *msb, u16 pba)
+{
+	int error, try;
+	if (msb->read_only)
+		return -EROFS;
+
+	dbg_verbose("erasing pba %d", pba);
+
+	for (try = 1 ; try < 3 ; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = 0;
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->command_value = MS_CMD_BLOCK_ERASE;
+		msb->command_need_oob = false;
+
+
+		error = msb_run_state_machine(msb, h_msb_send_command);
+		if (!error || msb_reset(msb, true))
+			break;
+	}
+
+	if (error) {
+		pr_err("erase failed, marking pba %d as bad", pba);
+		msb_mark_bad(msb, pba);
+	}
+
+	dbg_verbose("erase success, marking pba %d as unused", pba);
+	msb_mark_block_unused(msb, pba);
+	__set_bit(pba, msb->erased_blocks_bitmap);
+	return error;
+}
+
+/* Reads one page from device */
+static int msb_read_page(struct msb_data *msb,
+	u16 pba, u8 page, struct ms_extra_data_register *extra,
+					struct scatterlist *sg,  int offset)
+{
+	int try, error;
+
+	if (pba == MS_BLOCK_INVALID) {
+		unsigned long flags;
+		struct sg_mapping_iter miter;
+		size_t len = msb->page_size;
+
+		dbg_verbose("read unmapped sector. returning 0xFF");
+
+		local_irq_save(flags);
+		sg_miter_start(&miter, sg, sg_nents(sg),
+				SG_MITER_ATOMIC | SG_MITER_TO_SG);
+
+		while (sg_miter_next(&miter) && len > 0) {
+
+			int chunklen;
+
+			if (offset && offset >= miter.length) {
+				offset -= miter.length;
+				continue;
+			}
+
+			chunklen = min(miter.length - offset, len);
+			memset(miter.addr + offset, 0xFF, chunklen);
+			len -= chunklen;
+			offset = 0;
+		}
+
+		sg_miter_stop(&miter);
+		local_irq_restore(flags);
+
+		if (offset)
+			return -EFAULT;
+
+		if (extra)
+			memset(extra, 0xFF, sizeof(*extra));
+		return 0;
+	}
+
+	if (pba >= msb->block_count) {
+		pr_err("BUG: attempt to read beyond"
+					" the end of the card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	for (try = 1 ; try < 3 ; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = page;
+		msb->regs.param.cp = MEMSTICK_CP_PAGE;
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		error = msb_run_state_machine(msb, h_msb_read_page);
+
+
+		if (error == -EUCLEAN) {
+			pr_notice("correctable error on pba %d, page %d",
+				pba, page);
+			error = 0;
+		}
+
+		if (!error && extra)
+			*extra = msb->regs.extra_data;
+
+		if (!error || msb_reset(msb, true))
+			break;
+
+	}
+
+	/* Mark bad pages */
+	if (error == -EBADMSG) {
+		pr_err("uncorrectable error on read of pba %d, page %d",
+			pba, page);
+
+		if (msb->regs.extra_data.overwrite_flag &
+					MEMSTICK_OVERWRITE_PGST0)
+			msb_mark_page_bad(msb, pba, page);
+		return -EBADMSG;
+	}
+
+	if (error)
+		pr_err("read of pba %d, page %d failed with error %d",
+			pba, page, error);
+	return error;
+}
+
+/* Reads oob of page only */
+static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
+	struct ms_extra_data_register *extra)
+{
+	int error;
+
+	BUG_ON(!extra);
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
+
+	if (pba > msb->block_count) {
+		pr_err("BUG: attempt to read beyond"
+					" the end of card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	error = msb_run_state_machine(msb, h_msb_read_page);
+	*extra = msb->regs.extra_data;
+
+	if (error == -EUCLEAN) {
+		pr_notice("correctable error on pba %d, page %d",
+			pba, page);
+		return 0;
+	}
+
+	return error;
+}
+
+
+/* Reads a block and compares it with data contained in scatterlist orig_sg */
+static int msb_verify_block(struct msb_data *msb, u16 pba,
+				struct scatterlist *orig_sg,  int offset)
+{
+	struct scatterlist sg;
+	int page = 0, error;
+
+	sg_init_one(&sg, msb->block_buffer, msb->block_size);
+
+	while (page < msb->pages_in_block) {
+
+		error = msb_read_page(msb, pba, page,
+				NULL, &sg, page * msb->page_size);
+		if (error)
+			return error;
+		page++;
+	}
+
+	if (msb_sg_compare_to_buffer(orig_sg, offset,
+				msb->block_buffer, msb->block_size))
+		return -EIO;
+	return 0;
+}
+
+/* Writes exectly one block + oob */
+static int msb_write_block(struct msb_data *msb,
+			u16 pba, u32 lba, struct scatterlist *sg, int offset)
+{
+	int error, current_try = 1;
+	BUG_ON(sg->length < msb->page_size);
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (pba == MS_BLOCK_INVALID) {
+		pr_err(
+			"BUG: write: attempt to write MS_BLOCK_INVALID block");
+		return -EINVAL;
+	}
+
+	if (pba >= msb->block_count || lba >= msb->logical_block_count) {
+		pr_err(
+		"BUG: write: attempt to write beyond the end of device");
+		return -EINVAL;
+	}
+
+	if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+		pr_err("BUG: write: lba zone mismatch");
+		return -EINVAL;
+	}
+
+	if (pba == msb->boot_block_locations[0] ||
+		pba == msb->boot_block_locations[1]) {
+		pr_err("BUG: write: attempt to write to boot blocks!");
+		return -EINVAL;
+	}
+
+	while (1) {
+
+		if (msb->read_only)
+			return -EROFS;
+
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->regs.param.page_address = 0;
+		msb->regs.param.block_address = cpu_to_be16(pba);
+
+		msb->regs.extra_data.management_flag = 0xFF;
+		msb->regs.extra_data.overwrite_flag = 0xF8;
+		msb->regs.extra_data.logical_address = cpu_to_be16(lba);
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		msb->current_page = 0;
+
+		error = msb_run_state_machine(msb, h_msb_write_block);
+
+		/* Sector we just wrote to is assumed erased since its pba
+			was erased. If it wasn't erased, write will succeed
+			and will just clear the bits that were set in the block
+			thus test that what we have written,
+			matches what we expect.
+			We do trust the blocks that we erased */
+		if (!error && (verify_writes ||
+				!test_bit(pba, msb->erased_blocks_bitmap)))
+			error = msb_verify_block(msb, pba, sg, offset);
+
+		if (!error)
+			break;
+
+		if (current_try > 1 || msb_reset(msb, true))
+			break;
+
+		pr_err("write failed, trying to erase the pba %d", pba);
+		error = msb_erase_block(msb, pba);
+		if (error)
+			break;
+
+		current_try++;
+	}
+	return error;
+}
+
+/* Finds a free block for write replacement */
+static u16 msb_get_free_block(struct msb_data *msb, int zone)
+{
+	u16 pos;
+	int pba = zone * MS_BLOCKS_IN_ZONE;
+	int i;
+
+	get_random_bytes(&pos, sizeof(pos));
+
+	if (!msb->free_block_count[zone]) {
+		pr_err("NO free blocks in the zone %d, to use for a write, "
+			"(media is WORN out) switching to RO mode", zone);
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	pos %= msb->free_block_count[zone];
+
+	dbg_verbose("have %d choices for a free block, selected randomally: %d",
+		msb->free_block_count[zone], pos);
+
+	pba = find_next_zero_bit(msb->used_blocks_bitmap,
+							msb->block_count, pba);
+	for (i = 0 ; i < pos ; ++i)
+		pba = find_next_zero_bit(msb->used_blocks_bitmap,
+						msb->block_count, pba + 1);
+
+	dbg_verbose("result of the free blocks scan: pba %d", pba);
+
+	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
+		pr_err("BUG: cant get a free block");
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	msb_mark_block_used(msb, pba);
+	return pba;
+}
+
+static int msb_update_block(struct msb_data *msb, u16 lba,
+	struct scatterlist *sg, int offset)
+{
+	u16 pba, new_pba;
+	int error, try;
+
+	pba = msb->lba_to_pba_table[lba];
+	dbg_verbose("start of a block update at lba  %d, pba %d", lba, pba);
+
+	if (pba != MS_BLOCK_INVALID) {
+		dbg_verbose("setting the update flag on the block");
+		msb_set_overwrite_flag(msb, pba, 0,
+				0xFF & ~MEMSTICK_OVERWRITE_UDST);
+	}
+
+	for (try = 0 ; try < 3 ; try++) {
+		new_pba = msb_get_free_block(msb,
+			msb_get_zone_from_lba(lba));
+
+		if (new_pba == MS_BLOCK_INVALID) {
+			error = -EIO;
+			goto out;
+		}
+
+		dbg_verbose("block update: writing updated block to the pba %d",
+								new_pba);
+		error = msb_write_block(msb, new_pba, lba, sg, offset);
+		if (error == -EBADMSG) {
+			msb_mark_bad(msb, new_pba);
+			continue;
+		}
+
+		if (error)
+			goto out;
+
+		dbg_verbose("block update: erasing the old block");
+		msb_erase_block(msb, pba);
+		msb->lba_to_pba_table[lba] = new_pba;
+		return 0;
+	}
+out:
+	if (error) {
+		pr_err("block update error after %d tries, "
+						"switching to r/o mode", try);
+		msb->read_only = true;
+	}
+	return error;
+}
+
+/* Converts endiannes in the boot block for easy use */
+static void msb_fix_boot_page_endianness(struct ms_boot_page *p)
+{
+	p->header.block_id = be16_to_cpu(p->header.block_id);
+	p->header.format_reserved = be16_to_cpu(p->header.format_reserved);
+	p->entry.disabled_block.start_addr
+		= be32_to_cpu(p->entry.disabled_block.start_addr);
+	p->entry.disabled_block.data_size
+		= be32_to_cpu(p->entry.disabled_block.data_size);
+	p->entry.cis_idi.start_addr
+		= be32_to_cpu(p->entry.cis_idi.start_addr);
+	p->entry.cis_idi.data_size
+		= be32_to_cpu(p->entry.cis_idi.data_size);
+	p->attr.block_size = be16_to_cpu(p->attr.block_size);
+	p->attr.number_of_blocks = be16_to_cpu(p->attr.number_of_blocks);
+	p->attr.number_of_effective_blocks
+		= be16_to_cpu(p->attr.number_of_effective_blocks);
+	p->attr.page_size = be16_to_cpu(p->attr.page_size);
+	p->attr.memory_manufacturer_code
+		= be16_to_cpu(p->attr.memory_manufacturer_code);
+	p->attr.memory_device_code = be16_to_cpu(p->attr.memory_device_code);
+	p->attr.implemented_capacity
+		= be16_to_cpu(p->attr.implemented_capacity);
+	p->attr.controller_number = be16_to_cpu(p->attr.controller_number);
+	p->attr.controller_function = be16_to_cpu(p->attr.controller_function);
+}
+
+static int msb_read_boot_blocks(struct msb_data *msb)
+{
+	int pba = 0;
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	struct ms_boot_page *page;
+
+	msb->boot_block_locations[0] = MS_BLOCK_INVALID;
+	msb->boot_block_locations[1] = MS_BLOCK_INVALID;
+	msb->boot_block_count = 0;
+
+	dbg_verbose("Start of a scan for the boot blocks");
+
+	if (!msb->boot_page) {
+		page = kmalloc(sizeof(struct ms_boot_page)*2, GFP_KERNEL);
+		if (!page)
+			return -ENOMEM;
+
+		msb->boot_page = page;
+	} else
+		page = msb->boot_page;
+
+	msb->block_count = MS_BLOCK_MAX_BOOT_ADDR;
+
+	for (pba = 0 ; pba < MS_BLOCK_MAX_BOOT_ADDR ; pba++) {
+
+		sg_init_one(&sg, page, sizeof(*page));
+		if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) {
+			dbg("boot scan: can't read pba %d", pba);
+			continue;
+		}
+
+		if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) {
+			dbg("managment flag doesn't indicate boot block %d",
+									pba);
+			continue;
+		}
+
+		if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) {
+			dbg("the pba at %d doesn' contain boot block ID", pba);
+			continue;
+		}
+
+		msb_fix_boot_page_endianness(page);
+		msb->boot_block_locations[msb->boot_block_count] = pba;
+
+		page++;
+		msb->boot_block_count++;
+
+		if (msb->boot_block_count == 2)
+			break;
+	}
+
+	if (!msb->boot_block_count) {
+		pr_err("media doesn't contain master page, aborting");
+		return -EIO;
+	}
+
+	dbg_verbose("End of scan for boot blocks");
+	return 0;
+}
+
+static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
+{
+	struct ms_boot_page *boot_block;
+	struct scatterlist sg;
+	u16 *buffer = NULL;
+	int offset = 0;
+	int i, error = 0;
+	int data_size, data_offset, page, page_offset, size_to_read;
+	u16 pba;
+
+	BUG_ON(block_nr > 1);
+	boot_block = &msb->boot_page[block_nr];
+	pba = msb->boot_block_locations[block_nr];
+
+	if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID)
+		return -EINVAL;
+
+	data_size = boot_block->entry.disabled_block.data_size;
+	data_offset = sizeof(struct ms_boot_page) +
+			boot_block->entry.disabled_block.start_addr;
+	if (!data_size)
+		return 0;
+
+	page = data_offset / msb->page_size;
+	page_offset = data_offset % msb->page_size;
+	size_to_read =
+		DIV_ROUND_UP(data_size + page_offset, msb->page_size) *
+			msb->page_size;
+
+	dbg("reading bad block of boot block at pba %d, offset %d len %d",
+		pba, data_offset, data_size);
+
+	buffer = kzalloc(size_to_read, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	/* Read the buffer */
+	sg_init_one(&sg, buffer, size_to_read);
+
+	while (offset < size_to_read) {
+		error = msb_read_page(msb, pba, page, NULL, &sg, offset);
+		if (error)
+			goto out;
+
+		page++;
+		offset += msb->page_size;
+
+		if (page == msb->pages_in_block) {
+			pr_err(
+			"bad block table extends beyond the boot block");
+			break;
+		}
+	}
+
+	/* Process the bad block table */
+	for (i = page_offset ; i < data_size / sizeof(u16) ; i++) {
+
+		u16 bad_block = be16_to_cpu(buffer[i]);
+
+		if (bad_block >= msb->block_count) {
+			dbg("bad block table contains invalid block %d",
+								bad_block);
+			continue;
+		}
+
+		if (test_bit(bad_block, msb->used_blocks_bitmap))  {
+			dbg("duplicate bad block %d in the table",
+				bad_block);
+			continue;
+		}
+
+		dbg("block %d is marked as factory bad", bad_block);
+		msb_mark_block_used(msb, bad_block);
+	}
+out:
+	kfree(buffer);
+	return error;
+}
+
+static int msb_ftl_initialize(struct msb_data *msb)
+{
+	int i;
+
+	if (msb->ftl_initialized)
+		return 0;
+
+	msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE;
+	msb->logical_block_count = msb->zone_count * 496 - 2;
+
+	msb->used_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->erased_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->lba_to_pba_table =
+		kmalloc(msb->logical_block_count * sizeof(u16), GFP_KERNEL);
+
+	if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
+						!msb->erased_blocks_bitmap) {
+		kfree(msb->used_blocks_bitmap);
+		kfree(msb->lba_to_pba_table);
+		kfree(msb->erased_blocks_bitmap);
+		return -ENOMEM;
+	}
+
+	for (i = 0 ; i < msb->zone_count ; i++)
+		msb->free_block_count[i] = MS_BLOCKS_IN_ZONE;
+
+	memset(msb->lba_to_pba_table, MS_BLOCK_INVALID,
+			msb->logical_block_count * sizeof(u16));
+
+	dbg("initial FTL tables created. Zone count = %d, "
+					"Logical block count = %d",
+		msb->zone_count, msb->logical_block_count);
+
+	msb->ftl_initialized = true;
+	return 0;
+}
+
+static int msb_ftl_scan(struct msb_data *msb)
+{
+	u16 pba, lba, other_block;
+	u8 overwrite_flag, managment_flag, other_overwrite_flag;
+	int error;
+	struct ms_extra_data_register extra;
+	u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL);
+
+	if (!overwrite_flags)
+		return -ENOMEM;
+
+	dbg("Start of media scanning");
+	for (pba = 0 ; pba < msb->block_count ; pba++) {
+
+		if (pba == msb->boot_block_locations[0] ||
+			pba == msb->boot_block_locations[1]) {
+			dbg_verbose("pba %05d -> [boot block]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		if (test_bit(pba, msb->used_blocks_bitmap)) {
+			dbg_verbose("pba %05d -> [factory bad]", pba);
+			continue;
+		}
+
+		memset(&extra, 0, sizeof(extra));
+		error = msb_read_oob(msb, pba, 0, &extra);
+
+		/* can't trust the page if we can't read the oob */
+		if (error == -EBADMSG) {
+			pr_notice(
+			"oob of pba %d damaged, will try to erase it", pba);
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		} else if (error)
+			return error;
+
+		lba = be16_to_cpu(extra.logical_address);
+		managment_flag = extra.management_flag;
+		overwrite_flag = extra.overwrite_flag;
+		overwrite_flags[pba] = overwrite_flag;
+
+		/* Skip bad blocks */
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
+			dbg("pba %05d -> [BAD]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Skip system/drm blocks */
+		if ((managment_flag & MEMSTICK_MANAGMENT_FLAG_NORMAL) !=
+			MEMSTICK_MANAGMENT_FLAG_NORMAL) {
+			dbg("pba %05d -> [reserved managment flag %02x]",
+							pba, managment_flag);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Erase temporary tables */
+		if (!(managment_flag & MEMSTICK_MANAGEMENT_ATFLG)) {
+			dbg("pba %05d -> [temp table] - will erase", pba);
+
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		if (lba == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [free]", pba);
+			continue;
+		}
+
+		msb_mark_block_used(msb, pba);
+
+		/* Block has LBA not according to zoning*/
+		if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+			pr_notice("pba %05d -> [bad lba %05d] - will erase",
+								pba, lba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		/* No collisions - great */
+		if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [lba %05d]", pba, lba);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		other_block = msb->lba_to_pba_table[lba];
+		other_overwrite_flag = overwrite_flags[other_block];
+
+		pr_notice("Collision between pba %d and pba %d",
+			pba, other_block);
+
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			pr_notice("pba %d is marked as stable, use it", pba);
+			msb_erase_block(msb, other_block);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			pr_notice("pba %d is marked as stable, use it",
+								other_block);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		pr_notice("collision between blocks %d and %d,"
+		" without stable flag set on both, erasing pba %d",
+				pba, other_block, other_block);
+
+		msb_erase_block(msb, other_block);
+		msb->lba_to_pba_table[lba] = pba;
+	}
+
+	dbg("End of media scanning");
+	kfree(overwrite_flags);
+	return 0;
+}
+
+static void msb_cache_flush_timer(unsigned long data)
+{
+	struct msb_data *msb = (struct msb_data *)data;
+	msb->need_flush_cache = true;
+	queue_work(msb->io_queue, &msb->io_work);
+}
+
+
+static void msb_cache_discard(struct msb_data *msb)
+{
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return;
+
+	del_timer_sync(&msb->cache_flush_timer);
+
+	dbg_verbose("Discarding the write cache");
+	msb->cache_block_lba = MS_BLOCK_INVALID;
+	bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block);
+}
+
+static int msb_cache_init(struct msb_data *msb)
+{
+	setup_timer(&msb->cache_flush_timer, msb_cache_flush_timer,
+		(unsigned long)msb);
+
+	if (!msb->cache)
+		msb->cache = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->cache)
+		return -ENOMEM;
+
+	msb_cache_discard(msb);
+	return 0;
+}
+
+static int msb_cache_flush(struct msb_data *msb)
+{
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	int page, offset, error;
+	u16 pba, lba;
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return 0;
+
+	lba = msb->cache_block_lba;
+	pba = msb->lba_to_pba_table[lba];
+
+	dbg_verbose("Flushing the write cache of pba %d (LBA %d)",
+						pba, msb->cache_block_lba);
+
+	sg_init_one(&sg, msb->cache , msb->block_size);
+
+	/* Read all missing pages in cache */
+	for (page = 0 ; page < msb->pages_in_block ; page++) {
+
+		if (test_bit(page, &msb->valid_cache_bitmap))
+			continue;
+
+		offset = page * msb->page_size;
+
+		dbg_verbose("reading non-present sector %d of cache block %d",
+			page, lba);
+		error = msb_read_page(msb, pba, page, &extra, &sg, offset);
+
+		/* Bad pages are copied with 00 page status */
+		if (error == -EBADMSG) {
+			pr_err("read error on sector %d, contents probably"
+				" damaged", page);
+			continue;
+		}
+
+		if (error)
+			return error;
+
+		if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) !=
+							MEMSTICK_OV_PG_NORMAL) {
+			dbg("page %d is marked as bad", page);
+			continue;
+		}
+
+		set_bit(page, &msb->valid_cache_bitmap);
+	}
+
+	/* Write the cache now */
+	error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
+	pba = msb->lba_to_pba_table[msb->cache_block_lba];
+
+	/* Mark invalid pages */
+	if (!error) {
+		for (page = 0 ; page < msb->pages_in_block ; page++) {
+
+			if (test_bit(page, &msb->valid_cache_bitmap))
+				continue;
+
+			dbg("marking page %d as containing damaged data",
+				page);
+			msb_set_overwrite_flag(msb,
+				pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL);
+		}
+	}
+
+	msb_cache_discard(msb);
+	return error;
+}
+
+static int msb_cache_write(struct msb_data *msb, int lba,
+	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
+{
+	int error;
+	struct scatterlist sg_tmp[10];
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
+						lba != msb->cache_block_lba)
+		if (add_to_cache_only)
+			return 0;
+
+	/* If we need to write different block */
+	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
+						lba != msb->cache_block_lba) {
+		dbg_verbose("first flush the cache");
+		error = msb_cache_flush(msb);
+		if (error)
+			return error;
+	}
+
+	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
+		msb->cache_block_lba  = lba;
+		mod_timer(&msb->cache_flush_timer,
+			jiffies + msecs_to_jiffies(cache_flush_timeout));
+	}
+
+	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
+
+	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+	msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+
+	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
+		msb->cache + page * msb->page_size, msb->page_size);
+
+	set_bit(page, &msb->valid_cache_bitmap);
+	return 0;
+}
+
+static int msb_cache_read(struct msb_data *msb, int lba,
+				int page, struct scatterlist *sg, int offset)
+{
+	int pba = msb->lba_to_pba_table[lba];
+	struct scatterlist sg_tmp[10];
+	int error = 0;
+
+	if (lba == msb->cache_block_lba &&
+			test_bit(page, &msb->valid_cache_bitmap)) {
+
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from cache",
+							lba, pba, page);
+
+		sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+		msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+		sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp),
+			msb->cache + msb->page_size * page,
+							msb->page_size);
+	} else {
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from device",
+							lba, pba, page);
+
+		error = msb_read_page(msb, pba, page, NULL, sg, offset);
+		if (error)
+			return error;
+
+		msb_cache_write(msb, lba, page, true, sg, offset);
+	}
+	return error;
+}
+
+/* Emulated geometry table
+ * This table content isn't that importaint,
+ * One could put here different values, providing that they still
+ * cover whole disk.
+ * 64 MB entry is what windows reports for my 64M memstick */
+
+static const struct chs_entry chs_table[] = {
+/*        size sectors cylynders  heads */
+	{ 4,    16,    247,       2  },
+	{ 8,    16,    495,       2  },
+	{ 16,   16,    495,       4  },
+	{ 32,   16,    991,       4  },
+	{ 64,   16,    991,       8  },
+	{128,   16,    991,       16 },
+	{ 0 }
+};
+
+/* Load information about the card */
+static int msb_init_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	struct ms_boot_page *boot_block;
+	int error = 0, i, raw_size_in_megs;
+
+	msb->caps = 0;
+
+	if (card->id.class >= MEMSTICK_CLASS_ROM &&
+				card->id.class <= MEMSTICK_CLASS_ROM)
+		msb->read_only = true;
+
+	msb->state = -1;
+	error = msb_reset(msb, false);
+	if (error)
+		return error;
+
+	/* Due to a bug in Jmicron driver written by Alex Dubov,
+	 its serial mode barely works,
+	 so we switch to parallel mode right away */
+	if (host->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+
+	msb->page_size = sizeof(struct ms_boot_page);
+
+	/* Read the boot page */
+	error = msb_read_boot_blocks(msb);
+	if (error)
+		return -EIO;
+
+	boot_block = &msb->boot_page[0];
+
+	/* Save intersting attributes from boot page */
+	msb->block_count = boot_block->attr.number_of_blocks;
+	msb->page_size = boot_block->attr.page_size;
+
+	msb->pages_in_block = boot_block->attr.block_size * 2;
+	msb->block_size = msb->page_size * msb->pages_in_block;
+
+	if (msb->page_size > PAGE_SIZE) {
+		/* this isn't supported by linux at all, anyway*/
+		dbg("device page %d size isn't supported", msb->page_size);
+		return -EINVAL;
+	}
+
+	msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->block_buffer)
+		return -ENOMEM;
+
+	raw_size_in_megs = (msb->block_size * msb->block_count) >> 20;
+
+	for (i = 0 ; chs_table[i].size ; i++) {
+
+		if (chs_table[i].size != raw_size_in_megs)
+			continue;
+
+		msb->geometry.cylinders = chs_table[i].cyl;
+		msb->geometry.heads = chs_table[i].head;
+		msb->geometry.sectors = chs_table[i].sec;
+		break;
+	}
+
+	if (boot_block->attr.transfer_supporting == 1)
+		msb->caps |= MEMSTICK_CAP_PAR4;
+
+	if (boot_block->attr.device_type & 0x03)
+		msb->read_only = true;
+
+	dbg("Total block count = %d", msb->block_count);
+	dbg("Each block consists of %d pages", msb->pages_in_block);
+	dbg("Page size = %d bytes", msb->page_size);
+	dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4));
+	dbg("Read only: %d", msb->read_only);
+
+#if 0
+	/* Now we can switch the interface */
+	if (host->caps & msb->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+#endif
+
+	error = msb_cache_init(msb);
+	if (error)
+		return error;
+
+	error = msb_ftl_initialize(msb);
+	if (error)
+		return error;
+
+
+	/* Read the bad block table */
+	error = msb_read_bad_block_table(msb, 0);
+
+	if (error && error != -ENOMEM) {
+		dbg("failed to read bad block table from primary boot block,"
+							" trying from backup");
+		error = msb_read_bad_block_table(msb, 1);
+	}
+
+	if (error)
+		return error;
+
+	/* *drum roll* Scan the media */
+	error = msb_ftl_scan(msb);
+	if (error) {
+		pr_err("Scan of media failed");
+		return error;
+	}
+
+	return 0;
+
+}
+
+static int msb_do_write_request(struct msb_data *msb, int lba,
+	int page, struct scatterlist *sg, size_t len, int *sucessfuly_written)
+{
+	int error = 0;
+	off_t offset = 0;
+	*sucessfuly_written = 0;
+
+	while (offset < len) {
+		if (page == 0 && len - offset >= msb->block_size) {
+
+			if (msb->cache_block_lba == lba)
+				msb_cache_discard(msb);
+
+			dbg_verbose("Writing whole lba %d", lba);
+			error = msb_update_block(msb, lba, sg, offset);
+			if (error)
+				return error;
+
+			offset += msb->block_size;
+			*sucessfuly_written += msb->block_size;
+			lba++;
+			continue;
+		}
+
+		error = msb_cache_write(msb, lba, page, false, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_written += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static int msb_do_read_request(struct msb_data *msb, int lba,
+		int page, struct scatterlist *sg, int len, int *sucessfuly_read)
+{
+	int error = 0;
+	int offset = 0;
+	*sucessfuly_read = 0;
+
+	while (offset < len) {
+
+		error = msb_cache_read(msb, lba, page, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_read += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static void msb_io_work(struct work_struct *work)
+{
+	struct msb_data *msb = container_of(work, struct msb_data, io_work);
+	int page, error, len;
+	sector_t lba;
+	unsigned long flags;
+	struct scatterlist *sg = msb->prealloc_sg;
+
+	dbg_verbose("IO: work started");
+
+	while (1) {
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (msb->need_flush_cache) {
+			msb->need_flush_cache = false;
+			spin_unlock_irqrestore(&msb->q_lock, flags);
+			msb_cache_flush(msb);
+			continue;
+		}
+
+		if (!msb->req) {
+			msb->req = blk_fetch_request(msb->queue);
+			if (!msb->req) {
+				dbg_verbose("IO: no more requests exiting");
+				spin_unlock_irqrestore(&msb->q_lock, flags);
+				return;
+			}
+		}
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+
+		/* If card was removed meanwhile */
+		if (!msb->req)
+			return;
+
+		/* process the request */
+		dbg_verbose("IO: processing new request");
+		blk_rq_map_sg(msb->queue, msb->req, sg);
+
+		lba = blk_rq_pos(msb->req);
+
+		sector_div(lba, msb->page_size / 512);
+		page = do_div(lba, msb->pages_in_block);
+
+		if (rq_data_dir(msb->req) == READ)
+			error = msb_do_read_request(msb, lba, page, sg,
+				blk_rq_bytes(msb->req), &len);
+		else
+			error = msb_do_write_request(msb, lba, page, sg,
+				blk_rq_bytes(msb->req), &len);
+
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (len)
+			if (!__blk_end_request(msb->req, 0, len))
+				msb->req = NULL;
+
+		if (error && msb->req) {
+			dbg_verbose("IO: ending one sector "
+					"of the request with error");
+			if (!__blk_end_request(msb->req, error, msb->page_size))
+				msb->req = NULL;
+		}
+
+		if (msb->req)
+			dbg_verbose("IO: request still pending");
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+}
+
+static DEFINE_IDR(msb_disk_idr); /*set of used disk numbers */
+static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */
+
+static int msb_bd_open(struct block_device *bdev, fmode_t mode)
+{
+	struct gendisk *disk = bdev->bd_disk;
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device open");
+
+	mutex_lock(&msb_disk_lock);
+
+	if (msb && msb->card)
+		msb->usage_count++;
+
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static void msb_data_clear(struct msb_data *msb)
+{
+	kfree(msb->boot_page);
+	kfree(msb->used_blocks_bitmap);
+	kfree(msb->lba_to_pba_table);
+	kfree(msb->cache);
+	msb->card = NULL;
+}
+
+static int msb_disk_release(struct gendisk *disk)
+{
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device release");
+	mutex_lock(&msb_disk_lock);
+
+	if (msb) {
+		if (msb->usage_count)
+			msb->usage_count--;
+
+		if (!msb->usage_count) {
+			kfree(msb);
+			disk->private_data = NULL;
+			idr_remove(&msb_disk_idr, msb->disk_id);
+			put_disk(disk);
+		}
+	}
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static int msb_bd_release(struct gendisk *disk, fmode_t mode)
+{
+	return msb_disk_release(disk);
+}
+
+static int msb_bd_getgeo(struct block_device *bdev,
+				 struct hd_geometry *geo)
+{
+	struct msb_data *msb = bdev->bd_disk->private_data;
+	*geo = msb->geometry;
+	return 0;
+}
+
+static int msb_prepare_req(struct request_queue *q, struct request *req)
+{
+	if (req->cmd_type != REQ_TYPE_FS &&
+				req->cmd_type != REQ_TYPE_BLOCK_PC) {
+		blk_dump_rq_flags(req, "MS unsupported request");
+		return BLKPREP_KILL;
+	}
+	req->cmd_flags |= REQ_DONTPREP;
+	return BLKPREP_OK;
+}
+
+static void msb_submit_req(struct request_queue *q)
+{
+	struct memstick_dev *card = q->queuedata;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct request *req = NULL;
+
+	dbg_verbose("Submit request");
+
+	if (msb->card_dead) {
+		dbg("Refusing requests on removed card");
+
+		WARN_ON(!msb->io_queue_stopped);
+
+		while ((req = blk_fetch_request(q)) != NULL)
+			__blk_end_request_all(req, -ENODEV);
+		return;
+	}
+
+	if (msb->req)
+		return;
+
+	if (!msb->io_queue_stopped)
+		queue_work(msb->io_queue, &msb->io_work);
+}
+
+static int msb_check_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	return (msb->card_dead == 0);
+}
+
+static void msb_stop(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	dbg("Stopping all msblock IO");
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_stop_queue(msb->queue);
+	msb->io_queue_stopped = true;
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	del_timer_sync(&msb->cache_flush_timer);
+	flush_workqueue(msb->io_queue);
+
+	if (msb->req) {
+		spin_lock_irqsave(&msb->q_lock, flags);
+		blk_requeue_request(msb->queue, msb->req);
+		msb->req = NULL;
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+
+}
+
+static void msb_start(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	dbg("Resuming IO from msblock");
+
+	msb_invalidate_reg_window(msb);
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	if (!msb->io_queue_stopped || msb->card_dead) {
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Kick cache flush anyway, its harmless */
+	msb->need_flush_cache = true;
+	msb->io_queue_stopped = false;
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	queue_work(msb->io_queue, &msb->io_work);
+
+}
+
+static const struct block_device_operations msb_bdops = {
+	.open    = msb_bd_open,
+	.release = msb_bd_release,
+	.getgeo  = msb_bd_getgeo,
+	.owner   = THIS_MODULE
+};
+
+/* Registers the block device */
+static int msb_init_disk(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	int rc;
+	u64 limit = BLK_BOUNCE_HIGH;
+	unsigned long capacity;
+
+	if (host->dev.dma_mask && *(host->dev.dma_mask))
+		limit = *(host->dev.dma_mask);
+
+	mutex_lock(&msb_disk_lock);
+	msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL);
+	mutex_unlock(&msb_disk_lock);
+
+	if (msb->disk_id  < 0)
+		return msb->disk_id;
+
+	msb->disk = alloc_disk(0);
+	if (!msb->disk) {
+		rc = -ENOMEM;
+		goto out_release_id;
+	}
+
+	msb->queue = blk_init_queue(msb_submit_req, &msb->q_lock);
+	if (!msb->queue) {
+		rc = -ENOMEM;
+		goto out_put_disk;
+	}
+
+	msb->queue->queuedata = card;
+	blk_queue_prep_rq(msb->queue, msb_prepare_req);
+
+	blk_queue_bounce_limit(msb->queue, limit);
+	blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
+	blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
+	blk_queue_max_segment_size(msb->queue,
+				   MS_BLOCK_MAX_PAGES * msb->page_size);
+	blk_queue_logical_block_size(msb->queue, msb->page_size);
+
+	sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
+	msb->disk->fops = &msb_bdops;
+	msb->disk->private_data = msb;
+	msb->disk->queue = msb->queue;
+	msb->disk->driverfs_dev = &card->dev;
+	msb->disk->flags |= GENHD_FL_EXT_DEVT;
+
+	capacity = msb->pages_in_block * msb->logical_block_count;
+	capacity *= (msb->page_size / 512);
+	set_capacity(msb->disk, capacity);
+	dbg("Set total disk size to %lu sectors", capacity);
+
+	msb->usage_count = 1;
+	msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
+	INIT_WORK(&msb->io_work, msb_io_work);
+	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
+
+	if (msb->read_only)
+		set_disk_ro(msb->disk, 1);
+
+	msb_start(card);
+	add_disk(msb->disk);
+	dbg("Disk added");
+	return 0;
+
+out_put_disk:
+	put_disk(msb->disk);
+out_release_id:
+	mutex_lock(&msb_disk_lock);
+	idr_remove(&msb_disk_idr, msb->disk_id);
+	mutex_unlock(&msb_disk_lock);
+	return rc;
+}
+
+static int msb_probe(struct memstick_dev *card)
+{
+	struct msb_data *msb;
+	int rc = 0;
+
+	msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!msb)
+		return -ENOMEM;
+	memstick_set_drvdata(card, msb);
+	msb->card = card;
+	spin_lock_init(&msb->q_lock);
+
+	rc = msb_init_card(card);
+	if (rc)
+		goto out_free;
+
+	rc = msb_init_disk(card);
+	if (!rc) {
+		card->check = msb_check_card;
+		card->stop = msb_stop;
+		card->start = msb_start;
+		return 0;
+	}
+out_free:
+	memstick_set_drvdata(card, NULL);
+	msb_data_clear(msb);
+	kfree(msb);
+	return rc;
+}
+
+static void msb_remove(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	if (!msb->io_queue_stopped)
+		msb_stop(card);
+
+	dbg("Removing the disk device");
+
+	/* Take care of unhandled + new requests from now on */
+	spin_lock_irqsave(&msb->q_lock, flags);
+	msb->card_dead = true;
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Remove the disk */
+	del_gendisk(msb->disk);
+	blk_cleanup_queue(msb->queue);
+	msb->queue = NULL;
+
+	mutex_lock(&msb_disk_lock);
+	msb_data_clear(msb);
+	mutex_unlock(&msb_disk_lock);
+
+	msb_disk_release(msb->disk);
+	memstick_set_drvdata(card, NULL);
+}
+
+#ifdef CONFIG_PM
+
+static int msb_suspend(struct memstick_dev *card, pm_message_t state)
+{
+	msb_stop(card);
+	return 0;
+}
+
+static int msb_resume(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct msb_data *new_msb = NULL;
+	bool card_dead = true;
+
+#ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
+	msb->card_dead = true;
+	return 0;
+#endif
+	mutex_lock(&card->host->lock);
+
+	new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!new_msb)
+		goto out;
+
+	new_msb->card = card;
+	memstick_set_drvdata(card, new_msb);
+	spin_lock_init(&new_msb->q_lock);
+	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
+
+	if (msb_init_card(card))
+		goto out;
+
+	if (msb->block_size != new_msb->block_size)
+		goto out;
+
+	if (memcmp(msb->boot_page, new_msb->boot_page,
+					sizeof(struct ms_boot_page)))
+		goto out;
+
+	if (msb->logical_block_count != new_msb->logical_block_count ||
+		memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
+						msb->logical_block_count))
+		goto out;
+
+	if (msb->block_count != new_msb->block_count ||
+		memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
+							msb->block_count / 8))
+		goto out;
+
+	card_dead = false;
+out:
+	if (card_dead)
+		dbg("Card was removed/replaced during suspend");
+
+	msb->card_dead = card_dead;
+	memstick_set_drvdata(card, msb);
+
+	if (new_msb) {
+		msb_data_clear(new_msb);
+		kfree(new_msb);
+	}
+
+	msb_start(card);
+	mutex_unlock(&card->host->lock);
+	return 0;
+}
+#else
+
+#define msb_suspend NULL
+#define msb_resume NULL
+
+#endif /* CONFIG_PM */
+
+static struct memstick_device_id msb_id_tbl[] = {
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_FLASH},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_ROM},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_RO},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_WP},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
+	 MEMSTICK_CLASS_DUO},
+	{}
+};
+MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
+
+
+static struct memstick_driver msb_driver = {
+	.driver = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE
+	},
+	.id_table = msb_id_tbl,
+	.probe    = msb_probe,
+	.remove   = msb_remove,
+	.suspend  = msb_suspend,
+	.resume   = msb_resume
+};
+
+static int major;
+
+static int __init msb_init(void)
+{
+	int rc = register_blkdev(0, DRIVER_NAME);
+
+	if (rc < 0) {
+		pr_err("failed to register major (error %d)\n", rc);
+		return rc;
+	}
+
+	major = rc;
+	rc = memstick_register_driver(&msb_driver);
+	if (rc) {
+		unregister_blkdev(major, DRIVER_NAME);
+		pr_err("failed to register memstick driver (error %d)\n", rc);
+	}
+
+	return rc;
+}
+
+static void __exit msb_exit(void)
+{
+	memstick_unregister_driver(&msb_driver);
+	unregister_blkdev(major, DRIVER_NAME);
+	idr_destroy(&msb_disk_idr);
+}
+
+module_init(msb_init);
+module_exit(msb_exit);
+
+module_param(cache_flush_timeout, int, S_IRUGO);
+MODULE_PARM_DESC(cache_flush_timeout,
+				"Cache flush timeout in msec (1000 default)");
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+module_param(verify_writes, bool, S_IRUGO);
+MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Maxim Levitsky");
+MODULE_DESCRIPTION("Sony MemoryStick block device driver");
diff --git a/drivers/memstick/core/ms_block.h b/drivers/memstick/core/ms_block.h
new file mode 100644
index 0000000..91df347
--- /dev/null
+++ b/drivers/memstick/core/ms_block.h
@@ -0,0 +1,290 @@
+/*
+ *  ms_block.h - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2012 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver are copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ * Also ms structures were copied from old broken driver by same author
+ * These probably come from MS spec
+ *
+ */
+
+#ifndef MS_BLOCK_NEW_H
+#define MS_BLOCK_NEW_H
+
+#define MS_BLOCK_MAX_SEGS      32
+#define MS_BLOCK_MAX_PAGES     ((2 << 16) - 1)
+
+#define MS_BLOCK_MAX_BOOT_ADDR 0x000c
+#define MS_BLOCK_BOOT_ID       0x0001
+#define MS_BLOCK_INVALID       0xffff
+#define MS_MAX_ZONES           16
+#define MS_BLOCKS_IN_ZONE      512
+
+#define MS_BLOCK_MAP_LINE_SZ   16
+#define MS_BLOCK_PART_SHIFT    3
+
+
+#define MEMSTICK_UNCORR_ERROR (MEMSTICK_STATUS1_UCFG | \
+		MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCDT)
+
+#define MEMSTICK_CORR_ERROR (MEMSTICK_STATUS1_FGER | MEMSTICK_STATUS1_EXER | \
+	MEMSTICK_STATUS1_DTER)
+
+#define MEMSTICK_INT_ERROR (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)
+
+#define MEMSTICK_OVERWRITE_FLAG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | \
+	MEMSTICK_OVERWRITE_PGST0  | \
+	MEMSTICK_OVERWRITE_BKST)
+
+#define MEMSTICK_OV_PG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | MEMSTICK_OVERWRITE_PGST0)
+
+#define MEMSTICK_MANAGMENT_FLAG_NORMAL \
+	(MEMSTICK_MANAGEMENT_SYSFLG |  \
+	MEMSTICK_MANAGEMENT_SCMS1   |  \
+	MEMSTICK_MANAGEMENT_SCMS0)     \
+
+struct ms_boot_header {
+	unsigned short block_id;
+	unsigned short format_reserved;
+	unsigned char  reserved0[184];
+	unsigned char  data_entry;
+	unsigned char  reserved1[179];
+} __packed;
+
+
+struct ms_system_item {
+	unsigned int  start_addr;
+	unsigned int  data_size;
+	unsigned char data_type_id;
+	unsigned char reserved[3];
+} __packed;
+
+struct ms_system_entry {
+	struct ms_system_item disabled_block;
+	struct ms_system_item cis_idi;
+	unsigned char         reserved[24];
+} __packed;
+
+struct ms_boot_attr_info {
+	unsigned char      memorystick_class;
+	unsigned char      format_unique_value1;
+	unsigned short     block_size;
+	unsigned short     number_of_blocks;
+	unsigned short     number_of_effective_blocks;
+	unsigned short     page_size;
+	unsigned char      extra_data_size;
+	unsigned char      format_unique_value2;
+	unsigned char      assembly_time[8];
+	unsigned char      format_unique_value3;
+	unsigned char      serial_number[3];
+	unsigned char      assembly_manufacturer_code;
+	unsigned char      assembly_model_code[3];
+	unsigned short     memory_manufacturer_code;
+	unsigned short     memory_device_code;
+	unsigned short     implemented_capacity;
+	unsigned char      format_unique_value4[2];
+	unsigned char      vcc;
+	unsigned char      vpp;
+	unsigned short     controller_number;
+	unsigned short     controller_function;
+	unsigned char      reserved0[9];
+	unsigned char      transfer_supporting;
+	unsigned short     format_unique_value5;
+	unsigned char      format_type;
+	unsigned char      memorystick_application;
+	unsigned char      device_type;
+	unsigned char      reserved1[22];
+	unsigned char      format_uniqure_value6[2];
+	unsigned char      reserved2[15];
+} __packed;
+
+struct ms_cis_idi {
+	unsigned short general_config;
+	unsigned short logical_cylinders;
+	unsigned short reserved0;
+	unsigned short logical_heads;
+	unsigned short track_size;
+	unsigned short page_size;
+	unsigned short pages_per_track;
+	unsigned short msw;
+	unsigned short lsw;
+	unsigned short reserved1;
+	unsigned char  serial_number[20];
+	unsigned short buffer_type;
+	unsigned short buffer_size_increments;
+	unsigned short long_command_ecc;
+	unsigned char  firmware_version[28];
+	unsigned char  model_name[18];
+	unsigned short reserved2[5];
+	unsigned short pio_mode_number;
+	unsigned short dma_mode_number;
+	unsigned short field_validity;
+	unsigned short current_logical_cylinders;
+	unsigned short current_logical_heads;
+	unsigned short current_pages_per_track;
+	unsigned int   current_page_capacity;
+	unsigned short mutiple_page_setting;
+	unsigned int   addressable_pages;
+	unsigned short single_word_dma;
+	unsigned short multi_word_dma;
+	unsigned char  reserved3[128];
+} __packed;
+
+
+struct ms_boot_page {
+	struct ms_boot_header    header;
+	struct ms_system_entry   entry;
+	struct ms_boot_attr_info attr;
+} __packed;
+
+struct msb_data {
+	unsigned int			usage_count;
+	struct memstick_dev		*card;
+	struct gendisk			*disk;
+	struct request_queue		*queue;
+	spinlock_t			q_lock;
+	struct hd_geometry		geometry;
+	struct attribute_group		attr_group;
+	struct request			*req;
+	int				caps;
+	int				disk_id;
+
+	/* IO */
+	struct workqueue_struct		*io_queue;
+	bool				io_queue_stopped;
+	struct work_struct		io_work;
+	bool				card_dead;
+
+	/* Media properties */
+	struct ms_boot_page		*boot_page;
+	u16				boot_block_locations[2];
+	int				boot_block_count;
+
+	bool				read_only;
+	unsigned short			page_size;
+	int				block_size;
+	int				pages_in_block;
+	int				zone_count;
+	int				block_count;
+	int				logical_block_count;
+
+	/* FTL tables */
+	unsigned long			*used_blocks_bitmap;
+	unsigned long			*erased_blocks_bitmap;
+	u16				*lba_to_pba_table;
+	int				free_block_count[MS_MAX_ZONES];
+	bool				ftl_initialized;
+
+	/* Cache */
+	unsigned char			*cache;
+	unsigned long			valid_cache_bitmap;
+	int				cache_block_lba;
+	bool				need_flush_cache;
+	struct timer_list		cache_flush_timer;
+
+	/* Preallocated buffers */
+	unsigned char			*block_buffer;
+	struct scatterlist		prealloc_sg[MS_BLOCK_MAX_SEGS+1];
+
+
+	/* handler's local data */
+	struct ms_register_addr		reg_addr;
+	bool				addr_valid;
+
+	u8				command_value;
+	bool				command_need_oob;
+	struct scatterlist		*current_sg;
+	int				current_sg_offset;
+
+	struct ms_register		regs;
+	int				current_page;
+
+	int				state;
+	int				exit_error;
+	bool				int_polling;
+	unsigned long			int_timeout;
+
+};
+
+enum msb_readpage_states {
+	MSB_RP_SEND_BLOCK_ADDRESS = 0,
+	MSB_RP_SEND_READ_COMMAND,
+
+	MSB_RP_SEND_INT_REQ,
+	MSB_RP_RECEIVE_INT_REQ_RESULT,
+
+	MSB_RP_SEND_READ_STATUS_REG,
+	MSB_RP_RECIVE_STATUS_REG,
+
+	MSB_RP_SEND_OOB_READ,
+	MSB_RP_RECEIVE_OOB_READ,
+
+	MSB_RP_SEND_READ_DATA,
+	MSB_RP_RECEIVE_READ_DATA,
+};
+
+enum msb_write_block_states {
+	MSB_WB_SEND_WRITE_PARAMS = 0,
+	MSB_WB_SEND_WRITE_OOB,
+	MSB_WB_SEND_WRITE_COMMAND,
+
+	MSB_WB_SEND_INT_REQ,
+	MSB_WB_RECEIVE_INT_REQ,
+
+	MSB_WB_SEND_WRITE_DATA,
+	MSB_WB_RECEIVE_WRITE_CONFIRMATION,
+};
+
+enum msb_send_command_states {
+	MSB_SC_SEND_WRITE_PARAMS,
+	MSB_SC_SEND_WRITE_OOB,
+	MSB_SC_SEND_COMMAND,
+
+	MSB_SC_SEND_INT_REQ,
+	MSB_SC_RECEIVE_INT_REQ,
+
+};
+
+enum msb_reset_states {
+	MSB_RS_SEND,
+	MSB_RS_CONFIRM,
+};
+
+enum msb_par_switch_states {
+	MSB_PS_SEND_SWITCH_COMMAND,
+	MSB_PS_SWICH_HOST,
+	MSB_PS_CONFIRM,
+};
+
+struct chs_entry {
+	unsigned long size;
+	unsigned char sec;
+	unsigned short cyl;
+	unsigned char head;
+};
+
+static int msb_reset(struct msb_data *msb, bool full);
+
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq);
+
+#define __dbg(level, format, ...) \
+	do { \
+		if (debug >= level) \
+			pr_err(format "\n", ## __VA_ARGS__); \
+	} while (0)
+
+
+#define dbg(format, ...)		__dbg(1, format, ## __VA_ARGS__)
+#define dbg_verbose(format, ...)	__dbg(2, format, ## __VA_ARGS__)
+
+#endif
-- 
1.7.9.5

Driver for memestick standard cards

[Maxim Levitsky]

Hi!

I now properly updated this driver and tested against 3.10, fixed a bunch
of new checkpatch errors.

Could you merge this to 3.10?

Best regards,
	Maxim Levitsky

[PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

Based partially on MS standard spec quotes from Alex Dubov.

As any code that works with user data this driver isn't
recommended to use to write cards that contain valuable data.

It tries its best though to avoid data corruption
and possible damage to the card.

Tested on MS DUO 64 MB card on Ricoh R592 card reader.

Signed-off-by: Maxim Levitsky <maximlevitsky@gmail.com>
---
 MAINTAINERS                      |    5 +
 drivers/memstick/core/Kconfig    |   12 +
 drivers/memstick/core/Makefile   |    2 +-
 drivers/memstick/core/ms_block.c | 2385 ++++++++++++++++++++++++++++++++++++++
 drivers/memstick/core/ms_block.h |  290 +++++
 5 files changed, 2693 insertions(+), 1 deletion(-)
 create mode 100644 drivers/memstick/core/ms_block.c
 create mode 100644 drivers/memstick/core/ms_block.h

diff --git a/MAINTAINERS b/MAINTAINERS
index ad7e322..26a01e2 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -7519,6 +7519,11 @@ W:	http://tifmxx.berlios.de/
 S:	Maintained
 F:	drivers/memstick/host/tifm_ms.c
 
+SONY MEMORYSTICK STANDARD SUPPORT
+M:	Maxim Levitsky <maximlevitsky@gmail.com>
+S:	Maintained
+F:	drivers/memstick/core/ms_block.*
+
 SOUND
 M:	Jaroslav Kysela <perex@perex.cz>
 M:	Takashi Iwai <tiwai@suse.de>
diff --git a/drivers/memstick/core/Kconfig b/drivers/memstick/core/Kconfig
index 95f1814..1d38949 100644
--- a/drivers/memstick/core/Kconfig
+++ b/drivers/memstick/core/Kconfig
@@ -24,3 +24,15 @@ config MSPRO_BLOCK
 	  support. This provides a block device driver, which you can use
 	  to mount the filesystem. Almost everyone wishing MemoryStick
 	  support should say Y or M here.
+
+config MS_BLOCK
+	tristate "MemoryStick Standard device driver"
+	depends on BLOCK
+	help
+	  Say Y here to enable the MemoryStick Standard device driver
+	  support. This provides a block device driver, which you can use
+	  to mount the filesystem.
+	  This driver works with old (bulky) MemoryStick and MemoryStick Duo
+	  but not PRO. Say Y if you have such card.
+	  Driver is new and not yet well tested, thus it can damage your card
+	  (even permanently)
diff --git a/drivers/memstick/core/Makefile b/drivers/memstick/core/Makefile
index ecd0299..0d7f90c 100644
--- a/drivers/memstick/core/Makefile
+++ b/drivers/memstick/core/Makefile
@@ -3,5 +3,5 @@
 #
 
 obj-$(CONFIG_MEMSTICK)		+= memstick.o
-
+obj-$(CONFIG_MS_BLOCK)		+= ms_block.o
 obj-$(CONFIG_MSPRO_BLOCK)	+= mspro_block.o
diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
new file mode 100644
index 0000000..08e7023
--- /dev/null
+++ b/drivers/memstick/core/ms_block.c
@@ -0,0 +1,2385 @@
+/*
+ *  ms_block.c - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2013 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver were copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ */
+#define DRIVER_NAME "ms_block"
+#define pr_fmt(fmt) DRIVER_NAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/memstick.h>
+#include <linux/idr.h>
+#include <linux/hdreg.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/bitmap.h>
+#include <linux/scatterlist.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include "ms_block.h"
+
+static int debug;
+static int cache_flush_timeout = 1000;
+static bool verify_writes;
+
+/*
+ * Copies section of 'sg_from' starting from offset 'offset' and with length
+ * 'len' To another scatterlist of to_nents enties
+ */
+static size_t msb_sg_copy(struct scatterlist *sg_from,
+	struct scatterlist *sg_to, int to_nents, size_t offset, size_t len)
+{
+	size_t copied = 0;
+
+	while (offset > 0) {
+		if (offset >= sg_from->length) {
+			if (sg_is_last(sg_from))
+				return 0;
+
+			offset -= sg_from->length;
+			sg_from = sg_next(sg_from);
+			continue;
+		}
+
+		copied = min(len, sg_from->length - offset);
+		sg_set_page(sg_to, sg_page(sg_from),
+			copied, sg_from->offset + offset);
+
+		len -= copied;
+		offset = 0;
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_to = sg_next(sg_to);
+		to_nents--;
+		sg_from = sg_next(sg_from);
+	}
+
+	while (len > sg_from->length && to_nents--) {
+		len -= sg_from->length;
+		copied += sg_from->length;
+
+		sg_set_page(sg_to, sg_page(sg_from),
+				sg_from->length, sg_from->offset);
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_from = sg_next(sg_from);
+		sg_to = sg_next(sg_to);
+	}
+
+	if (len && to_nents) {
+		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
+		copied += len;
+	}
+out:
+	sg_mark_end(sg_to);
+	return copied;
+}
+
+/*
+ * Compares section of 'sg' starting from offset 'offset' and with length 'len'
+ * to linear buffer of length 'len' at address 'buffer'
+ * Returns 0 if equal and  -1 otherwice
+ */
+static int msb_sg_compare_to_buffer(struct scatterlist *sg,
+					size_t offset, u8 *buffer, size_t len)
+{
+	int retval = 0, cmplen;
+	struct sg_mapping_iter miter;
+
+	sg_miter_start(&miter, sg, sg_nents(sg),
+					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
+
+	while (sg_miter_next(&miter) && len > 0) {
+		if (offset >= miter.length) {
+			offset -= miter.length;
+			continue;
+		}
+
+		cmplen = min(miter.length - offset, len);
+		retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
+		if (retval)
+			break;
+
+		buffer += cmplen;
+		len -= cmplen;
+		offset = 0;
+	}
+
+	if (!retval && len)
+		retval = -1;
+
+	sg_miter_stop(&miter);
+	return retval;
+}
+
+
+/* Get zone at which block with logical address 'lba' lives
+ * Flash is broken into zones.
+ * Each zone consists of 512 eraseblocks, out of which in first
+ * zone 494 are used and 496 are for all following zones.
+ * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
+*/
+static int msb_get_zone_from_lba(int lba)
+{
+	if (lba < 494)
+		return 0;
+	return ((lba - 494) / 496) + 1;
+}
+
+/* Get zone of physical block. Trivial */
+static int msb_get_zone_from_pba(int pba)
+{
+	return pba / MS_BLOCKS_IN_ZONE;
+}
+
+/* Debug test to validate free block counts */
+static int msb_validate_used_block_bitmap(struct msb_data *msb)
+{
+	int total_free_blocks = 0;
+	int i;
+
+	if (!debug)
+		return 0;
+
+	for (i = 0; i < msb->zone_count; i++)
+		total_free_blocks += msb->free_block_count[i];
+
+	if (msb->block_count - bitmap_weight(msb->used_blocks_bitmap,
+					msb->block_count) == total_free_blocks)
+		return 0;
+
+	pr_err("BUG: free block counts don't match the bitmap");
+	msb->read_only = true;
+	return -EINVAL;
+}
+
+/* Mark physical block as used */
+static void msb_mark_block_used(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (test_bit(pba, msb->used_blocks_bitmap)) {
+		pr_err(
+		"BUG: attempt to mark already used pba %d as used", pba);
+		msb->read_only = true;
+		return;
+	}
+
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+
+	/* No races because all IO is single threaded */
+	__set_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]--;
+}
+
+/* Mark physical block as free */
+static void msb_mark_block_unused(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (!test_bit(pba, msb->used_blocks_bitmap)) {
+		pr_err("BUG: attempt to mark already unused pba %d as unused" , pba);
+		msb->read_only = true;
+		return;
+	}
+
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+
+	/* No races because all IO is single threaded */
+	__clear_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]++;
+}
+
+/* Invalidate current register window */
+static void msb_invalidate_reg_window(struct msb_data *msb)
+{
+	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.w_length = sizeof(struct ms_id_register);
+	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.r_length = sizeof(struct ms_id_register);
+	msb->addr_valid = false;
+}
+
+/* Start a state machine */
+static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
+		(struct memstick_dev *card, struct memstick_request **req))
+{
+	struct memstick_dev *card = msb->card;
+
+	WARN_ON(msb->state != -1);
+	msb->int_polling = false;
+	msb->state = 0;
+	msb->exit_error = 0;
+
+	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
+
+	card->next_request = state_func;
+	memstick_new_req(card->host);
+	wait_for_completion(&card->mrq_complete);
+
+	WARN_ON(msb->state != -1);
+	return msb->exit_error;
+}
+
+/* State machines call that to exit */
+static int msb_exit_state_machine(struct msb_data *msb, int error)
+{
+	WARN_ON(msb->state == -1);
+
+	msb->state = -1;
+	msb->exit_error = error;
+	msb->card->next_request = h_msb_default_bad;
+
+	/* Invalidate reg window on errors */
+	if (error)
+		msb_invalidate_reg_window(msb);
+
+	complete(&msb->card->mrq_complete);
+	return -ENXIO;
+}
+
+/* read INT register */
+static int msb_read_int_reg(struct msb_data *msb, long timeout)
+{
+	struct memstick_request *mrq = &msb->card->current_mrq;
+
+	WARN_ON(msb->state == -1);
+
+	if (!msb->int_polling) {
+		msb->int_timeout = jiffies +
+			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
+		msb->int_polling = true;
+	} else if (time_after(jiffies, msb->int_timeout)) {
+		mrq->data[0] = MEMSTICK_INT_CMDNAK;
+		return 0;
+	}
+
+	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
+				mrq->need_card_int && !mrq->error) {
+		mrq->data[0] = mrq->int_reg;
+		mrq->need_card_int = false;
+		return 0;
+	} else {
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		return 1;
+	}
+}
+
+/* Read a register */
+static int msb_read_regs(struct msb_data *msb, int offset, int len)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.r_offset != offset ||
+	    msb->reg_addr.r_length != len || !msb->addr_valid) {
+
+		msb->reg_addr.r_offset = offset;
+		msb->reg_addr.r_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
+	return 1;
+}
+
+/* Write a card register */
+static int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.w_offset != offset ||
+		msb->reg_addr.w_length != len  || !msb->addr_valid) {
+
+		msb->reg_addr.w_offset = offset;
+		msb->reg_addr.w_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
+	return 1;
+}
+
+/* Handler for absence of IO */
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq)
+{
+	return -ENXIO;
+}
+
+/*
+ * This function is a handler for reads of one page from device.
+ * Writes output to msb->current_sg, takes sector address from msb->reg.param
+ * Can also be used to read extra data only. Set params accordintly.
+ */
+static int h_msb_read_page(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 command, intreg;
+
+	if (mrq->error) {
+		dbg("read_page, unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+	case MSB_RP_SEND_BLOCK_ADDRESS:
+		/* msb_write_regs sometimes "fails" because it needs to update
+			the reg window, and thus it returns request for that.
+			Then we stay in this state and retry */
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			(unsigned char *)&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_RP_SEND_READ_COMMAND;
+		return 0;
+
+	case MSB_RP_SEND_READ_COMMAND:
+		command = MS_CMD_BLOCK_READ;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		msb->state = MSB_RP_SEND_INT_REQ;
+		return 0;
+
+	case MSB_RP_SEND_INT_REQ:
+		msb->state = MSB_RP_RECEIVE_INT_REQ_RESULT;
+		/* If dont actually need to send the int read request (only in
+			serial mode), then just fall through */
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_RP_RECEIVE_INT_REQ_RESULT:
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state = MSB_RP_SEND_INT_REQ;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state = (intreg & MEMSTICK_INT_ERR) ?
+			MSB_RP_SEND_READ_STATUS_REG : MSB_RP_SEND_OOB_READ;
+		goto again;
+
+	case MSB_RP_SEND_READ_STATUS_REG:
+		 /* read the status register to understand source of the INT_ERR */
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, status),
+			sizeof(struct ms_status_register)))
+			return 0;
+
+		msb->state = MSB_RP_RECEIVE_OOB_READ;
+		return 0;
+
+	case MSB_RP_RECIVE_STATUS_REG:
+		msb->regs.status = *(struct ms_status_register *)mrq->data;
+		msb->state = MSB_RP_SEND_OOB_READ;
+		/* fallthrough */
+
+	case MSB_RP_SEND_OOB_READ:
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register)))
+			return 0;
+
+		msb->state = MSB_RP_RECEIVE_OOB_READ;
+		return 0;
+
+	case MSB_RP_RECEIVE_OOB_READ:
+		msb->regs.extra_data =
+			*(struct ms_extra_data_register *) mrq->data;
+		msb->state = MSB_RP_SEND_READ_DATA;
+		/* fallthrough */
+
+	case MSB_RP_SEND_READ_DATA:
+		/* Skip that state if we only read the oob */
+		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
+			msb->state = MSB_RP_RECEIVE_READ_DATA;
+			goto again;
+		}
+
+		sg_init_table(sg, ARRAY_SIZE(sg));
+		msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size);
+
+		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
+		msb->state = MSB_RP_RECEIVE_READ_DATA;
+		return 0;
+
+	case MSB_RP_RECEIVE_READ_DATA:
+		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, 0);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
+			dbg("read_page: uncorrectable error");
+			return msb_exit_state_machine(msb, -EBADMSG);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
+			dbg("read_page: correctable error");
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, -EUCLEAN);
+		} else {
+			dbg("read_page: INT error, but no status error bits");
+			return msb_exit_state_machine(msb, -EIO);
+		}
+	}
+
+	BUG();
+}
+
+/*
+ * Handler of writes of exactly one block.
+ * Takes address from msb->regs.param.
+ * Writes same extra data to blocks, also taken
+ * from msb->regs.extra
+ * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
+ * device refuses to take the command or something else
+ */
+static int h_msb_write_block(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 intreg, command;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+again:
+	switch (msb->state) {
+
+	/* HACK: Jmicon handling of TPCs between 8 and
+	 *	sizeof(memstick_request.data) is broken due to hardware
+	 *	bug in PIO mode that is used for these TPCs
+	 *	Therefore split the write
+	 */
+
+	case MSB_WB_SEND_WRITE_PARAMS:
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_WB_SEND_WRITE_OOB;
+		return 0;
+
+	case MSB_WB_SEND_WRITE_OOB:
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+		msb->state = MSB_WB_SEND_WRITE_COMMAND;
+		return 0;
+
+
+	case MSB_WB_SEND_WRITE_COMMAND:
+		command = MS_CMD_BLOCK_WRITE;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		msb->state = MSB_WB_SEND_INT_REQ;
+		return 0;
+
+	case MSB_WB_SEND_INT_REQ:
+		msb->state = MSB_WB_RECEIVE_INT_REQ;
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_WB_RECEIVE_INT_REQ:
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		/* errors mean out of here, and fast... */
+		if (intreg & (MEMSTICK_INT_CMDNAK))
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+
+		/* for last page we need to poll CED */
+		if (msb->current_page == msb->pages_in_block) {
+			if (intreg & MEMSTICK_INT_CED)
+				return msb_exit_state_machine(msb, 0);
+			msb->state = MSB_WB_SEND_INT_REQ;
+			goto again;
+
+		}
+
+		/* for non-last page we need BREQ before writing next chunk */
+		if (!(intreg & MEMSTICK_INT_BREQ)) {
+			msb->state = MSB_WB_SEND_INT_REQ;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state = MSB_WB_SEND_WRITE_DATA;
+		/* fallthrough */
+
+	case MSB_WB_SEND_WRITE_DATA:
+		sg_init_table(sg, ARRAY_SIZE(sg));
+
+		if (msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size) < msb->page_size)
+			return msb_exit_state_machine(msb, -EIO);
+
+		memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
+		mrq->need_card_int = 1;
+		msb->state = MSB_WB_RECEIVE_WRITE_CONFIRMATION;
+		return 0;
+
+	case MSB_WB_RECEIVE_WRITE_CONFIRMATION:
+		msb->current_page++;
+		msb->current_sg_offset += msb->page_size;
+		msb->state = MSB_WB_SEND_INT_REQ;
+		goto again;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
+/*
+ * This function is used to send simple IO requests to device that consist
+ * of register write + command
+ */
+static int h_msb_send_command(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	u8 intreg;
+
+	if (mrq->error) {
+		dbg("send_command: unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+
+	/* HACK: see h_msb_write_block */
+	case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+		msb->state = MSB_SC_SEND_WRITE_OOB;
+		return 0;
+
+	case MSB_SC_SEND_WRITE_OOB:
+		if (!msb->command_need_oob) {
+			msb->state = MSB_SC_SEND_COMMAND;
+			goto again;
+		}
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+
+		msb->state = MSB_SC_SEND_COMMAND;
+		return 0;
+
+	case MSB_SC_SEND_COMMAND:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
+		msb->state = MSB_SC_SEND_INT_REQ;
+		return 0;
+
+	case MSB_SC_SEND_INT_REQ:
+		msb->state = MSB_SC_RECEIVE_INT_REQ;
+		if (msb_read_int_reg(msb, -1))
+			return 0;
+		/* fallthrough */
+
+	case MSB_SC_RECEIVE_INT_REQ:
+		intreg = mrq->data[0];
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state = MSB_SC_SEND_INT_REQ;
+			goto again;
+		}
+
+		return msb_exit_state_machine(msb, 0);
+	}
+
+	BUG();
+}
+
+/* Small handler for card reset */
+static int h_msb_reset(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	u8 command = MS_CMD_RESET;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+	switch (msb->state) {
+	case MSB_RS_SEND:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		mrq->need_card_int = 0;
+		msb->state = MSB_RS_CONFIRM;
+		return 0;
+	case MSB_RS_CONFIRM:
+		return msb_exit_state_machine(msb, 0);
+	}
+	BUG();
+}
+
+/* This handler is used to do serial->parallel switch */
+static int h_msb_parallel_switch(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct memstick_host *host = card->host;
+
+	if (mrq->error) {
+		dbg("parallel_switch: error");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+
+	switch (msb->state) {
+	case MSB_PS_SEND_SWITCH_COMMAND:
+		/* Set the parallel interface on memstick side */
+		msb->regs.param.system |= MEMSTICK_SYS_PAM;
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			1,
+			(unsigned char *)&msb->regs.param))
+			return 0;
+
+		msb->state = MSB_PS_SWICH_HOST;
+		return 0;
+
+	case MSB_PS_SWICH_HOST:
+		 /* Set parallel interface on our side + send a dummy request
+			to see if card responds */
+		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		msb->state = MSB_PS_CONFIRM;
+		return 0;
+
+	case MSB_PS_CONFIRM:
+		return msb_exit_state_machine(msb, 0);
+	}
+
+	BUG();
+}
+
+static int msb_switch_to_parallel(struct msb_data *msb);
+
+/* Reset the card, to guard against hw errors beeing treated as bad blocks */
+static int msb_reset(struct msb_data *msb, bool full)
+{
+
+	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
+	struct memstick_dev *card = msb->card;
+	struct memstick_host *host = card->host;
+	int error;
+
+	/* Reset the card */
+	msb->regs.param.system = MEMSTICK_SYS_BAMD;
+
+	if (full) {
+		error =  host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+		if (error)
+			goto out_error;
+
+		msb_invalidate_reg_window(msb);
+
+		error = host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_ON);
+		if (error)
+			goto out_error;
+
+		error = host->set_param(host,
+					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+		if (error) {
+out_error:
+			dbg("Failed to reset the host controller");
+			msb->read_only = true;
+			return -EFAULT;
+		}
+	}
+
+	error = msb_run_state_machine(msb, h_msb_reset);
+	if (error) {
+		dbg("Failed to reset the card");
+		msb->read_only = true;
+		return -ENODEV;
+	}
+
+	/* Set parallel mode */
+	if (was_parallel)
+		msb_switch_to_parallel(msb);
+	return 0;
+}
+
+/* Attempts to switch interface to parallel mode */
+static int msb_switch_to_parallel(struct msb_data *msb)
+{
+	int error;
+
+	error = msb_run_state_machine(msb, h_msb_parallel_switch);
+	if (error) {
+		pr_err("Switch to parallel failed");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		msb_reset(msb, true);
+		return -EFAULT;
+	}
+
+	msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
+	return 0;
+}
+
+/* Changes overwrite flag on a page */
+static int msb_set_overwrite_flag(struct msb_data *msb,
+						u16 pba, u8 page, u8 flag)
+{
+	if (msb->read_only)
+		return -EROFS;
+
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_OVERWRITE;
+	msb->regs.extra_data.overwrite_flag = flag;
+	msb->command_value = MS_CMD_BLOCK_WRITE;
+	msb->command_need_oob = true;
+
+	dbg_verbose("changing overwrite flag to %02x for sector %d, page %d",
+							flag, pba, page);
+	return msb_run_state_machine(msb, h_msb_send_command);
+}
+
+static int msb_mark_bad(struct msb_data *msb, int pba)
+{
+	pr_notice("marking pba %d as bad", pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(
+			msb, pba, 0, 0xFF & ~MEMSTICK_OVERWRITE_BKST);
+}
+
+static int msb_mark_page_bad(struct msb_data *msb, int pba, int page)
+{
+	dbg("marking page %d of pba %d as bad", page, pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(msb,
+		pba, page, ~MEMSTICK_OVERWRITE_PGST0);
+}
+
+/* Erases one physical block */
+static int msb_erase_block(struct msb_data *msb, u16 pba)
+{
+	int error, try;
+	if (msb->read_only)
+		return -EROFS;
+
+	dbg_verbose("erasing pba %d", pba);
+
+	for (try = 1; try < 3; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = 0;
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->command_value = MS_CMD_BLOCK_ERASE;
+		msb->command_need_oob = false;
+
+
+		error = msb_run_state_machine(msb, h_msb_send_command);
+		if (!error || msb_reset(msb, true))
+			break;
+	}
+
+	if (error) {
+		pr_err("erase failed, marking pba %d as bad", pba);
+		msb_mark_bad(msb, pba);
+	}
+
+	dbg_verbose("erase success, marking pba %d as unused", pba);
+	msb_mark_block_unused(msb, pba);
+	__set_bit(pba, msb->erased_blocks_bitmap);
+	return error;
+}
+
+/* Reads one page from device */
+static int msb_read_page(struct msb_data *msb,
+	u16 pba, u8 page, struct ms_extra_data_register *extra,
+					struct scatterlist *sg,  int offset)
+{
+	int try, error;
+
+	if (pba == MS_BLOCK_INVALID) {
+		unsigned long flags;
+		struct sg_mapping_iter miter;
+		size_t len = msb->page_size;
+
+		dbg_verbose("read unmapped sector. returning 0xFF");
+
+		local_irq_save(flags);
+		sg_miter_start(&miter, sg, sg_nents(sg),
+				SG_MITER_ATOMIC | SG_MITER_TO_SG);
+
+		while (sg_miter_next(&miter) && len > 0) {
+
+			int chunklen;
+
+			if (offset && offset >= miter.length) {
+				offset -= miter.length;
+				continue;
+			}
+
+			chunklen = min(miter.length - offset, len);
+			memset(miter.addr + offset, 0xFF, chunklen);
+			len -= chunklen;
+			offset = 0;
+		}
+
+		sg_miter_stop(&miter);
+		local_irq_restore(flags);
+
+		if (offset)
+			return -EFAULT;
+
+		if (extra)
+			memset(extra, 0xFF, sizeof(*extra));
+		return 0;
+	}
+
+	if (pba >= msb->block_count) {
+		pr_err("BUG: attempt to read beyond the end of the card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	for (try = 1; try < 3; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = page;
+		msb->regs.param.cp = MEMSTICK_CP_PAGE;
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		error = msb_run_state_machine(msb, h_msb_read_page);
+
+
+		if (error == -EUCLEAN) {
+			pr_notice("correctable error on pba %d, page %d",
+				pba, page);
+			error = 0;
+		}
+
+		if (!error && extra)
+			*extra = msb->regs.extra_data;
+
+		if (!error || msb_reset(msb, true))
+			break;
+
+	}
+
+	/* Mark bad pages */
+	if (error == -EBADMSG) {
+		pr_err("uncorrectable error on read of pba %d, page %d",
+			pba, page);
+
+		if (msb->regs.extra_data.overwrite_flag &
+					MEMSTICK_OVERWRITE_PGST0)
+			msb_mark_page_bad(msb, pba, page);
+		return -EBADMSG;
+	}
+
+	if (error)
+		pr_err("read of pba %d, page %d failed with error %d",
+			pba, page, error);
+	return error;
+}
+
+/* Reads oob of page only */
+static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
+	struct ms_extra_data_register *extra)
+{
+	int error;
+
+	BUG_ON(!extra);
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
+
+	if (pba > msb->block_count) {
+		pr_err("BUG: attempt to read beyond the end of card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	error = msb_run_state_machine(msb, h_msb_read_page);
+	*extra = msb->regs.extra_data;
+
+	if (error == -EUCLEAN) {
+		pr_notice("correctable error on pba %d, page %d",
+			pba, page);
+		return 0;
+	}
+
+	return error;
+}
+
+/* Reads a block and compares it with data contained in scatterlist orig_sg */
+static int msb_verify_block(struct msb_data *msb, u16 pba,
+				struct scatterlist *orig_sg,  int offset)
+{
+	struct scatterlist sg;
+	int page = 0, error;
+
+	sg_init_one(&sg, msb->block_buffer, msb->block_size);
+
+	while (page < msb->pages_in_block) {
+
+		error = msb_read_page(msb, pba, page,
+				NULL, &sg, page * msb->page_size);
+		if (error)
+			return error;
+		page++;
+	}
+
+	if (msb_sg_compare_to_buffer(orig_sg, offset,
+				msb->block_buffer, msb->block_size))
+		return -EIO;
+	return 0;
+}
+
+/* Writes exectly one block + oob */
+static int msb_write_block(struct msb_data *msb,
+			u16 pba, u32 lba, struct scatterlist *sg, int offset)
+{
+	int error, current_try = 1;
+	BUG_ON(sg->length < msb->page_size);
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (pba == MS_BLOCK_INVALID) {
+		pr_err(
+			"BUG: write: attempt to write MS_BLOCK_INVALID block");
+		return -EINVAL;
+	}
+
+	if (pba >= msb->block_count || lba >= msb->logical_block_count) {
+		pr_err(
+		"BUG: write: attempt to write beyond the end of device");
+		return -EINVAL;
+	}
+
+	if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+		pr_err("BUG: write: lba zone mismatch");
+		return -EINVAL;
+	}
+
+	if (pba == msb->boot_block_locations[0] ||
+		pba == msb->boot_block_locations[1]) {
+		pr_err("BUG: write: attempt to write to boot blocks!");
+		return -EINVAL;
+	}
+
+	while (1) {
+
+		if (msb->read_only)
+			return -EROFS;
+
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->regs.param.page_address = 0;
+		msb->regs.param.block_address = cpu_to_be16(pba);
+
+		msb->regs.extra_data.management_flag = 0xFF;
+		msb->regs.extra_data.overwrite_flag = 0xF8;
+		msb->regs.extra_data.logical_address = cpu_to_be16(lba);
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		msb->current_page = 0;
+
+		error = msb_run_state_machine(msb, h_msb_write_block);
+
+		/* Sector we just wrote to is assumed erased since its pba
+			was erased. If it wasn't erased, write will succeed
+			and will just clear the bits that were set in the block
+			thus test that what we have written,
+			matches what we expect.
+			We do trust the blocks that we erased */
+		if (!error && (verify_writes ||
+				!test_bit(pba, msb->erased_blocks_bitmap)))
+			error = msb_verify_block(msb, pba, sg, offset);
+
+		if (!error)
+			break;
+
+		if (current_try > 1 || msb_reset(msb, true))
+			break;
+
+		pr_err("write failed, trying to erase the pba %d", pba);
+		error = msb_erase_block(msb, pba);
+		if (error)
+			break;
+
+		current_try++;
+	}
+	return error;
+}
+
+/* Finds a free block for write replacement */
+static u16 msb_get_free_block(struct msb_data *msb, int zone)
+{
+	u16 pos;
+	int pba = zone * MS_BLOCKS_IN_ZONE;
+	int i;
+
+	get_random_bytes(&pos, sizeof(pos));
+
+	if (!msb->free_block_count[zone]) {
+		pr_err("NO free blocks in the zone %d, to use for a write, (media is WORN out) switching to RO mode", zone);
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	pos %= msb->free_block_count[zone];
+
+	dbg_verbose("have %d choices for a free block, selected randomally: %d",
+		msb->free_block_count[zone], pos);
+
+	pba = find_next_zero_bit(msb->used_blocks_bitmap,
+							msb->block_count, pba);
+	for (i = 0; i < pos; ++i)
+		pba = find_next_zero_bit(msb->used_blocks_bitmap,
+						msb->block_count, pba + 1);
+
+	dbg_verbose("result of the free blocks scan: pba %d", pba);
+
+	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
+		pr_err("BUG: cant get a free block");
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	msb_mark_block_used(msb, pba);
+	return pba;
+}
+
+static int msb_update_block(struct msb_data *msb, u16 lba,
+	struct scatterlist *sg, int offset)
+{
+	u16 pba, new_pba;
+	int error, try;
+
+	pba = msb->lba_to_pba_table[lba];
+	dbg_verbose("start of a block update at lba  %d, pba %d", lba, pba);
+
+	if (pba != MS_BLOCK_INVALID) {
+		dbg_verbose("setting the update flag on the block");
+		msb_set_overwrite_flag(msb, pba, 0,
+				0xFF & ~MEMSTICK_OVERWRITE_UDST);
+	}
+
+	for (try = 0; try < 3; try++) {
+		new_pba = msb_get_free_block(msb,
+			msb_get_zone_from_lba(lba));
+
+		if (new_pba == MS_BLOCK_INVALID) {
+			error = -EIO;
+			goto out;
+		}
+
+		dbg_verbose("block update: writing updated block to the pba %d",
+								new_pba);
+		error = msb_write_block(msb, new_pba, lba, sg, offset);
+		if (error == -EBADMSG) {
+			msb_mark_bad(msb, new_pba);
+			continue;
+		}
+
+		if (error)
+			goto out;
+
+		dbg_verbose("block update: erasing the old block");
+		msb_erase_block(msb, pba);
+		msb->lba_to_pba_table[lba] = new_pba;
+		return 0;
+	}
+out:
+	if (error) {
+		pr_err("block update error after %d tries,  switching to r/o mode", try);
+		msb->read_only = true;
+	}
+	return error;
+}
+
+/* Converts endiannes in the boot block for easy use */
+static void msb_fix_boot_page_endianness(struct ms_boot_page *p)
+{
+	p->header.block_id = be16_to_cpu(p->header.block_id);
+	p->header.format_reserved = be16_to_cpu(p->header.format_reserved);
+	p->entry.disabled_block.start_addr
+		= be32_to_cpu(p->entry.disabled_block.start_addr);
+	p->entry.disabled_block.data_size
+		= be32_to_cpu(p->entry.disabled_block.data_size);
+	p->entry.cis_idi.start_addr
+		= be32_to_cpu(p->entry.cis_idi.start_addr);
+	p->entry.cis_idi.data_size
+		= be32_to_cpu(p->entry.cis_idi.data_size);
+	p->attr.block_size = be16_to_cpu(p->attr.block_size);
+	p->attr.number_of_blocks = be16_to_cpu(p->attr.number_of_blocks);
+	p->attr.number_of_effective_blocks
+		= be16_to_cpu(p->attr.number_of_effective_blocks);
+	p->attr.page_size = be16_to_cpu(p->attr.page_size);
+	p->attr.memory_manufacturer_code
+		= be16_to_cpu(p->attr.memory_manufacturer_code);
+	p->attr.memory_device_code = be16_to_cpu(p->attr.memory_device_code);
+	p->attr.implemented_capacity
+		= be16_to_cpu(p->attr.implemented_capacity);
+	p->attr.controller_number = be16_to_cpu(p->attr.controller_number);
+	p->attr.controller_function = be16_to_cpu(p->attr.controller_function);
+}
+
+static int msb_read_boot_blocks(struct msb_data *msb)
+{
+	int pba = 0;
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	struct ms_boot_page *page;
+
+	msb->boot_block_locations[0] = MS_BLOCK_INVALID;
+	msb->boot_block_locations[1] = MS_BLOCK_INVALID;
+	msb->boot_block_count = 0;
+
+	dbg_verbose("Start of a scan for the boot blocks");
+
+	if (!msb->boot_page) {
+		page = kmalloc(sizeof(struct ms_boot_page)*2, GFP_KERNEL);
+		if (!page)
+			return -ENOMEM;
+
+		msb->boot_page = page;
+	} else
+		page = msb->boot_page;
+
+	msb->block_count = MS_BLOCK_MAX_BOOT_ADDR;
+
+	for (pba = 0; pba < MS_BLOCK_MAX_BOOT_ADDR; pba++) {
+
+		sg_init_one(&sg, page, sizeof(*page));
+		if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) {
+			dbg("boot scan: can't read pba %d", pba);
+			continue;
+		}
+
+		if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) {
+			dbg("managment flag doesn't indicate boot block %d",
+									pba);
+			continue;
+		}
+
+		if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) {
+			dbg("the pba at %d doesn' contain boot block ID", pba);
+			continue;
+		}
+
+		msb_fix_boot_page_endianness(page);
+		msb->boot_block_locations[msb->boot_block_count] = pba;
+
+		page++;
+		msb->boot_block_count++;
+
+		if (msb->boot_block_count == 2)
+			break;
+	}
+
+	if (!msb->boot_block_count) {
+		pr_err("media doesn't contain master page, aborting");
+		return -EIO;
+	}
+
+	dbg_verbose("End of scan for boot blocks");
+	return 0;
+}
+
+static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
+{
+	struct ms_boot_page *boot_block;
+	struct scatterlist sg;
+	u16 *buffer = NULL;
+	int offset = 0;
+	int i, error = 0;
+	int data_size, data_offset, page, page_offset, size_to_read;
+	u16 pba;
+
+	BUG_ON(block_nr > 1);
+	boot_block = &msb->boot_page[block_nr];
+	pba = msb->boot_block_locations[block_nr];
+
+	if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID)
+		return -EINVAL;
+
+	data_size = boot_block->entry.disabled_block.data_size;
+	data_offset = sizeof(struct ms_boot_page) +
+			boot_block->entry.disabled_block.start_addr;
+	if (!data_size)
+		return 0;
+
+	page = data_offset / msb->page_size;
+	page_offset = data_offset % msb->page_size;
+	size_to_read =
+		DIV_ROUND_UP(data_size + page_offset, msb->page_size) *
+			msb->page_size;
+
+	dbg("reading bad block of boot block at pba %d, offset %d len %d",
+		pba, data_offset, data_size);
+
+	buffer = kzalloc(size_to_read, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	/* Read the buffer */
+	sg_init_one(&sg, buffer, size_to_read);
+
+	while (offset < size_to_read) {
+		error = msb_read_page(msb, pba, page, NULL, &sg, offset);
+		if (error)
+			goto out;
+
+		page++;
+		offset += msb->page_size;
+
+		if (page == msb->pages_in_block) {
+			pr_err(
+			"bad block table extends beyond the boot block");
+			break;
+		}
+	}
+
+	/* Process the bad block table */
+	for (i = page_offset; i < data_size / sizeof(u16); i++) {
+
+		u16 bad_block = be16_to_cpu(buffer[i]);
+
+		if (bad_block >= msb->block_count) {
+			dbg("bad block table contains invalid block %d",
+								bad_block);
+			continue;
+		}
+
+		if (test_bit(bad_block, msb->used_blocks_bitmap))  {
+			dbg("duplicate bad block %d in the table",
+				bad_block);
+			continue;
+		}
+
+		dbg("block %d is marked as factory bad", bad_block);
+		msb_mark_block_used(msb, bad_block);
+	}
+out:
+	kfree(buffer);
+	return error;
+}
+
+static int msb_ftl_initialize(struct msb_data *msb)
+{
+	int i;
+
+	if (msb->ftl_initialized)
+		return 0;
+
+	msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE;
+	msb->logical_block_count = msb->zone_count * 496 - 2;
+
+	msb->used_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->erased_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->lba_to_pba_table =
+		kmalloc(msb->logical_block_count * sizeof(u16), GFP_KERNEL);
+
+	if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
+						!msb->erased_blocks_bitmap) {
+		kfree(msb->used_blocks_bitmap);
+		kfree(msb->lba_to_pba_table);
+		kfree(msb->erased_blocks_bitmap);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < msb->zone_count; i++)
+		msb->free_block_count[i] = MS_BLOCKS_IN_ZONE;
+
+	memset(msb->lba_to_pba_table, MS_BLOCK_INVALID,
+			msb->logical_block_count * sizeof(u16));
+
+	dbg("initial FTL tables created. Zone count = %d, Logical block count = %d",
+		msb->zone_count, msb->logical_block_count);
+
+	msb->ftl_initialized = true;
+	return 0;
+}
+
+static int msb_ftl_scan(struct msb_data *msb)
+{
+	u16 pba, lba, other_block;
+	u8 overwrite_flag, managment_flag, other_overwrite_flag;
+	int error;
+	struct ms_extra_data_register extra;
+	u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL);
+
+	if (!overwrite_flags)
+		return -ENOMEM;
+
+	dbg("Start of media scanning");
+	for (pba = 0; pba < msb->block_count; pba++) {
+
+		if (pba == msb->boot_block_locations[0] ||
+			pba == msb->boot_block_locations[1]) {
+			dbg_verbose("pba %05d -> [boot block]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		if (test_bit(pba, msb->used_blocks_bitmap)) {
+			dbg_verbose("pba %05d -> [factory bad]", pba);
+			continue;
+		}
+
+		memset(&extra, 0, sizeof(extra));
+		error = msb_read_oob(msb, pba, 0, &extra);
+
+		/* can't trust the page if we can't read the oob */
+		if (error == -EBADMSG) {
+			pr_notice(
+			"oob of pba %d damaged, will try to erase it", pba);
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		} else if (error) {
+			pr_err("unknown error %d on read of oob of pba %d - aborting",
+				error, pba);
+
+			kfree(overwrite_flags);
+			return error;
+		}
+
+		lba = be16_to_cpu(extra.logical_address);
+		managment_flag = extra.management_flag;
+		overwrite_flag = extra.overwrite_flag;
+		overwrite_flags[pba] = overwrite_flag;
+
+		/* Skip bad blocks */
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
+			dbg("pba %05d -> [BAD]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Skip system/drm blocks */
+		if ((managment_flag & MEMSTICK_MANAGMENT_FLAG_NORMAL) !=
+			MEMSTICK_MANAGMENT_FLAG_NORMAL) {
+			dbg("pba %05d -> [reserved managment flag %02x]",
+							pba, managment_flag);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Erase temporary tables */
+		if (!(managment_flag & MEMSTICK_MANAGEMENT_ATFLG)) {
+			dbg("pba %05d -> [temp table] - will erase", pba);
+
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		if (lba == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [free]", pba);
+			continue;
+		}
+
+		msb_mark_block_used(msb, pba);
+
+		/* Block has LBA not according to zoning*/
+		if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+			pr_notice("pba %05d -> [bad lba %05d] - will erase",
+								pba, lba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		/* No collisions - great */
+		if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [lba %05d]", pba, lba);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		other_block = msb->lba_to_pba_table[lba];
+		other_overwrite_flag = overwrite_flags[other_block];
+
+		pr_notice("Collision between pba %d and pba %d",
+			pba, other_block);
+
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			pr_notice("pba %d is marked as stable, use it", pba);
+			msb_erase_block(msb, other_block);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			pr_notice("pba %d is marked as stable, use it",
+								other_block);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		pr_notice("collision between blocks %d and %d, without stable flag set on both, erasing pba %d",
+				pba, other_block, other_block);
+
+		msb_erase_block(msb, other_block);
+		msb->lba_to_pba_table[lba] = pba;
+	}
+
+	dbg("End of media scanning");
+	kfree(overwrite_flags);
+	return 0;
+}
+
+static void msb_cache_flush_timer(unsigned long data)
+{
+	struct msb_data *msb = (struct msb_data *)data;
+	msb->need_flush_cache = true;
+	queue_work(msb->io_queue, &msb->io_work);
+}
+
+
+static void msb_cache_discard(struct msb_data *msb)
+{
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return;
+
+	del_timer_sync(&msb->cache_flush_timer);
+
+	dbg_verbose("Discarding the write cache");
+	msb->cache_block_lba = MS_BLOCK_INVALID;
+	bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block);
+}
+
+static int msb_cache_init(struct msb_data *msb)
+{
+	setup_timer(&msb->cache_flush_timer, msb_cache_flush_timer,
+		(unsigned long)msb);
+
+	if (!msb->cache)
+		msb->cache = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->cache)
+		return -ENOMEM;
+
+	msb_cache_discard(msb);
+	return 0;
+}
+
+static int msb_cache_flush(struct msb_data *msb)
+{
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	int page, offset, error;
+	u16 pba, lba;
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return 0;
+
+	lba = msb->cache_block_lba;
+	pba = msb->lba_to_pba_table[lba];
+
+	dbg_verbose("Flushing the write cache of pba %d (LBA %d)",
+						pba, msb->cache_block_lba);
+
+	sg_init_one(&sg, msb->cache , msb->block_size);
+
+	/* Read all missing pages in cache */
+	for (page = 0; page < msb->pages_in_block; page++) {
+
+		if (test_bit(page, &msb->valid_cache_bitmap))
+			continue;
+
+		offset = page * msb->page_size;
+
+		dbg_verbose("reading non-present sector %d of cache block %d",
+			page, lba);
+		error = msb_read_page(msb, pba, page, &extra, &sg, offset);
+
+		/* Bad pages are copied with 00 page status */
+		if (error == -EBADMSG) {
+			pr_err("read error on sector %d, contents probably damaged", page);
+			continue;
+		}
+
+		if (error)
+			return error;
+
+		if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) !=
+							MEMSTICK_OV_PG_NORMAL) {
+			dbg("page %d is marked as bad", page);
+			continue;
+		}
+
+		set_bit(page, &msb->valid_cache_bitmap);
+	}
+
+	/* Write the cache now */
+	error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
+	pba = msb->lba_to_pba_table[msb->cache_block_lba];
+
+	/* Mark invalid pages */
+	if (!error) {
+		for (page = 0; page < msb->pages_in_block; page++) {
+
+			if (test_bit(page, &msb->valid_cache_bitmap))
+				continue;
+
+			dbg("marking page %d as containing damaged data",
+				page);
+			msb_set_overwrite_flag(msb,
+				pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL);
+		}
+	}
+
+	msb_cache_discard(msb);
+	return error;
+}
+
+static int msb_cache_write(struct msb_data *msb, int lba,
+	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
+{
+	int error;
+	struct scatterlist sg_tmp[10];
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
+						lba != msb->cache_block_lba)
+		if (add_to_cache_only)
+			return 0;
+
+	/* If we need to write different block */
+	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
+						lba != msb->cache_block_lba) {
+		dbg_verbose("first flush the cache");
+		error = msb_cache_flush(msb);
+		if (error)
+			return error;
+	}
+
+	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
+		msb->cache_block_lba  = lba;
+		mod_timer(&msb->cache_flush_timer,
+			jiffies + msecs_to_jiffies(cache_flush_timeout));
+	}
+
+	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
+
+	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+	msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+
+	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
+		msb->cache + page * msb->page_size, msb->page_size);
+
+	set_bit(page, &msb->valid_cache_bitmap);
+	return 0;
+}
+
+static int msb_cache_read(struct msb_data *msb, int lba,
+				int page, struct scatterlist *sg, int offset)
+{
+	int pba = msb->lba_to_pba_table[lba];
+	struct scatterlist sg_tmp[10];
+	int error = 0;
+
+	if (lba == msb->cache_block_lba &&
+			test_bit(page, &msb->valid_cache_bitmap)) {
+
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from cache",
+							lba, pba, page);
+
+		sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+		msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp),
+			offset, msb->page_size);
+		sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp),
+			msb->cache + msb->page_size * page,
+							msb->page_size);
+	} else {
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from device",
+							lba, pba, page);
+
+		error = msb_read_page(msb, pba, page, NULL, sg, offset);
+		if (error)
+			return error;
+
+		msb_cache_write(msb, lba, page, true, sg, offset);
+	}
+	return error;
+}
+
+/* Emulated geometry table
+ * This table content isn't that importaint,
+ * One could put here different values, providing that they still
+ * cover whole disk.
+ * 64 MB entry is what windows reports for my 64M memstick */
+
+static const struct chs_entry chs_table[] = {
+/*        size sectors cylynders  heads */
+	{ 4,    16,    247,       2  },
+	{ 8,    16,    495,       2  },
+	{ 16,   16,    495,       4  },
+	{ 32,   16,    991,       4  },
+	{ 64,   16,    991,       8  },
+	{128,   16,    991,       16 },
+	{ 0 }
+};
+
+/* Load information about the card */
+static int msb_init_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	struct ms_boot_page *boot_block;
+	int error = 0, i, raw_size_in_megs;
+
+	msb->caps = 0;
+
+	if (card->id.class >= MEMSTICK_CLASS_ROM &&
+				card->id.class <= MEMSTICK_CLASS_ROM)
+		msb->read_only = true;
+
+	msb->state = -1;
+	error = msb_reset(msb, false);
+	if (error)
+		return error;
+
+	/* Due to a bug in Jmicron driver written by Alex Dubov,
+	 its serial mode barely works,
+	 so we switch to parallel mode right away */
+	if (host->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+
+	msb->page_size = sizeof(struct ms_boot_page);
+
+	/* Read the boot page */
+	error = msb_read_boot_blocks(msb);
+	if (error)
+		return -EIO;
+
+	boot_block = &msb->boot_page[0];
+
+	/* Save intersting attributes from boot page */
+	msb->block_count = boot_block->attr.number_of_blocks;
+	msb->page_size = boot_block->attr.page_size;
+
+	msb->pages_in_block = boot_block->attr.block_size * 2;
+	msb->block_size = msb->page_size * msb->pages_in_block;
+
+	if (msb->page_size > PAGE_SIZE) {
+		/* this isn't supported by linux at all, anyway*/
+		dbg("device page %d size isn't supported", msb->page_size);
+		return -EINVAL;
+	}
+
+	msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->block_buffer)
+		return -ENOMEM;
+
+	raw_size_in_megs = (msb->block_size * msb->block_count) >> 20;
+
+	for (i = 0; chs_table[i].size; i++) {
+
+		if (chs_table[i].size != raw_size_in_megs)
+			continue;
+
+		msb->geometry.cylinders = chs_table[i].cyl;
+		msb->geometry.heads = chs_table[i].head;
+		msb->geometry.sectors = chs_table[i].sec;
+		break;
+	}
+
+	if (boot_block->attr.transfer_supporting == 1)
+		msb->caps |= MEMSTICK_CAP_PAR4;
+
+	if (boot_block->attr.device_type & 0x03)
+		msb->read_only = true;
+
+	dbg("Total block count = %d", msb->block_count);
+	dbg("Each block consists of %d pages", msb->pages_in_block);
+	dbg("Page size = %d bytes", msb->page_size);
+	dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4));
+	dbg("Read only: %d", msb->read_only);
+
+#if 0
+	/* Now we can switch the interface */
+	if (host->caps & msb->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+#endif
+
+	error = msb_cache_init(msb);
+	if (error)
+		return error;
+
+	error = msb_ftl_initialize(msb);
+	if (error)
+		return error;
+
+
+	/* Read the bad block table */
+	error = msb_read_bad_block_table(msb, 0);
+
+	if (error && error != -ENOMEM) {
+		dbg("failed to read bad block table from primary boot block, trying from backup");
+		error = msb_read_bad_block_table(msb, 1);
+	}
+
+	if (error)
+		return error;
+
+	/* *drum roll* Scan the media */
+	error = msb_ftl_scan(msb);
+	if (error) {
+		pr_err("Scan of media failed");
+		return error;
+	}
+
+	return 0;
+
+}
+
+static int msb_do_write_request(struct msb_data *msb, int lba,
+	int page, struct scatterlist *sg, size_t len, int *sucessfuly_written)
+{
+	int error = 0;
+	off_t offset = 0;
+	*sucessfuly_written = 0;
+
+	while (offset < len) {
+		if (page == 0 && len - offset >= msb->block_size) {
+
+			if (msb->cache_block_lba == lba)
+				msb_cache_discard(msb);
+
+			dbg_verbose("Writing whole lba %d", lba);
+			error = msb_update_block(msb, lba, sg, offset);
+			if (error)
+				return error;
+
+			offset += msb->block_size;
+			*sucessfuly_written += msb->block_size;
+			lba++;
+			continue;
+		}
+
+		error = msb_cache_write(msb, lba, page, false, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_written += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static int msb_do_read_request(struct msb_data *msb, int lba,
+		int page, struct scatterlist *sg, int len, int *sucessfuly_read)
+{
+	int error = 0;
+	int offset = 0;
+	*sucessfuly_read = 0;
+
+	while (offset < len) {
+
+		error = msb_cache_read(msb, lba, page, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_read += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static void msb_io_work(struct work_struct *work)
+{
+	struct msb_data *msb = container_of(work, struct msb_data, io_work);
+	int page, error, len;
+	sector_t lba;
+	unsigned long flags;
+	struct scatterlist *sg = msb->prealloc_sg;
+
+	dbg_verbose("IO: work started");
+
+	while (1) {
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (msb->need_flush_cache) {
+			msb->need_flush_cache = false;
+			spin_unlock_irqrestore(&msb->q_lock, flags);
+			msb_cache_flush(msb);
+			continue;
+		}
+
+		if (!msb->req) {
+			msb->req = blk_fetch_request(msb->queue);
+			if (!msb->req) {
+				dbg_verbose("IO: no more requests exiting");
+				spin_unlock_irqrestore(&msb->q_lock, flags);
+				return;
+			}
+		}
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+
+		/* If card was removed meanwhile */
+		if (!msb->req)
+			return;
+
+		/* process the request */
+		dbg_verbose("IO: processing new request");
+		blk_rq_map_sg(msb->queue, msb->req, sg);
+
+		lba = blk_rq_pos(msb->req);
+
+		sector_div(lba, msb->page_size / 512);
+		page = do_div(lba, msb->pages_in_block);
+
+		if (rq_data_dir(msb->req) == READ)
+			error = msb_do_read_request(msb, lba, page, sg,
+				blk_rq_bytes(msb->req), &len);
+		else
+			error = msb_do_write_request(msb, lba, page, sg,
+				blk_rq_bytes(msb->req), &len);
+
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (len)
+			if (!__blk_end_request(msb->req, 0, len))
+				msb->req = NULL;
+
+		if (error && msb->req) {
+			dbg_verbose("IO: ending one sector of the request with error");
+			if (!__blk_end_request(msb->req, error, msb->page_size))
+				msb->req = NULL;
+		}
+
+		if (msb->req)
+			dbg_verbose("IO: request still pending");
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+}
+
+static DEFINE_IDR(msb_disk_idr); /*set of used disk numbers */
+static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */
+
+static int msb_bd_open(struct block_device *bdev, fmode_t mode)
+{
+	struct gendisk *disk = bdev->bd_disk;
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device open");
+
+	mutex_lock(&msb_disk_lock);
+
+	if (msb && msb->card)
+		msb->usage_count++;
+
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static void msb_data_clear(struct msb_data *msb)
+{
+	kfree(msb->boot_page);
+	kfree(msb->used_blocks_bitmap);
+	kfree(msb->lba_to_pba_table);
+	kfree(msb->cache);
+	msb->card = NULL;
+}
+
+static int msb_disk_release(struct gendisk *disk)
+{
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device release");
+	mutex_lock(&msb_disk_lock);
+
+	if (msb) {
+		if (msb->usage_count)
+			msb->usage_count--;
+
+		if (!msb->usage_count) {
+			disk->private_data = NULL;
+			idr_remove(&msb_disk_idr, msb->disk_id);
+			put_disk(disk);
+			kfree(msb);
+		}
+	}
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static void msb_bd_release(struct gendisk *disk, fmode_t mode)
+{
+	msb_disk_release(disk);
+}
+
+static int msb_bd_getgeo(struct block_device *bdev,
+				 struct hd_geometry *geo)
+{
+	struct msb_data *msb = bdev->bd_disk->private_data;
+	*geo = msb->geometry;
+	return 0;
+}
+
+static int msb_prepare_req(struct request_queue *q, struct request *req)
+{
+	if (req->cmd_type != REQ_TYPE_FS &&
+				req->cmd_type != REQ_TYPE_BLOCK_PC) {
+		blk_dump_rq_flags(req, "MS unsupported request");
+		return BLKPREP_KILL;
+	}
+	req->cmd_flags |= REQ_DONTPREP;
+	return BLKPREP_OK;
+}
+
+static void msb_submit_req(struct request_queue *q)
+{
+	struct memstick_dev *card = q->queuedata;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct request *req = NULL;
+
+	dbg_verbose("Submit request");
+
+	if (msb->card_dead) {
+		dbg("Refusing requests on removed card");
+
+		WARN_ON(!msb->io_queue_stopped);
+
+		while ((req = blk_fetch_request(q)) != NULL)
+			__blk_end_request_all(req, -ENODEV);
+		return;
+	}
+
+	if (msb->req)
+		return;
+
+	if (!msb->io_queue_stopped)
+		queue_work(msb->io_queue, &msb->io_work);
+}
+
+static int msb_check_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	return (msb->card_dead == 0);
+}
+
+static void msb_stop(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	dbg("Stopping all msblock IO");
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_stop_queue(msb->queue);
+	msb->io_queue_stopped = true;
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	del_timer_sync(&msb->cache_flush_timer);
+	flush_workqueue(msb->io_queue);
+
+	if (msb->req) {
+		spin_lock_irqsave(&msb->q_lock, flags);
+		blk_requeue_request(msb->queue, msb->req);
+		msb->req = NULL;
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+
+}
+
+static void msb_start(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	dbg("Resuming IO from msblock");
+
+	msb_invalidate_reg_window(msb);
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	if (!msb->io_queue_stopped || msb->card_dead) {
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Kick cache flush anyway, its harmless */
+	msb->need_flush_cache = true;
+	msb->io_queue_stopped = false;
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	queue_work(msb->io_queue, &msb->io_work);
+
+}
+
+static const struct block_device_operations msb_bdops = {
+	.open    = msb_bd_open,
+	.release = msb_bd_release,
+	.getgeo  = msb_bd_getgeo,
+	.owner   = THIS_MODULE
+};
+
+/* Registers the block device */
+static int msb_init_disk(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	int rc;
+	u64 limit = BLK_BOUNCE_HIGH;
+	unsigned long capacity;
+
+	if (host->dev.dma_mask && *(host->dev.dma_mask))
+		limit = *(host->dev.dma_mask);
+
+	mutex_lock(&msb_disk_lock);
+	msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL);
+	mutex_unlock(&msb_disk_lock);
+
+	if (msb->disk_id  < 0)
+		return msb->disk_id;
+
+	msb->disk = alloc_disk(0);
+	if (!msb->disk) {
+		rc = -ENOMEM;
+		goto out_release_id;
+	}
+
+	msb->queue = blk_init_queue(msb_submit_req, &msb->q_lock);
+	if (!msb->queue) {
+		rc = -ENOMEM;
+		goto out_put_disk;
+	}
+
+	msb->queue->queuedata = card;
+	blk_queue_prep_rq(msb->queue, msb_prepare_req);
+
+	blk_queue_bounce_limit(msb->queue, limit);
+	blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
+	blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
+	blk_queue_max_segment_size(msb->queue,
+				   MS_BLOCK_MAX_PAGES * msb->page_size);
+	blk_queue_logical_block_size(msb->queue, msb->page_size);
+
+	sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
+	msb->disk->fops = &msb_bdops;
+	msb->disk->private_data = msb;
+	msb->disk->queue = msb->queue;
+	msb->disk->driverfs_dev = &card->dev;
+	msb->disk->flags |= GENHD_FL_EXT_DEVT;
+
+	capacity = msb->pages_in_block * msb->logical_block_count;
+	capacity *= (msb->page_size / 512);
+	set_capacity(msb->disk, capacity);
+	dbg("Set total disk size to %lu sectors", capacity);
+
+	msb->usage_count = 1;
+	msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
+	INIT_WORK(&msb->io_work, msb_io_work);
+	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
+
+	if (msb->read_only)
+		set_disk_ro(msb->disk, 1);
+
+	msb_start(card);
+	add_disk(msb->disk);
+	dbg("Disk added");
+	return 0;
+
+out_put_disk:
+	put_disk(msb->disk);
+out_release_id:
+	mutex_lock(&msb_disk_lock);
+	idr_remove(&msb_disk_idr, msb->disk_id);
+	mutex_unlock(&msb_disk_lock);
+	return rc;
+}
+
+static int msb_probe(struct memstick_dev *card)
+{
+	struct msb_data *msb;
+	int rc = 0;
+
+	msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!msb)
+		return -ENOMEM;
+	memstick_set_drvdata(card, msb);
+	msb->card = card;
+	spin_lock_init(&msb->q_lock);
+
+	rc = msb_init_card(card);
+	if (rc)
+		goto out_free;
+
+	rc = msb_init_disk(card);
+	if (!rc) {
+		card->check = msb_check_card;
+		card->stop = msb_stop;
+		card->start = msb_start;
+		return 0;
+	}
+out_free:
+	memstick_set_drvdata(card, NULL);
+	msb_data_clear(msb);
+	kfree(msb);
+	return rc;
+}
+
+static void msb_remove(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	if (!msb->io_queue_stopped)
+		msb_stop(card);
+
+	dbg("Removing the disk device");
+
+	/* Take care of unhandled + new requests from now on */
+	spin_lock_irqsave(&msb->q_lock, flags);
+	msb->card_dead = true;
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Remove the disk */
+	del_gendisk(msb->disk);
+	blk_cleanup_queue(msb->queue);
+	msb->queue = NULL;
+
+	mutex_lock(&msb_disk_lock);
+	msb_data_clear(msb);
+	mutex_unlock(&msb_disk_lock);
+
+	msb_disk_release(msb->disk);
+	memstick_set_drvdata(card, NULL);
+}
+
+#ifdef CONFIG_PM
+
+static int msb_suspend(struct memstick_dev *card, pm_message_t state)
+{
+	msb_stop(card);
+	return 0;
+}
+
+static int msb_resume(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct msb_data *new_msb = NULL;
+	bool card_dead = true;
+
+#ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
+	msb->card_dead = true;
+	return 0;
+#endif
+	mutex_lock(&card->host->lock);
+
+	new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!new_msb)
+		goto out;
+
+	new_msb->card = card;
+	memstick_set_drvdata(card, new_msb);
+	spin_lock_init(&new_msb->q_lock);
+	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
+
+	if (msb_init_card(card))
+		goto out;
+
+	if (msb->block_size != new_msb->block_size)
+		goto out;
+
+	if (memcmp(msb->boot_page, new_msb->boot_page,
+					sizeof(struct ms_boot_page)))
+		goto out;
+
+	if (msb->logical_block_count != new_msb->logical_block_count ||
+		memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
+						msb->logical_block_count))
+		goto out;
+
+	if (msb->block_count != new_msb->block_count ||
+		memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
+							msb->block_count / 8))
+		goto out;
+
+	card_dead = false;
+out:
+	if (card_dead)
+		dbg("Card was removed/replaced during suspend");
+
+	msb->card_dead = card_dead;
+	memstick_set_drvdata(card, msb);
+
+	if (new_msb) {
+		msb_data_clear(new_msb);
+		kfree(new_msb);
+	}
+
+	msb_start(card);
+	mutex_unlock(&card->host->lock);
+	return 0;
+}
+#else
+
+#define msb_suspend NULL
+#define msb_resume NULL
+
+#endif /* CONFIG_PM */
+
+static struct memstick_device_id msb_id_tbl[] = {
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_FLASH},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_ROM},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_RO},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_WP},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
+	 MEMSTICK_CLASS_DUO},
+	{}
+};
+MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
+
+
+static struct memstick_driver msb_driver = {
+	.driver = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE
+	},
+	.id_table = msb_id_tbl,
+	.probe    = msb_probe,
+	.remove   = msb_remove,
+	.suspend  = msb_suspend,
+	.resume   = msb_resume
+};
+
+static int major;
+
+static int __init msb_init(void)
+{
+	int rc = register_blkdev(0, DRIVER_NAME);
+
+	if (rc < 0) {
+		pr_err("failed to register major (error %d)\n", rc);
+		return rc;
+	}
+
+	major = rc;
+	rc = memstick_register_driver(&msb_driver);
+	if (rc) {
+		unregister_blkdev(major, DRIVER_NAME);
+		pr_err("failed to register memstick driver (error %d)\n", rc);
+	}
+
+	return rc;
+}
+
+static void __exit msb_exit(void)
+{
+	memstick_unregister_driver(&msb_driver);
+	unregister_blkdev(major, DRIVER_NAME);
+	idr_destroy(&msb_disk_idr);
+}
+
+module_init(msb_init);
+module_exit(msb_exit);
+
+module_param(cache_flush_timeout, int, S_IRUGO);
+MODULE_PARM_DESC(cache_flush_timeout,
+				"Cache flush timeout in msec (1000 default)");
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+module_param(verify_writes, bool, S_IRUGO);
+MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Maxim Levitsky");
+MODULE_DESCRIPTION("Sony MemoryStick block device driver");
diff --git a/drivers/memstick/core/ms_block.h b/drivers/memstick/core/ms_block.h
new file mode 100644
index 0000000..96e6375
--- /dev/null
+++ b/drivers/memstick/core/ms_block.h
@@ -0,0 +1,290 @@
+/*
+ *  ms_block.h - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2013 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver are copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ * Also ms structures were copied from old broken driver by same author
+ * These probably come from MS spec
+ *
+ */
+
+#ifndef MS_BLOCK_NEW_H
+#define MS_BLOCK_NEW_H
+
+#define MS_BLOCK_MAX_SEGS      32
+#define MS_BLOCK_MAX_PAGES     ((2 << 16) - 1)
+
+#define MS_BLOCK_MAX_BOOT_ADDR 0x000c
+#define MS_BLOCK_BOOT_ID       0x0001
+#define MS_BLOCK_INVALID       0xffff
+#define MS_MAX_ZONES           16
+#define MS_BLOCKS_IN_ZONE      512
+
+#define MS_BLOCK_MAP_LINE_SZ   16
+#define MS_BLOCK_PART_SHIFT    3
+
+
+#define MEMSTICK_UNCORR_ERROR (MEMSTICK_STATUS1_UCFG | \
+		MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCDT)
+
+#define MEMSTICK_CORR_ERROR (MEMSTICK_STATUS1_FGER | MEMSTICK_STATUS1_EXER | \
+	MEMSTICK_STATUS1_DTER)
+
+#define MEMSTICK_INT_ERROR (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)
+
+#define MEMSTICK_OVERWRITE_FLAG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | \
+	MEMSTICK_OVERWRITE_PGST0  | \
+	MEMSTICK_OVERWRITE_BKST)
+
+#define MEMSTICK_OV_PG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | MEMSTICK_OVERWRITE_PGST0)
+
+#define MEMSTICK_MANAGMENT_FLAG_NORMAL \
+	(MEMSTICK_MANAGEMENT_SYSFLG |  \
+	MEMSTICK_MANAGEMENT_SCMS1   |  \
+	MEMSTICK_MANAGEMENT_SCMS0)     \
+
+struct ms_boot_header {
+	unsigned short block_id;
+	unsigned short format_reserved;
+	unsigned char  reserved0[184];
+	unsigned char  data_entry;
+	unsigned char  reserved1[179];
+} __packed;
+
+
+struct ms_system_item {
+	unsigned int  start_addr;
+	unsigned int  data_size;
+	unsigned char data_type_id;
+	unsigned char reserved[3];
+} __packed;
+
+struct ms_system_entry {
+	struct ms_system_item disabled_block;
+	struct ms_system_item cis_idi;
+	unsigned char         reserved[24];
+} __packed;
+
+struct ms_boot_attr_info {
+	unsigned char      memorystick_class;
+	unsigned char      format_unique_value1;
+	unsigned short     block_size;
+	unsigned short     number_of_blocks;
+	unsigned short     number_of_effective_blocks;
+	unsigned short     page_size;
+	unsigned char      extra_data_size;
+	unsigned char      format_unique_value2;
+	unsigned char      assembly_time[8];
+	unsigned char      format_unique_value3;
+	unsigned char      serial_number[3];
+	unsigned char      assembly_manufacturer_code;
+	unsigned char      assembly_model_code[3];
+	unsigned short     memory_manufacturer_code;
+	unsigned short     memory_device_code;
+	unsigned short     implemented_capacity;
+	unsigned char      format_unique_value4[2];
+	unsigned char      vcc;
+	unsigned char      vpp;
+	unsigned short     controller_number;
+	unsigned short     controller_function;
+	unsigned char      reserved0[9];
+	unsigned char      transfer_supporting;
+	unsigned short     format_unique_value5;
+	unsigned char      format_type;
+	unsigned char      memorystick_application;
+	unsigned char      device_type;
+	unsigned char      reserved1[22];
+	unsigned char      format_uniqure_value6[2];
+	unsigned char      reserved2[15];
+} __packed;
+
+struct ms_cis_idi {
+	unsigned short general_config;
+	unsigned short logical_cylinders;
+	unsigned short reserved0;
+	unsigned short logical_heads;
+	unsigned short track_size;
+	unsigned short page_size;
+	unsigned short pages_per_track;
+	unsigned short msw;
+	unsigned short lsw;
+	unsigned short reserved1;
+	unsigned char  serial_number[20];
+	unsigned short buffer_type;
+	unsigned short buffer_size_increments;
+	unsigned short long_command_ecc;
+	unsigned char  firmware_version[28];
+	unsigned char  model_name[18];
+	unsigned short reserved2[5];
+	unsigned short pio_mode_number;
+	unsigned short dma_mode_number;
+	unsigned short field_validity;
+	unsigned short current_logical_cylinders;
+	unsigned short current_logical_heads;
+	unsigned short current_pages_per_track;
+	unsigned int   current_page_capacity;
+	unsigned short mutiple_page_setting;
+	unsigned int   addressable_pages;
+	unsigned short single_word_dma;
+	unsigned short multi_word_dma;
+	unsigned char  reserved3[128];
+} __packed;
+
+
+struct ms_boot_page {
+	struct ms_boot_header    header;
+	struct ms_system_entry   entry;
+	struct ms_boot_attr_info attr;
+} __packed;
+
+struct msb_data {
+	unsigned int			usage_count;
+	struct memstick_dev		*card;
+	struct gendisk			*disk;
+	struct request_queue		*queue;
+	spinlock_t			q_lock;
+	struct hd_geometry		geometry;
+	struct attribute_group		attr_group;
+	struct request			*req;
+	int				caps;
+	int				disk_id;
+
+	/* IO */
+	struct workqueue_struct		*io_queue;
+	bool				io_queue_stopped;
+	struct work_struct		io_work;
+	bool				card_dead;
+
+	/* Media properties */
+	struct ms_boot_page		*boot_page;
+	u16				boot_block_locations[2];
+	int				boot_block_count;
+
+	bool				read_only;
+	unsigned short			page_size;
+	int				block_size;
+	int				pages_in_block;
+	int				zone_count;
+	int				block_count;
+	int				logical_block_count;
+
+	/* FTL tables */
+	unsigned long			*used_blocks_bitmap;
+	unsigned long			*erased_blocks_bitmap;
+	u16				*lba_to_pba_table;
+	int				free_block_count[MS_MAX_ZONES];
+	bool				ftl_initialized;
+
+	/* Cache */
+	unsigned char			*cache;
+	unsigned long			valid_cache_bitmap;
+	int				cache_block_lba;
+	bool				need_flush_cache;
+	struct timer_list		cache_flush_timer;
+
+	/* Preallocated buffers */
+	unsigned char			*block_buffer;
+	struct scatterlist		prealloc_sg[MS_BLOCK_MAX_SEGS+1];
+
+
+	/* handler's local data */
+	struct ms_register_addr		reg_addr;
+	bool				addr_valid;
+
+	u8				command_value;
+	bool				command_need_oob;
+	struct scatterlist		*current_sg;
+	int				current_sg_offset;
+
+	struct ms_register		regs;
+	int				current_page;
+
+	int				state;
+	int				exit_error;
+	bool				int_polling;
+	unsigned long			int_timeout;
+
+};
+
+enum msb_readpage_states {
+	MSB_RP_SEND_BLOCK_ADDRESS = 0,
+	MSB_RP_SEND_READ_COMMAND,
+
+	MSB_RP_SEND_INT_REQ,
+	MSB_RP_RECEIVE_INT_REQ_RESULT,
+
+	MSB_RP_SEND_READ_STATUS_REG,
+	MSB_RP_RECIVE_STATUS_REG,
+
+	MSB_RP_SEND_OOB_READ,
+	MSB_RP_RECEIVE_OOB_READ,
+
+	MSB_RP_SEND_READ_DATA,
+	MSB_RP_RECEIVE_READ_DATA,
+};
+
+enum msb_write_block_states {
+	MSB_WB_SEND_WRITE_PARAMS = 0,
+	MSB_WB_SEND_WRITE_OOB,
+	MSB_WB_SEND_WRITE_COMMAND,
+
+	MSB_WB_SEND_INT_REQ,
+	MSB_WB_RECEIVE_INT_REQ,
+
+	MSB_WB_SEND_WRITE_DATA,
+	MSB_WB_RECEIVE_WRITE_CONFIRMATION,
+};
+
+enum msb_send_command_states {
+	MSB_SC_SEND_WRITE_PARAMS,
+	MSB_SC_SEND_WRITE_OOB,
+	MSB_SC_SEND_COMMAND,
+
+	MSB_SC_SEND_INT_REQ,
+	MSB_SC_RECEIVE_INT_REQ,
+
+};
+
+enum msb_reset_states {
+	MSB_RS_SEND,
+	MSB_RS_CONFIRM,
+};
+
+enum msb_par_switch_states {
+	MSB_PS_SEND_SWITCH_COMMAND,
+	MSB_PS_SWICH_HOST,
+	MSB_PS_CONFIRM,
+};
+
+struct chs_entry {
+	unsigned long size;
+	unsigned char sec;
+	unsigned short cyl;
+	unsigned char head;
+};
+
+static int msb_reset(struct msb_data *msb, bool full);
+
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq);
+
+#define __dbg(level, format, ...) \
+	do { \
+		if (debug >= level) \
+			pr_err(format "\n", ## __VA_ARGS__); \
+	} while (0)
+
+
+#define dbg(format, ...)		__dbg(1, format, ## __VA_ARGS__)
+#define dbg_verbose(format, ...)	__dbg(2, format, ## __VA_ARGS__)
+
+#endif
-- 
1.7.9.5

Re: [PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

Any update?


Best regards,
Maxim Levitsky


On Mon, 2013-07-08 at 23:54 +0300, Maxim Levitsky wrote: 
> Based partially on MS standard spec quotes from Alex Dubov.
> 
> As any code that works with user data this driver isn't
> recommended to use to write cards that contain valuable data.
> 
> It tries its best though to avoid data corruption
> and possible damage to the card.
> 
> Tested on MS DUO 64 MB card on Ricoh R592 card reader.
> 
> Signed-off-by: Maxim Levitsky <maximlevitsky@gmail.com>
> ---
>  MAINTAINERS                      |    5 +
>  drivers/memstick/core/Kconfig    |   12 +
>  drivers/memstick/core/Makefile   |    2 +-
>  drivers/memstick/core/ms_block.c | 2385 ++++++++++++++++++++++++++++++++++++++
>  drivers/memstick/core/ms_block.h |  290 +++++
>  5 files changed, 2693 insertions(+), 1 deletion(-)
>  create mode 100644 drivers/memstick/core/ms_block.c
>  create mode 100644 drivers/memstick/core/ms_block.h
> 
> diff --git a/MAINTAINERS b/MAINTAINERS
> index ad7e322..26a01e2 100644
> --- a/MAINTAINERS
> +++ b/MAINTAINERS
> @@ -7519,6 +7519,11 @@ W:	http://tifmxx.berlios.de/
>  S:	Maintained
>  F:	drivers/memstick/host/tifm_ms.c
>  
> +SONY MEMORYSTICK STANDARD SUPPORT
> +M:	Maxim Levitsky <maximlevitsky@gmail.com>
> +S:	Maintained
> +F:	drivers/memstick/core/ms_block.*
> +
>  SOUND
>  M:	Jaroslav Kysela <perex@perex.cz>
>  M:	Takashi Iwai <tiwai@suse.de>
> diff --git a/drivers/memstick/core/Kconfig b/drivers/memstick/core/Kconfig
> index 95f1814..1d38949 100644
> --- a/drivers/memstick/core/Kconfig
> +++ b/drivers/memstick/core/Kconfig
> @@ -24,3 +24,15 @@ config MSPRO_BLOCK
>  	  support. This provides a block device driver, which you can use
>  	  to mount the filesystem. Almost everyone wishing MemoryStick
>  	  support should say Y or M here.
> +
> +config MS_BLOCK
> +	tristate "MemoryStick Standard device driver"
> +	depends on BLOCK
> +	help
> +	  Say Y here to enable the MemoryStick Standard device driver
> +	  support. This provides a block device driver, which you can use
> +	  to mount the filesystem.
> +	  This driver works with old (bulky) MemoryStick and MemoryStick Duo
> +	  but not PRO. Say Y if you have such card.
> +	  Driver is new and not yet well tested, thus it can damage your card
> +	  (even permanently)
> diff --git a/drivers/memstick/core/Makefile b/drivers/memstick/core/Makefile
> index ecd0299..0d7f90c 100644
> --- a/drivers/memstick/core/Makefile
> +++ b/drivers/memstick/core/Makefile
> @@ -3,5 +3,5 @@
>  #
>  
>  obj-$(CONFIG_MEMSTICK)		+= memstick.o
> -
> +obj-$(CONFIG_MS_BLOCK)		+= ms_block.o
>  obj-$(CONFIG_MSPRO_BLOCK)	+= mspro_block.o
> diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
> new file mode 100644
> index 0000000..08e7023
> --- /dev/null
> +++ b/drivers/memstick/core/ms_block.c
> @@ -0,0 +1,2385 @@
> +/*
> + *  ms_block.c - Sony MemoryStick (legacy) storage support
> +
> + *  Copyright (C) 2013 Maxim Levitsky <maximlevitsky@gmail.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * Minor portions of the driver were copied from mspro_block.c which is
> + * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
> + *
> + */
> +#define DRIVER_NAME "ms_block"
> +#define pr_fmt(fmt) DRIVER_NAME ": " fmt
> +
> +#include <linux/module.h>
> +#include <linux/blkdev.h>
> +#include <linux/memstick.h>
> +#include <linux/idr.h>
> +#include <linux/hdreg.h>
> +#include <linux/delay.h>
> +#include <linux/slab.h>
> +#include <linux/random.h>
> +#include <linux/bitmap.h>
> +#include <linux/scatterlist.h>
> +#include <linux/jiffies.h>
> +#include <linux/workqueue.h>
> +#include <linux/mutex.h>
> +#include "ms_block.h"
> +
> +static int debug;
> +static int cache_flush_timeout = 1000;
> +static bool verify_writes;
> +
> +/*
> + * Copies section of 'sg_from' starting from offset 'offset' and with length
> + * 'len' To another scatterlist of to_nents enties
> + */
> +static size_t msb_sg_copy(struct scatterlist *sg_from,
> +	struct scatterlist *sg_to, int to_nents, size_t offset, size_t len)
> +{
> +	size_t copied = 0;
> +
> +	while (offset > 0) {
> +		if (offset >= sg_from->length) {
> +			if (sg_is_last(sg_from))
> +				return 0;
> +
> +			offset -= sg_from->length;
> +			sg_from = sg_next(sg_from);
> +			continue;
> +		}
> +
> +		copied = min(len, sg_from->length - offset);
> +		sg_set_page(sg_to, sg_page(sg_from),
> +			copied, sg_from->offset + offset);
> +
> +		len -= copied;
> +		offset = 0;
> +
> +		if (sg_is_last(sg_from) || !len)
> +			goto out;
> +
> +		sg_to = sg_next(sg_to);
> +		to_nents--;
> +		sg_from = sg_next(sg_from);
> +	}
> +
> +	while (len > sg_from->length && to_nents--) {
> +		len -= sg_from->length;
> +		copied += sg_from->length;
> +
> +		sg_set_page(sg_to, sg_page(sg_from),
> +				sg_from->length, sg_from->offset);
> +
> +		if (sg_is_last(sg_from) || !len)
> +			goto out;
> +
> +		sg_from = sg_next(sg_from);
> +		sg_to = sg_next(sg_to);
> +	}
> +
> +	if (len && to_nents) {
> +		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
> +		copied += len;
> +	}
> +out:
> +	sg_mark_end(sg_to);
> +	return copied;
> +}
> +
> +/*
> + * Compares section of 'sg' starting from offset 'offset' and with length 'len'
> + * to linear buffer of length 'len' at address 'buffer'
> + * Returns 0 if equal and  -1 otherwice
> + */
> +static int msb_sg_compare_to_buffer(struct scatterlist *sg,
> +					size_t offset, u8 *buffer, size_t len)
> +{
> +	int retval = 0, cmplen;
> +	struct sg_mapping_iter miter;
> +
> +	sg_miter_start(&miter, sg, sg_nents(sg),
> +					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
> +
> +	while (sg_miter_next(&miter) && len > 0) {
> +		if (offset >= miter.length) {
> +			offset -= miter.length;
> +			continue;
> +		}
> +
> +		cmplen = min(miter.length - offset, len);
> +		retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
> +		if (retval)
> +			break;
> +
> +		buffer += cmplen;
> +		len -= cmplen;
> +		offset = 0;
> +	}
> +
> +	if (!retval && len)
> +		retval = -1;
> +
> +	sg_miter_stop(&miter);
> +	return retval;
> +}
> +
> +
> +/* Get zone at which block with logical address 'lba' lives
> + * Flash is broken into zones.
> + * Each zone consists of 512 eraseblocks, out of which in first
> + * zone 494 are used and 496 are for all following zones.
> + * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
> +*/
> +static int msb_get_zone_from_lba(int lba)
> +{
> +	if (lba < 494)
> +		return 0;
> +	return ((lba - 494) / 496) + 1;
> +}
> +
> +/* Get zone of physical block. Trivial */
> +static int msb_get_zone_from_pba(int pba)
> +{
> +	return pba / MS_BLOCKS_IN_ZONE;
> +}
> +
> +/* Debug test to validate free block counts */
> +static int msb_validate_used_block_bitmap(struct msb_data *msb)
> +{
> +	int total_free_blocks = 0;
> +	int i;
> +
> +	if (!debug)
> +		return 0;
> +
> +	for (i = 0; i < msb->zone_count; i++)
> +		total_free_blocks += msb->free_block_count[i];
> +
> +	if (msb->block_count - bitmap_weight(msb->used_blocks_bitmap,
> +					msb->block_count) == total_free_blocks)
> +		return 0;
> +
> +	pr_err("BUG: free block counts don't match the bitmap");
> +	msb->read_only = true;
> +	return -EINVAL;
> +}
> +
> +/* Mark physical block as used */
> +static void msb_mark_block_used(struct msb_data *msb, int pba)
> +{
> +	int zone = msb_get_zone_from_pba(pba);
> +
> +	if (test_bit(pba, msb->used_blocks_bitmap)) {
> +		pr_err(
> +		"BUG: attempt to mark already used pba %d as used", pba);
> +		msb->read_only = true;
> +		return;
> +	}
> +
> +	if (msb_validate_used_block_bitmap(msb))
> +		return;
> +
> +	/* No races because all IO is single threaded */
> +	__set_bit(pba, msb->used_blocks_bitmap);
> +	msb->free_block_count[zone]--;
> +}
> +
> +/* Mark physical block as free */
> +static void msb_mark_block_unused(struct msb_data *msb, int pba)
> +{
> +	int zone = msb_get_zone_from_pba(pba);
> +
> +	if (!test_bit(pba, msb->used_blocks_bitmap)) {
> +		pr_err("BUG: attempt to mark already unused pba %d as unused" , pba);
> +		msb->read_only = true;
> +		return;
> +	}
> +
> +	if (msb_validate_used_block_bitmap(msb))
> +		return;
> +
> +	/* No races because all IO is single threaded */
> +	__clear_bit(pba, msb->used_blocks_bitmap);
> +	msb->free_block_count[zone]++;
> +}
> +
> +/* Invalidate current register window */
> +static void msb_invalidate_reg_window(struct msb_data *msb)
> +{
> +	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
> +	msb->reg_addr.w_length = sizeof(struct ms_id_register);
> +	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
> +	msb->reg_addr.r_length = sizeof(struct ms_id_register);
> +	msb->addr_valid = false;
> +}
> +
> +/* Start a state machine */
> +static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
> +		(struct memstick_dev *card, struct memstick_request **req))
> +{
> +	struct memstick_dev *card = msb->card;
> +
> +	WARN_ON(msb->state != -1);
> +	msb->int_polling = false;
> +	msb->state = 0;
> +	msb->exit_error = 0;
> +
> +	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
> +
> +	card->next_request = state_func;
> +	memstick_new_req(card->host);
> +	wait_for_completion(&card->mrq_complete);
> +
> +	WARN_ON(msb->state != -1);
> +	return msb->exit_error;
> +}
> +
> +/* State machines call that to exit */
> +static int msb_exit_state_machine(struct msb_data *msb, int error)
> +{
> +	WARN_ON(msb->state == -1);
> +
> +	msb->state = -1;
> +	msb->exit_error = error;
> +	msb->card->next_request = h_msb_default_bad;
> +
> +	/* Invalidate reg window on errors */
> +	if (error)
> +		msb_invalidate_reg_window(msb);
> +
> +	complete(&msb->card->mrq_complete);
> +	return -ENXIO;
> +}
> +
> +/* read INT register */
> +static int msb_read_int_reg(struct msb_data *msb, long timeout)
> +{
> +	struct memstick_request *mrq = &msb->card->current_mrq;
> +
> +	WARN_ON(msb->state == -1);
> +
> +	if (!msb->int_polling) {
> +		msb->int_timeout = jiffies +
> +			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
> +		msb->int_polling = true;
> +	} else if (time_after(jiffies, msb->int_timeout)) {
> +		mrq->data[0] = MEMSTICK_INT_CMDNAK;
> +		return 0;
> +	}
> +
> +	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
> +				mrq->need_card_int && !mrq->error) {
> +		mrq->data[0] = mrq->int_reg;
> +		mrq->need_card_int = false;
> +		return 0;
> +	} else {
> +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> +		return 1;
> +	}
> +}
> +
> +/* Read a register */
> +static int msb_read_regs(struct msb_data *msb, int offset, int len)
> +{
> +	struct memstick_request *req = &msb->card->current_mrq;
> +
> +	if (msb->reg_addr.r_offset != offset ||
> +	    msb->reg_addr.r_length != len || !msb->addr_valid) {
> +
> +		msb->reg_addr.r_offset = offset;
> +		msb->reg_addr.r_length = len;
> +		msb->addr_valid = true;
> +
> +		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
> +			&msb->reg_addr, sizeof(msb->reg_addr));
> +		return 0;
> +	}
> +
> +	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
> +	return 1;
> +}
> +
> +/* Write a card register */
> +static int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
> +{
> +	struct memstick_request *req = &msb->card->current_mrq;
> +
> +	if (msb->reg_addr.w_offset != offset ||
> +		msb->reg_addr.w_length != len  || !msb->addr_valid) {
> +
> +		msb->reg_addr.w_offset = offset;
> +		msb->reg_addr.w_length = len;
> +		msb->addr_valid = true;
> +
> +		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
> +			&msb->reg_addr, sizeof(msb->reg_addr));
> +		return 0;
> +	}
> +
> +	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
> +	return 1;
> +}
> +
> +/* Handler for absence of IO */
> +static int h_msb_default_bad(struct memstick_dev *card,
> +						struct memstick_request **mrq)
> +{
> +	return -ENXIO;
> +}
> +
> +/*
> + * This function is a handler for reads of one page from device.
> + * Writes output to msb->current_sg, takes sector address from msb->reg.param
> + * Can also be used to read extra data only. Set params accordintly.
> + */
> +static int h_msb_read_page(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +	struct scatterlist sg[2];
> +	u8 command, intreg;
> +
> +	if (mrq->error) {
> +		dbg("read_page, unknown error");
> +		return msb_exit_state_machine(msb, mrq->error);
> +	}
> +again:
> +	switch (msb->state) {
> +	case MSB_RP_SEND_BLOCK_ADDRESS:
> +		/* msb_write_regs sometimes "fails" because it needs to update
> +			the reg window, and thus it returns request for that.
> +			Then we stay in this state and retry */
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, param),
> +			sizeof(struct ms_param_register),
> +			(unsigned char *)&msb->regs.param))
> +			return 0;
> +
> +		msb->state = MSB_RP_SEND_READ_COMMAND;
> +		return 0;
> +
> +	case MSB_RP_SEND_READ_COMMAND:
> +		command = MS_CMD_BLOCK_READ;
> +		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
> +		msb->state = MSB_RP_SEND_INT_REQ;
> +		return 0;
> +
> +	case MSB_RP_SEND_INT_REQ:
> +		msb->state = MSB_RP_RECEIVE_INT_REQ_RESULT;
> +		/* If dont actually need to send the int read request (only in
> +			serial mode), then just fall through */
> +		if (msb_read_int_reg(msb, -1))
> +			return 0;
> +		/* fallthrough */
> +
> +	case MSB_RP_RECEIVE_INT_REQ_RESULT:
> +		intreg = mrq->data[0];
> +		msb->regs.status.interrupt = intreg;
> +
> +		if (intreg & MEMSTICK_INT_CMDNAK)
> +			return msb_exit_state_machine(msb, -EIO);
> +
> +		if (!(intreg & MEMSTICK_INT_CED)) {
> +			msb->state = MSB_RP_SEND_INT_REQ;
> +			goto again;
> +		}
> +
> +		msb->int_polling = false;
> +		msb->state = (intreg & MEMSTICK_INT_ERR) ?
> +			MSB_RP_SEND_READ_STATUS_REG : MSB_RP_SEND_OOB_READ;
> +		goto again;
> +
> +	case MSB_RP_SEND_READ_STATUS_REG:
> +		 /* read the status register to understand source of the INT_ERR */
> +		if (!msb_read_regs(msb,
> +			offsetof(struct ms_register, status),
> +			sizeof(struct ms_status_register)))
> +			return 0;
> +
> +		msb->state = MSB_RP_RECEIVE_OOB_READ;
> +		return 0;
> +
> +	case MSB_RP_RECIVE_STATUS_REG:
> +		msb->regs.status = *(struct ms_status_register *)mrq->data;
> +		msb->state = MSB_RP_SEND_OOB_READ;
> +		/* fallthrough */
> +
> +	case MSB_RP_SEND_OOB_READ:
> +		if (!msb_read_regs(msb,
> +			offsetof(struct ms_register, extra_data),
> +			sizeof(struct ms_extra_data_register)))
> +			return 0;
> +
> +		msb->state = MSB_RP_RECEIVE_OOB_READ;
> +		return 0;
> +
> +	case MSB_RP_RECEIVE_OOB_READ:
> +		msb->regs.extra_data =
> +			*(struct ms_extra_data_register *) mrq->data;
> +		msb->state = MSB_RP_SEND_READ_DATA;
> +		/* fallthrough */
> +
> +	case MSB_RP_SEND_READ_DATA:
> +		/* Skip that state if we only read the oob */
> +		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
> +			msb->state = MSB_RP_RECEIVE_READ_DATA;
> +			goto again;
> +		}
> +
> +		sg_init_table(sg, ARRAY_SIZE(sg));
> +		msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
> +			msb->current_sg_offset,
> +			msb->page_size);
> +
> +		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
> +		msb->state = MSB_RP_RECEIVE_READ_DATA;
> +		return 0;
> +
> +	case MSB_RP_RECEIVE_READ_DATA:
> +		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
> +			msb->current_sg_offset += msb->page_size;
> +			return msb_exit_state_machine(msb, 0);
> +		}
> +
> +		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
> +			dbg("read_page: uncorrectable error");
> +			return msb_exit_state_machine(msb, -EBADMSG);
> +		}
> +
> +		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
> +			dbg("read_page: correctable error");
> +			msb->current_sg_offset += msb->page_size;
> +			return msb_exit_state_machine(msb, -EUCLEAN);
> +		} else {
> +			dbg("read_page: INT error, but no status error bits");
> +			return msb_exit_state_machine(msb, -EIO);
> +		}
> +	}
> +
> +	BUG();
> +}
> +
> +/*
> + * Handler of writes of exactly one block.
> + * Takes address from msb->regs.param.
> + * Writes same extra data to blocks, also taken
> + * from msb->regs.extra
> + * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
> + * device refuses to take the command or something else
> + */
> +static int h_msb_write_block(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +	struct scatterlist sg[2];
> +	u8 intreg, command;
> +
> +	if (mrq->error)
> +		return msb_exit_state_machine(msb, mrq->error);
> +
> +again:
> +	switch (msb->state) {
> +
> +	/* HACK: Jmicon handling of TPCs between 8 and
> +	 *	sizeof(memstick_request.data) is broken due to hardware
> +	 *	bug in PIO mode that is used for these TPCs
> +	 *	Therefore split the write
> +	 */
> +
> +	case MSB_WB_SEND_WRITE_PARAMS:
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, param),
> +			sizeof(struct ms_param_register),
> +			&msb->regs.param))
> +			return 0;
> +
> +		msb->state = MSB_WB_SEND_WRITE_OOB;
> +		return 0;
> +
> +	case MSB_WB_SEND_WRITE_OOB:
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, extra_data),
> +			sizeof(struct ms_extra_data_register),
> +			&msb->regs.extra_data))
> +			return 0;
> +		msb->state = MSB_WB_SEND_WRITE_COMMAND;
> +		return 0;
> +
> +
> +	case MSB_WB_SEND_WRITE_COMMAND:
> +		command = MS_CMD_BLOCK_WRITE;
> +		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
> +		msb->state = MSB_WB_SEND_INT_REQ;
> +		return 0;
> +
> +	case MSB_WB_SEND_INT_REQ:
> +		msb->state = MSB_WB_RECEIVE_INT_REQ;
> +		if (msb_read_int_reg(msb, -1))
> +			return 0;
> +		/* fallthrough */
> +
> +	case MSB_WB_RECEIVE_INT_REQ:
> +		intreg = mrq->data[0];
> +		msb->regs.status.interrupt = intreg;
> +
> +		/* errors mean out of here, and fast... */
> +		if (intreg & (MEMSTICK_INT_CMDNAK))
> +			return msb_exit_state_machine(msb, -EIO);
> +
> +		if (intreg & MEMSTICK_INT_ERR)
> +			return msb_exit_state_machine(msb, -EBADMSG);
> +
> +
> +		/* for last page we need to poll CED */
> +		if (msb->current_page == msb->pages_in_block) {
> +			if (intreg & MEMSTICK_INT_CED)
> +				return msb_exit_state_machine(msb, 0);
> +			msb->state = MSB_WB_SEND_INT_REQ;
> +			goto again;
> +
> +		}
> +
> +		/* for non-last page we need BREQ before writing next chunk */
> +		if (!(intreg & MEMSTICK_INT_BREQ)) {
> +			msb->state = MSB_WB_SEND_INT_REQ;
> +			goto again;
> +		}
> +
> +		msb->int_polling = false;
> +		msb->state = MSB_WB_SEND_WRITE_DATA;
> +		/* fallthrough */
> +
> +	case MSB_WB_SEND_WRITE_DATA:
> +		sg_init_table(sg, ARRAY_SIZE(sg));
> +
> +		if (msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
> +			msb->current_sg_offset,
> +			msb->page_size) < msb->page_size)
> +			return msb_exit_state_machine(msb, -EIO);
> +
> +		memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
> +		mrq->need_card_int = 1;
> +		msb->state = MSB_WB_RECEIVE_WRITE_CONFIRMATION;
> +		return 0;
> +
> +	case MSB_WB_RECEIVE_WRITE_CONFIRMATION:
> +		msb->current_page++;
> +		msb->current_sg_offset += msb->page_size;
> +		msb->state = MSB_WB_SEND_INT_REQ;
> +		goto again;
> +	default:
> +		BUG();
> +	}
> +
> +	return 0;
> +}
> +
> +/*
> + * This function is used to send simple IO requests to device that consist
> + * of register write + command
> + */
> +static int h_msb_send_command(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +	u8 intreg;
> +
> +	if (mrq->error) {
> +		dbg("send_command: unknown error");
> +		return msb_exit_state_machine(msb, mrq->error);
> +	}
> +again:
> +	switch (msb->state) {
> +
> +	/* HACK: see h_msb_write_block */
> +	case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, param),
> +			sizeof(struct ms_param_register),
> +			&msb->regs.param))
> +			return 0;
> +		msb->state = MSB_SC_SEND_WRITE_OOB;
> +		return 0;
> +
> +	case MSB_SC_SEND_WRITE_OOB:
> +		if (!msb->command_need_oob) {
> +			msb->state = MSB_SC_SEND_COMMAND;
> +			goto again;
> +		}
> +
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, extra_data),
> +			sizeof(struct ms_extra_data_register),
> +			&msb->regs.extra_data))
> +			return 0;
> +
> +		msb->state = MSB_SC_SEND_COMMAND;
> +		return 0;
> +
> +	case MSB_SC_SEND_COMMAND:
> +		memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
> +		msb->state = MSB_SC_SEND_INT_REQ;
> +		return 0;
> +
> +	case MSB_SC_SEND_INT_REQ:
> +		msb->state = MSB_SC_RECEIVE_INT_REQ;
> +		if (msb_read_int_reg(msb, -1))
> +			return 0;
> +		/* fallthrough */
> +
> +	case MSB_SC_RECEIVE_INT_REQ:
> +		intreg = mrq->data[0];
> +
> +		if (intreg & MEMSTICK_INT_CMDNAK)
> +			return msb_exit_state_machine(msb, -EIO);
> +		if (intreg & MEMSTICK_INT_ERR)
> +			return msb_exit_state_machine(msb, -EBADMSG);
> +
> +		if (!(intreg & MEMSTICK_INT_CED)) {
> +			msb->state = MSB_SC_SEND_INT_REQ;
> +			goto again;
> +		}
> +
> +		return msb_exit_state_machine(msb, 0);
> +	}
> +
> +	BUG();
> +}
> +
> +/* Small handler for card reset */
> +static int h_msb_reset(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	u8 command = MS_CMD_RESET;
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +
> +	if (mrq->error)
> +		return msb_exit_state_machine(msb, mrq->error);
> +
> +	switch (msb->state) {
> +	case MSB_RS_SEND:
> +		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
> +		mrq->need_card_int = 0;
> +		msb->state = MSB_RS_CONFIRM;
> +		return 0;
> +	case MSB_RS_CONFIRM:
> +		return msb_exit_state_machine(msb, 0);
> +	}
> +	BUG();
> +}
> +
> +/* This handler is used to do serial->parallel switch */
> +static int h_msb_parallel_switch(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +	struct memstick_host *host = card->host;
> +
> +	if (mrq->error) {
> +		dbg("parallel_switch: error");
> +		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
> +		return msb_exit_state_machine(msb, mrq->error);
> +	}
> +
> +	switch (msb->state) {
> +	case MSB_PS_SEND_SWITCH_COMMAND:
> +		/* Set the parallel interface on memstick side */
> +		msb->regs.param.system |= MEMSTICK_SYS_PAM;
> +
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, param),
> +			1,
> +			(unsigned char *)&msb->regs.param))
> +			return 0;
> +
> +		msb->state = MSB_PS_SWICH_HOST;
> +		return 0;
> +
> +	case MSB_PS_SWICH_HOST:
> +		 /* Set parallel interface on our side + send a dummy request
> +			to see if card responds */
> +		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
> +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> +		msb->state = MSB_PS_CONFIRM;
> +		return 0;
> +
> +	case MSB_PS_CONFIRM:
> +		return msb_exit_state_machine(msb, 0);
> +	}
> +
> +	BUG();
> +}
> +
> +static int msb_switch_to_parallel(struct msb_data *msb);
> +
> +/* Reset the card, to guard against hw errors beeing treated as bad blocks */
> +static int msb_reset(struct msb_data *msb, bool full)
> +{
> +
> +	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
> +	struct memstick_dev *card = msb->card;
> +	struct memstick_host *host = card->host;
> +	int error;
> +
> +	/* Reset the card */
> +	msb->regs.param.system = MEMSTICK_SYS_BAMD;
> +
> +	if (full) {
> +		error =  host->set_param(host,
> +					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
> +		if (error)
> +			goto out_error;
> +
> +		msb_invalidate_reg_window(msb);
> +
> +		error = host->set_param(host,
> +					MEMSTICK_POWER, MEMSTICK_POWER_ON);
> +		if (error)
> +			goto out_error;
> +
> +		error = host->set_param(host,
> +					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
> +		if (error) {
> +out_error:
> +			dbg("Failed to reset the host controller");
> +			msb->read_only = true;
> +			return -EFAULT;
> +		}
> +	}
> +
> +	error = msb_run_state_machine(msb, h_msb_reset);
> +	if (error) {
> +		dbg("Failed to reset the card");
> +		msb->read_only = true;
> +		return -ENODEV;
> +	}
> +
> +	/* Set parallel mode */
> +	if (was_parallel)
> +		msb_switch_to_parallel(msb);
> +	return 0;
> +}
> +
> +/* Attempts to switch interface to parallel mode */
> +static int msb_switch_to_parallel(struct msb_data *msb)
> +{
> +	int error;
> +
> +	error = msb_run_state_machine(msb, h_msb_parallel_switch);
> +	if (error) {
> +		pr_err("Switch to parallel failed");
> +		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
> +		msb_reset(msb, true);
> +		return -EFAULT;
> +	}
> +
> +	msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
> +	return 0;
> +}
> +
> +/* Changes overwrite flag on a page */
> +static int msb_set_overwrite_flag(struct msb_data *msb,
> +						u16 pba, u8 page, u8 flag)
> +{
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	msb->regs.param.block_address = cpu_to_be16(pba);
> +	msb->regs.param.page_address = page;
> +	msb->regs.param.cp = MEMSTICK_CP_OVERWRITE;
> +	msb->regs.extra_data.overwrite_flag = flag;
> +	msb->command_value = MS_CMD_BLOCK_WRITE;
> +	msb->command_need_oob = true;
> +
> +	dbg_verbose("changing overwrite flag to %02x for sector %d, page %d",
> +							flag, pba, page);
> +	return msb_run_state_machine(msb, h_msb_send_command);
> +}
> +
> +static int msb_mark_bad(struct msb_data *msb, int pba)
> +{
> +	pr_notice("marking pba %d as bad", pba);
> +	msb_reset(msb, true);
> +	return msb_set_overwrite_flag(
> +			msb, pba, 0, 0xFF & ~MEMSTICK_OVERWRITE_BKST);
> +}
> +
> +static int msb_mark_page_bad(struct msb_data *msb, int pba, int page)
> +{
> +	dbg("marking page %d of pba %d as bad", page, pba);
> +	msb_reset(msb, true);
> +	return msb_set_overwrite_flag(msb,
> +		pba, page, ~MEMSTICK_OVERWRITE_PGST0);
> +}
> +
> +/* Erases one physical block */
> +static int msb_erase_block(struct msb_data *msb, u16 pba)
> +{
> +	int error, try;
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	dbg_verbose("erasing pba %d", pba);
> +
> +	for (try = 1; try < 3; try++) {
> +		msb->regs.param.block_address = cpu_to_be16(pba);
> +		msb->regs.param.page_address = 0;
> +		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
> +		msb->command_value = MS_CMD_BLOCK_ERASE;
> +		msb->command_need_oob = false;
> +
> +
> +		error = msb_run_state_machine(msb, h_msb_send_command);
> +		if (!error || msb_reset(msb, true))
> +			break;
> +	}
> +
> +	if (error) {
> +		pr_err("erase failed, marking pba %d as bad", pba);
> +		msb_mark_bad(msb, pba);
> +	}
> +
> +	dbg_verbose("erase success, marking pba %d as unused", pba);
> +	msb_mark_block_unused(msb, pba);
> +	__set_bit(pba, msb->erased_blocks_bitmap);
> +	return error;
> +}
> +
> +/* Reads one page from device */
> +static int msb_read_page(struct msb_data *msb,
> +	u16 pba, u8 page, struct ms_extra_data_register *extra,
> +					struct scatterlist *sg,  int offset)
> +{
> +	int try, error;
> +
> +	if (pba == MS_BLOCK_INVALID) {
> +		unsigned long flags;
> +		struct sg_mapping_iter miter;
> +		size_t len = msb->page_size;
> +
> +		dbg_verbose("read unmapped sector. returning 0xFF");
> +
> +		local_irq_save(flags);
> +		sg_miter_start(&miter, sg, sg_nents(sg),
> +				SG_MITER_ATOMIC | SG_MITER_TO_SG);
> +
> +		while (sg_miter_next(&miter) && len > 0) {
> +
> +			int chunklen;
> +
> +			if (offset && offset >= miter.length) {
> +				offset -= miter.length;
> +				continue;
> +			}
> +
> +			chunklen = min(miter.length - offset, len);
> +			memset(miter.addr + offset, 0xFF, chunklen);
> +			len -= chunklen;
> +			offset = 0;
> +		}
> +
> +		sg_miter_stop(&miter);
> +		local_irq_restore(flags);
> +
> +		if (offset)
> +			return -EFAULT;
> +
> +		if (extra)
> +			memset(extra, 0xFF, sizeof(*extra));
> +		return 0;
> +	}
> +
> +	if (pba >= msb->block_count) {
> +		pr_err("BUG: attempt to read beyond the end of the card at pba %d", pba);
> +		return -EINVAL;
> +	}
> +
> +	for (try = 1; try < 3; try++) {
> +		msb->regs.param.block_address = cpu_to_be16(pba);
> +		msb->regs.param.page_address = page;
> +		msb->regs.param.cp = MEMSTICK_CP_PAGE;
> +
> +		msb->current_sg = sg;
> +		msb->current_sg_offset = offset;
> +		error = msb_run_state_machine(msb, h_msb_read_page);
> +
> +
> +		if (error == -EUCLEAN) {
> +			pr_notice("correctable error on pba %d, page %d",
> +				pba, page);
> +			error = 0;
> +		}
> +
> +		if (!error && extra)
> +			*extra = msb->regs.extra_data;
> +
> +		if (!error || msb_reset(msb, true))
> +			break;
> +
> +	}
> +
> +	/* Mark bad pages */
> +	if (error == -EBADMSG) {
> +		pr_err("uncorrectable error on read of pba %d, page %d",
> +			pba, page);
> +
> +		if (msb->regs.extra_data.overwrite_flag &
> +					MEMSTICK_OVERWRITE_PGST0)
> +			msb_mark_page_bad(msb, pba, page);
> +		return -EBADMSG;
> +	}
> +
> +	if (error)
> +		pr_err("read of pba %d, page %d failed with error %d",
> +			pba, page, error);
> +	return error;
> +}
> +
> +/* Reads oob of page only */
> +static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
> +	struct ms_extra_data_register *extra)
> +{
> +	int error;
> +
> +	BUG_ON(!extra);
> +	msb->regs.param.block_address = cpu_to_be16(pba);
> +	msb->regs.param.page_address = page;
> +	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
> +
> +	if (pba > msb->block_count) {
> +		pr_err("BUG: attempt to read beyond the end of card at pba %d", pba);
> +		return -EINVAL;
> +	}
> +
> +	error = msb_run_state_machine(msb, h_msb_read_page);
> +	*extra = msb->regs.extra_data;
> +
> +	if (error == -EUCLEAN) {
> +		pr_notice("correctable error on pba %d, page %d",
> +			pba, page);
> +		return 0;
> +	}
> +
> +	return error;
> +}
> +
> +/* Reads a block and compares it with data contained in scatterlist orig_sg */
> +static int msb_verify_block(struct msb_data *msb, u16 pba,
> +				struct scatterlist *orig_sg,  int offset)
> +{
> +	struct scatterlist sg;
> +	int page = 0, error;
> +
> +	sg_init_one(&sg, msb->block_buffer, msb->block_size);
> +
> +	while (page < msb->pages_in_block) {
> +
> +		error = msb_read_page(msb, pba, page,
> +				NULL, &sg, page * msb->page_size);
> +		if (error)
> +			return error;
> +		page++;
> +	}
> +
> +	if (msb_sg_compare_to_buffer(orig_sg, offset,
> +				msb->block_buffer, msb->block_size))
> +		return -EIO;
> +	return 0;
> +}
> +
> +/* Writes exectly one block + oob */
> +static int msb_write_block(struct msb_data *msb,
> +			u16 pba, u32 lba, struct scatterlist *sg, int offset)
> +{
> +	int error, current_try = 1;
> +	BUG_ON(sg->length < msb->page_size);
> +
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	if (pba == MS_BLOCK_INVALID) {
> +		pr_err(
> +			"BUG: write: attempt to write MS_BLOCK_INVALID block");
> +		return -EINVAL;
> +	}
> +
> +	if (pba >= msb->block_count || lba >= msb->logical_block_count) {
> +		pr_err(
> +		"BUG: write: attempt to write beyond the end of device");
> +		return -EINVAL;
> +	}
> +
> +	if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
> +		pr_err("BUG: write: lba zone mismatch");
> +		return -EINVAL;
> +	}
> +
> +	if (pba == msb->boot_block_locations[0] ||
> +		pba == msb->boot_block_locations[1]) {
> +		pr_err("BUG: write: attempt to write to boot blocks!");
> +		return -EINVAL;
> +	}
> +
> +	while (1) {
> +
> +		if (msb->read_only)
> +			return -EROFS;
> +
> +		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
> +		msb->regs.param.page_address = 0;
> +		msb->regs.param.block_address = cpu_to_be16(pba);
> +
> +		msb->regs.extra_data.management_flag = 0xFF;
> +		msb->regs.extra_data.overwrite_flag = 0xF8;
> +		msb->regs.extra_data.logical_address = cpu_to_be16(lba);
> +
> +		msb->current_sg = sg;
> +		msb->current_sg_offset = offset;
> +		msb->current_page = 0;
> +
> +		error = msb_run_state_machine(msb, h_msb_write_block);
> +
> +		/* Sector we just wrote to is assumed erased since its pba
> +			was erased. If it wasn't erased, write will succeed
> +			and will just clear the bits that were set in the block
> +			thus test that what we have written,
> +			matches what we expect.
> +			We do trust the blocks that we erased */
> +		if (!error && (verify_writes ||
> +				!test_bit(pba, msb->erased_blocks_bitmap)))
> +			error = msb_verify_block(msb, pba, sg, offset);
> +
> +		if (!error)
> +			break;
> +
> +		if (current_try > 1 || msb_reset(msb, true))
> +			break;
> +
> +		pr_err("write failed, trying to erase the pba %d", pba);
> +		error = msb_erase_block(msb, pba);
> +		if (error)
> +			break;
> +
> +		current_try++;
> +	}
> +	return error;
> +}
> +
> +/* Finds a free block for write replacement */
> +static u16 msb_get_free_block(struct msb_data *msb, int zone)
> +{
> +	u16 pos;
> +	int pba = zone * MS_BLOCKS_IN_ZONE;
> +	int i;
> +
> +	get_random_bytes(&pos, sizeof(pos));
> +
> +	if (!msb->free_block_count[zone]) {
> +		pr_err("NO free blocks in the zone %d, to use for a write, (media is WORN out) switching to RO mode", zone);
> +		msb->read_only = true;
> +		return MS_BLOCK_INVALID;
> +	}
> +
> +	pos %= msb->free_block_count[zone];
> +
> +	dbg_verbose("have %d choices for a free block, selected randomally: %d",
> +		msb->free_block_count[zone], pos);
> +
> +	pba = find_next_zero_bit(msb->used_blocks_bitmap,
> +							msb->block_count, pba);
> +	for (i = 0; i < pos; ++i)
> +		pba = find_next_zero_bit(msb->used_blocks_bitmap,
> +						msb->block_count, pba + 1);
> +
> +	dbg_verbose("result of the free blocks scan: pba %d", pba);
> +
> +	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
> +		pr_err("BUG: cant get a free block");
> +		msb->read_only = true;
> +		return MS_BLOCK_INVALID;
> +	}
> +
> +	msb_mark_block_used(msb, pba);
> +	return pba;
> +}
> +
> +static int msb_update_block(struct msb_data *msb, u16 lba,
> +	struct scatterlist *sg, int offset)
> +{
> +	u16 pba, new_pba;
> +	int error, try;
> +
> +	pba = msb->lba_to_pba_table[lba];
> +	dbg_verbose("start of a block update at lba  %d, pba %d", lba, pba);
> +
> +	if (pba != MS_BLOCK_INVALID) {
> +		dbg_verbose("setting the update flag on the block");
> +		msb_set_overwrite_flag(msb, pba, 0,
> +				0xFF & ~MEMSTICK_OVERWRITE_UDST);
> +	}
> +
> +	for (try = 0; try < 3; try++) {
> +		new_pba = msb_get_free_block(msb,
> +			msb_get_zone_from_lba(lba));
> +
> +		if (new_pba == MS_BLOCK_INVALID) {
> +			error = -EIO;
> +			goto out;
> +		}
> +
> +		dbg_verbose("block update: writing updated block to the pba %d",
> +								new_pba);
> +		error = msb_write_block(msb, new_pba, lba, sg, offset);
> +		if (error == -EBADMSG) {
> +			msb_mark_bad(msb, new_pba);
> +			continue;
> +		}
> +
> +		if (error)
> +			goto out;
> +
> +		dbg_verbose("block update: erasing the old block");
> +		msb_erase_block(msb, pba);
> +		msb->lba_to_pba_table[lba] = new_pba;
> +		return 0;
> +	}
> +out:
> +	if (error) {
> +		pr_err("block update error after %d tries,  switching to r/o mode", try);
> +		msb->read_only = true;
> +	}
> +	return error;
> +}
> +
> +/* Converts endiannes in the boot block for easy use */
> +static void msb_fix_boot_page_endianness(struct ms_boot_page *p)
> +{
> +	p->header.block_id = be16_to_cpu(p->header.block_id);
> +	p->header.format_reserved = be16_to_cpu(p->header.format_reserved);
> +	p->entry.disabled_block.start_addr
> +		= be32_to_cpu(p->entry.disabled_block.start_addr);
> +	p->entry.disabled_block.data_size
> +		= be32_to_cpu(p->entry.disabled_block.data_size);
> +	p->entry.cis_idi.start_addr
> +		= be32_to_cpu(p->entry.cis_idi.start_addr);
> +	p->entry.cis_idi.data_size
> +		= be32_to_cpu(p->entry.cis_idi.data_size);
> +	p->attr.block_size = be16_to_cpu(p->attr.block_size);
> +	p->attr.number_of_blocks = be16_to_cpu(p->attr.number_of_blocks);
> +	p->attr.number_of_effective_blocks
> +		= be16_to_cpu(p->attr.number_of_effective_blocks);
> +	p->attr.page_size = be16_to_cpu(p->attr.page_size);
> +	p->attr.memory_manufacturer_code
> +		= be16_to_cpu(p->attr.memory_manufacturer_code);
> +	p->attr.memory_device_code = be16_to_cpu(p->attr.memory_device_code);
> +	p->attr.implemented_capacity
> +		= be16_to_cpu(p->attr.implemented_capacity);
> +	p->attr.controller_number = be16_to_cpu(p->attr.controller_number);
> +	p->attr.controller_function = be16_to_cpu(p->attr.controller_function);
> +}
> +
> +static int msb_read_boot_blocks(struct msb_data *msb)
> +{
> +	int pba = 0;
> +	struct scatterlist sg;
> +	struct ms_extra_data_register extra;
> +	struct ms_boot_page *page;
> +
> +	msb->boot_block_locations[0] = MS_BLOCK_INVALID;
> +	msb->boot_block_locations[1] = MS_BLOCK_INVALID;
> +	msb->boot_block_count = 0;
> +
> +	dbg_verbose("Start of a scan for the boot blocks");
> +
> +	if (!msb->boot_page) {
> +		page = kmalloc(sizeof(struct ms_boot_page)*2, GFP_KERNEL);
> +		if (!page)
> +			return -ENOMEM;
> +
> +		msb->boot_page = page;
> +	} else
> +		page = msb->boot_page;
> +
> +	msb->block_count = MS_BLOCK_MAX_BOOT_ADDR;
> +
> +	for (pba = 0; pba < MS_BLOCK_MAX_BOOT_ADDR; pba++) {
> +
> +		sg_init_one(&sg, page, sizeof(*page));
> +		if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) {
> +			dbg("boot scan: can't read pba %d", pba);
> +			continue;
> +		}
> +
> +		if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) {
> +			dbg("managment flag doesn't indicate boot block %d",
> +									pba);
> +			continue;
> +		}
> +
> +		if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) {
> +			dbg("the pba at %d doesn' contain boot block ID", pba);
> +			continue;
> +		}
> +
> +		msb_fix_boot_page_endianness(page);
> +		msb->boot_block_locations[msb->boot_block_count] = pba;
> +
> +		page++;
> +		msb->boot_block_count++;
> +
> +		if (msb->boot_block_count == 2)
> +			break;
> +	}
> +
> +	if (!msb->boot_block_count) {
> +		pr_err("media doesn't contain master page, aborting");
> +		return -EIO;
> +	}
> +
> +	dbg_verbose("End of scan for boot blocks");
> +	return 0;
> +}
> +
> +static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
> +{
> +	struct ms_boot_page *boot_block;
> +	struct scatterlist sg;
> +	u16 *buffer = NULL;
> +	int offset = 0;
> +	int i, error = 0;
> +	int data_size, data_offset, page, page_offset, size_to_read;
> +	u16 pba;
> +
> +	BUG_ON(block_nr > 1);
> +	boot_block = &msb->boot_page[block_nr];
> +	pba = msb->boot_block_locations[block_nr];
> +
> +	if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID)
> +		return -EINVAL;
> +
> +	data_size = boot_block->entry.disabled_block.data_size;
> +	data_offset = sizeof(struct ms_boot_page) +
> +			boot_block->entry.disabled_block.start_addr;
> +	if (!data_size)
> +		return 0;
> +
> +	page = data_offset / msb->page_size;
> +	page_offset = data_offset % msb->page_size;
> +	size_to_read =
> +		DIV_ROUND_UP(data_size + page_offset, msb->page_size) *
> +			msb->page_size;
> +
> +	dbg("reading bad block of boot block at pba %d, offset %d len %d",
> +		pba, data_offset, data_size);
> +
> +	buffer = kzalloc(size_to_read, GFP_KERNEL);
> +	if (!buffer)
> +		return -ENOMEM;
> +
> +	/* Read the buffer */
> +	sg_init_one(&sg, buffer, size_to_read);
> +
> +	while (offset < size_to_read) {
> +		error = msb_read_page(msb, pba, page, NULL, &sg, offset);
> +		if (error)
> +			goto out;
> +
> +		page++;
> +		offset += msb->page_size;
> +
> +		if (page == msb->pages_in_block) {
> +			pr_err(
> +			"bad block table extends beyond the boot block");
> +			break;
> +		}
> +	}
> +
> +	/* Process the bad block table */
> +	for (i = page_offset; i < data_size / sizeof(u16); i++) {
> +
> +		u16 bad_block = be16_to_cpu(buffer[i]);
> +
> +		if (bad_block >= msb->block_count) {
> +			dbg("bad block table contains invalid block %d",
> +								bad_block);
> +			continue;
> +		}
> +
> +		if (test_bit(bad_block, msb->used_blocks_bitmap))  {
> +			dbg("duplicate bad block %d in the table",
> +				bad_block);
> +			continue;
> +		}
> +
> +		dbg("block %d is marked as factory bad", bad_block);
> +		msb_mark_block_used(msb, bad_block);
> +	}
> +out:
> +	kfree(buffer);
> +	return error;
> +}
> +
> +static int msb_ftl_initialize(struct msb_data *msb)
> +{
> +	int i;
> +
> +	if (msb->ftl_initialized)
> +		return 0;
> +
> +	msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE;
> +	msb->logical_block_count = msb->zone_count * 496 - 2;
> +
> +	msb->used_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
> +	msb->erased_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
> +	msb->lba_to_pba_table =
> +		kmalloc(msb->logical_block_count * sizeof(u16), GFP_KERNEL);
> +
> +	if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
> +						!msb->erased_blocks_bitmap) {
> +		kfree(msb->used_blocks_bitmap);
> +		kfree(msb->lba_to_pba_table);
> +		kfree(msb->erased_blocks_bitmap);
> +		return -ENOMEM;
> +	}
> +
> +	for (i = 0; i < msb->zone_count; i++)
> +		msb->free_block_count[i] = MS_BLOCKS_IN_ZONE;
> +
> +	memset(msb->lba_to_pba_table, MS_BLOCK_INVALID,
> +			msb->logical_block_count * sizeof(u16));
> +
> +	dbg("initial FTL tables created. Zone count = %d, Logical block count = %d",
> +		msb->zone_count, msb->logical_block_count);
> +
> +	msb->ftl_initialized = true;
> +	return 0;
> +}
> +
> +static int msb_ftl_scan(struct msb_data *msb)
> +{
> +	u16 pba, lba, other_block;
> +	u8 overwrite_flag, managment_flag, other_overwrite_flag;
> +	int error;
> +	struct ms_extra_data_register extra;
> +	u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL);
> +
> +	if (!overwrite_flags)
> +		return -ENOMEM;
> +
> +	dbg("Start of media scanning");
> +	for (pba = 0; pba < msb->block_count; pba++) {
> +
> +		if (pba == msb->boot_block_locations[0] ||
> +			pba == msb->boot_block_locations[1]) {
> +			dbg_verbose("pba %05d -> [boot block]", pba);
> +			msb_mark_block_used(msb, pba);
> +			continue;
> +		}
> +
> +		if (test_bit(pba, msb->used_blocks_bitmap)) {
> +			dbg_verbose("pba %05d -> [factory bad]", pba);
> +			continue;
> +		}
> +
> +		memset(&extra, 0, sizeof(extra));
> +		error = msb_read_oob(msb, pba, 0, &extra);
> +
> +		/* can't trust the page if we can't read the oob */
> +		if (error == -EBADMSG) {
> +			pr_notice(
> +			"oob of pba %d damaged, will try to erase it", pba);
> +			msb_mark_block_used(msb, pba);
> +			msb_erase_block(msb, pba);
> +			continue;
> +		} else if (error) {
> +			pr_err("unknown error %d on read of oob of pba %d - aborting",
> +				error, pba);
> +
> +			kfree(overwrite_flags);
> +			return error;
> +		}
> +
> +		lba = be16_to_cpu(extra.logical_address);
> +		managment_flag = extra.management_flag;
> +		overwrite_flag = extra.overwrite_flag;
> +		overwrite_flags[pba] = overwrite_flag;
> +
> +		/* Skip bad blocks */
> +		if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
> +			dbg("pba %05d -> [BAD]", pba);
> +			msb_mark_block_used(msb, pba);
> +			continue;
> +		}
> +
> +		/* Skip system/drm blocks */
> +		if ((managment_flag & MEMSTICK_MANAGMENT_FLAG_NORMAL) !=
> +			MEMSTICK_MANAGMENT_FLAG_NORMAL) {
> +			dbg("pba %05d -> [reserved managment flag %02x]",
> +							pba, managment_flag);
> +			msb_mark_block_used(msb, pba);
> +			continue;
> +		}
> +
> +		/* Erase temporary tables */
> +		if (!(managment_flag & MEMSTICK_MANAGEMENT_ATFLG)) {
> +			dbg("pba %05d -> [temp table] - will erase", pba);
> +
> +			msb_mark_block_used(msb, pba);
> +			msb_erase_block(msb, pba);
> +			continue;
> +		}
> +
> +		if (lba == MS_BLOCK_INVALID) {
> +			dbg_verbose("pba %05d -> [free]", pba);
> +			continue;
> +		}
> +
> +		msb_mark_block_used(msb, pba);
> +
> +		/* Block has LBA not according to zoning*/
> +		if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
> +			pr_notice("pba %05d -> [bad lba %05d] - will erase",
> +								pba, lba);
> +			msb_erase_block(msb, pba);
> +			continue;
> +		}
> +
> +		/* No collisions - great */
> +		if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) {
> +			dbg_verbose("pba %05d -> [lba %05d]", pba, lba);
> +			msb->lba_to_pba_table[lba] = pba;
> +			continue;
> +		}
> +
> +		other_block = msb->lba_to_pba_table[lba];
> +		other_overwrite_flag = overwrite_flags[other_block];
> +
> +		pr_notice("Collision between pba %d and pba %d",
> +			pba, other_block);
> +
> +		if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
> +			pr_notice("pba %d is marked as stable, use it", pba);
> +			msb_erase_block(msb, other_block);
> +			msb->lba_to_pba_table[lba] = pba;
> +			continue;
> +		}
> +
> +		if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
> +			pr_notice("pba %d is marked as stable, use it",
> +								other_block);
> +			msb_erase_block(msb, pba);
> +			continue;
> +		}
> +
> +		pr_notice("collision between blocks %d and %d, without stable flag set on both, erasing pba %d",
> +				pba, other_block, other_block);
> +
> +		msb_erase_block(msb, other_block);
> +		msb->lba_to_pba_table[lba] = pba;
> +	}
> +
> +	dbg("End of media scanning");
> +	kfree(overwrite_flags);
> +	return 0;
> +}
> +
> +static void msb_cache_flush_timer(unsigned long data)
> +{
> +	struct msb_data *msb = (struct msb_data *)data;
> +	msb->need_flush_cache = true;
> +	queue_work(msb->io_queue, &msb->io_work);
> +}
> +
> +
> +static void msb_cache_discard(struct msb_data *msb)
> +{
> +	if (msb->cache_block_lba == MS_BLOCK_INVALID)
> +		return;
> +
> +	del_timer_sync(&msb->cache_flush_timer);
> +
> +	dbg_verbose("Discarding the write cache");
> +	msb->cache_block_lba = MS_BLOCK_INVALID;
> +	bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block);
> +}
> +
> +static int msb_cache_init(struct msb_data *msb)
> +{
> +	setup_timer(&msb->cache_flush_timer, msb_cache_flush_timer,
> +		(unsigned long)msb);
> +
> +	if (!msb->cache)
> +		msb->cache = kzalloc(msb->block_size, GFP_KERNEL);
> +	if (!msb->cache)
> +		return -ENOMEM;
> +
> +	msb_cache_discard(msb);
> +	return 0;
> +}
> +
> +static int msb_cache_flush(struct msb_data *msb)
> +{
> +	struct scatterlist sg;
> +	struct ms_extra_data_register extra;
> +	int page, offset, error;
> +	u16 pba, lba;
> +
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	if (msb->cache_block_lba == MS_BLOCK_INVALID)
> +		return 0;
> +
> +	lba = msb->cache_block_lba;
> +	pba = msb->lba_to_pba_table[lba];
> +
> +	dbg_verbose("Flushing the write cache of pba %d (LBA %d)",
> +						pba, msb->cache_block_lba);
> +
> +	sg_init_one(&sg, msb->cache , msb->block_size);
> +
> +	/* Read all missing pages in cache */
> +	for (page = 0; page < msb->pages_in_block; page++) {
> +
> +		if (test_bit(page, &msb->valid_cache_bitmap))
> +			continue;
> +
> +		offset = page * msb->page_size;
> +
> +		dbg_verbose("reading non-present sector %d of cache block %d",
> +			page, lba);
> +		error = msb_read_page(msb, pba, page, &extra, &sg, offset);
> +
> +		/* Bad pages are copied with 00 page status */
> +		if (error == -EBADMSG) {
> +			pr_err("read error on sector %d, contents probably damaged", page);
> +			continue;
> +		}
> +
> +		if (error)
> +			return error;
> +
> +		if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) !=
> +							MEMSTICK_OV_PG_NORMAL) {
> +			dbg("page %d is marked as bad", page);
> +			continue;
> +		}
> +
> +		set_bit(page, &msb->valid_cache_bitmap);
> +	}
> +
> +	/* Write the cache now */
> +	error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
> +	pba = msb->lba_to_pba_table[msb->cache_block_lba];
> +
> +	/* Mark invalid pages */
> +	if (!error) {
> +		for (page = 0; page < msb->pages_in_block; page++) {
> +
> +			if (test_bit(page, &msb->valid_cache_bitmap))
> +				continue;
> +
> +			dbg("marking page %d as containing damaged data",
> +				page);
> +			msb_set_overwrite_flag(msb,
> +				pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL);
> +		}
> +	}
> +
> +	msb_cache_discard(msb);
> +	return error;
> +}
> +
> +static int msb_cache_write(struct msb_data *msb, int lba,
> +	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
> +{
> +	int error;
> +	struct scatterlist sg_tmp[10];
> +
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
> +						lba != msb->cache_block_lba)
> +		if (add_to_cache_only)
> +			return 0;
> +
> +	/* If we need to write different block */
> +	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
> +						lba != msb->cache_block_lba) {
> +		dbg_verbose("first flush the cache");
> +		error = msb_cache_flush(msb);
> +		if (error)
> +			return error;
> +	}
> +
> +	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
> +		msb->cache_block_lba  = lba;
> +		mod_timer(&msb->cache_flush_timer,
> +			jiffies + msecs_to_jiffies(cache_flush_timeout));
> +	}
> +
> +	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
> +
> +	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
> +	msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
> +
> +	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
> +		msb->cache + page * msb->page_size, msb->page_size);
> +
> +	set_bit(page, &msb->valid_cache_bitmap);
> +	return 0;
> +}
> +
> +static int msb_cache_read(struct msb_data *msb, int lba,
> +				int page, struct scatterlist *sg, int offset)
> +{
> +	int pba = msb->lba_to_pba_table[lba];
> +	struct scatterlist sg_tmp[10];
> +	int error = 0;
> +
> +	if (lba == msb->cache_block_lba &&
> +			test_bit(page, &msb->valid_cache_bitmap)) {
> +
> +		dbg_verbose("Read of LBA %d (pba %d) sector %d from cache",
> +							lba, pba, page);
> +
> +		sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
> +		msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp),
> +			offset, msb->page_size);
> +		sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp),
> +			msb->cache + msb->page_size * page,
> +							msb->page_size);
> +	} else {
> +		dbg_verbose("Read of LBA %d (pba %d) sector %d from device",
> +							lba, pba, page);
> +
> +		error = msb_read_page(msb, pba, page, NULL, sg, offset);
> +		if (error)
> +			return error;
> +
> +		msb_cache_write(msb, lba, page, true, sg, offset);
> +	}
> +	return error;
> +}
> +
> +/* Emulated geometry table
> + * This table content isn't that importaint,
> + * One could put here different values, providing that they still
> + * cover whole disk.
> + * 64 MB entry is what windows reports for my 64M memstick */
> +
> +static const struct chs_entry chs_table[] = {
> +/*        size sectors cylynders  heads */
> +	{ 4,    16,    247,       2  },
> +	{ 8,    16,    495,       2  },
> +	{ 16,   16,    495,       4  },
> +	{ 32,   16,    991,       4  },
> +	{ 64,   16,    991,       8  },
> +	{128,   16,    991,       16 },
> +	{ 0 }
> +};
> +
> +/* Load information about the card */
> +static int msb_init_card(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_host *host = card->host;
> +	struct ms_boot_page *boot_block;
> +	int error = 0, i, raw_size_in_megs;
> +
> +	msb->caps = 0;
> +
> +	if (card->id.class >= MEMSTICK_CLASS_ROM &&
> +				card->id.class <= MEMSTICK_CLASS_ROM)
> +		msb->read_only = true;
> +
> +	msb->state = -1;
> +	error = msb_reset(msb, false);
> +	if (error)
> +		return error;
> +
> +	/* Due to a bug in Jmicron driver written by Alex Dubov,
> +	 its serial mode barely works,
> +	 so we switch to parallel mode right away */
> +	if (host->caps & MEMSTICK_CAP_PAR4)
> +		msb_switch_to_parallel(msb);
> +
> +	msb->page_size = sizeof(struct ms_boot_page);
> +
> +	/* Read the boot page */
> +	error = msb_read_boot_blocks(msb);
> +	if (error)
> +		return -EIO;
> +
> +	boot_block = &msb->boot_page[0];
> +
> +	/* Save intersting attributes from boot page */
> +	msb->block_count = boot_block->attr.number_of_blocks;
> +	msb->page_size = boot_block->attr.page_size;
> +
> +	msb->pages_in_block = boot_block->attr.block_size * 2;
> +	msb->block_size = msb->page_size * msb->pages_in_block;
> +
> +	if (msb->page_size > PAGE_SIZE) {
> +		/* this isn't supported by linux at all, anyway*/
> +		dbg("device page %d size isn't supported", msb->page_size);
> +		return -EINVAL;
> +	}
> +
> +	msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL);
> +	if (!msb->block_buffer)
> +		return -ENOMEM;
> +
> +	raw_size_in_megs = (msb->block_size * msb->block_count) >> 20;
> +
> +	for (i = 0; chs_table[i].size; i++) {
> +
> +		if (chs_table[i].size != raw_size_in_megs)
> +			continue;
> +
> +		msb->geometry.cylinders = chs_table[i].cyl;
> +		msb->geometry.heads = chs_table[i].head;
> +		msb->geometry.sectors = chs_table[i].sec;
> +		break;
> +	}
> +
> +	if (boot_block->attr.transfer_supporting == 1)
> +		msb->caps |= MEMSTICK_CAP_PAR4;
> +
> +	if (boot_block->attr.device_type & 0x03)
> +		msb->read_only = true;
> +
> +	dbg("Total block count = %d", msb->block_count);
> +	dbg("Each block consists of %d pages", msb->pages_in_block);
> +	dbg("Page size = %d bytes", msb->page_size);
> +	dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4));
> +	dbg("Read only: %d", msb->read_only);
> +
> +#if 0
> +	/* Now we can switch the interface */
> +	if (host->caps & msb->caps & MEMSTICK_CAP_PAR4)
> +		msb_switch_to_parallel(msb);
> +#endif
> +
> +	error = msb_cache_init(msb);
> +	if (error)
> +		return error;
> +
> +	error = msb_ftl_initialize(msb);
> +	if (error)
> +		return error;
> +
> +
> +	/* Read the bad block table */
> +	error = msb_read_bad_block_table(msb, 0);
> +
> +	if (error && error != -ENOMEM) {
> +		dbg("failed to read bad block table from primary boot block, trying from backup");
> +		error = msb_read_bad_block_table(msb, 1);
> +	}
> +
> +	if (error)
> +		return error;
> +
> +	/* *drum roll* Scan the media */
> +	error = msb_ftl_scan(msb);
> +	if (error) {
> +		pr_err("Scan of media failed");
> +		return error;
> +	}
> +
> +	return 0;
> +
> +}
> +
> +static int msb_do_write_request(struct msb_data *msb, int lba,
> +	int page, struct scatterlist *sg, size_t len, int *sucessfuly_written)
> +{
> +	int error = 0;
> +	off_t offset = 0;
> +	*sucessfuly_written = 0;
> +
> +	while (offset < len) {
> +		if (page == 0 && len - offset >= msb->block_size) {
> +
> +			if (msb->cache_block_lba == lba)
> +				msb_cache_discard(msb);
> +
> +			dbg_verbose("Writing whole lba %d", lba);
> +			error = msb_update_block(msb, lba, sg, offset);
> +			if (error)
> +				return error;
> +
> +			offset += msb->block_size;
> +			*sucessfuly_written += msb->block_size;
> +			lba++;
> +			continue;
> +		}
> +
> +		error = msb_cache_write(msb, lba, page, false, sg, offset);
> +		if (error)
> +			return error;
> +
> +		offset += msb->page_size;
> +		*sucessfuly_written += msb->page_size;
> +
> +		page++;
> +		if (page == msb->pages_in_block) {
> +			page = 0;
> +			lba++;
> +		}
> +	}
> +	return 0;
> +}
> +
> +static int msb_do_read_request(struct msb_data *msb, int lba,
> +		int page, struct scatterlist *sg, int len, int *sucessfuly_read)
> +{
> +	int error = 0;
> +	int offset = 0;
> +	*sucessfuly_read = 0;
> +
> +	while (offset < len) {
> +
> +		error = msb_cache_read(msb, lba, page, sg, offset);
> +		if (error)
> +			return error;
> +
> +		offset += msb->page_size;
> +		*sucessfuly_read += msb->page_size;
> +
> +		page++;
> +		if (page == msb->pages_in_block) {
> +			page = 0;
> +			lba++;
> +		}
> +	}
> +	return 0;
> +}
> +
> +static void msb_io_work(struct work_struct *work)
> +{
> +	struct msb_data *msb = container_of(work, struct msb_data, io_work);
> +	int page, error, len;
> +	sector_t lba;
> +	unsigned long flags;
> +	struct scatterlist *sg = msb->prealloc_sg;
> +
> +	dbg_verbose("IO: work started");
> +
> +	while (1) {
> +		spin_lock_irqsave(&msb->q_lock, flags);
> +
> +		if (msb->need_flush_cache) {
> +			msb->need_flush_cache = false;
> +			spin_unlock_irqrestore(&msb->q_lock, flags);
> +			msb_cache_flush(msb);
> +			continue;
> +		}
> +
> +		if (!msb->req) {
> +			msb->req = blk_fetch_request(msb->queue);
> +			if (!msb->req) {
> +				dbg_verbose("IO: no more requests exiting");
> +				spin_unlock_irqrestore(&msb->q_lock, flags);
> +				return;
> +			}
> +		}
> +
> +		spin_unlock_irqrestore(&msb->q_lock, flags);
> +
> +		/* If card was removed meanwhile */
> +		if (!msb->req)
> +			return;
> +
> +		/* process the request */
> +		dbg_verbose("IO: processing new request");
> +		blk_rq_map_sg(msb->queue, msb->req, sg);
> +
> +		lba = blk_rq_pos(msb->req);
> +
> +		sector_div(lba, msb->page_size / 512);
> +		page = do_div(lba, msb->pages_in_block);
> +
> +		if (rq_data_dir(msb->req) == READ)
> +			error = msb_do_read_request(msb, lba, page, sg,
> +				blk_rq_bytes(msb->req), &len);
> +		else
> +			error = msb_do_write_request(msb, lba, page, sg,
> +				blk_rq_bytes(msb->req), &len);
> +
> +		spin_lock_irqsave(&msb->q_lock, flags);
> +
> +		if (len)
> +			if (!__blk_end_request(msb->req, 0, len))
> +				msb->req = NULL;
> +
> +		if (error && msb->req) {
> +			dbg_verbose("IO: ending one sector of the request with error");
> +			if (!__blk_end_request(msb->req, error, msb->page_size))
> +				msb->req = NULL;
> +		}
> +
> +		if (msb->req)
> +			dbg_verbose("IO: request still pending");
> +
> +		spin_unlock_irqrestore(&msb->q_lock, flags);
> +	}
> +}
> +
> +static DEFINE_IDR(msb_disk_idr); /*set of used disk numbers */
> +static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */
> +
> +static int msb_bd_open(struct block_device *bdev, fmode_t mode)
> +{
> +	struct gendisk *disk = bdev->bd_disk;
> +	struct msb_data *msb = disk->private_data;
> +
> +	dbg_verbose("block device open");
> +
> +	mutex_lock(&msb_disk_lock);
> +
> +	if (msb && msb->card)
> +		msb->usage_count++;
> +
> +	mutex_unlock(&msb_disk_lock);
> +	return 0;
> +}
> +
> +static void msb_data_clear(struct msb_data *msb)
> +{
> +	kfree(msb->boot_page);
> +	kfree(msb->used_blocks_bitmap);
> +	kfree(msb->lba_to_pba_table);
> +	kfree(msb->cache);
> +	msb->card = NULL;
> +}
> +
> +static int msb_disk_release(struct gendisk *disk)
> +{
> +	struct msb_data *msb = disk->private_data;
> +
> +	dbg_verbose("block device release");
> +	mutex_lock(&msb_disk_lock);
> +
> +	if (msb) {
> +		if (msb->usage_count)
> +			msb->usage_count--;
> +
> +		if (!msb->usage_count) {
> +			disk->private_data = NULL;
> +			idr_remove(&msb_disk_idr, msb->disk_id);
> +			put_disk(disk);
> +			kfree(msb);
> +		}
> +	}
> +	mutex_unlock(&msb_disk_lock);
> +	return 0;
> +}
> +
> +static void msb_bd_release(struct gendisk *disk, fmode_t mode)
> +{
> +	msb_disk_release(disk);
> +}
> +
> +static int msb_bd_getgeo(struct block_device *bdev,
> +				 struct hd_geometry *geo)
> +{
> +	struct msb_data *msb = bdev->bd_disk->private_data;
> +	*geo = msb->geometry;
> +	return 0;
> +}
> +
> +static int msb_prepare_req(struct request_queue *q, struct request *req)
> +{
> +	if (req->cmd_type != REQ_TYPE_FS &&
> +				req->cmd_type != REQ_TYPE_BLOCK_PC) {
> +		blk_dump_rq_flags(req, "MS unsupported request");
> +		return BLKPREP_KILL;
> +	}
> +	req->cmd_flags |= REQ_DONTPREP;
> +	return BLKPREP_OK;
> +}
> +
> +static void msb_submit_req(struct request_queue *q)
> +{
> +	struct memstick_dev *card = q->queuedata;
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct request *req = NULL;
> +
> +	dbg_verbose("Submit request");
> +
> +	if (msb->card_dead) {
> +		dbg("Refusing requests on removed card");
> +
> +		WARN_ON(!msb->io_queue_stopped);
> +
> +		while ((req = blk_fetch_request(q)) != NULL)
> +			__blk_end_request_all(req, -ENODEV);
> +		return;
> +	}
> +
> +	if (msb->req)
> +		return;
> +
> +	if (!msb->io_queue_stopped)
> +		queue_work(msb->io_queue, &msb->io_work);
> +}
> +
> +static int msb_check_card(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	return (msb->card_dead == 0);
> +}
> +
> +static void msb_stop(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	unsigned long flags;
> +
> +	dbg("Stopping all msblock IO");
> +
> +	spin_lock_irqsave(&msb->q_lock, flags);
> +	blk_stop_queue(msb->queue);
> +	msb->io_queue_stopped = true;
> +	spin_unlock_irqrestore(&msb->q_lock, flags);
> +
> +	del_timer_sync(&msb->cache_flush_timer);
> +	flush_workqueue(msb->io_queue);
> +
> +	if (msb->req) {
> +		spin_lock_irqsave(&msb->q_lock, flags);
> +		blk_requeue_request(msb->queue, msb->req);
> +		msb->req = NULL;
> +		spin_unlock_irqrestore(&msb->q_lock, flags);
> +	}
> +
> +}
> +
> +static void msb_start(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	unsigned long flags;
> +
> +	dbg("Resuming IO from msblock");
> +
> +	msb_invalidate_reg_window(msb);
> +
> +	spin_lock_irqsave(&msb->q_lock, flags);
> +	if (!msb->io_queue_stopped || msb->card_dead) {
> +		spin_unlock_irqrestore(&msb->q_lock, flags);
> +		return;
> +	}
> +	spin_unlock_irqrestore(&msb->q_lock, flags);
> +
> +	/* Kick cache flush anyway, its harmless */
> +	msb->need_flush_cache = true;
> +	msb->io_queue_stopped = false;
> +
> +	spin_lock_irqsave(&msb->q_lock, flags);
> +	blk_start_queue(msb->queue);
> +	spin_unlock_irqrestore(&msb->q_lock, flags);
> +
> +	queue_work(msb->io_queue, &msb->io_work);
> +
> +}
> +
> +static const struct block_device_operations msb_bdops = {
> +	.open    = msb_bd_open,
> +	.release = msb_bd_release,
> +	.getgeo  = msb_bd_getgeo,
> +	.owner   = THIS_MODULE
> +};
> +
> +/* Registers the block device */
> +static int msb_init_disk(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_host *host = card->host;
> +	int rc;
> +	u64 limit = BLK_BOUNCE_HIGH;
> +	unsigned long capacity;
> +
> +	if (host->dev.dma_mask && *(host->dev.dma_mask))
> +		limit = *(host->dev.dma_mask);
> +
> +	mutex_lock(&msb_disk_lock);
> +	msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL);
> +	mutex_unlock(&msb_disk_lock);
> +
> +	if (msb->disk_id  < 0)
> +		return msb->disk_id;
> +
> +	msb->disk = alloc_disk(0);
> +	if (!msb->disk) {
> +		rc = -ENOMEM;
> +		goto out_release_id;
> +	}
> +
> +	msb->queue = blk_init_queue(msb_submit_req, &msb->q_lock);
> +	if (!msb->queue) {
> +		rc = -ENOMEM;
> +		goto out_put_disk;
> +	}
> +
> +	msb->queue->queuedata = card;
> +	blk_queue_prep_rq(msb->queue, msb_prepare_req);
> +
> +	blk_queue_bounce_limit(msb->queue, limit);
> +	blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
> +	blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
> +	blk_queue_max_segment_size(msb->queue,
> +				   MS_BLOCK_MAX_PAGES * msb->page_size);
> +	blk_queue_logical_block_size(msb->queue, msb->page_size);
> +
> +	sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
> +	msb->disk->fops = &msb_bdops;
> +	msb->disk->private_data = msb;
> +	msb->disk->queue = msb->queue;
> +	msb->disk->driverfs_dev = &card->dev;
> +	msb->disk->flags |= GENHD_FL_EXT_DEVT;
> +
> +	capacity = msb->pages_in_block * msb->logical_block_count;
> +	capacity *= (msb->page_size / 512);
> +	set_capacity(msb->disk, capacity);
> +	dbg("Set total disk size to %lu sectors", capacity);
> +
> +	msb->usage_count = 1;
> +	msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
> +	INIT_WORK(&msb->io_work, msb_io_work);
> +	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
> +
> +	if (msb->read_only)
> +		set_disk_ro(msb->disk, 1);
> +
> +	msb_start(card);
> +	add_disk(msb->disk);
> +	dbg("Disk added");
> +	return 0;
> +
> +out_put_disk:
> +	put_disk(msb->disk);
> +out_release_id:
> +	mutex_lock(&msb_disk_lock);
> +	idr_remove(&msb_disk_idr, msb->disk_id);
> +	mutex_unlock(&msb_disk_lock);
> +	return rc;
> +}
> +
> +static int msb_probe(struct memstick_dev *card)
> +{
> +	struct msb_data *msb;
> +	int rc = 0;
> +
> +	msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
> +	if (!msb)
> +		return -ENOMEM;
> +	memstick_set_drvdata(card, msb);
> +	msb->card = card;
> +	spin_lock_init(&msb->q_lock);
> +
> +	rc = msb_init_card(card);
> +	if (rc)
> +		goto out_free;
> +
> +	rc = msb_init_disk(card);
> +	if (!rc) {
> +		card->check = msb_check_card;
> +		card->stop = msb_stop;
> +		card->start = msb_start;
> +		return 0;
> +	}
> +out_free:
> +	memstick_set_drvdata(card, NULL);
> +	msb_data_clear(msb);
> +	kfree(msb);
> +	return rc;
> +}
> +
> +static void msb_remove(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	unsigned long flags;
> +
> +	if (!msb->io_queue_stopped)
> +		msb_stop(card);
> +
> +	dbg("Removing the disk device");
> +
> +	/* Take care of unhandled + new requests from now on */
> +	spin_lock_irqsave(&msb->q_lock, flags);
> +	msb->card_dead = true;
> +	blk_start_queue(msb->queue);
> +	spin_unlock_irqrestore(&msb->q_lock, flags);
> +
> +	/* Remove the disk */
> +	del_gendisk(msb->disk);
> +	blk_cleanup_queue(msb->queue);
> +	msb->queue = NULL;
> +
> +	mutex_lock(&msb_disk_lock);
> +	msb_data_clear(msb);
> +	mutex_unlock(&msb_disk_lock);
> +
> +	msb_disk_release(msb->disk);
> +	memstick_set_drvdata(card, NULL);
> +}
> +
> +#ifdef CONFIG_PM
> +
> +static int msb_suspend(struct memstick_dev *card, pm_message_t state)
> +{
> +	msb_stop(card);
> +	return 0;
> +}
> +
> +static int msb_resume(struct memstick_dev *card)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct msb_data *new_msb = NULL;
> +	bool card_dead = true;
> +
> +#ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
> +	msb->card_dead = true;
> +	return 0;
> +#endif
> +	mutex_lock(&card->host->lock);
> +
> +	new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
> +	if (!new_msb)
> +		goto out;
> +
> +	new_msb->card = card;
> +	memstick_set_drvdata(card, new_msb);
> +	spin_lock_init(&new_msb->q_lock);
> +	sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
> +
> +	if (msb_init_card(card))
> +		goto out;
> +
> +	if (msb->block_size != new_msb->block_size)
> +		goto out;
> +
> +	if (memcmp(msb->boot_page, new_msb->boot_page,
> +					sizeof(struct ms_boot_page)))
> +		goto out;
> +
> +	if (msb->logical_block_count != new_msb->logical_block_count ||
> +		memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
> +						msb->logical_block_count))
> +		goto out;
> +
> +	if (msb->block_count != new_msb->block_count ||
> +		memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
> +							msb->block_count / 8))
> +		goto out;
> +
> +	card_dead = false;
> +out:
> +	if (card_dead)
> +		dbg("Card was removed/replaced during suspend");
> +
> +	msb->card_dead = card_dead;
> +	memstick_set_drvdata(card, msb);
> +
> +	if (new_msb) {
> +		msb_data_clear(new_msb);
> +		kfree(new_msb);
> +	}
> +
> +	msb_start(card);
> +	mutex_unlock(&card->host->lock);
> +	return 0;
> +}
> +#else
> +
> +#define msb_suspend NULL
> +#define msb_resume NULL
> +
> +#endif /* CONFIG_PM */
> +
> +static struct memstick_device_id msb_id_tbl[] = {
> +	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
> +	 MEMSTICK_CLASS_FLASH},
> +
> +	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
> +	 MEMSTICK_CLASS_ROM},
> +
> +	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
> +	 MEMSTICK_CLASS_RO},
> +
> +	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
> +	 MEMSTICK_CLASS_WP},
> +
> +	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
> +	 MEMSTICK_CLASS_DUO},
> +	{}
> +};
> +MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
> +
> +
> +static struct memstick_driver msb_driver = {
> +	.driver = {
> +		.name  = DRIVER_NAME,
> +		.owner = THIS_MODULE
> +	},
> +	.id_table = msb_id_tbl,
> +	.probe    = msb_probe,
> +	.remove   = msb_remove,
> +	.suspend  = msb_suspend,
> +	.resume   = msb_resume
> +};
> +
> +static int major;
> +
> +static int __init msb_init(void)
> +{
> +	int rc = register_blkdev(0, DRIVER_NAME);
> +
> +	if (rc < 0) {
> +		pr_err("failed to register major (error %d)\n", rc);
> +		return rc;
> +	}
> +
> +	major = rc;
> +	rc = memstick_register_driver(&msb_driver);
> +	if (rc) {
> +		unregister_blkdev(major, DRIVER_NAME);
> +		pr_err("failed to register memstick driver (error %d)\n", rc);
> +	}
> +
> +	return rc;
> +}
> +
> +static void __exit msb_exit(void)
> +{
> +	memstick_unregister_driver(&msb_driver);
> +	unregister_blkdev(major, DRIVER_NAME);
> +	idr_destroy(&msb_disk_idr);
> +}
> +
> +module_init(msb_init);
> +module_exit(msb_exit);
> +
> +module_param(cache_flush_timeout, int, S_IRUGO);
> +MODULE_PARM_DESC(cache_flush_timeout,
> +				"Cache flush timeout in msec (1000 default)");
> +module_param(debug, int, S_IRUGO | S_IWUSR);
> +MODULE_PARM_DESC(debug, "Debug level (0-2)");
> +
> +module_param(verify_writes, bool, S_IRUGO);
> +MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
> +
> +MODULE_LICENSE("GPL");
> +MODULE_AUTHOR("Maxim Levitsky");
> +MODULE_DESCRIPTION("Sony MemoryStick block device driver");
> diff --git a/drivers/memstick/core/ms_block.h b/drivers/memstick/core/ms_block.h
> new file mode 100644
> index 0000000..96e6375
> --- /dev/null
> +++ b/drivers/memstick/core/ms_block.h
> @@ -0,0 +1,290 @@
> +/*
> + *  ms_block.h - Sony MemoryStick (legacy) storage support
> +
> + *  Copyright (C) 2013 Maxim Levitsky <maximlevitsky@gmail.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * Minor portions of the driver are copied from mspro_block.c which is
> + * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
> + *
> + * Also ms structures were copied from old broken driver by same author
> + * These probably come from MS spec
> + *
> + */
> +
> +#ifndef MS_BLOCK_NEW_H
> +#define MS_BLOCK_NEW_H
> +
> +#define MS_BLOCK_MAX_SEGS      32
> +#define MS_BLOCK_MAX_PAGES     ((2 << 16) - 1)
> +
> +#define MS_BLOCK_MAX_BOOT_ADDR 0x000c
> +#define MS_BLOCK_BOOT_ID       0x0001
> +#define MS_BLOCK_INVALID       0xffff
> +#define MS_MAX_ZONES           16
> +#define MS_BLOCKS_IN_ZONE      512
> +
> +#define MS_BLOCK_MAP_LINE_SZ   16
> +#define MS_BLOCK_PART_SHIFT    3
> +
> +
> +#define MEMSTICK_UNCORR_ERROR (MEMSTICK_STATUS1_UCFG | \
> +		MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCDT)
> +
> +#define MEMSTICK_CORR_ERROR (MEMSTICK_STATUS1_FGER | MEMSTICK_STATUS1_EXER | \
> +	MEMSTICK_STATUS1_DTER)
> +
> +#define MEMSTICK_INT_ERROR (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)
> +
> +#define MEMSTICK_OVERWRITE_FLAG_NORMAL \
> +	(MEMSTICK_OVERWRITE_PGST1 | \
> +	MEMSTICK_OVERWRITE_PGST0  | \
> +	MEMSTICK_OVERWRITE_BKST)
> +
> +#define MEMSTICK_OV_PG_NORMAL \
> +	(MEMSTICK_OVERWRITE_PGST1 | MEMSTICK_OVERWRITE_PGST0)
> +
> +#define MEMSTICK_MANAGMENT_FLAG_NORMAL \
> +	(MEMSTICK_MANAGEMENT_SYSFLG |  \
> +	MEMSTICK_MANAGEMENT_SCMS1   |  \
> +	MEMSTICK_MANAGEMENT_SCMS0)     \
> +
> +struct ms_boot_header {
> +	unsigned short block_id;
> +	unsigned short format_reserved;
> +	unsigned char  reserved0[184];
> +	unsigned char  data_entry;
> +	unsigned char  reserved1[179];
> +} __packed;
> +
> +
> +struct ms_system_item {
> +	unsigned int  start_addr;
> +	unsigned int  data_size;
> +	unsigned char data_type_id;
> +	unsigned char reserved[3];
> +} __packed;
> +
> +struct ms_system_entry {
> +	struct ms_system_item disabled_block;
> +	struct ms_system_item cis_idi;
> +	unsigned char         reserved[24];
> +} __packed;
> +
> +struct ms_boot_attr_info {
> +	unsigned char      memorystick_class;
> +	unsigned char      format_unique_value1;
> +	unsigned short     block_size;
> +	unsigned short     number_of_blocks;
> +	unsigned short     number_of_effective_blocks;
> +	unsigned short     page_size;
> +	unsigned char      extra_data_size;
> +	unsigned char      format_unique_value2;
> +	unsigned char      assembly_time[8];
> +	unsigned char      format_unique_value3;
> +	unsigned char      serial_number[3];
> +	unsigned char      assembly_manufacturer_code;
> +	unsigned char      assembly_model_code[3];
> +	unsigned short     memory_manufacturer_code;
> +	unsigned short     memory_device_code;
> +	unsigned short     implemented_capacity;
> +	unsigned char      format_unique_value4[2];
> +	unsigned char      vcc;
> +	unsigned char      vpp;
> +	unsigned short     controller_number;
> +	unsigned short     controller_function;
> +	unsigned char      reserved0[9];
> +	unsigned char      transfer_supporting;
> +	unsigned short     format_unique_value5;
> +	unsigned char      format_type;
> +	unsigned char      memorystick_application;
> +	unsigned char      device_type;
> +	unsigned char      reserved1[22];
> +	unsigned char      format_uniqure_value6[2];
> +	unsigned char      reserved2[15];
> +} __packed;
> +
> +struct ms_cis_idi {
> +	unsigned short general_config;
> +	unsigned short logical_cylinders;
> +	unsigned short reserved0;
> +	unsigned short logical_heads;
> +	unsigned short track_size;
> +	unsigned short page_size;
> +	unsigned short pages_per_track;
> +	unsigned short msw;
> +	unsigned short lsw;
> +	unsigned short reserved1;
> +	unsigned char  serial_number[20];
> +	unsigned short buffer_type;
> +	unsigned short buffer_size_increments;
> +	unsigned short long_command_ecc;
> +	unsigned char  firmware_version[28];
> +	unsigned char  model_name[18];
> +	unsigned short reserved2[5];
> +	unsigned short pio_mode_number;
> +	unsigned short dma_mode_number;
> +	unsigned short field_validity;
> +	unsigned short current_logical_cylinders;
> +	unsigned short current_logical_heads;
> +	unsigned short current_pages_per_track;
> +	unsigned int   current_page_capacity;
> +	unsigned short mutiple_page_setting;
> +	unsigned int   addressable_pages;
> +	unsigned short single_word_dma;
> +	unsigned short multi_word_dma;
> +	unsigned char  reserved3[128];
> +} __packed;
> +
> +
> +struct ms_boot_page {
> +	struct ms_boot_header    header;
> +	struct ms_system_entry   entry;
> +	struct ms_boot_attr_info attr;
> +} __packed;
> +
> +struct msb_data {
> +	unsigned int			usage_count;
> +	struct memstick_dev		*card;
> +	struct gendisk			*disk;
> +	struct request_queue		*queue;
> +	spinlock_t			q_lock;
> +	struct hd_geometry		geometry;
> +	struct attribute_group		attr_group;
> +	struct request			*req;
> +	int				caps;
> +	int				disk_id;
> +
> +	/* IO */
> +	struct workqueue_struct		*io_queue;
> +	bool				io_queue_stopped;
> +	struct work_struct		io_work;
> +	bool				card_dead;
> +
> +	/* Media properties */
> +	struct ms_boot_page		*boot_page;
> +	u16				boot_block_locations[2];
> +	int				boot_block_count;
> +
> +	bool				read_only;
> +	unsigned short			page_size;
> +	int				block_size;
> +	int				pages_in_block;
> +	int				zone_count;
> +	int				block_count;
> +	int				logical_block_count;
> +
> +	/* FTL tables */
> +	unsigned long			*used_blocks_bitmap;
> +	unsigned long			*erased_blocks_bitmap;
> +	u16				*lba_to_pba_table;
> +	int				free_block_count[MS_MAX_ZONES];
> +	bool				ftl_initialized;
> +
> +	/* Cache */
> +	unsigned char			*cache;
> +	unsigned long			valid_cache_bitmap;
> +	int				cache_block_lba;
> +	bool				need_flush_cache;
> +	struct timer_list		cache_flush_timer;
> +
> +	/* Preallocated buffers */
> +	unsigned char			*block_buffer;
> +	struct scatterlist		prealloc_sg[MS_BLOCK_MAX_SEGS+1];
> +
> +
> +	/* handler's local data */
> +	struct ms_register_addr		reg_addr;
> +	bool				addr_valid;
> +
> +	u8				command_value;
> +	bool				command_need_oob;
> +	struct scatterlist		*current_sg;
> +	int				current_sg_offset;
> +
> +	struct ms_register		regs;
> +	int				current_page;
> +
> +	int				state;
> +	int				exit_error;
> +	bool				int_polling;
> +	unsigned long			int_timeout;
> +
> +};
> +
> +enum msb_readpage_states {
> +	MSB_RP_SEND_BLOCK_ADDRESS = 0,
> +	MSB_RP_SEND_READ_COMMAND,
> +
> +	MSB_RP_SEND_INT_REQ,
> +	MSB_RP_RECEIVE_INT_REQ_RESULT,
> +
> +	MSB_RP_SEND_READ_STATUS_REG,
> +	MSB_RP_RECIVE_STATUS_REG,
> +
> +	MSB_RP_SEND_OOB_READ,
> +	MSB_RP_RECEIVE_OOB_READ,
> +
> +	MSB_RP_SEND_READ_DATA,
> +	MSB_RP_RECEIVE_READ_DATA,
> +};
> +
> +enum msb_write_block_states {
> +	MSB_WB_SEND_WRITE_PARAMS = 0,
> +	MSB_WB_SEND_WRITE_OOB,
> +	MSB_WB_SEND_WRITE_COMMAND,
> +
> +	MSB_WB_SEND_INT_REQ,
> +	MSB_WB_RECEIVE_INT_REQ,
> +
> +	MSB_WB_SEND_WRITE_DATA,
> +	MSB_WB_RECEIVE_WRITE_CONFIRMATION,
> +};
> +
> +enum msb_send_command_states {
> +	MSB_SC_SEND_WRITE_PARAMS,
> +	MSB_SC_SEND_WRITE_OOB,
> +	MSB_SC_SEND_COMMAND,
> +
> +	MSB_SC_SEND_INT_REQ,
> +	MSB_SC_RECEIVE_INT_REQ,
> +
> +};
> +
> +enum msb_reset_states {
> +	MSB_RS_SEND,
> +	MSB_RS_CONFIRM,
> +};
> +
> +enum msb_par_switch_states {
> +	MSB_PS_SEND_SWITCH_COMMAND,
> +	MSB_PS_SWICH_HOST,
> +	MSB_PS_CONFIRM,
> +};
> +
> +struct chs_entry {
> +	unsigned long size;
> +	unsigned char sec;
> +	unsigned short cyl;
> +	unsigned char head;
> +};
> +
> +static int msb_reset(struct msb_data *msb, bool full);
> +
> +static int h_msb_default_bad(struct memstick_dev *card,
> +						struct memstick_request **mrq);
> +
> +#define __dbg(level, format, ...) \
> +	do { \
> +		if (debug >= level) \
> +			pr_err(format "\n", ## __VA_ARGS__); \
> +	} while (0)
> +
> +
> +#define dbg(format, ...)		__dbg(1, format, ## __VA_ARGS__)
> +#define dbg_verbose(format, ...)	__dbg(2, format, ## __VA_ARGS__)
> +
> +#endif

-- 
Best regards,
        Maxim Levitsky

Visit my blog: http://maximlevitsky.wordpress.com
Warning: Above blog contains rants.

Re: [PATCH] memstick: add support for legacy memorysticks

[Andrew Morton]

On Thu, 11 Jul 2013 03:28:37 +0300 Maxim Levitsky <maximlevitsky@gmail.com> wrote:

> On Mon, 2013-07-08 at 23:54 +0300, Maxim Levitsky wrote: 
> > Based partially on MS standard spec quotes from Alex Dubov.
> > 
> > As any code that works with user data this driver isn't
> > recommended to use to write cards that contain valuable data.
> > 
> > It tries its best though to avoid data corruption
> > and possible damage to the card.
> > 
> > Tested on MS DUO 64 MB card on Ricoh R592 card reader.
> > 

(top-posting repaired.  Please don't do that).

> Any update?

You sent a large patch in the middle of the 3.11 merge window, when
nobody is looking at large new patches.

I have it saved away for consideration after -rc1.

Re: [PATCH] memstick: add support for legacy memorysticks

[Andrew Morton]

On Mon,  8 Jul 2013 23:54:55 +0300 Maxim Levitsky <maximlevitsky@gmail.com> wrote:

> Based partially on MS standard spec quotes from Alex Dubov.
> 
> As any code that works with user data this driver isn't
> recommended to use to write cards that contain valuable data.
> 
> It tries its best though to avoid data corruption
> and possible damage to the card.
> 
> Tested on MS DUO 64 MB card on Ricoh R592 card reader.
> 
> ...
>
> +config MS_BLOCK
> +	tristate "MemoryStick Standard device driver"
> +	depends on BLOCK
> +	help
> +	  Say Y here to enable the MemoryStick Standard device driver
> +	  support. This provides a block device driver, which you can use
> +	  to mount the filesystem.
> +	  This driver works with old (bulky) MemoryStick and MemoryStick Duo
> +	  but not PRO. Say Y if you have such card.
> +	  Driver is new and not yet well tested, thus it can damage your card
> +	  (even permanently)

Yikes.  How true is this?

> 
> ...
>
> +static size_t msb_sg_copy(struct scatterlist *sg_from,
> +	struct scatterlist *sg_to, int to_nents, size_t offset, size_t len)
> +{
> +	size_t copied = 0;
> +
> +	while (offset > 0) {
> +		if (offset >= sg_from->length) {
> +			if (sg_is_last(sg_from))
> +				return 0;
> +
> +			offset -= sg_from->length;
> +			sg_from = sg_next(sg_from);
> +			continue;
> +		}
> +
> +		copied = min(len, sg_from->length - offset);
> +		sg_set_page(sg_to, sg_page(sg_from),
> +			copied, sg_from->offset + offset);
> +
> +		len -= copied;
> +		offset = 0;
> +
> +		if (sg_is_last(sg_from) || !len)
> +			goto out;
> +
> +		sg_to = sg_next(sg_to);
> +		to_nents--;
> +		sg_from = sg_next(sg_from);
> +	}
> +
> +	while (len > sg_from->length && to_nents--) {
> +		len -= sg_from->length;
> +		copied += sg_from->length;
> +
> +		sg_set_page(sg_to, sg_page(sg_from),
> +				sg_from->length, sg_from->offset);
> +
> +		if (sg_is_last(sg_from) || !len)
> +			goto out;
> +
> +		sg_from = sg_next(sg_from);
> +		sg_to = sg_next(sg_to);
> +	}
> +
> +	if (len && to_nents) {
> +		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
> +		copied += len;
> +	}
> +out:
> +	sg_mark_end(sg_to);
> +	return copied;
> +}

There's nothing memstick-specific about this.  Did you consider placing
it in lib/?


> +/*
> + * Compares section of 'sg' starting from offset 'offset' and with length 'len'
> + * to linear buffer of length 'len' at address 'buffer'
> + * Returns 0 if equal and  -1 otherwice
> + */
> +static int msb_sg_compare_to_buffer(struct scatterlist *sg,
> +					size_t offset, u8 *buffer, size_t len)
> +{
> +	int retval = 0, cmplen;
> +	struct sg_mapping_iter miter;
> +
> +	sg_miter_start(&miter, sg, sg_nents(sg),
> +					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
> +
> +	while (sg_miter_next(&miter) && len > 0) {
> +		if (offset >= miter.length) {
> +			offset -= miter.length;
> +			continue;
> +		}
> +
> +		cmplen = min(miter.length - offset, len);
> +		retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
> +		if (retval)
> +			break;
> +
> +		buffer += cmplen;
> +		len -= cmplen;
> +		offset = 0;
> +	}
> +
> +	if (!retval && len)
> +		retval = -1;
> +
> +	sg_miter_stop(&miter);
> +	return retval;
> +}

And this, perhaps.

> 
> ...
>
> +static void msb_mark_block_used(struct msb_data *msb, int pba)
> +{
> +	int zone = msb_get_zone_from_pba(pba);
> +
> +	if (test_bit(pba, msb->used_blocks_bitmap)) {
> +		pr_err(
> +		"BUG: attempt to mark already used pba %d as used", pba);
> +		msb->read_only = true;
> +		return;
> +	}
> +
> +	if (msb_validate_used_block_bitmap(msb))
> +		return;
> +
> +	/* No races because all IO is single threaded */

What guarantees that all IO is single threaded?  Big lock?  All done by
a single kernel thread?

> +	__set_bit(pba, msb->used_blocks_bitmap);
> +	msb->free_block_count[zone]--;
> +}
> +
> 
> ...
>
> +static int msb_read_int_reg(struct msb_data *msb, long timeout)
> +{
> +	struct memstick_request *mrq = &msb->card->current_mrq;
> +
> +	WARN_ON(msb->state == -1);
> +
> +	if (!msb->int_polling) {
> +		msb->int_timeout = jiffies +
> +			msecs_to_jiffies(timeout == -1 ? 500 : timeout);

All callers pass timeout=-1, so there's some pointless code here.

> +		msb->int_polling = true;
> +	} else if (time_after(jiffies, msb->int_timeout)) {
> +		mrq->data[0] = MEMSTICK_INT_CMDNAK;
> +		return 0;
> +	}
> +
> +	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
> +				mrq->need_card_int && !mrq->error) {
> +		mrq->data[0] = mrq->int_reg;
> +		mrq->need_card_int = false;
> +		return 0;
> +	} else {
> +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> +		return 1;
> +	}
> +}

Generally speaking, one wya in whcih kernel code gets real confusing
real fast is when it uses a mixture of time units: jiffies and
milliseconds and seconds, etc.  This code falls into that trap, a
little bit.  One really good way to clarify things is to embed the
units within the variable names.  So s/timeout/timeout_ms/ and
s/int_timeout/int_timeout_jifs/ would be good.

> 
> ...
>
> +static int msb_switch_to_parallel(struct msb_data *msb)
> +{
> +	int error;
> +
> +	error = msb_run_state_machine(msb, h_msb_parallel_switch);
> +	if (error) {
> +		pr_err("Switch to parallel failed");

Might want to display the value of `error' here.

> +		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
> +		msb_reset(msb, true);
> +		return -EFAULT;

EFAULT is just wrong, surely.  Can't we propagate `error' here?

> +	}
> +
> +	msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
> +	return 0;
> +}
> +
> 
> ...
>
> +static u16 msb_get_free_block(struct msb_data *msb, int zone)
> +{
> +	u16 pos;
> +	int pba = zone * MS_BLOCKS_IN_ZONE;
> +	int i;
> +
> +	get_random_bytes(&pos, sizeof(pos));

This isn't good.  get_random_bytes() is the high-quality random number
generator.  If memstick calls this frequently it will degrade the
entropy pool, thus impacting the quality (ie: security) of key
generation, TCP sequence numbers, etc.  It's also slow.

Do you really require cryptographic quality randomness here, or would
prandom_bytes() suffice?

> +	if (!msb->free_block_count[zone]) {
> +		pr_err("NO free blocks in the zone %d, to use for a write, (media is WORN out) switching to RO mode", zone);
> +		msb->read_only = true;
> +		return MS_BLOCK_INVALID;
> +	}
> +
> +	pos %= msb->free_block_count[zone];
> +
> +	dbg_verbose("have %d choices for a free block, selected randomally: %d",
> +		msb->free_block_count[zone], pos);
> +
> +	pba = find_next_zero_bit(msb->used_blocks_bitmap,
> +							msb->block_count, pba);
> +	for (i = 0; i < pos; ++i)
> +		pba = find_next_zero_bit(msb->used_blocks_bitmap,
> +						msb->block_count, pba + 1);
> +
> +	dbg_verbose("result of the free blocks scan: pba %d", pba);
> +
> +	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
> +		pr_err("BUG: cant get a free block");
> +		msb->read_only = true;
> +		return MS_BLOCK_INVALID;
> +	}
> +
> +	msb_mark_block_used(msb, pba);
> +	return pba;
> +}
> 
> ...
>
> +static int msb_cache_write(struct msb_data *msb, int lba,
> +	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
> +{
> +	int error;
> +	struct scatterlist sg_tmp[10];

That's a lot of stack.

> +
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
> +						lba != msb->cache_block_lba)
> +		if (add_to_cache_only)
> +			return 0;
> +
> +	/* If we need to write different block */
> +	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
> +						lba != msb->cache_block_lba) {
> +		dbg_verbose("first flush the cache");
> +		error = msb_cache_flush(msb);
> +		if (error)
> +			return error;
> +	}
> +
> +	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
> +		msb->cache_block_lba  = lba;
> +		mod_timer(&msb->cache_flush_timer,
> +			jiffies + msecs_to_jiffies(cache_flush_timeout));
> +	}
> +
> +	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
> +
> +	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
> +	msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
> +
> +	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
> +		msb->cache + page * msb->page_size, msb->page_size);
> +
> +	set_bit(page, &msb->valid_cache_bitmap);
> +	return 0;
> +}
> 
> ...
>
> +static int msb_bd_open(struct block_device *bdev, fmode_t mode)
> +{
> +	struct gendisk *disk = bdev->bd_disk;
> +	struct msb_data *msb = disk->private_data;
> +
> +	dbg_verbose("block device open");
> +
> +	mutex_lock(&msb_disk_lock);
> +
> +	if (msb && msb->card)
> +		msb->usage_count++;

hm, what are we refcounting here?  Unfortunately the rather important
msb_data is undocumented :(  What is an msb_data?

> +	mutex_unlock(&msb_disk_lock);
> +	return 0;
> +}
> +
> 
> ...
>

(no subject)

[Maxim Levitsky]

Hi,

This is a revised generic driver for memstick standard cards,
that I posted a year ago.

I removed the scatter-gather list abuse from it, but otherwice
its unchanged.
I tested it quite a lot here and everything is fine.

Could you merge it?

Best regards,
        Maxim Levitsky

[PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

Based partially on MS standard spec quotes from Alex Dubov.

As any code that works with user data this driver isn't
recommended to use to write cards that contain valuable data.

It tries its best though to avoid data corruption
and possible damage to the card.

Tested on MS DUO 64 MB card on Ricoh R592 card reader..

Signed-off-by: Maxim Levitsky <maximlevitsky@gmail.com>
---
 MAINTAINERS                      |    5 +
 drivers/memstick/core/Kconfig    |   12 +
 drivers/memstick/core/Makefile   |    2 +-
 drivers/memstick/core/ms_block.c | 2424 ++++++++++++++++++++++++++++++++++++++
 drivers/memstick/core/ms_block.h |  246 ++++
 5 files changed, 2688 insertions(+), 1 deletion(-)
 create mode 100644 drivers/memstick/core/ms_block.c
 create mode 100644 drivers/memstick/core/ms_block.h

diff --git a/MAINTAINERS b/MAINTAINERS
index fdc0119..8fb6bf0 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -6393,6 +6393,11 @@ W:	http://tifmxx.berlios.de/
 S:	Maintained
 F:	drivers/memstick/host/tifm_ms.c
 
+SONY MEMORYSTICK STANDARD SUPPORT
+M:	Maxim Levitsky <maximlevitsky@gmail.com>
+S:	Maintained
+F:	drivers/memstick/core/ms_block.*
+
 SOUND
 M:	Jaroslav Kysela <perex@perex.cz>
 M:	Takashi Iwai <tiwai@suse.de>
diff --git a/drivers/memstick/core/Kconfig b/drivers/memstick/core/Kconfig
index 95f1814..f79f2a8 100644
--- a/drivers/memstick/core/Kconfig
+++ b/drivers/memstick/core/Kconfig
@@ -24,3 +24,15 @@ config MSPRO_BLOCK
 	  support. This provides a block device driver, which you can use
 	  to mount the filesystem. Almost everyone wishing MemoryStick
 	  support should say Y or M here.
+
+config MS_BLOCK
+	tristate "MemoryStick Standard device driver"
+	depends on BLOCK && EXPERIMENTAL
+	help
+	  Say Y here to enable the MemoryStick Standard device driver
+	  support. This provides a block device driver, which you can use
+	  to mount the filesystem.
+	  This driver works with old (bulky) MemoryStick and MemoryStick Duo
+	  but not PRO. Say Y if you have such card.
+	  Driver is new and not yet well tested, thus it can damage your card
+	  (even permanently)
diff --git a/drivers/memstick/core/Makefile b/drivers/memstick/core/Makefile
index ecd0299..0d7f90c 100644
--- a/drivers/memstick/core/Makefile
+++ b/drivers/memstick/core/Makefile
@@ -3,5 +3,5 @@
 #
 
 obj-$(CONFIG_MEMSTICK)		+= memstick.o
-
+obj-$(CONFIG_MS_BLOCK)		+= ms_block.o
 obj-$(CONFIG_MSPRO_BLOCK)	+= mspro_block.o
diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
new file mode 100644
index 0000000..c003867
--- /dev/null
+++ b/drivers/memstick/core/ms_block.c
@@ -0,0 +1,2424 @@
+/*
+ *  ms_block.c - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2012 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver were copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ */
+
+#include <linux/blkdev.h>
+#include <linux/mm.h>
+#include <linux/idr.h>
+#include <linux/hdreg.h>
+#include <linux/kthread.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/memstick.h>
+#include <linux/bitmap.h>
+#include <linux/scatterlist.h>
+#include <linux/sched.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include "ms_block.h"
+
+static int major;
+static int debug;
+static int cache_flush_timeout = 1000;
+static bool verify_writes;
+
+
+static int sg_nents(struct scatterlist *sg)
+{
+	int nents = 0;
+	while (sg) {
+		nents++;
+		sg = sg_next(sg);
+	}
+
+	return nents;
+}
+
+/*
+ * Copies section of 'sg_from' starting from offset 'offset' and with length
+ * 'len' To another scatterlist of to_nents enties
+ */
+static int sg_copy(struct scatterlist *sg_from, struct scatterlist *sg_to,
+					int to_nents, int offset, size_t len)
+{
+	int copied = 0;
+
+	while (offset > 0) {
+
+		if (offset >= sg_from->length) {
+			if (sg_is_last(sg_from))
+				return 0;
+
+			offset -= sg_from->length;
+			sg_from = sg_next(sg_from);
+			continue;
+		}
+
+		copied = min(len, (size_t)(sg_from->length - offset));
+		sg_set_page(sg_to, sg_page(sg_from),
+			copied, sg_from->offset + offset);
+
+		len -= copied;
+		offset = 0;
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_to = sg_next(sg_to);
+		to_nents--;
+		sg_from = sg_next(sg_from);
+	}
+
+	while (len > sg_from->length && to_nents--) {
+
+		len -= sg_from->length;
+		copied += sg_from->length;
+
+		sg_set_page(sg_to, sg_page(sg_from),
+				sg_from->length, sg_from->offset);
+
+		if (sg_is_last(sg_from) || !len)
+			goto out;
+
+		sg_from = sg_next(sg_from);
+		sg_to = sg_next(sg_to);
+	}
+
+	if (len && to_nents) {
+		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
+		copied += len;
+	}
+
+out:
+	sg_mark_end(sg_to);
+	return copied;
+}
+
+/*
+ * Compares section of 'sg' starting from offset 'offset' and with length 'len'
+ * to linear buffer of length 'len' at address 'buffer'
+ */
+static bool sg_compare_to_buffer(struct scatterlist *sg,
+					int offset, u8 *buffer, size_t len)
+{
+	unsigned long flags;
+	int retval = 0;
+	struct sg_mapping_iter miter;
+
+	local_irq_save(flags);
+	sg_miter_start(&miter, sg, sg_nents(sg),
+					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
+
+	while (sg_miter_next(&miter) && len > 0) {
+
+		int cmplen;
+
+		if (offset >= miter.length) {
+			offset -= miter.length;
+			continue;
+		}
+
+		cmplen = min(miter.length - offset, len);
+		retval = memcmp(miter.addr + offset, buffer, cmplen);
+		if (retval)
+			break;
+
+		buffer += cmplen;
+		len -= cmplen;
+		offset = 0;
+	}
+
+	if (!retval && len)
+		retval = -1;
+
+	sg_miter_stop(&miter);
+	local_irq_restore(flags);
+	return retval;
+}
+
+
+/* Get zone at which block with logical address 'lba' lives
+ * Flash is broken into zones.
+ * Each zone consists of 512 eraseblocks, out of which in first
+ * zone 494 are used and 496 are for all following zones.
+ * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
+*/
+static int msb_get_zone_from_lba(int lba)
+{
+	if (lba < 494)
+		return 0;
+	return ((lba - 494) / 496) + 1;
+}
+
+/* Get zone of physical block. Trivial */
+static int msb_get_zone_from_pba(int pba)
+{
+	return pba / MS_BLOCKS_IN_ZONE;
+}
+
+/* Debug test to validate free block counts */
+#ifdef DEBUG
+static int msb_validate_used_block_bitmap(struct msb_data *msb)
+{
+	int total_free_blocks = 0;
+	int i;
+
+	for (i = 0 ; i < msb->zone_count ; i++)
+		total_free_blocks += msb->free_block_count[i];
+
+	if (msb->block_count - bitmap_weight(msb->used_blocks_bitmap,
+					msb->block_count) == total_free_blocks)
+		return 0;
+
+	ms_printk("BUG: free block counts don't match the bitmap");
+	msb->read_only = true;
+	return -EINVAL;
+}
+#endif
+
+/* Mark physical block as used */
+static void msb_mark_block_used(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (test_bit(pba, msb->used_blocks_bitmap)) {
+		ms_printk("BUG: attempt to mark "
+			"already used pba %d as used", pba);
+		msb->read_only = true;
+		return;
+	}
+
+#ifdef DEBUG
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+#endif
+	set_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]--;
+}
+
+/* Mark physical block as free */
+static void msb_mark_block_unused(struct msb_data *msb, int pba)
+{
+	int zone = msb_get_zone_from_pba(pba);
+
+	if (!test_bit(pba, msb->used_blocks_bitmap)) {
+		ms_printk("BUG: attempt to mark "
+				"already unused pba %d as unused" , pba);
+		msb->read_only = true;
+		return;
+	}
+
+#ifdef DEBUG
+	if (msb_validate_used_block_bitmap(msb))
+		return;
+#endif
+	clear_bit(pba, msb->used_blocks_bitmap);
+	msb->free_block_count[zone]++;
+}
+
+/* Invalidate current register window*/
+static void msb_invalidate_reg_window(struct msb_data *msb)
+{
+	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.w_length = sizeof(struct ms_id_register);
+	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
+	msb->reg_addr.r_length = sizeof(struct ms_id_register);
+	msb->addr_valid = false;
+}
+
+/* Sane way to start a state machine*/
+static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
+		(struct memstick_dev *card, struct memstick_request **req))
+{
+	struct memstick_dev *card = msb->card;
+
+	WARN_ON(msb->state != -1);
+	msb->int_polling = false;
+	msb->state = 0;
+	msb->exit_error = 0;
+
+	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
+
+	card->next_request = state_func;
+	memstick_new_req(card->host);
+	wait_for_completion(&card->mrq_complete);
+
+	WARN_ON(msb->state != -1);
+	return msb->exit_error;
+}
+
+/* State machines call that to exit */
+int msb_exit_state_machine(struct msb_data *msb, int error)
+{
+	WARN_ON(msb->state == -1);
+
+	msb->state = -1;
+	msb->exit_error = error;
+	msb->card->next_request = h_msb_default_bad;
+
+	/* Invalidate reg window on errors */
+	if (error)
+		msb_invalidate_reg_window(msb);
+
+	complete(&msb->card->mrq_complete);
+	return -ENXIO;
+}
+
+/* read INT register */
+int msb_read_int_reg(struct msb_data *msb, long timeout)
+{
+	struct memstick_request *mrq = &msb->card->current_mrq;
+	WARN_ON(msb->state == -1);
+
+	if (!msb->int_polling) {
+		msb->int_timeout = jiffies +
+			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
+		msb->int_polling = true;
+	} else if (time_after(jiffies, msb->int_timeout)) {
+		mrq->data[0] = MEMSTICK_INT_CMDNAK;
+		return 0;
+	}
+
+	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
+				mrq->need_card_int && !mrq->error) {
+		mrq->data[0] = mrq->int_reg;
+		mrq->need_card_int = false;
+		return 0;
+	} else {
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		return 1;
+	}
+}
+
+/* Read a register */
+int msb_read_regs(struct msb_data *msb, int offset, int len)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.r_offset != offset ||
+	    msb->reg_addr.r_length != len || !msb->addr_valid) {
+
+		msb->reg_addr.r_offset = offset;
+		msb->reg_addr.r_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
+	return 1;
+}
+
+/* Write a card register */
+int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
+{
+	struct memstick_request *req = &msb->card->current_mrq;
+
+	if (msb->reg_addr.w_offset != offset ||
+		msb->reg_addr.w_length != len  || !msb->addr_valid) {
+
+		msb->reg_addr.w_offset = offset;
+		msb->reg_addr.w_length = len;
+		msb->addr_valid = true;
+
+		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
+			&msb->reg_addr, sizeof(msb->reg_addr));
+		return 0;
+	}
+
+	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
+	return 1;
+}
+
+/* Handler for absense of IO */
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq)
+{
+	return -ENXIO;
+}
+
+/*
+ * This function is a handler for reads of one page from device.
+ * Writes output to msb->current_sg, takes sector address from msb->reg.param
+ * Can also be used to read extra data only. Set params accordintly.
+ */
+static int h_msb_read_page(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 command, intreg;
+
+	if (mrq->error) {
+		dbg("read_page, unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+	case 0: /* Write the sector address */
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			(unsigned char *)&msb->regs.param))
+			return 0;
+		break;
+
+	case 1: /* Execute the read command*/
+		command = MS_CMD_BLOCK_READ;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		break;
+
+	case 2: /* send INT request */
+		if (msb_read_int_reg(msb, -1))
+			break;
+		msb->state++;
+
+	case 3: /* get result of the INT request*/
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state--;
+			goto again;
+		}
+
+		msb->int_polling = false;
+
+		if (intreg & MEMSTICK_INT_ERR)
+			msb->state++;
+		else
+			msb->state = 6;
+
+		goto again;
+
+	case 4: /* read the status register
+				to understand source of the INT_ERR */
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, status),
+			sizeof(struct ms_status_register)))
+			return 0;
+		break;
+
+	case 5: /* get results of status check */
+		msb->regs.status = *(struct ms_status_register *)mrq->data;
+		msb->state++;
+
+	case 6: /* Send extra data read request */
+		if (!msb_read_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register)))
+			return 0;
+		break;
+
+	case 7: /* Save result of extra data request */
+		msb->regs.extra_data =
+			*(struct ms_extra_data_register *) mrq->data;
+		msb->state++;
+
+	case 8: /* Send the  MS_TPC_READ_LONG_DATA to read IO buffer */
+
+		/* Skip that state if we only read the oob */
+		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
+			msb->state++;
+			goto again;
+		}
+
+		sg_init_table(sg, ARRAY_SIZE(sg));
+		sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size);
+
+		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
+		break;
+
+	case 9: /* check validity of data buffer & done */
+
+		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, 0);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
+			dbg("read_page: uncorrectable error");
+			return msb_exit_state_machine(msb, -EBADMSG);
+		}
+
+		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
+			dbg("read_page: correctable error");
+			msb->current_sg_offset += msb->page_size;
+			return msb_exit_state_machine(msb, -EUCLEAN);
+		} else {
+			dbg("read_page: INT error, but no status error bits");
+			return msb_exit_state_machine(msb, -EIO);
+		}
+	default:
+		BUG();
+	}
+	msb->state++;
+	return 0;
+}
+
+/*
+ * Handler of writes of exactly one block.
+ * Takes address from msb->regs.param.
+ * Writes same extra data to blocks, also taken
+ * from msb->regs.extra
+ * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
+ * device refuses to take the command or something else
+ */
+static int h_msb_write_block(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+	struct scatterlist sg[2];
+	u8 intreg, command;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+again:
+	switch (msb->state) {
+
+	/* HACK: Jmicon handling of TPCs between 8 and
+	 *	sizeof(memstick_request.data) is broken due to hardware
+	 *	bug in PIO mode that is used for these TPCs
+	 *	Therefore split the write
+	 */
+
+	case 0: /* write param register*/
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+		break;
+
+	case 1: /* write extra data */
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+		break;
+
+
+	case 2: /* execute the write command*/
+		command = MS_CMD_BLOCK_WRITE;
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		break;
+
+	case 3: /* send INT request */
+		if (msb_read_int_reg(msb, -1))
+			break;
+		msb->state++;
+
+	case 4: /* read INT response */
+		intreg = mrq->data[0];
+		msb->regs.status.interrupt = intreg;
+
+		/* errors mean out of here, and fast... */
+		if (intreg & (MEMSTICK_INT_CMDNAK))
+			return msb_exit_state_machine(msb, -EIO);
+
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+
+		/* for last page we need to poll CED */
+		if (msb->current_page == msb->pages_in_block) {
+			if (intreg & MEMSTICK_INT_CED)
+				return msb_exit_state_machine(msb, 0);
+			msb->state--;
+			goto again;
+
+		}
+
+		/* for non-last page we need BREQ before writing next chunk */
+		if (!(intreg & MEMSTICK_INT_BREQ)) {
+			msb->state--;
+			goto again;
+		}
+
+		msb->int_polling = false;
+		msb->state++;
+
+	case 5: /* send the MS_TPC_WRITE_LONG_DATA to perform the write*/
+		sg_init_table(sg, ARRAY_SIZE(sg));
+
+		if (sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
+			msb->current_sg_offset,
+			msb->page_size) < msb->page_size)
+			return msb_exit_state_machine(msb, -EIO);
+
+		memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
+		mrq->need_card_int = 1;
+		break;
+
+	case 6: /* Switch to next page + go back to int polling */
+		msb->current_page++;
+		msb->current_sg_offset += msb->page_size;
+		msb->state = 3;
+		goto again;
+	default:
+		BUG();
+	}
+	msb->state++;
+	return 0;
+}
+
+/*
+ * This function is used to send simple IO requests to device that consist
+ * of register write + command
+ */
+static int h_msb_send_command(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+
+	u8 intreg;
+
+	if (mrq->error) {
+		dbg("send_command: unknown error");
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+again:
+	switch (msb->state) {
+
+		/* HACK: see h_msb_write_block */
+
+	case 0: /* write param register*/
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			sizeof(struct ms_param_register),
+			&msb->regs.param))
+			return 0;
+		break;
+
+	case 1: /* write extra data */
+		if (!msb->command_need_oob) {
+			msb->state++;
+			goto again;
+		}
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, extra_data),
+			sizeof(struct ms_extra_data_register),
+			&msb->regs.extra_data))
+			return 0;
+		break;
+
+	case 2: /* execute the command*/
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
+		break;
+
+	case 3: /* send INT request */
+		if (msb_read_int_reg(msb, -1))
+			break;
+		msb->state++;
+
+	case 4: /* poll for int bits */
+		intreg = mrq->data[0];
+
+		if (intreg & MEMSTICK_INT_CMDNAK)
+			return msb_exit_state_machine(msb, -EIO);
+		if (intreg & MEMSTICK_INT_ERR)
+			return msb_exit_state_machine(msb, -EBADMSG);
+
+
+		if (!(intreg & MEMSTICK_INT_CED)) {
+			msb->state--;
+			goto again;
+		}
+
+		return msb_exit_state_machine(msb, 0);
+	}
+	msb->state++;
+	return 0;
+}
+
+/* Small handler for card reset */
+static int h_msb_reset(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	u8 command = MS_CMD_RESET;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+
+	if (mrq->error)
+		return msb_exit_state_machine(msb, mrq->error);
+
+	switch (msb->state) {
+	case 0:
+		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
+		mrq->need_card_int = 0;
+		break;
+	case 1:
+		return msb_exit_state_machine(msb, 0);
+	}
+	msb->state++;
+	return 0;
+}
+
+/* This handler is used to do serial->parallel switch */
+static int h_msb_parallel_switch(struct memstick_dev *card,
+					struct memstick_request **out_mrq)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
+
+	struct memstick_host *host = card->host;
+
+	if (mrq->error) {
+		dbg("parallel_switch: error");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		return msb_exit_state_machine(msb, mrq->error);
+	}
+
+	switch (msb->state) {
+	case 0: /* Set the parallel interface on memstick side */
+		msb->regs.param.system |= MEMSTICK_SYS_PAM;
+
+		if (!msb_write_regs(msb,
+			offsetof(struct ms_register, param),
+			1,
+			(unsigned char *)&msb->regs.param))
+			return 0;
+		break;
+
+	case 1: /* Set parallel interface on our side + send a dummy request
+			to see if card responds */
+		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
+		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
+		break;
+
+	case 2:
+		return msb_exit_state_machine(msb, 0);
+	}
+	msb->state++;
+	return 0;
+}
+
+static int msb_switch_to_parallel(struct msb_data *msb);
+
+/* Reset the card, to guard against hw errors beeing treated as bad blocks */
+static int msb_reset(struct msb_data *msb, bool full)
+{
+
+	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
+	struct memstick_dev *card = msb->card;
+	struct memstick_host *host = card->host;
+	int error;
+
+	/* Reset the card */
+	msb->regs.param.system = MEMSTICK_SYS_BAMD;
+
+	if (full) {
+		error =  host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+		if (error)
+			goto out_error;
+
+		msb_invalidate_reg_window(msb);
+
+		error = host->set_param(host,
+					MEMSTICK_POWER, MEMSTICK_POWER_ON);
+		if (error)
+			goto out_error;
+
+		error = host->set_param(host,
+					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+		if (error) {
+out_error:
+			dbg("Failed to reset the host controller");
+			msb->read_only = true;
+			return -EFAULT;
+		}
+	}
+
+	error = msb_run_state_machine(msb, h_msb_reset);
+	if (error) {
+		dbg("Failed to reset the card");
+		msb->read_only = true;
+		return -ENODEV;
+	}
+
+	/* Set parallel mode */
+	if (was_parallel)
+		msb_switch_to_parallel(msb);
+	return 0;
+}
+
+/* Attempts to switch interface to parallel mode */
+static int msb_switch_to_parallel(struct msb_data *msb)
+{
+	int error;
+
+	error = msb_run_state_machine(msb, h_msb_parallel_switch);
+	if (error) {
+		ms_printk("Switch to parallel failed");
+		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
+		msb_reset(msb, true);
+		return -EFAULT;
+	}
+
+	msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
+	return 0;
+}
+
+/* Changes overwrite flag on a page */
+static int msb_set_overwrite_flag(struct msb_data *msb,
+						u16 pba, u8 page, u8 flag)
+{
+	if (msb->read_only)
+		return -EROFS;
+
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_OVERWRITE;
+	msb->regs.extra_data.overwrite_flag = flag;
+	msb->command_value = MS_CMD_BLOCK_WRITE;
+	msb->command_need_oob = true;
+
+	dbg_verbose("changing overwrite flag to %02x for sector %d, page %d",
+							flag, pba, page);
+	return msb_run_state_machine(msb, h_msb_send_command);
+}
+
+static int msb_mark_bad(struct msb_data *msb, int pba)
+{
+	ms_printk("marking pba %d as bad", pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(
+			msb, pba, 0, 0xFF & ~MEMSTICK_OVERWRITE_BKST);
+}
+
+static int msb_mark_page_bad(struct msb_data *msb, int pba, int page)
+{
+	dbg("marking page %d of pba %d as bad", page, pba);
+	msb_reset(msb, true);
+	return msb_set_overwrite_flag(msb,
+		pba, page, ~MEMSTICK_OVERWRITE_PGST0);
+}
+
+/* Erases one physical block */
+static int msb_erase_block(struct msb_data *msb, u16 pba)
+{
+	int error, try;
+	if (msb->read_only)
+		return -EROFS;
+
+	dbg_verbose("erasing pba %d", pba);
+
+	for (try = 1 ; try < 3 ; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = 0;
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->command_value = MS_CMD_BLOCK_ERASE;
+		msb->command_need_oob = false;
+
+
+		error = msb_run_state_machine(msb, h_msb_send_command);
+		if (!error || msb_reset(msb, true))
+			break;
+	}
+
+	if (error) {
+		ms_printk("erase failed, marking pba %d as bad", pba);
+		msb_mark_bad(msb, pba);
+	}
+
+	dbg_verbose("erase success, marking pba %d as unused", pba);
+	msb_mark_block_unused(msb, pba);
+	set_bit(pba, msb->erased_blocks_bitmap);
+	return error;
+}
+
+/* Reads one page from device */
+static int msb_read_page(struct msb_data *msb,
+	u16 pba, u8 page, struct ms_extra_data_register *extra,
+					struct scatterlist *sg,  int offset)
+{
+	int try, error;
+
+	if (pba == MS_BLOCK_INVALID) {
+		unsigned long flags;
+		struct sg_mapping_iter miter;
+		size_t len = msb->page_size;
+
+		dbg_verbose("read unmapped sector. returning 0xFF");
+
+		local_irq_save(flags);
+		sg_miter_start(&miter, sg, sg_nents(sg),
+				SG_MITER_ATOMIC | SG_MITER_TO_SG);
+
+		while (sg_miter_next(&miter) && len > 0) {
+
+			int chunklen;
+
+			if (offset && offset >= miter.length) {
+				offset -= miter.length;
+				continue;
+			}
+
+			chunklen = min(miter.length - offset, len);
+			memset(miter.addr + offset, 0xFF, chunklen);
+			len -= chunklen;
+			offset = 0;
+		}
+
+		sg_miter_stop(&miter);
+		local_irq_restore(flags);
+
+		if (offset)
+			return -EFAULT;
+
+		if (extra)
+			memset(extra, 0xFF, sizeof(*extra));
+		return 0;
+	}
+
+	if (pba >= msb->block_count) {
+		ms_printk("BUG: attempt to read beyond"
+					" the end of the card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	for (try = 1 ; try < 3 ; try++) {
+		msb->regs.param.block_address = cpu_to_be16(pba);
+		msb->regs.param.page_address = page;
+		msb->regs.param.cp = MEMSTICK_CP_PAGE;
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		error = msb_run_state_machine(msb, h_msb_read_page);
+
+
+		if (error == -EUCLEAN) {
+			ms_printk("correctable error on pba %d, page %d",
+				pba, page);
+			error = 0;
+		}
+
+		if (!error && extra)
+			*extra = msb->regs.extra_data;
+
+		if (!error || msb_reset(msb, true))
+			break;
+
+	}
+
+	/* Mark bad pages */
+	if (error == -EBADMSG) {
+		ms_printk("uncorrectable error on read of pba %d, page %d",
+			pba, page);
+
+		if (msb->regs.extra_data.overwrite_flag &
+					MEMSTICK_OVERWRITE_PGST0)
+			msb_mark_page_bad(msb, pba, page);
+		return -EBADMSG;
+	}
+
+	if (error)
+		ms_printk("read of pba %d, page %d failed with error %d",
+			pba, page, error);
+	return error;
+}
+
+/* Reads oob of page only */
+static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
+	struct ms_extra_data_register *extra)
+{
+	int error;
+	BUG_ON(!extra);
+
+	msb->regs.param.block_address = cpu_to_be16(pba);
+	msb->regs.param.page_address = page;
+	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
+
+	if (pba > msb->block_count) {
+		ms_printk("BUG: attempt to read beyond"
+					" the end of card at pba %d", pba);
+		return -EINVAL;
+	}
+
+	error = msb_run_state_machine(msb, h_msb_read_page);
+	*extra = msb->regs.extra_data;
+
+	if (error == -EUCLEAN) {
+		ms_printk("correctable error on pba %d, page %d",
+			pba, page);
+		return 0;
+	}
+
+	return error;
+}
+
+
+/* Reads a block and compares it with data contained in scatterlist orig_sg */
+static bool msb_verify_block(struct msb_data *msb, u16 pba,
+				struct scatterlist *orig_sg,  int offset)
+{
+	struct scatterlist sg;
+	int page = 0, error;
+
+	sg_init_one(&sg, msb->block_buffer, msb->block_size);
+
+	while (page < msb->pages_in_block) {
+
+		error = msb_read_page(msb, pba, page,
+				NULL, &sg, page * msb->page_size);
+		if (error)
+			return -EIO;
+		page++;
+	}
+
+	if (sg_compare_to_buffer(orig_sg, offset,
+				msb->block_buffer, msb->block_size))
+		return -EIO;
+	return 0;
+}
+
+/* Writes exectly one block + oob */
+static int msb_write_block(struct msb_data *msb,
+			u16 pba, u32 lba, struct scatterlist *sg, int offset)
+{
+	int error, current_try = 1;
+	BUG_ON(sg->length < msb->page_size);
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (pba == MS_BLOCK_INVALID) {
+		ms_printk(
+			"BUG: write: attempt to write MS_BLOCK_INVALID block");
+		return -EINVAL;
+	}
+
+	if (pba >= msb->block_count || lba >= msb->logical_block_count) {
+		ms_printk(
+		"BUG: write: attempt to write beyond the end of device");
+		return -EINVAL;
+	}
+
+	if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+		ms_printk("BUG: write: lba zone mismatch");
+		return -EINVAL;
+	}
+
+	if (pba == msb->boot_block_locations[0] ||
+		pba == msb->boot_block_locations[1]) {
+		ms_printk("BUG: write: attempt to write to boot blocks!");
+		return -EINVAL;
+	}
+
+	while (1) {
+
+		if (msb->read_only)
+			return -EROFS;
+
+		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
+		msb->regs.param.page_address = 0;
+		msb->regs.param.block_address = cpu_to_be16(pba);
+
+		msb->regs.extra_data.management_flag = 0xFF;
+		msb->regs.extra_data.overwrite_flag = 0xF8;
+		msb->regs.extra_data.logical_address = cpu_to_be16(lba);
+
+		msb->current_sg = sg;
+		msb->current_sg_offset = offset;
+		msb->current_page = 0;
+
+		error = msb_run_state_machine(msb, h_msb_write_block);
+
+		/* Sector we just wrote to is assumed erased since its pba
+			was erased. If it wasn't erased, write will succeed
+			and will just clear the bits that were set in the block
+			thus test that what we have written,
+			matches what we expect.
+			We do trust the blocks that we erased */
+		if (!error && (verify_writes ||
+				!test_bit(pba, msb->erased_blocks_bitmap)))
+			error = msb_verify_block(msb, pba, sg, offset);
+
+		if (!error)
+			break;
+
+		if (current_try > 1 || msb_reset(msb, true))
+			break;
+
+		ms_printk("write failed, trying to erase the pba %d", pba);
+		error = msb_erase_block(msb, pba);
+		if (error)
+			break;
+
+		current_try++;
+	}
+	return error;
+}
+
+/* Finds a free block for write replacement */
+static u16 msb_get_free_block(struct msb_data *msb, int zone)
+{
+	u16 pos;
+	int pba = zone * MS_BLOCKS_IN_ZONE;
+	int i;
+
+	get_random_bytes(&pos, sizeof(pos));
+
+	if (!msb->free_block_count[zone]) {
+		ms_printk("NO free blocks in the zone %d, to use for a write, "
+			"(media is WORN out) switching to RO mode", zone);
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	pos %= msb->free_block_count[zone];
+
+	dbg_verbose("have %d choices for a free block, selected randomally: %d",
+		msb->free_block_count[zone], pos);
+
+	pba = find_next_zero_bit(msb->used_blocks_bitmap,
+							msb->block_count, pba);
+	for (i = 0 ; i < pos ; ++i)
+		pba = find_next_zero_bit(msb->used_blocks_bitmap,
+						msb->block_count, pba + 1);
+
+	dbg_verbose("result of the free blocks scan: pba %d", pba);
+
+	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
+		ms_printk("BUG: cant get a free block");
+		msb->read_only = true;
+		return MS_BLOCK_INVALID;
+	}
+
+	msb_mark_block_used(msb, pba);
+	return pba;
+}
+
+static int msb_update_block(struct msb_data *msb, u16 lba,
+	struct scatterlist *sg, int offset)
+{
+	u16 pba, new_pba;
+	int error, try;
+
+	pba = msb->lba_to_pba_table[lba];
+	dbg_verbose("start of a block update at lba  %d, pba %d", lba, pba);
+
+	if (pba != MS_BLOCK_INVALID) {
+		dbg_verbose("setting the update flag on the block");
+		msb_set_overwrite_flag(msb, pba, 0,
+				0xFF & ~MEMSTICK_OVERWRITE_UDST);
+	}
+
+	for (try = 0 ; try < 3 ; try++) {
+		new_pba = msb_get_free_block(msb,
+			msb_get_zone_from_lba(lba));
+
+		if (new_pba == MS_BLOCK_INVALID) {
+			error = -EIO;
+			goto out;
+		}
+
+		dbg_verbose("block update: writing updated block to the pba %d",
+								new_pba);
+		error = msb_write_block(msb, new_pba, lba, sg, offset);
+		if (error == -EBADMSG) {
+			msb_mark_bad(msb, new_pba);
+			continue;
+		}
+
+		if (error)
+			goto out;
+
+		dbg_verbose("block update: erasing the old block");
+		msb_erase_block(msb, pba);
+		msb->lba_to_pba_table[lba] = new_pba;
+		return 0;
+	}
+out:
+	if (error) {
+		ms_printk("block update error after %d tries, "
+						"switching to r/o mode", try);
+		msb->read_only = true;
+	}
+	return error;
+}
+
+/* Converts endiannes in the boot block for easy use */
+static void msb_fix_boot_page_endianness(struct ms_boot_page *p)
+{
+	p->header.block_id = be16_to_cpu(p->header.block_id);
+	p->header.format_reserved = be16_to_cpu(p->header.format_reserved);
+	p->entry.disabled_block.start_addr
+		= be32_to_cpu(p->entry.disabled_block.start_addr);
+	p->entry.disabled_block.data_size
+		= be32_to_cpu(p->entry.disabled_block.data_size);
+	p->entry.cis_idi.start_addr
+		= be32_to_cpu(p->entry.cis_idi.start_addr);
+	p->entry.cis_idi.data_size
+		= be32_to_cpu(p->entry.cis_idi.data_size);
+	p->attr.block_size = be16_to_cpu(p->attr.block_size);
+	p->attr.number_of_blocks = be16_to_cpu(p->attr.number_of_blocks);
+	p->attr.number_of_effective_blocks
+		= be16_to_cpu(p->attr.number_of_effective_blocks);
+	p->attr.page_size = be16_to_cpu(p->attr.page_size);
+	p->attr.memory_manufacturer_code
+		= be16_to_cpu(p->attr.memory_manufacturer_code);
+	p->attr.memory_device_code = be16_to_cpu(p->attr.memory_device_code);
+	p->attr.implemented_capacity
+		= be16_to_cpu(p->attr.implemented_capacity);
+	p->attr.controller_number = be16_to_cpu(p->attr.controller_number);
+	p->attr.controller_function = be16_to_cpu(p->attr.controller_function);
+}
+
+static int msb_read_boot_blocks(struct msb_data *msb)
+{
+	int pba = 0;
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	struct ms_boot_page *page;
+
+	msb->boot_block_locations[0] = MS_BLOCK_INVALID;
+	msb->boot_block_locations[1] = MS_BLOCK_INVALID;
+	msb->boot_block_count = 0;
+
+	dbg_verbose("Start of a scan for the boot blocks");
+
+	if (!msb->boot_page) {
+		page = kmalloc(sizeof(struct ms_boot_page)*2, GFP_KERNEL);
+		if (!page)
+			return -ENOMEM;
+
+		msb->boot_page = page;
+	} else
+		page = msb->boot_page;
+
+	msb->block_count = MS_BLOCK_MAX_BOOT_ADDR;
+
+	for (pba = 0 ; pba < MS_BLOCK_MAX_BOOT_ADDR ; pba++) {
+
+		sg_init_one(&sg, page, sizeof(*page));
+		if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) {
+			dbg("boot scan: can't read pba %d", pba);
+			continue;
+		}
+
+		if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) {
+			dbg("managment flag doesn't indicate boot block %d",
+									pba);
+			continue;
+		}
+
+		if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) {
+			dbg("the pba at %d doesn' contain boot block ID", pba);
+			continue;
+		}
+
+		msb_fix_boot_page_endianness(page);
+		msb->boot_block_locations[msb->boot_block_count] = pba;
+
+		page++;
+		msb->boot_block_count++;
+
+		if (msb->boot_block_count == 2)
+			break;
+	}
+
+	if (!msb->boot_block_count) {
+		ms_printk("media doesn't contain master page, aborting");
+		return -EIO;
+	}
+
+	dbg_verbose("End of scan for boot blocks");
+	return 0;
+}
+
+static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
+{
+	struct ms_boot_page *boot_block;
+	struct scatterlist sg;
+	u16 *buffer = NULL;
+	int offset = 0;
+
+	int i, error = 0;
+	int data_size, data_offset, page, page_offset, size_to_read;
+	u16 pba;
+
+	BUG_ON(block_nr > 1);
+
+	boot_block = &msb->boot_page[block_nr];
+	pba = msb->boot_block_locations[block_nr];
+
+	if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID)
+		return -EINVAL;
+
+	data_size = boot_block->entry.disabled_block.data_size;
+	data_offset = sizeof(struct ms_boot_page) +
+			boot_block->entry.disabled_block.start_addr;
+	if (!data_size)
+		return 0;
+
+	page = data_offset / msb->page_size;
+	page_offset = data_offset % msb->page_size;
+	size_to_read =
+		DIV_ROUND_UP(data_size + page_offset, msb->page_size) *
+			msb->page_size;
+
+	dbg("reading bad block of boot block at pba %d, offset %d len %d",
+		pba, data_offset, data_size);
+
+	buffer = kzalloc(size_to_read, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	/* Read the buffer */
+	sg_init_one(&sg, buffer, size_to_read);
+
+	while (offset < size_to_read) {
+		error = msb_read_page(msb, pba, page, NULL, &sg, offset);
+		if (error)
+			goto out;
+
+		page++;
+		offset += msb->page_size;
+
+		if (page == msb->pages_in_block) {
+			ms_printk(
+			"bad block table extends beyond the boot block");
+			break;
+		}
+	}
+
+	/* Process the bad block table */
+	for (i = page_offset ; i < data_size / sizeof(u16) ; i++) {
+
+		u16 bad_block = be16_to_cpu(buffer[i]);
+
+		if (bad_block >= msb->block_count) {
+			dbg("bad block table contains invalid block %d",
+								bad_block);
+			continue;
+		}
+
+		if (test_bit(bad_block, msb->used_blocks_bitmap))  {
+			dbg("duplicate bad block %d in the table",
+				bad_block);
+			continue;
+		}
+
+		dbg("block %d is marked as factory bad", bad_block);
+		msb_mark_block_used(msb, bad_block);
+	}
+out:
+	kfree(buffer);
+	return error;
+}
+
+static int msb_ftl_initialize(struct msb_data *msb)
+{
+	int i;
+
+	if (msb->ftl_initialized)
+		return 0;
+
+	msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE;
+	msb->logical_block_count = msb->zone_count * 496 - 2;
+
+	msb->used_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->erased_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
+	msb->lba_to_pba_table =
+		kmalloc(msb->logical_block_count * sizeof(u16), GFP_KERNEL);
+
+	if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
+						!msb->erased_blocks_bitmap) {
+		kfree(msb->used_blocks_bitmap);
+		kfree(msb->lba_to_pba_table);
+		kfree(msb->erased_blocks_bitmap);
+		return -ENOMEM;
+	}
+
+	for (i = 0 ; i < msb->zone_count ; i++)
+		msb->free_block_count[i] = MS_BLOCKS_IN_ZONE;
+
+	memset(msb->lba_to_pba_table, MS_BLOCK_INVALID,
+			msb->logical_block_count * sizeof(u16));
+
+	dbg("initial FTL tables created. Zone count = %d, "
+					"Logical block count = %d",
+		msb->zone_count, msb->logical_block_count);
+
+	msb->ftl_initialized = true;
+	return 0;
+}
+
+static int msb_ftl_scan(struct msb_data *msb)
+{
+	u16 pba, lba, other_block;
+	u8 overwrite_flag, managment_flag, other_overwrite_flag;
+	int error;
+	struct ms_extra_data_register extra;
+	u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL);
+
+	if (!overwrite_flags)
+		return -ENOMEM;
+
+	dbg("Start of media scanning");
+	for (pba = 0 ; pba < msb->block_count ; pba++) {
+
+		if (pba == msb->boot_block_locations[0] ||
+			pba == msb->boot_block_locations[1]) {
+			dbg_verbose("pba %05d -> [boot block]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		if (test_bit(pba, msb->used_blocks_bitmap)) {
+			dbg_verbose("pba %05d -> [factory bad]", pba);
+			continue;
+		}
+
+		memset(&extra, 0, sizeof(extra));
+		error = msb_read_oob(msb, pba, 0, &extra);
+
+		/* can't trust the page if we can't read the oob */
+		if (error == -EBADMSG) {
+			ms_printk(
+			"oob of pba %d damaged, will try to erase it", pba);
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		} else if (error)
+			return error;
+
+		lba = be16_to_cpu(extra.logical_address);
+		managment_flag = extra.management_flag;
+		overwrite_flag = extra.overwrite_flag;
+		overwrite_flags[pba] = overwrite_flag;
+
+		/* Skip bad blocks */
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
+			dbg("pba %05d -> [BAD]", pba);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Skip system/drm blocks */
+		if ((managment_flag & MEMSTICK_MANAGMENT_FLAG_NORMAL) !=
+			MEMSTICK_MANAGMENT_FLAG_NORMAL) {
+			dbg("pba %05d -> [reserved managment flag %02x]",
+							pba, managment_flag);
+			msb_mark_block_used(msb, pba);
+			continue;
+		}
+
+		/* Erase temporary tables */
+		if (!(managment_flag & MEMSTICK_MANAGEMENT_ATFLG)) {
+			dbg("pba %05d -> [temp table] - will erase", pba);
+
+			msb_mark_block_used(msb, pba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		if (lba == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [free]", pba);
+			continue;
+		}
+
+		msb_mark_block_used(msb, pba);
+
+		/* Block has LBA not according to zoning*/
+		if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
+			ms_printk("pba %05d -> [bad lba %05d] - will erase",
+								pba, lba);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		/* No collisions - great */
+		if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) {
+			dbg_verbose("pba %05d -> [lba %05d]", pba, lba);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		other_block = msb->lba_to_pba_table[lba];
+		other_overwrite_flag = overwrite_flags[other_block];
+
+		ms_printk("Collision between pba %d and pba %d",
+			pba, other_block);
+
+		if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			ms_printk("pba %d is marked as stable, use it", pba);
+			msb_erase_block(msb, other_block);
+			msb->lba_to_pba_table[lba] = pba;
+			continue;
+		}
+
+		if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
+			ms_printk("pba %d is marked as stable, use it",
+								other_block);
+			msb_erase_block(msb, pba);
+			continue;
+		}
+
+		ms_printk("collision between blocks %d and %d,"
+		" without stable flag set on both, erasing pba %d",
+				pba, other_block, other_block);
+
+		msb_erase_block(msb, other_block);
+		msb->lba_to_pba_table[lba] = pba;
+	}
+
+	dbg("End of media scanning");
+	kfree(overwrite_flags);
+	return 0;
+}
+
+static void msb_cache_flush_timer(unsigned long data)
+{
+	struct msb_data *msb = (struct msb_data *)data;
+	msb->need_flush_cache = true;
+	wake_up_process(msb->io_thread);
+}
+
+
+static void msb_cache_discard(struct msb_data *msb)
+{
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return;
+
+	del_timer_sync(&msb->cache_flush_timer);
+
+	dbg_verbose("Discarding the write cache");
+	msb->cache_block_lba = MS_BLOCK_INVALID;
+	bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block);
+}
+
+static int msb_cache_init(struct msb_data *msb)
+{
+	setup_timer(&msb->cache_flush_timer, msb_cache_flush_timer,
+		(unsigned long)msb);
+
+	if (!msb->cache)
+		msb->cache = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->cache)
+		return -ENOMEM;
+
+	msb_cache_discard(msb);
+	return 0;
+}
+
+static int msb_cache_flush(struct msb_data *msb)
+{
+	struct scatterlist sg;
+	struct ms_extra_data_register extra;
+	int page, offset, error;
+	u16 pba, lba;
+
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID)
+		return 0;
+
+	lba = msb->cache_block_lba;
+	pba = msb->lba_to_pba_table[lba];
+
+	dbg_verbose("Flushing the write cache of pba %d (LBA %d)",
+						pba, msb->cache_block_lba);
+
+	sg_init_one(&sg, msb->cache , msb->block_size);
+
+	/* Read all missing pages in cache */
+	for (page = 0 ; page < msb->pages_in_block ; page++) {
+
+		if (test_bit(page, &msb->valid_cache_bitmap))
+			continue;
+
+		offset = page * msb->page_size;
+
+		dbg_verbose("reading non-present sector %d of cache block %d",
+			page, lba);
+		error = msb_read_page(msb, pba, page, &extra, &sg, offset);
+
+		/* Bad pages are copied with 00 page status */
+		if (error == -EBADMSG) {
+			ms_printk("read error on sector %d, contents probably"
+				" damaged", page);
+			continue;
+		}
+
+		if (error)
+			return error;
+
+		if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) !=
+							MEMSTICK_OV_PG_NORMAL) {
+			dbg("page %d is marked as bad", page);
+			continue;
+		}
+
+		set_bit(page, &msb->valid_cache_bitmap);
+	}
+
+	/* Write the cache now */
+	error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
+	pba = msb->lba_to_pba_table[msb->cache_block_lba];
+
+	/* Mark invalid pages */
+	if (!error) {
+		for (page = 0 ; page < msb->pages_in_block ; page++) {
+
+			if (test_bit(page, &msb->valid_cache_bitmap))
+				continue;
+
+			dbg("marking page %d as containing damaged data",
+				page);
+			msb_set_overwrite_flag(msb,
+				pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL);
+		}
+	}
+
+	msb_cache_discard(msb);
+	return error;
+}
+
+static int msb_cache_write(struct msb_data *msb, int lba,
+	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
+{
+	int error;
+	struct scatterlist sg_tmp[10];
+	if (msb->read_only)
+		return -EROFS;
+
+	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
+						lba != msb->cache_block_lba)
+		if (add_to_cache_only)
+			return 0;
+
+	/* If we need to write different block */
+	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
+						lba != msb->cache_block_lba) {
+		dbg_verbose("first flush the cache");
+		error = msb_cache_flush(msb);
+		if (error)
+			return error;
+	}
+
+	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
+		msb->cache_block_lba  = lba;
+		mod_timer(&msb->cache_flush_timer,
+			jiffies + msecs_to_jiffies(cache_flush_timeout));
+	}
+
+	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
+
+	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+	sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+
+	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
+		msb->cache + page * msb->page_size, msb->page_size);
+
+	set_bit(page, &msb->valid_cache_bitmap);
+	return 0;
+}
+
+static int msb_cache_read(struct msb_data *msb, int lba,
+				int page, struct scatterlist *sg, int offset)
+{
+	int pba = msb->lba_to_pba_table[lba];
+	struct scatterlist sg_tmp[10];
+	int error = 0;
+
+	if (lba == msb->cache_block_lba &&
+			test_bit(page, &msb->valid_cache_bitmap)) {
+
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from cache",
+							lba, pba, page);
+
+		sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
+		sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
+		sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp),
+			msb->cache + msb->page_size * page,
+							msb->page_size);
+	} else {
+		dbg_verbose("Read of LBA %d (pba %d) sector %d from device",
+							lba, pba, page);
+
+		error = msb_read_page(msb, pba, page, NULL, sg, offset);
+		if (error)
+			return error;
+
+		msb_cache_write(msb, lba, page, true, sg, offset);
+	}
+	return error;
+}
+
+/* Emulated geometry table
+ * This table content isn't that importaint,
+ * One could put here different values, providing that they still
+ * cover whole disk.
+ * 64 MB entry is what windows reports for my 64M memstick */
+
+static const struct chs_entry chs_table[] = {
+/*        size sectors cylynders  heads */
+	{ 4,    16,    247,       2  },
+	{ 8,    16,    495,       2  },
+	{ 16,   16,    495,       4  },
+	{ 32,   16,    991,       4  },
+	{ 64,   16,    991,       8  },
+	{128,   16,    991,       16 },
+	{ 0 }
+};
+
+/* Load information about the card */
+static int msb_init_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	struct ms_boot_page *boot_block;
+	int error = 0, i, raw_size_in_megs;
+
+	msb->caps = 0;
+
+	if (card->id.class >= MEMSTICK_CLASS_ROM &&
+				card->id.class <= MEMSTICK_CLASS_ROM)
+		msb->read_only = true;
+
+	msb->state = -1;
+	error = msb_reset(msb, false);
+	if (error)
+		return error;
+
+	/* Due to a bug in Jmicron driver written by Alex Dubov,
+	 its serial mode barely works,
+	 so we switch to parallel mode right away */
+	if (host->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+
+	msb->page_size = sizeof(struct ms_boot_page);
+
+	/* Read the boot page */
+	error = msb_read_boot_blocks(msb);
+	if (error)
+		return -EIO;
+
+	boot_block = &msb->boot_page[0];
+
+	/* Save intersting attributes from boot page */
+	msb->block_count = boot_block->attr.number_of_blocks;
+	msb->page_size = boot_block->attr.page_size;
+
+	msb->pages_in_block = boot_block->attr.block_size * 2;
+	msb->block_size = msb->page_size * msb->pages_in_block;
+
+	if (msb->page_size > PAGE_SIZE) {
+		/* this isn't supported by linux at all, anyway*/
+		dbg("device page %d size isn't supported", msb->page_size);
+		return -EINVAL;
+	}
+
+	msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL);
+	if (!msb->block_buffer)
+		return -ENOMEM;
+
+	raw_size_in_megs = (msb->block_size * msb->block_count) >> 20;
+
+	for (i = 0 ; chs_table[i].size ; i++) {
+
+		if (chs_table[i].size != raw_size_in_megs)
+			continue;
+
+		msb->geometry.cylinders = chs_table[i].cyl;
+		msb->geometry.heads = chs_table[i].head;
+		msb->geometry.sectors = chs_table[i].sec;
+		break;
+	}
+
+	if (boot_block->attr.transfer_supporting == 1)
+		msb->caps |= MEMSTICK_CAP_PAR4;
+
+	if (boot_block->attr.device_type & 0x03)
+		msb->read_only = true;
+
+	dbg("Total block count = %d", msb->block_count);
+	dbg("Each block consists of %d pages", msb->pages_in_block);
+	dbg("Page size = %d bytes", msb->page_size);
+	dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4));
+	dbg("Read only: %d", msb->read_only);
+
+#if 0
+	/* Now we can switch the interface */
+	if (host->caps & msb->caps & MEMSTICK_CAP_PAR4)
+		msb_switch_to_parallel(msb);
+#endif
+
+	error = msb_cache_init(msb);
+	if (error)
+		return error;
+
+	error = msb_ftl_initialize(msb);
+	if (error)
+		return error;
+
+
+	/* Read the bad block table */
+	error = msb_read_bad_block_table(msb, 0);
+
+	if (error && error != -ENOMEM) {
+		dbg("failed to read bad block table from primary boot block,"
+							" trying from backup");
+		error = msb_read_bad_block_table(msb, 1);
+	}
+
+	if (error)
+		return error;
+
+	/* *drum roll* Scan the media */
+	error = msb_ftl_scan(msb);
+	if (error) {
+		ms_printk("Scan of media failed");
+		return error;
+	}
+
+	return 0;
+
+}
+
+static int msb_do_write_request(struct msb_data *msb, int lba,
+	int page, struct scatterlist *sg, int len, int *sucessfuly_written)
+{
+	int error = 0;
+	int offset = 0;
+	*sucessfuly_written = 0;
+
+	while (offset < len) {
+		if (page == 0 && len - offset >= msb->block_size) {
+
+			if (msb->cache_block_lba == lba)
+				msb_cache_discard(msb);
+
+			dbg_verbose("Writing whole lba %d", lba);
+			error = msb_update_block(msb, lba, sg, offset);
+			if (error)
+				return error;
+
+			offset += msb->block_size;
+			*sucessfuly_written += msb->block_size;
+			lba++;
+			continue;
+		}
+
+		error = msb_cache_write(msb, lba, page, false, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_written += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static int msb_do_read_request(struct msb_data *msb, int lba,
+		int page, struct scatterlist *sg, int len, int *sucessfuly_read)
+{
+	int error = 0;
+	int offset = 0;
+	*sucessfuly_read = 0;
+
+	while (offset < len) {
+
+		error = msb_cache_read(msb, lba, page, sg, offset);
+		if (error)
+			return error;
+
+		offset += msb->page_size;
+		*sucessfuly_read += msb->page_size;
+
+		page++;
+		if (page == msb->pages_in_block) {
+			page = 0;
+			lba++;
+		}
+	}
+	return 0;
+}
+
+static int msb_io_thread(void *data)
+{
+	struct msb_data *msb = data;
+	int page, error, len;
+	sector_t lba;
+	unsigned long flags;
+	struct scatterlist *sg;
+
+
+	sg = kmalloc((MS_BLOCK_MAX_SEGS+1) *
+				sizeof(struct scatterlist), GFP_KERNEL);
+	if (!sg)
+		return -ENOMEM;
+	sg_init_table(sg, MS_BLOCK_MAX_SEGS+1);
+
+	dbg("IO: thread started");
+
+	while (1) {
+
+		if (kthread_should_stop()) {
+			if (msb->req)
+				blk_requeue_request(msb->queue, msb->req);
+			break;
+		}
+
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (msb->need_flush_cache) {
+			msb->need_flush_cache = false;
+			spin_unlock_irqrestore(&msb->q_lock, flags);
+			msb_cache_flush(msb);
+			continue;
+		}
+
+		if (!msb->req) {
+			msb->req = blk_fetch_request(msb->queue);
+
+			if (!msb->req) {
+				dbg_verbose("IO: no more requests, sleeping");
+				set_current_state(TASK_INTERRUPTIBLE);
+				spin_unlock_irqrestore(&msb->q_lock, flags);
+				schedule();
+				dbg_verbose("IO: thread woken up");
+				continue;
+			}
+		}
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+
+		/* If card was removed meanwhile */
+		if (!msb->req)
+			continue;
+
+		/* process the request */
+		dbg_verbose("IO: thread processing new request");
+		blk_rq_map_sg(msb->queue, msb->req, sg);
+
+		lba = blk_rq_pos(msb->req);
+
+		sector_div(lba, msb->page_size / 512);
+		page = do_div(lba, msb->pages_in_block);
+
+		if (rq_data_dir(msb->req) == READ)
+			error = msb_do_read_request(
+				msb, lba, page, sg,
+					blk_rq_bytes(msb->req), &len);
+		else
+			error = msb_do_write_request(
+					msb, lba, page, sg,
+					blk_rq_bytes(msb->req), &len);
+
+		spin_lock_irqsave(&msb->q_lock, flags);
+
+		if (len)
+			if (!__blk_end_request(msb->req, 0, len))
+				msb->req = NULL;
+
+		if (error && msb->req) {
+			dbg_verbose("IO: ending one sector "
+					"of the request with error");
+			if (!__blk_end_request(msb->req, error, msb->page_size))
+				msb->req = NULL;
+		}
+
+		if (msb->req)
+			dbg_verbose("IO: request still pending");
+
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+	}
+
+	kfree(sg);
+	return 0;
+}
+
+static DEFINE_IDR(msb_disk_idr);
+static DEFINE_MUTEX(msb_disk_lock);
+
+static int msb_bd_open(struct block_device *bdev, fmode_t mode)
+{
+	struct gendisk *disk = bdev->bd_disk;
+	struct msb_data *msb = disk->private_data;
+
+	dbg_verbose("block device open");
+
+	mutex_lock(&msb_disk_lock);
+
+	if (msb && msb->card)
+		msb->usage_count++;
+
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static void msb_data_clear(struct msb_data *msb)
+{
+	kfree(msb->boot_page);
+	kfree(msb->used_blocks_bitmap);
+	kfree(msb->lba_to_pba_table);
+	kfree(msb->cache);
+	msb->card = NULL;
+}
+
+static int msb_disk_release(struct gendisk *disk)
+{
+	struct msb_data *msb = disk->private_data;
+	int disk_id = MINOR(disk_devt(disk)) >> MS_BLOCK_PART_SHIFT;
+
+	dbg_verbose("block device release");
+
+	mutex_lock(&msb_disk_lock);
+
+	if (msb) {
+		if (msb->usage_count)
+			msb->usage_count--;
+
+		if (!msb->usage_count) {
+			kfree(msb);
+			disk->private_data = NULL;
+			idr_remove(&msb_disk_idr, disk_id);
+			put_disk(disk);
+		}
+	}
+	mutex_unlock(&msb_disk_lock);
+	return 0;
+}
+
+static int msb_bd_release(struct gendisk *disk, fmode_t mode)
+{
+	return msb_disk_release(disk);
+}
+
+static int msb_bd_getgeo(struct block_device *bdev,
+				 struct hd_geometry *geo)
+{
+	struct msb_data *msb = bdev->bd_disk->private_data;
+	*geo = msb->geometry;
+	return 0;
+}
+
+static int msb_prepare_req(struct request_queue *q, struct request *req)
+{
+	if (req->cmd_type != REQ_TYPE_FS &&
+				req->cmd_type != REQ_TYPE_BLOCK_PC) {
+		blk_dump_rq_flags(req, "MS unsupported request");
+		return BLKPREP_KILL;
+	}
+	req->cmd_flags |= REQ_DONTPREP;
+	return BLKPREP_OK;
+}
+
+static void msb_submit_req(struct request_queue *q)
+{
+	struct memstick_dev *card = q->queuedata;
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct request *req = NULL;
+
+	dbg_verbose("Submit request");
+
+	if (msb->card_dead) {
+		dbg("Refusing requests on removed card");
+
+		WARN_ON(msb->io_thread);
+
+		while ((req = blk_fetch_request(q)) != NULL)
+			__blk_end_request_all(req, -ENODEV);
+		return;
+	}
+
+	if (msb->req)
+		return;
+
+	if (msb->io_thread)
+		wake_up_process(msb->io_thread);
+}
+
+static int msb_check_card(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	return (msb->card_dead == 0);
+}
+
+static void msb_stop(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+	struct task_struct *io_thread;
+
+	dbg("Stopping all msblock IO");
+
+	/* Just stop the IO thread.
+	   Be carefull not to race against submit_request
+	   If it is called, all pending requests will be processed by
+	   the IO thread as soon as msb_start is called */
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_stop_queue(msb->queue);
+	io_thread = msb->io_thread;
+	msb->io_thread = NULL;
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	del_timer_sync(&msb->cache_flush_timer);
+
+	if (io_thread)
+		kthread_stop(io_thread);
+}
+
+static void msb_start(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+	int disk_id = MINOR(disk_devt(msb->disk)) >> MS_BLOCK_PART_SHIFT;
+
+	dbg("Resuming IO from msblock");
+
+	msb_invalidate_reg_window(msb);
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	if (msb->io_thread || msb->card_dead) {
+		spin_unlock_irqrestore(&msb->q_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Kick cache flush anyway, its harmless */
+	msb->need_flush_cache = true;
+
+	msb->io_thread = kthread_run(msb_io_thread, msb, "kms_block%d",
+		disk_id);
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+}
+
+static const struct block_device_operations msb_bdops = {
+	.open    = msb_bd_open,
+	.release = msb_bd_release,
+	.getgeo  = msb_bd_getgeo,
+	.owner   = THIS_MODULE
+};
+
+/* Registers the block device */
+static int msb_init_disk(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct memstick_host *host = card->host;
+	int rc, disk_id;
+	u64 limit = BLK_BOUNCE_HIGH;
+	unsigned long capacity;
+
+	if (host->dev.dma_mask && *(host->dev.dma_mask))
+		limit = *(host->dev.dma_mask);
+
+	mutex_lock(&msb_disk_lock);
+
+	if (!idr_pre_get(&msb_disk_idr, GFP_KERNEL)) {
+		mutex_unlock(&msb_disk_lock);
+		return -ENOMEM;
+	}
+
+	rc = idr_get_new(&msb_disk_idr, card, &disk_id);
+	mutex_unlock(&msb_disk_lock);
+
+	if (rc)
+		return rc;
+
+	if ((disk_id << MS_BLOCK_PART_SHIFT) > 255) {
+		rc = -ENOSPC;
+		goto out_release_id;
+	}
+
+	msb->disk = alloc_disk(1 << MS_BLOCK_PART_SHIFT);
+	if (!msb->disk) {
+		rc = -ENOMEM;
+		goto out_release_id;
+	}
+
+	msb->queue = blk_init_queue(msb_submit_req, &msb->q_lock);
+	if (!msb->queue) {
+		rc = -ENOMEM;
+		goto out_put_disk;
+	}
+
+	msb->queue->queuedata = card;
+	blk_queue_prep_rq(msb->queue, msb_prepare_req);
+
+	blk_queue_bounce_limit(msb->queue, limit);
+	blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
+	blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
+	blk_queue_max_segment_size(msb->queue,
+				   MS_BLOCK_MAX_PAGES * msb->page_size);
+	msb->disk->major = major;
+	msb->disk->first_minor = disk_id << MS_BLOCK_PART_SHIFT;
+	msb->disk->fops = &msb_bdops;
+	msb->usage_count = 1;
+	msb->disk->private_data = msb;
+	msb->disk->queue = msb->queue;
+	msb->disk->driverfs_dev = &card->dev;
+
+	sprintf(msb->disk->disk_name, "msblk%d", disk_id);
+
+	blk_queue_logical_block_size(msb->queue, msb->page_size);
+
+	capacity = msb->pages_in_block * msb->logical_block_count;
+	capacity *= (msb->page_size / 512);
+
+	set_capacity(msb->disk, capacity);
+	dbg("Set total disk size to %lu sectors", capacity);
+
+	if (msb->read_only)
+		set_disk_ro(msb->disk, 1);
+
+	msb_start(card);
+	add_disk(msb->disk);
+	dbg("Disk added");
+	return 0;
+
+out_put_disk:
+	put_disk(msb->disk);
+out_release_id:
+	mutex_lock(&msb_disk_lock);
+	idr_remove(&msb_disk_idr, disk_id);
+	mutex_unlock(&msb_disk_lock);
+	return rc;
+}
+
+static int msb_probe(struct memstick_dev *card)
+{
+	struct msb_data *msb;
+	int rc = 0;
+
+	msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!msb)
+		return -ENOMEM;
+	memstick_set_drvdata(card, msb);
+	msb->card = card;
+	spin_lock_init(&msb->q_lock);
+
+	rc = msb_init_card(card);
+	if (rc)
+		goto out_free;
+
+	rc = msb_init_disk(card);
+	if (!rc) {
+		card->check = msb_check_card;
+		card->stop = msb_stop;
+		card->start = msb_start;
+		return 0;
+	}
+out_free:
+	memstick_set_drvdata(card, NULL);
+	msb_data_clear(msb);
+	kfree(msb);
+	return rc;
+}
+
+static void msb_remove(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	if (msb->io_thread)
+		msb_stop(card);
+
+	dbg("Removing the disk device");
+
+	/* Take care of unhandled + new requests from now on */
+	spin_lock_irqsave(&msb->q_lock, flags);
+	msb->card_dead = true;
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+
+	/* Remove the disk */
+	del_gendisk(msb->disk);
+	blk_cleanup_queue(msb->queue);
+	msb->queue = NULL;
+
+	mutex_lock(&msb_disk_lock);
+	msb_data_clear(msb);
+	mutex_unlock(&msb_disk_lock);
+
+	msb_disk_release(msb->disk);
+	memstick_set_drvdata(card, NULL);
+}
+
+#ifdef CONFIG_PM
+
+static int msb_suspend(struct memstick_dev *card, pm_message_t state)
+{
+	msb_stop(card);
+	return 0;
+}
+
+static int msb_resume(struct memstick_dev *card)
+{
+	struct msb_data *msb = memstick_get_drvdata(card);
+	struct msb_data *new_msb = NULL;
+	bool card_dead = true;
+
+#ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
+	msb->card_dead = true;
+	return 0;
+#endif
+	mutex_lock(&card->host->lock);
+
+	new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
+	if (!new_msb)
+		goto out;
+
+	new_msb->card = card;
+	memstick_set_drvdata(card, new_msb);
+	spin_lock_init(&new_msb->q_lock);
+
+	if (msb_init_card(card))
+		goto out;
+
+	if (msb->block_size != new_msb->block_size)
+		goto out;
+
+	if (memcmp(msb->boot_page, new_msb->boot_page,
+					sizeof(struct ms_boot_page)))
+		goto out;
+
+	if (msb->logical_block_count != new_msb->logical_block_count ||
+		memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
+						msb->logical_block_count))
+		goto out;
+
+	if (msb->block_count != new_msb->block_count ||
+		memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
+							msb->block_count / 8))
+		goto out;
+
+	card_dead = false;
+out:
+	if (card_dead)
+		dbg("Card was removed/replaced during suspend");
+
+	msb->card_dead = card_dead;
+	memstick_set_drvdata(card, msb);
+
+	if (new_msb) {
+		msb_data_clear(new_msb);
+		kfree(new_msb);
+	}
+
+	msb_start(card);
+	mutex_unlock(&card->host->lock);
+	return 0;
+}
+#else
+
+#define msb_suspend NULL
+#define msb_resume NULL
+
+#endif /* CONFIG_PM */
+
+static struct memstick_device_id msb_id_tbl[] = {
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_FLASH},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_ROM},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_RO},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
+	 MEMSTICK_CLASS_WP},
+
+	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
+	 MEMSTICK_CLASS_DUO},
+	{}
+};
+MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
+
+
+static struct memstick_driver msb_driver = {
+	.driver = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE
+	},
+	.id_table = msb_id_tbl,
+	.probe    = msb_probe,
+	.remove   = msb_remove,
+	.suspend  = msb_suspend,
+	.resume   = msb_resume
+};
+
+static int __init msb_init(void)
+{
+	int rc = -ENOMEM;
+
+	rc = register_blkdev(major, DRIVER_NAME);
+	if (rc < 0) {
+		printk(KERN_ERR DRIVER_NAME ": failed to register "
+		       "major %d, error %d\n", major, rc);
+		return rc;
+	}
+	if (!major)
+		major = rc;
+
+	rc = memstick_register_driver(&msb_driver);
+	if (rc)
+		unregister_blkdev(major, DRIVER_NAME);
+	return rc;
+}
+
+static void __exit msb_exit(void)
+{
+	memstick_unregister_driver(&msb_driver);
+	unregister_blkdev(major, DRIVER_NAME);
+	idr_destroy(&msb_disk_idr);
+}
+
+module_init(msb_init);
+module_exit(msb_exit);
+
+module_param(major, int, S_IRUGO);
+MODULE_PARM_DESC(major, "Major to use for block device (default auto)");
+
+module_param(cache_flush_timeout, int, S_IRUGO);
+MODULE_PARM_DESC(cache_flush_timeout,
+				"Cache flush timeout in msec (1000 default)");
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level (0-3)");
+
+module_param(verify_writes, bool, S_IRUGO);
+MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Maxim Levitsky");
+MODULE_DESCRIPTION("Sony MemoryStick block device driver");
diff --git a/drivers/memstick/core/ms_block.h b/drivers/memstick/core/ms_block.h
new file mode 100644
index 0000000..b1a485f
--- /dev/null
+++ b/drivers/memstick/core/ms_block.h
@@ -0,0 +1,246 @@
+/*
+ *  ms_block.c - Sony MemoryStick (legacy) storage support
+
+ *  Copyright (C) 2010 Maxim Levitsky <maximlevitsky@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Minor portions of the driver are copied from mspro_block.c which is
+ * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
+ *
+ * Also ms structures were copied from old broken driver by same author
+ * These probably come from MS spec
+ *
+ */
+
+#ifndef MS_BLOCK_NEW_H
+#define MS_BLOCK_NEW_H
+
+#define MS_BLOCK_MAX_SEGS      32
+#define MS_BLOCK_MAX_PAGES     ((2 << 16) - 1)
+
+#define MS_BLOCK_MAX_BOOT_ADDR 0x000c
+#define MS_BLOCK_BOOT_ID       0x0001
+#define MS_BLOCK_INVALID       0xffff
+#define MS_MAX_ZONES           16
+#define MS_BLOCKS_IN_ZONE      512
+
+#define MS_BLOCK_MAP_LINE_SZ   16
+#define MS_BLOCK_PART_SHIFT    3
+
+
+#define MEMSTICK_UNCORR_ERROR (MEMSTICK_STATUS1_UCFG | \
+		MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCDT)
+
+#define MEMSTICK_CORR_ERROR (MEMSTICK_STATUS1_FGER | MEMSTICK_STATUS1_EXER | \
+	MEMSTICK_STATUS1_DTER)
+
+#define MEMSTICK_INT_ERROR (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)
+
+#define MEMSTICK_OVERWRITE_FLAG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | \
+	MEMSTICK_OVERWRITE_PGST0  | \
+	MEMSTICK_OVERWRITE_BKST)
+
+#define MEMSTICK_OV_PG_NORMAL \
+	(MEMSTICK_OVERWRITE_PGST1 | MEMSTICK_OVERWRITE_PGST0)
+
+#define MEMSTICK_MANAGMENT_FLAG_NORMAL \
+	(MEMSTICK_MANAGEMENT_SYSFLG |  \
+	MEMSTICK_MANAGEMENT_SCMS1   |  \
+	MEMSTICK_MANAGEMENT_SCMS0)     \
+
+struct ms_boot_header {
+	unsigned short block_id;
+	unsigned short format_reserved;
+	unsigned char  reserved0[184];
+	unsigned char  data_entry;
+	unsigned char  reserved1[179];
+} __packed;
+
+
+struct ms_system_item {
+	unsigned int  start_addr;
+	unsigned int  data_size;
+	unsigned char data_type_id;
+	unsigned char reserved[3];
+} __packed;
+
+struct ms_system_entry {
+	struct ms_system_item disabled_block;
+	struct ms_system_item cis_idi;
+	unsigned char         reserved[24];
+} __packed;
+
+struct ms_boot_attr_info {
+	unsigned char      memorystick_class;
+	unsigned char      format_unique_value1;
+	unsigned short     block_size;
+	unsigned short     number_of_blocks;
+	unsigned short     number_of_effective_blocks;
+	unsigned short     page_size;
+	unsigned char      extra_data_size;
+	unsigned char      format_unique_value2;
+	unsigned char      assembly_time[8];
+	unsigned char      format_unique_value3;
+	unsigned char      serial_number[3];
+	unsigned char      assembly_manufacturer_code;
+	unsigned char      assembly_model_code[3];
+	unsigned short     memory_manufacturer_code;
+	unsigned short     memory_device_code;
+	unsigned short     implemented_capacity;
+	unsigned char      format_unique_value4[2];
+	unsigned char      vcc;
+	unsigned char      vpp;
+	unsigned short     controller_number;
+	unsigned short     controller_function;
+	unsigned char      reserved0[9];
+	unsigned char      transfer_supporting;
+	unsigned short     format_unique_value5;
+	unsigned char      format_type;
+	unsigned char      memorystick_application;
+	unsigned char      device_type;
+	unsigned char      reserved1[22];
+	unsigned char      format_uniqure_value6[2];
+	unsigned char      reserved2[15];
+} __packed;
+
+struct ms_cis_idi {
+	unsigned short general_config;
+	unsigned short logical_cylinders;
+	unsigned short reserved0;
+	unsigned short logical_heads;
+	unsigned short track_size;
+	unsigned short page_size;
+	unsigned short pages_per_track;
+	unsigned short msw;
+	unsigned short lsw;
+	unsigned short reserved1;
+	unsigned char  serial_number[20];
+	unsigned short buffer_type;
+	unsigned short buffer_size_increments;
+	unsigned short long_command_ecc;
+	unsigned char  firmware_version[28];
+	unsigned char  model_name[18];
+	unsigned short reserved2[5];
+	unsigned short pio_mode_number;
+	unsigned short dma_mode_number;
+	unsigned short field_validity;
+	unsigned short current_logical_cylinders;
+	unsigned short current_logical_heads;
+	unsigned short current_pages_per_track;
+	unsigned int   current_page_capacity;
+	unsigned short mutiple_page_setting;
+	unsigned int   addressable_pages;
+	unsigned short single_word_dma;
+	unsigned short multi_word_dma;
+	unsigned char  reserved3[128];
+} __packed;
+
+
+struct ms_boot_page {
+	struct ms_boot_header    header;
+	struct ms_system_entry   entry;
+	struct ms_boot_attr_info attr;
+} __packed;
+
+struct msb_data {
+	unsigned int			usage_count;
+	struct memstick_dev		*card;
+	struct gendisk			*disk;
+	struct request_queue		*queue;
+	spinlock_t			q_lock;
+	struct hd_geometry		geometry;
+	struct attribute_group		attr_group;
+	struct request			*req;
+	int				caps;
+
+	/* IO */
+	struct task_struct		*io_thread;
+	bool				card_dead;
+
+	/* Media properties */
+	struct ms_boot_page		*boot_page;
+	u16				boot_block_locations[2];
+	int				boot_block_count;
+
+	bool				read_only;
+	unsigned short			page_size;
+	int				block_size;
+	int				pages_in_block;
+	int				zone_count;
+	int				block_count;
+	int				logical_block_count;
+
+	/* FTL tables */
+	unsigned long			*used_blocks_bitmap;
+	unsigned long			*erased_blocks_bitmap;
+	u16				*lba_to_pba_table;
+	int				free_block_count[MS_MAX_ZONES];
+	bool				ftl_initialized;
+
+	/* Cache */
+	unsigned char			*cache;
+	unsigned long			valid_cache_bitmap;
+	int				cache_block_lba;
+	bool				need_flush_cache;
+	struct timer_list		cache_flush_timer;
+
+	/* Preallocated buffers */
+	unsigned char			*block_buffer;
+	struct scatterlist		sg[MS_BLOCK_MAX_SEGS+1];
+
+
+	/* handler's local data */
+	struct ms_register_addr		reg_addr;
+	bool				addr_valid;
+
+	u8				command_value;
+	bool				command_need_oob;
+	struct scatterlist		*current_sg;
+	int				current_sg_offset;
+
+	struct ms_register		regs;
+	int				current_page;
+
+	int				state;
+	int				exit_error;
+	bool				int_polling;
+	unsigned long			int_timeout;
+
+};
+
+
+struct chs_entry {
+	unsigned long size;
+	unsigned char sec;
+	unsigned short cyl;
+	unsigned char head;
+};
+
+static int msb_reset(struct msb_data *msb, bool full);
+
+static int h_msb_default_bad(struct memstick_dev *card,
+						struct memstick_request **mrq);
+
+
+
+#define DRIVER_NAME "ms_block"
+
+#define ms_printk(format, ...) \
+	printk(KERN_INFO DRIVER_NAME ": " format "\n", ## __VA_ARGS__)
+
+#define __dbg(level, format, ...) \
+	do { \
+		if (debug >= level) \
+			printk(KERN_DEBUG DRIVER_NAME \
+				": " format "\n", ## __VA_ARGS__); \
+	} while (0)
+
+
+#define dbg(format, ...)		__dbg(1, format, ## __VA_ARGS__)
+#define dbg_verbose(format, ...)	__dbg(2, format, ## __VA_ARGS__)
+
+#endif
-- 
1.7.9.5

Re: [PATCH] memstick: add support for legacy memorysticks

[Andrew Morton]

On Wed, 19 Sep 2012 16:19:02 +0300
Maxim Levitsky <maximlevitsky@gmail.com> wrote:

> Based partially on MS standard spec quotes from Alex Dubov.
> 
> As any code that works with user data this driver isn't
> recommended to use to write cards that contain valuable data.
> 
> It tries its best though to avoid data corruption
> and possible damage to the card.
> 
> Tested on MS DUO 64 MB card on Ricoh R592 card reader..
> 

Generally looks nice to me, although I wouldn't know a memstick if one
stuck to my shoe.

I have a bunch of fairly trivial comments, and one head-scratcher for
Tejun: what's up with the local-irqs-disabled requirement in sg_miter_next()?

> ...
>
> +static int sg_nents(struct scatterlist *sg)
> +{
> +	int nents = 0;
> +	while (sg) {
> +		nents++;
> +		sg = sg_next(sg);
> +	}
> +
> +	return nents;
> +}

I think we may as well put this in scatterlist.c immediately.  It
wouldn't surprise me if we already have open-coded copies of this lying
around the tree.

> +/*
> + * Copies section of 'sg_from' starting from offset 'offset' and with length
> + * 'len' To another scatterlist of to_nents enties
> + */
> +static int sg_copy(struct scatterlist *sg_from, struct scatterlist *sg_to,
> +					int to_nents, int offset, size_t len)

The should return a size_t, to match the type of `len'.  And the type
of `offset' is fishy - there's nothing which intrinsically limits these
things to 2G?

> +{
> +	int copied = 0;
> +
> +	while (offset > 0) {
> +
> +		if (offset >= sg_from->length) {
> +			if (sg_is_last(sg_from))
> +				return 0;
> +
> +			offset -= sg_from->length;
> +			sg_from = sg_next(sg_from);
> +			continue;
> +		}
> +
> +		copied = min(len, (size_t)(sg_from->length - offset));

min_t would be more conventional.  Or perhaps even min(), depending on
`offset's new type (but probably not).

> +		sg_set_page(sg_to, sg_page(sg_from),
> +			copied, sg_from->offset + offset);
> +
> +		len -= copied;
> +		offset = 0;
> +
> +		if (sg_is_last(sg_from) || !len)
> +			goto out;
> +
> +		sg_to = sg_next(sg_to);
> +		to_nents--;
> +		sg_from = sg_next(sg_from);
> +	}
> +
> +	while (len > sg_from->length && to_nents--) {
> +
> +		len -= sg_from->length;
> +		copied += sg_from->length;
> +
> +		sg_set_page(sg_to, sg_page(sg_from),
> +				sg_from->length, sg_from->offset);
> +
> +		if (sg_is_last(sg_from) || !len)
> +			goto out;
> +
> +		sg_from = sg_next(sg_from);
> +		sg_to = sg_next(sg_to);
> +	}
> +
> +	if (len && to_nents) {
> +		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
> +		copied += len;
> +	}
> +
> +out:
> +	sg_mark_end(sg_to);
> +	return copied;
> +}
> +
> +/*
> + * Compares section of 'sg' starting from offset 'offset' and with length 'len'
> + * to linear buffer of length 'len' at address 'buffer'

The comment should document the return value.

> + */
> +static bool sg_compare_to_buffer(struct scatterlist *sg,
> +					int offset, u8 *buffer, size_t len)
> +{
> +	unsigned long flags;
> +	int retval = 0;
> +	struct sg_mapping_iter miter;
> +
> +	local_irq_save(flags);

hm, the "IRQ disabled if SG_MITER_ATOMIC" requirement of
sg_miter_next() is weird and unexpected.

Tejun's 137d3edb48425f8 added the code whch has this strange
requirement, but it is unexplained.  Wazzup?

> +	sg_miter_start(&miter, sg, sg_nents(sg),
> +					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
> +
> +	while (sg_miter_next(&miter) && len > 0) {
> +
> +		int cmplen;
> +
> +		if (offset >= miter.length) {
> +			offset -= miter.length;
> +			continue;
> +		}
> +
> +		cmplen = min(miter.length - offset, len);
> +		retval = memcmp(miter.addr + offset, buffer, cmplen);
> +		if (retval)
> +			break;
> +
> +		buffer += cmplen;
> +		len -= cmplen;
> +		offset = 0;
> +	}
> +
> +	if (!retval && len)
> +		retval = -1;
> +
> +	sg_miter_stop(&miter);
> +	local_irq_restore(flags);
> +	return retval;
> +}
>
> ...
>
> +static void msb_mark_block_unused(struct msb_data *msb, int pba)
> +{
> +	int zone = msb_get_zone_from_pba(pba);
> +
> +	if (!test_bit(pba, msb->used_blocks_bitmap)) {
> +		ms_printk("BUG: attempt to mark "
> +				"already unused pba %d as unused" , pba);
> +		msb->read_only = true;
> +		return;
> +	}
> +
> +#ifdef DEBUG
> +	if (msb_validate_used_block_bitmap(msb))
> +		return;
> +#endif
> +	clear_bit(pba, msb->used_blocks_bitmap);
> +	msb->free_block_count[zone]++;
> +}

Presumably there is some lock which the caller must hold to prevent the
obvious races in the above two functions.  I suggest adding a comment
whcih documents this requirement, then take advantage of that lock to
switch to the more efficient __set_bit() and __clear_bit().

> +/* Invalidate current register window*/

hm, checkpatch misses the missing space before */

> +static void msb_invalidate_reg_window(struct msb_data *msb)
> +{
> +	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
> +	msb->reg_addr.w_length = sizeof(struct ms_id_register);
> +	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
> +	msb->reg_addr.r_length = sizeof(struct ms_id_register);
> +	msb->addr_valid = false;
> +}
> +
> +/* Sane way to start a state machine*/
> +static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
> +		(struct memstick_dev *card, struct memstick_request **req))
> +{
> +	struct memstick_dev *card = msb->card;
> +
> +	WARN_ON(msb->state != -1);
> +	msb->int_polling = false;
> +	msb->state = 0;
> +	msb->exit_error = 0;
> +
> +	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
> +
> +	card->next_request = state_func;
> +	memstick_new_req(card->host);
> +	wait_for_completion(&card->mrq_complete);
> +
> +	WARN_ON(msb->state != -1);
> +	return msb->exit_error;
> +}
> +
> +/* State machines call that to exit */
> +int msb_exit_state_machine(struct msb_data *msb, int error)

static

> +{
> +	WARN_ON(msb->state == -1);
> +
> +	msb->state = -1;
> +	msb->exit_error = error;
> +	msb->card->next_request = h_msb_default_bad;
> +
> +	/* Invalidate reg window on errors */
> +	if (error)
> +		msb_invalidate_reg_window(msb);
> +
> +	complete(&msb->card->mrq_complete);
> +	return -ENXIO;
> +}
> +
> +/* read INT register */
> +int msb_read_int_reg(struct msb_data *msb, long timeout)

static

> +{
> +	struct memstick_request *mrq = &msb->card->current_mrq;
> +	WARN_ON(msb->state == -1);

It's conventional to put a newline between end-of-locals and start-of-code.

> +	if (!msb->int_polling) {
> +		msb->int_timeout = jiffies +
> +			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
> +		msb->int_polling = true;
> +	} else if (time_after(jiffies, msb->int_timeout)) {
> +		mrq->data[0] = MEMSTICK_INT_CMDNAK;
> +		return 0;
> +	}
> +
> +	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
> +				mrq->need_card_int && !mrq->error) {
> +		mrq->data[0] = mrq->int_reg;
> +		mrq->need_card_int = false;
> +		return 0;
> +	} else {
> +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> +		return 1;
> +	}
> +}
> +
> +/* Read a register */
> +int msb_read_regs(struct msb_data *msb, int offset, int len)

static.  I'll stop checking them now ;)

> +{
> +	struct memstick_request *req = &msb->card->current_mrq;
> +
> +	if (msb->reg_addr.r_offset != offset ||
> +	    msb->reg_addr.r_length != len || !msb->addr_valid) {
> +
> +		msb->reg_addr.r_offset = offset;
> +		msb->reg_addr.r_length = len;
> +		msb->addr_valid = true;
> +
> +		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
> +			&msb->reg_addr, sizeof(msb->reg_addr));
> +		return 0;
> +	}
> +
> +	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
> +	return 1;
> +}
> +
> +/* Write a card register */
> +int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
> +{
> +	struct memstick_request *req = &msb->card->current_mrq;
> +
> +	if (msb->reg_addr.w_offset != offset ||
> +		msb->reg_addr.w_length != len  || !msb->addr_valid) {
> +
> +		msb->reg_addr.w_offset = offset;
> +		msb->reg_addr.w_length = len;
> +		msb->addr_valid = true;
> +
> +		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
> +			&msb->reg_addr, sizeof(msb->reg_addr));
> +		return 0;
> +	}
> +
> +	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
> +	return 1;
> +}
> +
> +/* Handler for absense of IO */

"absence" :)

> +static int h_msb_default_bad(struct memstick_dev *card,
> +						struct memstick_request **mrq)
> +{
> +	return -ENXIO;
> +}
> +
> +/*
> + * This function is a handler for reads of one page from device.
> + * Writes output to msb->current_sg, takes sector address from msb->reg.param
> + * Can also be used to read extra data only. Set params accordintly.
> + */
> +static int h_msb_read_page(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +	struct scatterlist sg[2];
> +	u8 command, intreg;
> +
> +	if (mrq->error) {
> +		dbg("read_page, unknown error");
> +		return msb_exit_state_machine(msb, mrq->error);
> +	}
> +again:
> +	switch (msb->state) {
> +	case 0: /* Write the sector address */
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, param),
> +			sizeof(struct ms_param_register),
> +			(unsigned char *)&msb->regs.param))
> +			return 0;
> +		break;
> +
> +	case 1: /* Execute the read command*/
> +		command = MS_CMD_BLOCK_READ;
> +		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
> +		break;
> +
> +	case 2: /* send INT request */
> +		if (msb_read_int_reg(msb, -1))
> +			break;
> +		msb->state++;
> +
> +	case 3: /* get result of the INT request*/
> +		intreg = mrq->data[0];
> +		msb->regs.status.interrupt = intreg;
> +
> +		if (intreg & MEMSTICK_INT_CMDNAK)
> +			return msb_exit_state_machine(msb, -EIO);
> +
> +		if (!(intreg & MEMSTICK_INT_CED)) {
> +			msb->state--;
> +			goto again;
> +		}
> +
> +		msb->int_polling = false;
> +
> +		if (intreg & MEMSTICK_INT_ERR)
> +			msb->state++;
> +		else
> +			msb->state = 6;
> +
> +		goto again;
> +
> +	case 4: /* read the status register
> +				to understand source of the INT_ERR */
> +		if (!msb_read_regs(msb,
> +			offsetof(struct ms_register, status),
> +			sizeof(struct ms_status_register)))
> +			return 0;
> +		break;
> +
> +	case 5: /* get results of status check */
> +		msb->regs.status = *(struct ms_status_register *)mrq->data;
> +		msb->state++;
> +
> +	case 6: /* Send extra data read request */
> +		if (!msb_read_regs(msb,
> +			offsetof(struct ms_register, extra_data),
> +			sizeof(struct ms_extra_data_register)))
> +			return 0;
> +		break;
> +
> +	case 7: /* Save result of extra data request */
> +		msb->regs.extra_data =
> +			*(struct ms_extra_data_register *) mrq->data;
> +		msb->state++;
> +
> +	case 8: /* Send the  MS_TPC_READ_LONG_DATA to read IO buffer */
> +
> +		/* Skip that state if we only read the oob */
> +		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
> +			msb->state++;
> +			goto again;
> +		}
> +
> +		sg_init_table(sg, ARRAY_SIZE(sg));
> +		sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
> +			msb->current_sg_offset,
> +			msb->page_size);
> +
> +		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
> +		break;
> +
> +	case 9: /* check validity of data buffer & done */
> +
> +		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
> +			msb->current_sg_offset += msb->page_size;
> +			return msb_exit_state_machine(msb, 0);
> +		}
> +
> +		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
> +			dbg("read_page: uncorrectable error");
> +			return msb_exit_state_machine(msb, -EBADMSG);
> +		}
> +
> +		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
> +			dbg("read_page: correctable error");
> +			msb->current_sg_offset += msb->page_size;
> +			return msb_exit_state_machine(msb, -EUCLEAN);
> +		} else {
> +			dbg("read_page: INT error, but no status error bits");
> +			return msb_exit_state_machine(msb, -EIO);
> +		}
> +	default:
> +		BUG();
> +	}
> +	msb->state++;

I think this code would be quite a lot cleaner, safer and more readable
if you were to do away with the "msb->state++" concept altogether.  Just do

enum msb_states {
	state_foo1,
	state_foo2
	...
};

and do

	msb->state = state_foo2;

in each switch case.

This way, the reader doesn't have to scratch his head over what the
state _used_ to be, and the states have nice names to help the reader
understand what's going on.


> +	return 0;
> +}
>
> ...
>
> +/*
> + * This function is used to send simple IO requests to device that consist
> + * of register write + command
> + */
> +static int h_msb_send_command(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +

stray newline

> +	u8 intreg;
> +
> +	if (mrq->error) {
> +		dbg("send_command: unknown error");
> +		return msb_exit_state_machine(msb, mrq->error);
> +	}
> +again:
> +	switch (msb->state) {
> +
> +		/* HACK: see h_msb_write_block */
> +
>
> ...
>
> +static int h_msb_parallel_switch(struct memstick_dev *card,
> +					struct memstick_request **out_mrq)
> +{
> +	struct msb_data *msb = memstick_get_drvdata(card);
> +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> +

stray newline.  Please fix 'em up - it does make the code a little
discordant to read.

> +	struct memstick_host *host = card->host;
> +
> +	if (mrq->error) {
> +		dbg("parallel_switch: error");
> +		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
> +		return msb_exit_state_machine(msb, mrq->error);
> +	}
> +
> +	switch (msb->state) {
> +	case 0: /* Set the parallel interface on memstick side */
> +		msb->regs.param.system |= MEMSTICK_SYS_PAM;
> +
> +		if (!msb_write_regs(msb,
> +			offsetof(struct ms_register, param),
> +			1,
> +			(unsigned char *)&msb->regs.param))
> +			return 0;
> +		break;
> +
> +	case 1: /* Set parallel interface on our side + send a dummy request
> +			to see if card responds */
> +		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
> +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> +		break;
> +
> +	case 2:
> +		return msb_exit_state_machine(msb, 0);
> +	}
> +	msb->state++;
> +	return 0;
> +}
> +
> +static int msb_switch_to_parallel(struct msb_data *msb);
> +
> +/* Reset the card, to guard against hw errors beeing treated as bad blocks */
> +static int msb_reset(struct msb_data *msb, bool full)
> +{
> +
> +	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;

fyi, alas, this is (probably) slower than using an `int'.  gcc will
convert the result of the expression into 1-or-0.

> +	struct memstick_dev *card = msb->card;
> +	struct memstick_host *host = card->host;
> +	int error;
> +
> +	/* Reset the card */
> +	msb->regs.param.system = MEMSTICK_SYS_BAMD;
> +
> +	if (full) {
> +		error =  host->set_param(host,
> +					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
> +		if (error)
> +			goto out_error;
> +
> +		msb_invalidate_reg_window(msb);
> +
> +		error = host->set_param(host,
> +					MEMSTICK_POWER, MEMSTICK_POWER_ON);
> +		if (error)
> +			goto out_error;
> +
> +		error = host->set_param(host,
> +					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
> +		if (error) {
>
> ...
>
> +static int msb_erase_block(struct msb_data *msb, u16 pba)
> +{
> +	int error, try;
> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	dbg_verbose("erasing pba %d", pba);
> +
> +	for (try = 1 ; try < 3 ; try++) {
> +		msb->regs.param.block_address = cpu_to_be16(pba);
> +		msb->regs.param.page_address = 0;
> +		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
> +		msb->command_value = MS_CMD_BLOCK_ERASE;
> +		msb->command_need_oob = false;
> +
> +
> +		error = msb_run_state_machine(msb, h_msb_send_command);
> +		if (!error || msb_reset(msb, true))
> +			break;
> +	}
> +
> +	if (error) {
> +		ms_printk("erase failed, marking pba %d as bad", pba);
> +		msb_mark_bad(msb, pba);
> +	}
> +
> +	dbg_verbose("erase success, marking pba %d as unused", pba);
> +	msb_mark_block_unused(msb, pba);
> +	set_bit(pba, msb->erased_blocks_bitmap);

Should be able to use __set_bit() here, as msb_mark_block_unused()
required caller-provided locking.

> +	return error;
> +}
> +
>
> ...
>
> +static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
> +	struct ms_extra_data_register *extra)
> +{
> +	int error;
> +	BUG_ON(!extra);
> +

vertical whitespace oddnesses

> +	msb->regs.param.block_address = cpu_to_be16(pba);
> +	msb->regs.param.page_address = page;
> +	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
> +
> +	if (pba > msb->block_count) {
> +		ms_printk("BUG: attempt to read beyond"
> +					" the end of card at pba %d", pba);
> +		return -EINVAL;
> +	}
> +
> +	error = msb_run_state_machine(msb, h_msb_read_page);
> +	*extra = msb->regs.extra_data;
> +
> +	if (error == -EUCLEAN) {
> +		ms_printk("correctable error on pba %d, page %d",
> +			pba, page);
> +		return 0;
> +	}
> +
> +	return error;
> +}
> +
> +
> +/* Reads a block and compares it with data contained in scatterlist orig_sg */
> +static bool msb_verify_block(struct msb_data *msb, u16 pba,
> +				struct scatterlist *orig_sg,  int offset)
> +{
> +	struct scatterlist sg;
> +	int page = 0, error;
> +
> +	sg_init_one(&sg, msb->block_buffer, msb->block_size);
> +
> +	while (page < msb->pages_in_block) {
> +
> +		error = msb_read_page(msb, pba, page,
> +				NULL, &sg, page * msb->page_size);
> +		if (error)
> +			return -EIO;

msb_read_page() returned an errno.  Propagate that, rather than
overwriting it?

> +		page++;
> +	}
> +
> +	if (sg_compare_to_buffer(orig_sg, offset,
> +				msb->block_buffer, msb->block_size))
> +		return -EIO;
> +	return 0;
> +}
> +
>
> ...
>
> +static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)

hm.  Should block_nr be a sector_t?  

> +{
> +	struct ms_boot_page *boot_block;
> +	struct scatterlist sg;
> +	u16 *buffer = NULL;
> +	int offset = 0;
> +
> +	int i, error = 0;
> +	int data_size, data_offset, page, page_offset, size_to_read;
> +	u16 pba;

vertical whitespace oddness

> +	BUG_ON(block_nr > 1);
> +
> +	boot_block = &msb->boot_page[block_nr];
> +	pba = msb->boot_block_locations[block_nr];
> +
>
> ...
>
> +static int msb_cache_write(struct msb_data *msb, int lba,
> +	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
> +{
> +	int error;
> +	struct scatterlist sg_tmp[10];

newline..

> +	if (msb->read_only)
> +		return -EROFS;
> +
> +	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
> +						lba != msb->cache_block_lba)
> +		if (add_to_cache_only)
> +			return 0;
> +
> +	/* If we need to write different block */
> +	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
> +						lba != msb->cache_block_lba) {
> +		dbg_verbose("first flush the cache");
> +		error = msb_cache_flush(msb);
> +		if (error)
> +			return error;
> +	}
> +
> +	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
> +		msb->cache_block_lba  = lba;
> +		mod_timer(&msb->cache_flush_timer,
> +			jiffies + msecs_to_jiffies(cache_flush_timeout));
> +	}
> +
> +	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
> +
> +	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
> +	sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
> +
> +	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
> +		msb->cache + page * msb->page_size, msb->page_size);
> +
> +	set_bit(page, &msb->valid_cache_bitmap);
> +	return 0;
> +}
> +
>
> ...
>
> +static int msb_do_write_request(struct msb_data *msb, int lba,
> +	int page, struct scatterlist *sg, int len, int *sucessfuly_written)
> +{
> +	int error = 0;
> +	int offset = 0;
> +	*sucessfuly_written = 0;

(more vertical whitespace glitches)

Again the types look a bit wrong.  I'd have expected `len' and `offset'
to be size_t.

> +	while (offset < len) {
> +		if (page == 0 && len - offset >= msb->block_size) {
> +
> +			if (msb->cache_block_lba == lba)
> +				msb_cache_discard(msb);
> +
> +			dbg_verbose("Writing whole lba %d", lba);
> +			error = msb_update_block(msb, lba, sg, offset);
> +			if (error)
> +				return error;
> +
> +			offset += msb->block_size;
> +			*sucessfuly_written += msb->block_size;
> +			lba++;
> +			continue;
> +		}
> +
> +		error = msb_cache_write(msb, lba, page, false, sg, offset);
> +		if (error)
> +			return error;
> +
> +		offset += msb->page_size;
> +		*sucessfuly_written += msb->page_size;
> +
> +		page++;
> +		if (page == msb->pages_in_block) {
> +			page = 0;
> +			lba++;
> +		}
> +	}
> +	return 0;
> +}
>
> ...
>
> +static DEFINE_IDR(msb_disk_idr);
> +static DEFINE_MUTEX(msb_disk_lock);

A little comment which describes the overall role of msb_disk_idr wold
be nice.

>
> ...
>

Re: [PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

On Wed, 2012-09-19 at 14:52 -0700, Andrew Morton wrote: 
> On Wed, 19 Sep 2012 16:19:02 +0300
> Maxim Levitsky <maximlevitsky@gmail.com> wrote:
> 
> > Based partially on MS standard spec quotes from Alex Dubov.
> > 
> > As any code that works with user data this driver isn't
> > recommended to use to write cards that contain valuable data.
> > 
> > It tries its best though to avoid data corruption
> > and possible damage to the card.
> > 
> > Tested on MS DUO 64 MB card on Ricoh R592 card reader..
> > 
> 
> Generally looks nice to me, although I wouldn't know a memstick if one
> stuck to my shoe.
> 
> I have a bunch of fairly trivial comments, and one head-scratcher for
> Tejun: what's up with the local-irqs-disabled requirement in sg_miter_next()?

Awesome, thanks for the review!
I will address all comments very soon.

> 
> > ...
> >
> > +static int sg_nents(struct scatterlist *sg)
> > +{
> > +	int nents = 0;
> > +	while (sg) {
> > +		nents++;
> > +		sg = sg_next(sg);
> > +	}
> > +
> > +	return nents;
> > +}
> 
> I think we may as well put this in scatterlist.c immediately.  It
> wouldn't surprise me if we already have open-coded copies of this lying
> around the tree.
I agree, but as usual afraid that it wont be accepted here.

> 
> > +/*
> > + * Copies section of 'sg_from' starting from offset 'offset' and with length
> > + * 'len' To another scatterlist of to_nents enties
> > + */
> > +static int sg_copy(struct scatterlist *sg_from, struct scatterlist *sg_to,
> > +					int to_nents, int offset, size_t len)
> 
> The should return a size_t, to match the type of `len'.  And the type
> of `offset' is fishy - there's nothing which intrinsically limits these
> things to 2G?
In my case, I will never have large offsets here, but I agree with you
completely. 
> 
> > +{
> > +	int copied = 0;
> > +
> > +	while (offset > 0) {
> > +
> > +		if (offset >= sg_from->length) {
> > +			if (sg_is_last(sg_from))
> > +				return 0;
> > +
> > +			offset -= sg_from->length;
> > +			sg_from = sg_next(sg_from);
> > +			continue;
> > +		}
> > +
> > +		copied = min(len, (size_t)(sg_from->length - offset));
> 
> min_t would be more conventional.  Or perhaps even min(), depending on
> `offset's new type (but probably not).
Agree. 
> 
> > +		sg_set_page(sg_to, sg_page(sg_from),
> > +			copied, sg_from->offset + offset);
> > +
> > +		len -= copied;
> > +		offset = 0;
> > +
> > +		if (sg_is_last(sg_from) || !len)
> > +			goto out;
> > +
> > +		sg_to = sg_next(sg_to);
> > +		to_nents--;
> > +		sg_from = sg_next(sg_from);
> > +	}
> > +
> > +	while (len > sg_from->length && to_nents--) {
> > +
> > +		len -= sg_from->length;
> > +		copied += sg_from->length;
> > +
> > +		sg_set_page(sg_to, sg_page(sg_from),
> > +				sg_from->length, sg_from->offset);
> > +
> > +		if (sg_is_last(sg_from) || !len)
> > +			goto out;
> > +
> > +		sg_from = sg_next(sg_from);
> > +		sg_to = sg_next(sg_to);
> > +	}
> > +
> > +	if (len && to_nents) {
> > +		sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
> > +		copied += len;
> > +	}
> > +
> > +out:
> > +	sg_mark_end(sg_to);
> > +	return copied;
> > +}
> > +
> > +/*
> > + * Compares section of 'sg' starting from offset 'offset' and with length 'len'
> > + * to linear buffer of length 'len' at address 'buffer'
> 
> The comment should document the return value.
> 
> > + */
> > +static bool sg_compare_to_buffer(struct scatterlist *sg,
> > +					int offset, u8 *buffer, size_t len)
> > +{
> > +	unsigned long flags;
> > +	int retval = 0;
> > +	struct sg_mapping_iter miter;
> > +
> > +	local_irq_save(flags);
> 
> hm, the "IRQ disabled if SG_MITER_ATOMIC" requirement of
> sg_miter_next() is weird and unexpected.
> 
> Tejun's 137d3edb48425f8 added the code whch has this strange
> requirement, but it is unexplained.  Wazzup?
> 
> > +	sg_miter_start(&miter, sg, sg_nents(sg),
> > +					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
> > +
> > +	while (sg_miter_next(&miter) && len > 0) {
> > +
> > +		int cmplen;
> > +
> > +		if (offset >= miter.length) {
> > +			offset -= miter.length;
> > +			continue;
> > +		}
> > +
> > +		cmplen = min(miter.length - offset, len);
> > +		retval = memcmp(miter.addr + offset, buffer, cmplen);
> > +		if (retval)
> > +			break;
> > +
> > +		buffer += cmplen;
> > +		len -= cmplen;
> > +		offset = 0;
> > +	}
> > +
> > +	if (!retval && len)
> > +		retval = -1;
> > +
> > +	sg_miter_stop(&miter);
> > +	local_irq_restore(flags);
> > +	return retval;
> > +}
> >
> > ...
> >
> > +static void msb_mark_block_unused(struct msb_data *msb, int pba)
> > +{
> > +	int zone = msb_get_zone_from_pba(pba);
> > +
> > +	if (!test_bit(pba, msb->used_blocks_bitmap)) {
> > +		ms_printk("BUG: attempt to mark "
> > +				"already unused pba %d as unused" , pba);
> > +		msb->read_only = true;
> > +		return;
> > +	}
> > +
> > +#ifdef DEBUG
> > +	if (msb_validate_used_block_bitmap(msb))
> > +		return;
> > +#endif
> > +	clear_bit(pba, msb->used_blocks_bitmap);
> > +	msb->free_block_count[zone]++;
> > +}
> 
> Presumably there is some lock which the caller must hold to prevent the
> obvious races in the above two functions.  I suggest adding a comment
> whcih documents this requirement, then take advantage of that lock to
> switch to the more efficient __set_bit() and __clear_bit().
There can't be races in the driver, since it contains a single thread
that does all the IO it got from block layer.
The thread is awaken each time the request function of block device is
called.
This why I didn't do much locking in here. In addition I found out that
this is quite common way to implement a block device driver.

> 
> > +/* Invalidate current register window*/
> 
> hm, checkpatch misses the missing space before */
> 
> > +static void msb_invalidate_reg_window(struct msb_data *msb)
> > +{
> > +	msb->reg_addr.w_offset = offsetof(struct ms_register, id);
> > +	msb->reg_addr.w_length = sizeof(struct ms_id_register);
> > +	msb->reg_addr.r_offset = offsetof(struct ms_register, id);
> > +	msb->reg_addr.r_length = sizeof(struct ms_id_register);
> > +	msb->addr_valid = false;
> > +}
> > +
> > +/* Sane way to start a state machine*/
> > +static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
> > +		(struct memstick_dev *card, struct memstick_request **req))
> > +{
> > +	struct memstick_dev *card = msb->card;
> > +
> > +	WARN_ON(msb->state != -1);
> > +	msb->int_polling = false;
> > +	msb->state = 0;
> > +	msb->exit_error = 0;
> > +
> > +	memset(&card->current_mrq, 0, sizeof(card->current_mrq));
> > +
> > +	card->next_request = state_func;
> > +	memstick_new_req(card->host);
> > +	wait_for_completion(&card->mrq_complete);
> > +
> > +	WARN_ON(msb->state != -1);
> > +	return msb->exit_error;
> > +}
> > +
> > +/* State machines call that to exit */
> > +int msb_exit_state_machine(struct msb_data *msb, int error)
> 
> static
> 
> > +{
> > +	WARN_ON(msb->state == -1);
> > +
> > +	msb->state = -1;
> > +	msb->exit_error = error;
> > +	msb->card->next_request = h_msb_default_bad;
> > +
> > +	/* Invalidate reg window on errors */
> > +	if (error)
> > +		msb_invalidate_reg_window(msb);
> > +
> > +	complete(&msb->card->mrq_complete);
> > +	return -ENXIO;
> > +}
> > +
> > +/* read INT register */
> > +int msb_read_int_reg(struct msb_data *msb, long timeout)
> 
> static
> 
> > +{
> > +	struct memstick_request *mrq = &msb->card->current_mrq;
> > +	WARN_ON(msb->state == -1);
> 
> It's conventional to put a newline between end-of-locals and start-of-code.
> 
> > +	if (!msb->int_polling) {
> > +		msb->int_timeout = jiffies +
> > +			msecs_to_jiffies(timeout == -1 ? 500 : timeout);
> > +		msb->int_polling = true;
> > +	} else if (time_after(jiffies, msb->int_timeout)) {
> > +		mrq->data[0] = MEMSTICK_INT_CMDNAK;
> > +		return 0;
> > +	}
> > +
> > +	if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
> > +				mrq->need_card_int && !mrq->error) {
> > +		mrq->data[0] = mrq->int_reg;
> > +		mrq->need_card_int = false;
> > +		return 0;
> > +	} else {
> > +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> > +		return 1;
> > +	}
> > +}
> > +
> > +/* Read a register */
> > +int msb_read_regs(struct msb_data *msb, int offset, int len)
> 
> static.  I'll stop checking them now ;)
> 
> > +{
> > +	struct memstick_request *req = &msb->card->current_mrq;
> > +
> > +	if (msb->reg_addr.r_offset != offset ||
> > +	    msb->reg_addr.r_length != len || !msb->addr_valid) {
> > +
> > +		msb->reg_addr.r_offset = offset;
> > +		msb->reg_addr.r_length = len;
> > +		msb->addr_valid = true;
> > +
> > +		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
> > +			&msb->reg_addr, sizeof(msb->reg_addr));
> > +		return 0;
> > +	}
> > +
> > +	memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
> > +	return 1;
> > +}
> > +
> > +/* Write a card register */
> > +int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
> > +{
> > +	struct memstick_request *req = &msb->card->current_mrq;
> > +
> > +	if (msb->reg_addr.w_offset != offset ||
> > +		msb->reg_addr.w_length != len  || !msb->addr_valid) {
> > +
> > +		msb->reg_addr.w_offset = offset;
> > +		msb->reg_addr.w_length = len;
> > +		msb->addr_valid = true;
> > +
> > +		memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
> > +			&msb->reg_addr, sizeof(msb->reg_addr));
> > +		return 0;
> > +	}
> > +
> > +	memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
> > +	return 1;
> > +}
> > +
> > +/* Handler for absense of IO */
> 
> "absence" :)
> 
> > +static int h_msb_default_bad(struct memstick_dev *card,
> > +						struct memstick_request **mrq)
> > +{
> > +	return -ENXIO;
> > +}
> > +
> > +/*
> > + * This function is a handler for reads of one page from device.
> > + * Writes output to msb->current_sg, takes sector address from msb->reg.param
> > + * Can also be used to read extra data only. Set params accordintly.
> > + */
> > +static int h_msb_read_page(struct memstick_dev *card,
> > +					struct memstick_request **out_mrq)
> > +{
> > +	struct msb_data *msb = memstick_get_drvdata(card);
> > +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> > +	struct scatterlist sg[2];
> > +	u8 command, intreg;
> > +
> > +	if (mrq->error) {
> > +		dbg("read_page, unknown error");
> > +		return msb_exit_state_machine(msb, mrq->error);
> > +	}
> > +again:
> > +	switch (msb->state) {
> > +	case 0: /* Write the sector address */
> > +		if (!msb_write_regs(msb,
> > +			offsetof(struct ms_register, param),
> > +			sizeof(struct ms_param_register),
> > +			(unsigned char *)&msb->regs.param))
> > +			return 0;
> > +		break;
> > +
> > +	case 1: /* Execute the read command*/
> > +		command = MS_CMD_BLOCK_READ;
> > +		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
> > +		break;
> > +
> > +	case 2: /* send INT request */
> > +		if (msb_read_int_reg(msb, -1))
> > +			break;
> > +		msb->state++;
> > +
> > +	case 3: /* get result of the INT request*/
> > +		intreg = mrq->data[0];
> > +		msb->regs.status.interrupt = intreg;
> > +
> > +		if (intreg & MEMSTICK_INT_CMDNAK)
> > +			return msb_exit_state_machine(msb, -EIO);
> > +
> > +		if (!(intreg & MEMSTICK_INT_CED)) {
> > +			msb->state--;
> > +			goto again;
> > +		}
> > +
> > +		msb->int_polling = false;
> > +
> > +		if (intreg & MEMSTICK_INT_ERR)
> > +			msb->state++;
> > +		else
> > +			msb->state = 6;
> > +
> > +		goto again;
> > +
> > +	case 4: /* read the status register
> > +				to understand source of the INT_ERR */
> > +		if (!msb_read_regs(msb,
> > +			offsetof(struct ms_register, status),
> > +			sizeof(struct ms_status_register)))
> > +			return 0;
> > +		break;
> > +
> > +	case 5: /* get results of status check */
> > +		msb->regs.status = *(struct ms_status_register *)mrq->data;
> > +		msb->state++;
> > +
> > +	case 6: /* Send extra data read request */
> > +		if (!msb_read_regs(msb,
> > +			offsetof(struct ms_register, extra_data),
> > +			sizeof(struct ms_extra_data_register)))
> > +			return 0;
> > +		break;
> > +
> > +	case 7: /* Save result of extra data request */
> > +		msb->regs.extra_data =
> > +			*(struct ms_extra_data_register *) mrq->data;
> > +		msb->state++;
> > +
> > +	case 8: /* Send the  MS_TPC_READ_LONG_DATA to read IO buffer */
> > +
> > +		/* Skip that state if we only read the oob */
> > +		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
> > +			msb->state++;
> > +			goto again;
> > +		}
> > +
> > +		sg_init_table(sg, ARRAY_SIZE(sg));
> > +		sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
> > +			msb->current_sg_offset,
> > +			msb->page_size);
> > +
> > +		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
> > +		break;
> > +
> > +	case 9: /* check validity of data buffer & done */
> > +
> > +		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
> > +			msb->current_sg_offset += msb->page_size;
> > +			return msb_exit_state_machine(msb, 0);
> > +		}
> > +
> > +		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
> > +			dbg("read_page: uncorrectable error");
> > +			return msb_exit_state_machine(msb, -EBADMSG);
> > +		}
> > +
> > +		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
> > +			dbg("read_page: correctable error");
> > +			msb->current_sg_offset += msb->page_size;
> > +			return msb_exit_state_machine(msb, -EUCLEAN);
> > +		} else {
> > +			dbg("read_page: INT error, but no status error bits");
> > +			return msb_exit_state_machine(msb, -EIO);
> > +		}
> > +	default:
> > +		BUG();
> > +	}
> > +	msb->state++;
> 
> I think this code would be quite a lot cleaner, safer and more readable
> if you were to do away with the "msb->state++" concept altogether.  Just do
> 
> enum msb_states {
> 	state_foo1,
> 	state_foo2
> 	...
> };
> 
> and do
> 
> 	msb->state = state_foo2;
> 
> in each switch case.
> 
> This way, the reader doesn't have to scratch his head over what the
> state _used_ to be, and the states have nice names to help the reader
> understand what's going on.
I now remember hearing about that back then too, and on second thought,
I'll do that.

> 
> 
> > +	return 0;
> > +}
> >
> > ...
> >
> > +/*
> > + * This function is used to send simple IO requests to device that consist
> > + * of register write + command
> > + */
> > +static int h_msb_send_command(struct memstick_dev *card,
> > +					struct memstick_request **out_mrq)
> > +{
> > +	struct msb_data *msb = memstick_get_drvdata(card);
> > +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> > +
> 
> stray newline
> 
> > +	u8 intreg;
> > +
> > +	if (mrq->error) {
> > +		dbg("send_command: unknown error");
> > +		return msb_exit_state_machine(msb, mrq->error);
> > +	}
> > +again:
> > +	switch (msb->state) {
> > +
> > +		/* HACK: see h_msb_write_block */
> > +
> >
> > ...
> >
> > +static int h_msb_parallel_switch(struct memstick_dev *card,
> > +					struct memstick_request **out_mrq)
> > +{
> > +	struct msb_data *msb = memstick_get_drvdata(card);
> > +	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
> > +
> 
> stray newline.  Please fix 'em up - it does make the code a little
> discordant to read.
> 
> > +	struct memstick_host *host = card->host;
> > +
> > +	if (mrq->error) {
> > +		dbg("parallel_switch: error");
> > +		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
> > +		return msb_exit_state_machine(msb, mrq->error);
> > +	}
> > +
> > +	switch (msb->state) {
> > +	case 0: /* Set the parallel interface on memstick side */
> > +		msb->regs.param.system |= MEMSTICK_SYS_PAM;
> > +
> > +		if (!msb_write_regs(msb,
> > +			offsetof(struct ms_register, param),
> > +			1,
> > +			(unsigned char *)&msb->regs.param))
> > +			return 0;
> > +		break;
> > +
> > +	case 1: /* Set parallel interface on our side + send a dummy request
> > +			to see if card responds */
> > +		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
> > +		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
> > +		break;
> > +
> > +	case 2:
> > +		return msb_exit_state_machine(msb, 0);
> > +	}
> > +	msb->state++;
> > +	return 0;
> > +}
> > +
> > +static int msb_switch_to_parallel(struct msb_data *msb);
> > +
> > +/* Reset the card, to guard against hw errors beeing treated as bad blocks */
> > +static int msb_reset(struct msb_data *msb, bool full)
> > +{
> > +
> > +	bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
> 
> fyi, alas, this is (probably) slower than using an `int'.  gcc will
> convert the result of the expression into 1-or-0.
Didn't knew about that. Here this code is executed once per card insert,
so it sure doesn't matter. 
> 
> > +	struct memstick_dev *card = msb->card;
> > +	struct memstick_host *host = card->host;
> > +	int error;
> > +
> > +	/* Reset the card */
> > +	msb->regs.param.system = MEMSTICK_SYS_BAMD;
> > +
> > +	if (full) {
> > +		error =  host->set_param(host,
> > +					MEMSTICK_POWER, MEMSTICK_POWER_OFF);
> > +		if (error)
> > +			goto out_error;
> > +
> > +		msb_invalidate_reg_window(msb);
> > +
> > +		error = host->set_param(host,
> > +					MEMSTICK_POWER, MEMSTICK_POWER_ON);
> > +		if (error)
> > +			goto out_error;
> > +
> > +		error = host->set_param(host,
> > +					MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
> > +		if (error) {
> >
> > ...
> >
> > +static int msb_erase_block(struct msb_data *msb, u16 pba)
> > +{
> > +	int error, try;
> > +	if (msb->read_only)
> > +		return -EROFS;
> > +
> > +	dbg_verbose("erasing pba %d", pba);
> > +
> > +	for (try = 1 ; try < 3 ; try++) {
> > +		msb->regs.param.block_address = cpu_to_be16(pba);
> > +		msb->regs.param.page_address = 0;
> > +		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
> > +		msb->command_value = MS_CMD_BLOCK_ERASE;
> > +		msb->command_need_oob = false;
> > +
> > +
> > +		error = msb_run_state_machine(msb, h_msb_send_command);
> > +		if (!error || msb_reset(msb, true))
> > +			break;
> > +	}
> > +
> > +	if (error) {
> > +		ms_printk("erase failed, marking pba %d as bad", pba);
> > +		msb_mark_bad(msb, pba);
> > +	}
> > +
> > +	dbg_verbose("erase success, marking pba %d as unused", pba);
> > +	msb_mark_block_unused(msb, pba);
> > +	set_bit(pba, msb->erased_blocks_bitmap);
> 
> Should be able to use __set_bit() here, as msb_mark_block_unused()
> required caller-provided locking.
Again, I think I don't need locking here. 
> 
> > +	return error;
> > +}
> > +
> >
> > ...
> >
> > +static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
> > +	struct ms_extra_data_register *extra)
> > +{
> > +	int error;
> > +	BUG_ON(!extra);
> > +
> 
> vertical whitespace oddnesses
> 
> > +	msb->regs.param.block_address = cpu_to_be16(pba);
> > +	msb->regs.param.page_address = page;
> > +	msb->regs.param.cp = MEMSTICK_CP_EXTRA;
> > +
> > +	if (pba > msb->block_count) {
> > +		ms_printk("BUG: attempt to read beyond"
> > +					" the end of card at pba %d", pba);
> > +		return -EINVAL;
> > +	}
> > +
> > +	error = msb_run_state_machine(msb, h_msb_read_page);
> > +	*extra = msb->regs.extra_data;
> > +
> > +	if (error == -EUCLEAN) {
> > +		ms_printk("correctable error on pba %d, page %d",
> > +			pba, page);
> > +		return 0;
> > +	}
> > +
> > +	return error;
> > +}
> > +
> > +
> > +/* Reads a block and compares it with data contained in scatterlist orig_sg */
> > +static bool msb_verify_block(struct msb_data *msb, u16 pba,
> > +				struct scatterlist *orig_sg,  int offset)
> > +{
> > +	struct scatterlist sg;
> > +	int page = 0, error;
> > +
> > +	sg_init_one(&sg, msb->block_buffer, msb->block_size);
> > +
> > +	while (page < msb->pages_in_block) {
> > +
> > +		error = msb_read_page(msb, pba, page,
> > +				NULL, &sg, page * msb->page_size);
> > +		if (error)
> > +			return -EIO;
> 
> msb_read_page() returned an errno.  Propagate that, rather than
> overwriting it?
Sure! 
> 
> > +		page++;
> > +	}
> > +
> > +	if (sg_compare_to_buffer(orig_sg, offset,
> > +				msb->block_buffer, msb->block_size))
> > +		return -EIO;
> > +	return 0;
> > +}
> > +
> >
> > ...
> >
> > +static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
> 
> hm.  Should block_nr be a sector_t?
Actually no, as its an index in internal array of 'boot blocks', aka
superblocks and there should be just two of them
msb_read_boot_blocks reads them.
Each of boot blocks contains a pointer to bad blocks table. 
> 
> 
> > +{
> > +	struct ms_boot_page *boot_block;
> > +	struct scatterlist sg;
> > +	u16 *buffer = NULL;
> > +	int offset = 0;
> > +
> > +	int i, error = 0;
> > +	int data_size, data_offset, page, page_offset, size_to_read;
> > +	u16 pba;
> 
> vertical whitespace oddness
> 
> > +	BUG_ON(block_nr > 1);
> > +
> > +	boot_block = &msb->boot_page[block_nr];
> > +	pba = msb->boot_block_locations[block_nr];
> > +
> >
> > ...
> >
> > +static int msb_cache_write(struct msb_data *msb, int lba,
> > +	int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
> > +{
> > +	int error;
> > +	struct scatterlist sg_tmp[10];
> 
> newline..
> 
> > +	if (msb->read_only)
> > +		return -EROFS;
> > +
> > +	if (msb->cache_block_lba == MS_BLOCK_INVALID ||
> > +						lba != msb->cache_block_lba)
> > +		if (add_to_cache_only)
> > +			return 0;
> > +
> > +	/* If we need to write different block */
> > +	if (msb->cache_block_lba != MS_BLOCK_INVALID &&
> > +						lba != msb->cache_block_lba) {
> > +		dbg_verbose("first flush the cache");
> > +		error = msb_cache_flush(msb);
> > +		if (error)
> > +			return error;
> > +	}
> > +
> > +	if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
> > +		msb->cache_block_lba  = lba;
> > +		mod_timer(&msb->cache_flush_timer,
> > +			jiffies + msecs_to_jiffies(cache_flush_timeout));
> > +	}
> > +
> > +	dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
> > +
> > +	sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
> > +	sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
> > +
> > +	sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
> > +		msb->cache + page * msb->page_size, msb->page_size);
> > +
> > +	set_bit(page, &msb->valid_cache_bitmap);
> > +	return 0;
> > +}
> > +
> >
> > ...
> >
> > +static int msb_do_write_request(struct msb_data *msb, int lba,
> > +	int page, struct scatterlist *sg, int len, int *sucessfuly_written)
> > +{
> > +	int error = 0;
> > +	int offset = 0;
> > +	*sucessfuly_written = 0;
> 
> (more vertical whitespace glitches)
> 
> Again the types look a bit wrong.  I'd have expected `len' and `offset'
> to be size_t.
> 
> > +	while (offset < len) {
> > +		if (page == 0 && len - offset >= msb->block_size) {
> > +
> > +			if (msb->cache_block_lba == lba)
> > +				msb_cache_discard(msb);
> > +
> > +			dbg_verbose("Writing whole lba %d", lba);
> > +			error = msb_update_block(msb, lba, sg, offset);
> > +			if (error)
> > +				return error;
> > +
> > +			offset += msb->block_size;
> > +			*sucessfuly_written += msb->block_size;
> > +			lba++;
> > +			continue;
> > +		}
> > +
> > +		error = msb_cache_write(msb, lba, page, false, sg, offset);
> > +		if (error)
> > +			return error;
> > +
> > +		offset += msb->page_size;
> > +		*sucessfuly_written += msb->page_size;
> > +
> > +		page++;
> > +		if (page == msb->pages_in_block) {
> > +			page = 0;
> > +			lba++;
> > +		}
> > +	}
> > +	return 0;
> > +}
> >
> > ...
> >
> > +static DEFINE_IDR(msb_disk_idr);
> > +static DEFINE_MUTEX(msb_disk_lock);
> 
> A little comment which describes the overall role of msb_disk_idr wold
> be nice. 
Sure!

The things I didn't comment on I agree too, I will fix all these
non-static functions and newlines. Newlines I put mostly to make the
code a bit more clear to myself, but I really don't mind removing these.

Thanks again for a review, I will address all the issues very soon.

-- 
Best regards,
        Maxim Levitsky

Re: [PATCH] memstick: add support for legacy memorysticks

[Tejun Heo]

On Thu, Sep 20, 2012 at 07:05:42AM +0300, Maxim Levitsky wrote:
> There can't be races in the driver, since it contains a single thread
> that does all the IO it got from block layer.
> The thread is awaken each time the request function of block device is
> called.
> This why I didn't do much locking in here. In addition I found out that
> this is quite common way to implement a block device driver.

Please use workqueue instead of raw kthread.

Thanks!

-- 
tejun

Re: [PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

On Thu, 2012-09-20 at 10:53 -0700, Tejun Heo wrote: 
> On Thu, Sep 20, 2012 at 07:05:42AM +0300, Maxim Levitsky wrote:
> > There can't be races in the driver, since it contains a single thread
> > that does all the IO it got from block layer.
> > The thread is awaken each time the request function of block device is
> > called.
> > This why I didn't do much locking in here. In addition I found out that
> > this is quite common way to implement a block device driver.
> 
> Please use workqueue instead of raw kthread.
> 
> Thanks!
Now that my exams done....
Can you spare me from using a workqueue?
The point is that using current model I wake the worker thread as much
as I want to, and I know that it will be woken once an will do all the
work till request queue is empty.
With workqueues, it doesn't work this way. I have to pass the request as
a work item or something like that.
Any pointers?

Best regards,
Maxim Levitsky 

Re: [PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

On Mon, 2012-09-24 at 16:59 +0200, Maxim Levitsky wrote: 
> On Thu, 2012-09-20 at 10:53 -0700, Tejun Heo wrote: 
> > On Thu, Sep 20, 2012 at 07:05:42AM +0300, Maxim Levitsky wrote:
> > > There can't be races in the driver, since it contains a single thread
> > > that does all the IO it got from block layer.
> > > The thread is awaken each time the request function of block device is
> > > called.
> > > This why I didn't do much locking in here. In addition I found out that
> > > this is quite common way to implement a block device driver.
> > 
> > Please use workqueue instead of raw kthread.
> > 
> > Thanks!
> Now that my exams done....
> Can you spare me from using a workqueue?
> The point is that using current model I wake the worker thread as much
> as I want to, and I know that it will be woken once an will do all the
> work till request queue is empty.
> With workqueues, it doesn't work this way. I have to pass the request as
> a work item or something like that.
> Any pointers?
Also probably due to that reason MMC doesn't use a workqueue ether, but
a raw kthread, in pretty much same way I do.


-- 
Best regards,
        Maxim Levitsky

Re: [PATCH] memstick: add support for legacy memorysticks

[Tejun Heo]

Hello,

On Mon, Sep 24, 2012 at 05:09:23PM +0200, Maxim Levitsky wrote:
> > Now that my exams done....
> > Can you spare me from using a workqueue?

I'd much prefer if you convert to workqueue.

> > The point is that using current model I wake the worker thread as much
> > as I want to, and I know that it will be woken once an will do all the
> > work till request queue is empty.

You can do exactly the same thing by scheduling the same work item
multiple times.  "Waking up" just becomes "scheduling the work item".

> > With workqueues, it doesn't work this way. I have to pass the request as
> > a work item or something like that.
> > Any pointers?

No, there's no reason to change the structure of the code in any way.
Just use a work item as you would use a kthread.

> Also probably due to that reason MMC doesn't use a workqueue ether, but
> a raw kthread, in pretty much same way I do.

Mostly because I haven't gotten around to convert it yet.  The
problems with direct kthread usage are that they're much more
difficult to get completely correct with freeze and exit conditions -
the last time I checked it was easier to spot broken ones than correct
ones - and they create dedicated threads which usually are
underutilized.

Thanks.

-- 
tejun

Re: [PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

On Mon, 2012-09-24 at 11:05 -0700, Tejun Heo wrote: 
> Hello,
> 
> On Mon, Sep 24, 2012 at 05:09:23PM +0200, Maxim Levitsky wrote:
> > > Now that my exams done....
> > > Can you spare me from using a workqueue?
> 
> I'd much prefer if you convert to workqueue.
> 
> > > The point is that using current model I wake the worker thread as much
> > > as I want to, and I know that it will be woken once an will do all the
> > > work till request queue is empty.
> 
> You can do exactly the same thing by scheduling the same work item
> multiple times.  "Waking up" just becomes "scheduling the work item".
I don't believe that will work this way.
I will dig through the source, and see how to do that.



> > > With workqueues, it doesn't work this way. I have to pass the request as
> > > a work item or something like that.
> > > Any pointers?
> 
> No, there's no reason to change the structure of the code in any way.
> Just use a work item as you would use a kthread.
Except that if I schedule a same work item few times, these work items
will be 'processed' in parallel, although there is just one work to do,
work of pulling the requests from block queue until it has them, and
dispatching them through my code.
Or I can get a guarantee that work items wont be processed in parallel?
Stiil, even with that only first work item will do the actual work,
others will wake the workqueue for nothing, but I am ok with that.



> 
> > Also probably due to that reason MMC doesn't use a workqueue ether, but
> > a raw kthread, in pretty much same way I do.
> 
> Mostly because I haven't gotten around to convert it yet.  The
> problems with direct kthread usage are that they're much more
> difficult to get completely correct with freeze and exit conditions -
Agreed, I had my own share of frustration with this. I got it right in
the end though.

> the last time I checked it was easier to spot broken ones than correct
> ones - and they create dedicated threads which usually are
> underutilized.
True too, although, a sleeping thread doesn't cost much. 
> 
> Thanks.
> 

-- 
Best regards,
        Maxim Levitsky

Re: [PATCH] memstick: add support for legacy memorysticks

[Maxim Levitsky]

On Mon, 2012-09-24 at 20:19 +0200, Maxim Levitsky wrote: 
> On Mon, 2012-09-24 at 11:05 -0700, Tejun Heo wrote: 
> > Hello,
> > 
> > On Mon, Sep 24, 2012 at 05:09:23PM +0200, Maxim Levitsky wrote:
> > > > Now that my exams done....
> > > > Can you spare me from using a workqueue?
> > 
> > I'd much prefer if you convert to workqueue.
> > 
> > > > The point is that using current model I wake the worker thread as much
> > > > as I want to, and I know that it will be woken once an will do all the
> > > > work till request queue is empty.
> > 
> > You can do exactly the same thing by scheduling the same work item
> > multiple times.  "Waking up" just becomes "scheduling the work item".
> I don't believe that will work this way.
> I will dig through the source, and see how to do that.
> 
> 
> 
> > > > With workqueues, it doesn't work this way. I have to pass the request as
> > > > a work item or something like that.
> > > > Any pointers?
> > 
> > No, there's no reason to change the structure of the code in any way.
> > Just use a work item as you would use a kthread.
> Except that if I schedule a same work item few times, these work items
> will be 'processed' in parallel, although there is just one work to do,
> work of pulling the requests from block queue until it has them, and
> dispatching them through my code.
> Or I can get a guarantee that work items wont be processed in parallel?
> Stiil, even with that only first work item will do the actual work,
> others will wake the workqueue for nothing, but I am ok with that.
Should have looked through the source. Understand now.
Just one quick question, should I create my own workqueue or use
schedule_work? if I use the later and my work function sleeps, will it
harmfully affect other users of this function?


-- 
Best regards,
        Maxim Levitsky

Re: [PATCH] memstick: add support for legacy memorysticks

[Tejun Heo]

Hello,

On Mon, Sep 24, 2012 at 08:24:37PM +0200, Maxim Levitsky wrote:
> Should have looked through the source. Understand now.
> Just one quick question, should I create my own workqueue or use
> schedule_work? if I use the later and my work function sleeps, will it
> harmfully affect other users of this function?

If memstick can be in memory reclaim path (most block devices are),
you would want to create a dedicated workqueue w/ rescuer.  For
details, please read Documentation/workqueue.txt.

Thanks.

-- 
tejun

Re: [PATCH] memstick: add support for legacy memorysticks

[Tejun Heo]

Hello,

On Mon, Sep 24, 2012 at 08:19:28PM +0200, Maxim Levitsky wrote:
> Except that if I schedule a same work item few times, these work items
> will be 'processed' in parallel, although there is just one work to do,
> work of pulling the requests from block queue until it has them, and
> dispatching them through my code.
> Or I can get a guarantee that work items wont be processed in parallel?

You need to use system_nrt_wq for that before 3.7-rc1.  After 3.7-rc1,
any workqueue will guarantee that.

> Stiil, even with that only first work item will do the actual work,
> others will wake the workqueue for nothing, but I am ok with that.

It's just like waking up spuriously.  The work item is guaranteed to
be executed at least once after any given schedule/queue_work() call.

Thanks.

-- 
tejun

Re: [PATCH] memstick: add support for legacy memorysticks

[Tejun Heo]

Hello, Andrew.

On Wed, Sep 19, 2012 at 02:52:28PM -0700, Andrew Morton wrote:
> I have a bunch of fairly trivial comments, and one head-scratcher for
> Tejun: what's up with the local-irqs-disabled requirement in sg_miter_next()?

> > +static bool sg_compare_to_buffer(struct scatterlist *sg,
> > +					int offset, u8 *buffer, size_t len)
> > +{
> > +	unsigned long flags;
> > +	int retval = 0;
> > +	struct sg_mapping_iter miter;
> > +
> > +	local_irq_save(flags);
> 
> hm, the "IRQ disabled if SG_MITER_ATOMIC" requirement of
> sg_miter_next() is weird and unexpected.
> 
> Tejun's 137d3edb48425f8 added the code whch has this strange
> requirement, but it is unexplained.  Wazzup?

Heh, that's long time ago.  Trying to remember... so it was written
before kmap_atomic() is converted to stack based implementation and
had to be supplied with the specific KMAP index.  Atomic mapping
iterator should be useable from irq context too (and there were other
irq handler paths using KM_BIO_SRC_IRQ as the name implies), so it
couldn't allow irq to happen while mapping is in use.  At the time,
having to disable IRQ to use any of KM_*_IRQs was rather self-evident.

I think we now can relax the requirement.  All that's required is not
to sleep or preempted while mapped.  Something like the following is
in order?

--------8<--------
Subject: scatterlist: atomic sg_mapping_iter no longer needs disabling IRQ

SG mapping iterator w/ SG_MITER_ATOMIC set required IRQ disabled
because it originally used KM_BIO_SRC_IRQ to allow use from IRQ
handlers.  kmap_atomic() has long been updated to handle stacking
atomic mapping requests on per-cpu basis and only requires not
sleeping while mapped.

Update sg_mapping_iter such that atomic iterators only require
disabling preemption instead of disabling IRQ.

While at it, convert wte weird @ARG@ notations to @ARG in the comment
of sg_miter_start().

Signed-off-by: Tejun Heo <tj@kernel.org>
Cc: Andrew Morton <akpm@linux-foundation.org>
---
 lib/scatterlist.c |   16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/lib/scatterlist.c b/lib/scatterlist.c
index fadae77..e76d85c 100644
--- a/lib/scatterlist.c
+++ b/lib/scatterlist.c
@@ -404,14 +404,13 @@ EXPORT_SYMBOL(sg_miter_start);
  * @miter: sg mapping iter to proceed
  *
  * Description:
- *   Proceeds @miter@ to the next mapping.  @miter@ should have been
- *   started using sg_miter_start().  On successful return,
- *   @miter@->page, @miter@->addr and @miter@->length point to the
- *   current mapping.
+ *   Proceeds @miter to the next mapping.  @miter should have been started
+ *   using sg_miter_start().  On successful return, @miter->page,
+ *   @miter->addr and @miter->length point to the current mapping.
  *
  * Context:
- *   IRQ disabled if SG_MITER_ATOMIC.  IRQ must stay disabled till
- *   @miter@ is stopped.  May sleep if !SG_MITER_ATOMIC.
+ *   Preemption disabled if SG_MITER_ATOMIC.  Preemption must stay disabled
+ *   till @miter is stopped.  May sleep if !SG_MITER_ATOMIC.
  *
  * Returns:
  *   true if @miter contains the next mapping.  false if end of sg
@@ -465,7 +464,8 @@ EXPORT_SYMBOL(sg_miter_next);
  *   resources (kmap) need to be released during iteration.
  *
  * Context:
- *   IRQ disabled if the SG_MITER_ATOMIC is set.  Don't care otherwise.
+ *   Preemption disabled if the SG_MITER_ATOMIC is set.  Don't care
+ *   otherwise.
  */
 void sg_miter_stop(struct sg_mapping_iter *miter)
 {
@@ -479,7 +479,7 @@ void sg_miter_stop(struct sg_mapping_iter *miter)
 			flush_kernel_dcache_page(miter->page);
 
 		if (miter->__flags & SG_MITER_ATOMIC) {
-			WARN_ON(!irqs_disabled());
+			WARN_ON_ONCE(preemptible());
 			kunmap_atomic(miter->addr);
 		} else
 			kunmap(miter->page);