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