/*
 *  Sony MemoryStick Pro storage 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/version.h>
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0)
#include <linux/module.h>
#endif
#include <linux/blkdev.h>
#include <linux/idr.h>
#include <linux/hdreg.h>
#include <linux/delay.h>
#include <linux/version.h>
#include "linux/memstick.h"
#include "linux/ver.h"
#include "linux/blk_compat.h"

#define DRIVER_NAME "mspro_block"

static int major;
module_param(major, int, 0644);

#define MSPRO_BLOCK_MAX_SEGS  32
#define MSPRO_BLOCK_MAX_PAGES ((2 << 16) - 1)

#define MSPRO_BLOCK_SIGNATURE        0xa5c3
#define MSPRO_BLOCK_MAX_ATTRIBUTES   41

#define MSPRO_BLOCK_PART_SHIFT 3

enum {
	MSPRO_BLOCK_ID_SYSINFO         = 0x10,
	MSPRO_BLOCK_ID_MODELNAME       = 0x15,
	MSPRO_BLOCK_ID_MBR             = 0x20,
	MSPRO_BLOCK_ID_PBR16           = 0x21,
	MSPRO_BLOCK_ID_PBR32           = 0x22,
	MSPRO_BLOCK_ID_SPECFILEVALUES1 = 0x25,
	MSPRO_BLOCK_ID_SPECFILEVALUES2 = 0x26,
	MSPRO_BLOCK_ID_DEVINFO         = 0x30
};

struct mspro_sys_attr {
	size_t                  size;
	void                    *data;
	unsigned char           id;
	char                    name[32];
	struct device_attribute dev_attr;
};

struct mspro_attr_entry {
	unsigned int  address;
	unsigned int  size;
	unsigned char id;
	unsigned char reserved[3];
} __attribute__((packed));

struct mspro_attribute {
	unsigned short          signature;
	unsigned short          version;
	unsigned char           count;
	unsigned char           reserved[11];
	struct mspro_attr_entry entries[];
} __attribute__((packed));

struct mspro_sys_info {
	unsigned char  class;
	unsigned char  reserved0;
	unsigned short block_size;
	unsigned short block_count;
	unsigned short user_block_count;
	unsigned short page_size;
	unsigned char  reserved1[2];
	unsigned char  assembly_date[8];
	unsigned int   serial_number;
	unsigned char  assembly_maker_code;
	unsigned char  assembly_model_code[3];
	unsigned short memory_maker_code;
	unsigned short memory_model_code;
	unsigned char  reserved2[4];
	unsigned char  vcc;
	unsigned char  vpp;
	unsigned short controller_number;
	unsigned short controller_function;
	unsigned short start_sector;
	unsigned short unit_size;
	unsigned char  ms_sub_class;
	unsigned char  reserved3[4];
	unsigned char  interface_type;
	unsigned short controller_code;
	unsigned char  format_type;
	unsigned char  reserved4;
	unsigned char  device_type;
	unsigned char  reserved5[7];
	unsigned char  mspro_id[16];
	unsigned char  reserved6[16];
} __attribute__((packed));

struct mspro_mbr {
	unsigned char boot_partition;
	unsigned char start_head;
	unsigned char start_sector;
	unsigned char start_cylinder;
	unsigned char partition_type;
	unsigned char end_head;
	unsigned char end_sector;
	unsigned char end_cylinder;
	unsigned int  start_sectors;
	unsigned int  sectors_per_partition;
} __attribute__((packed));

struct mspro_specfile {
	char           name[8];
	char           ext[3];
	unsigned char  attr;
	unsigned char  reserved[10];
	unsigned short time;
	unsigned short date;
	unsigned short cluster;
	unsigned int   size;
} __attribute__((packed));

struct mspro_devinfo {
	unsigned short cylinders;
	unsigned short heads;
	unsigned short bytes_per_track;
	unsigned short bytes_per_sector;
	unsigned short sectors_per_track;
	unsigned char  reserved[6];
} __attribute__((packed));

struct mspro_block_data {
	struct memstick_dev   *card;
	unsigned int          usage_count;
	unsigned int          caps;
	struct gendisk        *disk;
	struct request_queue  *queue;
	struct request        *block_req;
	spinlock_t            q_lock;

	unsigned short        page_size;
	unsigned short        cylinders;
	unsigned short        heads;
	unsigned short        sectors_per_track;

	unsigned char         system;
	unsigned char         read_only:1,
			      eject:1,
			      has_request:1,
			      data_dir:1,
			      active:1;
	unsigned char         transfer_cmd;

	int                   (*mrq_handler)(struct memstick_dev *card,
					     struct memstick_request **mrq);

	struct attribute_group attr_group;

	struct scatterlist    req_sg[MSPRO_BLOCK_MAX_SEGS];
	unsigned int          seg_count;
	unsigned int          current_seg;
	unsigned int          current_page;

	struct work_struct    WorkMsV1;
	unsigned char         BootData[512*2];
	unsigned short        NumSegments;
	unsigned long         NumBlocks;
	unsigned long         NumPhysBlocks;
	unsigned long         BlockSize;
	unsigned short        PagePerBlock;
	unsigned long         NumSectors;

	unsigned long         BootBlk;
	unsigned long         BkBootBlk;
	unsigned short        FreeBlkNum[16];
	unsigned short        FreeBlkTbl[16*16];
	unsigned short        LogiPhyTbl[512*16];
};

static struct workqueue_struct *workqueuemsv1;
static void worker_for_msv1(struct work_struct *work);

int ms_mount_process(struct memstick_dev *card);
int ms_set_read_write_reg_addrs(struct memstick_dev *card, u8 rAdr, u8 rSize, u8 wAdr, u8 wSize);
int ms_read_register(struct memstick_dev *card, pu8 pData, u8 size);
int ms_write_register(struct memstick_dev *card, pu8 pData, u8 size);
int ms_set_command(struct memstick_dev *card, u8 MsCardCmd);
int ms_read_page(struct memstick_dev *card, pu8 pData, bool bNeedCardInt, struct scatterlist *psg);
int ms_card_read_register(struct memstick_dev *card, u8 adrs, u8 size, pu8 pData);

static DEFINE_IDR(mspro_block_disk_idr);
static DEFINE_MUTEX(mspro_block_disk_lock);

static int mspro_block_complete_req(struct memstick_dev *card, int error);

/*** Block device ***/

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
static int mspro_block_bd_open(struct block_device *bdev, fmode_t mode)
{
	struct gendisk *disk = bdev->bd_disk;
#else
static int mspro_block_bd_open(struct inode *inode, struct file *filp)
{
	struct gendisk *disk = inode->i_bdev->bd_disk;
#endif
	struct mspro_block_data *msb = disk->private_data;
	int rc = -ENXIO;

	mutex_lock(&mspro_block_disk_lock);

	if (msb && msb->card) {
		msb->usage_count++;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
		if ((mode & FMODE_WRITE) && msb->read_only)
#else
		if ((filp->f_mode & FMODE_WRITE) && msb->read_only)
#endif
			rc = -EROFS;
		else
			rc = 0;
	}

	mutex_unlock(&mspro_block_disk_lock);

	return rc;
}

static int mspro_block_disk_release(struct gendisk *disk)
{
	struct mspro_block_data *msb = disk->private_data;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
	int disk_id = MINOR(disk_devt(disk)) >> MSPRO_BLOCK_PART_SHIFT;
#else
	int disk_id = disk->first_minor >> MSPRO_BLOCK_PART_SHIFT;
#endif
	mutex_lock(&mspro_block_disk_lock);

	if (msb) {
		if (msb->usage_count)
			msb->usage_count--;

		if (!msb->usage_count) {
			kfree(msb);
			disk->private_data = NULL;
			idr_remove(&mspro_block_disk_idr, disk_id);
			put_disk(disk);
		}
	}

	mutex_unlock(&mspro_block_disk_lock);

	return 0;
}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
static int mspro_block_bd_release(struct gendisk *disk, fmode_t mode)
{
#else
static int mspro_block_bd_release(struct inode *inode, struct file *filp)
{
	struct gendisk *disk = inode->i_bdev->bd_disk;
#endif
	return mspro_block_disk_release(disk);
}

static int mspro_block_bd_getgeo(struct block_device *bdev,
				 struct hd_geometry *geo)
{
	struct mspro_block_data *msb = bdev->bd_disk->private_data;

	geo->heads = msb->heads;
	geo->sectors = msb->sectors_per_track;
	geo->cylinders = msb->cylinders;

	return 0;
}

static struct block_device_operations ms_block_bdops = {
	.open    = mspro_block_bd_open,
	.release = mspro_block_bd_release,
	.getgeo  = mspro_block_bd_getgeo,
	.owner   = THIS_MODULE
};

/*** Information ***/

static struct mspro_sys_attr *mspro_from_sysfs_attr(struct attribute *attr)
{
	struct device_attribute *dev_attr
		= container_of(attr, struct device_attribute, attr);
	return container_of(dev_attr, struct mspro_sys_attr, dev_attr);
}

static const char *mspro_block_attr_name(unsigned char tag)
{
	switch (tag) {
	case MSPRO_BLOCK_ID_SYSINFO:
		return "attr_sysinfo";
	case MSPRO_BLOCK_ID_MODELNAME:
		return "attr_modelname";
	case MSPRO_BLOCK_ID_MBR:
		return "attr_mbr";
	case MSPRO_BLOCK_ID_PBR16:
		return "attr_pbr16";
	case MSPRO_BLOCK_ID_PBR32:
		return "attr_pbr32";
	case MSPRO_BLOCK_ID_SPECFILEVALUES1:
		return "attr_specfilevalues1";
	case MSPRO_BLOCK_ID_SPECFILEVALUES2:
		return "attr_specfilevalues2";
	case MSPRO_BLOCK_ID_DEVINFO:
		return "attr_devinfo";
	default:
		return NULL;
	};
}

typedef ssize_t (*sysfs_show_t)(struct device *dev,
				struct device_attribute *attr,
				char *buffer);

static ssize_t mspro_block_attr_show_default(struct device *dev,
					     struct device_attribute *attr,
					     char *buffer)
{
	struct mspro_sys_attr *s_attr = container_of(attr,
						     struct mspro_sys_attr,
						     dev_attr);

	ssize_t cnt, rc = 0;

	for (cnt = 0; cnt < s_attr->size; cnt++) {
		if (cnt && !(cnt % 16)) {
			if (PAGE_SIZE - rc)
				buffer[rc++] = '\n';
		}

		rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "%02x ",
				((unsigned char *)s_attr->data)[cnt]);
	}
	return rc;
}

static ssize_t mspro_block_attr_show_sysinfo(struct device *dev,
					     struct device_attribute *attr,
					     char *buffer)
{
	struct mspro_sys_attr *x_attr = container_of(attr,
						     struct mspro_sys_attr,
						     dev_attr);
	struct mspro_sys_info *x_sys = x_attr->data;
	ssize_t rc = 0;
	int date_tz = 0, date_tz_f = 0;

	if (x_sys->assembly_date[0] > 0x80U) {
		date_tz = (~x_sys->assembly_date[0]) + 1;
		date_tz_f = date_tz & 3;
		date_tz >>= 2;
		date_tz = -date_tz;
		date_tz_f *= 15;
	} else if (x_sys->assembly_date[0] < 0x80U) {
		date_tz = x_sys->assembly_date[0];
		date_tz_f = date_tz & 3;
		date_tz >>= 2;
		date_tz_f *= 15;
	}

	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "class: %x\n",
			x_sys->class);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block size: %x\n",
			be16_to_cpu(x_sys->block_size));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block count: %x\n",
			be16_to_cpu(x_sys->block_count));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "user block count: %x\n",
			be16_to_cpu(x_sys->user_block_count));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "page size: %x\n",
			be16_to_cpu(x_sys->page_size));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly date: "
			"GMT%+d:%d %04u-%02u-%02u %02u:%02u:%02u\n",
			date_tz, date_tz_f,
			be16_to_cpu(*(unsigned short *)
				    &x_sys->assembly_date[1]),
			x_sys->assembly_date[3], x_sys->assembly_date[4],
			x_sys->assembly_date[5], x_sys->assembly_date[6],
			x_sys->assembly_date[7]);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "serial number: %x\n",
			be32_to_cpu(x_sys->serial_number));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
			"assembly maker code: %x\n",
			x_sys->assembly_maker_code);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly model code: "
			"%02x%02x%02x\n", x_sys->assembly_model_code[0],
			x_sys->assembly_model_code[1],
			x_sys->assembly_model_code[2]);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory maker code: %x\n",
			be16_to_cpu(x_sys->memory_maker_code));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory model code: %x\n",
			be16_to_cpu(x_sys->memory_model_code));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vcc: %x\n",
			x_sys->vcc);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vpp: %x\n",
			x_sys->vpp);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller number: %x\n",
			be16_to_cpu(x_sys->controller_number));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
			"controller function: %x\n",
			be16_to_cpu(x_sys->controller_function));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
			be16_to_cpu(x_sys->start_sector));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "unit size: %x\n",
			be16_to_cpu(x_sys->unit_size));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sub class: %x\n",
			x_sys->ms_sub_class);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "interface type: %x\n",
			x_sys->interface_type);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller code: %x\n",
			be16_to_cpu(x_sys->controller_code));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "format type: %x\n",
			x_sys->format_type);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "device type: %x\n",
			x_sys->device_type);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "mspro id: %s\n",
			x_sys->mspro_id);
	return rc;
}

