drivers/memstick/core/ms_block.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  *  ms_block.c - Sony MemoryStick (legacy) storage support
   4
   5  *  Copyright (C) 2013 Maxim Levitsky <maximlevitsky@gmail.com>
   6  *
   7  * Minor portions of the driver were copied from mspro_block.c which is
   8  * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
   9  */
  10 #define DRIVER_NAME "ms_block"
  11 #define pr_fmt(fmt) DRIVER_NAME ": " fmt
  12
  13 #include <linux/module.h>
  14 #include <linux/blk-mq.h>
  15 #include <linux/memstick.h>
  16 #include <linux/idr.h>
  17 #include <linux/hdreg.h>
  18 #include <linux/delay.h>
  19 #include <linux/slab.h>
  20 #include <linux/random.h>
  21 #include <linux/bitmap.h>
  22 #include <linux/scatterlist.h>
  23 #include <linux/jiffies.h>
  24 #include <linux/workqueue.h>
  25 #include <linux/mutex.h>
  26 #include "ms_block.h"
  27
  28 static int debug;
  29 static int cache_flush_timeout = 1000;
  30 static bool verify_writes;
  31
  32 /*
  33  * Copies section of 'sg_from' starting from offset 'offset' and with length
  34  * 'len' To another scatterlist of to_nents enties
  35  */
  36 static size_t msb_sg_copy(struct scatterlist *sg_from,
  37         struct scatterlist *sg_to, int to_nents, size_t offset, size_t len)
  38 {
  39         size_t copied = 0;
  40
  41         while (offset > 0) {
  42                 if (offset >= sg_from->length) {
  43                         if (sg_is_last(sg_from))
  44                                 return 0;
  45
  46                         offset -= sg_from->length;
  47                         sg_from = sg_next(sg_from);
  48                         continue;
  49                 }
  50
  51                 copied = min(len, sg_from->length - offset);
  52                 sg_set_page(sg_to, sg_page(sg_from),
  53                         copied, sg_from->offset + offset);
  54
  55                 len -= copied;
  56                 offset = 0;
  57
  58                 if (sg_is_last(sg_from) || !len)
  59                         goto out;
  60
  61                 sg_to = sg_next(sg_to);
  62                 to_nents--;
  63                 sg_from = sg_next(sg_from);
  64         }
  65
  66         while (len > sg_from->length && to_nents--) {
  67                 len -= sg_from->length;
  68                 copied += sg_from->length;
  69
  70                 sg_set_page(sg_to, sg_page(sg_from),
  71                                 sg_from->length, sg_from->offset);
  72
  73                 if (sg_is_last(sg_from) || !len)
  74                         goto out;
  75
  76                 sg_from = sg_next(sg_from);
  77                 sg_to = sg_next(sg_to);
  78         }
  79
  80         if (len && to_nents) {
  81                 sg_set_page(sg_to, sg_page(sg_from), len, sg_from->offset);
  82                 copied += len;
  83         }
  84 out:
  85         sg_mark_end(sg_to);
  86         return copied;
  87 }
  88
  89 /*
  90  * Compares section of 'sg' starting from offset 'offset' and with length 'len'
  91  * to linear buffer of length 'len' at address 'buffer'
  92  * Returns 0 if equal and  -1 otherwice
  93  */
  94 static int msb_sg_compare_to_buffer(struct scatterlist *sg,
  95                                         size_t offset, u8 *buffer, size_t len)
  96 {
  97         int retval = 0, cmplen;
  98         struct sg_mapping_iter miter;
  99
 100         sg_miter_start(&miter, sg, sg_nents(sg),
 101                                         SG_MITER_ATOMIC | SG_MITER_FROM_SG);
 102
 103         while (sg_miter_next(&miter) && len > 0) {
 104                 if (offset >= miter.length) {
 105                         offset -= miter.length;
 106                         continue;
 107                 }
 108
 109                 cmplen = min(miter.length - offset, len);
 110                 retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
 111                 if (retval)
 112                         break;
 113
 114                 buffer += cmplen;
 115                 len -= cmplen;
 116                 offset = 0;
 117         }
 118
 119         if (!retval && len)
 120                 retval = -1;
 121
 122         sg_miter_stop(&miter);
 123         return retval;
 124 }
 125
 126
 127 /* Get zone at which block with logical address 'lba' lives
 128  * Flash is broken into zones.
 129  * Each zone consists of 512 eraseblocks, out of which in first
 130  * zone 494 are used and 496 are for all following zones.
 131  * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
 132 */
 133 static int msb_get_zone_from_lba(int lba)
 134 {
 135         if (lba < 494)
 136                 return 0;
 137         return ((lba - 494) / 496) + 1;
 138 }
 139
 140 /* Get zone of physical block. Trivial */
 141 static int msb_get_zone_from_pba(int pba)
 142 {
 143         return pba / MS_BLOCKS_IN_ZONE;
 144 }
 145
 146 /* Debug test to validate free block counts */
 147 static int msb_validate_used_block_bitmap(struct msb_data *msb)
 148 {
 149         int total_free_blocks = 0;
 150         int i;
 151
 152         if (!debug)
 153                 return 0;
 154
 155         for (i = 0; i < msb->zone_count; i++)
 156                 total_free_blocks += msb->free_block_count[i];
 157
 158         if (msb->block_count - bitmap_weight(msb->used_blocks_bitmap,
 159                                         msb->block_count) == total_free_blocks)
 160                 return 0;
 161
 162         pr_err("BUG: free block counts don't match the bitmap");
 163         msb->read_only = true;
 164         return -EINVAL;
 165 }
 166
 167 /* Mark physical block as used */
 168 static void msb_mark_block_used(struct msb_data *msb, int pba)
 169 {
 170         int zone = msb_get_zone_from_pba(pba);
 171
 172         if (test_bit(pba, msb->used_blocks_bitmap)) {
 173                 pr_err(
 174                 "BUG: attempt to mark already used pba %d as used", pba);
 175                 msb->read_only = true;
 176                 return;
 177         }
 178
 179         if (msb_validate_used_block_bitmap(msb))
 180                 return;
 181
 182         /* No races because all IO is single threaded */
 183         __set_bit(pba, msb->used_blocks_bitmap);
 184         msb->free_block_count[zone]--;
 185 }
 186
 187 /* Mark physical block as free */
 188 static void msb_mark_block_unused(struct msb_data *msb, int pba)
 189 {
 190         int zone = msb_get_zone_from_pba(pba);
 191
 192         if (!test_bit(pba, msb->used_blocks_bitmap)) {
 193                 pr_err("BUG: attempt to mark already unused pba %d as unused" , pba);
 194                 msb->read_only = true;
 195                 return;
 196         }
 197
 198         if (msb_validate_used_block_bitmap(msb))
 199                 return;
 200
 201         /* No races because all IO is single threaded */
 202         __clear_bit(pba, msb->used_blocks_bitmap);
 203         msb->free_block_count[zone]++;
 204 }
 205
 206 /* Invalidate current register window */
 207 static void msb_invalidate_reg_window(struct msb_data *msb)
 208 {
 209         msb->reg_addr.w_offset = offsetof(struct ms_register, id);
 210         msb->reg_addr.w_length = sizeof(struct ms_id_register);
 211         msb->reg_addr.r_offset = offsetof(struct ms_register, id);
 212         msb->reg_addr.r_length = sizeof(struct ms_id_register);
 213         msb->addr_valid = false;
 214 }
 215
 216 /* Start a state machine */
 217 static int msb_run_state_machine(struct msb_data *msb, int   (*state_func)
 218                 (struct memstick_dev *card, struct memstick_request **req))
 219 {
 220         struct memstick_dev *card = msb->card;
 221
 222         WARN_ON(msb->state != -1);
 223         msb->int_polling = false;
 224         msb->state = 0;
 225         msb->exit_error = 0;
 226
 227         memset(&card->current_mrq, 0, sizeof(card->current_mrq));
 228
 229         card->next_request = state_func;
 230         memstick_new_req(card->host);
 231         wait_for_completion(&card->mrq_complete);
 232
 233         WARN_ON(msb->state != -1);
 234         return msb->exit_error;
 235 }
 236
 237 /* State machines call that to exit */
 238 static int msb_exit_state_machine(struct msb_data *msb, int error)
 239 {
 240         WARN_ON(msb->state == -1);
 241
 242         msb->state = -1;
 243         msb->exit_error = error;
 244         msb->card->next_request = h_msb_default_bad;
 245
 246         /* Invalidate reg window on errors */
 247         if (error)
 248                 msb_invalidate_reg_window(msb);
 249
 250         complete(&msb->card->mrq_complete);
 251         return -ENXIO;
 252 }
 253
 254 /* read INT register */
 255 static int msb_read_int_reg(struct msb_data *msb, long timeout)
 256 {
 257         struct memstick_request *mrq = &msb->card->current_mrq;
 258
 259         WARN_ON(msb->state == -1);
 260
 261         if (!msb->int_polling) {
 262                 msb->int_timeout = jiffies +
 263                         msecs_to_jiffies(timeout == -1 ? 500 : timeout);
 264                 msb->int_polling = true;
 265         } else if (time_after(jiffies, msb->int_timeout)) {
 266                 mrq->data[0] = MEMSTICK_INT_CMDNAK;
 267                 return 0;
 268         }
 269
 270         if ((msb->caps & MEMSTICK_CAP_AUTO_GET_INT) &&
 271                                 mrq->need_card_int && !mrq->error) {
 272                 mrq->data[0] = mrq->int_reg;
 273                 mrq->need_card_int = false;
 274                 return 0;
 275         } else {
 276                 memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
 277                 return 1;
 278         }
 279 }
 280
 281 /* Read a register */
 282 static int msb_read_regs(struct msb_data *msb, int offset, int len)
 283 {
 284         struct memstick_request *req = &msb->card->current_mrq;
 285
 286         if (msb->reg_addr.r_offset != offset ||
 287             msb->reg_addr.r_length != len || !msb->addr_valid) {
 288
 289                 msb->reg_addr.r_offset = offset;
 290                 msb->reg_addr.r_length = len;
 291                 msb->addr_valid = true;
 292
 293                 memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
 294                         &msb->reg_addr, sizeof(msb->reg_addr));
 295                 return 0;
 296         }
 297
 298         memstick_init_req(req, MS_TPC_READ_REG, NULL, len);
 299         return 1;
 300 }
 301
 302 /* Write a card register */
 303 static int msb_write_regs(struct msb_data *msb, int offset, int len, void *buf)
 304 {
 305         struct memstick_request *req = &msb->card->current_mrq;
 306
 307         if (msb->reg_addr.w_offset != offset ||
 308                 msb->reg_addr.w_length != len  || !msb->addr_valid) {
 309
 310                 msb->reg_addr.w_offset = offset;
 311                 msb->reg_addr.w_length = len;
 312                 msb->addr_valid = true;
 313
 314                 memstick_init_req(req, MS_TPC_SET_RW_REG_ADRS,
 315                         &msb->reg_addr, sizeof(msb->reg_addr));
 316                 return 0;
 317         }
 318
 319         memstick_init_req(req, MS_TPC_WRITE_REG, buf, len);
 320         return 1;
 321 }
 322
 323 /* Handler for absence of IO */
 324 static int h_msb_default_bad(struct memstick_dev *card,
 325                                                 struct memstick_request **mrq)
 326 {
 327         return -ENXIO;
 328 }
 329
 330 /*
 331  * This function is a handler for reads of one page from device.
 332  * Writes output to msb->current_sg, takes sector address from msb->reg.param
 333  * Can also be used to read extra data only. Set params accordintly.
 334  */
 335 static int h_msb_read_page(struct memstick_dev *card,
 336                                         struct memstick_request **out_mrq)
 337 {
 338         struct msb_data *msb = memstick_get_drvdata(card);
 339         struct memstick_request *mrq = *out_mrq = &card->current_mrq;
 340         struct scatterlist sg[2];
 341         u8 command, intreg;
 342
 343         if (mrq->error) {
 344                 dbg("read_page, unknown error");
 345                 return msb_exit_state_machine(msb, mrq->error);
 346         }
 347 again:
 348         switch (msb->state) {
 349         case MSB_RP_SEND_BLOCK_ADDRESS:
 350                 /* msb_write_regs sometimes "fails" because it needs to update
 351                         the reg window, and thus it returns request for that.
 352                         Then we stay in this state and retry */
 353                 if (!msb_write_regs(msb,
 354                         offsetof(struct ms_register, param),
 355                         sizeof(struct ms_param_register),
 356                         (unsigned char *)&msb->regs.param))
 357                         return 0;
 358
 359                 msb->state = MSB_RP_SEND_READ_COMMAND;
 360                 return 0;
 361
 362         case MSB_RP_SEND_READ_COMMAND:
 363                 command = MS_CMD_BLOCK_READ;
 364                 memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
 365                 msb->state = MSB_RP_SEND_INT_REQ;
 366                 return 0;
 367
 368         case MSB_RP_SEND_INT_REQ:
 369                 msb->state = MSB_RP_RECEIVE_INT_REQ_RESULT;
 370                 /* If dont actually need to send the int read request (only in
 371                         serial mode), then just fall through */
 372                 if (msb_read_int_reg(msb, -1))
 373                         return 0;
 374                 /* fallthrough */
 375
 376         case MSB_RP_RECEIVE_INT_REQ_RESULT:
 377                 intreg = mrq->data[0];
 378                 msb->regs.status.interrupt = intreg;
 379
 380                 if (intreg & MEMSTICK_INT_CMDNAK)
 381                         return msb_exit_state_machine(msb, -EIO);
 382
 383                 if (!(intreg & MEMSTICK_INT_CED)) {
 384                         msb->state = MSB_RP_SEND_INT_REQ;
 385                         goto again;
 386                 }
 387
 388                 msb->int_polling = false;
 389                 msb->state = (intreg & MEMSTICK_INT_ERR) ?
 390                         MSB_RP_SEND_READ_STATUS_REG : MSB_RP_SEND_OOB_READ;
 391                 goto again;
 392
 393         case MSB_RP_SEND_READ_STATUS_REG:
 394                  /* read the status register to understand source of the INT_ERR */
 395                 if (!msb_read_regs(msb,
 396                         offsetof(struct ms_register, status),
 397                         sizeof(struct ms_status_register)))
 398                         return 0;
 399
 400                 msb->state = MSB_RP_RECEIVE_STATUS_REG;
 401                 return 0;
 402
 403         case MSB_RP_RECEIVE_STATUS_REG:
 404                 msb->regs.status = *(struct ms_status_register *)mrq->data;
 405                 msb->state = MSB_RP_SEND_OOB_READ;
 406                 /* fallthrough */
 407
 408         case MSB_RP_SEND_OOB_READ:
 409                 if (!msb_read_regs(msb,
 410                         offsetof(struct ms_register, extra_data),
 411                         sizeof(struct ms_extra_data_register)))
 412                         return 0;
 413
 414                 msb->state = MSB_RP_RECEIVE_OOB_READ;
 415                 return 0;
 416
 417         case MSB_RP_RECEIVE_OOB_READ:
 418                 msb->regs.extra_data =
 419                         *(struct ms_extra_data_register *) mrq->data;
 420                 msb->state = MSB_RP_SEND_READ_DATA;
 421                 /* fallthrough */
 422
 423         case MSB_RP_SEND_READ_DATA:
 424                 /* Skip that state if we only read the oob */
 425                 if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
 426                         msb->state = MSB_RP_RECEIVE_READ_DATA;
 427                         goto again;
 428                 }
 429
 430                 sg_init_table(sg, ARRAY_SIZE(sg));
 431                 msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
 432                         msb->current_sg_offset,
 433                         msb->page_size);
 434
 435                 memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
 436                 msb->state = MSB_RP_RECEIVE_READ_DATA;
 437                 return 0;
 438
 439         case MSB_RP_RECEIVE_READ_DATA:
 440                 if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
 441                         msb->current_sg_offset += msb->page_size;
 442                         return msb_exit_state_machine(msb, 0);
 443                 }
 444
 445                 if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
 446                         dbg("read_page: uncorrectable error");
 447                         return msb_exit_state_machine(msb, -EBADMSG);
 448                 }
 449
 450                 if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
 451                         dbg("read_page: correctable error");
 452                         msb->current_sg_offset += msb->page_size;
 453                         return msb_exit_state_machine(msb, -EUCLEAN);
 454                 } else {
 455                         dbg("read_page: INT error, but no status error bits");
 456                         return msb_exit_state_machine(msb, -EIO);
 457                 }
 458         }
 459
 460         BUG();
 461 }
 462
 463 /*
 464  * Handler of writes of exactly one block.
 465  * Takes address from msb->regs.param.
 466  * Writes same extra data to blocks, also taken
 467  * from msb->regs.extra
 468  * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
 469  * device refuses to take the command or something else
 470  */
 471 static int h_msb_write_block(struct memstick_dev *card,
 472                                         struct memstick_request **out_mrq)
 473 {
 474         struct msb_data *msb = memstick_get_drvdata(card);
 475         struct memstick_request *mrq = *out_mrq = &card->current_mrq;
 476         struct scatterlist sg[2];
 477         u8 intreg, command;
 478
 479         if (mrq->error)
 480                 return msb_exit_state_machine(msb, mrq->error);
 481
 482 again:
 483         switch (msb->state) {
 484
 485         /* HACK: Jmicon handling of TPCs between 8 and
 486          *      sizeof(memstick_request.data) is broken due to hardware
 487          *      bug in PIO mode that is used for these TPCs
 488          *      Therefore split the write
 489          */
 490
 491         case MSB_WB_SEND_WRITE_PARAMS:
 492                 if (!msb_write_regs(msb,
 493                         offsetof(struct ms_register, param),
 494                         sizeof(struct ms_param_register),
 495                         &msb->regs.param))
 496                         return 0;
 497
 498                 msb->state = MSB_WB_SEND_WRITE_OOB;
 499                 return 0;
 500
 501         case MSB_WB_SEND_WRITE_OOB:
 502                 if (!msb_write_regs(msb,
 503                         offsetof(struct ms_register, extra_data),
 504                         sizeof(struct ms_extra_data_register),
 505                         &msb->regs.extra_data))
 506                         return 0;
 507                 msb->state = MSB_WB_SEND_WRITE_COMMAND;
 508                 return 0;
 509
 510
 511         case MSB_WB_SEND_WRITE_COMMAND:
 512                 command = MS_CMD_BLOCK_WRITE;
 513                 memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
 514                 msb->state = MSB_WB_SEND_INT_REQ;
 515                 return 0;
 516
 517         case MSB_WB_SEND_INT_REQ:
 518                 msb->state = MSB_WB_RECEIVE_INT_REQ;
 519                 if (msb_read_int_reg(msb, -1))
 520                         return 0;
 521                 /* fallthrough */
 522
 523         case MSB_WB_RECEIVE_INT_REQ:
 524                 intreg = mrq->data[0];
 525                 msb->regs.status.interrupt = intreg;
 526
 527                 /* errors mean out of here, and fast... */
 528                 if (intreg & (MEMSTICK_INT_CMDNAK))
 529                         return msb_exit_state_machine(msb, -EIO);
 530
 531                 if (intreg & MEMSTICK_INT_ERR)
 532                         return msb_exit_state_machine(msb, -EBADMSG);
 533
 534
 535                 /* for last page we need to poll CED */
 536                 if (msb->current_page == msb->pages_in_block) {
 537                         if (intreg & MEMSTICK_INT_CED)
 538                                 return msb_exit_state_machine(msb, 0);
 539                         msb->state = MSB_WB_SEND_INT_REQ;
 540                         goto again;
 541
 542                 }
 543
 544                 /* for non-last page we need BREQ before writing next chunk */
 545                 if (!(intreg & MEMSTICK_INT_BREQ)) {
 546                         msb->state = MSB_WB_SEND_INT_REQ;
 547                         goto again;
 548                 }
 549
 550                 msb->int_polling = false;
 551                 msb->state = MSB_WB_SEND_WRITE_DATA;
 552                 /* fallthrough */
 553
 554         case MSB_WB_SEND_WRITE_DATA:
 555                 sg_init_table(sg, ARRAY_SIZE(sg));
 556
 557                 if (msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
 558                         msb->current_sg_offset,
 559                         msb->page_size) < msb->page_size)
 560                         return msb_exit_state_machine(msb, -EIO);
 561
 562                 memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
 563                 mrq->need_card_int = 1;
 564                 msb->state = MSB_WB_RECEIVE_WRITE_CONFIRMATION;
 565                 return 0;
 566
 567         case MSB_WB_RECEIVE_WRITE_CONFIRMATION:
 568                 msb->current_page++;
 569                 msb->current_sg_offset += msb->page_size;
 570                 msb->state = MSB_WB_SEND_INT_REQ;
 571                 goto again;
 572         default:
 573                 BUG();
 574         }
 575
 576         return 0;
 577 }
 578
 579 /*
 580  * This function is used to send simple IO requests to device that consist
 581  * of register write + command
 582  */
 583 static int h_msb_send_command(struct memstick_dev *card,
 584                                         struct memstick_request **out_mrq)
 585 {
 586         struct msb_data *msb = memstick_get_drvdata(card);
 587         struct memstick_request *mrq = *out_mrq = &card->current_mrq;
 588         u8 intreg;
 589
 590         if (mrq->error) {
 591                 dbg("send_command: unknown error");
 592                 return msb_exit_state_machine(msb, mrq->error);
 593         }
 594 again:
 595         switch (msb->state) {
 596
 597         /* HACK: see h_msb_write_block */
 598         case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
 599                 if (!msb_write_regs(msb,
 600                         offsetof(struct ms_register, param),
 601                         sizeof(struct ms_param_register),
 602                         &msb->regs.param))
 603                         return 0;
 604                 msb->state = MSB_SC_SEND_WRITE_OOB;
 605                 return 0;
 606
 607         case MSB_SC_SEND_WRITE_OOB:
 608                 if (!msb->command_need_oob) {
 609                         msb->state = MSB_SC_SEND_COMMAND;
 610                         goto again;
 611                 }
 612
 613                 if (!msb_write_regs(msb,
 614                         offsetof(struct ms_register, extra_data),
 615                         sizeof(struct ms_extra_data_register),
 616                         &msb->regs.extra_data))
 617                         return 0;
 618
 619                 msb->state = MSB_SC_SEND_COMMAND;
 620                 return 0;
 621
 622         case MSB_SC_SEND_COMMAND:
 623                 memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
 624                 msb->state = MSB_SC_SEND_INT_REQ;
 625                 return 0;
 626
 627         case MSB_SC_SEND_INT_REQ:
 628                 msb->state = MSB_SC_RECEIVE_INT_REQ;
 629                 if (msb_read_int_reg(msb, -1))
 630                         return 0;
 631                 /* fallthrough */
 632
 633         case MSB_SC_RECEIVE_INT_REQ:
 634                 intreg = mrq->data[0];
 635
 636                 if (intreg & MEMSTICK_INT_CMDNAK)
 637                         return msb_exit_state_machine(msb, -EIO);
 638                 if (intreg & MEMSTICK_INT_ERR)
 639                         return msb_exit_state_machine(msb, -EBADMSG);
 640
 641                 if (!(intreg & MEMSTICK_INT_CED)) {
 642                         msb->state = MSB_SC_SEND_INT_REQ;
 643                         goto again;
 644                 }
 645
 646                 return msb_exit_state_machine(msb, 0);
 647         }
 648
 649         BUG();
 650 }
 651
 652 /* Small handler for card reset */
 653 static int h_msb_reset(struct memstick_dev *card,
 654                                         struct memstick_request **out_mrq)
 655 {
 656         u8 command = MS_CMD_RESET;
 657         struct msb_data *msb = memstick_get_drvdata(card);
 658         struct memstick_request *mrq = *out_mrq = &card->current_mrq;
 659
 660         if (mrq->error)
 661                 return msb_exit_state_machine(msb, mrq->error);
 662
 663         switch (msb->state) {
 664         case MSB_RS_SEND:
 665                 memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
 666                 mrq->need_card_int = 0;
 667                 msb->state = MSB_RS_CONFIRM;
 668                 return 0;
 669         case MSB_RS_CONFIRM:
 670                 return msb_exit_state_machine(msb, 0);
 671         }
 672         BUG();
 673 }
 674
 675 /* This handler is used to do serial->parallel switch */
 676 static int h_msb_parallel_switch(struct memstick_dev *card,
 677                                         struct memstick_request **out_mrq)
 678 {
 679         struct msb_data *msb = memstick_get_drvdata(card);
 680         struct memstick_request *mrq = *out_mrq = &card->current_mrq;
 681         struct memstick_host *host = card->host;
 682
 683         if (mrq->error) {
 684                 dbg("parallel_switch: error");
 685                 msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
 686                 return msb_exit_state_machine(msb, mrq->error);
 687         }
 688
 689         switch (msb->state) {
 690         case MSB_PS_SEND_SWITCH_COMMAND:
 691                 /* Set the parallel interface on memstick side */
 692                 msb->regs.param.system |= MEMSTICK_SYS_PAM;
 693
 694                 if (!msb_write_regs(msb,
 695                         offsetof(struct ms_register, param),
 696                         1,
 697                         (unsigned char *)&msb->regs.param))
 698                         return 0;
 699
 700                 msb->state = MSB_PS_SWICH_HOST;
 701                 return 0;
 702
 703         case MSB_PS_SWICH_HOST:
 704                  /* Set parallel interface on our side + send a dummy request
 705                         to see if card responds */
 706                 host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
 707                 memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
 708                 msb->state = MSB_PS_CONFIRM;
 709                 return 0;
 710
 711         case MSB_PS_CONFIRM:
 712                 return msb_exit_state_machine(msb, 0);
 713         }
 714
 715         BUG();
 716 }
 717
 718 static int msb_switch_to_parallel(struct msb_data *msb);
 719
 720 /* Reset the card, to guard against hw errors beeing treated as bad blocks */
 721 static int msb_reset(struct msb_data *msb, bool full)
 722 {
 723
 724         bool was_parallel = msb->regs.param.system & MEMSTICK_SYS_PAM;
 725         struct memstick_dev *card = msb->card;
 726         struct memstick_host *host = card->host;
 727         int error;
 728
 729         /* Reset the card */
 730         msb->regs.param.system = MEMSTICK_SYS_BAMD;
 731
 732         if (full) {
 733                 error =  host->set_param(host,
 734                                         MEMSTICK_POWER, MEMSTICK_POWER_OFF);
 735                 if (error)
 736                         goto out_error;
 737
 738                 msb_invalidate_reg_window(msb);
 739
 740                 error = host->set_param(host,
 741                                         MEMSTICK_POWER, MEMSTICK_POWER_ON);
 742                 if (error)
 743                         goto out_error;
 744
 745                 error = host->set_param(host,
 746                                         MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
 747                 if (error) {
 748 out_error:
 749                         dbg("Failed to reset the host controller");
 750                         msb->read_only = true;
 751                         return -EFAULT;
 752                 }
 753         }
 754
 755         error = msb_run_state_machine(msb, h_msb_reset);
 756         if (error) {
 757                 dbg("Failed to reset the card");
 758                 msb->read_only = true;
 759                 return -ENODEV;
 760         }
 761
 762         /* Set parallel mode */
 763         if (was_parallel)
 764                 msb_switch_to_parallel(msb);
 765         return 0;
 766 }
 767
 768 /* Attempts to switch interface to parallel mode */
 769 static int msb_switch_to_parallel(struct msb_data *msb)
 770 {
 771         int error;
 772
 773         error = msb_run_state_machine(msb, h_msb_parallel_switch);
 774         if (error) {
 775                 pr_err("Switch to parallel failed");
 776                 msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
 777                 msb_reset(msb, true);
 778                 return -EFAULT;
 779         }
 780
 781         msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
 782         return 0;
 783 }
 784
 785 /* Changes overwrite flag on a page */
 786 static int msb_set_overwrite_flag(struct msb_data *msb,
 787                                                 u16 pba, u8 page, u8 flag)
 788 {
 789         if (msb->read_only)
 790                 return -EROFS;
 791
 792         msb->regs.param.block_address = cpu_to_be16(pba);
 793         msb->regs.param.page_address = page;
 794         msb->regs.param.cp = MEMSTICK_CP_OVERWRITE;
 795         msb->regs.extra_data.overwrite_flag = flag;
 796         msb->command_value = MS_CMD_BLOCK_WRITE;
 797         msb->command_need_oob = true;
 798
 799         dbg_verbose("changing overwrite flag to %02x for sector %d, page %d",
 800                                                         flag, pba, page);
 801         return msb_run_state_machine(msb, h_msb_send_command);
 802 }
 803
 804 static int msb_mark_bad(struct msb_data *msb, int pba)
 805 {
 806         pr_notice("marking pba %d as bad", pba);
 807         msb_reset(msb, true);
 808         return msb_set_overwrite_flag(
 809                         msb, pba, 0, 0xFF & ~MEMSTICK_OVERWRITE_BKST);
 810 }
 811
 812 static int msb_mark_page_bad(struct msb_data *msb, int pba, int page)
 813 {
 814         dbg("marking page %d of pba %d as bad", page, pba);
 815         msb_reset(msb, true);
 816         return msb_set_overwrite_flag(msb,
 817                 pba, page, ~MEMSTICK_OVERWRITE_PGST0);
 818 }
 819
 820 /* Erases one physical block */
 821 static int msb_erase_block(struct msb_data *msb, u16 pba)
 822 {
 823         int error, try;
 824         if (msb->read_only)
 825                 return -EROFS;
 826
 827         dbg_verbose("erasing pba %d", pba);
 828
 829         for (try = 1; try < 3; try++) {
 830                 msb->regs.param.block_address = cpu_to_be16(pba);
 831                 msb->regs.param.page_address = 0;
 832                 msb->regs.param.cp = MEMSTICK_CP_BLOCK;
 833                 msb->command_value = MS_CMD_BLOCK_ERASE;
 834                 msb->command_need_oob = false;
 835
 836
 837                 error = msb_run_state_machine(msb, h_msb_send_command);
 838                 if (!error || msb_reset(msb, true))
 839                         break;
 840         }
 841
 842         if (error) {
 843                 pr_err("erase failed, marking pba %d as bad", pba);
 844                 msb_mark_bad(msb, pba);
 845         }
 846
 847         dbg_verbose("erase success, marking pba %d as unused", pba);
 848         msb_mark_block_unused(msb, pba);
 849         __set_bit(pba, msb->erased_blocks_bitmap);
 850         return error;
 851 }
 852
 853 /* Reads one page from device */
 854 static int msb_read_page(struct msb_data *msb,
 855         u16 pba, u8 page, struct ms_extra_data_register *extra,
 856                                         struct scatterlist *sg,  int offset)
 857 {
 858         int try, error;
 859
 860         if (pba == MS_BLOCK_INVALID) {
 861                 unsigned long flags;
 862                 struct sg_mapping_iter miter;
 863                 size_t len = msb->page_size;
 864
 865                 dbg_verbose("read unmapped sector. returning 0xFF");
 866
 867                 local_irq_save(flags);
 868                 sg_miter_start(&miter, sg, sg_nents(sg),
 869                                 SG_MITER_ATOMIC | SG_MITER_TO_SG);
 870
 871                 while (sg_miter_next(&miter) && len > 0) {
 872
 873                         int chunklen;
 874
 875                         if (offset && offset >= miter.length) {
 876                                 offset -= miter.length;
 877                                 continue;
 878                         }
 879
 880                         chunklen = min(miter.length - offset, len);
 881                         memset(miter.addr + offset, 0xFF, chunklen);
 882                         len -= chunklen;
 883                         offset = 0;
 884                 }
 885
 886                 sg_miter_stop(&miter);
 887                 local_irq_restore(flags);
 888
 889                 if (offset)
 890                         return -EFAULT;
 891
 892                 if (extra)
 893                         memset(extra, 0xFF, sizeof(*extra));
 894                 return 0;
 895         }
 896
 897         if (pba >= msb->block_count) {
 898                 pr_err("BUG: attempt to read beyond the end of the card at pba %d", pba);
 899                 return -EINVAL;
 900         }
 901
 902         for (try = 1; try < 3; try++) {
 903                 msb->regs.param.block_address = cpu_to_be16(pba);
 904                 msb->regs.param.page_address = page;
 905                 msb->regs.param.cp = MEMSTICK_CP_PAGE;
 906
 907                 msb->current_sg = sg;
 908                 msb->current_sg_offset = offset;
 909                 error = msb_run_state_machine(msb, h_msb_read_page);
 910
 911
 912                 if (error == -EUCLEAN) {
 913                         pr_notice("correctable error on pba %d, page %d",
 914                                 pba, page);
 915                         error = 0;
 916                 }
 917
 918                 if (!error && extra)
 919                         *extra = msb->regs.extra_data;
 920
 921                 if (!error || msb_reset(msb, true))
 922                         break;
 923
 924         }
 925
 926         /* Mark bad pages */
 927         if (error == -EBADMSG) {
 928                 pr_err("uncorrectable error on read of pba %d, page %d",
 929                         pba, page);
 930
 931                 if (msb->regs.extra_data.overwrite_flag &
 932                                         MEMSTICK_OVERWRITE_PGST0)
 933                         msb_mark_page_bad(msb, pba, page);
 934                 return -EBADMSG;
 935         }
 936
 937         if (error)
 938                 pr_err("read of pba %d, page %d failed with error %d",
 939                         pba, page, error);
 940         return error;
 941 }
 942
 943 /* Reads oob of page only */
 944 static int msb_read_oob(struct msb_data *msb, u16 pba, u16 page,
 945         struct ms_extra_data_register *extra)
 946 {
 947         int error;
 948
 949         BUG_ON(!extra);
 950         msb->regs.param.block_address = cpu_to_be16(pba);
 951         msb->regs.param.page_address = page;
 952         msb->regs.param.cp = MEMSTICK_CP_EXTRA;
 953
 954         if (pba > msb->block_count) {
 955                 pr_err("BUG: attempt to read beyond the end of card at pba %d", pba);
 956                 return -EINVAL;
 957         }
 958
 959         error = msb_run_state_machine(msb, h_msb_read_page);
 960         *extra = msb->regs.extra_data;
 961
 962         if (error == -EUCLEAN) {
 963                 pr_notice("correctable error on pba %d, page %d",
 964                         pba, page);
 965                 return 0;
 966         }
 967
 968         return error;
 969 }
 970
 971 /* Reads a block and compares it with data contained in scatterlist orig_sg */
 972 static int msb_verify_block(struct msb_data *msb, u16 pba,
 973                                 struct scatterlist *orig_sg,  int offset)
 974 {
 975         struct scatterlist sg;
 976         int page = 0, error;
 977
 978         sg_init_one(&sg, msb->block_buffer, msb->block_size);
 979
 980         while (page < msb->pages_in_block) {
 981
 982                 error = msb_read_page(msb, pba, page,
 983                                 NULL, &sg, page * msb->page_size);
 984                 if (error)
 985                         return error;
 986                 page++;
 987         }
 988
 989         if (msb_sg_compare_to_buffer(orig_sg, offset,
 990                                 msb->block_buffer, msb->block_size))
 991                 return -EIO;
 992         return 0;
 993 }
 994
 995 /* Writes exectly one block + oob */
 996 static int msb_write_block(struct msb_data *msb,
 997                         u16 pba, u32 lba, struct scatterlist *sg, int offset)
 998 {
 999         int error, current_try = 1;
1000         BUG_ON(sg->length < msb->page_size);
1001
1002         if (msb->read_only)
1003                 return -EROFS;
1004
1005         if (pba == MS_BLOCK_INVALID) {
1006                 pr_err(
1007                         "BUG: write: attempt to write MS_BLOCK_INVALID block");
1008                 return -EINVAL;
1009         }
1010
1011         if (pba >= msb->block_count || lba >= msb->logical_block_count) {
1012                 pr_err(
1013                 "BUG: write: attempt to write beyond the end of device");
1014                 return -EINVAL;
1015         }
1016
1017         if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
1018                 pr_err("BUG: write: lba zone mismatch");
1019                 return -EINVAL;
1020         }
1021
1022         if (pba == msb->boot_block_locations[0] ||
1023                 pba == msb->boot_block_locations[1]) {
1024                 pr_err("BUG: write: attempt to write to boot blocks!");
1025                 return -EINVAL;
1026         }
1027
1028         while (1) {
1029
1030                 if (msb->read_only)
1031                         return -EROFS;
1032
1033                 msb->regs.param.cp = MEMSTICK_CP_BLOCK;
1034                 msb->regs.param.page_address = 0;
1035                 msb->regs.param.block_address = cpu_to_be16(pba);
1036
1037                 msb->regs.extra_data.management_flag = 0xFF;
1038                 msb->regs.extra_data.overwrite_flag = 0xF8;
1039                 msb->regs.extra_data.logical_address = cpu_to_be16(lba);
1040
1041                 msb->current_sg = sg;
1042                 msb->current_sg_offset = offset;
1043                 msb->current_page = 0;
1044
1045                 error = msb_run_state_machine(msb, h_msb_write_block);
1046
1047                 /* Sector we just wrote to is assumed erased since its pba
1048                         was erased. If it wasn't erased, write will succeed
1049                         and will just clear the bits that were set in the block
1050                         thus test that what we have written,
1051                         matches what we expect.
1052                         We do trust the blocks that we erased */
1053                 if (!error && (verify_writes ||
1054                                 !test_bit(pba, msb->erased_blocks_bitmap)))
1055                         error = msb_verify_block(msb, pba, sg, offset);
1056
1057                 if (!error)
1058                         break;
1059
1060                 if (current_try > 1 || msb_reset(msb, true))
1061                         break;
1062
1063                 pr_err("write failed, trying to erase the pba %d", pba);
1064                 error = msb_erase_block(msb, pba);
1065                 if (error)
1066                         break;
1067
1068                 current_try++;
1069         }
1070         return error;
1071 }
1072
1073 /* Finds a free block for write replacement */
1074 static u16 msb_get_free_block(struct msb_data *msb, int zone)
1075 {
1076         u16 pos;
1077         int pba = zone * MS_BLOCKS_IN_ZONE;
1078         int i;
1079
1080         get_random_bytes(&pos, sizeof(pos));
1081
1082         if (!msb->free_block_count[zone]) {
1083                 pr_err("NO free blocks in the zone %d, to use for a write, (media is WORN out) switching to RO mode", zone);
1084                 msb->read_only = true;
1085                 return MS_BLOCK_INVALID;
1086         }
1087
1088         pos %= msb->free_block_count[zone];
1089
1090         dbg_verbose("have %d choices for a free block, selected randomally: %d",
1091                 msb->free_block_count[zone], pos);
1092
1093         pba = find_next_zero_bit(msb->used_blocks_bitmap,
1094                                                         msb->block_count, pba);
1095         for (i = 0; i < pos; ++i)
1096                 pba = find_next_zero_bit(msb->used_blocks_bitmap,
1097                                                 msb->block_count, pba + 1);
1098
1099         dbg_verbose("result of the free blocks scan: pba %d", pba);
1100
1101         if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
1102                 pr_err("BUG: cant get a free block");
1103                 msb->read_only = true;
1104                 return MS_BLOCK_INVALID;
1105         }
1106
1107         msb_mark_block_used(msb, pba);
1108         return pba;
1109 }
1110
1111 static int msb_update_block(struct msb_data *msb, u16 lba,
1112         struct scatterlist *sg, int offset)
1113 {
1114         u16 pba, new_pba;
1115         int error, try;
1116
1117         pba = msb->lba_to_pba_table[lba];
1118         dbg_verbose("start of a block update at lba  %d, pba %d", lba, pba);
1119
1120         if (pba != MS_BLOCK_INVALID) {
1121                 dbg_verbose("setting the update flag on the block");
1122                 msb_set_overwrite_flag(msb, pba, 0,
1123                                 0xFF & ~MEMSTICK_OVERWRITE_UDST);
1124         }
1125
1126         for (try = 0; try < 3; try++) {
1127                 new_pba = msb_get_free_block(msb,
1128                         msb_get_zone_from_lba(lba));
1129
1130                 if (new_pba == MS_BLOCK_INVALID) {
1131                         error = -EIO;
1132                         goto out;
1133                 }
1134
1135                 dbg_verbose("block update: writing updated block to the pba %d",
1136                                                                 new_pba);
1137                 error = msb_write_block(msb, new_pba, lba, sg, offset);
1138                 if (error == -EBADMSG) {
1139                         msb_mark_bad(msb, new_pba);
1140                         continue;
1141                 }
1142
1143                 if (error)
1144                         goto out;
1145
1146                 dbg_verbose("block update: erasing the old block");
1147                 msb_erase_block(msb, pba);
1148                 msb->lba_to_pba_table[lba] = new_pba;
1149                 return 0;
1150         }
1151 out:
1152         if (error) {
1153                 pr_err("block update error after %d tries,  switching to r/o mode", try);
1154                 msb->read_only = true;
1155         }
1156         return error;
1157 }
1158
1159 /* Converts endiannes in the boot block for easy use */
1160 static void msb_fix_boot_page_endianness(struct ms_boot_page *p)
1161 {
1162         p->header.block_id = be16_to_cpu(p->header.block_id);
1163         p->header.format_reserved = be16_to_cpu(p->header.format_reserved);
1164         p->entry.disabled_block.start_addr
1165                 = be32_to_cpu(p->entry.disabled_block.start_addr);
1166         p->entry.disabled_block.data_size
1167                 = be32_to_cpu(p->entry.disabled_block.data_size);
1168         p->entry.cis_idi.start_addr
1169                 = be32_to_cpu(p->entry.cis_idi.start_addr);
1170         p->entry.cis_idi.data_size
1171                 = be32_to_cpu(p->entry.cis_idi.data_size);
1172         p->attr.block_size = be16_to_cpu(p->attr.block_size);
1173         p->attr.number_of_blocks = be16_to_cpu(p->attr.number_of_blocks);
1174         p->attr.number_of_effective_blocks
1175                 = be16_to_cpu(p->attr.number_of_effective_blocks);
1176         p->attr.page_size = be16_to_cpu(p->attr.page_size);
1177         p->attr.memory_manufacturer_code
1178                 = be16_to_cpu(p->attr.memory_manufacturer_code);
1179         p->attr.memory_device_code = be16_to_cpu(p->attr.memory_device_code);
1180         p->attr.implemented_capacity
1181                 = be16_to_cpu(p->attr.implemented_capacity);
1182         p->attr.controller_number = be16_to_cpu(p->attr.controller_number);
1183         p->attr.controller_function = be16_to_cpu(p->attr.controller_function);
1184 }
1185
1186 static int msb_read_boot_blocks(struct msb_data *msb)
1187 {
1188         int pba = 0;
1189         struct scatterlist sg;
1190         struct ms_extra_data_register extra;
1191         struct ms_boot_page *page;
1192
1193         msb->boot_block_locations[0] = MS_BLOCK_INVALID;
1194         msb->boot_block_locations[1] = MS_BLOCK_INVALID;
1195         msb->boot_block_count = 0;
1196
1197         dbg_verbose("Start of a scan for the boot blocks");
1198
1199         if (!msb->boot_page) {
1200                 page = kmalloc_array(2, sizeof(struct ms_boot_page),
1201                                      GFP_KERNEL);
1202                 if (!page)
1203                         return -ENOMEM;
1204
1205                 msb->boot_page = page;
1206         } else
1207                 page = msb->boot_page;
1208
1209         msb->block_count = MS_BLOCK_MAX_BOOT_ADDR;
1210
1211         for (pba = 0; pba < MS_BLOCK_MAX_BOOT_ADDR; pba++) {
1212
1213                 sg_init_one(&sg, page, sizeof(*page));
1214                 if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) {
1215                         dbg("boot scan: can't read pba %d", pba);
1216                         continue;
1217                 }
1218
1219                 if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) {
1220                         dbg("management flag doesn't indicate boot block %d",
1221                                                                         pba);
1222                         continue;
1223                 }
1224
1225                 if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) {
1226                         dbg("the pba at %d doesn' contain boot block ID", pba);
1227                         continue;
1228                 }
1229
1230                 msb_fix_boot_page_endianness(page);
1231                 msb->boot_block_locations[msb->boot_block_count] = pba;
1232
1233                 page++;
1234                 msb->boot_block_count++;
1235
1236                 if (msb->boot_block_count == 2)
1237                         break;
1238         }
1239
1240         if (!msb->boot_block_count) {
1241                 pr_err("media doesn't contain master page, aborting");
1242                 return -EIO;
1243         }
1244
1245         dbg_verbose("End of scan for boot blocks");
1246         return 0;
1247 }
1248
1249 static int msb_read_bad_block_table(struct msb_data *msb, int block_nr)
1250 {
1251         struct ms_boot_page *boot_block;
1252         struct scatterlist sg;
1253         u16 *buffer = NULL;
1254         int offset = 0;
1255         int i, error = 0;
1256         int data_size, data_offset, page, page_offset, size_to_read;
1257         u16 pba;
1258
1259         BUG_ON(block_nr > 1);
1260         boot_block = &msb->boot_page[block_nr];
1261         pba = msb->boot_block_locations[block_nr];
1262
1263         if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID)
1264                 return -EINVAL;
1265
1266         data_size = boot_block->entry.disabled_block.data_size;
1267         data_offset = sizeof(struct ms_boot_page) +
1268                         boot_block->entry.disabled_block.start_addr;
1269         if (!data_size)
1270                 return 0;
1271
1272         page = data_offset / msb->page_size;
1273         page_offset = data_offset % msb->page_size;
1274         size_to_read =
1275                 DIV_ROUND_UP(data_size + page_offset, msb->page_size) *
1276                         msb->page_size;
1277
1278         dbg("reading bad block of boot block at pba %d, offset %d len %d",
1279                 pba, data_offset, data_size);
1280
1281         buffer = kzalloc(size_to_read, GFP_KERNEL);
1282         if (!buffer)
1283                 return -ENOMEM;
1284
1285         /* Read the buffer */
1286         sg_init_one(&sg, buffer, size_to_read);
1287
1288         while (offset < size_to_read) {
1289                 error = msb_read_page(msb, pba, page, NULL, &sg, offset);
1290                 if (error)
1291                         goto out;
1292
1293                 page++;
1294                 offset += msb->page_size;
1295
1296                 if (page == msb->pages_in_block) {
1297                         pr_err(
1298                         "bad block table extends beyond the boot block");
1299                         break;
1300                 }
1301         }
1302
1303         /* Process the bad block table */
1304         for (i = page_offset; i < data_size / sizeof(u16); i++) {
1305
1306                 u16 bad_block = be16_to_cpu(buffer[i]);
1307
1308                 if (bad_block >= msb->block_count) {
1309                         dbg("bad block table contains invalid block %d",
1310                                                                 bad_block);
1311                         continue;
1312                 }
1313
1314                 if (test_bit(bad_block, msb->used_blocks_bitmap))  {
1315                         dbg("duplicate bad block %d in the table",
1316                                 bad_block);
1317                         continue;
1318                 }
1319
1320                 dbg("block %d is marked as factory bad", bad_block);
1321                 msb_mark_block_used(msb, bad_block);
1322         }
1323 out:
1324         kfree(buffer);
1325         return error;
1326 }
1327
1328 static int msb_ftl_initialize(struct msb_data *msb)
1329 {
1330         int i;
1331
1332         if (msb->ftl_initialized)
1333                 return 0;
1334
1335         msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE;
1336         msb->logical_block_count = msb->zone_count * 496 - 2;
1337
1338         msb->used_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
1339         msb->erased_blocks_bitmap = kzalloc(msb->block_count / 8, GFP_KERNEL);
1340         msb->lba_to_pba_table =
1341                 kmalloc_array(msb->logical_block_count, sizeof(u16),
1342                               GFP_KERNEL);
1343
1344         if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table ||
1345                                                 !msb->erased_blocks_bitmap) {
1346                 kfree(msb->used_blocks_bitmap);
1347                 kfree(msb->lba_to_pba_table);
1348                 kfree(msb->erased_blocks_bitmap);
1349                 return -ENOMEM;
1350         }
1351
1352         for (i = 0; i < msb->zone_count; i++)
1353                 msb->free_block_count[i] = MS_BLOCKS_IN_ZONE;
1354
1355         memset(msb->lba_to_pba_table, MS_BLOCK_INVALID,
1356                         msb->logical_block_count * sizeof(u16));
1357
1358         dbg("initial FTL tables created. Zone count = %d, Logical block count = %d",
1359                 msb->zone_count, msb->logical_block_count);
1360
1361         msb->ftl_initialized = true;
1362         return 0;
1363 }
1364
1365 static int msb_ftl_scan(struct msb_data *msb)
1366 {
1367         u16 pba, lba, other_block;
1368         u8 overwrite_flag, management_flag, other_overwrite_flag;
1369         int error;
1370         struct ms_extra_data_register extra;
1371         u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL);
1372
1373         if (!overwrite_flags)
1374                 return -ENOMEM;
1375
1376         dbg("Start of media scanning");
1377         for (pba = 0; pba < msb->block_count; pba++) {
1378
1379                 if (pba == msb->boot_block_locations[0] ||
1380                         pba == msb->boot_block_locations[1]) {
1381                         dbg_verbose("pba %05d -> [boot block]", pba);
1382                         msb_mark_block_used(msb, pba);
1383                         continue;
1384                 }
1385
1386                 if (test_bit(pba, msb->used_blocks_bitmap)) {
1387                         dbg_verbose("pba %05d -> [factory bad]", pba);
1388                         continue;
1389                 }
1390
1391                 memset(&extra, 0, sizeof(extra));
1392                 error = msb_read_oob(msb, pba, 0, &extra);
1393
1394                 /* can't trust the page if we can't read the oob */
1395                 if (error == -EBADMSG) {
1396                         pr_notice(
1397                         "oob of pba %d damaged, will try to erase it", pba);
1398                         msb_mark_block_used(msb, pba);
1399                         msb_erase_block(msb, pba);
1400                         continue;
1401                 } else if (error) {
1402                         pr_err("unknown error %d on read of oob of pba %d - aborting",
1403                                 error, pba);
1404
1405                         kfree(overwrite_flags);
1406                         return error;
1407                 }
1408
1409                 lba = be16_to_cpu(extra.logical_address);
1410                 management_flag = extra.management_flag;
1411                 overwrite_flag = extra.overwrite_flag;
1412                 overwrite_flags[pba] = overwrite_flag;
1413
1414                 /* Skip bad blocks */
1415                 if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) {
1416                         dbg("pba %05d -> [BAD]", pba);
1417                         msb_mark_block_used(msb, pba);
1418                         continue;
1419                 }
1420
1421                 /* Skip system/drm blocks */
1422                 if ((management_flag & MEMSTICK_MANAGEMENT_FLAG_NORMAL) !=
1423                         MEMSTICK_MANAGEMENT_FLAG_NORMAL) {
1424                         dbg("pba %05d -> [reserved management flag %02x]",
1425                                                         pba, management_flag);
1426                         msb_mark_block_used(msb, pba);
1427                         continue;
1428                 }
1429
1430                 /* Erase temporary tables */
1431                 if (!(management_flag & MEMSTICK_MANAGEMENT_ATFLG)) {
1432                         dbg("pba %05d -> [temp table] - will erase", pba);
1433
1434                         msb_mark_block_used(msb, pba);
1435                         msb_erase_block(msb, pba);
1436                         continue;
1437                 }
1438
1439                 if (lba == MS_BLOCK_INVALID) {
1440                         dbg_verbose("pba %05d -> [free]", pba);
1441                         continue;
1442                 }
1443
1444                 msb_mark_block_used(msb, pba);
1445
1446                 /* Block has LBA not according to zoning*/
1447                 if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
1448                         pr_notice("pba %05d -> [bad lba %05d] - will erase",
1449                                                                 pba, lba);
1450                         msb_erase_block(msb, pba);
1451                         continue;
1452                 }
1453
1454                 /* No collisions - great */
1455                 if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) {
1456                         dbg_verbose("pba %05d -> [lba %05d]", pba, lba);
1457                         msb->lba_to_pba_table[lba] = pba;
1458                         continue;
1459                 }
1460
1461                 other_block = msb->lba_to_pba_table[lba];
1462                 other_overwrite_flag = overwrite_flags[other_block];
1463
1464                 pr_notice("Collision between pba %d and pba %d",
1465                         pba, other_block);
1466
1467                 if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
1468                         pr_notice("pba %d is marked as stable, use it", pba);
1469                         msb_erase_block(msb, other_block);
1470                         msb->lba_to_pba_table[lba] = pba;
1471                         continue;
1472                 }
1473
1474                 if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) {
1475                         pr_notice("pba %d is marked as stable, use it",
1476                                                                 other_block);
1477                         msb_erase_block(msb, pba);
1478                         continue;
1479                 }
1480
1481                 pr_notice("collision between blocks %d and %d, without stable flag set on both, erasing pba %d",
1482                                 pba, other_block, other_block);
1483
1484                 msb_erase_block(msb, other_block);
1485                 msb->lba_to_pba_table[lba] = pba;
1486         }
1487
1488         dbg("End of media scanning");
1489         kfree(overwrite_flags);
1490         return 0;
1491 }
1492
1493 static void msb_cache_flush_timer(struct timer_list *t)
1494 {
1495         struct msb_data *msb = from_timer(msb, t, cache_flush_timer);
1496         msb->need_flush_cache = true;
1497         queue_work(msb->io_queue, &msb->io_work);
1498 }
1499
1500
1501 static void msb_cache_discard(struct msb_data *msb)
1502 {
1503         if (msb->cache_block_lba == MS_BLOCK_INVALID)
1504                 return;
1505
1506         del_timer_sync(&msb->cache_flush_timer);
1507
1508         dbg_verbose("Discarding the write cache");
1509         msb->cache_block_lba = MS_BLOCK_INVALID;
1510         bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block);
1511 }
1512
1513 static int msb_cache_init(struct msb_data *msb)
1514 {
1515         timer_setup(&msb->cache_flush_timer, msb_cache_flush_timer, 0);
1516
1517         if (!msb->cache)
1518                 msb->cache = kzalloc(msb->block_size, GFP_KERNEL);
1519         if (!msb->cache)
1520                 return -ENOMEM;
1521
1522         msb_cache_discard(msb);
1523         return 0;
1524 }
1525
1526 static int msb_cache_flush(struct msb_data *msb)
1527 {
1528         struct scatterlist sg;
1529         struct ms_extra_data_register extra;
1530         int page, offset, error;
1531         u16 pba, lba;
1532
1533         if (msb->read_only)
1534                 return -EROFS;
1535
1536         if (msb->cache_block_lba == MS_BLOCK_INVALID)
1537                 return 0;
1538
1539         lba = msb->cache_block_lba;
1540         pba = msb->lba_to_pba_table[lba];
1541
1542         dbg_verbose("Flushing the write cache of pba %d (LBA %d)",
1543                                                 pba, msb->cache_block_lba);
1544
1545         sg_init_one(&sg, msb->cache , msb->block_size);
1546
1547         /* Read all missing pages in cache */
1548         for (page = 0; page < msb->pages_in_block; page++) {
1549
1550                 if (test_bit(page, &msb->valid_cache_bitmap))
1551                         continue;
1552
1553                 offset = page * msb->page_size;
1554
1555                 dbg_verbose("reading non-present sector %d of cache block %d",
1556                         page, lba);
1557                 error = msb_read_page(msb, pba, page, &extra, &sg, offset);
1558
1559                 /* Bad pages are copied with 00 page status */
1560                 if (error == -EBADMSG) {
1561                         pr_err("read error on sector %d, contents probably damaged", page);
1562                         continue;
1563                 }
1564
1565                 if (error)
1566                         return error;
1567
1568                 if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) !=
1569                                                         MEMSTICK_OV_PG_NORMAL) {
1570                         dbg("page %d is marked as bad", page);
1571                         continue;
1572                 }
1573
1574                 set_bit(page, &msb->valid_cache_bitmap);
1575         }
1576
1577         /* Write the cache now */
1578         error = msb_update_block(msb, msb->cache_block_lba, &sg, 0);
1579         pba = msb->lba_to_pba_table[msb->cache_block_lba];
1580
1581         /* Mark invalid pages */
1582         if (!error) {
1583                 for (page = 0; page < msb->pages_in_block; page++) {
1584
1585                         if (test_bit(page, &msb->valid_cache_bitmap))
1586                                 continue;
1587
1588                         dbg("marking page %d as containing damaged data",
1589                                 page);
1590                         msb_set_overwrite_flag(msb,
1591                                 pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL);
1592                 }
1593         }
1594
1595         msb_cache_discard(msb);
1596         return error;
1597 }
1598
1599 static int msb_cache_write(struct msb_data *msb, int lba,
1600         int page, bool add_to_cache_only, struct scatterlist *sg, int offset)
1601 {
1602         int error;
1603         struct scatterlist sg_tmp[10];
1604
1605         if (msb->read_only)
1606                 return -EROFS;
1607
1608         if (msb->cache_block_lba == MS_BLOCK_INVALID ||
1609                                                 lba != msb->cache_block_lba)
1610                 if (add_to_cache_only)
1611                         return 0;
1612
1613         /* If we need to write different block */
1614         if (msb->cache_block_lba != MS_BLOCK_INVALID &&
1615                                                 lba != msb->cache_block_lba) {
1616                 dbg_verbose("first flush the cache");
1617                 error = msb_cache_flush(msb);
1618                 if (error)
1619                         return error;
1620         }
1621
1622         if (msb->cache_block_lba  == MS_BLOCK_INVALID) {
1623                 msb->cache_block_lba  = lba;
1624                 mod_timer(&msb->cache_flush_timer,
1625                         jiffies + msecs_to_jiffies(cache_flush_timeout));
1626         }
1627
1628         dbg_verbose("Write of LBA %d page %d to cache ", lba, page);
1629
1630         sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
1631         msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size);
1632
1633         sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp),
1634                 msb->cache + page * msb->page_size, msb->page_size);
1635
1636         set_bit(page, &msb->valid_cache_bitmap);
1637         return 0;
1638 }
1639
1640 static int msb_cache_read(struct msb_data *msb, int lba,
1641                                 int page, struct scatterlist *sg, int offset)
1642 {
1643         int pba = msb->lba_to_pba_table[lba];
1644         struct scatterlist sg_tmp[10];
1645         int error = 0;
1646
1647         if (lba == msb->cache_block_lba &&
1648                         test_bit(page, &msb->valid_cache_bitmap)) {
1649
1650                 dbg_verbose("Read of LBA %d (pba %d) sector %d from cache",
1651                                                         lba, pba, page);
1652
1653                 sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp));
1654                 msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp),
1655                         offset, msb->page_size);
1656                 sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp),
1657                         msb->cache + msb->page_size * page,
1658                                                         msb->page_size);
1659         } else {
1660                 dbg_verbose("Read of LBA %d (pba %d) sector %d from device",
1661                                                         lba, pba, page);
1662
1663                 error = msb_read_page(msb, pba, page, NULL, sg, offset);
1664                 if (error)
1665                         return error;
1666
1667                 msb_cache_write(msb, lba, page, true, sg, offset);
1668         }
1669         return error;
1670 }
1671
1672 /* Emulated geometry table
1673  * This table content isn't that importaint,
1674  * One could put here different values, providing that they still
1675  * cover whole disk.
1676  * 64 MB entry is what windows reports for my 64M memstick */
1677
1678 static const struct chs_entry chs_table[] = {
1679 /*        size sectors cylynders  heads */
1680         { 4,    16,    247,       2  },
1681         { 8,    16,    495,       2  },
1682         { 16,   16,    495,       4  },
1683         { 32,   16,    991,       4  },
1684         { 64,   16,    991,       8  },
1685         {128,   16,    991,       16 },
1686         { 0 }
1687 };
1688
1689 /* Load information about the card */
1690 static int msb_init_card(struct memstick_dev *card)
1691 {
1692         struct msb_data *msb = memstick_get_drvdata(card);
1693         struct memstick_host *host = card->host;
1694         struct ms_boot_page *boot_block;
1695         int error = 0, i, raw_size_in_megs;
1696
1697         msb->caps = 0;
1698
1699         if (card->id.class >= MEMSTICK_CLASS_ROM &&
1700                                 card->id.class <= MEMSTICK_CLASS_ROM)
1701                 msb->read_only = true;
1702
1703         msb->state = -1;
1704         error = msb_reset(msb, false);
1705         if (error)
1706                 return error;
1707
1708         /* Due to a bug in Jmicron driver written by Alex Dubov,
1709          its serial mode barely works,
1710          so we switch to parallel mode right away */
1711         if (host->caps & MEMSTICK_CAP_PAR4)
1712                 msb_switch_to_parallel(msb);
1713
1714         msb->page_size = sizeof(struct ms_boot_page);
1715
1716         /* Read the boot page */
1717         error = msb_read_boot_blocks(msb);
1718         if (error)
1719                 return -EIO;
1720
1721         boot_block = &msb->boot_page[0];
1722
1723         /* Save intersting attributes from boot page */
1724         msb->block_count = boot_block->attr.number_of_blocks;
1725         msb->page_size = boot_block->attr.page_size;
1726
1727         msb->pages_in_block = boot_block->attr.block_size * 2;
1728         msb->block_size = msb->page_size * msb->pages_in_block;
1729
1730         if (msb->page_size > PAGE_SIZE) {
1731                 /* this isn't supported by linux at all, anyway*/
1732                 dbg("device page %d size isn't supported", msb->page_size);
1733                 return -EINVAL;
1734         }
1735
1736         msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL);
1737         if (!msb->block_buffer)
1738                 return -ENOMEM;
1739
1740         raw_size_in_megs = (msb->block_size * msb->block_count) >> 20;
1741
1742         for (i = 0; chs_table[i].size; i++) {
1743
1744                 if (chs_table[i].size != raw_size_in_megs)
1745                         continue;
1746
1747                 msb->geometry.cylinders = chs_table[i].cyl;
1748                 msb->geometry.heads = chs_table[i].head;
1749                 msb->geometry.sectors = chs_table[i].sec;
1750                 break;
1751         }
1752
1753         if (boot_block->attr.transfer_supporting == 1)
1754                 msb->caps |= MEMSTICK_CAP_PAR4;
1755
1756         if (boot_block->attr.device_type & 0x03)
1757                 msb->read_only = true;
1758
1759         dbg("Total block count = %d", msb->block_count);
1760         dbg("Each block consists of %d pages", msb->pages_in_block);
1761         dbg("Page size = %d bytes", msb->page_size);
1762         dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4));
1763         dbg("Read only: %d", msb->read_only);
1764
1765 #if 0
1766         /* Now we can switch the interface */
1767         if (host->caps & msb->caps & MEMSTICK_CAP_PAR4)
1768                 msb_switch_to_parallel(msb);
1769 #endif
1770
1771         error = msb_cache_init(msb);
1772         if (error)
1773                 return error;
1774
1775         error = msb_ftl_initialize(msb);
1776         if (error)
1777                 return error;
1778
1779
1780         /* Read the bad block table */
1781         error = msb_read_bad_block_table(msb, 0);
1782
1783         if (error && error != -ENOMEM) {
1784                 dbg("failed to read bad block table from primary boot block, trying from backup");
1785                 error = msb_read_bad_block_table(msb, 1);
1786         }
1787
1788         if (error)
1789                 return error;
1790
1791         /* *drum roll* Scan the media */
1792         error = msb_ftl_scan(msb);
1793         if (error) {
1794                 pr_err("Scan of media failed");
1795                 return error;
1796         }
1797
1798         return 0;
1799
1800 }
1801
1802 static int msb_do_write_request(struct msb_data *msb, int lba,
1803         int page, struct scatterlist *sg, size_t len, int *sucessfuly_written)
1804 {
1805         int error = 0;
1806         off_t offset = 0;
1807         *sucessfuly_written = 0;
1808
1809         while (offset < len) {
1810                 if (page == 0 && len - offset >= msb->block_size) {
1811
1812                         if (msb->cache_block_lba == lba)
1813                                 msb_cache_discard(msb);
1814
1815                         dbg_verbose("Writing whole lba %d", lba);
1816                         error = msb_update_block(msb, lba, sg, offset);
1817                         if (error)
1818                                 return error;
1819
1820                         offset += msb->block_size;
1821                         *sucessfuly_written += msb->block_size;
1822                         lba++;
1823                         continue;
1824                 }
1825
1826                 error = msb_cache_write(msb, lba, page, false, sg, offset);
1827                 if (error)
1828                         return error;
1829
1830                 offset += msb->page_size;
1831                 *sucessfuly_written += msb->page_size;
1832
1833                 page++;
1834                 if (page == msb->pages_in_block) {
1835                         page = 0;
1836                         lba++;
1837                 }
1838         }
1839         return 0;
1840 }
1841
1842 static int msb_do_read_request(struct msb_data *msb, int lba,
1843                 int page, struct scatterlist *sg, int len, int *sucessfuly_read)
1844 {
1845         int error = 0;
1846         int offset = 0;
1847         *sucessfuly_read = 0;
1848
1849         while (offset < len) {
1850
1851                 error = msb_cache_read(msb, lba, page, sg, offset);
1852                 if (error)
1853                         return error;
1854
1855                 offset += msb->page_size;
1856                 *sucessfuly_read += msb->page_size;
1857
1858                 page++;
1859                 if (page == msb->pages_in_block) {
1860                         page = 0;
1861                         lba++;
1862                 }
1863         }
1864         return 0;
1865 }
1866
1867 static void msb_io_work(struct work_struct *work)
1868 {
1869         struct msb_data *msb = container_of(work, struct msb_data, io_work);
1870         int page, error, len;
1871         sector_t lba;
1872         struct scatterlist *sg = msb->prealloc_sg;
1873         struct request *req;
1874
1875         dbg_verbose("IO: work started");
1876
1877         while (1) {
1878                 spin_lock_irq(&msb->q_lock);
1879
1880                 if (msb->need_flush_cache) {
1881                         msb->need_flush_cache = false;
1882                         spin_unlock_irq(&msb->q_lock);
1883                         msb_cache_flush(msb);
1884                         continue;
1885                 }
1886
1887                 req = msb->req;
1888                 if (!req) {
1889                         dbg_verbose("IO: no more requests exiting");
1890                         spin_unlock_irq(&msb->q_lock);
1891                         return;
1892                 }
1893
1894                 spin_unlock_irq(&msb->q_lock);
1895
1896                 /* process the request */
1897                 dbg_verbose("IO: processing new request");
1898                 blk_rq_map_sg(msb->queue, req, sg);
1899
1900                 lba = blk_rq_pos(req);
1901
1902                 sector_div(lba, msb->page_size / 512);
1903                 page = sector_div(lba, msb->pages_in_block);
1904
1905                 if (rq_data_dir(msb->req) == READ)
1906                         error = msb_do_read_request(msb, lba, page, sg,
1907                                 blk_rq_bytes(req), &len);
1908                 else
1909                         error = msb_do_write_request(msb, lba, page, sg,
1910                                 blk_rq_bytes(req), &len);
1911
1912                 if (len && !blk_update_request(req, BLK_STS_OK, len)) {
1913                         __blk_mq_end_request(req, BLK_STS_OK);
1914                         spin_lock_irq(&msb->q_lock);
1915                         msb->req = NULL;
1916                         spin_unlock_irq(&msb->q_lock);
1917                 }
1918
1919                 if (error && msb->req) {
1920                         blk_status_t ret = errno_to_blk_status(error);
1921
1922                         dbg_verbose("IO: ending one sector of the request with error");
1923                         blk_mq_end_request(req, ret);
1924                         spin_lock_irq(&msb->q_lock);
1925                         msb->req = NULL;
1926                         spin_unlock_irq(&msb->q_lock);
1927                 }
1928
1929                 if (msb->req)
1930                         dbg_verbose("IO: request still pending");
1931         }
1932 }
1933
1934 static DEFINE_IDR(msb_disk_idr); /*set of used disk numbers */
1935 static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */
1936
1937 static int msb_bd_open(struct block_device *bdev, fmode_t mode)
1938 {
1939         struct gendisk *disk = bdev->bd_disk;
1940         struct msb_data *msb = disk->private_data;
1941
1942         dbg_verbose("block device open");
1943
1944         mutex_lock(&msb_disk_lock);
1945
1946         if (msb && msb->card)
1947                 msb->usage_count++;
1948
1949         mutex_unlock(&msb_disk_lock);
1950         return 0;
1951 }
1952
1953 static void msb_data_clear(struct msb_data *msb)
1954 {
1955         kfree(msb->boot_page);
1956         kfree(msb->used_blocks_bitmap);
1957         kfree(msb->lba_to_pba_table);
1958         kfree(msb->cache);
1959         msb->card = NULL;
1960 }
1961
1962 static int msb_disk_release(struct gendisk *disk)
1963 {
1964         struct msb_data *msb = disk->private_data;
1965
1966         dbg_verbose("block device release");
1967         mutex_lock(&msb_disk_lock);
1968
1969         if (msb) {
1970                 if (msb->usage_count)
1971                         msb->usage_count--;
1972
1973                 if (!msb->usage_count) {
1974                         disk->private_data = NULL;
1975                         idr_remove(&msb_disk_idr, msb->disk_id);
1976                         put_disk(disk);
1977                         kfree(msb);
1978                 }
1979         }
1980         mutex_unlock(&msb_disk_lock);
1981         return 0;
1982 }
1983
1984 static void msb_bd_release(struct gendisk *disk, fmode_t mode)
1985 {
1986         msb_disk_release(disk);
1987 }
1988
1989 static int msb_bd_getgeo(struct block_device *bdev,
1990                                  struct hd_geometry *geo)
1991 {
1992         struct msb_data *msb = bdev->bd_disk->private_data;
1993         *geo = msb->geometry;
1994         return 0;
1995 }
1996
1997 static blk_status_t msb_queue_rq(struct blk_mq_hw_ctx *hctx,
1998                                  const struct blk_mq_queue_data *bd)
1999 {
2000         struct memstick_dev *card = hctx->queue->queuedata;
2001         struct msb_data *msb = memstick_get_drvdata(card);
2002         struct request *req = bd->rq;
2003
2004         dbg_verbose("Submit request");
2005
2006         spin_lock_irq(&msb->q_lock);
2007
2008         if (msb->card_dead) {
2009                 dbg("Refusing requests on removed card");
2010
2011                 WARN_ON(!msb->io_queue_stopped);
2012
2013                 spin_unlock_irq(&msb->q_lock);
2014                 blk_mq_start_request(req);
2015                 return BLK_STS_IOERR;
2016         }
2017
2018         if (msb->req) {
2019                 spin_unlock_irq(&msb->q_lock);
2020                 return BLK_STS_DEV_RESOURCE;
2021         }
2022
2023         blk_mq_start_request(req);
2024         msb->req = req;
2025
2026         if (!msb->io_queue_stopped)
2027                 queue_work(msb->io_queue, &msb->io_work);
2028
2029         spin_unlock_irq(&msb->q_lock);
2030         return BLK_STS_OK;
2031 }
2032
2033 static int msb_check_card(struct memstick_dev *card)
2034 {
2035         struct msb_data *msb = memstick_get_drvdata(card);
2036         return (msb->card_dead == 0);
2037 }
2038
2039 static void msb_stop(struct memstick_dev *card)
2040 {
2041         struct msb_data *msb = memstick_get_drvdata(card);
2042         unsigned long flags;
2043
2044         dbg("Stopping all msblock IO");
2045
2046         blk_mq_stop_hw_queues(msb->queue);
2047         spin_lock_irqsave(&msb->q_lock, flags);
2048         msb->io_queue_stopped = true;
2049         spin_unlock_irqrestore(&msb->q_lock, flags);
2050
2051         del_timer_sync(&msb->cache_flush_timer);
2052         flush_workqueue(msb->io_queue);
2053
2054         spin_lock_irqsave(&msb->q_lock, flags);
2055         if (msb->req) {
2056                 blk_mq_requeue_request(msb->req, false);
2057                 msb->req = NULL;
2058         }
2059         spin_unlock_irqrestore(&msb->q_lock, flags);
2060 }
2061
2062 static void msb_start(struct memstick_dev *card)
2063 {
2064         struct msb_data *msb = memstick_get_drvdata(card);
2065         unsigned long flags;
2066
2067         dbg("Resuming IO from msblock");
2068
2069         msb_invalidate_reg_window(msb);
2070
2071         spin_lock_irqsave(&msb->q_lock, flags);
2072         if (!msb->io_queue_stopped || msb->card_dead) {
2073                 spin_unlock_irqrestore(&msb->q_lock, flags);
2074                 return;
2075         }
2076         spin_unlock_irqrestore(&msb->q_lock, flags);
2077
2078         /* Kick cache flush anyway, its harmless */
2079         msb->need_flush_cache = true;
2080         msb->io_queue_stopped = false;
2081
2082         blk_mq_start_hw_queues(msb->queue);
2083
2084         queue_work(msb->io_queue, &msb->io_work);
2085
2086 }
2087
2088 static const struct block_device_operations msb_bdops = {
2089         .open    = msb_bd_open,
2090         .release = msb_bd_release,
2091         .getgeo  = msb_bd_getgeo,
2092         .owner   = THIS_MODULE
2093 };
2094
2095 static const struct blk_mq_ops msb_mq_ops = {
2096         .queue_rq       = msb_queue_rq,
2097 };
2098
2099 /* Registers the block device */
2100 static int msb_init_disk(struct memstick_dev *card)
2101 {
2102         struct msb_data *msb = memstick_get_drvdata(card);
2103         int rc;
2104         unsigned long capacity;
2105
2106         mutex_lock(&msb_disk_lock);
2107         msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL);
2108         mutex_unlock(&msb_disk_lock);
2109
2110         if (msb->disk_id  < 0)
2111                 return msb->disk_id;
2112
2113         msb->disk = alloc_disk(0);
2114         if (!msb->disk) {
2115                 rc = -ENOMEM;
2116                 goto out_release_id;
2117         }
2118
2119         msb->queue = blk_mq_init_sq_queue(&msb->tag_set, &msb_mq_ops, 2,
2120                                                 BLK_MQ_F_SHOULD_MERGE);
2121         if (IS_ERR(msb->queue)) {
2122                 rc = PTR_ERR(msb->queue);
2123                 msb->queue = NULL;
2124                 goto out_put_disk;
2125         }
2126
2127         msb->queue->queuedata = card;
2128
2129         blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
2130         blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
2131         blk_queue_max_segment_size(msb->queue,
2132                                    MS_BLOCK_MAX_PAGES * msb->page_size);
2133         blk_queue_logical_block_size(msb->queue, msb->page_size);
2134
2135         sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
2136         msb->disk->fops = &msb_bdops;
2137         msb->disk->private_data = msb;
2138         msb->disk->queue = msb->queue;
2139         msb->disk->flags |= GENHD_FL_EXT_DEVT;
2140
2141         capacity = msb->pages_in_block * msb->logical_block_count;
2142         capacity *= (msb->page_size / 512);
2143         set_capacity(msb->disk, capacity);
2144         dbg("Set total disk size to %lu sectors", capacity);
2145
2146         msb->usage_count = 1;
2147         msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
2148         INIT_WORK(&msb->io_work, msb_io_work);
2149         sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
2150
2151         if (msb->read_only)
2152                 set_disk_ro(msb->disk, 1);
2153
2154         msb_start(card);
2155         device_add_disk(&card->dev, msb->disk, NULL);
2156         dbg("Disk added");
2157         return 0;
2158
2159 out_put_disk:
2160         put_disk(msb->disk);
2161 out_release_id:
2162         mutex_lock(&msb_disk_lock);
2163         idr_remove(&msb_disk_idr, msb->disk_id);
2164         mutex_unlock(&msb_disk_lock);
2165         return rc;
2166 }
2167
2168 static int msb_probe(struct memstick_dev *card)
2169 {
2170         struct msb_data *msb;
2171         int rc = 0;
2172
2173         msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
2174         if (!msb)
2175                 return -ENOMEM;
2176         memstick_set_drvdata(card, msb);
2177         msb->card = card;
2178         spin_lock_init(&msb->q_lock);
2179
2180         rc = msb_init_card(card);
2181         if (rc)
2182                 goto out_free;
2183
2184         rc = msb_init_disk(card);
2185         if (!rc) {
2186                 card->check = msb_check_card;
2187                 card->stop = msb_stop;
2188                 card->start = msb_start;
2189                 return 0;
2190         }
2191 out_free:
2192         memstick_set_drvdata(card, NULL);
2193         msb_data_clear(msb);
2194         kfree(msb);
2195         return rc;
2196 }
2197
2198 static void msb_remove(struct memstick_dev *card)
2199 {
2200         struct msb_data *msb = memstick_get_drvdata(card);
2201         unsigned long flags;
2202
2203         if (!msb->io_queue_stopped)
2204                 msb_stop(card);
2205
2206         dbg("Removing the disk device");
2207
2208         /* Take care of unhandled + new requests from now on */
2209         spin_lock_irqsave(&msb->q_lock, flags);
2210         msb->card_dead = true;
2211         spin_unlock_irqrestore(&msb->q_lock, flags);
2212         blk_mq_start_hw_queues(msb->queue);
2213
2214         /* Remove the disk */
2215         del_gendisk(msb->disk);
2216         blk_cleanup_queue(msb->queue);
2217         blk_mq_free_tag_set(&msb->tag_set);
2218         msb->queue = NULL;
2219
2220         mutex_lock(&msb_disk_lock);
2221         msb_data_clear(msb);
2222         mutex_unlock(&msb_disk_lock);
2223
2224         msb_disk_release(msb->disk);
2225         memstick_set_drvdata(card, NULL);
2226 }
2227
2228 #ifdef CONFIG_PM
2229
2230 static int msb_suspend(struct memstick_dev *card, pm_message_t state)
2231 {
2232         msb_stop(card);
2233         return 0;
2234 }
2235
2236 static int msb_resume(struct memstick_dev *card)
2237 {
2238         struct msb_data *msb = memstick_get_drvdata(card);
2239         struct msb_data *new_msb = NULL;
2240         bool card_dead = true;
2241
2242 #ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
2243         msb->card_dead = true;
2244         return 0;
2245 #endif
2246         mutex_lock(&card->host->lock);
2247
2248         new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
2249         if (!new_msb)
2250                 goto out;
2251
2252         new_msb->card = card;
2253         memstick_set_drvdata(card, new_msb);
2254         spin_lock_init(&new_msb->q_lock);
2255         sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
2256
2257         if (msb_init_card(card))
2258                 goto out;
2259
2260         if (msb->block_size != new_msb->block_size)
2261                 goto out;
2262
2263         if (memcmp(msb->boot_page, new_msb->boot_page,
2264                                         sizeof(struct ms_boot_page)))
2265                 goto out;
2266
2267         if (msb->logical_block_count != new_msb->logical_block_count ||
2268                 memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
2269                                                 msb->logical_block_count))
2270                 goto out;
2271
2272         if (msb->block_count != new_msb->block_count ||
2273                 memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
2274                                                         msb->block_count / 8))
2275                 goto out;
2276
2277         card_dead = false;
2278 out:
2279         if (card_dead)
2280                 dbg("Card was removed/replaced during suspend");
2281
2282         msb->card_dead = card_dead;
2283         memstick_set_drvdata(card, msb);
2284
2285         if (new_msb) {
2286                 msb_data_clear(new_msb);
2287                 kfree(new_msb);
2288         }
2289
2290         msb_start(card);
2291         mutex_unlock(&card->host->lock);
2292         return 0;
2293 }
2294 #else
2295
2296 #define msb_suspend NULL
2297 #define msb_resume NULL
2298
2299 #endif /* CONFIG_PM */
2300
2301 static struct memstick_device_id msb_id_tbl[] = {
2302         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2303          MEMSTICK_CLASS_FLASH},
2304
2305         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2306          MEMSTICK_CLASS_ROM},
2307
2308         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2309          MEMSTICK_CLASS_RO},
2310
2311         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2312          MEMSTICK_CLASS_WP},
2313
2314         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
2315          MEMSTICK_CLASS_DUO},
2316         {}
2317 };
2318 MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
2319
2320
2321 static struct memstick_driver msb_driver = {
2322         .driver = {
2323                 .name  = DRIVER_NAME,
2324                 .owner = THIS_MODULE
2325         },
2326         .id_table = msb_id_tbl,
2327         .probe    = msb_probe,
2328         .remove   = msb_remove,
2329         .suspend  = msb_suspend,
2330         .resume   = msb_resume
2331 };
2332
2333 static int __init msb_init(void)
2334 {
2335         int rc = memstick_register_driver(&msb_driver);
2336         if (rc)
2337                 pr_err("failed to register memstick driver (error %d)\n", rc);
2338
2339         return rc;
2340 }
2341
2342 static void __exit msb_exit(void)
2343 {
2344         memstick_unregister_driver(&msb_driver);
2345         idr_destroy(&msb_disk_idr);
2346 }
2347
2348 module_init(msb_init);
2349 module_exit(msb_exit);
2350
2351 module_param(cache_flush_timeout, int, S_IRUGO);
2352 MODULE_PARM_DESC(cache_flush_timeout,
2353                                 "Cache flush timeout in msec (1000 default)");
2354 module_param(debug, int, S_IRUGO | S_IWUSR);
2355 MODULE_PARM_DESC(debug, "Debug level (0-2)");
2356
2357 module_param(verify_writes, bool, S_IRUGO);
2358 MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
2359
2360 MODULE_LICENSE("GPL");
2361 MODULE_AUTHOR("Maxim Levitsky");
2362 MODULE_DESCRIPTION("Sony MemoryStick block device driver");