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