static ssize_t mspro_block_attr_show_modelname(struct device *dev,
					       struct device_attribute *attr,
					       char *buffer)
{
	struct mspro_sys_attr *s_attr = container_of(attr,
						     struct mspro_sys_attr,
						     dev_attr);

	return scnprintf(buffer, PAGE_SIZE, "%s", (char *)s_attr->data);
}

static ssize_t mspro_block_attr_show_mbr(struct device *dev,
					 struct device_attribute *attr,
					 char *buffer)
{
	struct mspro_sys_attr *x_attr = container_of(attr,
						     struct mspro_sys_attr,
						     dev_attr);
	struct mspro_mbr *x_mbr = x_attr->data;
	ssize_t rc = 0;

	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "boot partition: %x\n",
			x_mbr->boot_partition);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start head: %x\n",
			x_mbr->start_head);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
			x_mbr->start_sector);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cylinder: %x\n",
			x_mbr->start_cylinder);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "partition type: %x\n",
			x_mbr->partition_type);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end head: %x\n",
			x_mbr->end_head);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end sector: %x\n",
			x_mbr->end_sector);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end cylinder: %x\n",
			x_mbr->end_cylinder);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sectors: %x\n",
			x_mbr->start_sectors);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
			"sectors per partition: %x\n",
			x_mbr->sectors_per_partition);
	return rc;
}

static ssize_t mspro_block_attr_show_specfile(struct device *dev,
					      struct device_attribute *attr,
					      char *buffer)
{
	struct mspro_sys_attr *x_attr = container_of(attr,
						     struct mspro_sys_attr,
						     dev_attr);
	struct mspro_specfile *x_spfile = x_attr->data;
	char name[9], ext[4];
	ssize_t rc = 0;

	memcpy(name, x_spfile->name, 8);
	name[8] = 0;
	memcpy(ext, x_spfile->ext, 3);
	ext[3] = 0;

	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "name: %s\n", name);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "ext: %s\n", ext);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "attribute: %x\n",
			x_spfile->attr);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "time: %d:%d:%d\n",
			x_spfile->time >> 11,
			(x_spfile->time >> 5) & 0x3f,
			(x_spfile->time & 0x1f) * 2);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "date: %d-%d-%d\n",
			(x_spfile->date >> 9) + 1980,
			(x_spfile->date >> 5) & 0xf,
			x_spfile->date & 0x1f);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cluster: %x\n",
			x_spfile->cluster);
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "size: %x\n",
			x_spfile->size);
	return rc;
}

static ssize_t mspro_block_attr_show_devinfo(struct device *dev,
					     struct device_attribute *attr,
					     char *buffer)
{
	struct mspro_sys_attr *x_attr = container_of(attr,
						     struct mspro_sys_attr,
						     dev_attr);
	struct mspro_devinfo *x_devinfo = x_attr->data;
	ssize_t rc = 0;

	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "cylinders: %x\n",
			be16_to_cpu(x_devinfo->cylinders));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "heads: %x\n",
			be16_to_cpu(x_devinfo->heads));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per track: %x\n",
			be16_to_cpu(x_devinfo->bytes_per_track));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per sector: %x\n",
			be16_to_cpu(x_devinfo->bytes_per_sector));
	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sectors per track: %x\n",
			be16_to_cpu(x_devinfo->sectors_per_track));
	return rc;
}

static sysfs_show_t mspro_block_attr_show(unsigned char tag)
{
	switch (tag) {
	case MSPRO_BLOCK_ID_SYSINFO:
		return mspro_block_attr_show_sysinfo;
	case MSPRO_BLOCK_ID_MODELNAME:
		return mspro_block_attr_show_modelname;
	case MSPRO_BLOCK_ID_MBR:
		return mspro_block_attr_show_mbr;
	case MSPRO_BLOCK_ID_SPECFILEVALUES1:
	case MSPRO_BLOCK_ID_SPECFILEVALUES2:
		return mspro_block_attr_show_specfile;
	case MSPRO_BLOCK_ID_DEVINFO:
		return mspro_block_attr_show_devinfo;
	default:
		return mspro_block_attr_show_default;
	}
}

/*** Protocol handlers ***/

/*
 * 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_mspro_block_req_init(struct memstick_dev *card,
				  struct memstick_request **mrq)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	*mrq = &card->current_mrq;
	card->next_request = msb->mrq_handler;
	return 0;
}

static int h_mspro_block_default(struct memstick_dev *card,
				 struct memstick_request **mrq)
{
	return mspro_block_complete_req(card, (*mrq)->error);
}

static int h_mspro_block_default_bad(struct memstick_dev *card,
				     struct memstick_request **mrq)
{
	return -ENXIO;
}

static int h_mspro_block_get_ro(struct memstick_dev *card,
				struct memstick_request **mrq)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	if (!(*mrq)->error) {
		if ((*mrq)->data[offsetof(struct ms_status_register, status0)]
		    & MEMSTICK_STATUS0_WP)
			msb->read_only = 1;
		else
			msb->read_only = 0;
	}

	return mspro_block_complete_req(card, (*mrq)->error);
}

static int h_mspro_block_wait_for_ced(struct memstick_dev *card,
				      struct memstick_request **mrq)
{
	dev_dbg(&card->dev, "wait for ced: value %x\n", (*mrq)->data[0]);

	if (!(*mrq)->error) {
		if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR))
			(*mrq)->error = -EFAULT;
		else if (!((*mrq)->data[0] & MEMSTICK_INT_CED))
			return 0;
	}

	return mspro_block_complete_req(card, (*mrq)->error);
}

static int h_mspro_block_transfer_data(struct memstick_dev *card,
				       struct memstick_request **mrq)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	unsigned char t_val = 0;
	struct scatterlist t_sg = { 0 };
	size_t t_offset;

	if ((*mrq)->error)
		return mspro_block_complete_req(card, (*mrq)->error);

	switch ((*mrq)->tpc) {
	case MS_TPC_WRITE_REG:
		memstick_init_req(*mrq, MS_TPC_SET_CMD, &msb->transfer_cmd, 1);
		(*mrq)->need_card_int = 1;
		return 0;
	case MS_TPC_SET_CMD:
		t_val = (*mrq)->int_reg;
		memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
		if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT)
			goto has_int_reg;
		return 0;
	case MS_TPC_GET_INT:
		t_val = (*mrq)->data[0];
has_int_reg:
		if (t_val & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) {
			t_val = MSPRO_CMD_STOP;
			memstick_init_req(*mrq, MS_TPC_SET_CMD, &t_val, 1);
			card->next_request = h_mspro_block_default;
			return 0;
		}

		if (msb->current_page
		    == (msb->req_sg[msb->current_seg].length
			/ msb->page_size)) {
			msb->current_page = 0;
			msb->current_seg++;

			if (msb->current_seg == msb->seg_count) {
				if (t_val & MEMSTICK_INT_CED) {
					return mspro_block_complete_req(card,
									0);
				} else {
					card->next_request
						= h_mspro_block_wait_for_ced;
					memstick_init_req(*mrq, MS_TPC_GET_INT,
							  NULL, 1);
					return 0;
				}
			}
		}

		if (!(t_val & MEMSTICK_INT_BREQ)) {
			memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
			return 0;
		}

		t_offset = msb->req_sg[msb->current_seg].offset;
		t_offset += msb->current_page * msb->page_size;

		sg_set_page(&t_sg,
			    nth_page(sg_page(&(msb->req_sg[msb->current_seg])),
				     t_offset >> PAGE_SHIFT),
			    msb->page_size, offset_in_page(t_offset));

		memstick_init_req_sg(*mrq, msb->data_dir == READ
					   ? MS_TPC_READ_LONG_DATA
					   : MS_TPC_WRITE_LONG_DATA,
				     &t_sg);
		(*mrq)->need_card_int = 1;
		return 0;
	case MS_TPC_READ_LONG_DATA:
	case MS_TPC_WRITE_LONG_DATA:
		msb->current_page++;
		if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT) {
			t_val = (*mrq)->int_reg;
			goto has_int_reg;
		} else {
			memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
			return 0;
		}

	default:
		BUG();
	}
}

/*** Data transfer ***/

static int mspro_block_issue_req(struct memstick_dev *card, int chunk)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	sector_t t_sec;
	unsigned int count;
	struct mspro_param_register param;

try_again:
	while (chunk) {
		msb->current_page = 0;
		msb->current_seg = 0;
		msb->seg_count = blk_rq_map_sg(msb->block_req->q,
				       msb->block_req, msb->req_sg);

		if (!msb->seg_count) {
			chunk = __blk_end_request(msb->block_req, -ENOMEM,
		    			blk_rq_cur_bytes(msb->block_req));
			continue;
    		}

		t_sec = blk_rq_pos(msb->block_req) << 9;
		sector_div(t_sec, msb->page_size);

		count = blk_rq_sectors(msb->block_req) << 9;
		count /= msb->page_size;

		param.system = msb->system;
		param.data_count = cpu_to_be16(count);
		param.data_address = cpu_to_be32((uint32_t)t_sec);
		param.tpc_param = 0;

		msb->data_dir = rq_data_dir(msb->block_req);
		msb->transfer_cmd = msb->data_dir == READ
						? MSPRO_CMD_READ_DATA
						: MSPRO_CMD_WRITE_DATA;

		dev_dbg(&card->dev, "data transfer: cmd %x, "
			"lba %x, count %x\n", msb->transfer_cmd,
			be32_to_cpu(param.data_address), count);

		card->next_request = h_mspro_block_req_init;
		msb->mrq_handler = h_mspro_block_transfer_data;
		memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
					&param, sizeof(param));
		memstick_new_req(card->host);
		return 0;
	}

	dev_dbg(&card->dev, "elv_next\n");
	msb->block_req = blk_fetch_request(msb->queue);
	if (!msb->block_req) {
		dev_dbg(&card->dev, "issue end\n");
		return -EAGAIN;
	}

	dev_dbg(&card->dev, "trying again\n");
	chunk = 1;
	goto try_again;
}

static int mspro_block_complete_req(struct memstick_dev *card, int error)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int chunk, cnt;
	unsigned int t_len = 0;
	unsigned long flags;

	spin_lock_irqsave(&msb->q_lock, flags);
	dev_dbg(&card->dev, "complete %d, %d\n", msb->has_request ? 1 : 0, error);

	if ((msb->has_request) && (card->card_type & 0x0020))
	{
		/* Nothing to do - not really an error */
		if (error == -EAGAIN)
			error = 0;

		if (error || (card->current_mrq.tpc == MSPRO_CMD_STOP)) {
			if (msb->data_dir == READ) {
				for (cnt = 0; cnt < msb->current_seg; cnt++)
					t_len += msb->req_sg[cnt].length
						 / msb->page_size;

					if (msb->current_page)
						t_len += msb->current_page - 1;

					t_len *= msb->page_size;
			}
		} else
			t_len = blk_rq_sectors(msb->block_req) << 9;

		dev_dbg(&card->dev, "transferred %x (%d)\n", t_len, error);

		if (error && !t_len)
			t_len = blk_rq_cur_bytes(msb->block_req);

		chunk = __blk_end_request(msb->block_req, error, t_len);

		error = mspro_block_issue_req(card, chunk);

		if (!error)
			goto out;
		else
			msb->has_request = 0;
	} else {
		if (!error)
			error = -EAGAIN;
	}

	card->next_request = h_mspro_block_default_bad;
	complete_all(&card->mrq_complete);
out:
	spin_unlock_irqrestore(&msb->q_lock, flags);
	return error;
}

static void mspro_block_stop(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = 0;
	unsigned long flags;

	dev_dbg(&card->dev, "mspro_block_stop\n");

	dev_dbg(&card->dev, "flush ms workqueue");
	flush_workqueue(workqueuemsv1);

	while (1) {
		spin_lock_irqsave(&msb->q_lock, flags);
		if (!msb->has_request) {
			blk_stop_queue(msb->queue);
			rc = 1;
		}
		spin_unlock_irqrestore(&msb->q_lock, flags);

		if (rc)
			break;

		wait_for_completion(&card->mrq_complete);
	}
}

static void mspro_block_start(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	unsigned long flags;

	dev_dbg(&card->dev, "mspro_block_start\n");

	spin_lock_irqsave(&msb->q_lock, flags);
	blk_start_queue(msb->queue);
	spin_unlock_irqrestore(&msb->q_lock, flags);
}

static int mspro_block_prepare_req(struct request_queue *q, struct request *req)
{
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)	
	if ((req->cmd_type!=REQ_TYPE_FS) && (req->cmd_type!=REQ_TYPE_BLOCK_PC)) {
#else
	if (!blk_fs_request(req) && !blk_pc_request(req)) {
#endif
		blk_dump_rq_flags(req, "MSPro unsupported request");
		return BLKPREP_KILL;
	}

	req->cmd_flags |= REQ_DONTPREP;

	return BLKPREP_OK;
}

