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