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