static void mspro_block_submit_req(struct request_queue *q)
{
	struct memstick_dev *card = q->queuedata;
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	struct request *req = NULL;

	if (msb->has_request)
		return;

	if (msb->eject) {
		while ((req = blk_fetch_request(q)) != NULL)
			end_queued_request(req, -ENODEV);

		return;
	}

	msb->has_request = 1;

	if (card->card_type & 0x0010) {
		queue_work(workqueuemsv1, &msb->WorkMsV1);
        	return;
	}

	if (mspro_block_issue_req(card, 0))
		msb->has_request = 0;
}

/*** Initialization ***/

static int mspro_block_wait_for_ced(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_wait_for_ced;
	memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);
	return card->current_mrq.error;
}

static int mspro_block_set_interface(struct memstick_dev *card,
				     unsigned char sys_reg)
{
	struct memstick_host *host = card->host;
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	struct mspro_param_register param = {
		.system = sys_reg,
		.data_count = 0,
		.data_address = 0,
		.tpc_param = 0
	};

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_SET_RW_REG_ADRS, &card->reg_addr, 4);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
			  sizeof(param));
	memstick_new_req(host);
	wait_for_completion(&card->mrq_complete);
	return card->current_mrq.error;
}

static int mspro_block_switch_interface(struct memstick_dev *card)
{
	struct memstick_host *host = card->host;
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = 0;

	rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR4);

	if (rc) {
		printk(KERN_WARNING
		       "%s: could not switch to 4-bit mode, error %d\n",
		       dev_name(&card->dev), rc);
		return 0;
	}

	msb->system = MEMSTICK_SYS_PAR4;
	host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
	printk(KERN_INFO "%s: switching to 4-bit parallel mode\n",
	       dev_name(&card->dev));
// disable 8-bit parallel mode
/*
	if (msb->caps & MEMSTICK_CAP_PAR8) {
		rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8);

		if (!rc) {
			msb->system = MEMSTICK_SYS_PAR8;
			host->set_param(host, MEMSTICK_INTERFACE,
					MEMSTICK_PAR8);
			printk(KERN_INFO
			       "%s: switching to 8-bit parallel mode\n",
			       dev_name(&card->dev);
		} else
			printk(KERN_WARNING
			       "%s: could not switch to 8-bit mode, error %d\n",
			       dev_name(&card->dev), rc);
	}
*/
	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);
	rc = card->current_mrq.error;

	if (rc) {
		printk(KERN_WARNING
		       "%s: interface error, trying to fall back to serial\n",
		       dev_name(&card->dev));
		msb->system = MEMSTICK_SYS_SERIAL;
		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
		msleep(10);
		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);

		rc = memstick_set_rw_addr(card);
		if (!rc)
			rc = mspro_block_set_interface(card, msb->system);
	}
	return rc;
}

/* Memory allocated for attributes by this function should be freed by
 * mspro_block_data_clear, no matter if the initialization process succeded
 * or failed.
 */
static int mspro_block_read_attributes(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	struct mspro_param_register param = {
		.system = msb->system,
		.data_count = cpu_to_be16(1),
		.data_address = 0,
		.tpc_param = 0
	};
	struct mspro_attribute *attr = NULL;
	struct mspro_sys_attr *s_attr = NULL;
	unsigned char *buffer = NULL;
	int cnt, rc, attr_count;
	unsigned int addr;
	unsigned short page_count;

	attr = kmalloc(msb->page_size, GFP_KERNEL);
	if (!attr)
		return -ENOMEM;

	sg_init_one(&msb->req_sg[0], attr, msb->page_size);
	msb->seg_count = 1;
	msb->current_seg = 0;
	msb->current_page = 0;
	msb->data_dir = READ;
	msb->transfer_cmd = MSPRO_CMD_READ_ATRB;

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_transfer_data;
	memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
			  sizeof(param));
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);
	if (card->current_mrq.error) {
		rc = card->current_mrq.error;
		goto out_free_attr;
	}

	if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) {
		printk(KERN_ERR "%s: unrecognized device signature %x\n",
		       dev_name(&card->dev), be16_to_cpu(attr->signature));
		rc = -ENODEV;
		goto out_free_attr;
	}

	if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) {
		printk(KERN_WARNING "%s: way too many attribute entries\n",
		       dev_name(&card->dev));
		attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES;
	} else
		attr_count = attr->count;

	msb->attr_group.attrs = kzalloc((attr_count + 1)
					* sizeof(struct attribute),
					GFP_KERNEL);
	if (!msb->attr_group.attrs) {
		rc = -ENOMEM;
		goto out_free_attr;
	}
	msb->attr_group.name = "media_attributes";

	buffer = kmalloc(msb->page_size, GFP_KERNEL);
	if (!buffer) {
		rc = -ENOMEM;
		goto out_free_attr;
	}
	memcpy(buffer, (char *)attr, msb->page_size);
	page_count = 1;

	for (cnt = 0; cnt < attr_count; ++cnt) {
		s_attr = kzalloc(sizeof(struct mspro_sys_attr), GFP_KERNEL);
		if (!s_attr) {
			rc = -ENOMEM;
			goto out_free_buffer;
		}

		msb->attr_group.attrs[cnt] = &s_attr->dev_attr.attr;
		addr = be32_to_cpu(attr->entries[cnt].address);
		rc = be32_to_cpu(attr->entries[cnt].size);
		dev_dbg(&card->dev, "adding attribute %d: id %x, address %x, "
			"size %x\n", cnt, attr->entries[cnt].id, addr, rc);
		s_attr->id = attr->entries[cnt].id;
		if (mspro_block_attr_name(s_attr->id))
			snprintf(s_attr->name, sizeof(s_attr->name), "%s",
				 mspro_block_attr_name(attr->entries[cnt].id));
		else
			snprintf(s_attr->name, sizeof(s_attr->name),
				 "attr_x%02x", attr->entries[cnt].id);

		s_attr->dev_attr.attr.name = s_attr->name;
		s_attr->dev_attr.attr.mode = S_IRUGO;
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,36)		
		s_attr->dev_attr.attr.owner = THIS_MODULE;
#endif
		s_attr->dev_attr.show = mspro_block_attr_show(s_attr->id);

		if (!rc)
			continue;

		s_attr->size = rc;
		s_attr->data = kmalloc(rc, GFP_KERNEL);
		if (!s_attr->data) {
			rc = -ENOMEM;
			goto out_free_buffer;
		}

		if (((addr / msb->page_size)
		     == be32_to_cpu(param.data_address))
		    && (((addr + rc - 1) / msb->page_size)
			== be32_to_cpu(param.data_address))) {
			memcpy(s_attr->data, buffer + addr % msb->page_size,
			       rc);
			continue;
		}

		if (page_count <= (rc / msb->page_size)) {
			kfree(buffer);
			page_count = (rc / msb->page_size) + 1;
			buffer = kmalloc(page_count * msb->page_size,
					 GFP_KERNEL);
			if (!buffer) {
				rc = -ENOMEM;
				goto out_free_attr;
			}
		}

		param.system = msb->system;
		param.data_count = cpu_to_be16((rc / msb->page_size) + 1);
		param.data_address = cpu_to_be32(addr / msb->page_size);
		param.tpc_param = 0;

		sg_init_one(&msb->req_sg[0], buffer,
			    be16_to_cpu(param.data_count) * msb->page_size);
		msb->seg_count = 1;
		msb->current_seg = 0;
		msb->current_page = 0;
		msb->data_dir = READ;
		msb->transfer_cmd = MSPRO_CMD_READ_ATRB;

		dev_dbg(&card->dev, "reading attribute pages %x, %x\n",
			be32_to_cpu(param.data_address),
			be16_to_cpu(param.data_count));

		card->next_request = h_mspro_block_req_init;
		msb->mrq_handler = h_mspro_block_transfer_data;
		memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
				  (char *)&param, sizeof(param));
		memstick_new_req(card->host);
		wait_for_completion(&card->mrq_complete);
		if (card->current_mrq.error) {
			rc = card->current_mrq.error;
			goto out_free_buffer;
		}

		memcpy(s_attr->data, buffer + addr % msb->page_size, rc);
	}

	rc = 0;
out_free_buffer:
	kfree(buffer);
out_free_attr:
	kfree(attr);
	return rc;
}

static int mspro_block_init_card(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	struct memstick_host *host = card->host;
	u8   tmpReg[8];
	int rc = 0;

	msb->system = MEMSTICK_SYS_SERIAL;
	card->reg_addr.r_offset = offsetof(struct mspro_register, status);
	card->reg_addr.r_length = sizeof(struct ms_status_register);
	card->reg_addr.w_offset = offsetof(struct mspro_register, param);
	card->reg_addr.w_length = sizeof(struct mspro_param_register);

	if (memstick_set_rw_addr(card))
		return -EIO;

	msb->caps = host->caps;

	dev_dbg(&card->dev, "IdReg: type= %02xh, category= %02xh, calss= %02xh, ifmode= %02xh\n",
		card->id.type, card->id.category, card->id.class, card->id.ifmode);

	msb->page_size = 0x200;

	if (card->card_type & 0x0010) {
		if ((card->id.type == 0xFF) && (card->id.category == 0xFF) &&
				((card->id.class >= 1) && (card->id.class <= 3)))
			msb->read_only = 1;

		rc = ms_card_read_register(card, 0, 8, tmpReg);
		if (rc) {
			dev_dbg(&card->dev, "ms_card_read_register Failed!\n");
		} else {
			dev_dbg(&card->dev, "Got Reg[0-8]: %02x %02x %02x %02x %02x %02x %02x %02x\n",
				tmpReg[0], tmpReg[1], tmpReg[2], tmpReg[3],
				tmpReg[4], tmpReg[5], tmpReg[6], tmpReg[7]);
			if (tmpReg[0x02] & MEMSTICK_STATUS0_WP)
				msb->read_only = 1;
		}

		dev_dbg(&card->dev, "card r/w status %d\n", msb->read_only ? 0 : 1);

		rc = ms_mount_process(card);

		return rc;

	} else {
		msleep(200);

		rc = mspro_block_wait_for_ced(card);
		if (rc)
			return rc;

		rc = mspro_block_switch_interface(card);
		if (rc)
			return rc;

		dev_dbg(&card->dev, "card activated\n");
		if (msb->system != MEMSTICK_SYS_SERIAL)
			msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;

		card->next_request = h_mspro_block_req_init;
		msb->mrq_handler = h_mspro_block_get_ro;
		memstick_init_req(&card->current_mrq, MS_TPC_READ_REG, NULL,
				  sizeof(struct ms_status_register));
		memstick_new_req(card->host);
		wait_for_completion(&card->mrq_complete);

		if (card->current_mrq.error)
			return card->current_mrq.error;

		dev_dbg(&card->dev, "card r/w status %d\n", msb->read_only ? 0 : 1);

		msb->page_size = 512;

		rc = mspro_block_read_attributes(card);
		if (rc)
			return rc;

		dev_dbg(&card->dev, "attributes loaded\n");
		return 0;
	}
}

static int mspro_block_init_disk(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	struct memstick_host *host = card->host;
	struct mspro_devinfo *dev_info = NULL;
	struct mspro_sys_info *sys_info = NULL;
	struct mspro_sys_attr *s_attr = NULL;
	int rc, disk_id;
	u64 limit = BLK_BOUNCE_HIGH;
	unsigned long capacity;

	if (host->dev.dma_mask && *(host->dev.dma_mask))
		limit = *(host->dev.dma_mask);

	if (card->card_type & 0x0010) {
		msb->page_size = 0x200;
		msb->sectors_per_track = 16;
		if (msb->BlockSize == 8192) {
			if (msb->NumSegments == 1) {
				msb->cylinders = 0xF7;
				msb->heads     = 2;
			} else {
				msb->cylinders = 0x1EF;
				msb->heads     = 2;
			}
		} else {
			switch(msb->NumSegments) {
				case 4:
					msb->cylinders = 0x3DF;
					msb->heads     = 4;
					break;
				case 8:
					msb->cylinders = 0x3DF;
					msb->heads     = 8;
					break;
				case 16:
					msb->cylinders = 0x3DF;
					msb->heads     = 16;
					break;

				case 2:
				default:
					msb->cylinders = 0x1EF;
					msb->heads     = 4;
					break;
			}
		}
	} else {
		for (rc = 0; msb->attr_group.attrs[rc]; ++rc) {
			s_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[rc]);

			if (s_attr->id == MSPRO_BLOCK_ID_DEVINFO)
			dev_info = s_attr->data;
			else if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO)
			sys_info = s_attr->data;
		}

		if (!dev_info || !sys_info)
		return -ENODEV;

		msb->cylinders = be16_to_cpu(dev_info->cylinders);
		msb->heads = be16_to_cpu(dev_info->heads);
		msb->sectors_per_track = be16_to_cpu(dev_info->sectors_per_track);

		msb->page_size = be16_to_cpu(sys_info->unit_size);
	}

	if (!idr_pre_get(&mspro_block_disk_idr, GFP_KERNEL))
		return -ENOMEM;

	mutex_lock(&mspro_block_disk_lock);
	rc = idr_get_new(&mspro_block_disk_idr, card, &disk_id);
	mutex_unlock(&mspro_block_disk_lock);

	if (rc)
		return rc;

	if ((disk_id << MSPRO_BLOCK_PART_SHIFT) > 255) {
		rc = -ENOSPC;
		goto out_release_id;
	}

	msb->disk = alloc_disk(1 << MSPRO_BLOCK_PART_SHIFT);
	if (!msb->disk) {
		rc = -ENOMEM;
		goto out_release_id;
	}

	msb->queue = blk_init_queue(mspro_block_submit_req, &msb->q_lock);
	if (!msb->queue) {
		rc = -ENOMEM;
		goto out_put_disk;
	}

	msb->queue->queuedata = card;
	blk_queue_prep_rq(msb->queue, mspro_block_prepare_req);

	blk_queue_bounce_limit(msb->queue, limit);
	blk_queue_max_sectors(msb->queue, MSPRO_BLOCK_MAX_PAGES);
	blk_queue_max_phys_segments(msb->queue, MSPRO_BLOCK_MAX_SEGS);
	blk_queue_max_hw_segments(msb->queue, MSPRO_BLOCK_MAX_SEGS);
	blk_queue_max_segment_size(msb->queue,
				   MSPRO_BLOCK_MAX_PAGES * msb->page_size);

	msb->disk->major = major;
	msb->disk->first_minor = disk_id << MSPRO_BLOCK_PART_SHIFT;
	msb->disk->fops = &ms_block_bdops;
	msb->usage_count = 1;
	msb->disk->private_data = msb;
	msb->disk->queue = msb->queue;
	msb->disk->driverfs_dev = &card->dev;

	sprintf(msb->disk->disk_name, "mspblk%d", disk_id);

	blk_queue_logical_block_size(msb->queue, msb->page_size);

	if (card->card_type & 0x0010) {
		capacity = msb->NumSectors;
	} else {
		capacity = be16_to_cpu(sys_info->user_block_count);
		capacity *= be16_to_cpu(sys_info->block_size);
		capacity *= msb->page_size >> 9;
	}

	set_capacity(msb->disk, capacity);
	dev_dbg(&card->dev, "capacity set %ld\n", capacity);

	add_disk(msb->disk);
	msb->active = 1;
	return 0;

