#define DRIVER_NAME "ms_block"
#define pr_fmt(fmt) DRIVER_NAME ": " fmt
#include <linux/module.h>
#include <linux/blk-mq.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;
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;
}
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;
}
static int msb_get_zone_from_lba(int lba)
{
if (lba < 494)
return 0;
return ((lba - 494) / 496) + 1;
}
static int msb_get_zone_from_pba(int pba)
{
return pba / MS_BLOCKS_IN_ZONE;
}
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;
}
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;
__set_bit(pba, msb->used_blocks_bitmap);
msb->free_block_count[zone]--;
}
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;
__clear_bit(pba, msb->used_blocks_bitmap);
msb->free_block_count[zone]++;
}
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;
}
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;
}
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;
if (error)
msb_invalidate_reg_window(msb);
complete(&msb->card->mrq_complete);
return -ENXIO;
}
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;
}
}
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;
}
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;
}
static int h_msb_default_bad(struct memstick_dev *card,
struct memstick_request **mrq)
{
return -ENXIO;
}
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:
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 (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:
if (!msb_read_regs(msb,
offsetof(struct ms_register, status),
sizeof(struct ms_status_register)))
return 0;
msb->state = MSB_RP_RECEIVE_STATUS_REG;
return 0;
case MSB_RP_RECEIVE_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:
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();
}
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) {
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;
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 (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;
}
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;
}
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) {
case MSB_SC_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_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();
}
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();
}
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:
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:
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);
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;
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;
}
if (was_parallel)
msb_switch_to_parallel(msb);
return 0;
}
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;
}
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);
}
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;
}
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;
}
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;
}
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;
}
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;
}
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);
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;
}
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 randomly: %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: can't 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;
}
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_objs(struct ms_boot_page, 2);
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("management 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't 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;
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;
}
}
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 = bitmap_zalloc(msb->block_count, GFP_KERNEL);
msb->erased_blocks_bitmap = bitmap_zalloc(msb->block_count, GFP_KERNEL);
msb->lba_to_pba_table =
kmalloc_array(msb->logical_block_count, sizeof(u16),
GFP_KERNEL);
if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
!msb->erased_blocks_bitmap) {
bitmap_free(msb->used_blocks_bitmap);
bitmap_free(msb->erased_blocks_bitmap);
kfree(msb->lba_to_pba_table);
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, management_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);
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);
management_flag = extra.management_flag;
overwrite_flag = extra.overwrite_flag;
overwrite_flags[pba] = overwrite_flag;
if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
dbg("pba %05d -> [BAD]", pba);
msb_mark_block_used(msb, pba);
continue;
}
if ((management_flag & MEMSTICK_MANAGEMENT_FLAG_NORMAL) !=
MEMSTICK_MANAGEMENT_FLAG_NORMAL) {
dbg("pba %05d -> [reserved management flag %02x]",
pba, management_flag);
msb_mark_block_used(msb, pba);
continue;
}
if (!(management_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);
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;
}
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(struct timer_list *t)
{
struct msb_data *msb = timer_container_of(msb, t, cache_flush_timer);
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;
timer_delete_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)
{
timer_setup(&msb->cache_flush_timer, msb_cache_flush_timer, 0);
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);
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);
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);
}
error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
pba = msb->lba_to_pba_table[msb->cache_block_lba];
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 (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;
}
static const struct chs_entry chs_table[] = {
{ 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 }
};
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;
if (host->caps & MEMSTICK_CAP_PAR4)
msb_switch_to_parallel(msb);
msb->page_size = sizeof(struct ms_boot_page);
error = msb_read_boot_blocks(msb);
if (error)
return -EIO;
boot_block = &msb->boot_page[0];
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 ((size_t)msb->page_size > PAGE_SIZE) {
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
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;
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;
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;
struct scatterlist *sg = msb->prealloc_sg;
struct request *req;
dbg_verbose("IO: work started");
while (1) {
spin_lock_irq(&msb->q_lock);
if (msb->need_flush_cache) {
msb->need_flush_cache = false;
spin_unlock_irq(&msb->q_lock);
msb_cache_flush(msb);
continue;
}
req = msb->req;
if (!req) {
dbg_verbose("IO: no more requests exiting");
spin_unlock_irq(&msb->q_lock);
return;
}
spin_unlock_irq(&msb->q_lock);
dbg_verbose("IO: processing new request");
blk_rq_map_sg(req, sg);
lba = blk_rq_pos(req);
sector_div(lba, msb->page_size / 512);
page = sector_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(req), &len);
else
error = msb_do_write_request(msb, lba, page, sg,
blk_rq_bytes(req), &len);
if (len && !blk_update_request(req, BLK_STS_OK, len)) {
__blk_mq_end_request(req, BLK_STS_OK);
spin_lock_irq(&msb->q_lock);
msb->req = NULL;
spin_unlock_irq(&msb->q_lock);
}
if (error && msb->req) {
blk_status_t ret = errno_to_blk_status(error);
dbg_verbose("IO: ending one sector of the request with error");
blk_mq_end_request(req, ret);
spin_lock_irq(&msb->q_lock);
msb->req = NULL;
spin_unlock_irq(&msb->q_lock);
}
if (msb->req)
dbg_verbose("IO: request still pending");
}
}
static DEFINE_IDR(msb_disk_idr);
static DEFINE_MUTEX(msb_disk_lock);
static void msb_data_clear(struct msb_data *msb)
{
kfree(msb->boot_page);
bitmap_free(msb->used_blocks_bitmap);
bitmap_free(msb->erased_blocks_bitmap);
kfree(msb->lba_to_pba_table);
kfree(msb->cache);
msb->card = NULL;
}
static int msb_bd_getgeo(struct gendisk *disk,
struct hd_geometry *geo)
{
struct msb_data *msb = disk->private_data;
*geo = msb->geometry;
return 0;
}
static void msb_bd_free_disk(struct gendisk *disk)
{
struct msb_data *msb = disk->private_data;
mutex_lock(&msb_disk_lock);
idr_remove(&msb_disk_idr, msb->disk_id);
mutex_unlock(&msb_disk_lock);
kfree(msb);
}
static blk_status_t msb_queue_rq(struct blk_mq_hw_ctx *hctx,
const struct blk_mq_queue_data *bd)
{
struct memstick_dev *card = hctx->queue->queuedata;
struct msb_data *msb = memstick_get_drvdata(card);
struct request *req = bd->rq;
dbg_verbose("Submit request");
spin_lock_irq(&msb->q_lock);
if (msb->card_dead) {
dbg("Refusing requests on removed card");
WARN_ON(!msb->io_queue_stopped);
spin_unlock_irq(&msb->q_lock);
blk_mq_start_request(req);
return BLK_STS_IOERR;
}
if (msb->req) {
spin_unlock_irq(&msb->q_lock);
return BLK_STS_DEV_RESOURCE;
}
blk_mq_start_request(req);
msb->req = req;
if (!msb->io_queue_stopped)
queue_work(msb->io_queue, &msb->io_work);
spin_unlock_irq(&msb->q_lock);
return BLK_STS_OK;
}
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");
blk_mq_stop_hw_queues(msb->queue);
spin_lock_irqsave(&msb->q_lock, flags);
msb->io_queue_stopped = true;
spin_unlock_irqrestore(&msb->q_lock, flags);
timer_delete_sync(&msb->cache_flush_timer);
flush_workqueue(msb->io_queue);
spin_lock_irqsave(&msb->q_lock, flags);
if (msb->req) {
blk_mq_requeue_request(msb->req, false);
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);
msb->need_flush_cache = true;
msb->io_queue_stopped = false;
blk_mq_start_hw_queues(msb->queue);
queue_work(msb->io_queue, &msb->io_work);
}
static const struct block_device_operations msb_bdops = {
.owner = THIS_MODULE,
.getgeo = msb_bd_getgeo,
.free_disk = msb_bd_free_disk,
};
static const struct blk_mq_ops msb_mq_ops = {
.queue_rq = msb_queue_rq,
};
static int msb_init_disk(struct memstick_dev *card)
{
struct msb_data *msb = memstick_get_drvdata(card);
struct queue_limits lim = {
.logical_block_size = msb->page_size,
.max_hw_sectors = MS_BLOCK_MAX_PAGES,
.max_segments = MS_BLOCK_MAX_SEGS,
.max_segment_size = MS_BLOCK_MAX_PAGES * msb->page_size,
};
int rc;
unsigned long capacity;
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;
rc = blk_mq_alloc_sq_tag_set(&msb->tag_set, &msb_mq_ops, 2, 0);
if (rc)
goto out_release_id;
msb->disk = blk_mq_alloc_disk(&msb->tag_set, &lim, card);
if (IS_ERR(msb->disk)) {
rc = PTR_ERR(msb->disk);
goto out_free_tag_set;
}
msb->queue = msb->disk->queue;
sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
msb->disk->fops = &msb_bdops;
msb->disk->private_data = msb;
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->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
if (!msb->io_queue) {
rc = -ENOMEM;
goto out_cleanup_disk;
}
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);
rc = device_add_disk(&card->dev, msb->disk, NULL);
if (rc)
goto out_destroy_workqueue;
dbg("Disk added");
return 0;
out_destroy_workqueue:
destroy_workqueue(msb->io_queue);
out_cleanup_disk:
put_disk(msb->disk);
out_free_tag_set:
blk_mq_free_tag_set(&msb->tag_set);
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_obj(struct msb_data);
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");
spin_lock_irqsave(&msb->q_lock, flags);
msb->card_dead = true;
spin_unlock_irqrestore(&msb->q_lock, flags);
blk_mq_start_hw_queues(msb->queue);
del_gendisk(msb->disk);
blk_mq_free_tag_set(&msb->tag_set);
msb->queue = NULL;
mutex_lock(&msb_disk_lock);
msb_data_clear(msb);
mutex_unlock(&msb_disk_lock);
put_disk(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_obj(struct msb_data);
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 ||
!bitmap_equal(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
msb->block_count))
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
static const struct memstick_device_id msb_id_tbl[] = {
{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
MEMSTICK_CLASS_FLASH},
{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
MEMSTICK_CLASS_ROM},
{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
MEMSTICK_CLASS_RO},
{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
MEMSTICK_CLASS_WP},
{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
MEMSTICK_CLASS_DUO},
{}
};
MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
static struct memstick_driver msb_driver = {
.driver = {
.name = DRIVER_NAME,
.owner = THIS_MODULE
},
.id_table = msb_id_tbl,
.probe = msb_probe,
.remove = msb_remove,
.suspend = msb_suspend,
.resume = msb_resume
};
static int __init msb_init(void)
{
int rc = memstick_register_driver(&msb_driver);
if (rc)
pr_err("failed to register memstick driver (error %d)\n", rc);
return rc;
}
static void __exit msb_exit(void)
{
memstick_unregister_driver(&msb_driver);
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");
