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 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 int rc; 2082 unsigned long capacity; 2083 2084 mutex_lock(&msb_disk_lock); 2085 msb->disk_id = idr_alloc(&msb_disk_idr, card, 0, 256, GFP_KERNEL); 2086 mutex_unlock(&msb_disk_lock); 2087 2088 if (msb->disk_id < 0) 2089 return msb->disk_id; 2090 2091 rc = blk_mq_alloc_sq_tag_set(&msb->tag_set, &msb_mq_ops, 2, 2092 BLK_MQ_F_SHOULD_MERGE); 2093 if (rc) 2094 goto out_release_id; 2095 2096 msb->disk = blk_mq_alloc_disk(&msb->tag_set, card); 2097 if (IS_ERR(msb->disk)) { 2098 rc = PTR_ERR(msb->disk); 2099 goto out_free_tag_set; 2100 } 2101 msb->queue = msb->disk->queue; 2102 2103 blk_queue_max_hw_sectors(msb->queue, MS_BLOCK_MAX_PAGES); 2104 blk_queue_max_segments(msb->queue, MS_BLOCK_MAX_SEGS); 2105 blk_queue_max_segment_size(msb->queue, 2106 MS_BLOCK_MAX_PAGES * msb->page_size); 2107 blk_queue_logical_block_size(msb->queue, msb->page_size); 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 INIT_WORK(&msb->io_work, msb_io_work); 2120 sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1); 2121 2122 if (msb->read_only) 2123 set_disk_ro(msb->disk, 1); 2124 2125 msb_start(card); 2126 rc = device_add_disk(&card->dev, msb->disk, NULL); 2127 if (rc) 2128 goto out_cleanup_disk; 2129 dbg("Disk added"); 2130 return 0; 2131 2132 out_cleanup_disk: 2133 put_disk(msb->disk); 2134 out_free_tag_set: 2135 blk_mq_free_tag_set(&msb->tag_set); 2136 out_release_id: 2137 mutex_lock(&msb_disk_lock); 2138 idr_remove(&msb_disk_idr, msb->disk_id); 2139 mutex_unlock(&msb_disk_lock); 2140 return rc; 2141 } 2142 2143 static int msb_probe(struct memstick_dev *card) 2144 { 2145 struct msb_data *msb; 2146 int rc = 0; 2147 2148 msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL); 2149 if (!msb) 2150 return -ENOMEM; 2151 memstick_set_drvdata(card, msb); 2152 msb->card = card; 2153 spin_lock_init(&msb->q_lock); 2154 2155 rc = msb_init_card(card); 2156 if (rc) 2157 goto out_free; 2158 2159 rc = msb_init_disk(card); 2160 if (!rc) { 2161 card->check = msb_check_card; 2162 card->stop = msb_stop; 2163 card->start = msb_start; 2164 return 0; 2165 } 2166 out_free: 2167 memstick_set_drvdata(card, NULL); 2168 msb_data_clear(msb); 2169 kfree(msb); 2170 return rc; 2171 } 2172 2173 static void msb_remove(struct memstick_dev *card) 2174 { 2175 struct msb_data *msb = memstick_get_drvdata(card); 2176 unsigned long flags; 2177 2178 if (!msb->io_queue_stopped) 2179 msb_stop(card); 2180 2181 dbg("Removing the disk device"); 2182 2183 /* Take care of unhandled + new requests from now on */ 2184 spin_lock_irqsave(&msb->q_lock, flags); 2185 msb->card_dead = true; 2186 spin_unlock_irqrestore(&msb->q_lock, flags); 2187 blk_mq_start_hw_queues(msb->queue); 2188 2189 /* Remove the disk */ 2190 del_gendisk(msb->disk); 2191 blk_mq_free_tag_set(&msb->tag_set); 2192 msb->queue = NULL; 2193 2194 mutex_lock(&msb_disk_lock); 2195 msb_data_clear(msb); 2196 mutex_unlock(&msb_disk_lock); 2197 2198 put_disk(msb->disk); 2199 memstick_set_drvdata(card, NULL); 2200 } 2201 2202 #ifdef CONFIG_PM 2203 2204 static int msb_suspend(struct memstick_dev *card, pm_message_t state) 2205 { 2206 msb_stop(card); 2207 return 0; 2208 } 2209 2210 static int msb_resume(struct memstick_dev *card) 2211 { 2212 struct msb_data *msb = memstick_get_drvdata(card); 2213 struct msb_data *new_msb = NULL; 2214 bool card_dead = true; 2215 2216 #ifndef CONFIG_MEMSTICK_UNSAFE_RESUME 2217 msb->card_dead = true; 2218 return 0; 2219 #endif 2220 mutex_lock(&card->host->lock); 2221 2222 new_msb = kzalloc(sizeof(struct msb_data), GFP_KERNEL); 2223 if (!new_msb) 2224 goto out; 2225 2226 new_msb->card = card; 2227 memstick_set_drvdata(card, new_msb); 2228 spin_lock_init(&new_msb->q_lock); 2229 sg_init_table(msb->prealloc_sg, MS_BLOCK_MAX_SEGS+1); 2230 2231 if (msb_init_card(card)) 2232 goto out; 2233 2234 if (msb->block_size != new_msb->block_size) 2235 goto out; 2236 2237 if (memcmp(msb->boot_page, new_msb->boot_page, 2238 sizeof(struct ms_boot_page))) 2239 goto out; 2240 2241 if (msb->logical_block_count != new_msb->logical_block_count || 2242 memcmp(msb->lba_to_pba_table, new_msb->lba_to_pba_table, 2243 msb->logical_block_count)) 2244 goto out; 2245 2246 if (msb->block_count != new_msb->block_count || 2247 !bitmap_equal(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap, 2248 msb->block_count)) 2249 goto out; 2250 2251 card_dead = false; 2252 out: 2253 if (card_dead) 2254 dbg("Card was removed/replaced during suspend"); 2255 2256 msb->card_dead = card_dead; 2257 memstick_set_drvdata(card, msb); 2258 2259 if (new_msb) { 2260 msb_data_clear(new_msb); 2261 kfree(new_msb); 2262 } 2263 2264 msb_start(card); 2265 mutex_unlock(&card->host->lock); 2266 return 0; 2267 } 2268 #else 2269 2270 #define msb_suspend NULL 2271 #define msb_resume NULL 2272 2273 #endif /* CONFIG_PM */ 2274 2275 static struct memstick_device_id msb_id_tbl[] = { 2276 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2277 MEMSTICK_CLASS_FLASH}, 2278 2279 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2280 MEMSTICK_CLASS_ROM}, 2281 2282 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2283 MEMSTICK_CLASS_RO}, 2284 2285 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_LEGACY, MEMSTICK_CATEGORY_STORAGE, 2286 MEMSTICK_CLASS_WP}, 2287 2288 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_DUO, MEMSTICK_CATEGORY_STORAGE_DUO, 2289 MEMSTICK_CLASS_DUO}, 2290 {} 2291 }; 2292 MODULE_DEVICE_TABLE(memstick, msb_id_tbl); 2293 2294 2295 static struct memstick_driver msb_driver = { 2296 .driver = { 2297 .name = DRIVER_NAME, 2298 .owner = THIS_MODULE 2299 }, 2300 .id_table = msb_id_tbl, 2301 .probe = msb_probe, 2302 .remove = msb_remove, 2303 .suspend = msb_suspend, 2304 .resume = msb_resume 2305 }; 2306 2307 static int __init msb_init(void) 2308 { 2309 int rc = memstick_register_driver(&msb_driver); 2310 2311 if (rc) 2312 pr_err("failed to register memstick driver (error %d)\n", rc); 2313 2314 return rc; 2315 } 2316 2317 static void __exit msb_exit(void) 2318 { 2319 memstick_unregister_driver(&msb_driver); 2320 idr_destroy(&msb_disk_idr); 2321 } 2322 2323 module_init(msb_init); 2324 module_exit(msb_exit); 2325 2326 module_param(cache_flush_timeout, int, S_IRUGO); 2327 MODULE_PARM_DESC(cache_flush_timeout, 2328 "Cache flush timeout in msec (1000 default)"); 2329 module_param(debug, int, S_IRUGO | S_IWUSR); 2330 MODULE_PARM_DESC(debug, "Debug level (0-2)"); 2331 2332 module_param(verify_writes, bool, S_IRUGO); 2333 MODULE_PARM_DESC(verify_writes, "Read back and check all data that is written"); 2334 2335 MODULE_LICENSE("GPL"); 2336 MODULE_AUTHOR("Maxim Levitsky"); 2337 MODULE_DESCRIPTION("Sony MemoryStick block device driver"); 2338