xref: /linux/drivers/memstick/core/ms_block.c (revision 79790b6818e96c58fe2bffee1b418c16e64e7b80)
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  */
msb_sg_copy(struct scatterlist * sg_from,struct scatterlist * sg_to,int to_nents,size_t offset,size_t len)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  */
msb_sg_compare_to_buffer(struct scatterlist * sg,size_t offset,u8 * buffer,size_t len)94 static int msb_sg_compare_to_buffer(struct scatterlist *sg,
95 					size_t offset, u8 *buffer, size_t len)
96 {
97 	int retval = 0, cmplen;
98 	struct sg_mapping_iter miter;
99 
100 	sg_miter_start(&miter, sg, sg_nents(sg),
101 					SG_MITER_ATOMIC | SG_MITER_FROM_SG);
102 
103 	while (sg_miter_next(&miter) && len > 0) {
104 		if (offset >= miter.length) {
105 			offset -= miter.length;
106 			continue;
107 		}
108 
109 		cmplen = min(miter.length - offset, len);
110 		retval = memcmp(miter.addr + offset, buffer, cmplen) ? -1 : 0;
111 		if (retval)
112 			break;
113 
114 		buffer += cmplen;
115 		len -= cmplen;
116 		offset = 0;
117 	}
118 
119 	if (!retval && len)
120 		retval = -1;
121 
122 	sg_miter_stop(&miter);
123 	return retval;
124 }
125 
126 
127 /* Get zone at which block with logical address 'lba' lives
128  * Flash is broken into zones.
129  * Each zone consists of 512 eraseblocks, out of which in first
130  * zone 494 are used and 496 are for all following zones.
131  * Therefore zone #0 hosts blocks 0-493, zone #1 blocks 494-988, etc...
132  */
msb_get_zone_from_lba(int lba)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 */
msb_get_zone_from_pba(int pba)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 */
msb_validate_used_block_bitmap(struct msb_data * msb)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 */
msb_mark_block_used(struct msb_data * msb,int pba)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 */
msb_mark_block_unused(struct msb_data * msb,int pba)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 */
msb_invalidate_reg_window(struct msb_data * msb)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 */
msb_run_state_machine(struct msb_data * msb,int (* state_func)(struct memstick_dev * card,struct memstick_request ** req))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 */
msb_exit_state_machine(struct msb_data * msb,int error)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 */
msb_read_int_reg(struct msb_data * msb,long timeout)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 */
msb_read_regs(struct msb_data * msb,int offset,int len)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 */
msb_write_regs(struct msb_data * msb,int offset,int len,void * buf)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 */
h_msb_default_bad(struct memstick_dev * card,struct memstick_request ** mrq)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  */
h_msb_read_page(struct memstick_dev * card,struct memstick_request ** out_mrq)335 static int h_msb_read_page(struct memstick_dev *card,
336 					struct memstick_request **out_mrq)
337 {
338 	struct msb_data *msb = memstick_get_drvdata(card);
339 	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
340 	struct scatterlist sg[2];
341 	u8 command, intreg;
342 
343 	if (mrq->error) {
344 		dbg("read_page, unknown error");
345 		return msb_exit_state_machine(msb, mrq->error);
346 	}
347 again:
348 	switch (msb->state) {
349 	case MSB_RP_SEND_BLOCK_ADDRESS:
350 		/* msb_write_regs sometimes "fails" because it needs to update
351 		 * the reg window, and thus it returns request for that.
352 		 * Then we stay in this state and retry
353 		 */
354 		if (!msb_write_regs(msb,
355 			offsetof(struct ms_register, param),
356 			sizeof(struct ms_param_register),
357 			(unsigned char *)&msb->regs.param))
358 			return 0;
359 
360 		msb->state = MSB_RP_SEND_READ_COMMAND;
361 		return 0;
362 
363 	case MSB_RP_SEND_READ_COMMAND:
364 		command = MS_CMD_BLOCK_READ;
365 		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
366 		msb->state = MSB_RP_SEND_INT_REQ;
367 		return 0;
368 
369 	case MSB_RP_SEND_INT_REQ:
370 		msb->state = MSB_RP_RECEIVE_INT_REQ_RESULT;
371 		/* If dont actually need to send the int read request (only in
372 		 * serial mode), then just fall through
373 		 */
374 		if (msb_read_int_reg(msb, -1))
375 			return 0;
376 		fallthrough;
377 
378 	case MSB_RP_RECEIVE_INT_REQ_RESULT:
379 		intreg = mrq->data[0];
380 		msb->regs.status.interrupt = intreg;
381 
382 		if (intreg & MEMSTICK_INT_CMDNAK)
383 			return msb_exit_state_machine(msb, -EIO);
384 
385 		if (!(intreg & MEMSTICK_INT_CED)) {
386 			msb->state = MSB_RP_SEND_INT_REQ;
387 			goto again;
388 		}
389 
390 		msb->int_polling = false;
391 		msb->state = (intreg & MEMSTICK_INT_ERR) ?
392 			MSB_RP_SEND_READ_STATUS_REG : MSB_RP_SEND_OOB_READ;
393 		goto again;
394 
395 	case MSB_RP_SEND_READ_STATUS_REG:
396 		 /* read the status register to understand source of the INT_ERR */
397 		if (!msb_read_regs(msb,
398 			offsetof(struct ms_register, status),
399 			sizeof(struct ms_status_register)))
400 			return 0;
401 
402 		msb->state = MSB_RP_RECEIVE_STATUS_REG;
403 		return 0;
404 
405 	case MSB_RP_RECEIVE_STATUS_REG:
406 		msb->regs.status = *(struct ms_status_register *)mrq->data;
407 		msb->state = MSB_RP_SEND_OOB_READ;
408 		fallthrough;
409 
410 	case MSB_RP_SEND_OOB_READ:
411 		if (!msb_read_regs(msb,
412 			offsetof(struct ms_register, extra_data),
413 			sizeof(struct ms_extra_data_register)))
414 			return 0;
415 
416 		msb->state = MSB_RP_RECEIVE_OOB_READ;
417 		return 0;
418 
419 	case MSB_RP_RECEIVE_OOB_READ:
420 		msb->regs.extra_data =
421 			*(struct ms_extra_data_register *) mrq->data;
422 		msb->state = MSB_RP_SEND_READ_DATA;
423 		fallthrough;
424 
425 	case MSB_RP_SEND_READ_DATA:
426 		/* Skip that state if we only read the oob */
427 		if (msb->regs.param.cp == MEMSTICK_CP_EXTRA) {
428 			msb->state = MSB_RP_RECEIVE_READ_DATA;
429 			goto again;
430 		}
431 
432 		sg_init_table(sg, ARRAY_SIZE(sg));
433 		msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
434 			msb->current_sg_offset,
435 			msb->page_size);
436 
437 		memstick_init_req_sg(mrq, MS_TPC_READ_LONG_DATA, sg);
438 		msb->state = MSB_RP_RECEIVE_READ_DATA;
439 		return 0;
440 
441 	case MSB_RP_RECEIVE_READ_DATA:
442 		if (!(msb->regs.status.interrupt & MEMSTICK_INT_ERR)) {
443 			msb->current_sg_offset += msb->page_size;
444 			return msb_exit_state_machine(msb, 0);
445 		}
446 
447 		if (msb->regs.status.status1 & MEMSTICK_UNCORR_ERROR) {
448 			dbg("read_page: uncorrectable error");
449 			return msb_exit_state_machine(msb, -EBADMSG);
450 		}
451 
452 		if (msb->regs.status.status1 & MEMSTICK_CORR_ERROR) {
453 			dbg("read_page: correctable error");
454 			msb->current_sg_offset += msb->page_size;
455 			return msb_exit_state_machine(msb, -EUCLEAN);
456 		} else {
457 			dbg("read_page: INT error, but no status error bits");
458 			return msb_exit_state_machine(msb, -EIO);
459 		}
460 	}
461 
462 	BUG();
463 }
464 
465 /*
466  * Handler of writes of exactly one block.
467  * Takes address from msb->regs.param.
468  * Writes same extra data to blocks, also taken
469  * from msb->regs.extra
470  * Returns -EBADMSG if write fails due to uncorrectable error, or -EIO if
471  * device refuses to take the command or something else
472  */
h_msb_write_block(struct memstick_dev * card,struct memstick_request ** out_mrq)473 static int h_msb_write_block(struct memstick_dev *card,
474 					struct memstick_request **out_mrq)
475 {
476 	struct msb_data *msb = memstick_get_drvdata(card);
477 	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
478 	struct scatterlist sg[2];
479 	u8 intreg, command;
480 
481 	if (mrq->error)
482 		return msb_exit_state_machine(msb, mrq->error);
483 
484 again:
485 	switch (msb->state) {
486 
487 	/* HACK: Jmicon handling of TPCs between 8 and
488 	 *	sizeof(memstick_request.data) is broken due to hardware
489 	 *	bug in PIO mode that is used for these TPCs
490 	 *	Therefore split the write
491 	 */
492 
493 	case MSB_WB_SEND_WRITE_PARAMS:
494 		if (!msb_write_regs(msb,
495 			offsetof(struct ms_register, param),
496 			sizeof(struct ms_param_register),
497 			&msb->regs.param))
498 			return 0;
499 
500 		msb->state = MSB_WB_SEND_WRITE_OOB;
501 		return 0;
502 
503 	case MSB_WB_SEND_WRITE_OOB:
504 		if (!msb_write_regs(msb,
505 			offsetof(struct ms_register, extra_data),
506 			sizeof(struct ms_extra_data_register),
507 			&msb->regs.extra_data))
508 			return 0;
509 		msb->state = MSB_WB_SEND_WRITE_COMMAND;
510 		return 0;
511 
512 
513 	case MSB_WB_SEND_WRITE_COMMAND:
514 		command = MS_CMD_BLOCK_WRITE;
515 		memstick_init_req(mrq, MS_TPC_SET_CMD, &command, 1);
516 		msb->state = MSB_WB_SEND_INT_REQ;
517 		return 0;
518 
519 	case MSB_WB_SEND_INT_REQ:
520 		msb->state = MSB_WB_RECEIVE_INT_REQ;
521 		if (msb_read_int_reg(msb, -1))
522 			return 0;
523 		fallthrough;
524 
525 	case MSB_WB_RECEIVE_INT_REQ:
526 		intreg = mrq->data[0];
527 		msb->regs.status.interrupt = intreg;
528 
529 		/* errors mean out of here, and fast... */
530 		if (intreg & (MEMSTICK_INT_CMDNAK))
531 			return msb_exit_state_machine(msb, -EIO);
532 
533 		if (intreg & MEMSTICK_INT_ERR)
534 			return msb_exit_state_machine(msb, -EBADMSG);
535 
536 
537 		/* for last page we need to poll CED */
538 		if (msb->current_page == msb->pages_in_block) {
539 			if (intreg & MEMSTICK_INT_CED)
540 				return msb_exit_state_machine(msb, 0);
541 			msb->state = MSB_WB_SEND_INT_REQ;
542 			goto again;
543 
544 		}
545 
546 		/* for non-last page we need BREQ before writing next chunk */
547 		if (!(intreg & MEMSTICK_INT_BREQ)) {
548 			msb->state = MSB_WB_SEND_INT_REQ;
549 			goto again;
550 		}
551 
552 		msb->int_polling = false;
553 		msb->state = MSB_WB_SEND_WRITE_DATA;
554 		fallthrough;
555 
556 	case MSB_WB_SEND_WRITE_DATA:
557 		sg_init_table(sg, ARRAY_SIZE(sg));
558 
559 		if (msb_sg_copy(msb->current_sg, sg, ARRAY_SIZE(sg),
560 			msb->current_sg_offset,
561 			msb->page_size) < msb->page_size)
562 			return msb_exit_state_machine(msb, -EIO);
563 
564 		memstick_init_req_sg(mrq, MS_TPC_WRITE_LONG_DATA, sg);
565 		mrq->need_card_int = 1;
566 		msb->state = MSB_WB_RECEIVE_WRITE_CONFIRMATION;
567 		return 0;
568 
569 	case MSB_WB_RECEIVE_WRITE_CONFIRMATION:
570 		msb->current_page++;
571 		msb->current_sg_offset += msb->page_size;
572 		msb->state = MSB_WB_SEND_INT_REQ;
573 		goto again;
574 	default:
575 		BUG();
576 	}
577 
578 	return 0;
579 }
580 
581 /*
582  * This function is used to send simple IO requests to device that consist
583  * of register write + command
584  */
h_msb_send_command(struct memstick_dev * card,struct memstick_request ** out_mrq)585 static int h_msb_send_command(struct memstick_dev *card,
586 					struct memstick_request **out_mrq)
587 {
588 	struct msb_data *msb = memstick_get_drvdata(card);
589 	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
590 	u8 intreg;
591 
592 	if (mrq->error) {
593 		dbg("send_command: unknown error");
594 		return msb_exit_state_machine(msb, mrq->error);
595 	}
596 again:
597 	switch (msb->state) {
598 
599 	/* HACK: see h_msb_write_block */
600 	case MSB_SC_SEND_WRITE_PARAMS: /* write param register*/
601 		if (!msb_write_regs(msb,
602 			offsetof(struct ms_register, param),
603 			sizeof(struct ms_param_register),
604 			&msb->regs.param))
605 			return 0;
606 		msb->state = MSB_SC_SEND_WRITE_OOB;
607 		return 0;
608 
609 	case MSB_SC_SEND_WRITE_OOB:
610 		if (!msb->command_need_oob) {
611 			msb->state = MSB_SC_SEND_COMMAND;
612 			goto again;
613 		}
614 
615 		if (!msb_write_regs(msb,
616 			offsetof(struct ms_register, extra_data),
617 			sizeof(struct ms_extra_data_register),
618 			&msb->regs.extra_data))
619 			return 0;
620 
621 		msb->state = MSB_SC_SEND_COMMAND;
622 		return 0;
623 
624 	case MSB_SC_SEND_COMMAND:
625 		memstick_init_req(mrq, MS_TPC_SET_CMD, &msb->command_value, 1);
626 		msb->state = MSB_SC_SEND_INT_REQ;
627 		return 0;
628 
629 	case MSB_SC_SEND_INT_REQ:
630 		msb->state = MSB_SC_RECEIVE_INT_REQ;
631 		if (msb_read_int_reg(msb, -1))
632 			return 0;
633 		fallthrough;
634 
635 	case MSB_SC_RECEIVE_INT_REQ:
636 		intreg = mrq->data[0];
637 
638 		if (intreg & MEMSTICK_INT_CMDNAK)
639 			return msb_exit_state_machine(msb, -EIO);
640 		if (intreg & MEMSTICK_INT_ERR)
641 			return msb_exit_state_machine(msb, -EBADMSG);
642 
643 		if (!(intreg & MEMSTICK_INT_CED)) {
644 			msb->state = MSB_SC_SEND_INT_REQ;
645 			goto again;
646 		}
647 
648 		return msb_exit_state_machine(msb, 0);
649 	}
650 
651 	BUG();
652 }
653 
654 /* Small handler for card reset */
h_msb_reset(struct memstick_dev * card,struct memstick_request ** out_mrq)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 */
h_msb_parallel_switch(struct memstick_dev * card,struct memstick_request ** out_mrq)678 static int h_msb_parallel_switch(struct memstick_dev *card,
679 					struct memstick_request **out_mrq)
680 {
681 	struct msb_data *msb = memstick_get_drvdata(card);
682 	struct memstick_request *mrq = *out_mrq = &card->current_mrq;
683 	struct memstick_host *host = card->host;
684 
685 	if (mrq->error) {
686 		dbg("parallel_switch: error");
687 		msb->regs.param.system &= ~MEMSTICK_SYS_PAM;
688 		return msb_exit_state_machine(msb, mrq->error);
689 	}
690 
691 	switch (msb->state) {
692 	case MSB_PS_SEND_SWITCH_COMMAND:
693 		/* Set the parallel interface on memstick side */
694 		msb->regs.param.system |= MEMSTICK_SYS_PAM;
695 
696 		if (!msb_write_regs(msb,
697 			offsetof(struct ms_register, param),
698 			1,
699 			(unsigned char *)&msb->regs.param))
700 			return 0;
701 
702 		msb->state = MSB_PS_SWICH_HOST;
703 		return 0;
704 
705 	case MSB_PS_SWICH_HOST:
706 		 /* Set parallel interface on our side + send a dummy request
707 		  * to see if card responds
708 		  */
709 		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
710 		memstick_init_req(mrq, MS_TPC_GET_INT, NULL, 1);
711 		msb->state = MSB_PS_CONFIRM;
712 		return 0;
713 
714 	case MSB_PS_CONFIRM:
715 		return msb_exit_state_machine(msb, 0);
716 	}
717 
718 	BUG();
719 }
720 
721 static int msb_switch_to_parallel(struct msb_data *msb);
722 
723 /* Reset the card, to guard against hw errors beeing treated as bad blocks */
msb_reset(struct msb_data * msb,bool full)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 */
msb_switch_to_parallel(struct msb_data * msb)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 */
msb_set_overwrite_flag(struct msb_data * msb,u16 pba,u8 page,u8 flag)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 
msb_mark_bad(struct msb_data * msb,int pba)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 
msb_mark_page_bad(struct msb_data * msb,int pba,int page)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 */
msb_erase_block(struct msb_data * msb,u16 pba)824 static int msb_erase_block(struct msb_data *msb, u16 pba)
825 {
826 	int error, try;
827 
828 	if (msb->read_only)
829 		return -EROFS;
830 
831 	dbg_verbose("erasing pba %d", pba);
832 
833 	for (try = 1; try < 3; try++) {
834 		msb->regs.param.block_address = cpu_to_be16(pba);
835 		msb->regs.param.page_address = 0;
836 		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
837 		msb->command_value = MS_CMD_BLOCK_ERASE;
838 		msb->command_need_oob = false;
839 
840 
841 		error = msb_run_state_machine(msb, h_msb_send_command);
842 		if (!error || msb_reset(msb, true))
843 			break;
844 	}
845 
846 	if (error) {
847 		pr_err("erase failed, marking pba %d as bad", pba);
848 		msb_mark_bad(msb, pba);
849 	}
850 
851 	dbg_verbose("erase success, marking pba %d as unused", pba);
852 	msb_mark_block_unused(msb, pba);
853 	__set_bit(pba, msb->erased_blocks_bitmap);
854 	return error;
855 }
856 
857 /* Reads one page from device */
msb_read_page(struct msb_data * msb,u16 pba,u8 page,struct ms_extra_data_register * extra,struct scatterlist * sg,int offset)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 */
msb_read_oob(struct msb_data * msb,u16 pba,u16 page,struct ms_extra_data_register * extra)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 */
msb_verify_block(struct msb_data * msb,u16 pba,struct scatterlist * orig_sg,int offset)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 */
msb_write_block(struct msb_data * msb,u16 pba,u32 lba,struct scatterlist * sg,int offset)1000 static int msb_write_block(struct msb_data *msb,
1001 			u16 pba, u32 lba, struct scatterlist *sg, int offset)
1002 {
1003 	int error, current_try = 1;
1004 
1005 	BUG_ON(sg->length < msb->page_size);
1006 
1007 	if (msb->read_only)
1008 		return -EROFS;
1009 
1010 	if (pba == MS_BLOCK_INVALID) {
1011 		pr_err(
1012 			"BUG: write: attempt to write MS_BLOCK_INVALID block");
1013 		return -EINVAL;
1014 	}
1015 
1016 	if (pba >= msb->block_count || lba >= msb->logical_block_count) {
1017 		pr_err(
1018 		"BUG: write: attempt to write beyond the end of device");
1019 		return -EINVAL;
1020 	}
1021 
1022 	if (msb_get_zone_from_lba(lba) != msb_get_zone_from_pba(pba)) {
1023 		pr_err("BUG: write: lba zone mismatch");
1024 		return -EINVAL;
1025 	}
1026 
1027 	if (pba == msb->boot_block_locations[0] ||
1028 		pba == msb->boot_block_locations[1]) {
1029 		pr_err("BUG: write: attempt to write to boot blocks!");
1030 		return -EINVAL;
1031 	}
1032 
1033 	while (1) {
1034 
1035 		if (msb->read_only)
1036 			return -EROFS;
1037 
1038 		msb->regs.param.cp = MEMSTICK_CP_BLOCK;
1039 		msb->regs.param.page_address = 0;
1040 		msb->regs.param.block_address = cpu_to_be16(pba);
1041 
1042 		msb->regs.extra_data.management_flag = 0xFF;
1043 		msb->regs.extra_data.overwrite_flag = 0xF8;
1044 		msb->regs.extra_data.logical_address = cpu_to_be16(lba);
1045 
1046 		msb->current_sg = sg;
1047 		msb->current_sg_offset = offset;
1048 		msb->current_page = 0;
1049 
1050 		error = msb_run_state_machine(msb, h_msb_write_block);
1051 
1052 		/* Sector we just wrote to is assumed erased since its pba
1053 		 * was erased. If it wasn't erased, write will succeed
1054 		 * and will just clear the bits that were set in the block
1055 		 * thus test that what we have written,
1056 		 * matches what we expect.
1057 		 * We do trust the blocks that we erased
1058 		 */
1059 		if (!error && (verify_writes ||
1060 				!test_bit(pba, msb->erased_blocks_bitmap)))
1061 			error = msb_verify_block(msb, pba, sg, offset);
1062 
1063 		if (!error)
1064 			break;
1065 
1066 		if (current_try > 1 || msb_reset(msb, true))
1067 			break;
1068 
1069 		pr_err("write failed, trying to erase the pba %d", pba);
1070 		error = msb_erase_block(msb, pba);
1071 		if (error)
1072 			break;
1073 
1074 		current_try++;
1075 	}
1076 	return error;
1077 }
1078 
1079 /* Finds a free block for write replacement */
msb_get_free_block(struct msb_data * msb,int zone)1080 static u16 msb_get_free_block(struct msb_data *msb, int zone)
1081 {
1082 	u16 pos;
1083 	int pba = zone * MS_BLOCKS_IN_ZONE;
1084 	int i;
1085 
1086 	get_random_bytes(&pos, sizeof(pos));
1087 
1088 	if (!msb->free_block_count[zone]) {
1089 		pr_err("NO free blocks in the zone %d, to use for a write, (media is WORN out) switching to RO mode", zone);
1090 		msb->read_only = true;
1091 		return MS_BLOCK_INVALID;
1092 	}
1093 
1094 	pos %= msb->free_block_count[zone];
1095 
1096 	dbg_verbose("have %d choices for a free block, selected randomly: %d",
1097 		msb->free_block_count[zone], pos);
1098 
1099 	pba = find_next_zero_bit(msb->used_blocks_bitmap,
1100 							msb->block_count, pba);
1101 	for (i = 0; i < pos; ++i)
1102 		pba = find_next_zero_bit(msb->used_blocks_bitmap,
1103 						msb->block_count, pba + 1);
1104 
1105 	dbg_verbose("result of the free blocks scan: pba %d", pba);
1106 
1107 	if (pba == msb->block_count || (msb_get_zone_from_pba(pba)) != zone) {
1108 		pr_err("BUG: can't get a free block");
1109 		msb->read_only = true;
1110 		return MS_BLOCK_INVALID;
1111 	}
1112 
1113 	msb_mark_block_used(msb, pba);
1114 	return pba;
1115 }
1116 
msb_update_block(struct msb_data * msb,u16 lba,struct scatterlist * sg,int offset)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 */
msb_fix_boot_page_endianness(struct ms_boot_page * p)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 
msb_read_boot_blocks(struct msb_data * msb)1192 static int msb_read_boot_blocks(struct msb_data *msb)
1193 {
1194 	int pba = 0;
1195 	struct scatterlist sg;
1196 	struct ms_extra_data_register extra;
1197 	struct ms_boot_page *page;
1198 
1199 	msb->boot_block_locations[0] = MS_BLOCK_INVALID;
1200 	msb->boot_block_locations[1] = MS_BLOCK_INVALID;
1201 	msb->boot_block_count = 0;
1202 
1203 	dbg_verbose("Start of a scan for the boot blocks");
1204 
1205 	if (!msb->boot_page) {
1206 		page = kmalloc_array(2, 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, 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) {
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(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 
msb_read_bad_block_table(struct msb_data * msb,int block_nr)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 
msb_ftl_initialize(struct msb_data * msb)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(msb->block_count, GFP_KERNEL);
1345 	msb->erased_blocks_bitmap = bitmap_zalloc(msb->block_count, GFP_KERNEL);
1346 	msb->lba_to_pba_table =
1347 		kmalloc_array(msb->logical_block_count, 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(msb->used_blocks_bitmap);
1353 		bitmap_free(msb->erased_blocks_bitmap);
1354 		kfree(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 
msb_ftl_scan(struct msb_data * msb)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 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);
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, 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 
msb_cache_flush_timer(struct timer_list * t)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(msb->io_queue, &msb->io_work);
1505 }
1506 
1507 
msb_cache_discard(struct msb_data * msb)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 
msb_cache_init(struct msb_data * msb)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(msb->block_size, GFP_KERNEL);
1526 	if (!msb->cache)
1527 		return -ENOMEM;
1528 
1529 	msb_cache_discard(msb);
1530 	return 0;
1531 }
1532 
msb_cache_flush(struct msb_data * msb)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 
msb_cache_write(struct msb_data * msb,int lba,int page,bool add_to_cache_only,struct scatterlist * sg,int offset)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 
msb_cache_read(struct msb_data * msb,int lba,int page,struct scatterlist * sg,int offset)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.
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 */
msb_init_card(struct memstick_dev * 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,
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(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 
msb_do_write_request(struct msb_data * msb,int lba,int page,struct scatterlist * sg,size_t len,int * sucessfuly_written)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 
msb_do_read_request(struct msb_data * msb,int lba,int page,struct scatterlist * sg,int len,int * sucessfuly_read)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 
msb_io_work(struct work_struct * work)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(&msb->q_lock);
1888 
1889 		if (msb->need_flush_cache) {
1890 			msb->need_flush_cache = false;
1891 			spin_unlock_irq(&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(&msb->q_lock);
1900 			return;
1901 		}
1902 
1903 		spin_unlock_irq(&msb->q_lock);
1904 
1905 		/* process the request */
1906 		dbg_verbose("IO: processing new request");
1907 		blk_rq_map_sg(msb->queue, req, sg);
1908 
1909 		lba = blk_rq_pos(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 				blk_rq_bytes(req), &len);
1917 		else
1918 			error = msb_do_write_request(msb, lba, page, sg,
1919 				blk_rq_bytes(req), &len);
1920 
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 		}
1927 
1928 		if (error && msb->req) {
1929 			blk_status_t ret = errno_to_blk_status(error);
1930 
1931 			dbg_verbose("IO: ending one sector of the request with error");
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);
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 
msb_data_clear(struct msb_data * msb)1946 static void msb_data_clear(struct msb_data *msb)
1947 {
1948 	kfree(msb->boot_page);
1949 	bitmap_free(msb->used_blocks_bitmap);
1950 	bitmap_free(msb->erased_blocks_bitmap);
1951 	kfree(msb->lba_to_pba_table);
1952 	kfree(msb->cache);
1953 	msb->card = NULL;
1954 }
1955 
msb_bd_getgeo(struct block_device * bdev,struct hd_geometry * geo)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 
msb_bd_free_disk(struct gendisk * disk)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 
msb_queue_rq(struct blk_mq_hw_ctx * hctx,const struct blk_mq_queue_data * bd)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(&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(&msb->q_lock);
1992 		blk_mq_start_request(req);
1993 		return BLK_STS_IOERR;
1994 	}
1995 
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;
2003 
2004 	if (!msb->io_queue_stopped)
2005 		queue_work(msb->io_queue, &msb->io_work);
2006 
2007 	spin_unlock_irq(&msb->q_lock);
2008 	return BLK_STS_OK;
2009 }
2010 
msb_check_card(struct memstick_dev * card)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 
msb_stop(struct memstick_dev * card)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(msb->queue);
2026 	spin_lock_irqsave(&msb->q_lock, flags);
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 
2033 	spin_lock_irqsave(&msb->q_lock, flags);
2034 	if (msb->req) {
2035 		blk_mq_requeue_request(msb->req, false);
2036 		msb->req = NULL;
2037 	}
2038 	spin_unlock_irqrestore(&msb->q_lock, flags);
2039 }
2040 
msb_start(struct memstick_dev * card)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 
2061 	blk_mq_start_hw_queues(msb->queue);
2062 
2063 	queue_work(msb->io_queue, &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 */
msb_init_disk(struct memstick_dev * card)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, card, 0, 256, GFP_KERNEL);
2092 	mutex_unlock(&msb_disk_lock);
2093 
2094 	if (msb->disk_id  < 0)
2095 		return msb->disk_id;
2096 
2097 	rc = blk_mq_alloc_sq_tag_set(&msb->tag_set, &msb_mq_ops, 2,
2098 				     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(msb->disk)) {
2104 		rc = PTR_ERR(msb->disk);
2105 		goto out_free_tag_set;
2106 	}
2107 	msb->queue = msb->disk->queue;
2108 
2109 	sprintf(msb->disk->disk_name, "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(msb->disk, 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(msb->disk, 1);
2129 
2130 	msb_start(card);
2131 	rc = device_add_disk(&card->dev, 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(msb->io_queue);
2139 out_cleanup_disk:
2140 	put_disk(msb->disk);
2141 out_free_tag_set:
2142 	blk_mq_free_tag_set(&msb->tag_set);
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 
msb_probe(struct memstick_dev * card)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 
msb_remove(struct memstick_dev * card)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(&msb->q_lock, flags);
2194 	blk_mq_start_hw_queues(msb->queue);
2195 
2196 	/* Remove the disk */
2197 	del_gendisk(msb->disk);
2198 	blk_mq_free_tag_set(&msb->tag_set);
2199 	msb->queue = NULL;
2200 
2201 	mutex_lock(&msb_disk_lock);
2202 	msb_data_clear(msb);
2203 	mutex_unlock(&msb_disk_lock);
2204 
2205 	put_disk(msb->disk);
2206 	memstick_set_drvdata(card, NULL);
2207 }
2208 
2209 #ifdef CONFIG_PM
2210 
msb_suspend(struct memstick_dev * card,pm_message_t state)2211 static int msb_suspend(struct memstick_dev *card, pm_message_t state)
2212 {
2213 	msb_stop(card);
2214 	return 0;
2215 }
2216 
msb_resume(struct memstick_dev * card)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 ||
2254 	    !bitmap_equal(msb->used_blocks_bitmap, new_msb->used_blocks_bitmap,
2255 							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, 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 
msb_init(void)2314 static int __init msb_init(void)
2315 {
2316 	int rc = memstick_register_driver(&msb_driver);
2317 
2318 	if (rc)
2319 		pr_err("failed to register memstick driver (error %d)\n", rc);
2320 
2321 	return rc;
2322 }
2323 
msb_exit(void)2324 static void __exit msb_exit(void)
2325 {
2326 	memstick_unregister_driver(&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