drivers/memstick/core/mspro_block.c

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