out_put_disk:
	put_disk(msb->disk);
out_release_id:
	mutex_lock(&mspro_block_disk_lock);
	idr_remove(&mspro_block_disk_idr, disk_id);
	mutex_unlock(&mspro_block_disk_lock);
	return rc;
}

static void mspro_block_data_clear(struct mspro_block_data *msb)
{
	int cnt;
	struct mspro_sys_attr *s_attr;

	if (msb->attr_group.attrs) {
		for (cnt = 0; msb->attr_group.attrs[cnt]; ++cnt) {
			s_attr = mspro_from_sysfs_attr(msb->attr_group
							   .attrs[cnt]);
			kfree(s_attr->data);
			kfree(s_attr);
		}
		kfree(msb->attr_group.attrs);
	}

	msb->card = NULL;
}

static int mspro_block_check_card(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	dev_dbg(&card->dev, "mspro_block_check_card - msb->active= %d\n", msb->active);

	return (msb->active == 1);
}

static int mspro_block_probe(struct memstick_dev *card)
{
	struct mspro_block_data *msb;
	int rc = 0;

	msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
	if (!msb)
		return -ENOMEM;
	memstick_set_drvdata(card, msb);
	msb->card = card;
	spin_lock_init(&msb->q_lock);

	dev_dbg(&card->dev, "INIT_WORK!\n");
	INIT_WORK(&msb->WorkMsV1, worker_for_msv1);

	rc = mspro_block_init_card(card);

	if (rc)
		goto out_free;

	if (card->card_type & 0x0020) {
		rc = sysfs_create_group(&card->dev.kobj, &msb->attr_group);
		if (rc)
			goto out_free;
	}

	rc = mspro_block_init_disk(card);
	if (!rc) {
		card->check = mspro_block_check_card;
		card->stop = mspro_block_stop;
		card->start = mspro_block_start;
		dev_dbg(&card->dev, "mspro_block_probe done\n");
		mdelay(100);
		return 0;
	}

	if (card->card_type & 0x0020) {
		sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
	}

out_free:
	memstick_set_drvdata(card, NULL);
	mspro_block_data_clear(msb);
	kfree(msb);
	return rc;
}

static void mspro_block_remove(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	unsigned long flags;

	dev_dbg(&card->dev, "mspro block remove\n");

	flush_workqueue(workqueuemsv1);

	spin_lock_irqsave(&msb->q_lock, flags);
	msb->eject = 1;
	blk_start_queue(msb->queue);
	spin_unlock_irqrestore(&msb->q_lock, flags);

	del_gendisk(msb->disk);

	blk_cleanup_queue(msb->queue);
	msb->queue = NULL;

	if (card->card_type & 0x0020) {
		sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
	}

	mutex_lock(&mspro_block_disk_lock);
	mspro_block_data_clear(msb);
	mutex_unlock(&mspro_block_disk_lock);

	mspro_block_disk_release(msb->disk);
	memstick_set_drvdata(card, NULL);
}

#ifdef CONFIG_PM

static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	unsigned long flags;

	dev_dbg(&card->dev, "mspro block suspend\n");

	spin_lock_irqsave(&msb->q_lock, flags);
	blk_stop_queue(msb->queue);
	msb->active = 0;
	spin_unlock_irqrestore(&msb->q_lock, flags);

	return 0;
}

static int mspro_block_resume(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	unsigned long flags;
	int rc = 0;

#ifdef CONFIG_MEMSTICK_UNSAFE_RESUME

	struct mspro_block_data *new_msb;
	struct memstick_host *host = card->host;
	struct mspro_sys_attr *s_attr, *r_attr;
	unsigned char cnt;

	dev_dbg(&card->dev, "mspro block resume\n");

	mutex_lock(&host->lock);
	new_msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
	if (!new_msb) {
		rc = -ENOMEM;
		goto out_unlock;
	}

	new_msb->card = card;
	memstick_set_drvdata(card, new_msb);
	if (mspro_block_init_card(card))
		goto out_free;

	if (card->card_type & 0x0020) {
		for (cnt = 0; new_msb->attr_group.attrs[cnt]
				&& msb->attr_group.attrs[cnt]; ++cnt) {
			s_attr = mspro_from_sysfs_attr(new_msb->attr_group.attrs[cnt]);
			r_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[cnt]);

			if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO
					&& r_attr->id == s_attr->id) {
				if (memcmp(s_attr->data, r_attr->data, s_attr->size))
					break;

				msb->active = 1;
				break;
			}
		}
	} else {
		msb->active = 1;
	}

out_free:
	memstick_set_drvdata(card, msb);
	mspro_block_data_clear(new_msb);
	kfree(new_msb);
out_unlock:
	mutex_unlock(&host->lock);

#endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */

	spin_lock_irqsave(&msb->q_lock, flags);
	blk_start_queue(msb->queue);
	spin_unlock_irqrestore(&msb->q_lock, flags);
	return rc;
}

#else

#define mspro_block_suspend NULL
#define mspro_block_resume NULL

#endif /* CONFIG_PM */

int ms_set_read_write_reg_addrs(struct memstick_dev *card, u8 rAdr, u8 rSize, u8 wAdr, u8 wSize)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	u8 data[4];

	data[0] = rAdr;
	data[1] = rSize;
	data[2] = wAdr;
	data[3] = wSize;

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_SET_RW_REG_ADRS, data, 4);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);

	return card->current_mrq.error;
}

int ms_read_register(struct memstick_dev *card, pu8 pData, u8 size)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_READ_REG, pData, size);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);

	rc = card->current_mrq.error;
	if (rc == 0) {
		memcpy(pData, card->current_mrq.data, size);
	}

	return rc;
}

int ms_write_register(struct memstick_dev *card, pu8 pData, u8 size)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, pData, size);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);
	return card->current_mrq.error;
}

int ms_set_command(struct memstick_dev *card, u8 MsCardCmd)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	dev_dbg(&card->dev, "ms_set_command - Cmd= %02xh\n", MsCardCmd);

	msb->transfer_cmd = MsCardCmd;

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_SET_CMD, &MsCardCmd, 1);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);

	return card->current_mrq.error;
}

int ms_get_int(struct memstick_dev *card, pu8 pbIntReg)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	dev_dbg(&card->dev, "ms_get_int\n");

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, pbIntReg, 1);
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);

	return card->current_mrq.error;
}

int ms_read_page(struct memstick_dev *card, pu8 pData, bool bNeedCardInt, struct scatterlist *psg)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);

	int rc = -EFAULT;
	struct scatterlist t_sg = { 0 };
	struct scatterlist *psg_dma = NULL;
	pu8 pDmaBuf = NULL;

	dev_dbg(&card->dev, "ms_read_page\n");

	if (psg != NULL) {
		psg_dma = psg;
	} else {
		psg_dma = &t_sg;
		if (pData != NULL) {
			pDmaBuf = pData;
		} else {
			pDmaBuf = kmalloc(msb->page_size, GFP_KERNEL);
			if (!pDmaBuf)
				return -ENOMEM;
		}

		sg_init_one(&t_sg, pDmaBuf, msb->page_size);
	}

	dev_dbg(&card->dev, "ReadPage - sg offset= %04xh, length= %04xh\n", psg_dma->offset, psg_dma->length);

	if (card->card_type & 0x0020)
		bNeedCardInt = true;

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req_sg(&card->current_mrq, MS_TPC_READ_LONG_DATA, psg_dma);
	card->current_mrq.need_card_int = bNeedCardInt;
	memstick_new_req(card->host);

	wait_for_completion(&card->mrq_complete);

	rc = card->current_mrq.error;
	if ((psg == NULL) && (pData == NULL))
		kfree(pDmaBuf);
	if (rc)
		dev_dbg(&card->dev, "ms_read_page Failed!\n");

	return rc;
}

int ms_write_page(struct memstick_dev *card, pu8 pData, struct scatterlist *psg)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = -EFAULT;
	struct scatterlist t_sg = { 0 };
	struct scatterlist *psg_dma = NULL;

	dev_dbg(&card->dev, "ms_write_page\n");

	if (psg != NULL) {
		psg_dma = psg;
	} else {
		if (pData == NULL) {
			dev_dbg(&card->dev, "WritePage - pData and psg == NULL!\n");
			return -EINVAL;
		}

		psg_dma = &t_sg;
		sg_init_one(&t_sg, pData, msb->page_size);
	}

	dev_dbg(&card->dev, "WritePage - sg offset= %04xh, length= %04xh\n", psg_dma->offset, psg_dma->length);

	card->next_request = h_mspro_block_req_init;
	msb->mrq_handler = h_mspro_block_default;
	memstick_init_req_sg(&card->current_mrq, MS_TPC_WRITE_LONG_DATA, psg_dma);
	card->current_mrq.need_card_int = true;
	memstick_new_req(card->host);
	wait_for_completion(&card->mrq_complete);

	rc = card->current_mrq.error;
	if (rc)
		dev_dbg(&card->dev, "ms_write_page Failed!\n");

	return rc;
}

int ms_card_read_register(struct memstick_dev *card, u8 adrs, u8 size, pu8 pData)
{
	int rc = 0;
	u8 ActualSize;

	dev_dbg(&card->dev, "Card_ReadReg - adrs= %02x, size= %d\n", adrs, size);

	while (size > 0) {
		if (size > MS_MAX_PARAMETER_SIZE) {
			ActualSize = MS_MAX_PARAMETER_SIZE;
		} else {
			ActualSize = size;
		}

		dev_dbg(&card->dev, "Set Read Card Register Address...\n");

		rc = ms_set_read_write_reg_addrs(card, adrs, ActualSize, 0x10, 0);
		if (rc) {
			dev_dbg(&card->dev, "Set ReadReg Addr Failed!\n");
			return rc;
		}

		dev_dbg(&card->dev, "Read Card Register Value...\n");

		rc = ms_read_register(card, pData, ActualSize);
		if (rc) {
			dev_dbg(&card->dev, "Read Card Reg Failed!\n");
			return rc;
		}

		size  -= ActualSize;
		adrs  += ActualSize;
		pData += ActualSize;
	}

	return rc;
}

int ms_card_write_register(struct memstick_dev *card, u8 adrs, u8 size, pu8 pData)
{
	int rc = 0;
	u8 ActualSize;

	dev_dbg(&card->dev, "Card_WriteReg - adrs= %02x, size= %d\n", adrs, size);

	while (size > 0) {
		if (size > MS_MAX_PARAMETER_SIZE) {
			ActualSize = MS_MAX_PARAMETER_SIZE;
		} else {
			ActualSize = size;
		}

		dev_dbg(&card->dev, "Set Write Card Register Address...\n");

		rc = ms_set_read_write_reg_addrs(card, 0x02, 1, adrs, ActualSize);
		if (rc) {
			dev_dbg(&card->dev, "Set Write Reg Addr Failed!\n");
			return rc;
		}

		dev_dbg(&card->dev, "Write Card Register Value...\n");

		rc = ms_write_register(card, pData, ActualSize);
		if (rc) {
			dev_dbg(&card->dev, "Write Card Reg Failed!\n");
			return rc;
		}

		size  -= ActualSize;
		adrs  += ActualSize;
		pData += ActualSize;
	}

	return rc;
}

