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