drivers/memstick/core/mspro_block.c

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