int ms_set_para_extra(struct memstick_dev *card, u32 pblk, u8 page, u8 atype, pu8 extradata, u8 mode)
{
	int rc = 0;
	u16 wdata;
	u8 size;
	u8 data[9];

	dev_dbg(&card->dev, "ms_set_para_extra - pblk= %08xh, page= %02xh, atype= %02xh, mode= %d\n",
		pblk, page, atype, mode);

	if (1 == mode) {
		if (MEMSTICK_CP_OVERWRITE != atype) {
			size    = 9;
			data[8] = extradata[3];
			data[7] = extradata[2];
		} else {
			size = 6;
		}

		data[6] = extradata[1];
		data[5] = extradata[0];
	} else {
		size = 5;
	}

	data[4] = page;
	data[3] = atype;

	wdata   = lword(pblk);
	data[2] = lbyte(wdata);
	data[1] = hbyte(wdata);
	data[0] = lbyte(hword(pblk));

	rc = ms_card_write_register(card, 0x11, size, data);

	return rc;
}


int ms_read_extra_data(struct memstick_dev *card, u32 pblk, u8 page, pu8 extradata)
{
	int rc;
	bool uc_flag;
	u8 stts1;

	dev_dbg(&card->dev, "ms_read_extra_data - pblk= %08xh, page= %02xh\n", pblk, page);

	rc = ms_set_para_extra(card, pblk, page, MEMSTICK_CP_EXTRA, NULL, 0);
	if (rc)
		return rc;

	uc_flag = false;

	rc = ms_set_command(card, MS_CMD_BLOCK_READ);
	if (rc) {
		if ((card->current_mrq.int_reg &
				(MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
			return rc;

		rc = ms_card_read_register(card, 0x03, 1, &stts1);
		if (rc)
			return rc;

		if (stts1 & (MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCFG)) {
			uc_flag = true;
		}
	}

	rc = ms_card_read_register(card, 0x16, 4, extradata);
	if (rc)
		return rc;

	if (uc_flag)
		return 0xC0000102;

	return 0;
}

int ms_over_write_extra_data(struct memstick_dev *card, u32 pblk, u8 page, pu8 extradata)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;

	if (msb->read_only) {
		dev_dbg(&card->dev, "ms_over_write_extra_data - Card is Read Only!!\n");
		return -EROFS;
	}

	rc = ms_set_para_extra(card, pblk, page, MEMSTICK_CP_OVERWRITE, extradata, 1);
	if (rc)
		return rc;

	rc = ms_set_command(card, MS_CMD_BLOCK_WRITE);
	if (rc) {
		if ((card->current_mrq.int_reg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
			return rc;
	}

	return 0;
}


int ms_write_extra_data(struct memstick_dev *card, u32 pblk, u8 page,pu8 extradata)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u8 extra[4];

	dev_dbg(&card->dev, "ms_write_extra_data - pblk= %08xh, page= %02xh\n", pblk, page);

	if (msb->read_only) {
		dev_dbg(&card->dev, "ms_write_extra_data - Card is Read Only!!\n");
		return -EROFS;
	}

	rc = ms_set_para_extra(card, pblk, page, MEMSTICK_CP_EXTRA, extradata, 1);
	if (rc)
		return rc;

	rc = ms_set_command(card, MS_CMD_BLOCK_WRITE);
	if (rc) {
		if ((card->current_mrq.int_reg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
			return rc;

		extra[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST));

		rc = ms_over_write_extra_data(card, pblk, 0, extra);
		if (rc)
			return rc;

		return -EACCES;
	}

	return 0;
}

int ms_erase_blk(struct memstick_dev *card, u32 pblk)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u8 extradata[4];

	if (msb->read_only) {
		dev_dbg(&card->dev, "ms_erase_blk - Card is Read Only!!\n");
		return -EROFS;
	}

	rc = ms_set_para_extra(card, pblk, 0, MEMSTICK_CP_PAGE, NULL, 0);
	if (rc)
		return rc;

	rc = ms_set_command(card, MS_CMD_BLOCK_ERASE);
	if (card) {
		if ((card->current_mrq.int_reg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
			return rc;

		extradata[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST));

		rc = ms_over_write_extra_data(card, pblk, 0, extradata);
		if (rc)
	            return rc;

		return 0xC0000104;
	}

	return 0;
}

int ms_card_change_ifmode(struct memstick_dev *card, u16 IfMode)
{
	struct memstick_host *host = card->host;
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u16 IfMode1;
	u8 sys_para;

	dev_dbg(&card->dev, "Card Change IfMode - IfMode = %x\n", IfMode);

	switch(IfMode) {
		case 1:
			IfMode1     = IfMode;
			sys_para    = MEMSTICK_SYS_BAMD;
			break;
		case 4:
			IfMode1     = 2;
			sys_para    = (MEMSTICK_SYS_BAMD | MEMSTICK_SYS_PAM);
			break;
		case 5:
			IfMode1     = 2;
			sys_para    = 0x00;
			break;
		default:
			IfMode1     = IfMode;
			sys_para    = 0x40;
			break;
	}

	rc = ms_card_write_register(card, 0x10, 1, &sys_para);
	if (rc) {
		dev_dbg(&card->dev, "Card_ChangeIfMode - Failed!\n");
		return rc;
	}

	if (IfMode1 == 2) {
		msb->system = MEMSTICK_SYS_PAR4;
		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
	} else if (IfMode1 == 3) {
		msb->system = MEMSTICK_SYS_PAR8;
		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR8);
	} else {
		msb->system = MEMSTICK_SYS_SERIAL;
		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
	}

	return 0;
}

int ms_read_boot_blk(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = -EFAULT;
	u32 pblk;
	pu8 rdata = NULL;
	u8 extradata[4];
	u8 stts1, ovflg, manaflg;

	dev_dbg(&card->dev, "ms_read_boot_blk\n");

	pblk = 0;
	msb->BootBlk = 0xFFFFFFFF;
	msb->BkBootBlk = 0xFFFFFFFF;
	rdata = msb->BootData;

	do {
		dev_dbg(&card->dev, "Check Physical Block %d\n", pblk);

		rc = ms_set_para_extra(card, pblk, 0, MEMSTICK_CP_PAGE, NULL, 0);
		if (rc)
			return rc;

		rc = ms_set_command(card, MS_CMD_BLOCK_READ);
		if (rc) {
			dev_dbg(&card->dev, "SetCmd - MS_CMD_BLOCK_READ failed! int_reg= %02xh\n",
				card->current_mrq.int_reg);

			if ((card->current_mrq.int_reg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
				return rc;

			rc = ms_card_read_register(card, 0x03, 1, &stts1);
			if (rc)
				return rc;

			if (stts1 & (MEMSTICK_STATUS1_UCDT | MEMSTICK_STATUS1_UCEX | MEMSTICK_STATUS1_UCFG)) {
				dev_dbg(&card->dev, "Uncorrectable Error!\n");
				rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
				if (rc)
					return rc;

				continue;
			}
		}

		rc = ms_card_read_register(card, 0x16, 4, extradata);
		if (rc)
			return rc;

		ovflg = extradata[0];
		manaflg = extradata[1];

		if ((!(ovflg & MEMSTICK_OVERWRITE_BKST)) ||
				(manaflg & MEMSTICK_MANAGEMENT_SYSFLG)) {

			rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
			if (rc)
				return rc;

			continue;
		}

		rc = ms_read_page(card, rdata, false, NULL);
		if (rc) {
			dev_dbg(&card->dev, "ms_read_page Failed!\n");
			return rc;
		}

		if ((0x00 != rdata[0]) || (0x01 != rdata[1])) {
			dev_dbg(&card->dev, "Invalid BootBlock ID at Block %d\n", pblk);
			continue;
		}

		if (msb->BootBlk == 0xFFFFFFFF) {
			msb->BootBlk   = pblk;
		} else {
			msb->BkBootBlk = pblk;
			return 0;
		}

	} while (pblk ++ < 11);

	if (msb->BootBlk == 0xFFFFFFFF) {
		return -EIO;
	} else {
		return 0;
	}
}

int ms_boot_blk_confirm(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = -1;
	u32 sadrs, data_size;
	u32 pblk, dblk_num, seg_num, page, ms_no;
	u32 blk_size, blk_num, effect_blk_num, dblk;
	pu8 btdt, dblkdt;
	u8 stts1;

	const static u16 def_blk_size[6] = {
		0x08, 0x08, 0x10, 0x10, 0x10, 0x10 };
	const static u16 def_blk_num[6] = {
		0x0200, 0x0400, 0x0400, 0x0800, 0x1000, 0x2000 };
	const static u16 def_effect_blk_num[6] = {
		0x01F0, 0x03E0, 0x03E0, 0x07C0, 0x0F80, 0x1F00 };

	dev_dbg(&card->dev, "ms_boot_blk_confirm\n");

	pblk = msb->BootBlk;
	btdt = msb->BootData;
	while (-1 != pblk) {

		if (btdt[0x1D3] == 0x01) {
			printk(KERN_INFO "%s: switching to 4-bit parallel mode\n"
				, dev_name(&card->dev));

			rc = ms_card_change_ifmode(card, 4);
			if (rc)
				return rc;
		}

		blk_size = mkword(btdt[0x1A2], btdt[0x1A3]);
		blk_num = mkword(btdt[0x1A4], btdt[0x1A5]);
		effect_blk_num = mkword(btdt[0x1A6], btdt[0x1A7]);
		seg_num = blk_num >> 9;

		ms_no = 0;

		while (seg_num >>= 1)
			ms_no ++;

		if (0x10 == blk_size)
			ms_no ++;

		if (5 < ms_no)
			return -EFAULT;

		if (def_blk_size[ms_no] != blk_size)
			return -EFAULT;

		if (def_blk_num[ms_no] != blk_num)
			return -EFAULT;

		if (def_effect_blk_num[ms_no] != effect_blk_num)
			return -EFAULT;

		if (0x01 != btdt[0x178])
			return -EFAULT;

		data_size = mklong(   mkword(btdt[0x176], btdt[0x177]),
					mkword(btdt[0x174], btdt[0x175]));

		if (0 == data_size)
			return -EFAULT;

		sadrs = mklong(   mkword(btdt[0x172], btdt[0x173]),
				    mkword(btdt[0x170], btdt[0x171]));

		if (0x1FF & sadrs)
			return -EFAULT;

		page = (sadrs >> 9) + 1;
		if ((blk_size << 1) <= page)
			return -EFAULT;

		rc = ms_set_para_extra(card, pblk, (u8)page, MEMSTICK_CP_PAGE, NULL, 0);
		if (rc)
			return rc;

		rc = ms_set_command(card, MS_CMD_BLOCK_READ);
		if (rc) {
			if ((card->current_mrq.int_reg &
				 (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) !=
				  MEMSTICK_INT_ERR)
				return rc;

			rc = ms_card_read_register(card, 0x03, 1, &stts1);
			if (rc)
				return rc;

			if (stts1 & (MEMSTICK_STATUS1_UCDT |
				     MEMSTICK_STATUS1_UCEX |
				     MEMSTICK_STATUS1_UCFG)) {

				rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
				if (rc)
					return rc;

				pblk           = msb->BkBootBlk;
				msb->BootBlk   = pblk;
				msb->BkBootBlk = -1;
				continue;
			}
		}

		dblkdt = &(btdt[512]);

		rc = ms_read_page(card, dblkdt, false, NULL);
		if (rc)
			return rc;

		dblk_num = 0;

		do {
			dblk = mkword(*dblkdt, *(dblkdt + 1));
			if (blk_num > dblk)
				dblk_num ++;

			dblkdt += 2;
		} while (0xFFFF != dblk);

		if ((data_size >> 1) != dblk_num)
			return -EFAULT;

		return 0;
	}

	return -EFAULT;
}

int ms_tbl_inittab(struct mspro_block_data *msb, u16 seg)
{
	u16 sseg, eseg, sladrs; 
	u32 size, i;
	pu16 pwTmp;

	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9)  - 1;
	if (seg > eseg)
		return -EINVAL;

	if (0 == seg) {
		sladrs = 0;
		size = 494;
	} else {
		sladrs = 494 + 496 * (seg - 1);
		size = 496;
	}

	pwTmp = &(msb->LogiPhyTbl[sladrs]);
	for(i=0 ; i<size ; i++)
		pwTmp[i] = 0xFFFF;

	sseg = 16 * seg;
	pwTmp = &(msb->FreeBlkTbl[sseg]);
	for(i=0 ; i<16 ; i++)
		pwTmp[i] = 0xFFFF;

	msb->FreeBlkNum[seg] = 0;

	return 0;
}

int ms_tbl_check_use_blk(struct mspro_block_data *msb, u32 pblk)
{
	u32 cnt, disblk, disblk_num;
	pu8 disblk_tbl;

	disblk_num = mklong(mkword(msb->BootData[0x176], msb->BootData[0x177]),
			    mkword(msb->BootData[0x174], msb->BootData[0x175]));
	disblk_tbl = &(msb->BootData[512]);

	for(cnt = 0; cnt < disblk_num; cnt += 2) {
		disblk = mkword(disblk_tbl[cnt], disblk_tbl[cnt + 1]);
		if (disblk == pblk)
			return -EFAULT;
	}

	return 0;
}

int ms_tbl_log2phy(struct mspro_block_data *msb, u16 seg, u16 ladrs, pu32 pblk)
{
	u16 eseg, ladrs_size, sladrs;

	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9)  - 1;
	if (seg > eseg)
		return -EINVAL;

	if (0 == seg) {
		sladrs = 0;
		ladrs_size = 494 - 1;
	} else {
		sladrs = 494 + (496 * (seg - 1));
		ladrs_size = 496 - 1;
	}

	if ((ladrs < sladrs) || (ladrs - sladrs > ladrs_size))
		return -EINVAL;

	*pblk = msb->LogiPhyTbl[ladrs];

	return 0;
}

int ms_tbl_update_log_phy_tbl(struct mspro_block_data *msb, u16 seg, u16 ladrs, u32 pblk)
{
	u32 spblk;
	u16 sladrs, eseg, ladrs_size;

	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9) - 1;
	if (seg > eseg)
		return -EINVAL;

	if (0 == seg) {
		sladrs = 0;
		ladrs_size = 494 - 1;
	} else {
		sladrs = 494 + 496 * (seg - 1);
		ladrs_size = 496 - 1;
	}

	if ((u16)ladrs_size < (u16)(ladrs - sladrs))
		return -EINVAL;

	spblk = seg * 512;
	if ((512 - 1) < (pblk - spblk))
		return -EINVAL;

	msb->LogiPhyTbl[ladrs] = (u16)pblk;

	return 0;
}

int ms_tbl_update_free_blk(struct mspro_block_data *msb, u16 seg, pu16 ftbl, u32 fn)
{
	u16 eseg;
	u32 fblk_num;
	pu16 fblk_tbl;
	u8 minfn;

	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9)  - 1;
	if (seg > eseg)
		return -EINVAL;

	fblk_num = msb->FreeBlkNum[seg];
	if (fn > (16 - fblk_num))
		return -EINVAL;

	if (eseg == seg) {
		minfn = 1;
	} else {
		minfn = 0;
	}

	if ((fblk_num + fn) <= minfn) {
		msb->read_only = 1;
		return 0;
	}

	fblk_tbl = &(msb->FreeBlkTbl[seg * 16 + fblk_num]);
	memcpy(fblk_tbl, ftbl, fn * sizeof(u16));

	msb->FreeBlkNum[seg] = (u16)(fblk_num + fn);

	return 0;
}


int ms_tbl_get_free_blk(struct mspro_block_data *msb, u16 *pblk, u16 seg)
{
	u16 eseg, fn;
	u16 *ftbl;
	u8 minfn;

	eseg = msb->NumSegments - 1;

	if (seg == eseg) {
		minfn = 1;
	} else {
		minfn = 0;
	}

	fn = msb->FreeBlkNum[seg];
	ftbl = &(msb->FreeBlkTbl[seg << 4]);
	if (minfn >= fn) {
		dev_dbg(&msb->card->dev, "ms_tbl_get_free_blk - Insufficient Free Block %u!\n", fn);
		msb->read_only = 1;
		return -EACCES;
	}

	*pblk = ftbl[0];
	fn --;

	memcpy(ftbl, ftbl+1, fn << 1);
	ftbl[fn] = 0xFFFF;
	msb->FreeBlkNum[seg] = fn;

	return 0;
}

int ms_gen_log_phy_tbl(struct memstick_dev *card, u16 seg, pu16 ftbl, pu32 fn)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u32 cur_pblk, bef_pblk, spblk, epblk;
	u32 fblk_num;
	u16 ladrs, eseg;
	u8 extradata[4];
	u8 ovflg, manaflg, pgst;

	dev_dbg(&card->dev, "ms_gen_log_phy_tbl - seg= %04xh\n", seg);

	if (0 == seg) {
		spblk = msb->BkBootBlk;
		if (spblk == 0xFFFFFFFF)
			spblk = msb->BootBlk;
		spblk ++;
		epblk = 512;
	} else {
		spblk = 512 * seg;
		epblk = spblk + 512;
	}

	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9)  - 1;

	rc = ms_tbl_inittab(msb, seg);
	if (rc)
		return rc;

	pgst = 0;
	fblk_num = *fn;
	for(cur_pblk = spblk; epblk > cur_pblk; cur_pblk ++) {
		rc = ms_tbl_check_use_blk(msb, cur_pblk);
		if (rc)
			continue;

		rc = ms_read_extra_data(card, cur_pblk, 0, extradata);
		ovflg   = extradata[0];
		if (rc) {
			if (rc != 0xC0000102)
				return rc;

			if (ovflg & MEMSTICK_OVERWRITE_BKST) {
				extradata[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST));
				rc = ms_over_write_extra_data(card,
						 cur_pblk, 0, extradata);
				if (rc) {
					if (rc != -EROFS)
						return rc;
				}
			}
			continue;
		}

		manaflg = extradata[1];
		pgst = (ovflg & (MEMSTICK_OVERWRITE_PGST0 |
				 MEMSTICK_OVERWRITE_PGST1)) >> 5;

		if (!(ovflg & MEMSTICK_OVERWRITE_BKST))
			continue;

		if (2 > (pgst - 1)) {
			extradata[0] = (u8)(~MEMSTICK_OVERWRITE_BKST);
 			rc = ms_over_write_extra_data(card,
						 cur_pblk, 0, extradata);
			if (rc) {
		                if (rc != -EROFS)
					return rc;
			}
			continue;
		}

		if (eseg == seg) {
			if (!(manaflg & MEMSTICK_MANAGEMENT_ATFLG)) {
				rc = ms_erase_blk(card, cur_pblk);
				if (rc) {
					if ((rc != -EROFS) && (rc != 0xC0000104))
						return rc;
				continue;
				}

				*ftbl ++ = (u16)cur_pblk;
				fblk_num ++;
				continue;
			}
		}

		ladrs = mkword(extradata[2], extradata[3]);

		rc = ms_tbl_log2phy(msb, seg, ladrs, &bef_pblk);
		if (rc) {
			rc = ms_erase_blk(card, cur_pblk);
			if (rc) {
				if ((rc != -EROFS) && (rc != 0xC0000104))
					return rc;
				continue;
			}

			*ftbl ++ = (u16)cur_pblk;
			fblk_num ++;
			continue;
		}

		if (0xFFFF == bef_pblk) {
			rc = ms_tbl_update_log_phy_tbl(msb, seg, ladrs, cur_pblk);
			if (rc)
				return rc;

			continue;
		}

		if (ovflg & MEMSTICK_OVERWRITE_UDST) {
			rc = ms_read_extra_data(card, bef_pblk, 0, extradata);
			if (rc) {
				if (rc != 0xC0000102)
					return rc;

				extradata[0] = (u8)(~MEMSTICK_OVERWRITE_BKST);

				rc = ms_over_write_extra_data(card,
						 bef_pblk, 0, extradata);
				if (rc) {
					if (rc != -EROFS)
						return rc;
				}

				rc = ms_tbl_update_log_phy_tbl(msb, seg,
							 ladrs, cur_pblk);
				if (rc)
					return rc;

				continue;
			}

			ovflg = extradata[0];
			if (!(ovflg & MEMSTICK_OVERWRITE_UDST)) {
				rc = ms_erase_blk(card, cur_pblk);
				if (rc) {
					if ((rc != -EROFS) && (rc != 0xC0000104))
						return rc;
					continue;
				}

				*ftbl ++ = (u16)cur_pblk;
				fblk_num ++;

				continue;
			}
		}

		rc = ms_tbl_update_log_phy_tbl(msb, seg, ladrs, cur_pblk);
		if (rc)
			return rc;

		rc = ms_erase_blk(card, bef_pblk);
		if (rc) {
			if ((rc != -EROFS) && (rc != 0xC0000104))
				return rc;
			continue;
		}

		*ftbl ++ = (u16)bef_pblk;
		fblk_num ++;
		continue;
	}

	*fn = fblk_num;

	return 0;
}


