1 /*
2  *  Sony MemoryStick Pro storage support
3  *
4  *  Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * Special thanks to Carlos Corbacho for providing various MemoryStick cards
11  * that made this driver possible.
12  *
13  */
14 
15 #include <linux/blkdev.h>
16 #include <linux/idr.h>
17 #include <linux/hdreg.h>
18 #include <linux/kthread.h>
19 #include <linux/delay.h>
20 #include <linux/slab.h>
21 #include <linux/mutex.h>
22 #include <linux/memstick.h>
23 #include <linux/module.h>
24 
25 #define DRIVER_NAME "mspro_block"
26 
27 static int major;
28 module_param(major, int, 0644);
29 
30 #define MSPRO_BLOCK_MAX_SEGS  32
31 #define MSPRO_BLOCK_MAX_PAGES ((2 << 16) - 1)
32 
33 #define MSPRO_BLOCK_SIGNATURE        0xa5c3
34 #define MSPRO_BLOCK_MAX_ATTRIBUTES   41
35 
36 #define MSPRO_BLOCK_PART_SHIFT 3
37 
38 enum {
39 	MSPRO_BLOCK_ID_SYSINFO         = 0x10,
40 	MSPRO_BLOCK_ID_MODELNAME       = 0x15,
41 	MSPRO_BLOCK_ID_MBR             = 0x20,
42 	MSPRO_BLOCK_ID_PBR16           = 0x21,
43 	MSPRO_BLOCK_ID_PBR32           = 0x22,
44 	MSPRO_BLOCK_ID_SPECFILEVALUES1 = 0x25,
45 	MSPRO_BLOCK_ID_SPECFILEVALUES2 = 0x26,
46 	MSPRO_BLOCK_ID_DEVINFO         = 0x30
47 };
48 
49 struct mspro_sys_attr {
50 	size_t                  size;
51 	void                    *data;
52 	unsigned char           id;
53 	char                    name[32];
54 	struct device_attribute dev_attr;
55 };
56 
57 struct mspro_attr_entry {
58 	__be32 address;
59 	__be32 size;
60 	unsigned char id;
61 	unsigned char reserved[3];
62 } __attribute__((packed));
63 
64 struct mspro_attribute {
65 	__be16 signature;
66 	unsigned short          version;
67 	unsigned char           count;
68 	unsigned char           reserved[11];
69 	struct mspro_attr_entry entries[];
70 } __attribute__((packed));
71 
72 struct mspro_sys_info {
73 	unsigned char  class;
74 	unsigned char  reserved0;
75 	__be16 block_size;
76 	__be16 block_count;
77 	__be16 user_block_count;
78 	__be16 page_size;
79 	unsigned char  reserved1[2];
80 	unsigned char  assembly_date[8];
81 	__be32 serial_number;
82 	unsigned char  assembly_maker_code;
83 	unsigned char  assembly_model_code[3];
84 	__be16 memory_maker_code;
85 	__be16 memory_model_code;
86 	unsigned char  reserved2[4];
87 	unsigned char  vcc;
88 	unsigned char  vpp;
89 	__be16 controller_number;
90 	__be16 controller_function;
91 	__be16 start_sector;
92 	__be16 unit_size;
93 	unsigned char  ms_sub_class;
94 	unsigned char  reserved3[4];
95 	unsigned char  interface_type;
96 	__be16 controller_code;
97 	unsigned char  format_type;
98 	unsigned char  reserved4;
99 	unsigned char  device_type;
100 	unsigned char  reserved5[7];
101 	unsigned char  mspro_id[16];
102 	unsigned char  reserved6[16];
103 } __attribute__((packed));
104 
105 struct mspro_mbr {
106 	unsigned char boot_partition;
107 	unsigned char start_head;
108 	unsigned char start_sector;
109 	unsigned char start_cylinder;
110 	unsigned char partition_type;
111 	unsigned char end_head;
112 	unsigned char end_sector;
113 	unsigned char end_cylinder;
114 	unsigned int  start_sectors;
115 	unsigned int  sectors_per_partition;
116 } __attribute__((packed));
117 
118 struct mspro_specfile {
119 	char           name[8];
120 	char           ext[3];
121 	unsigned char  attr;
122 	unsigned char  reserved[10];
123 	unsigned short time;
124 	unsigned short date;
125 	unsigned short cluster;
126 	unsigned int   size;
127 } __attribute__((packed));
128 
129 struct mspro_devinfo {
130 	__be16 cylinders;
131 	__be16 heads;
132 	__be16 bytes_per_track;
133 	__be16 bytes_per_sector;
134 	__be16 sectors_per_track;
135 	unsigned char  reserved[6];
136 } __attribute__((packed));
137 
138 struct mspro_block_data {
139 	struct memstick_dev   *card;
140 	unsigned int          usage_count;
141 	unsigned int          caps;
142 	struct gendisk        *disk;
143 	struct request_queue  *queue;
144 	struct request        *block_req;
145 	spinlock_t            q_lock;
146 
147 	unsigned short        page_size;
148 	unsigned short        cylinders;
149 	unsigned short        heads;
150 	unsigned short        sectors_per_track;
151 
152 	unsigned char         system;
153 	unsigned char         read_only:1,
154 			      eject:1,
155 			      has_request:1,
156 			      data_dir:1,
157 			      active:1;
158 	unsigned char         transfer_cmd;
159 
160 	int                   (*mrq_handler)(struct memstick_dev *card,
161 					     struct memstick_request **mrq);
162 
163 
164 	/* Default request setup function for data access method preferred by
165 	 * this host instance.
166 	 */
167 	void                  (*setup_transfer)(struct memstick_dev *card,
168 						u64 offset, size_t length);
169 
170 	struct attribute_group attr_group;
171 
172 	struct scatterlist    req_sg[MSPRO_BLOCK_MAX_SEGS];
173 	unsigned int          seg_count;
174 	unsigned int          current_seg;
175 	unsigned int          current_page;
176 };
177 
178 static DEFINE_IDR(mspro_block_disk_idr);
179 static DEFINE_MUTEX(mspro_block_disk_lock);
180 
181 static int mspro_block_complete_req(struct memstick_dev *card, int error);
182 
183 /*** Block device ***/
184 
185 static int mspro_block_bd_open(struct block_device *bdev, fmode_t mode)
186 {
187 	struct gendisk *disk = bdev->bd_disk;
188 	struct mspro_block_data *msb = disk->private_data;
189 	int rc = -ENXIO;
190 
191 	mutex_lock(&mspro_block_disk_lock);
192 
193 	if (msb && msb->card) {
194 		msb->usage_count++;
195 		if ((mode & FMODE_WRITE) && msb->read_only)
196 			rc = -EROFS;
197 		else
198 			rc = 0;
199 	}
200 
201 	mutex_unlock(&mspro_block_disk_lock);
202 
203 	return rc;
204 }
205 
206 
207 static void mspro_block_disk_release(struct gendisk *disk)
208 {
209 	struct mspro_block_data *msb = disk->private_data;
210 	int disk_id = MINOR(disk_devt(disk)) >> MSPRO_BLOCK_PART_SHIFT;
211 
212 	mutex_lock(&mspro_block_disk_lock);
213 
214 	if (msb) {
215 		if (msb->usage_count)
216 			msb->usage_count--;
217 
218 		if (!msb->usage_count) {
219 			kfree(msb);
220 			disk->private_data = NULL;
221 			idr_remove(&mspro_block_disk_idr, disk_id);
222 			put_disk(disk);
223 		}
224 	}
225 
226 	mutex_unlock(&mspro_block_disk_lock);
227 }
228 
229 static void mspro_block_bd_release(struct gendisk *disk, fmode_t mode)
230 {
231 	mspro_block_disk_release(disk);
232 }
233 
234 static int mspro_block_bd_getgeo(struct block_device *bdev,
235 				 struct hd_geometry *geo)
236 {
237 	struct mspro_block_data *msb = bdev->bd_disk->private_data;
238 
239 	geo->heads = msb->heads;
240 	geo->sectors = msb->sectors_per_track;
241 	geo->cylinders = msb->cylinders;
242 
243 	return 0;
244 }
245 
246 static const struct block_device_operations ms_block_bdops = {
247 	.open    = mspro_block_bd_open,
248 	.release = mspro_block_bd_release,
249 	.getgeo  = mspro_block_bd_getgeo,
250 	.owner   = THIS_MODULE
251 };
252 
253 /*** Information ***/
254 
255 static struct mspro_sys_attr *mspro_from_sysfs_attr(struct attribute *attr)
256 {
257 	struct device_attribute *dev_attr
258 		= container_of(attr, struct device_attribute, attr);
259 	return container_of(dev_attr, struct mspro_sys_attr, dev_attr);
260 }
261 
262 static const char *mspro_block_attr_name(unsigned char tag)
263 {
264 	switch (tag) {
265 	case MSPRO_BLOCK_ID_SYSINFO:
266 		return "attr_sysinfo";
267 	case MSPRO_BLOCK_ID_MODELNAME:
268 		return "attr_modelname";
269 	case MSPRO_BLOCK_ID_MBR:
270 		return "attr_mbr";
271 	case MSPRO_BLOCK_ID_PBR16:
272 		return "attr_pbr16";
273 	case MSPRO_BLOCK_ID_PBR32:
274 		return "attr_pbr32";
275 	case MSPRO_BLOCK_ID_SPECFILEVALUES1:
276 		return "attr_specfilevalues1";
277 	case MSPRO_BLOCK_ID_SPECFILEVALUES2:
278 		return "attr_specfilevalues2";
279 	case MSPRO_BLOCK_ID_DEVINFO:
280 		return "attr_devinfo";
281 	default:
282 		return NULL;
283 	};
284 }
285 
286 typedef ssize_t (*sysfs_show_t)(struct device *dev,
287 				struct device_attribute *attr,
288 				char *buffer);
289 
290 static ssize_t mspro_block_attr_show_default(struct device *dev,
291 					     struct device_attribute *attr,
292 					     char *buffer)
293 {
294 	struct mspro_sys_attr *s_attr = container_of(attr,
295 						     struct mspro_sys_attr,
296 						     dev_attr);
297 
298 	ssize_t cnt, rc = 0;
299 
300 	for (cnt = 0; cnt < s_attr->size; cnt++) {
301 		if (cnt && !(cnt % 16)) {
302 			if (PAGE_SIZE - rc)
303 				buffer[rc++] = '\n';
304 		}
305 
306 		rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "%02x ",
307 				((unsigned char *)s_attr->data)[cnt]);
308 	}
309 	return rc;
310 }
311 
312 static ssize_t mspro_block_attr_show_sysinfo(struct device *dev,
313 					     struct device_attribute *attr,
314 					     char *buffer)
315 {
316 	struct mspro_sys_attr *x_attr = container_of(attr,
317 						     struct mspro_sys_attr,
318 						     dev_attr);
319 	struct mspro_sys_info *x_sys = x_attr->data;
320 	ssize_t rc = 0;
321 	int date_tz = 0, date_tz_f = 0;
322 
323 	if (x_sys->assembly_date[0] > 0x80U) {
324 		date_tz = (~x_sys->assembly_date[0]) + 1;
325 		date_tz_f = date_tz & 3;
326 		date_tz >>= 2;
327 		date_tz = -date_tz;
328 		date_tz_f *= 15;
329 	} else if (x_sys->assembly_date[0] < 0x80U) {
330 		date_tz = x_sys->assembly_date[0];
331 		date_tz_f = date_tz & 3;
332 		date_tz >>= 2;
333 		date_tz_f *= 15;
334 	}
335 
336 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "class: %x\n",
337 			x_sys->class);
338 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block size: %x\n",
339 			be16_to_cpu(x_sys->block_size));
340 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block count: %x\n",
341 			be16_to_cpu(x_sys->block_count));
342 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "user block count: %x\n",
343 			be16_to_cpu(x_sys->user_block_count));
344 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "page size: %x\n",
345 			be16_to_cpu(x_sys->page_size));
346 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly date: "
347 			"GMT%+d:%d %04u-%02u-%02u %02u:%02u:%02u\n",
348 			date_tz, date_tz_f,
349 			be16_to_cpup((__be16 *)&x_sys->assembly_date[1]),
350 			x_sys->assembly_date[3], x_sys->assembly_date[4],
351 			x_sys->assembly_date[5], x_sys->assembly_date[6],
352 			x_sys->assembly_date[7]);
353 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "serial number: %x\n",
354 			be32_to_cpu(x_sys->serial_number));
355 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
356 			"assembly maker code: %x\n",
357 			x_sys->assembly_maker_code);
358 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly model code: "
359 			"%02x%02x%02x\n", x_sys->assembly_model_code[0],
360 			x_sys->assembly_model_code[1],
361 			x_sys->assembly_model_code[2]);
362 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory maker code: %x\n",
363 			be16_to_cpu(x_sys->memory_maker_code));
364 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory model code: %x\n",
365 			be16_to_cpu(x_sys->memory_model_code));
366 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vcc: %x\n",
367 			x_sys->vcc);
368 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vpp: %x\n",
369 			x_sys->vpp);
370 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller number: %x\n",
371 			be16_to_cpu(x_sys->controller_number));
372 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
373 			"controller function: %x\n",
374 			be16_to_cpu(x_sys->controller_function));
375 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
376 			be16_to_cpu(x_sys->start_sector));
377 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "unit size: %x\n",
378 			be16_to_cpu(x_sys->unit_size));
379 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sub class: %x\n",
380 			x_sys->ms_sub_class);
381 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "interface type: %x\n",
382 			x_sys->interface_type);
383 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller code: %x\n",
384 			be16_to_cpu(x_sys->controller_code));
385 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "format type: %x\n",
386 			x_sys->format_type);
387 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "device type: %x\n",
388 			x_sys->device_type);
389 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "mspro id: %s\n",
390 			x_sys->mspro_id);
391 	return rc;
392 }
393 
394 static ssize_t mspro_block_attr_show_modelname(struct device *dev,
395 					       struct device_attribute *attr,
396 					       char *buffer)
397 {
398 	struct mspro_sys_attr *s_attr = container_of(attr,
399 						     struct mspro_sys_attr,
400 						     dev_attr);
401 
402 	return scnprintf(buffer, PAGE_SIZE, "%s", (char *)s_attr->data);
403 }
404 
405 static ssize_t mspro_block_attr_show_mbr(struct device *dev,
406 					 struct device_attribute *attr,
407 					 char *buffer)
408 {
409 	struct mspro_sys_attr *x_attr = container_of(attr,
410 						     struct mspro_sys_attr,
411 						     dev_attr);
412 	struct mspro_mbr *x_mbr = x_attr->data;
413 	ssize_t rc = 0;
414 
415 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "boot partition: %x\n",
416 			x_mbr->boot_partition);
417 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start head: %x\n",
418 			x_mbr->start_head);
419 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
420 			x_mbr->start_sector);
421 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cylinder: %x\n",
422 			x_mbr->start_cylinder);
423 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "partition type: %x\n",
424 			x_mbr->partition_type);
425 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end head: %x\n",
426 			x_mbr->end_head);
427 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end sector: %x\n",
428 			x_mbr->end_sector);
429 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end cylinder: %x\n",
430 			x_mbr->end_cylinder);
431 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sectors: %x\n",
432 			x_mbr->start_sectors);
433 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
434 			"sectors per partition: %x\n",
435 			x_mbr->sectors_per_partition);
436 	return rc;
437 }
438 
439 static ssize_t mspro_block_attr_show_specfile(struct device *dev,
440 					      struct device_attribute *attr,
441 					      char *buffer)
442 {
443 	struct mspro_sys_attr *x_attr = container_of(attr,
444 						     struct mspro_sys_attr,
445 						     dev_attr);
446 	struct mspro_specfile *x_spfile = x_attr->data;
447 	char name[9], ext[4];
448 	ssize_t rc = 0;
449 
450 	memcpy(name, x_spfile->name, 8);
451 	name[8] = 0;
452 	memcpy(ext, x_spfile->ext, 3);
453 	ext[3] = 0;
454 
455 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "name: %s\n", name);
456 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "ext: %s\n", ext);
457 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "attribute: %x\n",
458 			x_spfile->attr);
459 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "time: %d:%d:%d\n",
460 			x_spfile->time >> 11,
461 			(x_spfile->time >> 5) & 0x3f,
462 			(x_spfile->time & 0x1f) * 2);
463 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "date: %d-%d-%d\n",
464 			(x_spfile->date >> 9) + 1980,
465 			(x_spfile->date >> 5) & 0xf,
466 			x_spfile->date & 0x1f);
467 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cluster: %x\n",
468 			x_spfile->cluster);
469 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "size: %x\n",
470 			x_spfile->size);
471 	return rc;
472 }
473 
474 static ssize_t mspro_block_attr_show_devinfo(struct device *dev,
475 					     struct device_attribute *attr,
476 					     char *buffer)
477 {
478 	struct mspro_sys_attr *x_attr = container_of(attr,
479 						     struct mspro_sys_attr,
480 						     dev_attr);
481 	struct mspro_devinfo *x_devinfo = x_attr->data;
482 	ssize_t rc = 0;
483 
484 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "cylinders: %x\n",
485 			be16_to_cpu(x_devinfo->cylinders));
486 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "heads: %x\n",
487 			be16_to_cpu(x_devinfo->heads));
488 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per track: %x\n",
489 			be16_to_cpu(x_devinfo->bytes_per_track));
490 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per sector: %x\n",
491 			be16_to_cpu(x_devinfo->bytes_per_sector));
492 	rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sectors per track: %x\n",
493 			be16_to_cpu(x_devinfo->sectors_per_track));
494 	return rc;
495 }
496 
497 static sysfs_show_t mspro_block_attr_show(unsigned char tag)
498 {
499 	switch (tag) {
500 	case MSPRO_BLOCK_ID_SYSINFO:
501 		return mspro_block_attr_show_sysinfo;
502 	case MSPRO_BLOCK_ID_MODELNAME:
503 		return mspro_block_attr_show_modelname;
504 	case MSPRO_BLOCK_ID_MBR:
505 		return mspro_block_attr_show_mbr;
506 	case MSPRO_BLOCK_ID_SPECFILEVALUES1:
507 	case MSPRO_BLOCK_ID_SPECFILEVALUES2:
508 		return mspro_block_attr_show_specfile;
509 	case MSPRO_BLOCK_ID_DEVINFO:
510 		return mspro_block_attr_show_devinfo;
511 	default:
512 		return mspro_block_attr_show_default;
513 	}
514 }
515 
516 /*** Protocol handlers ***/
517 
518 /*
519  * Functions prefixed with "h_" are protocol callbacks. They can be called from
520  * interrupt context. Return value of 0 means that request processing is still
521  * ongoing, while special error value of -EAGAIN means that current request is
522  * finished (and request processor should come back some time later).
523  */
524 
525 static int h_mspro_block_req_init(struct memstick_dev *card,
526 				  struct memstick_request **mrq)
527 {
528 	struct mspro_block_data *msb = memstick_get_drvdata(card);
529 
530 	*mrq = &card->current_mrq;
531 	card->next_request = msb->mrq_handler;
532 	return 0;
533 }
534 
535 static int h_mspro_block_default(struct memstick_dev *card,
536 				 struct memstick_request **mrq)
537 {
538 	return mspro_block_complete_req(card, (*mrq)->error);
539 }
540 
541 static int h_mspro_block_default_bad(struct memstick_dev *card,
542 				     struct memstick_request **mrq)
543 {
544 	return -ENXIO;
545 }
546 
547 static int h_mspro_block_get_ro(struct memstick_dev *card,
548 				struct memstick_request **mrq)
549 {
550 	struct mspro_block_data *msb = memstick_get_drvdata(card);
551 
552 	if (!(*mrq)->error) {
553 		if ((*mrq)->data[offsetof(struct ms_status_register, status0)]
554 		    & MEMSTICK_STATUS0_WP)
555 			msb->read_only = 1;
556 		else
557 			msb->read_only = 0;
558 	}
559 
560 	return mspro_block_complete_req(card, (*mrq)->error);
561 }
562 
563 static int h_mspro_block_wait_for_ced(struct memstick_dev *card,
564 				      struct memstick_request **mrq)
565 {
566 	dev_dbg(&card->dev, "wait for ced: value %x\n", (*mrq)->data[0]);
567 
568 	if (!(*mrq)->error) {
569 		if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR))
570 			(*mrq)->error = -EFAULT;
571 		else if (!((*mrq)->data[0] & MEMSTICK_INT_CED))
572 			return 0;
573 	}
574 
575 	return mspro_block_complete_req(card, (*mrq)->error);
576 }
577 
578 static int h_mspro_block_transfer_data(struct memstick_dev *card,
579 				       struct memstick_request **mrq)
580 {
581 	struct mspro_block_data *msb = memstick_get_drvdata(card);
582 	unsigned char t_val = 0;
583 	struct scatterlist t_sg = { 0 };
584 	size_t t_offset;
585 
586 	if ((*mrq)->error)
587 		return mspro_block_complete_req(card, (*mrq)->error);
588 
589 	switch ((*mrq)->tpc) {
590 	case MS_TPC_WRITE_REG:
591 		memstick_init_req(*mrq, MS_TPC_SET_CMD, &msb->transfer_cmd, 1);
592 		(*mrq)->need_card_int = 1;
593 		return 0;
594 	case MS_TPC_SET_CMD:
595 		t_val = (*mrq)->int_reg;
596 		memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
597 		if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT)
598 			goto has_int_reg;
599 		return 0;
600 	case MS_TPC_GET_INT:
601 		t_val = (*mrq)->data[0];
602 has_int_reg:
603 		if (t_val & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) {
604 			t_val = MSPRO_CMD_STOP;
605 			memstick_init_req(*mrq, MS_TPC_SET_CMD, &t_val, 1);
606 			card->next_request = h_mspro_block_default;
607 			return 0;
608 		}
609 
610 		if (msb->current_page
611 		    == (msb->req_sg[msb->current_seg].length
612 			/ msb->page_size)) {
613 			msb->current_page = 0;
614 			msb->current_seg++;
615 
616 			if (msb->current_seg == msb->seg_count) {
617 				if (t_val & MEMSTICK_INT_CED) {
618 					return mspro_block_complete_req(card,
619 									0);
620 				} else {
621 					card->next_request
622 						= h_mspro_block_wait_for_ced;
623 					memstick_init_req(*mrq, MS_TPC_GET_INT,
624 							  NULL, 1);
625 					return 0;
626 				}
627 			}
628 		}
629 
630 		if (!(t_val & MEMSTICK_INT_BREQ)) {
631 			memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
632 			return 0;
633 		}
634 
635 		t_offset = msb->req_sg[msb->current_seg].offset;
636 		t_offset += msb->current_page * msb->page_size;
637 
638 		sg_set_page(&t_sg,
639 			    nth_page(sg_page(&(msb->req_sg[msb->current_seg])),
640 				     t_offset >> PAGE_SHIFT),
641 			    msb->page_size, offset_in_page(t_offset));
642 
643 		memstick_init_req_sg(*mrq, msb->data_dir == READ
644 					   ? MS_TPC_READ_LONG_DATA
645 					   : MS_TPC_WRITE_LONG_DATA,
646 				     &t_sg);
647 		(*mrq)->need_card_int = 1;
648 		return 0;
649 	case MS_TPC_READ_LONG_DATA:
650 	case MS_TPC_WRITE_LONG_DATA:
651 		msb->current_page++;
652 		if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT) {
653 			t_val = (*mrq)->int_reg;
654 			goto has_int_reg;
655 		} else {
656 			memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
657 			return 0;
658 		}
659 
660 	default:
661 		BUG();
662 	}
663 }
664 
665 /*** Transfer setup functions for different access methods. ***/
666 
667 /** Setup data transfer request for SET_CMD TPC with arguments in card
668  *  registers.
669  *
670  *  @card    Current media instance
671  *  @offset  Target data offset in bytes
672  *  @length  Required transfer length in bytes.
673  */
674 static void h_mspro_block_setup_cmd(struct memstick_dev *card, u64 offset,
675 				    size_t length)
676 {
677 	struct mspro_block_data *msb = memstick_get_drvdata(card);
678 	struct mspro_param_register param = {
679 		.system = msb->system,
680 		.data_count = cpu_to_be16((uint16_t)(length / msb->page_size)),
681 		/* ISO C90 warning precludes direct initialization for now. */
682 		.data_address = 0,
683 		.tpc_param = 0
684 	};
685 
686 	do_div(offset, msb->page_size);
687 	param.data_address = cpu_to_be32((uint32_t)offset);
688 
689 	card->next_request = h_mspro_block_req_init;
690 	msb->mrq_handler = h_mspro_block_transfer_data;
691 	memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
692 			  &param, sizeof(param));
693 }
694 
695 /*** Data transfer ***/
696 
697 static int mspro_block_issue_req(struct memstick_dev *card, int chunk)
698 {
699 	struct mspro_block_data *msb = memstick_get_drvdata(card);
700 	u64 t_off;
701 	unsigned int count;
702 
703 try_again:
704 	while (chunk) {
705 		msb->current_page = 0;
706 		msb->current_seg = 0;
707 		msb->seg_count = blk_rq_map_sg(msb->block_req->q,
708 					       msb->block_req,
709 					       msb->req_sg);
710 
711 		if (!msb->seg_count) {
712 			chunk = __blk_end_request_cur(msb->block_req, -ENOMEM);
713 			continue;
714 		}
715 
716 		t_off = blk_rq_pos(msb->block_req);
717 		t_off <<= 9;
718 		count = blk_rq_bytes(msb->block_req);
719 
720 		msb->setup_transfer(card, t_off, count);
721 
722 		msb->data_dir = rq_data_dir(msb->block_req);
723 		msb->transfer_cmd = msb->data_dir == READ
724 				    ? MSPRO_CMD_READ_DATA
725 				    : MSPRO_CMD_WRITE_DATA;
726 
727 		memstick_new_req(card->host);
728 		return 0;
729 	}
730 
731 	dev_dbg(&card->dev, "blk_fetch\n");
732 	msb->block_req = blk_fetch_request(msb->queue);
733 	if (!msb->block_req) {
734 		dev_dbg(&card->dev, "issue end\n");
735 		return -EAGAIN;
736 	}
737 
738 	dev_dbg(&card->dev, "trying again\n");
739 	chunk = 1;
740 	goto try_again;
741 }
742 
743 static int mspro_block_complete_req(struct memstick_dev *card, int error)
744 {
745 	struct mspro_block_data *msb = memstick_get_drvdata(card);
746 	int chunk, cnt;
747 	unsigned int t_len = 0;
748 	unsigned long flags;
749 
750 	spin_lock_irqsave(&msb->q_lock, flags);
751 	dev_dbg(&card->dev, "complete %d, %d\n", msb->has_request ? 1 : 0,
752 		error);
753 
754 	if (msb->has_request) {
755 		/* Nothing to do - not really an error */
756 		if (error == -EAGAIN)
757 			error = 0;
758 
759 		if (error || (card->current_mrq.tpc == MSPRO_CMD_STOP)) {
760 			if (msb->data_dir == READ) {
761 				for (cnt = 0; cnt < msb->current_seg; cnt++) {
762 					t_len += msb->req_sg[cnt].length
763 						 / msb->page_size;
764 
765 					if (msb->current_page)
766 						t_len += msb->current_page - 1;
767 
768 					t_len *= msb->page_size;
769 				}
770 			}
771 		} else
772 			t_len = blk_rq_bytes(msb->block_req);
773 
774 		dev_dbg(&card->dev, "transferred %x (%d)\n", t_len, error);
775 
776 		if (error && !t_len)
777 			t_len = blk_rq_cur_bytes(msb->block_req);
778 
779 		chunk = __blk_end_request(msb->block_req, error, t_len);
780 
781 		error = mspro_block_issue_req(card, chunk);
782 
783 		if (!error)
784 			goto out;
785 		else
786 			msb->has_request = 0;
787 	} else {
788 		if (!error)
789 			error = -EAGAIN;
790 	}
791 
792 	card->next_request = h_mspro_block_default_bad;
793 	complete_all(&card->mrq_complete);
794 out:
795 	spin_unlock_irqrestore(&msb->q_lock, flags);
796 	return error;
797 }
798 
799 static void mspro_block_stop(struct memstick_dev *card)
800 {
801 	struct mspro_block_data *msb = memstick_get_drvdata(card);
802 	int rc = 0;
803 	unsigned long flags;
804 
805 	while (1) {
806 		spin_lock_irqsave(&msb->q_lock, flags);
807 		if (!msb->has_request) {
808 			blk_stop_queue(msb->queue);
809 			rc = 1;
810 		}
811 		spin_unlock_irqrestore(&msb->q_lock, flags);
812 
813 		if (rc)
814 			break;
815 
816 		wait_for_completion(&card->mrq_complete);
817 	}
818 }
819 
820 static void mspro_block_start(struct memstick_dev *card)
821 {
822 	struct mspro_block_data *msb = memstick_get_drvdata(card);
823 	unsigned long flags;
824 
825 	spin_lock_irqsave(&msb->q_lock, flags);
826 	blk_start_queue(msb->queue);
827 	spin_unlock_irqrestore(&msb->q_lock, flags);
828 }
829 
830 static int mspro_block_prepare_req(struct request_queue *q, struct request *req)
831 {
832 	if (req->cmd_type != REQ_TYPE_FS &&
833 	    req->cmd_type != REQ_TYPE_BLOCK_PC) {
834 		blk_dump_rq_flags(req, "MSPro unsupported request");
835 		return BLKPREP_KILL;
836 	}
837 
838 	req->cmd_flags |= REQ_DONTPREP;
839 
840 	return BLKPREP_OK;
841 }
842 
843 static void mspro_block_submit_req(struct request_queue *q)
844 {
845 	struct memstick_dev *card = q->queuedata;
846 	struct mspro_block_data *msb = memstick_get_drvdata(card);
847 	struct request *req = NULL;
848 
849 	if (msb->has_request)
850 		return;
851 
852 	if (msb->eject) {
853 		while ((req = blk_fetch_request(q)) != NULL)
854 			__blk_end_request_all(req, -ENODEV);
855 
856 		return;
857 	}
858 
859 	msb->has_request = 1;
860 	if (mspro_block_issue_req(card, 0))
861 		msb->has_request = 0;
862 }
863 
864 /*** Initialization ***/
865 
866 static int mspro_block_wait_for_ced(struct memstick_dev *card)
867 {
868 	struct mspro_block_data *msb = memstick_get_drvdata(card);
869 
870 	card->next_request = h_mspro_block_req_init;
871 	msb->mrq_handler = h_mspro_block_wait_for_ced;
872 	memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
873 	memstick_new_req(card->host);
874 	wait_for_completion(&card->mrq_complete);
875 	return card->current_mrq.error;
876 }
877 
878 static int mspro_block_set_interface(struct memstick_dev *card,
879 				     unsigned char sys_reg)
880 {
881 	struct memstick_host *host = card->host;
882 	struct mspro_block_data *msb = memstick_get_drvdata(card);
883 	struct mspro_param_register param = {
884 		.system = sys_reg,
885 		.data_count = 0,
886 		.data_address = 0,
887 		.tpc_param = 0
888 	};
889 
890 	card->next_request = h_mspro_block_req_init;
891 	msb->mrq_handler = h_mspro_block_default;
892 	memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
893 			  sizeof(param));
894 	memstick_new_req(host);
895 	wait_for_completion(&card->mrq_complete);
896 	return card->current_mrq.error;
897 }
898 
899 static int mspro_block_switch_interface(struct memstick_dev *card)
900 {
901 	struct memstick_host *host = card->host;
902 	struct mspro_block_data *msb = memstick_get_drvdata(card);
903 	int rc = 0;
904 
905 try_again:
906 	if (msb->caps & MEMSTICK_CAP_PAR4)
907 		rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR4);
908 	else
909 		return 0;
910 
911 	if (rc) {
912 		printk(KERN_WARNING
913 		       "%s: could not switch to 4-bit mode, error %d\n",
914 		       dev_name(&card->dev), rc);
915 		return 0;
916 	}
917 
918 	msb->system = MEMSTICK_SYS_PAR4;
919 	host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
920 	printk(KERN_INFO "%s: switching to 4-bit parallel mode\n",
921 	       dev_name(&card->dev));
922 
923 	if (msb->caps & MEMSTICK_CAP_PAR8) {
924 		rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8);
925 
926 		if (!rc) {
927 			msb->system = MEMSTICK_SYS_PAR8;
928 			host->set_param(host, MEMSTICK_INTERFACE,
929 					MEMSTICK_PAR8);
930 			printk(KERN_INFO
931 			       "%s: switching to 8-bit parallel mode\n",
932 			       dev_name(&card->dev));
933 		} else
934 			printk(KERN_WARNING
935 			       "%s: could not switch to 8-bit mode, error %d\n",
936 			       dev_name(&card->dev), rc);
937 	}
938 
939 	card->next_request = h_mspro_block_req_init;
940 	msb->mrq_handler = h_mspro_block_default;
941 	memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
942 	memstick_new_req(card->host);
943 	wait_for_completion(&card->mrq_complete);
944 	rc = card->current_mrq.error;
945 
946 	if (rc) {
947 		printk(KERN_WARNING
948 		       "%s: interface error, trying to fall back to serial\n",
949 		       dev_name(&card->dev));
950 		msb->system = MEMSTICK_SYS_SERIAL;
951 		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
952 		msleep(10);
953 		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
954 		host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
955 
956 		rc = memstick_set_rw_addr(card);
957 		if (!rc)
958 			rc = mspro_block_set_interface(card, msb->system);
959 
960 		if (!rc) {
961 			msleep(150);
962 			rc = mspro_block_wait_for_ced(card);
963 			if (rc)
964 				return rc;
965 
966 			if (msb->caps & MEMSTICK_CAP_PAR8) {
967 				msb->caps &= ~MEMSTICK_CAP_PAR8;
968 				goto try_again;
969 			}
970 		}
971 	}
972 	return rc;
973 }
974 
975 /* Memory allocated for attributes by this function should be freed by
976  * mspro_block_data_clear, no matter if the initialization process succeeded
977  * or failed.
978  */
979 static int mspro_block_read_attributes(struct memstick_dev *card)
980 {
981 	struct mspro_block_data *msb = memstick_get_drvdata(card);
982 	struct mspro_attribute *attr = NULL;
983 	struct mspro_sys_attr *s_attr = NULL;
984 	unsigned char *buffer = NULL;
985 	int cnt, rc, attr_count;
986 	/* While normally physical device offsets, represented here by
987 	 * attr_offset and attr_len will be of large numeric types, we can be
988 	 * sure, that attributes are close enough to the beginning of the
989 	 * device, to save ourselves some trouble.
990 	 */
991 	unsigned int addr, attr_offset = 0, attr_len = msb->page_size;
992 
993 	attr = kmalloc(msb->page_size, GFP_KERNEL);
994 	if (!attr)
995 		return -ENOMEM;
996 
997 	sg_init_one(&msb->req_sg[0], attr, msb->page_size);
998 	msb->seg_count = 1;
999 	msb->current_seg = 0;
1000 	msb->current_page = 0;
1001 	msb->data_dir = READ;
1002 	msb->transfer_cmd = MSPRO_CMD_READ_ATRB;
1003 
1004 	msb->setup_transfer(card, attr_offset, attr_len);
1005 
1006 	memstick_new_req(card->host);
1007 	wait_for_completion(&card->mrq_complete);
1008 	if (card->current_mrq.error) {
1009 		rc = card->current_mrq.error;
1010 		goto out_free_attr;
1011 	}
1012 
1013 	if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) {
1014 		printk(KERN_ERR "%s: unrecognized device signature %x\n",
1015 		       dev_name(&card->dev), be16_to_cpu(attr->signature));
1016 		rc = -ENODEV;
1017 		goto out_free_attr;
1018 	}
1019 
1020 	if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) {
1021 		printk(KERN_WARNING "%s: way too many attribute entries\n",
1022 		       dev_name(&card->dev));
1023 		attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES;
1024 	} else
1025 		attr_count = attr->count;
1026 
1027 	msb->attr_group.attrs = kcalloc(attr_count + 1,
1028 					sizeof(*msb->attr_group.attrs),
1029 					GFP_KERNEL);
1030 	if (!msb->attr_group.attrs) {
1031 		rc = -ENOMEM;
1032 		goto out_free_attr;
1033 	}
1034 	msb->attr_group.name = "media_attributes";
1035 
1036 	buffer = kmalloc(attr_len, GFP_KERNEL);
1037 	if (!buffer) {
1038 		rc = -ENOMEM;
1039 		goto out_free_attr;
1040 	}
1041 	memcpy(buffer, (char *)attr, attr_len);
1042 
1043 	for (cnt = 0; cnt < attr_count; ++cnt) {
1044 		s_attr = kzalloc(sizeof(struct mspro_sys_attr), GFP_KERNEL);
1045 		if (!s_attr) {
1046 			rc = -ENOMEM;
1047 			goto out_free_buffer;
1048 		}
1049 
1050 		msb->attr_group.attrs[cnt] = &s_attr->dev_attr.attr;
1051 		addr = be32_to_cpu(attr->entries[cnt].address);
1052 		s_attr->size = be32_to_cpu(attr->entries[cnt].size);
1053 		dev_dbg(&card->dev, "adding attribute %d: id %x, address %x, "
1054 			"size %zx\n", cnt, attr->entries[cnt].id, addr,
1055 			s_attr->size);
1056 		s_attr->id = attr->entries[cnt].id;
1057 		if (mspro_block_attr_name(s_attr->id))
1058 			snprintf(s_attr->name, sizeof(s_attr->name), "%s",
1059 				 mspro_block_attr_name(attr->entries[cnt].id));
1060 		else
1061 			snprintf(s_attr->name, sizeof(s_attr->name),
1062 				 "attr_x%02x", attr->entries[cnt].id);
1063 
1064 		sysfs_attr_init(&s_attr->dev_attr.attr);
1065 		s_attr->dev_attr.attr.name = s_attr->name;
1066 		s_attr->dev_attr.attr.mode = S_IRUGO;
1067 		s_attr->dev_attr.show = mspro_block_attr_show(s_attr->id);
1068 
1069 		if (!s_attr->size)
1070 			continue;
1071 
1072 		s_attr->data = kmalloc(s_attr->size, GFP_KERNEL);
1073 		if (!s_attr->data) {
1074 			rc = -ENOMEM;
1075 			goto out_free_buffer;
1076 		}
1077 
1078 		if (((addr / msb->page_size) == (attr_offset / msb->page_size))
1079 		    && (((addr + s_attr->size - 1) / msb->page_size)
1080 			== (attr_offset / msb->page_size))) {
1081 			memcpy(s_attr->data, buffer + addr % msb->page_size,
1082 			       s_attr->size);
1083 			continue;
1084 		}
1085 
1086 		attr_offset = (addr / msb->page_size) * msb->page_size;
1087 
1088 		if ((attr_offset + attr_len) < (addr + s_attr->size)) {
1089 			kfree(buffer);
1090 			attr_len = (((addr + s_attr->size) / msb->page_size)
1091 				    + 1 ) * msb->page_size - attr_offset;
1092 			buffer = kmalloc(attr_len, GFP_KERNEL);
1093 			if (!buffer) {
1094 				rc = -ENOMEM;
1095 				goto out_free_attr;
1096 			}
1097 		}
1098 
1099 		sg_init_one(&msb->req_sg[0], buffer, attr_len);
1100 		msb->seg_count = 1;
1101 		msb->current_seg = 0;
1102 		msb->current_page = 0;
1103 		msb->data_dir = READ;
1104 		msb->transfer_cmd = MSPRO_CMD_READ_ATRB;
1105 
1106 		dev_dbg(&card->dev, "reading attribute range %x, %x\n",
1107 			attr_offset, attr_len);
1108 
1109 		msb->setup_transfer(card, attr_offset, attr_len);
1110 		memstick_new_req(card->host);
1111 		wait_for_completion(&card->mrq_complete);
1112 		if (card->current_mrq.error) {
1113 			rc = card->current_mrq.error;
1114 			goto out_free_buffer;
1115 		}
1116 
1117 		memcpy(s_attr->data, buffer + addr % msb->page_size,
1118 		       s_attr->size);
1119 	}
1120 
1121 	rc = 0;
1122 out_free_buffer:
1123 	kfree(buffer);
1124 out_free_attr:
1125 	kfree(attr);
1126 	return rc;
1127 }
1128 
1129 static int mspro_block_init_card(struct memstick_dev *card)
1130 {
1131 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1132 	struct memstick_host *host = card->host;
1133 	int rc = 0;
1134 
1135 	msb->system = MEMSTICK_SYS_SERIAL;
1136 	msb->setup_transfer = h_mspro_block_setup_cmd;
1137 
1138 	card->reg_addr.r_offset = offsetof(struct mspro_register, status);
1139 	card->reg_addr.r_length = sizeof(struct ms_status_register);
1140 	card->reg_addr.w_offset = offsetof(struct mspro_register, param);
1141 	card->reg_addr.w_length = sizeof(struct mspro_param_register);
1142 
1143 	if (memstick_set_rw_addr(card))
1144 		return -EIO;
1145 
1146 	msb->caps = host->caps;
1147 
1148 	msleep(150);
1149 	rc = mspro_block_wait_for_ced(card);
1150 	if (rc)
1151 		return rc;
1152 
1153 	rc = mspro_block_switch_interface(card);
1154 	if (rc)
1155 		return rc;
1156 
1157 	dev_dbg(&card->dev, "card activated\n");
1158 	if (msb->system != MEMSTICK_SYS_SERIAL)
1159 		msb->caps |= MEMSTICK_CAP_AUTO_GET_INT;
1160 
1161 	card->next_request = h_mspro_block_req_init;
1162 	msb->mrq_handler = h_mspro_block_get_ro;
1163 	memstick_init_req(&card->current_mrq, MS_TPC_READ_REG, NULL,
1164 			  sizeof(struct ms_status_register));
1165 	memstick_new_req(card->host);
1166 	wait_for_completion(&card->mrq_complete);
1167 	if (card->current_mrq.error)
1168 		return card->current_mrq.error;
1169 
1170 	dev_dbg(&card->dev, "card r/w status %d\n", msb->read_only ? 0 : 1);
1171 
1172 	msb->page_size = 512;
1173 	rc = mspro_block_read_attributes(card);
1174 	if (rc)
1175 		return rc;
1176 
1177 	dev_dbg(&card->dev, "attributes loaded\n");
1178 	return 0;
1179 
1180 }
1181 
1182 static int mspro_block_init_disk(struct memstick_dev *card)
1183 {
1184 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1185 	struct memstick_host *host = card->host;
1186 	struct mspro_devinfo *dev_info = NULL;
1187 	struct mspro_sys_info *sys_info = NULL;
1188 	struct mspro_sys_attr *s_attr = NULL;
1189 	int rc, disk_id;
1190 	u64 limit = BLK_BOUNCE_HIGH;
1191 	unsigned long capacity;
1192 
1193 	if (host->dev.dma_mask && *(host->dev.dma_mask))
1194 		limit = *(host->dev.dma_mask);
1195 
1196 	for (rc = 0; msb->attr_group.attrs[rc]; ++rc) {
1197 		s_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[rc]);
1198 
1199 		if (s_attr->id == MSPRO_BLOCK_ID_DEVINFO)
1200 			dev_info = s_attr->data;
1201 		else if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO)
1202 			sys_info = s_attr->data;
1203 	}
1204 
1205 	if (!dev_info || !sys_info)
1206 		return -ENODEV;
1207 
1208 	msb->cylinders = be16_to_cpu(dev_info->cylinders);
1209 	msb->heads = be16_to_cpu(dev_info->heads);
1210 	msb->sectors_per_track = be16_to_cpu(dev_info->sectors_per_track);
1211 
1212 	msb->page_size = be16_to_cpu(sys_info->unit_size);
1213 
1214 	mutex_lock(&mspro_block_disk_lock);
1215 	disk_id = idr_alloc(&mspro_block_disk_idr, card, 0, 256, GFP_KERNEL);
1216 	mutex_unlock(&mspro_block_disk_lock);
1217 	if (disk_id < 0)
1218 		return disk_id;
1219 
1220 	msb->disk = alloc_disk(1 << MSPRO_BLOCK_PART_SHIFT);
1221 	if (!msb->disk) {
1222 		rc = -ENOMEM;
1223 		goto out_release_id;
1224 	}
1225 
1226 	msb->queue = blk_init_queue(mspro_block_submit_req, &msb->q_lock);
1227 	if (!msb->queue) {
1228 		rc = -ENOMEM;
1229 		goto out_put_disk;
1230 	}
1231 
1232 	msb->queue->queuedata = card;
1233 	blk_queue_prep_rq(msb->queue, mspro_block_prepare_req);
1234 
1235 	blk_queue_bounce_limit(msb->queue, limit);
1236 	blk_queue_max_hw_sectors(msb->queue, MSPRO_BLOCK_MAX_PAGES);
1237 	blk_queue_max_segments(msb->queue, MSPRO_BLOCK_MAX_SEGS);
1238 	blk_queue_max_segment_size(msb->queue,
1239 				   MSPRO_BLOCK_MAX_PAGES * msb->page_size);
1240 
1241 	msb->disk->major = major;
1242 	msb->disk->first_minor = disk_id << MSPRO_BLOCK_PART_SHIFT;
1243 	msb->disk->fops = &ms_block_bdops;
1244 	msb->usage_count = 1;
1245 	msb->disk->private_data = msb;
1246 	msb->disk->queue = msb->queue;
1247 	msb->disk->driverfs_dev = &card->dev;
1248 
1249 	sprintf(msb->disk->disk_name, "mspblk%d", disk_id);
1250 
1251 	blk_queue_logical_block_size(msb->queue, msb->page_size);
1252 
1253 	capacity = be16_to_cpu(sys_info->user_block_count);
1254 	capacity *= be16_to_cpu(sys_info->block_size);
1255 	capacity *= msb->page_size >> 9;
1256 	set_capacity(msb->disk, capacity);
1257 	dev_dbg(&card->dev, "capacity set %ld\n", capacity);
1258 
1259 	add_disk(msb->disk);
1260 	msb->active = 1;
1261 	return 0;
1262 
1263 out_put_disk:
1264 	put_disk(msb->disk);
1265 out_release_id:
1266 	mutex_lock(&mspro_block_disk_lock);
1267 	idr_remove(&mspro_block_disk_idr, disk_id);
1268 	mutex_unlock(&mspro_block_disk_lock);
1269 	return rc;
1270 }
1271 
1272 static void mspro_block_data_clear(struct mspro_block_data *msb)
1273 {
1274 	int cnt;
1275 	struct mspro_sys_attr *s_attr;
1276 
1277 	if (msb->attr_group.attrs) {
1278 		for (cnt = 0; msb->attr_group.attrs[cnt]; ++cnt) {
1279 			s_attr = mspro_from_sysfs_attr(msb->attr_group
1280 							   .attrs[cnt]);
1281 			kfree(s_attr->data);
1282 			kfree(s_attr);
1283 		}
1284 		kfree(msb->attr_group.attrs);
1285 	}
1286 
1287 	msb->card = NULL;
1288 }
1289 
1290 static int mspro_block_check_card(struct memstick_dev *card)
1291 {
1292 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1293 
1294 	return (msb->active == 1);
1295 }
1296 
1297 static int mspro_block_probe(struct memstick_dev *card)
1298 {
1299 	struct mspro_block_data *msb;
1300 	int rc = 0;
1301 
1302 	msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
1303 	if (!msb)
1304 		return -ENOMEM;
1305 	memstick_set_drvdata(card, msb);
1306 	msb->card = card;
1307 	spin_lock_init(&msb->q_lock);
1308 
1309 	rc = mspro_block_init_card(card);
1310 
1311 	if (rc)
1312 		goto out_free;
1313 
1314 	rc = sysfs_create_group(&card->dev.kobj, &msb->attr_group);
1315 	if (rc)
1316 		goto out_free;
1317 
1318 	rc = mspro_block_init_disk(card);
1319 	if (!rc) {
1320 		card->check = mspro_block_check_card;
1321 		card->stop = mspro_block_stop;
1322 		card->start = mspro_block_start;
1323 		return 0;
1324 	}
1325 
1326 	sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
1327 out_free:
1328 	memstick_set_drvdata(card, NULL);
1329 	mspro_block_data_clear(msb);
1330 	kfree(msb);
1331 	return rc;
1332 }
1333 
1334 static void mspro_block_remove(struct memstick_dev *card)
1335 {
1336 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1337 	unsigned long flags;
1338 
1339 	spin_lock_irqsave(&msb->q_lock, flags);
1340 	msb->eject = 1;
1341 	blk_start_queue(msb->queue);
1342 	spin_unlock_irqrestore(&msb->q_lock, flags);
1343 
1344 	del_gendisk(msb->disk);
1345 	dev_dbg(&card->dev, "mspro block remove\n");
1346 
1347 	blk_cleanup_queue(msb->queue);
1348 	msb->queue = NULL;
1349 
1350 	sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
1351 
1352 	mutex_lock(&mspro_block_disk_lock);
1353 	mspro_block_data_clear(msb);
1354 	mutex_unlock(&mspro_block_disk_lock);
1355 
1356 	mspro_block_disk_release(msb->disk);
1357 	memstick_set_drvdata(card, NULL);
1358 }
1359 
1360 #ifdef CONFIG_PM
1361 
1362 static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state)
1363 {
1364 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1365 	unsigned long flags;
1366 
1367 	spin_lock_irqsave(&msb->q_lock, flags);
1368 	blk_stop_queue(msb->queue);
1369 	msb->active = 0;
1370 	spin_unlock_irqrestore(&msb->q_lock, flags);
1371 
1372 	return 0;
1373 }
1374 
1375 static int mspro_block_resume(struct memstick_dev *card)
1376 {
1377 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1378 	unsigned long flags;
1379 	int rc = 0;
1380 
1381 #ifdef CONFIG_MEMSTICK_UNSAFE_RESUME
1382 
1383 	struct mspro_block_data *new_msb;
1384 	struct memstick_host *host = card->host;
1385 	struct mspro_sys_attr *s_attr, *r_attr;
1386 	unsigned char cnt;
1387 
1388 	mutex_lock(&host->lock);
1389 	new_msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
1390 	if (!new_msb) {
1391 		rc = -ENOMEM;
1392 		goto out_unlock;
1393 	}
1394 
1395 	new_msb->card = card;
1396 	memstick_set_drvdata(card, new_msb);
1397 	if (mspro_block_init_card(card))
1398 		goto out_free;
1399 
1400 	for (cnt = 0; new_msb->attr_group.attrs[cnt]
1401 		      && msb->attr_group.attrs[cnt]; ++cnt) {
1402 		s_attr = mspro_from_sysfs_attr(new_msb->attr_group.attrs[cnt]);
1403 		r_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[cnt]);
1404 
1405 		if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO
1406 		    && r_attr->id == s_attr->id) {
1407 			if (memcmp(s_attr->data, r_attr->data, s_attr->size))
1408 				break;
1409 
1410 			msb->active = 1;
1411 			break;
1412 		}
1413 	}
1414 
1415 out_free:
1416 	memstick_set_drvdata(card, msb);
1417 	mspro_block_data_clear(new_msb);
1418 	kfree(new_msb);
1419 out_unlock:
1420 	mutex_unlock(&host->lock);
1421 
1422 #endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */
1423 
1424 	spin_lock_irqsave(&msb->q_lock, flags);
1425 	blk_start_queue(msb->queue);
1426 	spin_unlock_irqrestore(&msb->q_lock, flags);
1427 	return rc;
1428 }
1429 
1430 #else
1431 
1432 #define mspro_block_suspend NULL
1433 #define mspro_block_resume NULL
1434 
1435 #endif /* CONFIG_PM */
1436 
1437 static struct memstick_device_id mspro_block_id_tbl[] = {
1438 	{MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_PRO, MEMSTICK_CATEGORY_STORAGE_DUO,
1439 	 MEMSTICK_CLASS_DUO},
1440 	{}
1441 };
1442 
1443 
1444 static struct memstick_driver mspro_block_driver = {
1445 	.driver = {
1446 		.name  = DRIVER_NAME,
1447 		.owner = THIS_MODULE
1448 	},
1449 	.id_table = mspro_block_id_tbl,
1450 	.probe    = mspro_block_probe,
1451 	.remove   = mspro_block_remove,
1452 	.suspend  = mspro_block_suspend,
1453 	.resume   = mspro_block_resume
1454 };
1455 
1456 static int __init mspro_block_init(void)
1457 {
1458 	int rc = -ENOMEM;
1459 
1460 	rc = register_blkdev(major, DRIVER_NAME);
1461 	if (rc < 0) {
1462 		printk(KERN_ERR DRIVER_NAME ": failed to register "
1463 		       "major %d, error %d\n", major, rc);
1464 		return rc;
1465 	}
1466 	if (!major)
1467 		major = rc;
1468 
1469 	rc = memstick_register_driver(&mspro_block_driver);
1470 	if (rc)
1471 		unregister_blkdev(major, DRIVER_NAME);
1472 	return rc;
1473 }
1474 
1475 static void __exit mspro_block_exit(void)
1476 {
1477 	memstick_unregister_driver(&mspro_block_driver);
1478 	unregister_blkdev(major, DRIVER_NAME);
1479 	idr_destroy(&mspro_block_disk_idr);
1480 }
1481 
1482 module_init(mspro_block_init);
1483 module_exit(mspro_block_exit);
1484 
1485 MODULE_LICENSE("GPL");
1486 MODULE_AUTHOR("Alex Dubov");
1487 MODULE_DESCRIPTION("Sony MemoryStickPro block device driver");
1488 MODULE_DEVICE_TABLE(memstick, mspro_block_id_tbl);
1489