?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge tag 'block-5.11-2021-01-10' of git://git.kernel.dk/linux-block
[linux/fpc-iii.git] / drivers / memstick / core / ms_block.c
blob8004dd64d09a87bc8b203e42077d5bd2fed29e53
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 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 /* If dont actually need to send the int read request (only in
371 serial mode), then just fall through */
372 if (msb_read_int_reg(msb, -1))
373 return 0;
374 fallthrough;
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 /* read the status register to understand source of the INT_ERR */
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 fallthrough;
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 fallthrough;
422
423 case MSB_RP_SEND_READ_DATA:
424 /* Skip that state if we only read the oob */
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 * Handler of writes of exactly one block.
465 * Takes address from msb->regs.param.
466 * Writes same extra data to blocks, also taken
467 * from msb->regs.extra
468 * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
469 * device refuses to take the command or something else
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 /* HACK: Jmicon handling of TPCs between 8 and
486 * sizeof(memstick_request.data) is broken due to hardware
487 * bug in PIO mode that is used for these TPCs
488 * Therefore split the write
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 fallthrough;
522
523 case MSB_WB_RECEIVE_INT_REQ:
524 intreg = mrq->data[0];
525 msb->regs.status.interrupt = intreg;
526
527 /* errors mean out of here, and fast... */
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 /* for last page we need to poll CED */
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 /* for non-last page we need BREQ before writing next chunk */
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 fallthrough;
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 * This function is used to send simple IO requests to device that consist
581 * of register write + command
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 /* HACK: see h_msb_write_block */
598 case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
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 fallthrough;
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 /* Small handler for card reset */
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 /* This handler is used to do serial->parallel switch */
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 /* Set the parallel interface on memstick side */
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 /* Set parallel interface on our side + send a dummy request
705 to see if card responds */
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 /* Reset the card, to guard against hw errors beeing treated as bad blocks */
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 /* Reset the card */
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 /* Set parallel mode */
763 if (was_parallel)
764 msb_switch_to_parallel(msb);
765 return 0;
766 }
767
768 /* Attempts to switch interface to parallel mode */
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 /* Changes overwrite flag on a page */
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 /* Erases one physical block */
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 /* Reads one page from device */
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 /* Mark bad pages */
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 /* Reads oob of page only */
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 /* Reads a block and compares it with data contained in scatterlist orig_sg */
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 /* Writes exectly one block + oob */
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 /* Sector we just wrote to is assumed erased since its pba
1048 was erased. If it wasn't erased, write will succeed
1049 and will just clear the bits that were set in the block
1050 thus test that what we have written,
1051 matches what we expect.
1052 We do trust the blocks that we erased */
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 /* Finds a free block for write replacement */
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 /* Converts endiannes in the boot block for easy use */
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't 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 /* Read the buffer */
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 /* Process the bad block table */
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 /* can't trust the page if we can't read the oob */
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 /* Skip bad blocks */
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 /* Skip system/drm blocks */
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 /* Erase temporary tables */
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 /* Block has LBA not according to zoning*/
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 /* No collisions - great */
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 /* Read all missing pages in cache */
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 /* Bad pages are copied with 00 page status */
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 /* Write the cache now */
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 /* Mark invalid pages */
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 /* If we need to write different block */
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 /* Emulated geometry table
1673 * This table content isn't that importaint,
1674 * One could put here different values, providing that they still
1675 * cover whole disk.
1676 * 64 MB entry is what windows reports for my 64M memstick */
1677
1678 static const struct chs_entry chs_table[] = {
1679 /* size sectors cylynders heads */
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 /* Load information about the card */
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 /* Due to a bug in Jmicron driver written by Alex Dubov,
1709 its serial mode barely works,
1710 so we switch to parallel mode right away */
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 /* Read the boot page */
1717 error = msb_read_boot_blocks(msb);
1718 if (error)
1719 return -EIO;
1720
1721 boot_block = &msb->boot_page[0];
1722
1723 /* Save intersting attributes from boot page */
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 /* this isn't supported by linux at all, anyway*/
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 /* Now we can switch the interface */
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 /* Read the bad block table */
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 /* *drum roll* Scan the media */
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 /* process the request */
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); /*set of used disk numbers */
1935 static DEFINE_MUTEX(msb_disk_lock); /* protects against races in open/release */
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 /* Kick cache flush anyway, its harmless */
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 /* Registers the block device */
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 /* Take care of unhandled + new requests from now on */
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 /* Remove the disk */
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 /* CONFIG_PM */
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");
Linux with added FPC-III support