int ms_cp_book_blk(struct memstick_dev *card, u32 rpblk, u32 wpblk, u8 spage, u8 epage)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc, rc_br;
	u8 page, stts1, ovflg;
	bool uc_flg;
	u8 extra[4];
	u8 PageData[512];
	u8 bIntReg;

	if (rpblk == wpblk)
		return 0;

	if (spage > epage)
		return -EINVAL;

	if (msb->read_only)
		return -EROFS;

	uc_flg = false;
	page = spage;

	do {
		rc = ms_set_para_extra(card, rpblk, page, MEMSTICK_CP_PAGE,
				       NULL, 0);
		if (rc)
			return rc;

		rc_br = ms_set_command(card, MS_CMD_BLOCK_READ);
		bIntReg = card->current_mrq.int_reg;

		rc = ms_card_read_register(card, 0x16, 4, extra);
		if (rc)
			return rc;

		if (rc_br) {
			if ((bIntReg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
				return rc_br;
		}

		rc = ms_card_read_register(card, 0x03, 1, &stts1);
		if (rc)
			return rc;

		if (stts1 & (MEMSTICK_STATUS1_UCDT |
			     MEMSTICK_STATUS1_UCEX |
			     MEMSTICK_STATUS1_UCFG)) {
			ovflg = extra[0];
			if ((ovflg & MEMSTICK_OVERWRITE_PGST0) &&
			    (ovflg & MEMSTICK_OVERWRITE_PGST1)) {
				extra[0] = (u8)(~MEMSTICK_OVERWRITE_PGST0);

				rc = ms_over_write_extra_data(card, rpblk,
							      page, extra);
				if (rc)
					return rc;
			}

			extra[0] &= (u8)(~(MEMSTICK_OVERWRITE_PGST0 |
					   MEMSTICK_OVERWRITE_PGST1));
			uc_flg = true;
		}

		rc = ms_read_page(card, PageData, false, NULL);
		if (rc)
			return rc;

		rc = ms_write_page(card, PageData, NULL);
		if (rc)
			return rc;

		ovflg = extra[0];
		if (!(ovflg & MEMSTICK_OVERWRITE_PGST0) ||
		    !(ovflg & MEMSTICK_OVERWRITE_PGST1)) {
			if (ovflg & (u8)(MEMSTICK_OVERWRITE_PGST0 |
					 MEMSTICK_OVERWRITE_PGST1))
				uc_flg = true;

			extra[0] &= (u8)(~(MEMSTICK_OVERWRITE_PGST0 |
					   MEMSTICK_OVERWRITE_PGST1));
		}

		extra[0] |= (u8)(MEMSTICK_OVERWRITE_UDST |
				 MEMSTICK_OVERWRITE_BKST);

		rc = ms_set_para_extra(card, wpblk, page, MEMSTICK_CP_PAGE, extra, 1);
		if (rc)
			return rc;

		rc = ms_set_command(card, MS_CMD_BLOCK_WRITE);
		if (rc) {
			if ((card->current_mrq.int_reg &
			    (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) !=
							 MEMSTICK_INT_ERR) {
				if (card->current_mrq.int_reg & MEMSTICK_INT_CMDNAK)
					return rc;

				rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
				if (rc)
					return rc;

				return -EINVAL;
			}

			extra[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST));

			rc = ms_over_write_extra_data(card, wpblk, 0, extra);
			if (rc)
				return rc;

			return 0xC0000103;
		}
	} while (page ++ < epage);

	if (uc_flg == true)
		return 0xC0000102;

	return 0;
}

int ms_log_addrs_confirm(struct memstick_dev *card, u16 seg, pu16 ftbl, pu32 fn)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	int minfn, freeblk, fblk_num;
	u32 pblk;
	u16 eseg, ladrs, sladrs, eladrs;
	u8 extradata[4];

	fblk_num = *fn;
	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9)  - 1;
	if (eseg == seg) {
		minfn = 1;
	} else {
		minfn = 0;
	}

	if (0 == seg) {
		sladrs = 0;
		eladrs = 494;
	} else {
		sladrs = 494 + (496 * (seg - 1));
		eladrs = sladrs + 496;
	}

	if (msb->read_only)
		return 0;

	for(ladrs = sladrs; eladrs > ladrs; ladrs ++) {
		rc = ms_tbl_log2phy(msb, seg, ladrs, &pblk);
		if (rc)
			return rc;

		if (0xFFFF != pblk)
			continue;

		extradata[1] = 0xFF;
		extradata[2] = hbyte(ladrs);
		extradata[3] = lbyte(ladrs);
		while (minfn < fblk_num) {
			extradata[0]   = 0xF8;

			fblk_num --;
			freeblk = ftbl[fblk_num];
			rc = ms_write_extra_data(card, freeblk, 0, extradata);
			if (rc) {
				if (rc != -EACCES)
					return rc;
				continue;
			}

			pblk = freeblk;
			rc = ms_tbl_update_log_phy_tbl(msb, seg, ladrs, pblk);
			if (rc)
				return rc;
			break;
		}

		*fn = fblk_num;

		if (0xFFFF == pblk)
			return 0;
	}

	return 0;
}

