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