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