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