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