int ms_complete_log_phy_tbl(struct memstick_dev *card, u16 start_seg, u16 end_seg)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = -1;
	u16 seg, eseg;
	u16 ftbl[512];
	u32 fn;

	dev_dbg(&card->dev,
		"ms_complete_log_phy_tbl - StartSeg= %d, EndSeg= %d\n",
		start_seg, end_seg);

	eseg = (mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]) >> 9)  - 1;

	if (end_seg > eseg)
		return -EFAULT;

	if (start_seg > end_seg)
		return -EFAULT;

	for(seg = start_seg; seg <= end_seg; seg ++) {
		fn = 0;

		rc = ms_gen_log_phy_tbl(card, seg, ftbl, &fn);
		if (rc)
			return rc;

		rc = ms_log_addrs_confirm(card, seg, ftbl, &fn);
		if (rc)
			return rc;

		rc = ms_tbl_update_free_blk(msb, seg, ftbl, fn);
		if (rc)
	            return rc;
	}

	return 0;
}

void ms_set_disk_prop(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	u16 blk_size, effect_blk_num, unit_size;

	blk_size       = mkword(msb->BootData[0x1A2], msb->BootData[0x1A3]);
	effect_blk_num = mkword(msb->BootData[0x1A6], msb->BootData[0x1A7]);

	if (card->card_type & 0x0020) {
		unit_size = mkword(msb->BootData[0x1CC], msb->BootData[0x1CD]);
	} else {
		unit_size = 1024;
	}

	if (card->card_type & 0x0010) {
		msb->NumBlocks = effect_blk_num - 2;
	} else {
		msb->NumBlocks = effect_blk_num;
	}

	msb->BlockSize     = blk_size * unit_size;

	msb->NumPhysBlocks = mkword(msb->BootData[0x1A4], msb->BootData[0x1A5]);
	msb->NumSegments   = (u16)(msb->NumPhysBlocks / 512);
	msb->PagePerBlock  = (u16)(msb->BlockSize / 0x200);
	msb->NumSectors    = msb->NumBlocks * msb->PagePerBlock;

	dev_dbg(&card->dev, "ms NumSectors= %luh, NumSegs= %huh, NumPhysBlks= %luh, BlockSize= %luh\n",
			msb->NumSectors, msb->NumSegments, msb->NumPhysBlocks, msb->BlockSize);
}

int ms_mount_process(struct memstick_dev *card)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = 0;
	u16 eseg;

	dev_dbg(&card->dev, "Start ms mounting process...\n");

	rc = ms_read_boot_blk(card);
	if (rc) {
		dev_dbg(&card->dev, "ms_read_boot_blk Failed!\n");
		return rc;
	} else {
		dev_dbg(&card->dev, "BootBlock at Block %lu\n", msb->BootBlk);
	}

	rc = ms_boot_blk_confirm(card);
	if (rc) {
		dev_dbg(&card->dev, "ms_boot_blk_confirm Failed!\n");
		return rc;
	}

	dev_dbg(&card->dev, "ms_boot_blk_confirm OK!\n");

	eseg = (mkword(msb->BootData[0x1A4],
				msb->BootData[0x1A5]) >> 9)  - 1;

	rc = ms_complete_log_phy_tbl(card, 0, eseg);
	if (rc == 0)
		dev_dbg(&card->dev, "ms_complete_log_phy_tbl OK!\n");

	ms_set_disk_prop(card);

	return rc;
}

int ms_read_blk(struct memstick_dev *card, u32 pblk, u8 spage, u8 epage, pu8 extradata)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc = -1;
	bool uc_flg, loop_flag;
	u8 page, intreg, stts1, ovflg, manaflg;
	u8 wextra[4];
	u32 t_offset;
	struct scatterlist t_sg = { 0 };

	if (spage > epage)
		return -EINVAL;

	rc = ms_set_para_extra(card, pblk, spage, MEMSTICK_CP_BLOCK, NULL, 0);
	if (rc)
		return rc;

	rc = ms_set_command(card, MS_CMD_BLOCK_READ);

	uc_flg = false;
	page = spage;
	loop_flag = true;

	do {
		if (msb->current_seg >= msb->seg_count) {
			dev_dbg(&card->dev,"ms_read_blk - current_seg > seg_count!\n");
			return -EFAULT;
		}

		t_offset = msb->req_sg[msb->current_seg].offset;
		t_offset += msb->current_page * msb->page_size;
		sg_set_page(&t_sg,
				nth_page(sg_page(&(msb->req_sg[msb->current_seg])), t_offset >> PAGE_SHIFT),
				msb->page_size,
				offset_in_page(t_offset));

		intreg = card->current_mrq.int_reg;
		if (rc) {
			if ((intreg & (MEMSTICK_INT_ERR |
				       MEMSTICK_INT_CMDNAK)) !=
						MEMSTICK_INT_ERR) {
				if (intreg & MEMSTICK_INT_CMDNAK)
					return rc;

				return -EINVAL;
			}

			rc = ms_card_read_register(card, 0x03, 1, &stts1);
			if (rc)
				return rc;

			if (stts1 & (MEMSTICK_STATUS1_UCDT |
				     MEMSTICK_STATUS1_UCEX |
				     MEMSTICK_STATUS1_UCFG)) {
				if (!(intreg & MEMSTICK_INT_CED)) {
					rc = ms_set_command(card, MS_CMD_BLOCK_END);
					if (rc) {
						if ((card->current_mrq.int_reg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR)
							return rc;
					}
				}

			rc = ms_card_read_register(card, 0x16, 4, extradata);
			if (rc)
				return rc;

			ovflg = extradata[0];
			if ((ovflg & MEMSTICK_OVERWRITE_PGST0) &&
			    (ovflg & MEMSTICK_OVERWRITE_PGST1)) {
				wextra[0] = (u8)(~MEMSTICK_OVERWRITE_PGST0);
				rc = ms_over_write_extra_data(card, pblk, page, wextra);
				if (rc) {
					if (rc != -EROFS)
						return rc;
				}
			}

			extradata[0] = (u8)(~(MEMSTICK_OVERWRITE_PGST0));
			uc_flg = true;

			rc = ms_read_page(card, NULL, false, &t_sg);
			if (rc)
				return rc;

			extradata += 4;

			msb->current_page++;
			if (msb->current_page == (msb->req_sg[msb->current_seg].length / msb->page_size)) {
					msb->current_page = 0;
					msb->current_seg++;
			}

			if (page == epage)
				break;

			page ++;

			rc = ms_set_para_extra(card, pblk, page, MEMSTICK_CP_BLOCK, NULL, 0);
			if (rc)
				return rc;

			rc = ms_set_command(card, MS_CMD_BLOCK_READ);

			continue;
			}
		}

		if (intreg & MEMSTICK_INT_CED) {
			loop_flag = false;
		} else {
			if (page == epage) {
				rc = ms_set_command(card, MS_CMD_BLOCK_END);
				if (rc) {
					if ((card->current_mrq.int_reg &
					    (MEMSTICK_INT_ERR |
					     MEMSTICK_INT_CMDNAK)) !=
							MEMSTICK_INT_ERR)
						return rc;
				}
				loop_flag = false;
			} else {
				page ++;
			}
		}

		rc = ms_card_read_register(card, 0x16, 4, extradata);
		if (rc)
			return rc;

		manaflg = extradata[1];
		if (!(manaflg & MEMSTICK_MANAGEMENT_SCMS0) ||
		    !(manaflg & MEMSTICK_MANAGEMENT_SCMS1)) {
			rc = ms_read_page(card, NULL, loop_flag, NULL);
		} else {
			rc = ms_read_page(card, NULL, loop_flag, &t_sg);
		}

		if (rc)
			return rc;

		extradata += 4;
		msb->current_page++;
		if (msb->current_page == (msb->req_sg[msb->current_seg].length / msb->page_size)) {
			msb->current_page = 0;
			msb->current_seg++;
		}
	} while (loop_flag);

	if (page != epage)
		return -EINVAL;

	if (uc_flg == true)
		return 0xC0000102;

	return 0;
}

int ms_read_lba(struct memstick_dev *card, u32 lba, u32 PageCount)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	long cnt;
	u32 pblk;
	u16 seg, ladrs, last_ladrs;
	u8 extradata[4 * 32];
	u8 *extra;
	u8 page, manaflg, ovflg;
	u8 spage, epage, last_page;
	u32 PagePerBlock;

	dev_dbg(&card->dev, "ms_read_lba - LBA= %08xh, PageCount= %d\n",
				lba, PageCount);

	PagePerBlock = msb->PagePerBlock;
	ladrs = (u16)(lba / PagePerBlock);
	spage = (u8)(lba % PagePerBlock);
	last_page = (u8)(PagePerBlock - 1);

	cnt = ladrs - 494 - 1;
	seg = 0;
	while (cnt > 0) {
		cnt -= 496;
		seg ++;
	}

	last_ladrs = 494 + seg * 496 - 1;

	while (PageCount > 0) {
		if (PagePerBlock < (spage + PageCount)) {
			epage = last_page;
		} else {
			epage = spage + (u8)(PageCount - 1);
		}

		if (last_ladrs < ladrs) {
			seg ++;
			last_ladrs += 496;
		}

		pblk = msb->LogiPhyTbl[ladrs];

		if (0xFFFF == pblk)
			return -EFAULT;

		rc = ms_read_blk(card, pblk, spage, epage, extradata);
		if (rc) {
			if (rc != 0xC0000102)
				return rc;

			return -EACCES;
		}

		extra = extradata;
		for(page = 0; epage >= spage; spage ++, page ++) {
			manaflg = extra[1];
			ovflg   = extra[0];

			if (!(ovflg & MEMSTICK_OVERWRITE_PGST0) ||
			    !(ovflg & MEMSTICK_OVERWRITE_PGST1))
				return -EACCES;

			if (!(manaflg & MEMSTICK_MANAGEMENT_SCMS0) ||
			    !(manaflg & MEMSTICK_MANAGEMENT_SCMS1))
				return -EACCES;

			extra += 4;
		}

		PageCount -= page;
		spage = 0;
		ladrs++;
	}

	return 0;
}

int ms_cp_blk(struct memstick_dev *card, u32 rpblk, u32 wpblk, u8 spage, u8 epage, pu8 pExtraData)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc, rc_br;
	u8 page, ps;
	bool uc_flg;
	u8 rextra[4], wextra[4];
	u8 stts1, ovflg, manaflg;
	u8 rextra0[4];
	u8 bIntReg;

	if (rpblk == wpblk)
		return 0;

	if (spage > epage)
		return -EINVAL;

	if (msb->read_only)
		return -EROFS;

	wextra[0] = 0xF8;
	wextra[1] = 0xFF;
	wextra[2] = pExtraData[2];
	wextra[3] = pExtraData[3];

	uc_flg = false;
	page   = spage;
	do {
		ps = 3;

		rc = ms_set_para_extra(card, rpblk, page, MEMSTICK_CP_PAGE, 0, 0);
		if (rc)
			return rc;

		rc_br = ms_set_command(card, MS_CMD_BLOCK_READ);
		bIntReg = card->current_mrq.int_reg;

		rc = ms_card_read_register(card, 0x16, 4, rextra);
		if (rc)
			return rc;

		if (rc_br) {
			if ((bIntReg & (MEMSTICK_INT_ERR |
					MEMSTICK_INT_CMDNAK)) !=
							MEMSTICK_INT_ERR) {
				if ((bIntReg & MEMSTICK_INT_CMDNAK) == 0)
					return rc_br;
			}

			return -EINVAL;
		}

		rc = ms_card_read_register(card, 0x03, 1, &stts1);
		if (rc)
			return rc;

		if (stts1 & (MEMSTICK_STATUS1_UCDT |
			     MEMSTICK_STATUS1_UCEX |
			     MEMSTICK_STATUS1_UCFG)) {
			ovflg = rextra[0];
			if ((ovflg & MEMSTICK_OVERWRITE_PGST0) &&
			    (ovflg & MEMSTICK_OVERWRITE_PGST1)) {
				rc = ms_set_para_extra(card, rpblk, 0,
						 MEMSTICK_CP_EXTRA, 0, 0);
				if (rc)
					return rc;

				rc = ms_set_command(card, MS_CMD_BLOCK_READ);
				if (rc)
					return rc;

				rc = ms_card_read_register(card, 0x16, 4, rextra0);
				if (rc)
					return rc;

				rextra0[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST |
						    MEMSTICK_OVERWRITE_UDST));

				rc = ms_over_write_extra_data(card, rpblk, 0, rextra0);
				if (rc)
					return rc;

				rextra[0] = (u8)(~MEMSTICK_OVERWRITE_PGST0);

				rc = ms_over_write_extra_data(card, rpblk, page, rextra);
				if (rc)
					return rc;
			}

			ps = 0;
			uc_flg = true;
		}

		ovflg = rextra[0];
		if (!(ovflg & MEMSTICK_OVERWRITE_PGST0) ||
		    !(ovflg & MEMSTICK_OVERWRITE_PGST1)) {
			if (ovflg & (u8)(MEMSTICK_OVERWRITE_PGST0 |
					 MEMSTICK_OVERWRITE_PGST1))
				uc_flg  = true;
			ps = 0;
		}

		if (ps) {
			wextra[0] |= (MEMSTICK_OVERWRITE_PGST0 |
				      MEMSTICK_OVERWRITE_PGST1);
		} else {
			wextra[0] &= (u8)(~(MEMSTICK_OVERWRITE_PGST0 |
					    MEMSTICK_OVERWRITE_PGST1));
		}

		manaflg = rextra[1];
		if (!(manaflg & MEMSTICK_MANAGEMENT_SCMS0) ||
		    !(manaflg & MEMSTICK_MANAGEMENT_SCMS1)) {
			rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
			if (rc)
				return rc;

			return -EACCES;
		}

		rc = ms_set_para_extra(card, wpblk, page, MEMSTICK_CP_PAGE, wextra, 1);
		if (rc)
			return rc;

		rc = ms_set_command(card, MS_CMD_BLOCK_WRITE);
		if (rc) {
			bIntReg = card->current_mrq.int_reg;
			if ((bIntReg & (MEMSTICK_INT_ERR|MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR) {
				if ((bIntReg & MEMSTICK_INT_CMDNAK) == 0)
					return rc;

				rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
				if (rc)
					return rc;

				return -EINVAL;
			}

			wextra[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST));
			rc = ms_over_write_extra_data(card, wpblk, 0, wextra);
			if (rc)
				return rc;

			return 0xC0000103;
		}
	} while (page ++ < epage);

	if (uc_flg == true)
		return 0xC0000102;

	return 0;
}

