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