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, msb->req_sg); 631 632 if (!msb->seg_count) { 633 unsigned int bytes = blk_rq_cur_bytes(msb->block_req); 634 bool chunk; 635 636 chunk = blk_update_request(msb->block_req, 637 BLK_STS_RESOURCE, 638 bytes); 639 if (chunk) 640 continue; 641 __blk_mq_end_request(msb->block_req, 642 BLK_STS_RESOURCE); 643 msb->block_req = NULL; 644 return -EAGAIN; 645 } 646 647 t_off = blk_rq_pos(msb->block_req); 648 t_off <<= 9; 649 count = blk_rq_bytes(msb->block_req); 650 651 msb->setup_transfer(card, t_off, count); 652 653 msb->data_dir = rq_data_dir(msb->block_req); 654 msb->transfer_cmd = msb->data_dir == READ 655 ? MSPRO_CMD_READ_DATA 656 : MSPRO_CMD_WRITE_DATA; 657 658 memstick_new_req(card->host); 659 return 0; 660 } 661 } 662 663 static int mspro_block_complete_req(struct memstick_dev *card, int error) 664 { 665 struct mspro_block_data *msb = memstick_get_drvdata(card); 666 int cnt; 667 bool chunk; 668 unsigned int t_len = 0; 669 unsigned long flags; 670 671 spin_lock_irqsave(&msb->q_lock, flags); 672 dev_dbg(&card->dev, "complete %d, %d\n", msb->block_req ? 1 : 0, 673 error); 674 675 if (msb->block_req) { 676 /* Nothing to do - not really an error */ 677 if (error == -EAGAIN) 678 error = 0; 679 680 if (error || (card->current_mrq.tpc == MSPRO_CMD_STOP)) { 681 if (msb->data_dir == READ) { 682 for (cnt = 0; cnt < msb->current_seg; cnt++) { 683 t_len += msb->req_sg[cnt].length 684 / msb->page_size; 685 686 if (msb->current_page) 687 t_len += msb->current_page - 1; 688 689 t_len *= msb->page_size; 690 } 691 } 692 } else 693 t_len = blk_rq_bytes(msb->block_req); 694 695 dev_dbg(&card->dev, "transferred %x (%d)\n", t_len, error); 696 697 if (error && !t_len) 698 t_len = blk_rq_cur_bytes(msb->block_req); 699 700 chunk = blk_update_request(msb->block_req, 701 errno_to_blk_status(error), t_len); 702 if (chunk) { 703 error = mspro_block_issue_req(card); 704 if (!error) 705 goto out; 706 } else { 707 __blk_mq_end_request(msb->block_req, 708 errno_to_blk_status(error)); 709 msb->block_req = NULL; 710 } 711 } else { 712 if (!error) 713 error = -EAGAIN; 714 } 715 716 card->next_request = h_mspro_block_default_bad; 717 complete_all(&card->mrq_complete); 718 out: 719 spin_unlock_irqrestore(&msb->q_lock, flags); 720 return error; 721 } 722 723 static void mspro_block_stop(struct memstick_dev *card) 724 { 725 struct mspro_block_data *msb = memstick_get_drvdata(card); 726 int rc = 0; 727 unsigned long flags; 728 729 while (1) { 730 spin_lock_irqsave(&msb->q_lock, flags); 731 if (!msb->block_req) { 732 blk_mq_stop_hw_queues(msb->queue); 733 rc = 1; 734 } 735 spin_unlock_irqrestore(&msb->q_lock, flags); 736 737 if (rc) 738 break; 739 740 wait_for_completion(&card->mrq_complete); 741 } 742 } 743 744 static void mspro_block_start(struct memstick_dev *card) 745 { 746 struct mspro_block_data *msb = memstick_get_drvdata(card); 747 748 blk_mq_start_hw_queues(msb->queue); 749 } 750 751 static blk_status_t mspro_queue_rq(struct blk_mq_hw_ctx *hctx, 752 const struct blk_mq_queue_data *bd) 753 { 754 struct memstick_dev *card = hctx->queue->queuedata; 755 struct mspro_block_data *msb = memstick_get_drvdata(card); 756 757 spin_lock_irq(&msb->q_lock); 758 759 if (msb->block_req) { 760 spin_unlock_irq(&msb->q_lock); 761 return BLK_STS_DEV_RESOURCE; 762 } 763 764 if (msb->eject) { 765 spin_unlock_irq(&msb->q_lock); 766 blk_mq_start_request(bd->rq); 767 return BLK_STS_IOERR; 768 } 769 770 msb->block_req = bd->rq; 771 blk_mq_start_request(bd->rq); 772 773 if (mspro_block_issue_req(card)) 774 msb->block_req = NULL; 775 776 spin_unlock_irq(&msb->q_lock); 777 return BLK_STS_OK; 778 } 779 780 /*** Initialization ***/ 781 782 static int mspro_block_wait_for_ced(struct memstick_dev *card) 783 { 784 struct mspro_block_data *msb = memstick_get_drvdata(card); 785 786 card->next_request = h_mspro_block_req_init; 787 msb->mrq_handler = h_mspro_block_wait_for_ced; 788 memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1); 789 memstick_new_req(card->host); 790 wait_for_completion(&card->mrq_complete); 791 return card->current_mrq.error; 792 } 793 794 static int mspro_block_set_interface(struct memstick_dev *card, 795 unsigned char sys_reg) 796 { 797 struct memstick_host *host = card->host; 798 struct mspro_block_data *msb = memstick_get_drvdata(card); 799 struct mspro_param_register param = { 800 .system = sys_reg, 801 .data_count = 0, 802 .data_address = 0, 803 .tpc_param = 0 804 }; 805 806 card->next_request = h_mspro_block_req_init; 807 msb->mrq_handler = h_mspro_block_default; 808 memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, ¶m, 809 sizeof(param)); 810 memstick_new_req(host); 811 wait_for_completion(&card->mrq_complete); 812 return card->current_mrq.error; 813 } 814 815 static int mspro_block_switch_interface(struct memstick_dev *card) 816 { 817 struct memstick_host *host = card->host; 818 struct mspro_block_data *msb = memstick_get_drvdata(card); 819 int rc = 0; 820 821 try_again: 822 if (msb->caps & MEMSTICK_CAP_PAR4) 823 rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR4); 824 else 825 return 0; 826 827 if (rc) { 828 printk(KERN_WARNING 829 "%s: could not switch to 4-bit mode, error %d\n", 830 dev_name(&card->dev), rc); 831 return 0; 832 } 833 834 msb->system = MEMSTICK_SYS_PAR4; 835 host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4); 836 printk(KERN_INFO "%s: switching to 4-bit parallel mode\n", 837 dev_name(&card->dev)); 838 839 if (msb->caps & MEMSTICK_CAP_PAR8) { 840 rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8); 841 842 if (!rc) { 843 msb->system = MEMSTICK_SYS_PAR8; 844 host->set_param(host, MEMSTICK_INTERFACE, 845 MEMSTICK_PAR8); 846 printk(KERN_INFO 847 "%s: switching to 8-bit parallel mode\n", 848 dev_name(&card->dev)); 849 } else 850 printk(KERN_WARNING 851 "%s: could not switch to 8-bit mode, error %d\n", 852 dev_name(&card->dev), rc); 853 } 854 855 card->next_request = h_mspro_block_req_init; 856 msb->mrq_handler = h_mspro_block_default; 857 memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1); 858 memstick_new_req(card->host); 859 wait_for_completion(&card->mrq_complete); 860 rc = card->current_mrq.error; 861 862 if (rc) { 863 printk(KERN_WARNING 864 "%s: interface error, trying to fall back to serial\n", 865 dev_name(&card->dev)); 866 msb->system = MEMSTICK_SYS_SERIAL; 867 host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF); 868 msleep(10); 869 host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON); 870 host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL); 871 872 rc = memstick_set_rw_addr(card); 873 if (!rc) 874 rc = mspro_block_set_interface(card, msb->system); 875 876 if (!rc) { 877 msleep(150); 878 rc = mspro_block_wait_for_ced(card); 879 if (rc) 880 return rc; 881 882 if (msb->caps & MEMSTICK_CAP_PAR8) { 883 msb->caps &= ~MEMSTICK_CAP_PAR8; 884 goto try_again; 885 } 886 } 887 } 888 return rc; 889 } 890 891 /* Memory allocated for attributes by this function should be freed by 892 * mspro_block_data_clear, no matter if the initialization process succeeded 893 * or failed. 894 */ 895 static int mspro_block_read_attributes(struct memstick_dev *card) 896 { 897 struct mspro_block_data *msb = memstick_get_drvdata(card); 898 struct mspro_attribute *attr = NULL; 899 struct mspro_sys_attr *s_attr = NULL; 900 unsigned char *buffer = NULL; 901 int cnt, rc, attr_count; 902 /* While normally physical device offsets, represented here by 903 * attr_offset and attr_len will be of large numeric types, we can be 904 * sure, that attributes are close enough to the beginning of the 905 * device, to save ourselves some trouble. 906 */ 907 unsigned int addr, attr_offset = 0, attr_len = msb->page_size; 908 909 attr = kmalloc(msb->page_size, GFP_KERNEL); 910 if (!attr) 911 return -ENOMEM; 912 913 sg_init_one(&msb->req_sg[0], attr, msb->page_size); 914 msb->seg_count = 1; 915 msb->current_seg = 0; 916 msb->current_page = 0; 917 msb->data_dir = READ; 918 msb->transfer_cmd = MSPRO_CMD_READ_ATRB; 919 920 msb->setup_transfer(card, attr_offset, attr_len); 921 922 memstick_new_req(card->host); 923 wait_for_completion(&card->mrq_complete); 924 if (card->current_mrq.error) { 925 rc = card->current_mrq.error; 926 goto out_free_attr; 927 } 928 929 if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) { 930 printk(KERN_ERR "%s: unrecognized device signature %x\n", 931 dev_name(&card->dev), be16_to_cpu(attr->signature)); 932 rc = -ENODEV; 933 goto out_free_attr; 934 } 935 936 if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) { 937 printk(KERN_WARNING "%s: way too many attribute entries\n", 938 dev_name(&card->dev)); 939 attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES; 940 } else 941 attr_count = attr->count; 942 943 msb->attr_group.attrs = kcalloc(attr_count + 1, 944 sizeof(*msb->attr_group.attrs), 945 GFP_KERNEL); 946 if (!msb->attr_group.attrs) { 947 rc = -ENOMEM; 948 goto out_free_attr; 949 } 950 msb->attr_group.name = "media_attributes"; 951 952 buffer = kmemdup(attr, attr_len, GFP_KERNEL); 953 if (!buffer) { 954 rc = -ENOMEM; 955 goto out_free_attr; 956 } 957 958 for (cnt = 0; cnt < attr_count; ++cnt) { 959 s_attr = kzalloc(sizeof(struct mspro_sys_attr), GFP_KERNEL); 960 if (!s_attr) { 961 rc = -ENOMEM; 962 goto out_free_buffer; 963 } 964 965 msb->attr_group.attrs[cnt] = &s_attr->dev_attr.attr; 966 addr = be32_to_cpu(attr->entries[cnt].address); 967 s_attr->size = be32_to_cpu(attr->entries[cnt].size); 968 dev_dbg(&card->dev, "adding attribute %d: id %x, address %x, " 969 "size %zx\n", cnt, attr->entries[cnt].id, addr, 970 s_attr->size); 971 s_attr->id = attr->entries[cnt].id; 972 if (mspro_block_attr_name(s_attr->id)) 973 snprintf(s_attr->name, sizeof(s_attr->name), "%s", 974 mspro_block_attr_name(attr->entries[cnt].id)); 975 else 976 snprintf(s_attr->name, sizeof(s_attr->name), 977 "attr_x%02x", attr->entries[cnt].id); 978 979 sysfs_attr_init(&s_attr->dev_attr.attr); 980 s_attr->dev_attr.attr.name = s_attr->name; 981 s_attr->dev_attr.attr.mode = S_IRUGO; 982 s_attr->dev_attr.show = mspro_block_attr_show(s_attr->id); 983 984 if (!s_attr->size) 985 continue; 986 987 s_attr->data = kmalloc(s_attr->size, GFP_KERNEL); 988 if (!s_attr->data) { 989 rc = -ENOMEM; 990 goto out_free_buffer; 991 } 992 993 if (((addr / msb->page_size) == (attr_offset / msb->page_size)) 994 && (((addr + s_attr->size - 1) / msb->page_size) 995 == (attr_offset / msb->page_size))) { 996 memcpy(s_attr->data, buffer + addr % msb->page_size, 997 s_attr->size); 998 continue; 999 } 1000 1001 attr_offset = (addr / msb->page_size) * msb->page_size; 1002 1003 if ((attr_offset + attr_len) < (addr + s_attr->size)) { 1004 kfree(buffer); 1005 attr_len = (((addr + s_attr->size) / msb->page_size) 1006 + 1 ) * msb->page_size - attr_offset; 1007 buffer = kmalloc(attr_len, GFP_KERNEL); 1008 if (!buffer) { 1009 rc = -ENOMEM; 1010 goto out_free_attr; 1011 } 1012 } 1013 1014 sg_init_one(&msb->req_sg[0], buffer, attr_len); 1015 msb->seg_count = 1; 1016 msb->current_seg = 0; 1017 msb->current_page = 0; 1018 msb->data_dir = READ; 1019 msb->transfer_cmd = MSPRO_CMD_READ_ATRB; 1020 1021 dev_dbg(&card->dev, "reading attribute range %x, %x\n", 1022 attr_offset, attr_len); 1023 1024 msb->setup_transfer(card, attr_offset, attr_len); 1025 memstick_new_req(card->host); 1026 wait_for_completion(&card->mrq_complete); 1027 if (card->current_mrq.error) { 1028 rc = card->current_mrq.error; 1029 goto out_free_buffer; 1030 } 1031 1032 memcpy(s_attr->data, buffer + addr % msb->page_size, 1033 s_attr->size); 1034 } 1035 1036 rc = 0; 1037 out_free_buffer: 1038 kfree(buffer); 1039 out_free_attr: 1040 kfree(attr); 1041 return rc; 1042 } 1043 1044 static int mspro_block_init_card(struct memstick_dev *card) 1045 { 1046 struct mspro_block_data *msb = memstick_get_drvdata(card); 1047 struct memstick_host *host = card->host; 1048 int rc = 0; 1049 1050 msb->system = MEMSTICK_SYS_SERIAL; 1051 msb->setup_transfer = h_mspro_block_setup_cmd; 1052 1053 card->reg_addr.r_offset = offsetof(struct mspro_register, status); 1054 card->reg_addr.r_length = sizeof(struct ms_status_register); 1055 card->reg_addr.w_offset = offsetof(struct mspro_register, param); 1056 card->reg_addr.w_length = sizeof(struct mspro_param_register); 1057 1058 if (memstick_set_rw_addr(card)) 1059 return -EIO; 1060 1061 msb->caps = host->caps; 1062 1063 msleep(150); 1064 rc = mspro_block_wait_for_ced(card); 1065 if (rc) 1066 return rc; 1067 1068 rc = mspro_block_switch_interface(card); 1069 if (rc) 1070 return rc; 1071 1072 dev_dbg(&card->dev, "card activated\n"); 1073 if (msb->system != MEMSTICK_SYS_SERIAL) 1074 msb->caps |= MEMSTICK_CAP_AUTO_GET_INT; 1075 1076 card->next_request = h_mspro_block_req_init; 1077 msb->mrq_handler = h_mspro_block_get_ro; 1078 memstick_init_req(&card->current_mrq, MS_TPC_READ_REG, NULL, 1079 sizeof(struct ms_status_register)); 1080 memstick_new_req(card->host); 1081 wait_for_completion(&card->mrq_complete); 1082 if (card->current_mrq.error) 1083 return card->current_mrq.error; 1084 1085 dev_dbg(&card->dev, "card r/w status %d\n", msb->read_only ? 0 : 1); 1086 1087 msb->page_size = 512; 1088 rc = mspro_block_read_attributes(card); 1089 if (rc) 1090 return rc; 1091 1092 dev_dbg(&card->dev, "attributes loaded\n"); 1093 return 0; 1094 1095 } 1096 1097 static const struct blk_mq_ops mspro_mq_ops = { 1098 .queue_rq = mspro_queue_rq, 1099 }; 1100 1101 static int mspro_block_init_disk(struct memstick_dev *card) 1102 { 1103 struct mspro_block_data *msb = memstick_get_drvdata(card); 1104 struct queue_limits lim = { 1105 .logical_block_size = msb->page_size, 1106 .max_hw_sectors = MSPRO_BLOCK_MAX_PAGES, 1107 .max_segments = MSPRO_BLOCK_MAX_SEGS, 1108 .max_segment_size = MSPRO_BLOCK_MAX_PAGES * msb->page_size, 1109 }; 1110 struct mspro_devinfo *dev_info = NULL; 1111 struct mspro_sys_info *sys_info = NULL; 1112 struct mspro_sys_attr *s_attr = NULL; 1113 int rc, disk_id; 1114 unsigned long capacity; 1115 1116 for (rc = 0; msb->attr_group.attrs[rc]; ++rc) { 1117 s_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[rc]); 1118 1119 if (s_attr->id == MSPRO_BLOCK_ID_DEVINFO) 1120 dev_info = s_attr->data; 1121 else if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO) 1122 sys_info = s_attr->data; 1123 } 1124 1125 if (!dev_info || !sys_info) 1126 return -ENODEV; 1127 1128 msb->cylinders = be16_to_cpu(dev_info->cylinders); 1129 msb->heads = be16_to_cpu(dev_info->heads); 1130 msb->sectors_per_track = be16_to_cpu(dev_info->sectors_per_track); 1131 1132 msb->page_size = be16_to_cpu(sys_info->unit_size); 1133 1134 mutex_lock(&mspro_block_disk_lock); 1135 disk_id = idr_alloc(&mspro_block_disk_idr, card, 0, 256, GFP_KERNEL); 1136 mutex_unlock(&mspro_block_disk_lock); 1137 if (disk_id < 0) 1138 return disk_id; 1139 1140 rc = blk_mq_alloc_sq_tag_set(&msb->tag_set, &mspro_mq_ops, 2, 0); 1141 if (rc) 1142 goto out_release_id; 1143 1144 msb->disk = blk_mq_alloc_disk(&msb->tag_set, &lim, card); 1145 if (IS_ERR(msb->disk)) { 1146 rc = PTR_ERR(msb->disk); 1147 goto out_free_tag_set; 1148 } 1149 msb->queue = msb->disk->queue; 1150 1151 msb->disk->major = major; 1152 msb->disk->first_minor = disk_id << MSPRO_BLOCK_PART_SHIFT; 1153 msb->disk->minors = 1 << MSPRO_BLOCK_PART_SHIFT; 1154 msb->disk->fops = &ms_block_bdops; 1155 msb->disk->private_data = msb; 1156 1157 sprintf(msb->disk->disk_name, "mspblk%d", disk_id); 1158 1159 capacity = be16_to_cpu(sys_info->user_block_count); 1160 capacity *= be16_to_cpu(sys_info->block_size); 1161 capacity *= msb->page_size >> 9; 1162 set_capacity(msb->disk, capacity); 1163 dev_dbg(&card->dev, "capacity set %ld\n", capacity); 1164 1165 if (msb->read_only) 1166 set_disk_ro(msb->disk, true); 1167 1168 rc = device_add_disk(&card->dev, msb->disk, NULL); 1169 if (rc) 1170 goto out_cleanup_disk; 1171 msb->active = 1; 1172 return 0; 1173 1174 out_cleanup_disk: 1175 put_disk(msb->disk); 1176 out_free_tag_set: 1177 blk_mq_free_tag_set(&msb->tag_set); 1178 out_release_id: 1179 mutex_lock(&mspro_block_disk_lock); 1180 idr_remove(&mspro_block_disk_idr, disk_id); 1181 mutex_unlock(&mspro_block_disk_lock); 1182 return rc; 1183 } 1184 1185 static void mspro_block_data_clear(struct mspro_block_data *msb) 1186 { 1187 int cnt; 1188 struct mspro_sys_attr *s_attr; 1189 1190 if (msb->attr_group.attrs) { 1191 for (cnt = 0; msb->attr_group.attrs[cnt]; ++cnt) { 1192 s_attr = mspro_from_sysfs_attr(msb->attr_group 1193 .attrs[cnt]); 1194 kfree(s_attr->data); 1195 kfree(s_attr); 1196 } 1197 kfree(msb->attr_group.attrs); 1198 } 1199 1200 msb->card = NULL; 1201 } 1202 1203 static int mspro_block_check_card(struct memstick_dev *card) 1204 { 1205 struct mspro_block_data *msb = memstick_get_drvdata(card); 1206 1207 return (msb->active == 1); 1208 } 1209 1210 static int mspro_block_probe(struct memstick_dev *card) 1211 { 1212 struct mspro_block_data *msb; 1213 int rc = 0; 1214 1215 msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL); 1216 if (!msb) 1217 return -ENOMEM; 1218 memstick_set_drvdata(card, msb); 1219 msb->card = card; 1220 spin_lock_init(&msb->q_lock); 1221 1222 rc = mspro_block_init_card(card); 1223 1224 if (rc) 1225 goto out_free; 1226 1227 rc = sysfs_create_group(&card->dev.kobj, &msb->attr_group); 1228 if (rc) 1229 goto out_free; 1230 1231 rc = mspro_block_init_disk(card); 1232 if (!rc) { 1233 card->check = mspro_block_check_card; 1234 card->stop = mspro_block_stop; 1235 card->start = mspro_block_start; 1236 return 0; 1237 } 1238 1239 sysfs_remove_group(&card->dev.kobj, &msb->attr_group); 1240 out_free: 1241 memstick_set_drvdata(card, NULL); 1242 mspro_block_data_clear(msb); 1243 kfree(msb); 1244 return rc; 1245 } 1246 1247 static void mspro_block_remove(struct memstick_dev *card) 1248 { 1249 struct mspro_block_data *msb = memstick_get_drvdata(card); 1250 unsigned long flags; 1251 1252 spin_lock_irqsave(&msb->q_lock, flags); 1253 msb->eject = 1; 1254 spin_unlock_irqrestore(&msb->q_lock, flags); 1255 blk_mq_start_hw_queues(msb->queue); 1256 1257 del_gendisk(msb->disk); 1258 dev_dbg(&card->dev, "mspro block remove\n"); 1259 1260 blk_mq_free_tag_set(&msb->tag_set); 1261 msb->queue = NULL; 1262 1263 sysfs_remove_group(&card->dev.kobj, &msb->attr_group); 1264 1265 mutex_lock(&mspro_block_disk_lock); 1266 mspro_block_data_clear(msb); 1267 mutex_unlock(&mspro_block_disk_lock); 1268 1269 put_disk(msb->disk); 1270 memstick_set_drvdata(card, NULL); 1271 } 1272 1273 #ifdef CONFIG_PM 1274 1275 static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state) 1276 { 1277 struct mspro_block_data *msb = memstick_get_drvdata(card); 1278 unsigned long flags; 1279 1280 blk_mq_stop_hw_queues(msb->queue); 1281 1282 spin_lock_irqsave(&msb->q_lock, flags); 1283 msb->active = 0; 1284 spin_unlock_irqrestore(&msb->q_lock, flags); 1285 1286 return 0; 1287 } 1288 1289 static int mspro_block_resume(struct memstick_dev *card) 1290 { 1291 struct mspro_block_data *msb = memstick_get_drvdata(card); 1292 int rc = 0; 1293 1294 #ifdef CONFIG_MEMSTICK_UNSAFE_RESUME 1295 1296 struct mspro_block_data *new_msb; 1297 struct memstick_host *host = card->host; 1298 struct mspro_sys_attr *s_attr, *r_attr; 1299 unsigned char cnt; 1300 1301 mutex_lock(&host->lock); 1302 new_msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL); 1303 if (!new_msb) { 1304 rc = -ENOMEM; 1305 goto out_unlock; 1306 } 1307 1308 new_msb->card = card; 1309 memstick_set_drvdata(card, new_msb); 1310 rc = mspro_block_init_card(card); 1311 if (rc) 1312 goto out_free; 1313 1314 for (cnt = 0; new_msb->attr_group.attrs[cnt] 1315 && msb->attr_group.attrs[cnt]; ++cnt) { 1316 s_attr = mspro_from_sysfs_attr(new_msb->attr_group.attrs[cnt]); 1317 r_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[cnt]); 1318 1319 if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO 1320 && r_attr->id == s_attr->id) { 1321 if (memcmp(s_attr->data, r_attr->data, s_attr->size)) 1322 break; 1323 1324 msb->active = 1; 1325 break; 1326 } 1327 } 1328 1329 out_free: 1330 memstick_set_drvdata(card, msb); 1331 mspro_block_data_clear(new_msb); 1332 kfree(new_msb); 1333 out_unlock: 1334 mutex_unlock(&host->lock); 1335 1336 #endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */ 1337 1338 blk_mq_start_hw_queues(msb->queue); 1339 return rc; 1340 } 1341 1342 #else 1343 1344 #define mspro_block_suspend NULL 1345 #define mspro_block_resume NULL 1346 1347 #endif /* CONFIG_PM */ 1348 1349 static const struct memstick_device_id mspro_block_id_tbl[] = { 1350 {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_PRO, MEMSTICK_CATEGORY_STORAGE_DUO, 1351 MEMSTICK_CLASS_DUO}, 1352 {} 1353 }; 1354 1355 1356 static struct memstick_driver mspro_block_driver = { 1357 .driver = { 1358 .name = DRIVER_NAME, 1359 .owner = THIS_MODULE 1360 }, 1361 .id_table = mspro_block_id_tbl, 1362 .probe = mspro_block_probe, 1363 .remove = mspro_block_remove, 1364 .suspend = mspro_block_suspend, 1365 .resume = mspro_block_resume 1366 }; 1367 1368 static int __init mspro_block_init(void) 1369 { 1370 int rc = -ENOMEM; 1371 1372 rc = register_blkdev(major, DRIVER_NAME); 1373 if (rc < 0) { 1374 printk(KERN_ERR DRIVER_NAME ": failed to register " 1375 "major %d, error %d\n", major, rc); 1376 return rc; 1377 } 1378 if (!major) 1379 major = rc; 1380 1381 rc = memstick_register_driver(&mspro_block_driver); 1382 if (rc) 1383 unregister_blkdev(major, DRIVER_NAME); 1384 return rc; 1385 } 1386 1387 static void __exit mspro_block_exit(void) 1388 { 1389 memstick_unregister_driver(&mspro_block_driver); 1390 unregister_blkdev(major, DRIVER_NAME); 1391 idr_destroy(&mspro_block_disk_idr); 1392 } 1393 1394 module_init(mspro_block_init); 1395 module_exit(mspro_block_exit); 1396 1397 MODULE_LICENSE("GPL"); 1398 MODULE_AUTHOR("Alex Dubov"); 1399 MODULE_DESCRIPTION("Sony MemoryStickPro block device driver"); 1400 MODULE_DEVICE_TABLE(memstick, mspro_block_id_tbl); 1401