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 */ 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 being 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 exactly 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_objs(struct ms_boot_page, 2); 1207 if (!page) 1208 return -ENOMEM; 1209 1210 msb->boot_page = page; 1211 } else 1212 page = msb->boot_page; 1213 1214 msb->block_count = MS_BLOCK_MAX_BOOT_ADDR; 1215 1216 for (pba = 0; pba < MS_BLOCK_MAX_BOOT_ADDR; pba++) { 1217 1218 sg_init_one(&sg, page, sizeof(*page)); 1219 if (msb_read_page(msb, pba, 0, &extra, &sg, 0)) { 1220 dbg("boot scan: can't read pba %d", pba); 1221 continue; 1222 } 1223 1224 if (extra.management_flag & MEMSTICK_MANAGEMENT_SYSFLG) { 1225 dbg("management flag doesn't indicate boot block %d", 1226 pba); 1227 continue; 1228 } 1229 1230 if (be16_to_cpu(page->header.block_id) != MS_BLOCK_BOOT_ID) { 1231 dbg("the pba at %d doesn't contain boot block ID", pba); 1232 continue; 1233 } 1234 1235 msb_fix_boot_page_endianness(page); 1236 msb->boot_block_locations[msb->boot_block_count] = pba; 1237 1238 page++; 1239 msb->boot_block_count++; 1240 1241 if (msb->boot_block_count == 2) 1242 break; 1243 } 1244 1245 if (!msb->boot_block_count) { 1246 pr_err("media doesn't contain master page, aborting"); 1247 return -EIO; 1248 } 1249 1250 dbg_verbose("End of scan for boot blocks"); 1251 return 0; 1252 } 1253 1254 static int msb_read_bad_block_table(struct msb_data *msb, int block_nr) 1255 { 1256 struct ms_boot_page *boot_block; 1257 struct scatterlist sg; 1258 u16 *buffer = NULL; 1259 int offset = 0; 1260 int i, error = 0; 1261 int data_size, data_offset, page, page_offset, size_to_read; 1262 u16 pba; 1263 1264 BUG_ON(block_nr > 1); 1265 boot_block = &msb->boot_page[block_nr]; 1266 pba = msb->boot_block_locations[block_nr]; 1267 1268 if (msb->boot_block_locations[block_nr] == MS_BLOCK_INVALID) 1269 return -EINVAL; 1270 1271 data_size = boot_block->entry.disabled_block.data_size; 1272 data_offset = sizeof(struct ms_boot_page) + 1273 boot_block->entry.disabled_block.start_addr; 1274 if (!data_size) 1275 return 0; 1276 1277 page = data_offset / msb->page_size; 1278 page_offset = data_offset % msb->page_size; 1279 size_to_read = 1280 DIV_ROUND_UP(data_size + page_offset, msb->page_size) * 1281 msb->page_size; 1282 1283 dbg("reading bad block of boot block at pba %d, offset %d len %d", 1284 pba, data_offset, data_size); 1285 1286 buffer = kzalloc(size_to_read, GFP_KERNEL); 1287 if (!buffer) 1288 return -ENOMEM; 1289 1290 /* Read the buffer */ 1291 sg_init_one(&sg, buffer, size_to_read); 1292 1293 while (offset < size_to_read) { 1294 error = msb_read_page(msb, pba, page, NULL, &sg, offset); 1295 if (error) 1296 goto out; 1297 1298 page++; 1299 offset += msb->page_size; 1300 1301 if (page == msb->pages_in_block) { 1302 pr_err( 1303 "bad block table extends beyond the boot block"); 1304 break; 1305 } 1306 } 1307 1308 /* Process the bad block table */ 1309 for (i = page_offset; i < data_size / sizeof(u16); i++) { 1310 1311 u16 bad_block = be16_to_cpu(buffer[i]); 1312 1313 if (bad_block >= msb->block_count) { 1314 dbg("bad block table contains invalid block %d", 1315 bad_block); 1316 continue; 1317 } 1318 1319 if (test_bit(bad_block, msb->used_blocks_bitmap)) { 1320 dbg("duplicate bad block %d in the table", 1321 bad_block); 1322 continue; 1323 } 1324 1325 dbg("block %d is marked as factory bad", bad_block); 1326 msb_mark_block_used(msb, bad_block); 1327 } 1328 out: 1329 kfree(buffer); 1330 return error; 1331 } 1332 1333 static int msb_ftl_initialize(struct msb_data *msb) 1334 { 1335 int i; 1336 1337 if (msb->ftl_initialized) 1338 return 0; 1339 1340 msb->zone_count = msb->block_count / MS_BLOCKS_IN_ZONE; 1341 if (msb->block_count > MS_MAX_ZONES * MS_BLOCKS_IN_ZONE) { 1342 pr_err("Too many blocks: %d\n", msb->block_count); 1343 return -EINVAL; 1344 } 1345 msb->logical_block_count = msb->zone_count * 496 - 2; 1346 1347 msb->used_blocks_bitmap = bitmap_zalloc(msb->block_count, GFP_KERNEL); 1348 msb->erased_blocks_bitmap = bitmap_zalloc(msb->block_count, GFP_KERNEL); 1349 msb->lba_to_pba_table = 1350 kmalloc_objs(u16, msb->logical_block_count); 1351 1352 if (!msb->used_blocks_bitmap || !msb->lba_to_pba_table || 1353 !msb->erased_blocks_bitmap) { 1354 bitmap_free(msb->used_blocks_bitmap); 1355 bitmap_free(msb->erased_blocks_bitmap); 1356 kfree(msb->lba_to_pba_table); 1357 return -ENOMEM; 1358 } 1359 1360 for (i = 0; i < msb->zone_count; i++) 1361 msb->free_block_count[i] = MS_BLOCKS_IN_ZONE; 1362 1363 memset(msb->lba_to_pba_table, MS_BLOCK_INVALID, 1364 msb->logical_block_count * sizeof(u16)); 1365 1366 dbg("initial FTL tables created. Zone count = %d, Logical block count = %d", 1367 msb->zone_count, msb->logical_block_count); 1368 1369 msb->ftl_initialized = true; 1370 return 0; 1371 } 1372 1373 static int msb_ftl_scan(struct msb_data *msb) 1374 { 1375 u16 pba, lba, other_block; 1376 u8 overwrite_flag, management_flag, other_overwrite_flag; 1377 int error; 1378 struct ms_extra_data_register extra; 1379 u8 *overwrite_flags = kzalloc(msb->block_count, GFP_KERNEL); 1380 1381 if (!overwrite_flags) 1382 return -ENOMEM; 1383 1384 dbg("Start of media scanning"); 1385 for (pba = 0; pba < msb->block_count; pba++) { 1386 1387 if (pba == msb->boot_block_locations[0] || 1388 pba == msb->boot_block_locations[1]) { 1389 dbg_verbose("pba %05d -> [boot block]", pba); 1390 msb_mark_block_used(msb, pba); 1391 continue; 1392 } 1393 1394 if (test_bit(pba, msb->used_blocks_bitmap)) { 1395 dbg_verbose("pba %05d -> [factory bad]", pba); 1396 continue; 1397 } 1398 1399 memset(&extra, 0, sizeof(extra)); 1400 error = msb_read_oob(msb, pba, 0, &extra); 1401 1402 /* can't trust the page if we can't read the oob */ 1403 if (error == -EBADMSG) { 1404 pr_notice( 1405 "oob of pba %d damaged, will try to erase it", pba); 1406 msb_mark_block_used(msb, pba); 1407 msb_erase_block(msb, pba); 1408 continue; 1409 } else if (error) { 1410 pr_err("unknown error %d on read of oob of pba %d - aborting", 1411 error, pba); 1412 1413 kfree(overwrite_flags); 1414 return error; 1415 } 1416 1417 lba = be16_to_cpu(extra.logical_address); 1418 management_flag = extra.management_flag; 1419 overwrite_flag = extra.overwrite_flag; 1420 overwrite_flags[pba] = overwrite_flag; 1421 1422 /* Skip bad blocks */ 1423 if (!(overwrite_flag & MEMSTICK_OVERWRITE_BKST)) { 1424 dbg("pba %05d -> [BAD]", pba); 1425 msb_mark_block_used(msb, pba); 1426 continue; 1427 } 1428 1429 /* Skip system/drm blocks */ 1430 if ((management_flag & MEMSTICK_MANAGEMENT_FLAG_NORMAL) != 1431 MEMSTICK_MANAGEMENT_FLAG_NORMAL) { 1432 dbg("pba %05d -> [reserved management flag %02x]", 1433 pba, management_flag); 1434 msb_mark_block_used(msb, pba); 1435 continue; 1436 } 1437 1438 /* Erase temporary tables */ 1439 if (!(management_flag & MEMSTICK_MANAGEMENT_ATFLG)) { 1440 dbg("pba %05d -> [temp table] - will erase", pba); 1441 1442 msb_mark_block_used(msb, pba); 1443 msb_erase_block(msb, pba); 1444 continue; 1445 } 1446 1447 if (lba == MS_BLOCK_INVALID) { 1448 dbg_verbose("pba %05d -> [free]", pba); 1449 continue; 1450 } 1451 1452 msb_mark_block_used(msb, pba); 1453 1454 /* Block has LBA not according to zoning*/ 1455 if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) { 1456 pr_notice("pba %05d -> [bad lba %05d] - will erase", 1457 pba, lba); 1458 msb_erase_block(msb, pba); 1459 continue; 1460 } 1461 1462 /* No collisions - great */ 1463 if (msb->lba_to_pba_table[lba] == MS_BLOCK_INVALID) { 1464 dbg_verbose("pba %05d -> [lba %05d]", pba, lba); 1465 msb->lba_to_pba_table[lba] = pba; 1466 continue; 1467 } 1468 1469 other_block = msb->lba_to_pba_table[lba]; 1470 other_overwrite_flag = overwrite_flags[other_block]; 1471 1472 pr_notice("Collision between pba %d and pba %d", 1473 pba, other_block); 1474 1475 if (!(overwrite_flag & MEMSTICK_OVERWRITE_UDST)) { 1476 pr_notice("pba %d is marked as stable, use it", pba); 1477 msb_erase_block(msb, other_block); 1478 msb->lba_to_pba_table[lba] = pba; 1479 continue; 1480 } 1481 1482 if (!(other_overwrite_flag & MEMSTICK_OVERWRITE_UDST)) { 1483 pr_notice("pba %d is marked as stable, use it", 1484 other_block); 1485 msb_erase_block(msb, pba); 1486 continue; 1487 } 1488 1489 pr_notice("collision between blocks %d and %d, without stable flag set on both, erasing pba %d", 1490 pba, other_block, other_block); 1491 1492 msb_erase_block(msb, other_block); 1493 msb->lba_to_pba_table[lba] = pba; 1494 } 1495 1496 dbg("End of media scanning"); 1497 kfree(overwrite_flags); 1498 return 0; 1499 } 1500 1501 static void msb_cache_flush_timer(struct timer_list *t) 1502 { 1503 struct msb_data *msb = timer_container_of(msb, t, cache_flush_timer); 1504 1505 msb->need_flush_cache = true; 1506 queue_work(msb->io_queue, &msb->io_work); 1507 } 1508 1509 1510 static void msb_cache_discard(struct msb_data *msb) 1511 { 1512 if (msb->cache_block_lba == MS_BLOCK_INVALID) 1513 return; 1514 1515 timer_delete_sync(&msb->cache_flush_timer); 1516 1517 dbg_verbose("Discarding the write cache"); 1518 msb->cache_block_lba = MS_BLOCK_INVALID; 1519 bitmap_zero(&msb->valid_cache_bitmap, msb->pages_in_block); 1520 } 1521 1522 static int msb_cache_init(struct msb_data *msb) 1523 { 1524 timer_setup(&msb->cache_flush_timer, msb_cache_flush_timer, 0); 1525 1526 if (!msb->cache) 1527 msb->cache = kzalloc(msb->block_size, GFP_KERNEL); 1528 if (!msb->cache) 1529 return -ENOMEM; 1530 1531 msb_cache_discard(msb); 1532 return 0; 1533 } 1534 1535 static int msb_cache_flush(struct msb_data *msb) 1536 { 1537 struct scatterlist sg; 1538 struct ms_extra_data_register extra; 1539 int page, offset, error; 1540 u16 pba, lba; 1541 1542 if (msb->read_only) 1543 return -EROFS; 1544 1545 if (msb->cache_block_lba == MS_BLOCK_INVALID) 1546 return 0; 1547 1548 lba = msb->cache_block_lba; 1549 pba = msb->lba_to_pba_table[lba]; 1550 1551 dbg_verbose("Flushing the write cache of pba %d (LBA %d)", 1552 pba, msb->cache_block_lba); 1553 1554 sg_init_one(&sg, msb->cache , msb->block_size); 1555 1556 /* Read all missing pages in cache */ 1557 for (page = 0; page < msb->pages_in_block; page++) { 1558 1559 if (test_bit(page, &msb->valid_cache_bitmap)) 1560 continue; 1561 1562 offset = page * msb->page_size; 1563 1564 dbg_verbose("reading non-present sector %d of cache block %d", 1565 page, lba); 1566 error = msb_read_page(msb, pba, page, &extra, &sg, offset); 1567 1568 /* Bad pages are copied with 00 page status */ 1569 if (error == -EBADMSG) { 1570 pr_err("read error on sector %d, contents probably damaged", page); 1571 continue; 1572 } 1573 1574 if (error) 1575 return error; 1576 1577 if ((extra.overwrite_flag & MEMSTICK_OV_PG_NORMAL) != 1578 MEMSTICK_OV_PG_NORMAL) { 1579 dbg("page %d is marked as bad", page); 1580 continue; 1581 } 1582 1583 set_bit(page, &msb->valid_cache_bitmap); 1584 } 1585 1586 /* Write the cache now */ 1587 error = msb_update_block(msb, msb->cache_block_lba, &sg, 0); 1588 pba = msb->lba_to_pba_table[msb->cache_block_lba]; 1589 1590 /* Mark invalid pages */ 1591 if (!error) { 1592 for (page = 0; page < msb->pages_in_block; page++) { 1593 1594 if (test_bit(page, &msb->valid_cache_bitmap)) 1595 continue; 1596 1597 dbg("marking page %d as containing damaged data", 1598 page); 1599 msb_set_overwrite_flag(msb, 1600 pba , page, 0xFF & ~MEMSTICK_OV_PG_NORMAL); 1601 } 1602 } 1603 1604 msb_cache_discard(msb); 1605 return error; 1606 } 1607 1608 static int msb_cache_write(struct msb_data *msb, int lba, 1609 int page, bool add_to_cache_only, struct scatterlist *sg, int offset) 1610 { 1611 int error; 1612 struct scatterlist sg_tmp[10]; 1613 1614 if (msb->read_only) 1615 return -EROFS; 1616 1617 if (msb->cache_block_lba == MS_BLOCK_INVALID || 1618 lba != msb->cache_block_lba) 1619 if (add_to_cache_only) 1620 return 0; 1621 1622 /* If we need to write different block */ 1623 if (msb->cache_block_lba != MS_BLOCK_INVALID && 1624 lba != msb->cache_block_lba) { 1625 dbg_verbose("first flush the cache"); 1626 error = msb_cache_flush(msb); 1627 if (error) 1628 return error; 1629 } 1630 1631 if (msb->cache_block_lba == MS_BLOCK_INVALID) { 1632 msb->cache_block_lba = lba; 1633 mod_timer(&msb->cache_flush_timer, 1634 jiffies + msecs_to_jiffies(cache_flush_timeout)); 1635 } 1636 1637 dbg_verbose("Write of LBA %d page %d to cache ", lba, page); 1638 1639 sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp)); 1640 msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), offset, msb->page_size); 1641 1642 sg_copy_to_buffer(sg_tmp, sg_nents(sg_tmp), 1643 msb->cache + page * msb->page_size, msb->page_size); 1644 1645 set_bit(page, &msb->valid_cache_bitmap); 1646 return 0; 1647 } 1648 1649 static int msb_cache_read(struct msb_data *msb, int lba, 1650 int page, struct scatterlist *sg, int offset) 1651 { 1652 int pba = msb->lba_to_pba_table[lba]; 1653 struct scatterlist sg_tmp[10]; 1654 int error = 0; 1655 1656 if (lba == msb->cache_block_lba && 1657 test_bit(page, &msb->valid_cache_bitmap)) { 1658 1659 dbg_verbose("Read of LBA %d (pba %d) sector %d from cache", 1660 lba, pba, page); 1661 1662 sg_init_table(sg_tmp, ARRAY_SIZE(sg_tmp)); 1663 msb_sg_copy(sg, sg_tmp, ARRAY_SIZE(sg_tmp), 1664 offset, msb->page_size); 1665 sg_copy_from_buffer(sg_tmp, sg_nents(sg_tmp), 1666 msb->cache + msb->page_size * page, 1667 msb->page_size); 1668 } else { 1669 dbg_verbose("Read of LBA %d (pba %d) sector %d from device", 1670 lba, pba, page); 1671 1672 error = msb_read_page(msb, pba, page, NULL, sg, offset); 1673 if (error) 1674 return error; 1675 1676 msb_cache_write(msb, lba, page, true, sg, offset); 1677 } 1678 return error; 1679 } 1680 1681 /* Emulated geometry table 1682 * This table content isn't that importaint, 1683 * One could put here different values, providing that they still 1684 * cover whole disk. 1685 * 64 MB entry is what windows reports for my 64M memstick 1686 */ 1687 1688 static const struct chs_entry chs_table[] = { 1689 /* size sectors cylinders heads */ 1690 { 4, 16, 247, 2 }, 1691 { 8, 16, 495, 2 }, 1692 { 16, 16, 495, 4 }, 1693 { 32, 16, 991, 4 }, 1694 { 64, 16, 991, 8 }, 1695 {128, 16, 991, 16 }, 1696 { 0 } 1697 }; 1698 1699 /* Load information about the card */ 1700 static int msb_init_card(struct memstick_dev *card) 1701 { 1702 struct msb_data *msb = memstick_get_drvdata(card); 1703 struct memstick_host *host = card->host; 1704 struct ms_boot_page *boot_block; 1705 int error = 0, i, raw_size_in_megs; 1706 1707 msb->caps = 0; 1708 1709 if (card->id.class >= MEMSTICK_CLASS_ROM && 1710 card->id.class <= MEMSTICK_CLASS_ROM) 1711 msb->read_only = true; 1712 1713 msb->state = -1; 1714 error = msb_reset(msb, false); 1715 if (error) 1716 return error; 1717 1718 /* Due to a bug in Jmicron driver written by Alex Dubov, 1719 * its serial mode barely works, 1720 * so we switch to parallel mode right away 1721 */ 1722 if (host->caps & MEMSTICK_CAP_PAR4) 1723 msb_switch_to_parallel(msb); 1724 1725 msb->page_size = sizeof(struct ms_boot_page); 1726 1727 /* Read the boot page */ 1728 error = msb_read_boot_blocks(msb); 1729 if (error) 1730 return -EIO; 1731 1732 boot_block = &msb->boot_page[0]; 1733 1734 /* Save interesting attributes from boot page */ 1735 msb->block_count = boot_block->attr.number_of_blocks; 1736 msb->page_size = boot_block->attr.page_size; 1737 1738 msb->pages_in_block = boot_block->attr.block_size * 2; 1739 msb->block_size = msb->page_size * msb->pages_in_block; 1740 1741 if ((size_t)msb->page_size > PAGE_SIZE) { 1742 /* this isn't supported by linux at all, anyway*/ 1743 dbg("device page %d size isn't supported", msb->page_size); 1744 return -EINVAL; 1745 } 1746 1747 msb->block_buffer = kzalloc(msb->block_size, GFP_KERNEL); 1748 if (!msb->block_buffer) 1749 return -ENOMEM; 1750 1751 raw_size_in_megs = (msb->block_size * msb->block_count) >> 20; 1752 1753 for (i = 0; chs_table[i].size; i++) { 1754 1755 if (chs_table[i].size != raw_size_in_megs) 1756 continue; 1757 1758 msb->geometry.cylinders = chs_table[i].cyl; 1759 msb->geometry.heads = chs_table[i].head; 1760 msb->geometry.sectors = chs_table[i].sec; 1761 break; 1762 } 1763 1764 if (boot_block->attr.transfer_supporting == 1) 1765 msb->caps |= MEMSTICK_CAP_PAR4; 1766 1767 if (boot_block->attr.device_type & 0x03) 1768 msb->read_only = true; 1769 1770 dbg("Total block count = %d", msb->block_count); 1771 dbg("Each block consists of %d pages", msb->pages_in_block); 1772 dbg("Page size = %d bytes", msb->page_size); 1773 dbg("Parallel mode supported: %d", !!(msb->caps & MEMSTICK_CAP_PAR4)); 1774 dbg("Read only: %d", msb->read_only); 1775 1776 #if 0 1777 /* Now we can switch the interface */ 1778 if (host->caps & msb->caps & MEMSTICK_CAP_PAR4) 1779 msb_switch_to_parallel(msb); 1780 #endif 1781 1782 error = msb_cache_init(msb); 1783 if (error) 1784 return error; 1785 1786 error = msb_ftl_initialize(msb); 1787 if (error) 1788 return error; 1789 1790 1791 /* Read the bad block table */ 1792 error = msb_read_bad_block_table(msb, 0); 1793 1794 if (error && error != -ENOMEM) { 1795 dbg("failed to read bad block table from primary boot block, trying from backup"); 1796 error = msb_read_bad_block_table(msb, 1); 1797 } 1798 1799 if (error) 1800 return error; 1801 1802 /* *drum roll* Scan the media */ 1803 error = msb_ftl_scan(msb); 1804 if (error) { 1805 pr_err("Scan of media failed"); 1806 return error; 1807 } 1808 1809 return 0; 1810 1811 } 1812 1813 static int msb_do_write_request(struct msb_data *msb, int lba, 1814 int page, struct scatterlist *sg, size_t len, int *sucessfuly_written) 1815 { 1816 int error = 0; 1817 off_t offset = 0; 1818 *sucessfuly_written = 0; 1819 1820 while (offset < len) { 1821 if (page == 0 && len - offset >= msb->block_size) { 1822 1823 if (msb->cache_block_lba == lba) 1824 msb_cache_discard(msb); 1825 1826 dbg_verbose("Writing whole lba %d", lba); 1827 error = msb_update_block(msb, lba, sg, offset); 1828 if (error) 1829 return error; 1830 1831 offset += msb->block_size; 1832 *sucessfuly_written += msb->block_size; 1833 lba++; 1834 continue; 1835 } 1836 1837 error = msb_cache_write(msb, lba, page, false, sg, offset); 1838 if (error) 1839 return error; 1840 1841 offset += msb->page_size; 1842 *sucessfuly_written += msb->page_size; 1843 1844 page++; 1845 if (page == msb->pages_in_block) { 1846 page = 0; 1847 lba++; 1848 } 1849 } 1850 return 0; 1851 } 1852 1853 static int msb_do_read_request(struct msb_data *msb, int lba, 1854 int page, struct scatterlist *sg, int len, int *sucessfuly_read) 1855 { 1856 int error = 0; 1857 int offset = 0; 1858 *sucessfuly_read = 0; 1859 1860 while (offset < len) { 1861 1862 error = msb_cache_read(msb, lba, page, sg, offset); 1863 if (error) 1864 return error; 1865 1866 offset += msb->page_size; 1867 *sucessfuly_read += msb->page_size; 1868 1869 page++; 1870 if (page == msb->pages_in_block) { 1871 page = 0; 1872 lba++; 1873 } 1874 } 1875 return 0; 1876 } 1877 1878 static void msb_io_work(struct work_struct *work) 1879 { 1880 struct msb_data *msb = container_of(work, struct msb_data, io_work); 1881 int page, error, len; 1882 sector_t lba; 1883 struct scatterlist *sg = msb->prealloc_sg; 1884 struct request *req; 1885 1886 dbg_verbose("IO: work started"); 1887 1888 while (1) { 1889 spin_lock_irq(&msb->q_lock); 1890 1891 if (msb->need_flush_cache) { 1892 msb->need_flush_cache = false; 1893 spin_unlock_irq(&msb->q_lock); 1894 msb_cache_flush(msb); 1895 continue; 1896 } 1897 1898 req = msb->req; 1899 if (!req) { 1900 dbg_verbose("IO: no more requests exiting"); 1901 spin_unlock_irq(&msb->q_lock); 1902 return; 1903 } 1904 1905 spin_unlock_irq(&msb->q_lock); 1906 1907 /* process the request */ 1908 dbg_verbose("IO: processing new request"); 1909 blk_rq_map_sg(req, sg); 1910 1911 lba = blk_rq_pos(req); 1912 1913 sector_div(lba, msb->page_size / 512); 1914 page = sector_div(lba, msb->pages_in_block); 1915 1916 if (rq_data_dir(msb->req) == READ) 1917 error = msb_do_read_request(msb, lba, page, sg, 1918 blk_rq_bytes(req), &len); 1919 else 1920 error = msb_do_write_request(msb, lba, page, sg, 1921 blk_rq_bytes(req), &len); 1922 1923 if (len && !blk_update_request(req, BLK_STS_OK, len)) { 1924 __blk_mq_end_request(req, BLK_STS_OK); 1925 spin_lock_irq(&msb->q_lock); 1926 msb->req = NULL; 1927 spin_unlock_irq(&msb->q_lock); 1928 } 1929 1930 if (error && msb->req) { 1931 blk_status_t ret = errno_to_blk_status(error); 1932 1933 dbg_verbose("IO: ending one sector of the request with error"); 1934 blk_mq_end_request(req, ret); 1935 spin_lock_irq(&msb->q_lock); 1936 msb->req = NULL; 1937 spin_unlock_irq(&msb->q_lock); 1938 } 1939 1940 if (msb->req) 1941 dbg_verbose("IO: request still pending"); 1942 } 1943 } 1944 1945 static DEFINE_IDR(msb_disk_idr); /*set of used disk numbers */ 1946 static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */ 1947 1948 static void msb_data_clear(struct msb_data *msb) 1949 { 1950 kfree(msb->boot_page); 1951 bitmap_free(msb->used_blocks_bitmap); 1952 bitmap_free(msb->erased_blocks_bitmap); 1953 kfree(msb->lba_to_pba_table); 1954 kfree(msb->cache); 1955 msb->card = NULL; 1956 } 1957 1958 static int msb_bd_getgeo(struct gendisk *disk, 1959 struct hd_geometry *geo) 1960 { 1961 struct msb_data *msb = disk->private_data; 1962 *geo = msb->geometry; 1963 return 0; 1964 } 1965 1966 static void msb_bd_free_disk(struct gendisk *disk) 1967 { 1968 struct msb_data *msb = disk->private_data; 1969 1970 mutex_lock(&msb_disk_lock); 1971 idr_remove(&msb_disk_idr, msb->disk_id); 1972 mutex_unlock(&msb_disk_lock); 1973 1974 kfree(msb); 1975 } 1976 1977 static blk_status_t msb_queue_rq(struct blk_mq_hw_ctx *hctx, 1978 const struct blk_mq_queue_data *bd) 1979 { 1980 struct memstick_dev *card = hctx->queue->queuedata; 1981 struct msb_data *msb = memstick_get_drvdata(card); 1982 struct request *req = bd->rq; 1983 1984 dbg_verbose("Submit request"); 1985 1986 spin_lock_irq(&msb->q_lock); 1987 1988 if (msb->card_dead) { 1989 dbg("Refusing requests on removed card"); 1990 1991 WARN_ON(!msb->io_queue_stopped); 1992 1993 spin_unlock_irq(&msb->q_lock); 1994 blk_mq_start_request(req); 1995 return BLK_STS_IOERR; 1996 } 1997 1998 if (msb->req) { 1999 spin_unlock_irq(&msb->q_lock); 2000 return BLK_STS_DEV_RESOURCE; 2001 } 2002 2003 blk_mq_start_request(req); 2004 msb->req = req; 2005 2006 if (!msb->io_queue_stopped) 2007 queue_work(msb->io_queue, &msb->io_work); 2008 2009 spin_unlock_irq(&msb->q_lock); 2010 return BLK_STS_OK; 2011 } 2012 2013 static int msb_check_card(struct memstick_dev *card) 2014 { 2015 struct msb_data *msb = memstick_get_drvdata(card); 2016 2017 return (msb->card_dead == 0); 2018 } 2019 2020 static void msb_stop(struct memstick_dev *card) 2021 { 2022 struct msb_data *msb = memstick_get_drvdata(card); 2023 unsigned long flags; 2024 2025 dbg("Stopping all msblock IO"); 2026 2027 blk_mq_stop_hw_queues(msb->queue); 2028 spin_lock_irqsave(&msb->q_lock, flags); 2029 msb->io_queue_stopped = true; 2030 spin_unlock_irqrestore(&msb->q_lock, flags); 2031 2032 timer_delete_sync(&msb->cache_flush_timer); 2033 flush_workqueue(msb->io_queue); 2034 2035 spin_lock_irqsave(&msb->q_lock, flags); 2036 if (msb->req) { 2037 blk_mq_requeue_request(msb->req, false); 2038 msb->req = NULL; 2039 } 2040 spin_unlock_irqrestore(&msb->q_lock, flags); 2041 } 2042 2043 static void msb_start(struct memstick_dev *card) 2044 { 2045 struct msb_data *msb = memstick_get_drvdata(card); 2046 unsigned long flags; 2047 2048 dbg("Resuming IO from msblock"); 2049 2050 msb_invalidate_reg_window(msb); 2051 2052 spin_lock_irqsave(&msb->q_lock, flags); 2053 if (!msb->io_queue_stopped || msb->card_dead) { 2054 spin_unlock_irqrestore(&msb->q_lock, flags); 2055 return; 2056 } 2057 spin_unlock_irqrestore(&msb->q_lock, flags); 2058 2059 /* Kick cache flush anyway, its harmless */ 2060 msb->need_flush_cache = true; 2061 msb->io_queue_stopped = false; 2062 2063 blk_mq_start_hw_queues(msb->queue); 2064 2065 queue_work(msb->io_queue, &msb->io_work); 2066 2067 } 2068 2069 static const struct block_device_operations msb_bdops = { 2070 .owner = THIS_MODULE, 2071 .getgeo = msb_bd_getgeo, 2072 .free_disk = msb_bd_free_disk, 2073 }; 2074 2075 static const struct blk_mq_ops msb_mq_ops = { 2076 .queue_rq = msb_queue_rq, 2077 }; 2078 2079 /* Registers the block device */ 2080 static int msb_init_disk(struct memstick_dev *card) 2081 { 2082 struct msb_data *msb = memstick_get_drvdata(card); 2083 struct queue_limits lim = { 2084 .logical_block_size = msb->page_size, 2085 .max_hw_sectors = MS_BLOCK_MAX_PAGES, 2086 .max_segments = MS_BLOCK_MAX_SEGS, 2087 .max_segment_size = MS_BLOCK_MAX_PAGES * msb->page_size, 2088 }; 2089 int rc; 2090 unsigned long capacity; 2091 2092 mutex_lock(&msb_disk_lock); 2093 msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL); 2094 mutex_unlock(&msb_disk_lock); 2095 2096 if (msb->disk_id < 0) 2097 return msb->disk_id; 2098 2099 rc = blk_mq_alloc_sq_tag_set(&msb->tag_set, &msb_mq_ops, 2, 0); 2100 if (rc) 2101 goto out_release_id; 2102 2103 msb->disk = blk_mq_alloc_disk(&msb->tag_set, &lim, card); 2104 if (IS_ERR(msb->disk)) { 2105 rc = PTR_ERR(msb->disk); 2106 goto out_free_tag_set; 2107 } 2108 msb->queue = msb->disk->queue; 2109 2110 sprintf(msb->disk->disk_name, "msblk%d", msb->disk_id); 2111 msb->disk->fops = &msb_bdops; 2112 msb->disk->private_data = msb; 2113 2114 capacity = msb->pages_in_block * msb->logical_block_count; 2115 capacity *= (msb->page_size / 512); 2116 set_capacity(msb->disk, capacity); 2117 dbg("Set total disk size to %lu sectors", capacity); 2118 2119 msb->io_queue = alloc_ordered_workqueue("ms_block", WQ_MEM_RECLAIM); 2120 if (!msb->io_queue) { 2121 rc = -ENOMEM; 2122 goto out_cleanup_disk; 2123 } 2124 2125 INIT_WORK(&msb->io_work, msb_io_work); 2126 sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1); 2127 2128 if (msb->read_only) 2129 set_disk_ro(msb->disk, 1); 2130 2131 msb_start(card); 2132 rc = device_add_disk(&card->dev, msb->disk, NULL); 2133 if (rc) 2134 goto out_destroy_workqueue; 2135 dbg("Disk added"); 2136 return 0; 2137 2138 out_destroy_workqueue: 2139 destroy_workqueue(msb->io_queue); 2140 out_cleanup_disk: 2141 put_disk(msb->disk); 2142 out_free_tag_set: 2143 blk_mq_free_tag_set(&msb->tag_set); 2144 out_release_id: 2145 mutex_lock(&msb_disk_lock); 2146 idr_remove(&msb_disk_idr, msb->disk_id); 2147 mutex_unlock(&msb_disk_lock); 2148 return rc; 2149 } 2150 2151 static int msb_probe(struct memstick_dev *card) 2152 { 2153 struct msb_data *msb; 2154 int rc = 0; 2155 2156 msb = kzalloc_obj(struct msb_data); 2157 if (!msb) 2158 return -ENOMEM; 2159 memstick_set_drvdata(card, msb); 2160 msb->card = card; 2161 spin_lock_init(&msb->q_lock); 2162 2163 rc = msb_init_card(card); 2164 if (rc) 2165 goto out_free; 2166 2167 rc = msb_init_disk(card); 2168 if (!rc) { 2169 card->check = msb_check_card; 2170 card->stop = msb_stop; 2171 card->start = msb_start; 2172 return 0; 2173 } 2174 out_free: 2175 memstick_set_drvdata(card, NULL); 2176 msb_data_clear(msb); 2177 kfree(msb); 2178 return rc; 2179 } 2180 2181 static void msb_remove(struct memstick_dev *card) 2182 { 2183 struct msb_data *msb = memstick_get_drvdata(card); 2184 unsigned long flags; 2185 2186 if (!msb->io_queue_stopped) 2187 msb_stop(card); 2188 2189 dbg("Removing the disk device"); 2190 2191 /* Take care of unhandled + new requests from now on */ 2192 spin_lock_irqsave(&msb->q_lock, flags); 2193 msb->card_dead = true; 2194 spin_unlock_irqrestore(&msb->q_lock, flags); 2195 blk_mq_start_hw_queues(msb->queue); 2196 2197 /* Remove the disk */ 2198 del_gendisk(msb->disk); 2199 blk_mq_free_tag_set(&msb->tag_set); 2200 msb->queue = NULL; 2201 2202 mutex_lock(&msb_disk_lock); 2203 msb_data_clear(msb); 2204 mutex_unlock(&msb_disk_lock); 2205 2206 destroy_workqueue(msb->io_queue); 2207 2208 put_disk(msb->disk); 2209 memstick_set_drvdata(card, NULL); 2210 } 2211 2212 #ifdef CONFIG_PM 2213 2214 static int msb_suspend(struct memstick_dev *card, pm_message_t state) 2215 { 2216 msb_stop(card); 2217 return 0; 2218 } 2219 2220 static int msb_resume(struct memstick_dev *card) 2221 { 2222 struct msb_data *msb = memstick_get_drvdata(card); 2223 struct msb_data *new_msb = NULL; 2224 bool card_dead = true; 2225 2226 #ifndef CONFIG_MEMSTICK_UNSAFE_RESUME 2227 msb->card_dead = true; 2228 return 0; 2229 #endif 2230 mutex_lock(&card->host->lock); 2231 2232 new_msb = kzalloc_obj(struct msb_data); 2233 if (!new_msb) 2234 goto out; 2235 2236 new_msb->card = card; 2237 memstick_set_drvdata(card, new_msb); 2238 spin_lock_init(&new_msb->q_lock); 2239 sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1); 2240 2241 if (msb_init_card(card)) 2242 goto out; 2243 2244 if (msb->block_size != new_msb->block_size) 2245 goto out; 2246 2247 if (memcmp(msb->boot_page, new_msb->boot_page, 2248 sizeof(struct ms_boot_page))) 2249 goto out; 2250 2251 if (msb->logical_block_count != new_msb->logical_block_count || 2252 memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table, 2253 msb->logical_block_count)) 2254 goto out; 2255 2256 if (msb->block_count != new_msb->block_count || 2257 !bitmap_equal(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap, 2258 msb->block_count)) 2259 goto out; 2260 2261 card_dead = false; 2262 out: 2263 if (card_dead) 2264 dbg("Card was removed/replaced during suspend"); 2265 2266 msb->card_dead = card_dead; 2267 memstick_set_drvdata(card, msb); 2268 2269 if (new_msb) { 2270 msb_data_clear(new_msb); 2271 kfree(new_msb); 2272 } 2273 2274 msb_start(card); 2275 mutex_unlock(&card->host->lock); 2276 return 0; 2277 } 2278 #else 2279 2280 #define msb_suspend NULL 2281 #define msb_resume NULL 2282 2283 #endif /* CONFIG_PM */ 2284 2285 static const struct memstick_device_id msb_id_tbl[] = { 2286 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2287 MEMSTICK_CLASS_FLASH}, 2288 2289 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2290 MEMSTICK_CLASS_ROM}, 2291 2292 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2293 MEMSTICK_CLASS_RO}, 2294 2295 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2296 MEMSTICK_CLASS_WP}, 2297 2298 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO, 2299 MEMSTICK_CLASS_DUO}, 2300 {} 2301 }; 2302 MODULE_DEVICE_TABLE(memstick, msb_id_tbl); 2303 2304 2305 static struct memstick_driver msb_driver = { 2306 .driver = { 2307 .name = DRIVER_NAME, 2308 .owner = THIS_MODULE 2309 }, 2310 .id_table = msb_id_tbl, 2311 .probe = msb_probe, 2312 .remove = msb_remove, 2313 .suspend = msb_suspend, 2314 .resume = msb_resume 2315 }; 2316 2317 static int __init msb_init(void) 2318 { 2319 int rc = memstick_register_driver(&msb_driver); 2320 2321 if (rc) 2322 pr_err("failed to register memstick driver (error %d)\n", rc); 2323 2324 return rc; 2325 } 2326 2327 static void __exit msb_exit(void) 2328 { 2329 memstick_unregister_driver(&msb_driver); 2330 idr_destroy(&msb_disk_idr); 2331 } 2332 2333 module_init(msb_init); 2334 module_exit(msb_exit); 2335 2336 module_param(cache_flush_timeout, int, S_IRUGO); 2337 MODULE_PARM_DESC(cache_flush_timeout, 2338 "Cache flush timeout in msec (1000 default)"); 2339 module_param(debug, int, S_IRUGO | S_IWUSR); 2340 MODULE_PARM_DESC(debug, "Debug level (0-2)"); 2341 2342 module_param(verify_writes, bool, S_IRUGO); 2343 MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written"); 2344 2345 MODULE_LICENSE("GPL"); 2346 MODULE_AUTHOR("Maxim Levitsky"); 2347 MODULE_DESCRIPTION("Sony MemoryStick block device driver"); 2348