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