int ms_write_blk(struct memstick_dev *card, u16 pblk, u8 spage, u8 epage, pu8 extradata)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u8 page, intreg;
	bool loop_flag;
	u8 extra[4];
	u32 t_offset;
	struct scatterlist t_sg = { 0 };

	dev_dbg(&card->dev, "ms_write_blk - pblk= %04xh, spage= %02xh, epage= %02xh\n",
				pblk, spage, epage);

	if (spage > epage)
		return -EINVAL;

	if (msb->read_only)
		return -EROFS;

	rc = ms_set_para_extra(card, pblk, spage, MEMSTICK_CP_BLOCK, extradata, 1);
	if (rc)
		return rc;

	rc = ms_set_command(card, MS_CMD_BLOCK_WRITE);
	if (rc) {
		intreg = card->current_mrq.int_reg;
		if ((intreg & (MEMSTICK_INT_ERR |
			       MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR) {
			if ((intreg & MEMSTICK_INT_CMDNAK) == 0)
				return rc;

			return -EINVAL;
		}
	}

	page = spage;
	loop_flag = true;

	do {
		if (msb->current_seg >= msb->seg_count) {
			dev_dbg(&card->dev, "ms_write_blk - current_seg > seg_count!\n");
			return -EFAULT;
		}

		t_offset = msb->req_sg[msb->current_seg].offset;
		t_offset += msb->current_page * msb->page_size;
		sg_set_page(&t_sg,
				nth_page(sg_page(&(msb->req_sg[msb->current_seg])),
				t_offset >> PAGE_SHIFT),
				msb->page_size,
				offset_in_page(t_offset));

		rc = ms_write_page(card, NULL, &t_sg);
		if (rc) {
			intreg = card->current_mrq.int_reg;
			if ((intreg & (MEMSTICK_INT_ERR |
				       MEMSTICK_INT_CMDNAK)) != MEMSTICK_INT_ERR) {
				if ((intreg & MEMSTICK_INT_CMDNAK) == 0)
					return rc;
				return -EINVAL;
			}

			extra[0] = (u8)(~(MEMSTICK_OVERWRITE_BKST));

			rc = ms_over_write_extra_data(card, pblk, 0, extra);
			if (rc)
				return rc;

			rc = ms_set_command(card, MS_CMD_CLEAR_BUF);
			if (rc)
				return rc;

			return 0xC0000103;
		}

		msb->current_page++;
		if (msb->current_page == (msb->req_sg[msb->current_seg].length / msb->page_size)) {
			msb->current_page = 0;
			msb->current_seg++;
		}

		intreg = card->current_mrq.int_reg;
		if (intreg & MEMSTICK_INT_CED)
			break;

		if (page == epage) {
			rc = ms_set_command(card, MS_CMD_BLOCK_END);
			if (rc) {
				dev_dbg(&card->dev, "Set BlockEnd Failed! (%d)\n", rc);
				if ((card->current_mrq.int_reg &
				    MEMSTICK_INT_CMDNAK) == 0)
					return rc;

				return -EACCES;
			}

			loop_flag = false;
	        } else {
			page ++;
		}

	} while (loop_flag);

	if (page == epage) {
		return 0;
	} else {
		return -EINVAL;
	}
}

int ms_update_blk(struct memstick_dev *card, u16 ladrs, u8 spage, u8 epage)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u32 cnt;
	u16 seg, rpblk, wpblk;
	u8 end_page;
	bool PSErrFlg;
	u32 fn;
	u16 pblk;
	u8 extra[4];
	u8 ovflg;

	PSErrFlg = false;

	dev_dbg(&card->dev, "ms_update_blk ladrs= %04xh, spage= %02xh, epage= %02xh\n",
				ladrs, spage, epage);

	if (ladrs > 493) {
		cnt = ladrs - 493;
		seg = 1;
		while (cnt > 496) {
			cnt -= 496;
			seg ++;
		}
	} else {
		seg = 0;
	}

	rpblk = msb->LogiPhyTbl[ladrs];

	rc = ms_read_extra_data(card, rpblk, 0, extra);
	ovflg = extra[0];
	if (rc) {
		if (rc != 0xC0000102)
			return rc;

		if ((ovflg & MEMSTICK_OVERWRITE_PGST0) &&
		    (ovflg & MEMSTICK_OVERWRITE_PGST1)) {
			extra[0] = (u8)(~MEMSTICK_OVERWRITE_PGST0);
			rc = ms_over_write_extra_data(card, rpblk, 0, extra);
			if (rc)
				return rc;
		}
	        PSErrFlg = true;
	}

	if (ovflg & MEMSTICK_OVERWRITE_UDST) {
		extra[0] = (u8)(~(MEMSTICK_OVERWRITE_UDST));
		rc = ms_over_write_extra_data(card, rpblk, 0, extra);
		if (rc)
			return rc;
	}

	end_page = msb->PagePerBlock - 1;

	extra[1] = 0xFF;
	extra[2] = hbyte(ladrs);
	extra[3] = lbyte(ladrs);
	while (1) {
		rc = ms_tbl_get_free_blk(msb, &wpblk, seg);
		if (rc) {
			dev_dbg(&card->dev, "ms_tbl_get_free_blk failed! (%d)\n", rc);
			break;
		}

		extra[0] = 0xF8;

		if (spage > 0) {
			rc = ms_cp_blk(card, rpblk, wpblk, 0, spage - 1, extra);
			if (rc) {
				dev_dbg(&card->dev, "ms_cp_blk failed-1! (%d)\n", rc);
				if (rc != 0xC0000102) {
					if (rc != 0xC0000103)
						return rc;
					continue;
				}

				PSErrFlg = true;
			}
		}

		rc = ms_write_blk(card, wpblk, spage, epage, extra);
		if (rc) {
			dev_dbg(&card->dev, "ms_write_blk failed! (%d)\n", rc);
			if (rc != 0xC0000103)
				return rc;

			continue;
		}

		if (end_page != epage) {
			rc = ms_cp_blk(card, rpblk, wpblk, epage + 1, end_page, extra);
			if (rc) {
				dev_dbg(&card->dev, "ms_cp_blk failed-2! (%d)\n", rc);
				if (rc != 0xC0000102) {
					if (rc != 0xC0000103)
						return rc;

					continue;
				}
				PSErrFlg = true;
			}
		}

		rc = ms_tbl_update_log_phy_tbl(msb, seg, ladrs, wpblk);
		if (rc) {
			dev_dbg(&card->dev, "ms_tbl_update_log_phy_tbl failed! (%d)\n", rc);
			return rc;
		}

		if (!PSErrFlg) {
			rc = ms_erase_blk(card, rpblk);
			if (rc) {
				dev_dbg(&card->dev, "ms_erase_blk failed! (%d)\n", rc);
				if (rc != 0xC0000104)
					return rc;

				fn = 0;
			} else {
				fn = 1;
			}

			pblk = rpblk;
			rc = ms_tbl_update_free_blk(msb, seg, &pblk, fn);
			if (rc) {
				dev_dbg(&card->dev, "ms_tbl_update_free_blk failed! (%d)\n", rc);
				return rc;
			}
		} else {
			extra[0] = (u8)(~MEMSTICK_OVERWRITE_BKST);
			rc = ms_over_write_extra_data(card, rpblk, 0, extra);
			if (rc)
				return rc;
		}

		return 0;
	}

	return rc;
}

int ms_write_lba(struct memstick_dev *card, u32 lba, u32 PageCount)
{
	struct mspro_block_data *msb = memstick_get_drvdata(card);
	int rc;
	u32 spage, epage, last_page;
	u16 ladrs;
	u32 PagePerBlock;

	dev_dbg(&card->dev, "ms_write_lba - LBA= %08xh, PageCount= %d\n",
				lba, PageCount);

	if (msb->read_only)
		return -EROFS;

	PagePerBlock = msb->PagePerBlock;
	ladrs        = (u16)(lba / PagePerBlock);
	spage        = (u8)(lba % PagePerBlock);
	last_page    = (u8)PagePerBlock - 1;

	while (PageCount > 0) {
		if ((spage + PageCount) > PagePerBlock) {
			epage = last_page;
		} else {
			epage = spage + PageCount - 1;
		}

		rc = ms_update_blk(card, ladrs, (u8)spage, (u8)epage);
		if (rc) {
			dev_dbg(&card->dev, "ms_update_blk failed! (%d)\n", rc);
			return rc;
		}

		PageCount -= (epage - spage + 1);
		ladrs ++;
		spage = 0;
	}

	return 0;
}

static void worker_for_msv1(struct work_struct *work)
{
	struct mspro_block_data *msb = container_of(work, struct mspro_block_data,
							WorkMsV1);
	struct memstick_dev *card = msb->card;
	struct request *req;
	int chunk, rc;
	unsigned long flags;

	dev_dbg(&card->dev, "worker_for_msv1\n");
	msleep(1);

	while ((req = blk_fetch_request(msb->queue)) != NULL) {
		spin_lock_irqsave(&msb->q_lock, flags);
		msb->seg_count = blk_rq_map_sg(req->q, req, msb->req_sg);
		msb->current_seg = 0;
		msb->current_page = 0;
		if (msb->seg_count == 0) {
			chunk = __blk_end_request(msb->block_req, -ENOMEM, blk_rq_cur_bytes(msb->block_req));
	    		spin_unlock_irqrestore(&msb->q_lock, flags);

			continue;
		}
		spin_unlock_irqrestore(&msb->q_lock, flags);

		if (rq_data_dir(req) == READ) {
			rc = ms_read_lba(card, blk_rq_pos(req), blk_rq_sectors(req));
			if (rc)
				dev_dbg(&card->dev, "ms_read_lba failed! (%d)\n", rc);
		} else {
			rc = ms_write_lba(card, blk_rq_pos(req), blk_rq_sectors(req));
			if (rc)
				dev_dbg(&card->dev, "ms_write_lba failed! (%d)\n", rc);
		}

		spin_lock_irqsave(&msb->q_lock, flags);
		chunk = __blk_end_request(req, rc, blk_rq_bytes(req));
		spin_unlock_irqrestore(&msb->q_lock, flags);

		dev_dbg(&card->dev, "__blk_end_request - chunk = %d\n", chunk);
	}

	msb->has_request = 0;

	dev_dbg(&card->dev, "Exit worker_for_msv1\n");
}

static struct memstick_device_id mspro_block_id_tbl[] = {
	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_PRO, MEMSTICK_CATEGORY_STORAGE_DUO,
	 MEMSTICK_CLASS_DUO},
	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
	 MEMSTICK_CLASS_FLASH},
	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
	 MEMSTICK_CLASS_DUO},
	{}
};


static struct memstick_driver mspro_block_driver = {
	.driver = {
		.name  = DRIVER_NAME,
		.owner = THIS_MODULE
	},
	.id_table = mspro_block_id_tbl,
	.probe    = mspro_block_probe,
	.remove   = mspro_block_remove,
	.suspend  = mspro_block_suspend,
	.resume   = mspro_block_resume
};

static int __init mspro_block_init(void)
{
	int rc = -ENOMEM;

	workqueuemsv1 = create_workqueue("kmsv1block");
	if (!workqueuemsv1)
		return -ENOMEM;

	rc = register_blkdev(major, DRIVER_NAME);
	if (rc < 0) {
		printk(KERN_ERR DRIVER_NAME ": failed to register "
		       "major %d, error %d\n", major, rc);
		destroy_workqueue(workqueuemsv1);
		return rc;
	}
	if (!major)
		major = rc;

	rc = memstick_register_driver(&mspro_block_driver);
	if (rc)
	{
		unregister_blkdev(major, DRIVER_NAME);
		destroy_workqueue(workqueuemsv1);
	}
	return rc;
}

static void __exit mspro_block_exit(void)
{
	memstick_unregister_driver(&mspro_block_driver);
	unregister_blkdev(major, DRIVER_NAME);
	idr_destroy(&mspro_block_disk_idr);
	destroy_workqueue(workqueuemsv1);
}

module_init(mspro_block_init);
module_exit(mspro_block_exit);

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Alex Dubov");
MODULE_DESCRIPTION("Sony MemoryStickPro block device driver");
MODULE_VERSION(DRIVER_VERSION);
MODULE_DEVICE_TABLE(memstick, mspro_block_id_tbl);
