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