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