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