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 randomly: %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't 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 = bitmap_zalloc(msb->block_count, GFP_KERNEL);
1339         msb->erased_blocks_bitmap = bitmap_zalloc(msb->block_count, 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                 bitmap_free(msb->used_blocks_bitmap);
1347                 bitmap_free(msb->erased_blocks_bitmap);
1348                 kfree(msb->lba_to_pba_table);
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 ((size_t)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         bitmap_free(msb->used_blocks_bitmap);
1957         bitmap_free(msb->erased_blocks_bitmap);
1958         kfree(msb->lba_to_pba_table);
1959         kfree(msb->cache);
1960         msb->card = NULL;
1961 }
1962
1963 static int msb_disk_release(struct gendisk *disk)
1964 {
1965         struct msb_data *msb = disk->private_data;
1966
1967         dbg_verbose("block device release");
1968         mutex_lock(&msb_disk_lock);
1969
1970         if (msb) {
1971                 if (msb->usage_count)
1972                         msb->usage_count--;
1973
1974                 if (!msb->usage_count) {
1975                         disk->private_data = NULL;
1976                         idr_remove(&msb_disk_idr, msb->disk_id);
1977                         put_disk(disk);
1978                         kfree(msb);
1979                 }
1980         }
1981         mutex_unlock(&msb_disk_lock);
1982         return 0;
1983 }
1984
1985 static void msb_bd_release(struct gendisk *disk, fmode_t mode)
1986 {
1987         msb_disk_release(disk);
1988 }
1989
1990 static int msb_bd_getgeo(struct block_device *bdev,
1991                                  struct hd_geometry *geo)
1992 {
1993         struct msb_data *msb = bdev->bd_disk->private_data;
1994         *geo = msb->geometry;
1995         return 0;
1996 }
1997
1998 static blk_status_t msb_queue_rq(struct blk_mq_hw_ctx *hctx,
1999                                  const struct blk_mq_queue_data *bd)
2000 {
2001         struct memstick_dev *card = hctx->queue->queuedata;
2002         struct msb_data *msb = memstick_get_drvdata(card);
2003         struct request *req = bd->rq;
2004
2005         dbg_verbose("Submit request");
2006
2007         spin_lock_irq(&msb->q_lock);
2008
2009         if (msb->card_dead) {
2010                 dbg("Refusing requests on removed card");
2011
2012                 WARN_ON(!msb->io_queue_stopped);
2013
2014                 spin_unlock_irq(&msb->q_lock);
2015                 blk_mq_start_request(req);
2016                 return BLK_STS_IOERR;
2017         }
2018
2019         if (msb->req) {
2020                 spin_unlock_irq(&msb->q_lock);
2021                 return BLK_STS_DEV_RESOURCE;
2022         }
2023
2024         blk_mq_start_request(req);
2025         msb->req = req;
2026
2027         if (!msb->io_queue_stopped)
2028                 queue_work(msb->io_queue, &msb->io_work);
2029
2030         spin_unlock_irq(&msb->q_lock);
2031         return BLK_STS_OK;
2032 }
2033
2034 static int msb_check_card(struct memstick_dev *card)
2035 {
2036         struct msb_data *msb = memstick_get_drvdata(card);
2037         return (msb->card_dead == 0);
2038 }
2039
2040 static void msb_stop(struct memstick_dev *card)
2041 {
2042         struct msb_data *msb = memstick_get_drvdata(card);
2043         unsigned long flags;
2044
2045         dbg("Stopping all msblock IO");
2046
2047         blk_mq_stop_hw_queues(msb->queue);
2048         spin_lock_irqsave(&msb->q_lock, flags);
2049         msb->io_queue_stopped = true;
2050         spin_unlock_irqrestore(&msb->q_lock, flags);
2051
2052         del_timer_sync(&msb->cache_flush_timer);
2053         flush_workqueue(msb->io_queue);
2054
2055         spin_lock_irqsave(&msb->q_lock, flags);
2056         if (msb->req) {
2057                 blk_mq_requeue_request(msb->req, false);
2058                 msb->req = NULL;
2059         }
2060         spin_unlock_irqrestore(&msb->q_lock, flags);
2061 }
2062
2063 static void msb_start(struct memstick_dev *card)
2064 {
2065         struct msb_data *msb = memstick_get_drvdata(card);
2066         unsigned long flags;
2067
2068         dbg("Resuming IO from msblock");
2069
2070         msb_invalidate_reg_window(msb);
2071
2072         spin_lock_irqsave(&msb->q_lock, flags);
2073         if (!msb->io_queue_stopped || msb->card_dead) {
2074                 spin_unlock_irqrestore(&msb->q_lock, flags);
2075                 return;
2076         }
2077         spin_unlock_irqrestore(&msb->q_lock, flags);
2078
2079         /* Kick cache flush anyway, its harmless */
2080         msb->need_flush_cache = true;
2081         msb->io_queue_stopped = false;
2082
2083         blk_mq_start_hw_queues(msb->queue);
2084
2085         queue_work(msb->io_queue, &msb->io_work);
2086
2087 }
2088
2089 static const struct block_device_operations msb_bdops = {
2090         .open    = msb_bd_open,
2091         .release = msb_bd_release,
2092         .getgeo  = msb_bd_getgeo,
2093         .owner   = THIS_MODULE
2094 };
2095
2096 static const struct blk_mq_ops msb_mq_ops = {
2097         .queue_rq       = msb_queue_rq,
2098 };
2099
2100 /* Registers the block device */
2101 static int msb_init_disk(struct memstick_dev *card)
2102 {
2103         struct msb_data *msb = memstick_get_drvdata(card);
2104         int rc;
2105         unsigned long capacity;
2106
2107         mutex_lock(&msb_disk_lock);
2108         msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL);
2109         mutex_unlock(&msb_disk_lock);
2110
2111         if (msb->disk_id  < 0)
2112                 return msb->disk_id;
2113
2114         msb->disk = alloc_disk(0);
2115         if (!msb->disk) {
2116                 rc = -ENOMEM;
2117                 goto out_release_id;
2118         }
2119
2120         msb->queue = blk_mq_init_sq_queue(&msb->tag_set, &msb_mq_ops, 2,
2121                                                 BLK_MQ_F_SHOULD_MERGE);
2122         if (IS_ERR(msb->queue)) {
2123                 rc = PTR_ERR(msb->queue);
2124                 msb->queue = NULL;
2125                 goto out_put_disk;
2126         }
2127
2128         msb->queue->queuedata = card;
2129
2130         blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES);
2131         blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS);
2132         blk_queue_max_segment_size(msb->queue,
2133                                    MS_BLOCK_MAX_PAGES * msb->page_size);
2134         blk_queue_logical_block_size(msb->queue, msb->page_size);
2135
2136         sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id);
2137         msb->disk->fops = &msb_bdops;
2138         msb->disk->private_data = msb;
2139         msb->disk->queue = msb->queue;
2140         msb->disk->flags |= GENHD_FL_EXT_DEVT;
2141
2142         capacity = msb->pages_in_block * msb->logical_block_count;
2143         capacity *= (msb->page_size / 512);
2144         set_capacity(msb->disk, capacity);
2145         dbg("Set total disk size to %lu sectors", capacity);
2146
2147         msb->usage_count = 1;
2148         msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM);
2149         INIT_WORK(&msb->io_work, msb_io_work);
2150         sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
2151
2152         if (msb->read_only)
2153                 set_disk_ro(msb->disk, 1);
2154
2155         msb_start(card);
2156         device_add_disk(&card->dev, msb->disk, NULL);
2157         dbg("Disk added");
2158         return 0;
2159
2160 out_put_disk:
2161         put_disk(msb->disk);
2162 out_release_id:
2163         mutex_lock(&msb_disk_lock);
2164         idr_remove(&msb_disk_idr, msb->disk_id);
2165         mutex_unlock(&msb_disk_lock);
2166         return rc;
2167 }
2168
2169 static int msb_probe(struct memstick_dev *card)
2170 {
2171         struct msb_data *msb;
2172         int rc = 0;
2173
2174         msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
2175         if (!msb)
2176                 return -ENOMEM;
2177         memstick_set_drvdata(card, msb);
2178         msb->card = card;
2179         spin_lock_init(&msb->q_lock);
2180
2181         rc = msb_init_card(card);
2182         if (rc)
2183                 goto out_free;
2184
2185         rc = msb_init_disk(card);
2186         if (!rc) {
2187                 card->check = msb_check_card;
2188                 card->stop = msb_stop;
2189                 card->start = msb_start;
2190                 return 0;
2191         }
2192 out_free:
2193         memstick_set_drvdata(card, NULL);
2194         msb_data_clear(msb);
2195         kfree(msb);
2196         return rc;
2197 }
2198
2199 static void msb_remove(struct memstick_dev *card)
2200 {
2201         struct msb_data *msb = memstick_get_drvdata(card);
2202         unsigned long flags;
2203
2204         if (!msb->io_queue_stopped)
2205                 msb_stop(card);
2206
2207         dbg("Removing the disk device");
2208
2209         /* Take care of unhandled + new requests from now on */
2210         spin_lock_irqsave(&msb->q_lock, flags);
2211         msb->card_dead = true;
2212         spin_unlock_irqrestore(&msb->q_lock, flags);
2213         blk_mq_start_hw_queues(msb->queue);
2214
2215         /* Remove the disk */
2216         del_gendisk(msb->disk);
2217         blk_cleanup_queue(msb->queue);
2218         blk_mq_free_tag_set(&msb->tag_set);
2219         msb->queue = NULL;
2220
2221         mutex_lock(&msb_disk_lock);
2222         msb_data_clear(msb);
2223         mutex_unlock(&msb_disk_lock);
2224
2225         msb_disk_release(msb->disk);
2226         memstick_set_drvdata(card, NULL);
2227 }
2228
2229 #ifdef CONFIG_PM
2230
2231 static int msb_suspend(struct memstick_dev *card, pm_message_t state)
2232 {
2233         msb_stop(card);
2234         return 0;
2235 }
2236
2237 static int msb_resume(struct memstick_dev *card)
2238 {
2239         struct msb_data *msb = memstick_get_drvdata(card);
2240         struct msb_data *new_msb = NULL;
2241         bool card_dead = true;
2242
2243 #ifndef CONFIG_MEMSTICK_UNSAFE_RESUME
2244         msb->card_dead = true;
2245         return 0;
2246 #endif
2247         mutex_lock(&card->host->lock);
2248
2249         new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL);
2250         if (!new_msb)
2251                 goto out;
2252
2253         new_msb->card = card;
2254         memstick_set_drvdata(card, new_msb);
2255         spin_lock_init(&new_msb->q_lock);
2256         sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1);
2257
2258         if (msb_init_card(card))
2259                 goto out;
2260
2261         if (msb->block_size != new_msb->block_size)
2262                 goto out;
2263
2264         if (memcmp(msb->boot_page, new_msb->boot_page,
2265                                         sizeof(struct ms_boot_page)))
2266                 goto out;
2267
2268         if (msb->logical_block_count != new_msb->logical_block_count ||
2269                 memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table,
2270                                                 msb->logical_block_count))
2271                 goto out;
2272
2273         if (msb->block_count != new_msb->block_count ||
2274                 memcmp(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
2275                                                         msb->block_count / 8))
2276                 goto out;
2277
2278         card_dead = false;
2279 out:
2280         if (card_dead)
2281                 dbg("Card was removed/replaced during suspend");
2282
2283         msb->card_dead = card_dead;
2284         memstick_set_drvdata(card, msb);
2285
2286         if (new_msb) {
2287                 msb_data_clear(new_msb);
2288                 kfree(new_msb);
2289         }
2290
2291         msb_start(card);
2292         mutex_unlock(&card->host->lock);
2293         return 0;
2294 }
2295 #else
2296
2297 #define msb_suspend NULL
2298 #define msb_resume NULL
2299
2300 #endif /* CONFIG_PM */
2301
2302 static struct memstick_device_id msb_id_tbl[] = {
2303         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2304          MEMSTICK_CLASS_FLASH},
2305
2306         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2307          MEMSTICK_CLASS_ROM},
2308
2309         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2310          MEMSTICK_CLASS_RO},
2311
2312         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE,
2313          MEMSTICK_CLASS_WP},
2314
2315         {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO,
2316          MEMSTICK_CLASS_DUO},
2317         {}
2318 };
2319 MODULE_DEVICE_TABLE(memstick, msb_id_tbl);
2320
2321
2322 static struct memstick_driver msb_driver = {
2323         .driver = {
2324                 .name  = DRIVER_NAME,
2325                 .owner = THIS_MODULE
2326         },
2327         .id_table = msb_id_tbl,
2328         .probe    = msb_probe,
2329         .remove   = msb_remove,
2330         .suspend  = msb_suspend,
2331         .resume   = msb_resume
2332 };
2333
2334 static int __init msb_init(void)
2335 {
2336         int rc = memstick_register_driver(&msb_driver);
2337         if (rc)
2338                 pr_err("failed to register memstick driver (error %d)\n", rc);
2339
2340         return rc;
2341 }
2342
2343 static void __exit msb_exit(void)
2344 {
2345         memstick_unregister_driver(&msb_driver);
2346         idr_destroy(&msb_disk_idr);
2347 }
2348
2349 module_init(msb_init);
2350 module_exit(msb_exit);
2351
2352 module_param(cache_flush_timeout, int, S_IRUGO);
2353 MODULE_PARM_DESC(cache_flush_timeout,
2354                                 "Cache flush timeout in msec (1000 default)");
2355 module_param(debug, int, S_IRUGO | S_IWUSR);
2356 MODULE_PARM_DESC(debug, "Debug level (0-2)");
2357
2358 module_param(verify_writes, bool, S_IRUGO);
2359 MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written");
2360
2361 MODULE_LICENSE("GPL");
2362 MODULE_AUTHOR("Maxim Levitsky");
2363 MODULE_DESCRIPTION("Sony MemoryStick block device driver");