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