Fixed MTP to work with TWRP

drivers/memstick/Kconfig

@@ -0,0 +1,26 @@
+#
+# MemoryStick subsystem configuration
+#
+
+menuconfig MEMSTICK
+	tristate "Sony MemoryStick card support"
+	help
+	  Sony MemoryStick is a proprietary storage/extension card protocol.
+
+	  If you want MemoryStick support, you should say Y here and also
+	  to the specific driver for your MemoryStick interface.
+
+if MEMSTICK
+
+config MEMSTICK_DEBUG
+	bool "MemoryStick debugging"
+	help
+	  This is an option for use by developers; most people should
+	  say N here.  This enables MemoryStick core and driver debugging.
+
+
+source "drivers/memstick/core/Kconfig"
+
+source "drivers/memstick/host/Kconfig"
+
+endif # MEMSTICK

drivers/memstick/Makefile

@@ -0,0 +1,9 @@
+#
+# Makefile for the kernel MemoryStick device drivers.
+#
+
+subdir-ccflags-$(CONFIG_MEMSTICK_DEBUG) := -DDEBUG
+
+obj-$(CONFIG_MEMSTICK)		+= core/
+obj-$(CONFIG_MEMSTICK)		+= host/
+

drivers/memstick/core/Kconfig

@@ -0,0 +1,38 @@
+#
+# MemoryStick core configuration
+#
+
+comment "MemoryStick drivers"
+
+config MEMSTICK_UNSAFE_RESUME
+        bool "Allow unsafe resume (DANGEROUS)"
+        help
+          If you say Y here, the MemoryStick layer will assume that all
+          cards stayed in their respective slots during the suspend. The
+          normal behaviour is to remove them at suspend and
+          redetecting them at resume. Breaking this assumption will
+          in most cases result in data corruption.
+
+          This option is usually just for embedded systems which use
+          a MemoryStick card for rootfs. Most people should say N here.
+
+config MSPRO_BLOCK
+	tristate "MemoryStick Pro block device driver"
+	depends on BLOCK
+	help
+	  Say Y here to enable the MemoryStick Pro block device driver
+	  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)

drivers/memstick/core/Makefile

@@ -0,0 +1,7 @@
+#
+# Makefile for the kernel MemoryStick core.
+#
+
+obj-$(CONFIG_MEMSTICK)		+= memstick.o
+obj-$(CONFIG_MS_BLOCK)		+= ms_block.o
+obj-$(CONFIG_MSPRO_BLOCK)	+= mspro_block.o

drivers/memstick/core/memstick.c

@@ -0,0 +1,654 @@
+/*
+ *  Sony MemoryStick support
+ *
+ *  Copyright (C) 2007 Alex Dubov <oakad@yahoo.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.
+ *
+ * Special thanks to Carlos Corbacho for providing various MemoryStick cards
+ * that made this driver possible.
+ *
+ */
+
+#include <linux/memstick.h>
+#include <linux/idr.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+
+#define DRIVER_NAME "memstick"
+
+static unsigned int cmd_retries = 3;
+module_param(cmd_retries, uint, 0644);
+
+static struct workqueue_struct *workqueue;
+static DEFINE_IDR(memstick_host_idr);
+static DEFINE_SPINLOCK(memstick_host_lock);
+
+static int memstick_dev_match(struct memstick_dev *card,
+			      struct memstick_device_id *id)
+{
+	if (id->match_flags & MEMSTICK_MATCH_ALL) {
+		if ((id->type == card->id.type)
+		    && (id->category == card->id.category)
+		    && (id->class == card->id.class))
+			return 1;
+	}
+
+	return 0;
+}
+
+static int memstick_bus_match(struct device *dev, struct device_driver *drv)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						 dev);
+	struct memstick_driver *ms_drv = container_of(drv,
+						      struct memstick_driver,
+						      driver);
+	struct memstick_device_id *ids = ms_drv->id_table;
+
+	if (ids) {
+		while (ids->match_flags) {
+			if (memstick_dev_match(card, ids))
+				return 1;
+			++ids;
+		}
+	}
+	return 0;
+}
+
+static int memstick_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						  dev);
+
+	if (add_uevent_var(env, "MEMSTICK_TYPE=%02X", card->id.type))
+		return -ENOMEM;
+
+	if (add_uevent_var(env, "MEMSTICK_CATEGORY=%02X", card->id.category))
+		return -ENOMEM;
+
+	if (add_uevent_var(env, "MEMSTICK_CLASS=%02X", card->id.class))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int memstick_device_probe(struct device *dev)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						 dev);
+	struct memstick_driver *drv = container_of(dev->driver,
+						   struct memstick_driver,
+						   driver);
+	int rc = -ENODEV;
+
+	if (dev->driver && drv->probe) {
+		rc = drv->probe(card);
+		if (!rc)
+			get_device(dev);
+	}
+	return rc;
+}
+
+static int memstick_device_remove(struct device *dev)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						  dev);
+	struct memstick_driver *drv = container_of(dev->driver,
+						   struct memstick_driver,
+						   driver);
+
+	if (dev->driver && drv->remove) {
+		drv->remove(card);
+		card->dev.driver = NULL;
+	}
+
+	put_device(dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int memstick_device_suspend(struct device *dev, pm_message_t state)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						  dev);
+	struct memstick_driver *drv = container_of(dev->driver,
+						   struct memstick_driver,
+						   driver);
+
+	if (dev->driver && drv->suspend)
+		return drv->suspend(card, state);
+	return 0;
+}
+
+static int memstick_device_resume(struct device *dev)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						  dev);
+	struct memstick_driver *drv = container_of(dev->driver,
+						   struct memstick_driver,
+						   driver);
+
+	if (dev->driver && drv->resume)
+		return drv->resume(card);
+	return 0;
+}
+
+#else
+
+#define memstick_device_suspend NULL
+#define memstick_device_resume NULL
+
+#endif /* CONFIG_PM */
+
+#define MEMSTICK_ATTR(name, format)                                           \
+static ssize_t name##_show(struct device *dev, struct device_attribute *attr, \
+			    char *buf)                                        \
+{                                                                             \
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,    \
+						 dev);                        \
+	return sprintf(buf, format, card->id.name);                           \
+}                                                                             \
+static DEVICE_ATTR_RO(name);
+
+MEMSTICK_ATTR(type, "%02X");
+MEMSTICK_ATTR(category, "%02X");
+MEMSTICK_ATTR(class, "%02X");
+
+static struct attribute *memstick_dev_attrs[] = {
+	&dev_attr_type.attr,
+	&dev_attr_category.attr,
+	&dev_attr_class.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(memstick_dev);
+
+static struct bus_type memstick_bus_type = {
+	.name           = "memstick",
+	.dev_groups	= memstick_dev_groups,
+	.match          = memstick_bus_match,
+	.uevent         = memstick_uevent,
+	.probe          = memstick_device_probe,
+	.remove         = memstick_device_remove,
+	.suspend        = memstick_device_suspend,
+	.resume         = memstick_device_resume
+};
+
+static void memstick_free(struct device *dev)
+{
+	struct memstick_host *host = container_of(dev, struct memstick_host,
+						  dev);
+	kfree(host);
+}
+
+static struct class memstick_host_class = {
+	.name        = "memstick_host",
+	.dev_release = memstick_free
+};
+
+static void memstick_free_card(struct device *dev)
+{
+	struct memstick_dev *card = container_of(dev, struct memstick_dev,
+						 dev);
+	kfree(card);
+}
+
+static int memstick_dummy_check(struct memstick_dev *card)
+{
+	return 0;
+}
+
+/**
+ * memstick_detect_change - schedule media detection on memstick host
+ * @host - host to use
+ */
+void memstick_detect_change(struct memstick_host *host)
+{
+	queue_work(workqueue, &host->media_checker);
+}
+EXPORT_SYMBOL(memstick_detect_change);
+
+/**
+ * memstick_next_req - called by host driver to obtain next request to process
+ * @host - host to use
+ * @mrq - pointer to stick the request to
+ *
+ * Host calls this function from idle state (*mrq == NULL) or after finishing
+ * previous request (*mrq should point to it). If previous request was
+ * unsuccessful, it is retried for predetermined number of times. Return value
+ * of 0 means that new request was assigned to the host.
+ */
+int memstick_next_req(struct memstick_host *host, struct memstick_request **mrq)
+{
+	int rc = -ENXIO;
+
+	if ((*mrq) && (*mrq)->error && host->retries) {
+		(*mrq)->error = rc;
+		host->retries--;
+		return 0;
+	}
+
+	if (host->card && host->card->next_request)
+		rc = host->card->next_request(host->card, mrq);
+
+	if (!rc)
+		host->retries = cmd_retries > 1 ? cmd_retries - 1 : 1;
+	else
+		*mrq = NULL;
+
+	return rc;
+}
+EXPORT_SYMBOL(memstick_next_req);
+
+/**
+ * memstick_new_req - notify the host that some requests are pending
+ * @host - host to use
+ */
+void memstick_new_req(struct memstick_host *host)
+{
+	if (host->card) {
+		host->retries = cmd_retries;
+		reinit_completion(&host->card->mrq_complete);
+		host->request(host);
+	}
+}
+EXPORT_SYMBOL(memstick_new_req);
+
+/**
+ * memstick_init_req_sg - set request fields needed for bulk data transfer
+ * @mrq - request to use
+ * @tpc - memstick Transport Protocol Command
+ * @sg - TPC argument
+ */
+void memstick_init_req_sg(struct memstick_request *mrq, unsigned char tpc,
+			  const struct scatterlist *sg)
+{
+	mrq->tpc = tpc;
+	if (tpc & 8)
+		mrq->data_dir = WRITE;
+	else
+		mrq->data_dir = READ;
+
+	mrq->sg = *sg;
+	mrq->long_data = 1;
+
+	if (tpc == MS_TPC_SET_CMD || tpc == MS_TPC_EX_SET_CMD)
+		mrq->need_card_int = 1;
+	else
+		mrq->need_card_int = 0;
+}
+EXPORT_SYMBOL(memstick_init_req_sg);
+
+/**
+ * memstick_init_req - set request fields needed for short data transfer
+ * @mrq - request to use
+ * @tpc - memstick Transport Protocol Command
+ * @buf - TPC argument buffer
+ * @length - TPC argument size
+ *
+ * The intended use of this function (transfer of data items several bytes
+ * in size) allows us to just copy the value between request structure and
+ * user supplied buffer.
+ */
+void memstick_init_req(struct memstick_request *mrq, unsigned char tpc,
+		       const void *buf, size_t length)
+{
+	mrq->tpc = tpc;
+	if (tpc & 8)
+		mrq->data_dir = WRITE;
+	else
+		mrq->data_dir = READ;
+
+	mrq->data_len = length > sizeof(mrq->data) ? sizeof(mrq->data) : length;
+	if (mrq->data_dir == WRITE)
+		memcpy(mrq->data, buf, mrq->data_len);
+
+	mrq->long_data = 0;
+
+	if (tpc == MS_TPC_SET_CMD || tpc == MS_TPC_EX_SET_CMD)
+		mrq->need_card_int = 1;
+	else
+		mrq->need_card_int = 0;
+}
+EXPORT_SYMBOL(memstick_init_req);
+
+/*
+ * Functions prefixed with "h_" are protocol callbacks. They can be called from
+ * interrupt context. Return value of 0 means that request processing is still
+ * ongoing, while special error value of -EAGAIN means that current request is
+ * finished (and request processor should come back some time later).
+ */
+
+static int h_memstick_read_dev_id(struct memstick_dev *card,
+				  struct memstick_request **mrq)
+{
+	struct ms_id_register id_reg;
+
+	if (!(*mrq)) {
+		memstick_init_req(&card->current_mrq, MS_TPC_READ_REG, NULL,
+				  sizeof(struct ms_id_register));
+		*mrq = &card->current_mrq;
+		return 0;
+	} else {
+		if (!(*mrq)->error) {
+			memcpy(&id_reg, (*mrq)->data, sizeof(id_reg));
+			card->id.match_flags = MEMSTICK_MATCH_ALL;
+			card->id.type = id_reg.type;
+			card->id.category = id_reg.category;
+			card->id.class = id_reg.class;
+			dev_dbg(&card->dev, "if_mode = %02x\n", id_reg.if_mode);
+		}
+		complete(&card->mrq_complete);
+		return -EAGAIN;
+	}
+}
+
+static int h_memstick_set_rw_addr(struct memstick_dev *card,
+				  struct memstick_request **mrq)
+{
+	if (!(*mrq)) {
+		memstick_init_req(&card->current_mrq, MS_TPC_SET_RW_REG_ADRS,
+				  (char *)&card->reg_addr,
+				  sizeof(card->reg_addr));
+		*mrq = &card->current_mrq;
+		return 0;
+	} else {
+		complete(&card->mrq_complete);
+		return -EAGAIN;
+	}
+}
+
+/**
+ * memstick_set_rw_addr - issue SET_RW_REG_ADDR request and wait for it to
+ *                        complete
+ * @card - media device to use
+ */
+int memstick_set_rw_addr(struct memstick_dev *card)
+{
+	card->next_request = h_memstick_set_rw_addr;
+	memstick_new_req(card->host);
+	wait_for_completion(&card->mrq_complete);
+
+	return card->current_mrq.error;
+}
+EXPORT_SYMBOL(memstick_set_rw_addr);
+
+static struct memstick_dev *memstick_alloc_card(struct memstick_host *host)
+{
+	struct memstick_dev *card = kzalloc(sizeof(struct memstick_dev),
+					    GFP_KERNEL);
+	struct memstick_dev *old_card = host->card;
+	struct ms_id_register id_reg;
+
+	if (card) {
+		card->host = host;
+		dev_set_name(&card->dev, "%s", dev_name(&host->dev));
+		card->dev.parent = &host->dev;
+		card->dev.bus = &memstick_bus_type;
+		card->dev.release = memstick_free_card;
+		card->check = memstick_dummy_check;
+
+		card->reg_addr.r_offset = offsetof(struct ms_register, id);
+		card->reg_addr.r_length = sizeof(id_reg);
+		card->reg_addr.w_offset = offsetof(struct ms_register, id);
+		card->reg_addr.w_length = sizeof(id_reg);
+
+		init_completion(&card->mrq_complete);
+
+		host->card = card;
+		if (memstick_set_rw_addr(card))
+			goto err_out;
+
+		card->next_request = h_memstick_read_dev_id;
+		memstick_new_req(host);
+		wait_for_completion(&card->mrq_complete);
+
+		if (card->current_mrq.error)
+			goto err_out;
+	}
+	host->card = old_card;
+	return card;
+err_out:
+	host->card = old_card;
+	kfree(card);
+	return NULL;
+}
+
+static int memstick_power_on(struct memstick_host *host)
+{
+	int rc = host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
+
+	if (!rc)
+		rc = host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+
+	return rc;
+}
+
+static void memstick_check(struct work_struct *work)
+{
+	struct memstick_host *host = container_of(work, struct memstick_host,
+						  media_checker);
+	struct memstick_dev *card;
+
+	dev_dbg(&host->dev, "memstick_check started\n");
+	mutex_lock(&host->lock);
+	if (!host->card) {
+		if (memstick_power_on(host))
+			goto out_power_off;
+	} else if (host->card->stop)
+		host->card->stop(host->card);
+
+	card = memstick_alloc_card(host);
+
+	if (!card) {
+		if (host->card) {
+			device_unregister(&host->card->dev);
+			host->card = NULL;
+		}
+	} else {
+		dev_dbg(&host->dev, "new card %02x, %02x, %02x\n",
+			card->id.type, card->id.category, card->id.class);
+		if (host->card) {
+			if (memstick_set_rw_addr(host->card)
+			    || !memstick_dev_match(host->card, &card->id)
+			    || !(host->card->check(host->card))) {
+				device_unregister(&host->card->dev);
+				host->card = NULL;
+			} else if (host->card->start)
+				host->card->start(host->card);
+		}
+
+		if (!host->card) {
+			host->card = card;
+			if (device_register(&card->dev)) {
+				put_device(&card->dev);
+				kfree(host->card);
+				host->card = NULL;
+			}
+		} else
+			kfree(card);
+	}
+
+out_power_off:
+	if (!host->card)
+		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+
+	mutex_unlock(&host->lock);
+	dev_dbg(&host->dev, "memstick_check finished\n");
+}
+
+/**
+ * memstick_alloc_host - allocate a memstick_host structure
+ * @extra: size of the user private data to allocate
+ * @dev: parent device of the host
+ */
+struct memstick_host *memstick_alloc_host(unsigned int extra,
+					  struct device *dev)
+{
+	struct memstick_host *host;
+
+	host = kzalloc(sizeof(struct memstick_host) + extra, GFP_KERNEL);
+	if (host) {
+		mutex_init(&host->lock);
+		INIT_WORK(&host->media_checker, memstick_check);
+		host->dev.class = &memstick_host_class;
+		host->dev.parent = dev;
+		device_initialize(&host->dev);
+	}
+	return host;
+}
+EXPORT_SYMBOL(memstick_alloc_host);
+
+/**
+ * memstick_add_host - start request processing on memstick host
+ * @host - host to use
+ */
+int memstick_add_host(struct memstick_host *host)
+{
+	int rc;
+
+	idr_preload(GFP_KERNEL);
+	spin_lock(&memstick_host_lock);
+
+	rc = idr_alloc(&memstick_host_idr, host, 0, 0, GFP_NOWAIT);
+	if (rc >= 0)
+		host->id = rc;
+
+	spin_unlock(&memstick_host_lock);
+	idr_preload_end();
+	if (rc < 0)
+		return rc;
+
+	dev_set_name(&host->dev, "memstick%u", host->id);
+
+	rc = device_add(&host->dev);
+	if (rc) {
+		spin_lock(&memstick_host_lock);
+		idr_remove(&memstick_host_idr, host->id);
+		spin_unlock(&memstick_host_lock);
+		return rc;
+	}
+
+	host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+	memstick_detect_change(host);
+	return 0;
+}
+EXPORT_SYMBOL(memstick_add_host);
+
+/**
+ * memstick_remove_host - stop request processing on memstick host
+ * @host - host to use
+ */
+void memstick_remove_host(struct memstick_host *host)
+{
+	flush_workqueue(workqueue);
+	mutex_lock(&host->lock);
+	if (host->card)
+		device_unregister(&host->card->dev);
+	host->card = NULL;
+	host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+	mutex_unlock(&host->lock);
+
+	spin_lock(&memstick_host_lock);
+	idr_remove(&memstick_host_idr, host->id);
+	spin_unlock(&memstick_host_lock);
+	device_del(&host->dev);
+}
+EXPORT_SYMBOL(memstick_remove_host);
+
+/**
+ * memstick_free_host - free memstick host
+ * @host - host to use
+ */
+void memstick_free_host(struct memstick_host *host)
+{
+	mutex_destroy(&host->lock);
+	put_device(&host->dev);
+}
+EXPORT_SYMBOL(memstick_free_host);
+
+/**
+ * memstick_suspend_host - notify bus driver of host suspension
+ * @host - host to use
+ */
+void memstick_suspend_host(struct memstick_host *host)
+{
+	mutex_lock(&host->lock);
+	host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
+	mutex_unlock(&host->lock);
+}
+EXPORT_SYMBOL(memstick_suspend_host);
+
+/**
+ * memstick_resume_host - notify bus driver of host resumption
+ * @host - host to use
+ */
+void memstick_resume_host(struct memstick_host *host)
+{
+	int rc = 0;
+
+	mutex_lock(&host->lock);
+	if (host->card)
+		rc = memstick_power_on(host);
+	mutex_unlock(&host->lock);
+
+	if (!rc)
+		memstick_detect_change(host);
+}
+EXPORT_SYMBOL(memstick_resume_host);
+
+int memstick_register_driver(struct memstick_driver *drv)
+{
+	drv->driver.bus = &memstick_bus_type;
+
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL(memstick_register_driver);
+
+void memstick_unregister_driver(struct memstick_driver *drv)
+{
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL(memstick_unregister_driver);
+
+
+static int __init memstick_init(void)
+{
+	int rc;
+
+	workqueue = create_freezable_workqueue("kmemstick");
+	if (!workqueue)
+		return -ENOMEM;
+
+	rc = bus_register(&memstick_bus_type);
+	if (!rc)
+		rc = class_register(&memstick_host_class);
+
+	if (!rc)
+		return 0;
+
+	bus_unregister(&memstick_bus_type);
+	destroy_workqueue(workqueue);
+
+	return rc;
+}
+
+static void __exit memstick_exit(void)
+{
+	class_unregister(&memstick_host_class);
+	bus_unregister(&memstick_bus_type);
+	destroy_workqueue(workqueue);
+	idr_destroy(&memstick_host_idr);
+}
+
+module_init(memstick_init);
+module_exit(memstick_exit);
+
+MODULE_AUTHOR("Alex Dubov");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Sony MemoryStick core driver");

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_RECEIVE_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

drivers/memstick/host/Kconfig

@@ -0,0 +1,64 @@
+#
+# MemoryStick host controller drivers
+#
+
+comment "MemoryStick Host Controller Drivers"
+
+config MEMSTICK_TIFM_MS
+	tristate "TI Flash Media MemoryStick Interface support "
+	depends on PCI
+	select TIFM_CORE
+	help
+	  Say Y here if you want to be able to access MemoryStick cards with
+	  the Texas Instruments(R) Flash Media card reader, found in many
+	  laptops.
+	  This option 'selects' (turns on, enables) 'TIFM_CORE', but you
+	  probably also need appropriate card reader host adapter, such as
+	  'Misc devices: TI Flash Media PCI74xx/PCI76xx host adapter support
+	  (TIFM_7XX1)'.
+
+          To compile this driver as a module, choose M here: the
+	  module will be called tifm_ms.
+
+config MEMSTICK_JMICRON_38X
+	tristate "JMicron JMB38X MemoryStick interface support"
+	depends on PCI
+
+	help
+	  Say Y here if you want to be able to access MemoryStick cards with
+	  the JMicron(R) JMB38X MemoryStick card reader.
+
+          To compile this driver as a module, choose M here: the
+	  module will be called jmb38x_ms.
+
+config MEMSTICK_R592
+	tristate "Ricoh R5C592 MemoryStick interface support"
+	depends on PCI
+
+	help
+	  Say Y here if you want to be able to access MemoryStick cards with
+	  the Ricoh R5C592 MemoryStick card reader (which is part of 5 in one
+		multifunction reader)
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called r592.
+
+config MEMSTICK_REALTEK_PCI
+	tristate "Realtek PCI-E Memstick Card Interface Driver"
+	depends on MFD_RTSX_PCI
+	help
+	  Say Y here to include driver code to support Memstick card interface
+	  of Realtek PCI-E card reader
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called rtsx_pci_ms.
+
+config MEMSTICK_REALTEK_USB
+	tristate "Realtek USB Memstick Card Interface Driver"
+	depends on MFD_RTSX_USB
+	help
+	  Say Y here to include driver code to support Memstick card interface
+	  of Realtek RTS5129/39 series USB card reader
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called rts5139_ms.

drivers/memstick/host/Makefile

@@ -0,0 +1,9 @@
+#
+# Makefile for MemoryStick host controller drivers
+#
+
+obj-$(CONFIG_MEMSTICK_TIFM_MS)		+= tifm_ms.o
+obj-$(CONFIG_MEMSTICK_JMICRON_38X)	+= jmb38x_ms.o
+obj-$(CONFIG_MEMSTICK_R592)		+= r592.o
+obj-$(CONFIG_MEMSTICK_REALTEK_PCI)	+= rtsx_pci_ms.o
+obj-$(CONFIG_MEMSTICK_REALTEK_USB)	+= rtsx_usb_ms.o

drivers/memstick/host/r592.c

@@ -0,0 +1,898 @@
+/*
+ * Copyright (C) 2010 - Maxim Levitsky
+ * driver for Ricoh memstick readers
+ *
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/freezer.h>
+#include <linux/jiffies.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/highmem.h>
+#include <asm/byteorder.h>
+#include <linux/swab.h>
+#include "r592.h"
+
+static bool r592_enable_dma = 1;
+static int debug;
+
+static const char *tpc_names[] = {
+	"MS_TPC_READ_MG_STATUS",
+	"MS_TPC_READ_LONG_DATA",
+	"MS_TPC_READ_SHORT_DATA",
+	"MS_TPC_READ_REG",
+	"MS_TPC_READ_QUAD_DATA",
+	"INVALID",
+	"MS_TPC_GET_INT",
+	"MS_TPC_SET_RW_REG_ADRS",
+	"MS_TPC_EX_SET_CMD",
+	"MS_TPC_WRITE_QUAD_DATA",
+	"MS_TPC_WRITE_REG",
+	"MS_TPC_WRITE_SHORT_DATA",
+	"MS_TPC_WRITE_LONG_DATA",
+	"MS_TPC_SET_CMD",
+};
+
+/**
+ * memstick_debug_get_tpc_name - debug helper that returns string for
+ * a TPC number
+ */
+const char *memstick_debug_get_tpc_name(int tpc)
+{
+	return tpc_names[tpc-1];
+}
+EXPORT_SYMBOL(memstick_debug_get_tpc_name);
+
+
+/* Read a register*/
+static inline u32 r592_read_reg(struct r592_device *dev, int address)
+{
+	u32 value = readl(dev->mmio + address);
+	dbg_reg("reg #%02d == 0x%08x", address, value);
+	return value;
+}
+
+/* Write a register */
+static inline void r592_write_reg(struct r592_device *dev,
+							int address, u32 value)
+{
+	dbg_reg("reg #%02d <- 0x%08x", address, value);
+	writel(value, dev->mmio + address);
+}
+
+/* Reads a big endian DWORD register */
+static inline u32 r592_read_reg_raw_be(struct r592_device *dev, int address)
+{
+	u32 value = __raw_readl(dev->mmio + address);
+	dbg_reg("reg #%02d == 0x%08x", address, value);
+	return be32_to_cpu(value);
+}
+
+/* Writes a big endian DWORD register */
+static inline void r592_write_reg_raw_be(struct r592_device *dev,
+							int address, u32 value)
+{
+	dbg_reg("reg #%02d <- 0x%08x", address, value);
+	__raw_writel(cpu_to_be32(value), dev->mmio + address);
+}
+
+/* Set specific bits in a register (little endian) */
+static inline void r592_set_reg_mask(struct r592_device *dev,
+							int address, u32 mask)
+{
+	u32 reg = readl(dev->mmio + address);
+	dbg_reg("reg #%02d |= 0x%08x (old =0x%08x)", address, mask, reg);
+	writel(reg | mask , dev->mmio + address);
+}
+
+/* Clear specific bits in a register (little endian) */
+static inline void r592_clear_reg_mask(struct r592_device *dev,
+						int address, u32 mask)
+{
+	u32 reg = readl(dev->mmio + address);
+	dbg_reg("reg #%02d &= 0x%08x (old = 0x%08x, mask = 0x%08x)",
+						address, ~mask, reg, mask);
+	writel(reg & ~mask, dev->mmio + address);
+}
+
+
+/* Wait for status bits while checking for errors */
+static int r592_wait_status(struct r592_device *dev, u32 mask, u32 wanted_mask)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+	u32 reg = r592_read_reg(dev, R592_STATUS);
+
+	if ((reg & mask) == wanted_mask)
+		return 0;
+
+	while (time_before(jiffies, timeout)) {
+
+		reg = r592_read_reg(dev, R592_STATUS);
+
+		if ((reg & mask) == wanted_mask)
+			return 0;
+
+		if (reg & (R592_STATUS_SEND_ERR | R592_STATUS_RECV_ERR))
+			return -EIO;
+
+		cpu_relax();
+	}
+	return -ETIME;
+}
+
+
+/* Enable/disable device */
+static int r592_enable_device(struct r592_device *dev, bool enable)
+{
+	dbg("%sabling the device", enable ? "en" : "dis");
+
+	if (enable) {
+
+		/* Power up the card */
+		r592_write_reg(dev, R592_POWER, R592_POWER_0 | R592_POWER_1);
+
+		/* Perform a reset */
+		r592_set_reg_mask(dev, R592_IO, R592_IO_RESET);
+
+		msleep(100);
+	} else
+		/* Power down the card */
+		r592_write_reg(dev, R592_POWER, 0);
+
+	return 0;
+}
+
+/* Set serial/parallel mode */
+static int r592_set_mode(struct r592_device *dev, bool parallel_mode)
+{
+	if (!parallel_mode) {
+		dbg("switching to serial mode");
+
+		/* Set serial mode */
+		r592_write_reg(dev, R592_IO_MODE, R592_IO_MODE_SERIAL);
+
+		r592_clear_reg_mask(dev, R592_POWER, R592_POWER_20);
+
+	} else {
+		dbg("switching to parallel mode");
+
+		/* This setting should be set _before_ switch TPC */
+		r592_set_reg_mask(dev, R592_POWER, R592_POWER_20);
+
+		r592_clear_reg_mask(dev, R592_IO,
+			R592_IO_SERIAL1 | R592_IO_SERIAL2);
+
+		/* Set the parallel mode now */
+		r592_write_reg(dev, R592_IO_MODE, R592_IO_MODE_PARALLEL);
+	}
+
+	dev->parallel_mode = parallel_mode;
+	return 0;
+}
+
+/* Perform a controller reset without powering down the card */
+static void r592_host_reset(struct r592_device *dev)
+{
+	r592_set_reg_mask(dev, R592_IO, R592_IO_RESET);
+	msleep(100);
+	r592_set_mode(dev, dev->parallel_mode);
+}
+
+#ifdef CONFIG_PM_SLEEP
+/* Disable all hardware interrupts */
+static void r592_clear_interrupts(struct r592_device *dev)
+{
+	/* Disable & ACK all interrupts */
+	r592_clear_reg_mask(dev, R592_REG_MSC, IRQ_ALL_ACK_MASK);
+	r592_clear_reg_mask(dev, R592_REG_MSC, IRQ_ALL_EN_MASK);
+}
+#endif
+
+/* Tests if there is an CRC error */
+static int r592_test_io_error(struct r592_device *dev)
+{
+	if (!(r592_read_reg(dev, R592_STATUS) &
+		(R592_STATUS_SEND_ERR | R592_STATUS_RECV_ERR)))
+		return 0;
+
+	return -EIO;
+}
+
+/* Ensure that FIFO is ready for use */
+static int r592_test_fifo_empty(struct r592_device *dev)
+{
+	if (r592_read_reg(dev, R592_REG_MSC) & R592_REG_MSC_FIFO_EMPTY)
+		return 0;
+
+	dbg("FIFO not ready, trying to reset the device");
+	r592_host_reset(dev);
+
+	if (r592_read_reg(dev, R592_REG_MSC) & R592_REG_MSC_FIFO_EMPTY)
+		return 0;
+
+	message("FIFO still not ready, giving up");
+	return -EIO;
+}
+
+/* Activates the DMA transfer from to FIFO */
+static void r592_start_dma(struct r592_device *dev, bool is_write)
+{
+	unsigned long flags;
+	u32 reg;
+	spin_lock_irqsave(&dev->irq_lock, flags);
+
+	/* Ack interrupts (just in case) + enable them */
+	r592_clear_reg_mask(dev, R592_REG_MSC, DMA_IRQ_ACK_MASK);
+	r592_set_reg_mask(dev, R592_REG_MSC, DMA_IRQ_EN_MASK);
+
+	/* Set DMA address */
+	r592_write_reg(dev, R592_FIFO_DMA, sg_dma_address(&dev->req->sg));
+
+	/* Enable the DMA */
+	reg = r592_read_reg(dev, R592_FIFO_DMA_SETTINGS);
+	reg |= R592_FIFO_DMA_SETTINGS_EN;
+
+	if (!is_write)
+		reg |= R592_FIFO_DMA_SETTINGS_DIR;
+	else
+		reg &= ~R592_FIFO_DMA_SETTINGS_DIR;
+	r592_write_reg(dev, R592_FIFO_DMA_SETTINGS, reg);
+
+	spin_unlock_irqrestore(&dev->irq_lock, flags);
+}
+
+/* Cleanups DMA related settings */
+static void r592_stop_dma(struct r592_device *dev, int error)
+{
+	r592_clear_reg_mask(dev, R592_FIFO_DMA_SETTINGS,
+		R592_FIFO_DMA_SETTINGS_EN);
+
+	/* This is only a precation */
+	r592_write_reg(dev, R592_FIFO_DMA,
+			dev->dummy_dma_page_physical_address);
+
+	r592_clear_reg_mask(dev, R592_REG_MSC, DMA_IRQ_EN_MASK);
+	r592_clear_reg_mask(dev, R592_REG_MSC, DMA_IRQ_ACK_MASK);
+	dev->dma_error = error;
+}
+
+/* Test if hardware supports DMA */
+static void r592_check_dma(struct r592_device *dev)
+{
+	dev->dma_capable = r592_enable_dma &&
+		(r592_read_reg(dev, R592_FIFO_DMA_SETTINGS) &
+			R592_FIFO_DMA_SETTINGS_CAP);
+}
+
+/* Transfers fifo contents in/out using DMA */
+static int r592_transfer_fifo_dma(struct r592_device *dev)
+{
+	int len, sg_count;
+	bool is_write;
+
+	if (!dev->dma_capable || !dev->req->long_data)
+		return -EINVAL;
+
+	len = dev->req->sg.length;
+	is_write = dev->req->data_dir == WRITE;
+
+	if (len != R592_LFIFO_SIZE)
+		return -EINVAL;
+
+	dbg_verbose("doing dma transfer");
+
+	dev->dma_error = 0;
+	reinit_completion(&dev->dma_done);
+
+	/* TODO: hidden assumption about nenth beeing always 1 */
+	sg_count = dma_map_sg(&dev->pci_dev->dev, &dev->req->sg, 1, is_write ?
+		PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
+
+	if (sg_count != 1 ||
+			(sg_dma_len(&dev->req->sg) < dev->req->sg.length)) {
+		message("problem in dma_map_sg");
+		return -EIO;
+	}
+
+	r592_start_dma(dev, is_write);
+
+	/* Wait for DMA completion */
+	if (!wait_for_completion_timeout(
+			&dev->dma_done, msecs_to_jiffies(1000))) {
+		message("DMA timeout");
+		r592_stop_dma(dev, -ETIMEDOUT);
+	}
+
+	dma_unmap_sg(&dev->pci_dev->dev, &dev->req->sg, 1, is_write ?
+		PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
+
+
+	return dev->dma_error;
+}
+
+/*
+ * Writes the FIFO in 4 byte chunks.
+ * If length isn't 4 byte aligned, rest of the data if put to a fifo
+ * to be written later
+ * Use r592_flush_fifo_write to flush that fifo when writing for the
+ * last time
+ */
+static void r592_write_fifo_pio(struct r592_device *dev,
+					unsigned char *buffer, int len)
+{
+	/* flush spill from former write */
+	if (!kfifo_is_empty(&dev->pio_fifo)) {
+
+		u8 tmp[4] = {0};
+		int copy_len = kfifo_in(&dev->pio_fifo, buffer, len);
+
+		if (!kfifo_is_full(&dev->pio_fifo))
+			return;
+		len -= copy_len;
+		buffer += copy_len;
+
+		copy_len = kfifo_out(&dev->pio_fifo, tmp, 4);
+		WARN_ON(copy_len != 4);
+		r592_write_reg_raw_be(dev, R592_FIFO_PIO, *(u32 *)tmp);
+	}
+
+	WARN_ON(!kfifo_is_empty(&dev->pio_fifo));
+
+	/* write full dwords */
+	while (len >= 4) {
+		r592_write_reg_raw_be(dev, R592_FIFO_PIO, *(u32 *)buffer);
+		buffer += 4;
+		len -= 4;
+	}
+
+	/* put remaining bytes to the spill */
+	if (len)
+		kfifo_in(&dev->pio_fifo, buffer, len);
+}
+
+/* Flushes the temporary FIFO used to make aligned DWORD writes */
+static void r592_flush_fifo_write(struct r592_device *dev)
+{
+	u8 buffer[4] = { 0 };
+	int len;
+
+	if (kfifo_is_empty(&dev->pio_fifo))
+		return;
+
+	len = kfifo_out(&dev->pio_fifo, buffer, 4);
+	r592_write_reg_raw_be(dev, R592_FIFO_PIO, *(u32 *)buffer);
+}
+
+/*
+ * Read a fifo in 4 bytes chunks.
+ * If input doesn't fit the buffer, it places bytes of last dword in spill
+ * buffer, so that they don't get lost on last read, just throw these away.
+ */
+static void r592_read_fifo_pio(struct r592_device *dev,
+						unsigned char *buffer, int len)
+{
+	u8 tmp[4];
+
+	/* Read from last spill */
+	if (!kfifo_is_empty(&dev->pio_fifo)) {
+		int bytes_copied =
+			kfifo_out(&dev->pio_fifo, buffer, min(4, len));
+		buffer += bytes_copied;
+		len -= bytes_copied;
+
+		if (!kfifo_is_empty(&dev->pio_fifo))
+			return;
+	}
+
+	/* Reads dwords from FIFO */
+	while (len >= 4) {
+		*(u32 *)buffer = r592_read_reg_raw_be(dev, R592_FIFO_PIO);
+		buffer += 4;
+		len -= 4;
+	}
+
+	if (len) {
+		*(u32 *)tmp = r592_read_reg_raw_be(dev, R592_FIFO_PIO);
+		kfifo_in(&dev->pio_fifo, tmp, 4);
+		len -= kfifo_out(&dev->pio_fifo, buffer, len);
+	}
+
+	WARN_ON(len);
+	return;
+}
+
+/* Transfers actual data using PIO. */
+static int r592_transfer_fifo_pio(struct r592_device *dev)
+{
+	unsigned long flags;
+
+	bool is_write = dev->req->tpc >= MS_TPC_SET_RW_REG_ADRS;
+	struct sg_mapping_iter miter;
+
+	kfifo_reset(&dev->pio_fifo);
+
+	if (!dev->req->long_data) {
+		if (is_write) {
+			r592_write_fifo_pio(dev, dev->req->data,
+							dev->req->data_len);
+			r592_flush_fifo_write(dev);
+		} else
+			r592_read_fifo_pio(dev, dev->req->data,
+							dev->req->data_len);
+		return 0;
+	}
+
+	local_irq_save(flags);
+	sg_miter_start(&miter, &dev->req->sg, 1, SG_MITER_ATOMIC |
+		(is_write ? SG_MITER_FROM_SG : SG_MITER_TO_SG));
+
+	/* Do the transfer fifo<->memory*/
+	while (sg_miter_next(&miter))
+		if (is_write)
+			r592_write_fifo_pio(dev, miter.addr, miter.length);
+		else
+			r592_read_fifo_pio(dev, miter.addr, miter.length);
+
+
+	/* Write last few non aligned bytes*/
+	if (is_write)
+		r592_flush_fifo_write(dev);
+
+	sg_miter_stop(&miter);
+	local_irq_restore(flags);
+	return 0;
+}
+
+/* Executes one TPC (data is read/written from small or large fifo) */
+static void r592_execute_tpc(struct r592_device *dev)
+{
+	bool is_write;
+	int len, error;
+	u32 status, reg;
+
+	if (!dev->req) {
+		message("BUG: tpc execution without request!");
+		return;
+	}
+
+	is_write = dev->req->tpc >= MS_TPC_SET_RW_REG_ADRS;
+	len = dev->req->long_data ?
+		dev->req->sg.length : dev->req->data_len;
+
+	/* Ensure that FIFO can hold the input data */
+	if (len > R592_LFIFO_SIZE) {
+		message("IO: hardware doesn't support TPCs longer that 512");
+		error = -ENOSYS;
+		goto out;
+	}
+
+	if (!(r592_read_reg(dev, R592_REG_MSC) & R592_REG_MSC_PRSNT)) {
+		dbg("IO: refusing to send TPC because card is absent");
+		error = -ENODEV;
+		goto out;
+	}
+
+	dbg("IO: executing %s LEN=%d",
+			memstick_debug_get_tpc_name(dev->req->tpc), len);
+
+	/* Set IO direction */
+	if (is_write)
+		r592_set_reg_mask(dev, R592_IO, R592_IO_DIRECTION);
+	else
+		r592_clear_reg_mask(dev, R592_IO, R592_IO_DIRECTION);
+
+
+	error = r592_test_fifo_empty(dev);
+	if (error)
+		goto out;
+
+	/* Transfer write data */
+	if (is_write) {
+		error = r592_transfer_fifo_dma(dev);
+		if (error == -EINVAL)
+			error = r592_transfer_fifo_pio(dev);
+	}
+
+	if (error)
+		goto out;
+
+	/* Trigger the TPC */
+	reg = (len << R592_TPC_EXEC_LEN_SHIFT) |
+		(dev->req->tpc << R592_TPC_EXEC_TPC_SHIFT) |
+			R592_TPC_EXEC_BIG_FIFO;
+
+	r592_write_reg(dev, R592_TPC_EXEC, reg);
+
+	/* Wait for TPC completion */
+	status = R592_STATUS_RDY;
+	if (dev->req->need_card_int)
+		status |= R592_STATUS_CED;
+
+	error = r592_wait_status(dev, status, status);
+	if (error) {
+		message("card didn't respond");
+		goto out;
+	}
+
+	/* Test IO errors */
+	error = r592_test_io_error(dev);
+	if (error) {
+		dbg("IO error");
+		goto out;
+	}
+
+	/* Read data from FIFO */
+	if (!is_write) {
+		error = r592_transfer_fifo_dma(dev);
+		if (error == -EINVAL)
+			error = r592_transfer_fifo_pio(dev);
+	}
+
+	/* read INT reg. This can be shortened with shifts, but that way
+		its more readable */
+	if (dev->parallel_mode && dev->req->need_card_int) {
+
+		dev->req->int_reg = 0;
+		status = r592_read_reg(dev, R592_STATUS);
+
+		if (status & R592_STATUS_P_CMDNACK)
+			dev->req->int_reg |= MEMSTICK_INT_CMDNAK;
+		if (status & R592_STATUS_P_BREQ)
+			dev->req->int_reg |= MEMSTICK_INT_BREQ;
+		if (status & R592_STATUS_P_INTERR)
+			dev->req->int_reg |= MEMSTICK_INT_ERR;
+		if (status & R592_STATUS_P_CED)
+			dev->req->int_reg |= MEMSTICK_INT_CED;
+	}
+
+	if (error)
+		dbg("FIFO read error");
+out:
+	dev->req->error = error;
+	r592_clear_reg_mask(dev, R592_REG_MSC, R592_REG_MSC_LED);
+	return;
+}
+
+/* Main request processing thread */
+static int r592_process_thread(void *data)
+{
+	int error;
+	struct r592_device *dev = (struct r592_device *)data;
+	unsigned long flags;
+
+	while (!kthread_should_stop()) {
+		spin_lock_irqsave(&dev->io_thread_lock, flags);
+		set_current_state(TASK_INTERRUPTIBLE);
+		error = memstick_next_req(dev->host, &dev->req);
+		spin_unlock_irqrestore(&dev->io_thread_lock, flags);
+
+		if (error) {
+			if (error == -ENXIO || error == -EAGAIN) {
+				dbg_verbose("IO: done IO, sleeping");
+			} else {
+				dbg("IO: unknown error from "
+					"memstick_next_req %d", error);
+			}
+
+			if (kthread_should_stop())
+				set_current_state(TASK_RUNNING);
+
+			schedule();
+		} else {
+			set_current_state(TASK_RUNNING);
+			r592_execute_tpc(dev);
+		}
+	}
+	return 0;
+}
+
+/* Reprogram chip to detect change in card state */
+/* eg, if card is detected, arm it to detect removal, and vice versa */
+static void r592_update_card_detect(struct r592_device *dev)
+{
+	u32 reg = r592_read_reg(dev, R592_REG_MSC);
+	bool card_detected = reg & R592_REG_MSC_PRSNT;
+
+	dbg("update card detect. card state: %s", card_detected ?
+		"present" : "absent");
+
+	reg &= ~((R592_REG_MSC_IRQ_REMOVE | R592_REG_MSC_IRQ_INSERT) << 16);
+
+	if (card_detected)
+		reg |= (R592_REG_MSC_IRQ_REMOVE << 16);
+	else
+		reg |= (R592_REG_MSC_IRQ_INSERT << 16);
+
+	r592_write_reg(dev, R592_REG_MSC, reg);
+}
+
+/* Timer routine that fires 1 second after last card detection event, */
+static void r592_detect_timer(long unsigned int data)
+{
+	struct r592_device *dev = (struct r592_device *)data;
+	r592_update_card_detect(dev);
+	memstick_detect_change(dev->host);
+}
+
+/* Interrupt handler */
+static irqreturn_t r592_irq(int irq, void *data)
+{
+	struct r592_device *dev = (struct r592_device *)data;
+	irqreturn_t ret = IRQ_NONE;
+	u32 reg;
+	u16 irq_enable, irq_status;
+	unsigned long flags;
+	int error;
+
+	spin_lock_irqsave(&dev->irq_lock, flags);
+
+	reg = r592_read_reg(dev, R592_REG_MSC);
+	irq_enable = reg >> 16;
+	irq_status = reg & 0xFFFF;
+
+	/* Ack the interrupts */
+	reg &= ~irq_status;
+	r592_write_reg(dev, R592_REG_MSC, reg);
+
+	/* Get the IRQ status minus bits that aren't enabled */
+	irq_status &= (irq_enable);
+
+	/* Due to limitation of memstick core, we don't look at bits that
+		indicate that card was removed/inserted and/or present */
+	if (irq_status & (R592_REG_MSC_IRQ_INSERT | R592_REG_MSC_IRQ_REMOVE)) {
+
+		bool card_was_added = irq_status & R592_REG_MSC_IRQ_INSERT;
+		ret = IRQ_HANDLED;
+
+		message("IRQ: card %s", card_was_added ? "added" : "removed");
+
+		mod_timer(&dev->detect_timer,
+			jiffies + msecs_to_jiffies(card_was_added ? 500 : 50));
+	}
+
+	if (irq_status &
+		(R592_REG_MSC_FIFO_DMA_DONE | R592_REG_MSC_FIFO_DMA_ERR)) {
+		ret = IRQ_HANDLED;
+
+		if (irq_status & R592_REG_MSC_FIFO_DMA_ERR) {
+			message("IRQ: DMA error");
+			error = -EIO;
+		} else {
+			dbg_verbose("IRQ: dma done");
+			error = 0;
+		}
+
+		r592_stop_dma(dev, error);
+		complete(&dev->dma_done);
+	}
+
+	spin_unlock_irqrestore(&dev->irq_lock, flags);
+	return ret;
+}
+
+/* External inteface: set settings */
+static int r592_set_param(struct memstick_host *host,
+			enum memstick_param param, int value)
+{
+	struct r592_device *dev = memstick_priv(host);
+
+	switch (param) {
+	case MEMSTICK_POWER:
+		switch (value) {
+		case MEMSTICK_POWER_ON:
+			return r592_enable_device(dev, true);
+		case MEMSTICK_POWER_OFF:
+			return r592_enable_device(dev, false);
+		default:
+			return -EINVAL;
+		}
+	case MEMSTICK_INTERFACE:
+		switch (value) {
+		case MEMSTICK_SERIAL:
+			return r592_set_mode(dev, 0);
+		case MEMSTICK_PAR4:
+			return r592_set_mode(dev, 1);
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+/* External interface: submit requests */
+static void r592_submit_req(struct memstick_host *host)
+{
+	struct r592_device *dev = memstick_priv(host);
+	unsigned long flags;
+
+	if (dev->req)
+		return;
+
+	spin_lock_irqsave(&dev->io_thread_lock, flags);
+	if (wake_up_process(dev->io_thread))
+		dbg_verbose("IO thread woken to process requests");
+	spin_unlock_irqrestore(&dev->io_thread_lock, flags);
+}
+
+static const struct pci_device_id r592_pci_id_tbl[] = {
+
+	{ PCI_VDEVICE(RICOH, 0x0592), },
+	{ },
+};
+
+/* Main entry */
+static int r592_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	int error = -ENOMEM;
+	struct memstick_host *host;
+	struct r592_device *dev;
+
+	/* Allocate memory */
+	host = memstick_alloc_host(sizeof(struct r592_device), &pdev->dev);
+	if (!host)
+		goto error1;
+
+	dev = memstick_priv(host);
+	dev->host = host;
+	dev->pci_dev = pdev;
+	pci_set_drvdata(pdev, dev);
+
+	/* pci initialization */
+	error = pci_enable_device(pdev);
+	if (error)
+		goto error2;
+
+	pci_set_master(pdev);
+	error = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	if (error)
+		goto error3;
+
+	error = pci_request_regions(pdev, DRV_NAME);
+	if (error)
+		goto error3;
+
+	dev->mmio = pci_ioremap_bar(pdev, 0);
+	if (!dev->mmio)
+		goto error4;
+
+	dev->irq = pdev->irq;
+	spin_lock_init(&dev->irq_lock);
+	spin_lock_init(&dev->io_thread_lock);
+	init_completion(&dev->dma_done);
+	INIT_KFIFO(dev->pio_fifo);
+	setup_timer(&dev->detect_timer,
+		r592_detect_timer, (long unsigned int)dev);
+
+	/* Host initialization */
+	host->caps = MEMSTICK_CAP_PAR4;
+	host->request = r592_submit_req;
+	host->set_param = r592_set_param;
+	r592_check_dma(dev);
+
+	dev->io_thread = kthread_run(r592_process_thread, dev, "r592_io");
+	if (IS_ERR(dev->io_thread)) {
+		error = PTR_ERR(dev->io_thread);
+		goto error5;
+	}
+
+	/* This is just a precation, so don't fail */
+	dev->dummy_dma_page = pci_alloc_consistent(pdev, PAGE_SIZE,
+		&dev->dummy_dma_page_physical_address);
+	r592_stop_dma(dev , 0);
+
+	if (request_irq(dev->irq, &r592_irq, IRQF_SHARED,
+			  DRV_NAME, dev))
+		goto error6;
+
+	r592_update_card_detect(dev);
+	if (memstick_add_host(host))
+		goto error7;
+
+	message("driver successfully loaded");
+	return 0;
+error7:
+	free_irq(dev->irq, dev);
+error6:
+	if (dev->dummy_dma_page)
+		pci_free_consistent(pdev, PAGE_SIZE, dev->dummy_dma_page,
+			dev->dummy_dma_page_physical_address);
+
+	kthread_stop(dev->io_thread);
+error5:
+	iounmap(dev->mmio);
+error4:
+	pci_release_regions(pdev);
+error3:
+	pci_disable_device(pdev);
+error2:
+	memstick_free_host(host);
+error1:
+	return error;
+}
+
+static void r592_remove(struct pci_dev *pdev)
+{
+	int error = 0;
+	struct r592_device *dev = pci_get_drvdata(pdev);
+
+	/* Stop the processing thread.
+	That ensures that we won't take any more requests */
+	kthread_stop(dev->io_thread);
+
+	r592_enable_device(dev, false);
+
+	while (!error && dev->req) {
+		dev->req->error = -ETIME;
+		error = memstick_next_req(dev->host, &dev->req);
+	}
+	memstick_remove_host(dev->host);
+
+	free_irq(dev->irq, dev);
+	iounmap(dev->mmio);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	memstick_free_host(dev->host);
+
+	if (dev->dummy_dma_page)
+		pci_free_consistent(pdev, PAGE_SIZE, dev->dummy_dma_page,
+			dev->dummy_dma_page_physical_address);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int r592_suspend(struct device *core_dev)
+{
+	struct pci_dev *pdev = to_pci_dev(core_dev);
+	struct r592_device *dev = pci_get_drvdata(pdev);
+
+	r592_clear_interrupts(dev);
+	memstick_suspend_host(dev->host);
+	del_timer_sync(&dev->detect_timer);
+	return 0;
+}
+
+static int r592_resume(struct device *core_dev)
+{
+	struct pci_dev *pdev = to_pci_dev(core_dev);
+	struct r592_device *dev = pci_get_drvdata(pdev);
+
+	r592_clear_interrupts(dev);
+	r592_enable_device(dev, false);
+	memstick_resume_host(dev->host);
+	r592_update_card_detect(dev);
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(r592_pm_ops, r592_suspend, r592_resume);
+
+MODULE_DEVICE_TABLE(pci, r592_pci_id_tbl);
+
+static struct pci_driver r852_pci_driver = {
+	.name		= DRV_NAME,
+	.id_table	= r592_pci_id_tbl,
+	.probe		= r592_probe,
+	.remove		= r592_remove,
+	.driver.pm	= &r592_pm_ops,
+};
+
+module_pci_driver(r852_pci_driver);
+
+module_param_named(enable_dma, r592_enable_dma, bool, S_IRUGO);
+MODULE_PARM_DESC(enable_dma, "Enable usage of the DMA (default)");
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level (0-3)");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Maxim Levitsky <maximlevitsky@gmail.com>");
+MODULE_DESCRIPTION("Ricoh R5C592 Memstick/Memstick PRO card reader driver");

drivers/memstick/host/r592.h

@@ -0,0 +1,175 @@
+/*
+ * Copyright (C) 2010 - Maxim Levitsky
+ * driver for Ricoh memstick readers
+ *
+ * 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.
+ */
+
+#ifndef R592_H
+
+#include <linux/memstick.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/kfifo.h>
+#include <linux/ctype.h>
+
+/* write to this reg (number,len) triggers TPC execution */
+#define R592_TPC_EXEC			0x00
+#define R592_TPC_EXEC_LEN_SHIFT		16		/* Bits 16..25 are TPC len */
+#define R592_TPC_EXEC_BIG_FIFO		(1 << 26)	/* If bit 26 is set, large fifo is used (reg 48) */
+#define R592_TPC_EXEC_TPC_SHIFT		28		/* Bits 28..31 are the TPC number */
+
+
+/* Window for small TPC fifo (big endian)*/
+/* reads and writes always are done in  8 byte chunks */
+/* Not used in driver, because large fifo does better job */
+#define R592_SFIFO			0x08
+
+
+/* Status register (ms int, small fifo, IO)*/
+#define R592_STATUS			0x10
+							/* Parallel INT bits */
+#define R592_STATUS_P_CMDNACK		(1 << 16)	/* INT reg: NACK (parallel mode) */
+#define R592_STATUS_P_BREQ		(1 << 17)	/* INT reg: card ready (parallel mode)*/
+#define R592_STATUS_P_INTERR		(1 << 18)	/* INT reg: int error (parallel mode)*/
+#define R592_STATUS_P_CED		(1 << 19)	/* INT reg: command done (parallel mode) */
+
+							/* Fifo status */
+#define R592_STATUS_SFIFO_FULL		(1 << 20)	/* Small Fifo almost full (last chunk is written) */
+#define R592_STATUS_SFIFO_EMPTY		(1 << 21)	/* Small Fifo empty */
+
+							/* Error detection via CRC */
+#define R592_STATUS_SEND_ERR		(1 << 24)	/* Send failed */
+#define R592_STATUS_RECV_ERR		(1 << 25)	/* Receive failed */
+
+							/* Card state */
+#define R592_STATUS_RDY			(1 << 28)	/* RDY signal received */
+#define R592_STATUS_CED			(1 << 29)	/* INT: Command done (serial mode)*/
+#define R592_STATUS_SFIFO_INPUT		(1 << 30)	/* Small fifo received data*/
+
+#define R592_SFIFO_SIZE			32		/* total size of small fifo is 32 bytes */
+#define R592_SFIFO_PACKET		8		/* packet size of small fifo */
+
+/* IO control */
+#define R592_IO				0x18
+#define	R592_IO_16			(1 << 16)	/* Set by default, can be cleared */
+#define	R592_IO_18			(1 << 18)	/* Set by default, can be cleared */
+#define	R592_IO_SERIAL1			(1 << 20)	/* Set by default, can be cleared, (cleared on parallel) */
+#define	R592_IO_22			(1 << 22)	/* Set by default, can be cleared */
+#define R592_IO_DIRECTION		(1 << 24)	/* TPC direction (1 write 0 read) */
+#define	R592_IO_26			(1 << 26)	/* Set by default, can be cleared */
+#define	R592_IO_SERIAL2			(1 << 30)	/* Set by default, can be cleared (cleared on parallel), serial doesn't work if unset */
+#define R592_IO_RESET			(1 << 31)	/* Reset, sets defaults*/
+
+
+/* Turns hardware on/off */
+#define R592_POWER			0x20		/* bits 0-7 writeable */
+#define R592_POWER_0			(1 << 0)	/* set on start, cleared on stop - must be set*/
+#define R592_POWER_1			(1 << 1)	/* set on start, cleared on stop - must be set*/
+#define R592_POWER_3			(1 << 3)	/* must be clear */
+#define R592_POWER_20			(1 << 5)	/* set before switch to parallel */
+
+/* IO mode*/
+#define R592_IO_MODE			0x24
+#define R592_IO_MODE_SERIAL		1
+#define R592_IO_MODE_PARALLEL		3
+
+
+/* IRQ,card detection,large fifo (first word irq status, second enable) */
+/* IRQs are ACKed by clearing the bits */
+#define R592_REG_MSC			0x28
+#define R592_REG_MSC_PRSNT		(1 << 1)	/* card present (only status)*/
+#define R592_REG_MSC_IRQ_INSERT		(1 << 8)	/* detect insert / card insered */
+#define R592_REG_MSC_IRQ_REMOVE		(1 << 9)	/* detect removal / card removed */
+#define R592_REG_MSC_FIFO_EMPTY		(1 << 10)	/* fifo is empty */
+#define R592_REG_MSC_FIFO_DMA_DONE	(1 << 11)	/* dma enable / dma done */
+
+#define R592_REG_MSC_FIFO_USER_ORN	(1 << 12)	/* set if software reads empty fifo (if R592_REG_MSC_FIFO_EMPTY is set) */
+#define R592_REG_MSC_FIFO_MISMATH	(1 << 13)	/* set if amount of data in fifo doesn't match amount in TPC */
+#define R592_REG_MSC_FIFO_DMA_ERR	(1 << 14)	/* IO failure */
+#define R592_REG_MSC_LED		(1 << 15)	/* clear to turn led off (only status)*/
+
+#define DMA_IRQ_ACK_MASK \
+	(R592_REG_MSC_FIFO_DMA_DONE | R592_REG_MSC_FIFO_DMA_ERR)
+
+#define DMA_IRQ_EN_MASK (DMA_IRQ_ACK_MASK << 16)
+
+#define IRQ_ALL_ACK_MASK 0x00007F00
+#define IRQ_ALL_EN_MASK (IRQ_ALL_ACK_MASK << 16)
+
+/* DMA address for large FIFO read/writes*/
+#define R592_FIFO_DMA			0x2C
+
+/* PIO access to large FIFO (512 bytes) (big endian)*/
+#define R592_FIFO_PIO			0x30
+#define R592_LFIFO_SIZE			512		/* large fifo size */
+
+
+/* large FIFO DMA settings */
+#define R592_FIFO_DMA_SETTINGS		0x34
+#define R592_FIFO_DMA_SETTINGS_EN	(1 << 0)	/* DMA enabled */
+#define R592_FIFO_DMA_SETTINGS_DIR	(1 << 1)	/* Dma direction (1 read, 0 write) */
+#define R592_FIFO_DMA_SETTINGS_CAP	(1 << 24)	/* Dma is aviable */
+
+/* Maybe just an delay */
+/* Bits 17..19 are just number */
+/* bit 16 is set, then bit 20 is waited */
+/* time to wait is about 50 spins * 2 ^ (bits 17..19) */
+/* seems to be possible just to ignore */
+/* Probably debug register */
+#define R592_REG38			0x38
+#define R592_REG38_CHANGE		(1 << 16)	/* Start bit */
+#define R592_REG38_DONE			(1 << 20)	/* HW set this after the delay */
+#define R592_REG38_SHIFT		17
+
+/* Debug register, written (0xABCDEF00) when error happens - not used*/
+#define R592_REG_3C			0x3C
+
+struct r592_device {
+	struct pci_dev *pci_dev;
+	struct memstick_host	*host;		/* host backpointer */
+	struct memstick_request *req;		/* current request */
+
+	/* Registers, IRQ */
+	void __iomem *mmio;
+	int irq;
+	spinlock_t irq_lock;
+	spinlock_t io_thread_lock;
+	struct timer_list detect_timer;
+
+	struct task_struct *io_thread;
+	bool parallel_mode;
+
+	DECLARE_KFIFO(pio_fifo, u8, sizeof(u32));
+
+	/* DMA area */
+	int dma_capable;
+	int dma_error;
+	struct completion dma_done;
+	void *dummy_dma_page;
+	dma_addr_t dummy_dma_page_physical_address;
+
+};
+
+#define DRV_NAME "r592"
+
+
+#define message(format, ...) \
+	printk(KERN_INFO DRV_NAME ": " format "\n", ## __VA_ARGS__)
+
+#define __dbg(level, format, ...) \
+	do { \
+		if (debug >= level) \
+			printk(KERN_DEBUG DRV_NAME \
+				": " format "\n", ## __VA_ARGS__); \
+	} while (0)
+
+
+#define dbg(format, ...)		__dbg(1, format, ## __VA_ARGS__)
+#define dbg_verbose(format, ...)	__dbg(2, format, ## __VA_ARGS__)
+#define dbg_reg(format, ...)		__dbg(3, format, ## __VA_ARGS__)
+
+#endif

drivers/memstick/host/rtsx_pci_ms.c

@@ -0,0 +1,656 @@
+/* Realtek PCI-Express Memstick Card Interface driver
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author:
+ *   Wei WANG <wei_wang@realsil.com.cn>
+ */
+
+#include <linux/module.h>
+#include <linux/highmem.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/memstick.h>
+#include <linux/mfd/rtsx_pci.h>
+#include <asm/unaligned.h>
+
+struct realtek_pci_ms {
+	struct platform_device	*pdev;
+	struct rtsx_pcr		*pcr;
+	struct memstick_host	*msh;
+	struct memstick_request	*req;
+
+	struct mutex		host_mutex;
+	struct work_struct	handle_req;
+
+	u8			ssc_depth;
+	unsigned int		clock;
+	unsigned char           ifmode;
+	bool			eject;
+};
+
+static inline struct device *ms_dev(struct realtek_pci_ms *host)
+{
+	return &(host->pdev->dev);
+}
+
+static inline void ms_clear_error(struct realtek_pci_ms *host)
+{
+	rtsx_pci_write_register(host->pcr, CARD_STOP,
+			MS_STOP | MS_CLR_ERR, MS_STOP | MS_CLR_ERR);
+}
+
+#ifdef DEBUG
+
+static void ms_print_debug_regs(struct realtek_pci_ms *host)
+{
+	struct rtsx_pcr *pcr = host->pcr;
+	u16 i;
+	u8 *ptr;
+
+	/* Print MS host internal registers */
+	rtsx_pci_init_cmd(pcr);
+	for (i = 0xFD40; i <= 0xFD44; i++)
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD, i, 0, 0);
+	for (i = 0xFD52; i <= 0xFD69; i++)
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD, i, 0, 0);
+	rtsx_pci_send_cmd(pcr, 100);
+
+	ptr = rtsx_pci_get_cmd_data(pcr);
+	for (i = 0xFD40; i <= 0xFD44; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+	for (i = 0xFD52; i <= 0xFD69; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+}
+
+#else
+
+#define ms_print_debug_regs(host)
+
+#endif
+
+static int ms_power_on(struct realtek_pci_ms *host)
+{
+	struct rtsx_pcr *pcr = host->pcr;
+	int err;
+
+	rtsx_pci_init_cmd(pcr);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_SELECT, 0x07, MS_MOD_SEL);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_SHARE_MODE,
+			CARD_SHARE_MASK, CARD_SHARE_48_MS);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_CLK_EN,
+			MS_CLK_EN, MS_CLK_EN);
+	err = rtsx_pci_send_cmd(pcr, 100);
+	if (err < 0)
+		return err;
+
+	err = rtsx_pci_card_pull_ctl_enable(pcr, RTSX_MS_CARD);
+	if (err < 0)
+		return err;
+
+	err = rtsx_pci_card_power_on(pcr, RTSX_MS_CARD);
+	if (err < 0)
+		return err;
+
+	/* Wait ms power stable */
+	msleep(150);
+
+	err = rtsx_pci_write_register(pcr, CARD_OE,
+			MS_OUTPUT_EN, MS_OUTPUT_EN);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static int ms_power_off(struct realtek_pci_ms *host)
+{
+	struct rtsx_pcr *pcr = host->pcr;
+	int err;
+
+	rtsx_pci_init_cmd(pcr);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
+
+	err = rtsx_pci_send_cmd(pcr, 100);
+	if (err < 0)
+		return err;
+
+	err = rtsx_pci_card_power_off(pcr, RTSX_MS_CARD);
+	if (err < 0)
+		return err;
+
+	return rtsx_pci_card_pull_ctl_disable(pcr, RTSX_MS_CARD);
+}
+
+static int ms_transfer_data(struct realtek_pci_ms *host, unsigned char data_dir,
+		u8 tpc, u8 cfg, struct scatterlist *sg)
+{
+	struct rtsx_pcr *pcr = host->pcr;
+	int err;
+	unsigned int length = sg->length;
+	u16 sec_cnt = (u16)(length / 512);
+	u8 val, trans_mode, dma_dir;
+	struct memstick_dev *card = host->msh->card;
+	bool pro_card = card->id.type == MEMSTICK_TYPE_PRO;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x, data_dir = %s, length = %d\n",
+			__func__, tpc, (data_dir == READ) ? "READ" : "WRITE",
+			length);
+
+	if (data_dir == READ) {
+		dma_dir = DMA_DIR_FROM_CARD;
+		trans_mode = pro_card ? MS_TM_AUTO_READ : MS_TM_NORMAL_READ;
+	} else {
+		dma_dir = DMA_DIR_TO_CARD;
+		trans_mode = pro_card ? MS_TM_AUTO_WRITE : MS_TM_NORMAL_WRITE;
+	}
+
+	rtsx_pci_init_cmd(pcr);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	if (pro_card) {
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_SECTOR_CNT_H,
+				0xFF, (u8)(sec_cnt >> 8));
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_SECTOR_CNT_L,
+				0xFF, (u8)sec_cnt);
+	}
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, IRQSTAT0,
+			DMA_DONE_INT, DMA_DONE_INT);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, DMATC3, 0xFF, (u8)(length >> 24));
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, DMATC2, 0xFF, (u8)(length >> 16));
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, DMATC1, 0xFF, (u8)(length >> 8));
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, DMATC0, 0xFF, (u8)length);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, DMACTL,
+			0x03 | DMA_PACK_SIZE_MASK, dma_dir | DMA_EN | DMA_512);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, RING_BUFFER);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | trans_mode);
+	rtsx_pci_add_cmd(pcr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+
+	rtsx_pci_send_cmd_no_wait(pcr);
+
+	err = rtsx_pci_transfer_data(pcr, sg, 1, data_dir == READ, 10000);
+	if (err < 0) {
+		ms_clear_error(host);
+		return err;
+	}
+
+	rtsx_pci_read_register(pcr, MS_TRANS_CFG, &val);
+	if (pro_card) {
+		if (val & (MS_INT_CMDNK | MS_INT_ERR |
+				MS_CRC16_ERR | MS_RDY_TIMEOUT))
+			return -EIO;
+	} else {
+		if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT))
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static int ms_write_bytes(struct realtek_pci_ms *host, u8 tpc,
+		u8 cfg, u8 cnt, u8 *data, u8 *int_reg)
+{
+	struct rtsx_pcr *pcr = host->pcr;
+	int err, i;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x\n", __func__, tpc);
+
+	if (!data)
+		return -EINVAL;
+
+	rtsx_pci_init_cmd(pcr);
+
+	for (i = 0; i < cnt; i++)
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
+				PPBUF_BASE2 + i, 0xFF, data[i]);
+	if (cnt % 2)
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
+				PPBUF_BASE2 + i, 0xFF, 0xFF);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | MS_TM_WRITE_BYTES);
+	rtsx_pci_add_cmd(pcr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+	if (int_reg)
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	err = rtsx_pci_send_cmd(pcr, 5000);
+	if (err < 0) {
+		u8 val;
+
+		rtsx_pci_read_register(pcr, MS_TRANS_CFG, &val);
+		dev_dbg(ms_dev(host), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		if (int_reg)
+			*int_reg = val & 0x0F;
+
+		ms_print_debug_regs(host);
+
+		ms_clear_error(host);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR)
+				return -EIO;
+		} else {
+			if (!(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK))
+					return -EIO;
+			}
+		}
+
+		return -ETIMEDOUT;
+	}
+
+	if (int_reg) {
+		u8 *ptr = rtsx_pci_get_cmd_data(pcr) + 1;
+		*int_reg = *ptr & 0x0F;
+	}
+
+	return 0;
+}
+
+static int ms_read_bytes(struct realtek_pci_ms *host, u8 tpc,
+		u8 cfg, u8 cnt, u8 *data, u8 *int_reg)
+{
+	struct rtsx_pcr *pcr = host->pcr;
+	int err, i;
+	u8 *ptr;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x\n", __func__, tpc);
+
+	if (!data)
+		return -EINVAL;
+
+	rtsx_pci_init_cmd(pcr);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | MS_TM_READ_BYTES);
+	rtsx_pci_add_cmd(pcr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+	for (i = 0; i < cnt - 1; i++)
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
+	if (cnt % 2)
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD, PPBUF_BASE2 + cnt, 0, 0);
+	else
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD,
+				PPBUF_BASE2 + cnt - 1, 0, 0);
+	if (int_reg)
+		rtsx_pci_add_cmd(pcr, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	err = rtsx_pci_send_cmd(pcr, 5000);
+	if (err < 0) {
+		u8 val;
+
+		rtsx_pci_read_register(pcr, MS_TRANS_CFG, &val);
+		dev_dbg(ms_dev(host), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		if (int_reg)
+			*int_reg = val & 0x0F;
+
+		ms_print_debug_regs(host);
+
+		ms_clear_error(host);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR)
+				return -EIO;
+		} else {
+			if (!(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK))
+					return -EIO;
+			}
+		}
+
+		return -ETIMEDOUT;
+	}
+
+	ptr = rtsx_pci_get_cmd_data(pcr) + 1;
+	for (i = 0; i < cnt; i++)
+		data[i] = *ptr++;
+
+	if (int_reg)
+		*int_reg = *ptr & 0x0F;
+
+	return 0;
+}
+
+static int rtsx_pci_ms_issue_cmd(struct realtek_pci_ms *host)
+{
+	struct memstick_request *req = host->req;
+	int err = 0;
+	u8 cfg = 0, int_reg;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	if (req->need_card_int) {
+		if (host->ifmode != MEMSTICK_SERIAL)
+			cfg = WAIT_INT;
+	}
+
+	if (req->long_data) {
+		err = ms_transfer_data(host, req->data_dir,
+				req->tpc, cfg, &(req->sg));
+	} else {
+		if (req->data_dir == READ) {
+			err = ms_read_bytes(host, req->tpc, cfg,
+					req->data_len, req->data, &int_reg);
+		} else {
+			err = ms_write_bytes(host, req->tpc, cfg,
+					req->data_len, req->data, &int_reg);
+		}
+	}
+	if (err < 0)
+		return err;
+
+	if (req->need_card_int && (host->ifmode == MEMSTICK_SERIAL)) {
+		err = ms_read_bytes(host, MS_TPC_GET_INT,
+				NO_WAIT_INT, 1, &int_reg, NULL);
+		if (err < 0)
+			return err;
+	}
+
+	if (req->need_card_int) {
+		dev_dbg(ms_dev(host), "int_reg: 0x%02x\n", int_reg);
+
+		if (int_reg & MS_INT_CMDNK)
+			req->int_reg |= MEMSTICK_INT_CMDNAK;
+		if (int_reg & MS_INT_BREQ)
+			req->int_reg |= MEMSTICK_INT_BREQ;
+		if (int_reg & MS_INT_ERR)
+			req->int_reg |= MEMSTICK_INT_ERR;
+		if (int_reg & MS_INT_CED)
+			req->int_reg |= MEMSTICK_INT_CED;
+	}
+
+	return 0;
+}
+
+static void rtsx_pci_ms_handle_req(struct work_struct *work)
+{
+	struct realtek_pci_ms *host = container_of(work,
+			struct realtek_pci_ms, handle_req);
+	struct rtsx_pcr *pcr = host->pcr;
+	struct memstick_host *msh = host->msh;
+	int rc;
+
+	mutex_lock(&pcr->pcr_mutex);
+
+	rtsx_pci_start_run(pcr);
+
+	rtsx_pci_switch_clock(host->pcr, host->clock, host->ssc_depth,
+			false, true, false);
+	rtsx_pci_write_register(pcr, CARD_SELECT, 0x07, MS_MOD_SEL);
+	rtsx_pci_write_register(pcr, CARD_SHARE_MODE,
+			CARD_SHARE_MASK, CARD_SHARE_48_MS);
+
+	if (!host->req) {
+		do {
+			rc = memstick_next_req(msh, &host->req);
+			dev_dbg(ms_dev(host), "next req %d\n", rc);
+
+			if (!rc)
+				host->req->error = rtsx_pci_ms_issue_cmd(host);
+		} while (!rc);
+	}
+
+	mutex_unlock(&pcr->pcr_mutex);
+}
+
+static void rtsx_pci_ms_request(struct memstick_host *msh)
+{
+	struct realtek_pci_ms *host = memstick_priv(msh);
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	if (rtsx_pci_card_exclusive_check(host->pcr, RTSX_MS_CARD))
+		return;
+
+	schedule_work(&host->handle_req);
+}
+
+static int rtsx_pci_ms_set_param(struct memstick_host *msh,
+		enum memstick_param param, int value)
+{
+	struct realtek_pci_ms *host = memstick_priv(msh);
+	struct rtsx_pcr *pcr = host->pcr;
+	unsigned int clock = 0;
+	u8 ssc_depth = 0;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s: param = %d, value = %d\n",
+			__func__, param, value);
+
+	err = rtsx_pci_card_exclusive_check(host->pcr, RTSX_MS_CARD);
+	if (err)
+		return err;
+
+	switch (param) {
+	case MEMSTICK_POWER:
+		if (value == MEMSTICK_POWER_ON)
+			err = ms_power_on(host);
+		else if (value == MEMSTICK_POWER_OFF)
+			err = ms_power_off(host);
+		else
+			return -EINVAL;
+		break;
+
+	case MEMSTICK_INTERFACE:
+		if (value == MEMSTICK_SERIAL) {
+			clock = 19000000;
+			ssc_depth = RTSX_SSC_DEPTH_500K;
+
+			err = rtsx_pci_write_register(pcr, MS_CFG, 0x58,
+					MS_BUS_WIDTH_1 | PUSH_TIME_DEFAULT);
+			if (err < 0)
+				return err;
+		} else if (value == MEMSTICK_PAR4) {
+			clock = 39000000;
+			ssc_depth = RTSX_SSC_DEPTH_1M;
+
+			err = rtsx_pci_write_register(pcr, MS_CFG,
+					0x58, MS_BUS_WIDTH_4 | PUSH_TIME_ODD);
+			if (err < 0)
+				return err;
+		} else {
+			return -EINVAL;
+		}
+
+		err = rtsx_pci_switch_clock(pcr, clock,
+				ssc_depth, false, true, false);
+		if (err < 0)
+			return err;
+
+		host->ssc_depth = ssc_depth;
+		host->clock = clock;
+		host->ifmode = value;
+		break;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int rtsx_pci_ms_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct realtek_pci_ms *host = platform_get_drvdata(pdev);
+	struct memstick_host *msh = host->msh;
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	memstick_suspend_host(msh);
+	return 0;
+}
+
+static int rtsx_pci_ms_resume(struct platform_device *pdev)
+{
+	struct realtek_pci_ms *host = platform_get_drvdata(pdev);
+	struct memstick_host *msh = host->msh;
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	memstick_resume_host(msh);
+	return 0;
+}
+
+#else /* CONFIG_PM */
+
+#define rtsx_pci_ms_suspend NULL
+#define rtsx_pci_ms_resume NULL
+
+#endif /* CONFIG_PM */
+
+static void rtsx_pci_ms_card_event(struct platform_device *pdev)
+{
+	struct realtek_pci_ms *host = platform_get_drvdata(pdev);
+
+	memstick_detect_change(host->msh);
+}
+
+static int rtsx_pci_ms_drv_probe(struct platform_device *pdev)
+{
+	struct memstick_host *msh;
+	struct realtek_pci_ms *host;
+	struct rtsx_pcr *pcr;
+	struct pcr_handle *handle = pdev->dev.platform_data;
+	int rc;
+
+	if (!handle)
+		return -ENXIO;
+
+	pcr = handle->pcr;
+	if (!pcr)
+		return -ENXIO;
+
+	dev_dbg(&(pdev->dev),
+			": Realtek PCI-E Memstick controller found\n");
+
+	msh = memstick_alloc_host(sizeof(*host), &pdev->dev);
+	if (!msh)
+		return -ENOMEM;
+
+	host = memstick_priv(msh);
+	host->pcr = pcr;
+	host->msh = msh;
+	host->pdev = pdev;
+	platform_set_drvdata(pdev, host);
+	pcr->slots[RTSX_MS_CARD].p_dev = pdev;
+	pcr->slots[RTSX_MS_CARD].card_event = rtsx_pci_ms_card_event;
+
+	mutex_init(&host->host_mutex);
+
+	INIT_WORK(&host->handle_req, rtsx_pci_ms_handle_req);
+	msh->request = rtsx_pci_ms_request;
+	msh->set_param = rtsx_pci_ms_set_param;
+	msh->caps = MEMSTICK_CAP_PAR4;
+
+	rc = memstick_add_host(msh);
+	if (rc) {
+		memstick_free_host(msh);
+		return rc;
+	}
+
+	return 0;
+}
+
+static int rtsx_pci_ms_drv_remove(struct platform_device *pdev)
+{
+	struct realtek_pci_ms *host = platform_get_drvdata(pdev);
+	struct rtsx_pcr *pcr;
+	struct memstick_host *msh;
+	int rc;
+
+	if (!host)
+		return 0;
+
+	pcr = host->pcr;
+	pcr->slots[RTSX_MS_CARD].p_dev = NULL;
+	pcr->slots[RTSX_MS_CARD].card_event = NULL;
+	msh = host->msh;
+	host->eject = true;
+	cancel_work_sync(&host->handle_req);
+
+	mutex_lock(&host->host_mutex);
+	if (host->req) {
+		dev_dbg(&(pdev->dev),
+			"%s: Controller removed during transfer\n",
+			dev_name(&msh->dev));
+
+		rtsx_pci_complete_unfinished_transfer(pcr);
+
+		host->req->error = -ENOMEDIUM;
+		do {
+			rc = memstick_next_req(msh, &host->req);
+			if (!rc)
+				host->req->error = -ENOMEDIUM;
+		} while (!rc);
+	}
+	mutex_unlock(&host->host_mutex);
+
+	memstick_remove_host(msh);
+	memstick_free_host(msh);
+
+	dev_dbg(&(pdev->dev),
+		": Realtek PCI-E Memstick controller has been removed\n");
+
+	return 0;
+}
+
+static struct platform_device_id rtsx_pci_ms_ids[] = {
+	{
+		.name = DRV_NAME_RTSX_PCI_MS,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, rtsx_pci_ms_ids);
+
+static struct platform_driver rtsx_pci_ms_driver = {
+	.probe		= rtsx_pci_ms_drv_probe,
+	.remove		= rtsx_pci_ms_drv_remove,
+	.id_table       = rtsx_pci_ms_ids,
+	.suspend	= rtsx_pci_ms_suspend,
+	.resume		= rtsx_pci_ms_resume,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= DRV_NAME_RTSX_PCI_MS,
+	},
+};
+module_platform_driver(rtsx_pci_ms_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Wei WANG <wei_wang@realsil.com.cn>");
+MODULE_DESCRIPTION("Realtek PCI-E Memstick Card Host Driver");

drivers/memstick/host/rtsx_usb_ms.c

@@ -0,0 +1,839 @@
+/* Realtek USB Memstick Card Interface driver
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author:
+ *   Roger Tseng <rogerable@realtek.com>
+ */
+
+#include <linux/module.h>
+#include <linux/highmem.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/memstick.h>
+#include <linux/kthread.h>
+#include <linux/mfd/rtsx_usb.h>
+#include <linux/pm_runtime.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/completion.h>
+#include <asm/unaligned.h>
+
+struct rtsx_usb_ms {
+	struct platform_device	*pdev;
+	struct rtsx_ucr	*ucr;
+	struct memstick_host	*msh;
+	struct memstick_request	*req;
+
+	struct mutex		host_mutex;
+	struct work_struct	handle_req;
+
+	struct task_struct	*detect_ms;
+	struct completion	detect_ms_exit;
+
+	u8			ssc_depth;
+	unsigned int		clock;
+	int			power_mode;
+	unsigned char           ifmode;
+	bool			eject;
+};
+
+static inline struct device *ms_dev(struct rtsx_usb_ms *host)
+{
+	return &(host->pdev->dev);
+}
+
+static inline void ms_clear_error(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	rtsx_usb_ep0_write_register(ucr, CARD_STOP,
+				  MS_STOP | MS_CLR_ERR,
+				  MS_STOP | MS_CLR_ERR);
+
+	rtsx_usb_clear_dma_err(ucr);
+	rtsx_usb_clear_fsm_err(ucr);
+}
+
+#ifdef DEBUG
+
+static void ms_print_debug_regs(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	u16 i;
+	u8 *ptr;
+
+	/* Print MS host internal registers */
+	rtsx_usb_init_cmd(ucr);
+
+	/* MS_CFG to MS_INT_REG */
+	for (i = 0xFD40; i <= 0xFD44; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, i, 0, 0);
+
+	/* CARD_SHARE_MODE to CARD_GPIO */
+	for (i = 0xFD51; i <= 0xFD56; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, i, 0, 0);
+
+	/* CARD_PULL_CTLx */
+	for (i = 0xFD60; i <= 0xFD65; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, i, 0, 0);
+
+	/* CARD_DATA_SOURCE, CARD_SELECT, CARD_CLK_EN, CARD_PWR_CTL */
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_DATA_SOURCE, 0, 0);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_SELECT, 0, 0);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_CLK_EN, 0, 0);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_PWR_CTL, 0, 0);
+
+	rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	rtsx_usb_get_rsp(ucr, 21, 100);
+
+	ptr = ucr->rsp_buf;
+	for (i = 0xFD40; i <= 0xFD44; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+	for (i = 0xFD51; i <= 0xFD56; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+	for (i = 0xFD60; i <= 0xFD65; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_DATA_SOURCE, *(ptr++));
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_SELECT, *(ptr++));
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_CLK_EN, *(ptr++));
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_PWR_CTL, *(ptr++));
+}
+
+#else
+
+static void ms_print_debug_regs(struct rtsx_usb_ms *host)
+{
+}
+
+#endif
+
+static int ms_pull_ctl_disable_lqfp48(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_pull_ctl_disable_qfn24(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_pull_ctl_enable_lqfp48(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_pull_ctl_enable_qfn24(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_power_on(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	rtsx_usb_init_cmd(ucr);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_SELECT, 0x07, MS_MOD_SEL);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_SHARE_MODE,
+			CARD_SHARE_MASK, CARD_SHARE_MS);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_EN,
+			MS_CLK_EN, MS_CLK_EN);
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err < 0)
+		return err;
+
+	if (CHECK_PKG(ucr, LQFP48))
+		err = ms_pull_ctl_enable_lqfp48(ucr);
+	else
+		err = ms_pull_ctl_enable_qfn24(ucr);
+	if (err < 0)
+		return err;
+
+	err = rtsx_usb_write_register(ucr, CARD_PWR_CTL,
+			POWER_MASK, PARTIAL_POWER_ON);
+	if (err)
+		return err;
+
+	usleep_range(800, 1000);
+
+	rtsx_usb_init_cmd(ucr);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PWR_CTL,
+			POWER_MASK, POWER_ON);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_OE,
+			MS_OUTPUT_EN, MS_OUTPUT_EN);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_power_off(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err < 0)
+		return err;
+
+	if (CHECK_PKG(ucr, LQFP48))
+		return ms_pull_ctl_disable_lqfp48(ucr);
+
+	return ms_pull_ctl_disable_qfn24(ucr);
+}
+
+static int ms_transfer_data(struct rtsx_usb_ms *host, unsigned char data_dir,
+		u8 tpc, u8 cfg, struct scatterlist *sg)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	unsigned int length = sg->length;
+	u16 sec_cnt = (u16)(length / 512);
+	u8 trans_mode, dma_dir, flag;
+	unsigned int pipe;
+	struct memstick_dev *card = host->msh->card;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x, data_dir = %s, length = %d\n",
+			__func__, tpc, (data_dir == READ) ? "READ" : "WRITE",
+			length);
+
+	if (data_dir == READ) {
+		flag = MODE_CDIR;
+		dma_dir = DMA_DIR_FROM_CARD;
+		if (card->id.type != MEMSTICK_TYPE_PRO)
+			trans_mode = MS_TM_NORMAL_READ;
+		else
+			trans_mode = MS_TM_AUTO_READ;
+		pipe = usb_rcvbulkpipe(ucr->pusb_dev, EP_BULK_IN);
+	} else {
+		flag = MODE_CDOR;
+		dma_dir = DMA_DIR_TO_CARD;
+		if (card->id.type != MEMSTICK_TYPE_PRO)
+			trans_mode = MS_TM_NORMAL_WRITE;
+		else
+			trans_mode = MS_TM_AUTO_WRITE;
+		pipe = usb_sndbulkpipe(ucr->pusb_dev, EP_BULK_OUT);
+	}
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	if (card->id.type == MEMSTICK_TYPE_PRO) {
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_SECTOR_CNT_H,
+				0xFF, (u8)(sec_cnt >> 8));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_SECTOR_CNT_L,
+				0xFF, (u8)sec_cnt);
+	}
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC3,
+			0xFF, (u8)(length >> 24));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC2,
+			0xFF, (u8)(length >> 16));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC1,
+			0xFF, (u8)(length >> 8));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC0, 0xFF,
+			(u8)length);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_CTL,
+			0x03 | DMA_PACK_SIZE_MASK, dma_dir | DMA_EN | DMA_512);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, RING_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | trans_mode);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+
+	err = rtsx_usb_send_cmd(ucr, flag | STAGE_MS_STATUS, 100);
+	if (err)
+		return err;
+
+	err = rtsx_usb_transfer_data(ucr, pipe, sg, length,
+			1, NULL, 10000);
+	if (err)
+		goto err_out;
+
+	err = rtsx_usb_get_rsp(ucr, 3, 15000);
+	if (err)
+		goto err_out;
+
+	if (ucr->rsp_buf[0] & MS_TRANSFER_ERR ||
+	    ucr->rsp_buf[1] & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+		err = -EIO;
+		goto err_out;
+	}
+	return 0;
+err_out:
+	ms_clear_error(host);
+	return err;
+}
+
+static int ms_write_bytes(struct rtsx_usb_ms *host, u8 tpc,
+		u8 cfg, u8 cnt, u8 *data, u8 *int_reg)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err, i;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x\n", __func__, tpc);
+
+	rtsx_usb_init_cmd(ucr);
+
+	for (i = 0; i < cnt; i++)
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				PPBUF_BASE2 + i, 0xFF, data[i]);
+
+	if (cnt % 2)
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				PPBUF_BASE2 + i, 0xFF, 0xFF);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | MS_TM_WRITE_BYTES);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	if (err)
+		return err;
+
+	err = rtsx_usb_get_rsp(ucr, 2, 5000);
+	if (err || (ucr->rsp_buf[0] & MS_TRANSFER_ERR)) {
+		u8 val;
+
+		rtsx_usb_ep0_read_register(ucr, MS_TRANS_CFG, &val);
+		dev_dbg(ms_dev(host), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		if (int_reg)
+			*int_reg = val & 0x0F;
+
+		ms_print_debug_regs(host);
+
+		ms_clear_error(host);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR)
+				return -EIO;
+		} else {
+			if (!(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK))
+					return -EIO;
+			}
+		}
+
+		return -ETIMEDOUT;
+	}
+
+	if (int_reg)
+		*int_reg = ucr->rsp_buf[1] & 0x0F;
+
+	return 0;
+}
+
+static int ms_read_bytes(struct rtsx_usb_ms *host, u8 tpc,
+		u8 cfg, u8 cnt, u8 *data, u8 *int_reg)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err, i;
+	u8 *ptr;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x\n", __func__, tpc);
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | MS_TM_READ_BYTES);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+	for (i = 0; i < cnt - 1; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
+	if (cnt % 2)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, PPBUF_BASE2 + cnt, 0, 0);
+	else
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD,
+				PPBUF_BASE2 + cnt - 1, 0, 0);
+
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	if (err)
+		return err;
+
+	err = rtsx_usb_get_rsp(ucr, cnt + 2, 5000);
+	if (err || (ucr->rsp_buf[0] & MS_TRANSFER_ERR)) {
+		u8 val;
+
+		rtsx_usb_ep0_read_register(ucr, MS_TRANS_CFG, &val);
+		dev_dbg(ms_dev(host), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		if (int_reg && (host->ifmode != MEMSTICK_SERIAL))
+			*int_reg = val & 0x0F;
+
+		ms_print_debug_regs(host);
+
+		ms_clear_error(host);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR)
+				return -EIO;
+		} else {
+			if (!(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK))
+					return -EIO;
+			}
+		}
+
+		return -ETIMEDOUT;
+	}
+
+	ptr = ucr->rsp_buf + 1;
+	for (i = 0; i < cnt; i++)
+		data[i] = *ptr++;
+
+
+	if (int_reg && (host->ifmode != MEMSTICK_SERIAL))
+		*int_reg = *ptr & 0x0F;
+
+	return 0;
+}
+
+static int rtsx_usb_ms_issue_cmd(struct rtsx_usb_ms *host)
+{
+	struct memstick_request *req = host->req;
+	int err = 0;
+	u8 cfg = 0, int_reg;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	if (req->need_card_int) {
+		if (host->ifmode != MEMSTICK_SERIAL)
+			cfg = WAIT_INT;
+	}
+
+	if (req->long_data) {
+		err = ms_transfer_data(host, req->data_dir,
+				req->tpc, cfg, &(req->sg));
+	} else {
+		if (req->data_dir == READ)
+			err = ms_read_bytes(host, req->tpc, cfg,
+					req->data_len, req->data, &int_reg);
+		else
+			err = ms_write_bytes(host, req->tpc, cfg,
+					req->data_len, req->data, &int_reg);
+	}
+	if (err < 0)
+		return err;
+
+	if (req->need_card_int) {
+		if (host->ifmode == MEMSTICK_SERIAL) {
+			err = ms_read_bytes(host, MS_TPC_GET_INT,
+					NO_WAIT_INT, 1, &req->int_reg, NULL);
+			if (err < 0)
+				return err;
+		} else {
+
+			if (int_reg & MS_INT_CMDNK)
+				req->int_reg |= MEMSTICK_INT_CMDNAK;
+			if (int_reg & MS_INT_BREQ)
+				req->int_reg |= MEMSTICK_INT_BREQ;
+			if (int_reg & MS_INT_ERR)
+				req->int_reg |= MEMSTICK_INT_ERR;
+			if (int_reg & MS_INT_CED)
+				req->int_reg |= MEMSTICK_INT_CED;
+		}
+		dev_dbg(ms_dev(host), "int_reg: 0x%02x\n", req->int_reg);
+	}
+
+	return 0;
+}
+
+static void rtsx_usb_ms_handle_req(struct work_struct *work)
+{
+	struct rtsx_usb_ms *host = container_of(work,
+			struct rtsx_usb_ms, handle_req);
+	struct rtsx_ucr *ucr = host->ucr;
+	struct memstick_host *msh = host->msh;
+	int rc;
+
+	if (!host->req) {
+		do {
+			rc = memstick_next_req(msh, &host->req);
+			dev_dbg(ms_dev(host), "next req %d\n", rc);
+
+			if (!rc) {
+				mutex_lock(&ucr->dev_mutex);
+
+				if (rtsx_usb_card_exclusive_check(ucr,
+							RTSX_USB_MS_CARD))
+					host->req->error = -EIO;
+				else
+					host->req->error =
+						rtsx_usb_ms_issue_cmd(host);
+
+				mutex_unlock(&ucr->dev_mutex);
+
+				dev_dbg(ms_dev(host), "req result %d\n",
+						host->req->error);
+			}
+		} while (!rc);
+	}
+
+}
+
+static void rtsx_usb_ms_request(struct memstick_host *msh)
+{
+	struct rtsx_usb_ms *host = memstick_priv(msh);
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	if (!host->eject)
+		schedule_work(&host->handle_req);
+}
+
+static int rtsx_usb_ms_set_param(struct memstick_host *msh,
+		enum memstick_param param, int value)
+{
+	struct rtsx_usb_ms *host = memstick_priv(msh);
+	struct rtsx_ucr *ucr = host->ucr;
+	unsigned int clock = 0;
+	u8 ssc_depth = 0;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s: param = %d, value = %d\n",
+			__func__, param, value);
+
+	mutex_lock(&ucr->dev_mutex);
+
+	err = rtsx_usb_card_exclusive_check(ucr, RTSX_USB_MS_CARD);
+	if (err)
+		goto out;
+
+	switch (param) {
+	case MEMSTICK_POWER:
+		if (value == host->power_mode)
+			break;
+
+		if (value == MEMSTICK_POWER_ON) {
+			pm_runtime_get_sync(ms_dev(host));
+			err = ms_power_on(host);
+		} else if (value == MEMSTICK_POWER_OFF) {
+			err = ms_power_off(host);
+			if (host->msh->card)
+				pm_runtime_put_noidle(ms_dev(host));
+			else
+				pm_runtime_put(ms_dev(host));
+		} else
+			err = -EINVAL;
+		if (!err)
+			host->power_mode = value;
+		break;
+
+	case MEMSTICK_INTERFACE:
+		if (value == MEMSTICK_SERIAL) {
+			clock = 19000000;
+			ssc_depth = SSC_DEPTH_512K;
+			err = rtsx_usb_write_register(ucr, MS_CFG, 0x5A,
+				       MS_BUS_WIDTH_1 | PUSH_TIME_DEFAULT);
+			if (err < 0)
+				break;
+		} else if (value == MEMSTICK_PAR4) {
+			clock = 39000000;
+			ssc_depth = SSC_DEPTH_1M;
+
+			err = rtsx_usb_write_register(ucr, MS_CFG, 0x5A,
+					MS_BUS_WIDTH_4 | PUSH_TIME_ODD |
+					MS_NO_CHECK_INT);
+			if (err < 0)
+				break;
+		} else {
+			err = -EINVAL;
+			break;
+		}
+
+		err = rtsx_usb_switch_clock(ucr, clock,
+				ssc_depth, false, true, false);
+		if (err < 0) {
+			dev_dbg(ms_dev(host), "switch clock failed\n");
+			break;
+		}
+
+		host->ssc_depth = ssc_depth;
+		host->clock = clock;
+		host->ifmode = value;
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+out:
+	mutex_unlock(&ucr->dev_mutex);
+
+	/* power-on delay */
+	if (param == MEMSTICK_POWER && value == MEMSTICK_POWER_ON)
+		usleep_range(10000, 12000);
+
+	dev_dbg(ms_dev(host), "%s: return = %d\n", __func__, err);
+	return err;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int rtsx_usb_ms_suspend(struct device *dev)
+{
+	struct rtsx_usb_ms *host = dev_get_drvdata(dev);
+	struct memstick_host *msh = host->msh;
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	memstick_suspend_host(msh);
+	return 0;
+}
+
+static int rtsx_usb_ms_resume(struct device *dev)
+{
+	struct rtsx_usb_ms *host = dev_get_drvdata(dev);
+	struct memstick_host *msh = host->msh;
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	memstick_resume_host(msh);
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+/*
+ * Thread function of ms card slot detection. The thread starts right after
+ * successful host addition. It stops while the driver removal function sets
+ * host->eject true.
+ */
+static int rtsx_usb_detect_ms_card(void *__host)
+{
+	struct rtsx_usb_ms *host = (struct rtsx_usb_ms *)__host;
+	struct rtsx_ucr *ucr = host->ucr;
+	u8 val = 0;
+	int err;
+
+	for (;;) {
+		mutex_lock(&ucr->dev_mutex);
+
+		/* Check pending MS card changes */
+		err = rtsx_usb_read_register(ucr, CARD_INT_PEND, &val);
+		if (err) {
+			mutex_unlock(&ucr->dev_mutex);
+			goto poll_again;
+		}
+
+		/* Clear the pending */
+		rtsx_usb_write_register(ucr, CARD_INT_PEND,
+				XD_INT | MS_INT | SD_INT,
+				XD_INT | MS_INT | SD_INT);
+
+		mutex_unlock(&ucr->dev_mutex);
+
+		if (val & MS_INT) {
+			dev_dbg(ms_dev(host), "MS slot change detected\n");
+			memstick_detect_change(host->msh);
+		}
+
+poll_again:
+		if (host->eject)
+			break;
+
+		msleep(1000);
+	}
+
+	complete(&host->detect_ms_exit);
+	return 0;
+}
+
+static int rtsx_usb_ms_drv_probe(struct platform_device *pdev)
+{
+	struct memstick_host *msh;
+	struct rtsx_usb_ms *host;
+	struct rtsx_ucr *ucr;
+	int err;
+
+	ucr = usb_get_intfdata(to_usb_interface(pdev->dev.parent));
+	if (!ucr)
+		return -ENXIO;
+
+	dev_dbg(&(pdev->dev),
+			"Realtek USB Memstick controller found\n");
+
+	msh = memstick_alloc_host(sizeof(*host), &pdev->dev);
+	if (!msh)
+		return -ENOMEM;
+
+	host = memstick_priv(msh);
+	host->ucr = ucr;
+	host->msh = msh;
+	host->pdev = pdev;
+	host->power_mode = MEMSTICK_POWER_OFF;
+	platform_set_drvdata(pdev, host);
+
+	mutex_init(&host->host_mutex);
+	INIT_WORK(&host->handle_req, rtsx_usb_ms_handle_req);
+
+	init_completion(&host->detect_ms_exit);
+	host->detect_ms = kthread_create(rtsx_usb_detect_ms_card, host,
+			"rtsx_usb_ms_%d", pdev->id);
+	if (IS_ERR(host->detect_ms)) {
+		dev_dbg(&(pdev->dev),
+				"Unable to create polling thread.\n");
+		err = PTR_ERR(host->detect_ms);
+		goto err_out;
+	}
+
+	msh->request = rtsx_usb_ms_request;
+	msh->set_param = rtsx_usb_ms_set_param;
+	msh->caps = MEMSTICK_CAP_PAR4;
+
+	pm_runtime_enable(&pdev->dev);
+	err = memstick_add_host(msh);
+	if (err)
+		goto err_out;
+
+	wake_up_process(host->detect_ms);
+	return 0;
+err_out:
+	memstick_free_host(msh);
+	return err;
+}
+
+static int rtsx_usb_ms_drv_remove(struct platform_device *pdev)
+{
+	struct rtsx_usb_ms *host = platform_get_drvdata(pdev);
+	struct memstick_host *msh;
+	int err;
+
+	msh = host->msh;
+	host->eject = true;
+	cancel_work_sync(&host->handle_req);
+
+	mutex_lock(&host->host_mutex);
+	if (host->req) {
+		dev_dbg(&(pdev->dev),
+			"%s: Controller removed during transfer\n",
+			dev_name(&msh->dev));
+		host->req->error = -ENOMEDIUM;
+		do {
+			err = memstick_next_req(msh, &host->req);
+			if (!err)
+				host->req->error = -ENOMEDIUM;
+		} while (!err);
+	}
+	mutex_unlock(&host->host_mutex);
+
+	wait_for_completion(&host->detect_ms_exit);
+	memstick_remove_host(msh);
+	memstick_free_host(msh);
+
+	/* Balance possible unbalanced usage count
+	 * e.g. unconditional module removal
+	 */
+	if (pm_runtime_active(ms_dev(host)))
+		pm_runtime_put(ms_dev(host));
+
+	pm_runtime_disable(&pdev->dev);
+	platform_set_drvdata(pdev, NULL);
+
+	dev_dbg(&(pdev->dev),
+		": Realtek USB Memstick controller has been removed\n");
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(rtsx_usb_ms_pm_ops,
+		rtsx_usb_ms_suspend, rtsx_usb_ms_resume);
+
+static struct platform_device_id rtsx_usb_ms_ids[] = {
+	{
+		.name = "rtsx_usb_ms",
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, rtsx_usb_ms_ids);
+
+static struct platform_driver rtsx_usb_ms_driver = {
+	.probe		= rtsx_usb_ms_drv_probe,
+	.remove		= rtsx_usb_ms_drv_remove,
+	.id_table       = rtsx_usb_ms_ids,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "rtsx_usb_ms",
+		.pm	= &rtsx_usb_ms_pm_ops,
+	},
+};
+module_platform_driver(rtsx_usb_ms_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Roger Tseng <rogerable@realtek.com>");
+MODULE_DESCRIPTION("Realtek USB Memstick Card Host Driver");

drivers/memstick/host/tifm_ms.c

@@ -0,0 +1,688 @@
+/*
+ *  TI FlashMedia driver
+ *
+ *  Copyright (C) 2007 Alex Dubov <oakad@yahoo.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.
+ *
+ * Special thanks to Carlos Corbacho for providing various MemoryStick cards
+ * that made this driver possible.
+ *
+ */
+
+#include <linux/tifm.h>
+#include <linux/memstick.h>
+#include <linux/highmem.h>
+#include <linux/scatterlist.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+#include <asm/io.h>
+
+#define DRIVER_NAME "tifm_ms"
+
+static bool no_dma;
+module_param(no_dma, bool, 0644);
+
+/*
+ * Some control bits of TIFM appear to conform to Sony's reference design,
+ * so I'm just assuming they all are.
+ */
+
+#define TIFM_MS_STAT_DRQ     0x04000
+#define TIFM_MS_STAT_MSINT   0x02000
+#define TIFM_MS_STAT_RDY     0x01000
+#define TIFM_MS_STAT_CRC     0x00200
+#define TIFM_MS_STAT_TOE     0x00100
+#define TIFM_MS_STAT_EMP     0x00020
+#define TIFM_MS_STAT_FUL     0x00010
+#define TIFM_MS_STAT_CED     0x00008
+#define TIFM_MS_STAT_ERR     0x00004
+#define TIFM_MS_STAT_BRQ     0x00002
+#define TIFM_MS_STAT_CNK     0x00001
+
+#define TIFM_MS_SYS_DMA      0x10000
+#define TIFM_MS_SYS_RESET    0x08000
+#define TIFM_MS_SYS_SRAC     0x04000
+#define TIFM_MS_SYS_INTEN    0x02000
+#define TIFM_MS_SYS_NOCRC    0x01000
+#define TIFM_MS_SYS_INTCLR   0x00800
+#define TIFM_MS_SYS_MSIEN    0x00400
+#define TIFM_MS_SYS_FCLR     0x00200
+#define TIFM_MS_SYS_FDIR     0x00100
+#define TIFM_MS_SYS_DAM      0x00080
+#define TIFM_MS_SYS_DRM      0x00040
+#define TIFM_MS_SYS_DRQSL    0x00020
+#define TIFM_MS_SYS_REI      0x00010
+#define TIFM_MS_SYS_REO      0x00008
+#define TIFM_MS_SYS_BSY_MASK 0x00007
+
+#define TIFM_MS_SYS_FIFO     (TIFM_MS_SYS_INTEN | TIFM_MS_SYS_MSIEN \
+			      | TIFM_MS_SYS_FCLR | TIFM_MS_SYS_BSY_MASK)
+
+/* Hardware flags */
+enum {
+	CMD_READY  = 0x01,
+	FIFO_READY = 0x02,
+	CARD_INT   = 0x04
+};
+
+struct tifm_ms {
+	struct tifm_dev         *dev;
+	struct timer_list       timer;
+	struct memstick_request *req;
+	struct tasklet_struct   notify;
+	unsigned int            mode_mask;
+	unsigned int            block_pos;
+	unsigned long           timeout_jiffies;
+	unsigned char           eject:1,
+				use_dma:1;
+	unsigned char           cmd_flags;
+	unsigned char           io_pos;
+	unsigned int            io_word;
+};
+
+static unsigned int tifm_ms_read_data(struct tifm_ms *host,
+				      unsigned char *buf, unsigned int length)
+{
+	struct tifm_dev *sock = host->dev;
+	unsigned int off = 0;
+
+	while (host->io_pos && length) {
+		buf[off++] = host->io_word & 0xff;
+		host->io_word >>= 8;
+		length--;
+		host->io_pos--;
+	}
+
+	if (!length)
+		return off;
+
+	while (!(TIFM_MS_STAT_EMP & readl(sock->addr + SOCK_MS_STATUS))) {
+		if (length < 4)
+			break;
+		*(unsigned int *)(buf + off) = __raw_readl(sock->addr
+							   + SOCK_MS_DATA);
+		length -= 4;
+		off += 4;
+	}
+
+	if (length
+	    && !(TIFM_MS_STAT_EMP & readl(sock->addr + SOCK_MS_STATUS))) {
+		host->io_word = readl(sock->addr + SOCK_MS_DATA);
+		for (host->io_pos = 4; host->io_pos; --host->io_pos) {
+			buf[off++] = host->io_word & 0xff;
+			host->io_word >>= 8;
+			length--;
+			if (!length)
+				break;
+		}
+	}
+
+	return off;
+}
+
+static unsigned int tifm_ms_write_data(struct tifm_ms *host,
+				       unsigned char *buf, unsigned int length)
+{
+	struct tifm_dev *sock = host->dev;
+	unsigned int off = 0;
+
+	if (host->io_pos) {
+		while (host->io_pos < 4 && length) {
+			host->io_word |=  buf[off++] << (host->io_pos * 8);
+			host->io_pos++;
+			length--;
+		}
+	}
+
+	if (host->io_pos == 4
+	    && !(TIFM_MS_STAT_FUL & readl(sock->addr + SOCK_MS_STATUS))) {
+		writel(TIFM_MS_SYS_FDIR | readl(sock->addr + SOCK_MS_SYSTEM),
+		       sock->addr + SOCK_MS_SYSTEM);
+		writel(host->io_word, sock->addr + SOCK_MS_DATA);
+		host->io_pos = 0;
+		host->io_word = 0;
+	} else if (host->io_pos) {
+		return off;
+	}
+
+	if (!length)
+		return off;
+
+	while (!(TIFM_MS_STAT_FUL & readl(sock->addr + SOCK_MS_STATUS))) {
+		if (length < 4)
+			break;
+		writel(TIFM_MS_SYS_FDIR | readl(sock->addr + SOCK_MS_SYSTEM),
+		       sock->addr + SOCK_MS_SYSTEM);
+		__raw_writel(*(unsigned int *)(buf + off),
+			     sock->addr + SOCK_MS_DATA);
+		length -= 4;
+		off += 4;
+	}
+
+	switch (length) {
+	case 3:
+		host->io_word |= buf[off + 2] << 16;
+		host->io_pos++;
+	case 2:
+		host->io_word |= buf[off + 1] << 8;
+		host->io_pos++;
+	case 1:
+		host->io_word |= buf[off];
+		host->io_pos++;
+	}
+
+	off += host->io_pos;
+
+	return off;
+}
+
+static unsigned int tifm_ms_transfer_data(struct tifm_ms *host)
+{
+	struct tifm_dev *sock = host->dev;
+	unsigned int length;
+	unsigned int off;
+	unsigned int t_size, p_cnt;
+	unsigned char *buf;
+	struct page *pg;
+	unsigned long flags = 0;
+
+	if (host->req->long_data) {
+		length = host->req->sg.length - host->block_pos;
+		off = host->req->sg.offset + host->block_pos;
+	} else {
+		length = host->req->data_len - host->block_pos;
+		off = 0;
+	}
+	dev_dbg(&sock->dev, "fifo data transfer, %d, %d\n", length,
+		host->block_pos);
+
+	while (length) {
+		unsigned int uninitialized_var(p_off);
+
+		if (host->req->long_data) {
+			pg = nth_page(sg_page(&host->req->sg),
+				      off >> PAGE_SHIFT);
+			p_off = offset_in_page(off);
+			p_cnt = PAGE_SIZE - p_off;
+			p_cnt = min(p_cnt, length);
+
+			local_irq_save(flags);
+			buf = kmap_atomic(pg) + p_off;
+		} else {
+			buf = host->req->data + host->block_pos;
+			p_cnt = host->req->data_len - host->block_pos;
+		}
+
+		t_size = host->req->data_dir == WRITE
+			 ? tifm_ms_write_data(host, buf, p_cnt)
+			 : tifm_ms_read_data(host, buf, p_cnt);
+
+		if (host->req->long_data) {
+			kunmap_atomic(buf - p_off);
+			local_irq_restore(flags);
+		}
+
+		if (!t_size)
+			break;
+		host->block_pos += t_size;
+		length -= t_size;
+		off += t_size;
+	}
+
+	dev_dbg(&sock->dev, "fifo data transfer, %d remaining\n", length);
+	if (!length && (host->req->data_dir == WRITE)) {
+		if (host->io_pos) {
+			writel(TIFM_MS_SYS_FDIR
+			       | readl(sock->addr + SOCK_MS_SYSTEM),
+			       sock->addr + SOCK_MS_SYSTEM);
+			writel(host->io_word, sock->addr + SOCK_MS_DATA);
+		}
+		writel(TIFM_MS_SYS_FDIR
+		       | readl(sock->addr + SOCK_MS_SYSTEM),
+		       sock->addr + SOCK_MS_SYSTEM);
+		writel(0, sock->addr + SOCK_MS_DATA);
+	} else {
+		readl(sock->addr + SOCK_MS_DATA);
+	}
+
+	return length;
+}
+
+static int tifm_ms_issue_cmd(struct tifm_ms *host)
+{
+	struct tifm_dev *sock = host->dev;
+	unsigned char *data;
+	unsigned int data_len, cmd, sys_param;
+
+	host->cmd_flags = 0;
+	host->block_pos = 0;
+	host->io_pos = 0;
+	host->io_word = 0;
+	host->cmd_flags = 0;
+
+	data = host->req->data;
+
+	host->use_dma = !no_dma;
+
+	if (host->req->long_data) {
+		data_len = host->req->sg.length;
+		if (!is_power_of_2(data_len))
+			host->use_dma = 0;
+	} else {
+		data_len = host->req->data_len;
+		host->use_dma = 0;
+	}
+
+	writel(TIFM_FIFO_INT_SETALL,
+	       sock->addr + SOCK_DMA_FIFO_INT_ENABLE_CLEAR);
+	writel(TIFM_FIFO_ENABLE,
+	       sock->addr + SOCK_FIFO_CONTROL);
+
+	if (host->use_dma) {
+		if (1 != tifm_map_sg(sock, &host->req->sg, 1,
+				     host->req->data_dir == READ
+				     ? PCI_DMA_FROMDEVICE
+				     : PCI_DMA_TODEVICE)) {
+			host->req->error = -ENOMEM;
+			return host->req->error;
+		}
+		data_len = sg_dma_len(&host->req->sg);
+
+		writel(ilog2(data_len) - 2,
+		       sock->addr + SOCK_FIFO_PAGE_SIZE);
+		writel(TIFM_FIFO_INTMASK,
+		       sock->addr + SOCK_DMA_FIFO_INT_ENABLE_SET);
+		sys_param = TIFM_DMA_EN | (1 << 8);
+		if (host->req->data_dir == WRITE)
+			sys_param |= TIFM_DMA_TX;
+
+		writel(TIFM_FIFO_INTMASK,
+		       sock->addr + SOCK_DMA_FIFO_INT_ENABLE_SET);
+
+		writel(sg_dma_address(&host->req->sg),
+		       sock->addr + SOCK_DMA_ADDRESS);
+		writel(sys_param, sock->addr + SOCK_DMA_CONTROL);
+	} else {
+		writel(host->mode_mask | TIFM_MS_SYS_FIFO,
+		       sock->addr + SOCK_MS_SYSTEM);
+
+		writel(TIFM_FIFO_MORE,
+		       sock->addr + SOCK_DMA_FIFO_INT_ENABLE_SET);
+	}
+
+	mod_timer(&host->timer, jiffies + host->timeout_jiffies);
+	writel(TIFM_CTRL_LED | readl(sock->addr + SOCK_CONTROL),
+	       sock->addr + SOCK_CONTROL);
+	host->req->error = 0;
+
+	sys_param = readl(sock->addr + SOCK_MS_SYSTEM);
+	sys_param |= TIFM_MS_SYS_INTCLR;
+
+	if (host->use_dma)
+		sys_param |= TIFM_MS_SYS_DMA;
+	else
+		sys_param &= ~TIFM_MS_SYS_DMA;
+
+	writel(sys_param, sock->addr + SOCK_MS_SYSTEM);
+
+	cmd = (host->req->tpc & 0xf) << 12;
+	cmd |= data_len;
+	writel(cmd, sock->addr + SOCK_MS_COMMAND);
+
+	dev_dbg(&sock->dev, "executing TPC %x, %x\n", cmd, sys_param);
+	return 0;
+}
+
+static void tifm_ms_complete_cmd(struct tifm_ms *host)
+{
+	struct tifm_dev *sock = host->dev;
+	struct memstick_host *msh = tifm_get_drvdata(sock);
+	int rc;
+
+	del_timer(&host->timer);
+
+	host->req->int_reg = readl(sock->addr + SOCK_MS_STATUS) & 0xff;
+	host->req->int_reg = (host->req->int_reg & 1)
+			     | ((host->req->int_reg << 4) & 0xe0);
+
+	writel(TIFM_FIFO_INT_SETALL,
+	       sock->addr + SOCK_DMA_FIFO_INT_ENABLE_CLEAR);
+	writel(TIFM_DMA_RESET, sock->addr + SOCK_DMA_CONTROL);
+
+	if (host->use_dma) {
+		tifm_unmap_sg(sock, &host->req->sg, 1,
+			      host->req->data_dir == READ
+			      ? PCI_DMA_FROMDEVICE
+			      : PCI_DMA_TODEVICE);
+	}
+
+	writel((~TIFM_CTRL_LED) & readl(sock->addr + SOCK_CONTROL),
+	       sock->addr + SOCK_CONTROL);
+
+	dev_dbg(&sock->dev, "TPC complete\n");
+	do {
+		rc = memstick_next_req(msh, &host->req);
+	} while (!rc && tifm_ms_issue_cmd(host));
+}
+
+static int tifm_ms_check_status(struct tifm_ms *host)
+{
+	if (!host->req->error) {
+		if (!(host->cmd_flags & CMD_READY))
+			return 1;
+		if (!(host->cmd_flags & FIFO_READY))
+			return 1;
+		if (host->req->need_card_int
+		    && !(host->cmd_flags & CARD_INT))
+			return 1;
+	}
+	return 0;
+}
+
+/* Called from interrupt handler */
+static void tifm_ms_data_event(struct tifm_dev *sock)
+{
+	struct tifm_ms *host;
+	unsigned int fifo_status = 0, host_status = 0;
+	int rc = 1;
+
+	spin_lock(&sock->lock);
+	host = memstick_priv((struct memstick_host *)tifm_get_drvdata(sock));
+	fifo_status = readl(sock->addr + SOCK_DMA_FIFO_STATUS);
+	host_status = readl(sock->addr + SOCK_MS_STATUS);
+	dev_dbg(&sock->dev,
+		"data event: fifo_status %x, host_status %x, flags %x\n",
+		fifo_status, host_status, host->cmd_flags);
+
+	if (host->req) {
+		if (host->use_dma && (fifo_status & 1)) {
+			host->cmd_flags |= FIFO_READY;
+			rc = tifm_ms_check_status(host);
+		}
+		if (!host->use_dma && (fifo_status & TIFM_FIFO_MORE)) {
+			if (!tifm_ms_transfer_data(host)) {
+				host->cmd_flags |= FIFO_READY;
+				rc = tifm_ms_check_status(host);
+			}
+		}
+	}
+
+	writel(fifo_status, sock->addr + SOCK_DMA_FIFO_STATUS);
+	if (!rc)
+		tifm_ms_complete_cmd(host);
+
+	spin_unlock(&sock->lock);
+}
+
+
+/* Called from interrupt handler */
+static void tifm_ms_card_event(struct tifm_dev *sock)
+{
+	struct tifm_ms *host;
+	unsigned int host_status = 0;
+	int rc = 1;
+
+	spin_lock(&sock->lock);
+	host = memstick_priv((struct memstick_host *)tifm_get_drvdata(sock));
+	host_status = readl(sock->addr + SOCK_MS_STATUS);
+	dev_dbg(&sock->dev, "host event: host_status %x, flags %x\n",
+		host_status, host->cmd_flags);
+
+	if (host->req) {
+		if (host_status & TIFM_MS_STAT_TOE)
+			host->req->error = -ETIME;
+		else if (host_status & TIFM_MS_STAT_CRC)
+			host->req->error = -EILSEQ;
+
+		if (host_status & TIFM_MS_STAT_RDY)
+			host->cmd_flags |= CMD_READY;
+
+		if (host_status & TIFM_MS_STAT_MSINT)
+			host->cmd_flags |= CARD_INT;
+
+		rc = tifm_ms_check_status(host);
+
+	}
+
+	writel(TIFM_MS_SYS_INTCLR | readl(sock->addr + SOCK_MS_SYSTEM),
+	       sock->addr + SOCK_MS_SYSTEM);
+
+	if (!rc)
+		tifm_ms_complete_cmd(host);
+
+	spin_unlock(&sock->lock);
+	return;
+}
+
+static void tifm_ms_req_tasklet(unsigned long data)
+{
+	struct memstick_host *msh = (struct memstick_host *)data;
+	struct tifm_ms *host = memstick_priv(msh);
+	struct tifm_dev *sock = host->dev;
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&sock->lock, flags);
+	if (!host->req) {
+		if (host->eject) {
+			do {
+				rc = memstick_next_req(msh, &host->req);
+				if (!rc)
+					host->req->error = -ETIME;
+			} while (!rc);
+			spin_unlock_irqrestore(&sock->lock, flags);
+			return;
+		}
+
+		do {
+			rc = memstick_next_req(msh, &host->req);
+		} while (!rc && tifm_ms_issue_cmd(host));
+	}
+	spin_unlock_irqrestore(&sock->lock, flags);
+}
+
+static void tifm_ms_dummy_submit(struct memstick_host *msh)
+{
+	return;
+}
+
+static void tifm_ms_submit_req(struct memstick_host *msh)
+{
+	struct tifm_ms *host = memstick_priv(msh);
+
+	tasklet_schedule(&host->notify);
+}
+
+static int tifm_ms_set_param(struct memstick_host *msh,
+			     enum memstick_param param,
+			     int value)
+{
+	struct tifm_ms *host = memstick_priv(msh);
+	struct tifm_dev *sock = host->dev;
+
+	switch (param) {
+	case MEMSTICK_POWER:
+		/* also affected by media detection mechanism */
+		if (value == MEMSTICK_POWER_ON) {
+			host->mode_mask = TIFM_MS_SYS_SRAC | TIFM_MS_SYS_REI;
+			writel(TIFM_MS_SYS_RESET, sock->addr + SOCK_MS_SYSTEM);
+			writel(TIFM_MS_SYS_FCLR | TIFM_MS_SYS_INTCLR,
+			       sock->addr + SOCK_MS_SYSTEM);
+			writel(0xffffffff, sock->addr + SOCK_MS_STATUS);
+		} else if (value == MEMSTICK_POWER_OFF) {
+			writel(TIFM_MS_SYS_FCLR | TIFM_MS_SYS_INTCLR,
+			       sock->addr + SOCK_MS_SYSTEM);
+			writel(0xffffffff, sock->addr + SOCK_MS_STATUS);
+		} else
+			return -EINVAL;
+		break;
+	case MEMSTICK_INTERFACE:
+		if (value == MEMSTICK_SERIAL) {
+			host->mode_mask = TIFM_MS_SYS_SRAC | TIFM_MS_SYS_REI;
+			writel((~TIFM_CTRL_FAST_CLK)
+			       & readl(sock->addr + SOCK_CONTROL),
+			       sock->addr + SOCK_CONTROL);
+		} else if (value == MEMSTICK_PAR4) {
+			host->mode_mask = 0;
+			writel(TIFM_CTRL_FAST_CLK
+			       | readl(sock->addr + SOCK_CONTROL),
+			       sock->addr + SOCK_CONTROL);
+		} else
+			return -EINVAL;
+		break;
+	};
+
+	return 0;
+}
+
+static void tifm_ms_abort(unsigned long data)
+{
+	struct tifm_ms *host = (struct tifm_ms *)data;
+
+	dev_dbg(&host->dev->dev, "status %x\n",
+		readl(host->dev->addr + SOCK_MS_STATUS));
+	printk(KERN_ERR
+	       "%s : card failed to respond for a long period of time "
+	       "(%x, %x)\n",
+	       dev_name(&host->dev->dev), host->req ? host->req->tpc : 0,
+	       host->cmd_flags);
+
+	tifm_eject(host->dev);
+}
+
+static int tifm_ms_probe(struct tifm_dev *sock)
+{
+	struct memstick_host *msh;
+	struct tifm_ms *host;
+	int rc = -EIO;
+
+	if (!(TIFM_SOCK_STATE_OCCUPIED
+	      & readl(sock->addr + SOCK_PRESENT_STATE))) {
+		printk(KERN_WARNING "%s : card gone, unexpectedly\n",
+		       dev_name(&sock->dev));
+		return rc;
+	}
+
+	msh = memstick_alloc_host(sizeof(struct tifm_ms), &sock->dev);
+	if (!msh)
+		return -ENOMEM;
+
+	host = memstick_priv(msh);
+	tifm_set_drvdata(sock, msh);
+	host->dev = sock;
+	host->timeout_jiffies = msecs_to_jiffies(1000);
+
+	setup_timer(&host->timer, tifm_ms_abort, (unsigned long)host);
+	tasklet_init(&host->notify, tifm_ms_req_tasklet, (unsigned long)msh);
+
+	msh->request = tifm_ms_submit_req;
+	msh->set_param = tifm_ms_set_param;
+	sock->card_event = tifm_ms_card_event;
+	sock->data_event = tifm_ms_data_event;
+	if (tifm_has_ms_pif(sock))
+		msh->caps |= MEMSTICK_CAP_PAR4;
+
+	rc = memstick_add_host(msh);
+	if (!rc)
+		return 0;
+
+	memstick_free_host(msh);
+	return rc;
+}
+
+static void tifm_ms_remove(struct tifm_dev *sock)
+{
+	struct memstick_host *msh = tifm_get_drvdata(sock);
+	struct tifm_ms *host = memstick_priv(msh);
+	int rc = 0;
+	unsigned long flags;
+
+	msh->request = tifm_ms_dummy_submit;
+	tasklet_kill(&host->notify);
+	spin_lock_irqsave(&sock->lock, flags);
+	host->eject = 1;
+	if (host->req) {
+		del_timer(&host->timer);
+		writel(TIFM_FIFO_INT_SETALL,
+		       sock->addr + SOCK_DMA_FIFO_INT_ENABLE_CLEAR);
+		writel(TIFM_DMA_RESET, sock->addr + SOCK_DMA_CONTROL);
+		if (host->use_dma)
+			tifm_unmap_sg(sock, &host->req->sg, 1,
+				      host->req->data_dir == READ
+				      ? PCI_DMA_TODEVICE
+				      : PCI_DMA_FROMDEVICE);
+		host->req->error = -ETIME;
+
+		do {
+			rc = memstick_next_req(msh, &host->req);
+			if (!rc)
+				host->req->error = -ETIME;
+		} while (!rc);
+	}
+	spin_unlock_irqrestore(&sock->lock, flags);
+
+	memstick_remove_host(msh);
+	memstick_free_host(msh);
+}
+
+#ifdef CONFIG_PM
+
+static int tifm_ms_suspend(struct tifm_dev *sock, pm_message_t state)
+{
+	struct memstick_host *msh = tifm_get_drvdata(sock);
+
+	memstick_suspend_host(msh);
+	return 0;
+}
+
+static int tifm_ms_resume(struct tifm_dev *sock)
+{
+	struct memstick_host *msh = tifm_get_drvdata(sock);
+
+	memstick_resume_host(msh);
+	return 0;
+}
+
+#else
+
+#define tifm_ms_suspend NULL
+#define tifm_ms_resume NULL
+
+#endif /* CONFIG_PM */
+
+static struct tifm_device_id tifm_ms_id_tbl[] = {
+	{ TIFM_TYPE_MS }, { 0 }
+};
+
+static struct tifm_driver tifm_ms_driver = {
+	.driver = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE
+	},
+	.id_table = tifm_ms_id_tbl,
+	.probe    = tifm_ms_probe,
+	.remove   = tifm_ms_remove,
+	.suspend  = tifm_ms_suspend,
+	.resume   = tifm_ms_resume
+};
+
+static int __init tifm_ms_init(void)
+{
+	return tifm_register_driver(&tifm_ms_driver);
+}
+
+static void __exit tifm_ms_exit(void)
+{
+	tifm_unregister_driver(&tifm_ms_driver);
+}
+
+MODULE_AUTHOR("Alex Dubov");
+MODULE_DESCRIPTION("TI FlashMedia MemoryStick driver");
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(tifm, tifm_ms_id_tbl);
+
+module_init(tifm_ms_init);
+module_exit(tifm_ms_exit);