Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  *  Sony MemoryStick Pro storage support
0004  *
0005  *  Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
0006  *
0007  * Special thanks to Carlos Corbacho for providing various MemoryStick cards
0008  * that made this driver possible.
0009  */
0010 
0011 #include <linux/blk-mq.h>
0012 #include <linux/idr.h>
0013 #include <linux/hdreg.h>
0014 #include <linux/kthread.h>
0015 #include <linux/delay.h>
0016 #include <linux/slab.h>
0017 #include <linux/mutex.h>
0018 #include <linux/memstick.h>
0019 #include <linux/module.h>
0020 
0021 #define DRIVER_NAME "mspro_block"
0022 
0023 static int major;
0024 module_param(major, int, 0644);
0025 
0026 #define MSPRO_BLOCK_MAX_SEGS  32
0027 #define MSPRO_BLOCK_MAX_PAGES ((2 << 16) - 1)
0028 
0029 #define MSPRO_BLOCK_SIGNATURE        0xa5c3
0030 #define MSPRO_BLOCK_MAX_ATTRIBUTES   41
0031 
0032 #define MSPRO_BLOCK_PART_SHIFT 3
0033 
0034 enum {
0035     MSPRO_BLOCK_ID_SYSINFO         = 0x10,
0036     MSPRO_BLOCK_ID_MODELNAME       = 0x15,
0037     MSPRO_BLOCK_ID_MBR             = 0x20,
0038     MSPRO_BLOCK_ID_PBR16           = 0x21,
0039     MSPRO_BLOCK_ID_PBR32           = 0x22,
0040     MSPRO_BLOCK_ID_SPECFILEVALUES1 = 0x25,
0041     MSPRO_BLOCK_ID_SPECFILEVALUES2 = 0x26,
0042     MSPRO_BLOCK_ID_DEVINFO         = 0x30
0043 };
0044 
0045 struct mspro_sys_attr {
0046     size_t                  size;
0047     void                    *data;
0048     unsigned char           id;
0049     char                    name[32];
0050     struct device_attribute dev_attr;
0051 };
0052 
0053 struct mspro_attr_entry {
0054     __be32 address;
0055     __be32 size;
0056     unsigned char id;
0057     unsigned char reserved[3];
0058 } __attribute__((packed));
0059 
0060 struct mspro_attribute {
0061     __be16 signature;
0062     unsigned short          version;
0063     unsigned char           count;
0064     unsigned char           reserved[11];
0065     struct mspro_attr_entry entries[];
0066 } __attribute__((packed));
0067 
0068 struct mspro_sys_info {
0069     unsigned char  class;
0070     unsigned char  reserved0;
0071     __be16 block_size;
0072     __be16 block_count;
0073     __be16 user_block_count;
0074     __be16 page_size;
0075     unsigned char  reserved1[2];
0076     unsigned char  assembly_date[8];
0077     __be32 serial_number;
0078     unsigned char  assembly_maker_code;
0079     unsigned char  assembly_model_code[3];
0080     __be16 memory_maker_code;
0081     __be16 memory_model_code;
0082     unsigned char  reserved2[4];
0083     unsigned char  vcc;
0084     unsigned char  vpp;
0085     __be16 controller_number;
0086     __be16 controller_function;
0087     __be16 start_sector;
0088     __be16 unit_size;
0089     unsigned char  ms_sub_class;
0090     unsigned char  reserved3[4];
0091     unsigned char  interface_type;
0092     __be16 controller_code;
0093     unsigned char  format_type;
0094     unsigned char  reserved4;
0095     unsigned char  device_type;
0096     unsigned char  reserved5[7];
0097     unsigned char  mspro_id[16];
0098     unsigned char  reserved6[16];
0099 } __attribute__((packed));
0100 
0101 struct mspro_mbr {
0102     unsigned char boot_partition;
0103     unsigned char start_head;
0104     unsigned char start_sector;
0105     unsigned char start_cylinder;
0106     unsigned char partition_type;
0107     unsigned char end_head;
0108     unsigned char end_sector;
0109     unsigned char end_cylinder;
0110     unsigned int  start_sectors;
0111     unsigned int  sectors_per_partition;
0112 } __attribute__((packed));
0113 
0114 struct mspro_specfile {
0115     char           name[8];
0116     char           ext[3];
0117     unsigned char  attr;
0118     unsigned char  reserved[10];
0119     unsigned short time;
0120     unsigned short date;
0121     unsigned short cluster;
0122     unsigned int   size;
0123 } __attribute__((packed));
0124 
0125 struct mspro_devinfo {
0126     __be16 cylinders;
0127     __be16 heads;
0128     __be16 bytes_per_track;
0129     __be16 bytes_per_sector;
0130     __be16 sectors_per_track;
0131     unsigned char  reserved[6];
0132 } __attribute__((packed));
0133 
0134 struct mspro_block_data {
0135     struct memstick_dev   *card;
0136     unsigned int          caps;
0137     struct gendisk        *disk;
0138     struct request_queue  *queue;
0139     struct request        *block_req;
0140     struct blk_mq_tag_set tag_set;
0141     spinlock_t            q_lock;
0142 
0143     unsigned short        page_size;
0144     unsigned short        cylinders;
0145     unsigned short        heads;
0146     unsigned short        sectors_per_track;
0147 
0148     unsigned char         system;
0149     unsigned char         read_only:1,
0150                   eject:1,
0151                   data_dir:1,
0152                   active:1;
0153     unsigned char         transfer_cmd;
0154 
0155     int                   (*mrq_handler)(struct memstick_dev *card,
0156                          struct memstick_request **mrq);
0157 
0158 
0159     /* Default request setup function for data access method preferred by
0160      * this host instance.
0161      */
0162     void                  (*setup_transfer)(struct memstick_dev *card,
0163                         u64 offset, size_t length);
0164 
0165     struct attribute_group attr_group;
0166 
0167     struct scatterlist    req_sg[MSPRO_BLOCK_MAX_SEGS];
0168     unsigned int          seg_count;
0169     unsigned int          current_seg;
0170     unsigned int          current_page;
0171 };
0172 
0173 static DEFINE_IDR(mspro_block_disk_idr);
0174 static DEFINE_MUTEX(mspro_block_disk_lock);
0175 
0176 static int mspro_block_complete_req(struct memstick_dev *card, int error);
0177 
0178 /*** Block device ***/
0179 
0180 static void mspro_block_bd_free_disk(struct gendisk *disk)
0181 {
0182     struct mspro_block_data *msb = disk->private_data;
0183     int disk_id = MINOR(disk_devt(disk)) >> MSPRO_BLOCK_PART_SHIFT;
0184 
0185     mutex_lock(&mspro_block_disk_lock);
0186     idr_remove(&mspro_block_disk_idr, disk_id);
0187     mutex_unlock(&mspro_block_disk_lock);
0188 
0189     kfree(msb);
0190 }
0191 
0192 static int mspro_block_bd_getgeo(struct block_device *bdev,
0193                  struct hd_geometry *geo)
0194 {
0195     struct mspro_block_data *msb = bdev->bd_disk->private_data;
0196 
0197     geo->heads = msb->heads;
0198     geo->sectors = msb->sectors_per_track;
0199     geo->cylinders = msb->cylinders;
0200 
0201     return 0;
0202 }
0203 
0204 static const struct block_device_operations ms_block_bdops = {
0205     .owner      = THIS_MODULE,
0206     .getgeo     = mspro_block_bd_getgeo,
0207     .free_disk  = mspro_block_bd_free_disk,
0208 };
0209 
0210 /*** Information ***/
0211 
0212 static struct mspro_sys_attr *mspro_from_sysfs_attr(struct attribute *attr)
0213 {
0214     struct device_attribute *dev_attr
0215         = container_of(attr, struct device_attribute, attr);
0216     return container_of(dev_attr, struct mspro_sys_attr, dev_attr);
0217 }
0218 
0219 static const char *mspro_block_attr_name(unsigned char tag)
0220 {
0221     switch (tag) {
0222     case MSPRO_BLOCK_ID_SYSINFO:
0223         return "attr_sysinfo";
0224     case MSPRO_BLOCK_ID_MODELNAME:
0225         return "attr_modelname";
0226     case MSPRO_BLOCK_ID_MBR:
0227         return "attr_mbr";
0228     case MSPRO_BLOCK_ID_PBR16:
0229         return "attr_pbr16";
0230     case MSPRO_BLOCK_ID_PBR32:
0231         return "attr_pbr32";
0232     case MSPRO_BLOCK_ID_SPECFILEVALUES1:
0233         return "attr_specfilevalues1";
0234     case MSPRO_BLOCK_ID_SPECFILEVALUES2:
0235         return "attr_specfilevalues2";
0236     case MSPRO_BLOCK_ID_DEVINFO:
0237         return "attr_devinfo";
0238     default:
0239         return NULL;
0240     }
0241 }
0242 
0243 typedef ssize_t (*sysfs_show_t)(struct device *dev,
0244                 struct device_attribute *attr,
0245                 char *buffer);
0246 
0247 static ssize_t mspro_block_attr_show_default(struct device *dev,
0248                          struct device_attribute *attr,
0249                          char *buffer)
0250 {
0251     struct mspro_sys_attr *s_attr = container_of(attr,
0252                              struct mspro_sys_attr,
0253                              dev_attr);
0254 
0255     ssize_t cnt, rc = 0;
0256 
0257     for (cnt = 0; cnt < s_attr->size; cnt++) {
0258         if (cnt && !(cnt % 16)) {
0259             if (PAGE_SIZE - rc)
0260                 buffer[rc++] = '\n';
0261         }
0262 
0263         rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "%02x ",
0264                 ((unsigned char *)s_attr->data)[cnt]);
0265     }
0266     return rc;
0267 }
0268 
0269 static ssize_t mspro_block_attr_show_sysinfo(struct device *dev,
0270                          struct device_attribute *attr,
0271                          char *buffer)
0272 {
0273     struct mspro_sys_attr *x_attr = container_of(attr,
0274                              struct mspro_sys_attr,
0275                              dev_attr);
0276     struct mspro_sys_info *x_sys = x_attr->data;
0277     ssize_t rc = 0;
0278     int date_tz = 0, date_tz_f = 0;
0279 
0280     if (x_sys->assembly_date[0] > 0x80U) {
0281         date_tz = (~x_sys->assembly_date[0]) + 1;
0282         date_tz_f = date_tz & 3;
0283         date_tz >>= 2;
0284         date_tz = -date_tz;
0285         date_tz_f *= 15;
0286     } else if (x_sys->assembly_date[0] < 0x80U) {
0287         date_tz = x_sys->assembly_date[0];
0288         date_tz_f = date_tz & 3;
0289         date_tz >>= 2;
0290         date_tz_f *= 15;
0291     }
0292 
0293     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "class: %x\n",
0294             x_sys->class);
0295     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block size: %x\n",
0296             be16_to_cpu(x_sys->block_size));
0297     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block count: %x\n",
0298             be16_to_cpu(x_sys->block_count));
0299     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "user block count: %x\n",
0300             be16_to_cpu(x_sys->user_block_count));
0301     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "page size: %x\n",
0302             be16_to_cpu(x_sys->page_size));
0303     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly date: "
0304             "GMT%+d:%d %04u-%02u-%02u %02u:%02u:%02u\n",
0305             date_tz, date_tz_f,
0306             be16_to_cpup((__be16 *)&x_sys->assembly_date[1]),
0307             x_sys->assembly_date[3], x_sys->assembly_date[4],
0308             x_sys->assembly_date[5], x_sys->assembly_date[6],
0309             x_sys->assembly_date[7]);
0310     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "serial number: %x\n",
0311             be32_to_cpu(x_sys->serial_number));
0312     rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
0313             "assembly maker code: %x\n",
0314             x_sys->assembly_maker_code);
0315     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly model code: "
0316             "%02x%02x%02x\n", x_sys->assembly_model_code[0],
0317             x_sys->assembly_model_code[1],
0318             x_sys->assembly_model_code[2]);
0319     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory maker code: %x\n",
0320             be16_to_cpu(x_sys->memory_maker_code));
0321     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory model code: %x\n",
0322             be16_to_cpu(x_sys->memory_model_code));
0323     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vcc: %x\n",
0324             x_sys->vcc);
0325     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vpp: %x\n",
0326             x_sys->vpp);
0327     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller number: %x\n",
0328             be16_to_cpu(x_sys->controller_number));
0329     rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
0330             "controller function: %x\n",
0331             be16_to_cpu(x_sys->controller_function));
0332     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
0333             be16_to_cpu(x_sys->start_sector));
0334     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "unit size: %x\n",
0335             be16_to_cpu(x_sys->unit_size));
0336     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sub class: %x\n",
0337             x_sys->ms_sub_class);
0338     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "interface type: %x\n",
0339             x_sys->interface_type);
0340     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller code: %x\n",
0341             be16_to_cpu(x_sys->controller_code));
0342     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "format type: %x\n",
0343             x_sys->format_type);
0344     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "device type: %x\n",
0345             x_sys->device_type);
0346     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "mspro id: %s\n",
0347             x_sys->mspro_id);
0348     return rc;
0349 }
0350 
0351 static ssize_t mspro_block_attr_show_modelname(struct device *dev,
0352                            struct device_attribute *attr,
0353                            char *buffer)
0354 {
0355     struct mspro_sys_attr *s_attr = container_of(attr,
0356                              struct mspro_sys_attr,
0357                              dev_attr);
0358 
0359     return scnprintf(buffer, PAGE_SIZE, "%s", (char *)s_attr->data);
0360 }
0361 
0362 static ssize_t mspro_block_attr_show_mbr(struct device *dev,
0363                      struct device_attribute *attr,
0364                      char *buffer)
0365 {
0366     struct mspro_sys_attr *x_attr = container_of(attr,
0367                              struct mspro_sys_attr,
0368                              dev_attr);
0369     struct mspro_mbr *x_mbr = x_attr->data;
0370     ssize_t rc = 0;
0371 
0372     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "boot partition: %x\n",
0373             x_mbr->boot_partition);
0374     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start head: %x\n",
0375             x_mbr->start_head);
0376     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
0377             x_mbr->start_sector);
0378     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cylinder: %x\n",
0379             x_mbr->start_cylinder);
0380     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "partition type: %x\n",
0381             x_mbr->partition_type);
0382     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end head: %x\n",
0383             x_mbr->end_head);
0384     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end sector: %x\n",
0385             x_mbr->end_sector);
0386     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end cylinder: %x\n",
0387             x_mbr->end_cylinder);
0388     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sectors: %x\n",
0389             x_mbr->start_sectors);
0390     rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
0391             "sectors per partition: %x\n",
0392             x_mbr->sectors_per_partition);
0393     return rc;
0394 }
0395 
0396 static ssize_t mspro_block_attr_show_specfile(struct device *dev,
0397                           struct device_attribute *attr,
0398                           char *buffer)
0399 {
0400     struct mspro_sys_attr *x_attr = container_of(attr,
0401                              struct mspro_sys_attr,
0402                              dev_attr);
0403     struct mspro_specfile *x_spfile = x_attr->data;
0404     char name[9], ext[4];
0405     ssize_t rc = 0;
0406 
0407     memcpy(name, x_spfile->name, 8);
0408     name[8] = 0;
0409     memcpy(ext, x_spfile->ext, 3);
0410     ext[3] = 0;
0411 
0412     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "name: %s\n", name);
0413     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "ext: %s\n", ext);
0414     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "attribute: %x\n",
0415             x_spfile->attr);
0416     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "time: %d:%d:%d\n",
0417             x_spfile->time >> 11,
0418             (x_spfile->time >> 5) & 0x3f,
0419             (x_spfile->time & 0x1f) * 2);
0420     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "date: %d-%d-%d\n",
0421             (x_spfile->date >> 9) + 1980,
0422             (x_spfile->date >> 5) & 0xf,
0423             x_spfile->date & 0x1f);
0424     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cluster: %x\n",
0425             x_spfile->cluster);
0426     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "size: %x\n",
0427             x_spfile->size);
0428     return rc;
0429 }
0430 
0431 static ssize_t mspro_block_attr_show_devinfo(struct device *dev,
0432                          struct device_attribute *attr,
0433                          char *buffer)
0434 {
0435     struct mspro_sys_attr *x_attr = container_of(attr,
0436                              struct mspro_sys_attr,
0437                              dev_attr);
0438     struct mspro_devinfo *x_devinfo = x_attr->data;
0439     ssize_t rc = 0;
0440 
0441     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "cylinders: %x\n",
0442             be16_to_cpu(x_devinfo->cylinders));
0443     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "heads: %x\n",
0444             be16_to_cpu(x_devinfo->heads));
0445     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per track: %x\n",
0446             be16_to_cpu(x_devinfo->bytes_per_track));
0447     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per sector: %x\n",
0448             be16_to_cpu(x_devinfo->bytes_per_sector));
0449     rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sectors per track: %x\n",
0450             be16_to_cpu(x_devinfo->sectors_per_track));
0451     return rc;
0452 }
0453 
0454 static sysfs_show_t mspro_block_attr_show(unsigned char tag)
0455 {
0456     switch (tag) {
0457     case MSPRO_BLOCK_ID_SYSINFO:
0458         return mspro_block_attr_show_sysinfo;
0459     case MSPRO_BLOCK_ID_MODELNAME:
0460         return mspro_block_attr_show_modelname;
0461     case MSPRO_BLOCK_ID_MBR:
0462         return mspro_block_attr_show_mbr;
0463     case MSPRO_BLOCK_ID_SPECFILEVALUES1:
0464     case MSPRO_BLOCK_ID_SPECFILEVALUES2:
0465         return mspro_block_attr_show_specfile;
0466     case MSPRO_BLOCK_ID_DEVINFO:
0467         return mspro_block_attr_show_devinfo;
0468     default:
0469         return mspro_block_attr_show_default;
0470     }
0471 }
0472 
0473 /*** Protocol handlers ***/
0474 
0475 /*
0476  * Functions prefixed with "h_" are protocol callbacks. They can be called from
0477  * interrupt context. Return value of 0 means that request processing is still
0478  * ongoing, while special error value of -EAGAIN means that current request is
0479  * finished (and request processor should come back some time later).
0480  */
0481 
0482 static int h_mspro_block_req_init(struct memstick_dev *card,
0483                   struct memstick_request **mrq)
0484 {
0485     struct mspro_block_data *msb = memstick_get_drvdata(card);
0486 
0487     *mrq = &card->current_mrq;
0488     card->next_request = msb->mrq_handler;
0489     return 0;
0490 }
0491 
0492 static int h_mspro_block_default(struct memstick_dev *card,
0493                  struct memstick_request **mrq)
0494 {
0495     return mspro_block_complete_req(card, (*mrq)->error);
0496 }
0497 
0498 static int h_mspro_block_default_bad(struct memstick_dev *card,
0499                      struct memstick_request **mrq)
0500 {
0501     return -ENXIO;
0502 }
0503 
0504 static int h_mspro_block_get_ro(struct memstick_dev *card,
0505                 struct memstick_request **mrq)
0506 {
0507     struct mspro_block_data *msb = memstick_get_drvdata(card);
0508 
0509     if (!(*mrq)->error) {
0510         if ((*mrq)->data[offsetof(struct ms_status_register, status0)]
0511             & MEMSTICK_STATUS0_WP)
0512             msb->read_only = 1;
0513         else
0514             msb->read_only = 0;
0515     }
0516 
0517     return mspro_block_complete_req(card, (*mrq)->error);
0518 }
0519 
0520 static int h_mspro_block_wait_for_ced(struct memstick_dev *card,
0521                       struct memstick_request **mrq)
0522 {
0523     dev_dbg(&card->dev, "wait for ced: value %x\n", (*mrq)->data[0]);
0524 
0525     if (!(*mrq)->error) {
0526         if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR))
0527             (*mrq)->error = -EFAULT;
0528         else if (!((*mrq)->data[0] & MEMSTICK_INT_CED))
0529             return 0;
0530     }
0531 
0532     return mspro_block_complete_req(card, (*mrq)->error);
0533 }
0534 
0535 static int h_mspro_block_transfer_data(struct memstick_dev *card,
0536                        struct memstick_request **mrq)
0537 {
0538     struct mspro_block_data *msb = memstick_get_drvdata(card);
0539     unsigned char t_val = 0;
0540     struct scatterlist t_sg = { 0 };
0541     size_t t_offset;
0542 
0543     if ((*mrq)->error)
0544         return mspro_block_complete_req(card, (*mrq)->error);
0545 
0546     switch ((*mrq)->tpc) {
0547     case MS_TPC_WRITE_REG:
0548         memstick_init_req(*mrq, MS_TPC_SET_CMD, &msb->transfer_cmd, 1);
0549         (*mrq)->need_card_int = 1;
0550         return 0;
0551     case MS_TPC_SET_CMD:
0552         t_val = (*mrq)->int_reg;
0553         memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
0554         if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT)
0555             goto has_int_reg;
0556         return 0;
0557     case MS_TPC_GET_INT:
0558         t_val = (*mrq)->data[0];
0559 has_int_reg:
0560         if (t_val & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) {
0561             t_val = MSPRO_CMD_STOP;
0562             memstick_init_req(*mrq, MS_TPC_SET_CMD, &t_val, 1);
0563             card->next_request = h_mspro_block_default;
0564             return 0;
0565         }
0566 
0567         if (msb->current_page
0568             == (msb->req_sg[msb->current_seg].length
0569             / msb->page_size)) {
0570             msb->current_page = 0;
0571             msb->current_seg++;
0572 
0573             if (msb->current_seg == msb->seg_count) {
0574                 if (t_val & MEMSTICK_INT_CED) {
0575                     return mspro_block_complete_req(card,
0576                                     0);
0577                 } else {
0578                     card->next_request
0579                         = h_mspro_block_wait_for_ced;
0580                     memstick_init_req(*mrq, MS_TPC_GET_INT,
0581                               NULL, 1);
0582                     return 0;
0583                 }
0584             }
0585         }
0586 
0587         if (!(t_val & MEMSTICK_INT_BREQ)) {
0588             memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
0589             return 0;
0590         }
0591 
0592         t_offset = msb->req_sg[msb->current_seg].offset;
0593         t_offset += msb->current_page * msb->page_size;
0594 
0595         sg_set_page(&t_sg,
0596                 nth_page(sg_page(&(msb->req_sg[msb->current_seg])),
0597                      t_offset >> PAGE_SHIFT),
0598                 msb->page_size, offset_in_page(t_offset));
0599 
0600         memstick_init_req_sg(*mrq, msb->data_dir == READ
0601                        ? MS_TPC_READ_LONG_DATA
0602                        : MS_TPC_WRITE_LONG_DATA,
0603                      &t_sg);
0604         (*mrq)->need_card_int = 1;
0605         return 0;
0606     case MS_TPC_READ_LONG_DATA:
0607     case MS_TPC_WRITE_LONG_DATA:
0608         msb->current_page++;
0609         if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT) {
0610             t_val = (*mrq)->int_reg;
0611             goto has_int_reg;
0612         } else {
0613             memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
0614             return 0;
0615         }
0616 
0617     default:
0618         BUG();
0619     }
0620 }
0621 
0622 /*** Transfer setup functions for different access methods. ***/
0623 
0624 /** Setup data transfer request for SET_CMD TPC with arguments in card
0625  *  registers.
0626  *
0627  *  @card    Current media instance
0628  *  @offset  Target data offset in bytes
0629  *  @length  Required transfer length in bytes.
0630  */
0631 static void h_mspro_block_setup_cmd(struct memstick_dev *card, u64 offset,
0632                     size_t length)
0633 {
0634     struct mspro_block_data *msb = memstick_get_drvdata(card);
0635     struct mspro_param_register param = {
0636         .system = msb->system,
0637         .data_count = cpu_to_be16((uint16_t)(length / msb->page_size)),
0638         /* ISO C90 warning precludes direct initialization for now. */
0639         .data_address = 0,
0640         .tpc_param = 0
0641     };
0642 
0643     do_div(offset, msb->page_size);
0644     param.data_address = cpu_to_be32((uint32_t)offset);
0645 
0646     card->next_request = h_mspro_block_req_init;
0647     msb->mrq_handler = h_mspro_block_transfer_data;
0648     memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
0649               &param, sizeof(param));
0650 }
0651 
0652 /*** Data transfer ***/
0653 
0654 static int mspro_block_issue_req(struct memstick_dev *card)
0655 {
0656     struct mspro_block_data *msb = memstick_get_drvdata(card);
0657     u64 t_off;
0658     unsigned int count;
0659 
0660     while (true) {
0661         msb->current_page = 0;
0662         msb->current_seg = 0;
0663         msb->seg_count = blk_rq_map_sg(msb->block_req->q,
0664                            msb->block_req,
0665                            msb->req_sg);
0666 
0667         if (!msb->seg_count) {
0668             unsigned int bytes = blk_rq_cur_bytes(msb->block_req);
0669             bool chunk;
0670 
0671             chunk = blk_update_request(msb->block_req,
0672                             BLK_STS_RESOURCE,
0673                             bytes);
0674             if (chunk)
0675                 continue;
0676             __blk_mq_end_request(msb->block_req,
0677                         BLK_STS_RESOURCE);
0678             msb->block_req = NULL;
0679             return -EAGAIN;
0680         }
0681 
0682         t_off = blk_rq_pos(msb->block_req);
0683         t_off <<= 9;
0684         count = blk_rq_bytes(msb->block_req);
0685 
0686         msb->setup_transfer(card, t_off, count);
0687 
0688         msb->data_dir = rq_data_dir(msb->block_req);
0689         msb->transfer_cmd = msb->data_dir == READ
0690                     ? MSPRO_CMD_READ_DATA
0691                     : MSPRO_CMD_WRITE_DATA;
0692 
0693         memstick_new_req(card->host);
0694         return 0;
0695     }
0696 }
0697 
0698 static int mspro_block_complete_req(struct memstick_dev *card, int error)
0699 {
0700     struct mspro_block_data *msb = memstick_get_drvdata(card);
0701     int cnt;
0702     bool chunk;
0703     unsigned int t_len = 0;
0704     unsigned long flags;
0705 
0706     spin_lock_irqsave(&msb->q_lock, flags);
0707     dev_dbg(&card->dev, "complete %d, %d\n", msb->block_req ? 1 : 0,
0708         error);
0709 
0710     if (msb->block_req) {
0711         /* Nothing to do - not really an error */
0712         if (error == -EAGAIN)
0713             error = 0;
0714 
0715         if (error || (card->current_mrq.tpc == MSPRO_CMD_STOP)) {
0716             if (msb->data_dir == READ) {
0717                 for (cnt = 0; cnt < msb->current_seg; cnt++) {
0718                     t_len += msb->req_sg[cnt].length
0719                          / msb->page_size;
0720 
0721                     if (msb->current_page)
0722                         t_len += msb->current_page - 1;
0723 
0724                     t_len *= msb->page_size;
0725                 }
0726             }
0727         } else
0728             t_len = blk_rq_bytes(msb->block_req);
0729 
0730         dev_dbg(&card->dev, "transferred %x (%d)\n", t_len, error);
0731 
0732         if (error && !t_len)
0733             t_len = blk_rq_cur_bytes(msb->block_req);
0734 
0735         chunk = blk_update_request(msb->block_req,
0736                 errno_to_blk_status(error), t_len);
0737         if (chunk) {
0738             error = mspro_block_issue_req(card);
0739             if (!error)
0740                 goto out;
0741         } else {
0742             __blk_mq_end_request(msb->block_req,
0743                         errno_to_blk_status(error));
0744             msb->block_req = NULL;
0745         }
0746     } else {
0747         if (!error)
0748             error = -EAGAIN;
0749     }
0750 
0751     card->next_request = h_mspro_block_default_bad;
0752     complete_all(&card->mrq_complete);
0753 out:
0754     spin_unlock_irqrestore(&msb->q_lock, flags);
0755     return error;
0756 }
0757 
0758 static void mspro_block_stop(struct memstick_dev *card)
0759 {
0760     struct mspro_block_data *msb = memstick_get_drvdata(card);
0761     int rc = 0;
0762     unsigned long flags;
0763 
0764     while (1) {
0765         spin_lock_irqsave(&msb->q_lock, flags);
0766         if (!msb->block_req) {
0767             blk_mq_stop_hw_queues(msb->queue);
0768             rc = 1;
0769         }
0770         spin_unlock_irqrestore(&msb->q_lock, flags);
0771 
0772         if (rc)
0773             break;
0774 
0775         wait_for_completion(&card->mrq_complete);
0776     }
0777 }
0778 
0779 static void mspro_block_start(struct memstick_dev *card)
0780 {
0781     struct mspro_block_data *msb = memstick_get_drvdata(card);
0782 
0783     blk_mq_start_hw_queues(msb->queue);
0784 }
0785 
0786 static blk_status_t mspro_queue_rq(struct blk_mq_hw_ctx *hctx,
0787                    const struct blk_mq_queue_data *bd)
0788 {
0789     struct memstick_dev *card = hctx->queue->queuedata;
0790     struct mspro_block_data *msb = memstick_get_drvdata(card);
0791 
0792     spin_lock_irq(&msb->q_lock);
0793 
0794     if (msb->block_req) {
0795         spin_unlock_irq(&msb->q_lock);
0796         return BLK_STS_DEV_RESOURCE;
0797     }
0798 
0799     if (msb->eject) {
0800         spin_unlock_irq(&msb->q_lock);
0801         blk_mq_start_request(bd->rq);
0802         return BLK_STS_IOERR;
0803     }
0804 
0805     msb->block_req = bd->rq;
0806     blk_mq_start_request(bd->rq);
0807 
0808     if (mspro_block_issue_req(card))
0809         msb->block_req = NULL;
0810 
0811     spin_unlock_irq(&msb->q_lock);
0812     return BLK_STS_OK;
0813 }
0814 
0815 /*** Initialization ***/
0816 
0817 static int mspro_block_wait_for_ced(struct memstick_dev *card)
0818 {
0819     struct mspro_block_data *msb = memstick_get_drvdata(card);
0820 
0821     card->next_request = h_mspro_block_req_init;
0822     msb->mrq_handler = h_mspro_block_wait_for_ced;
0823     memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
0824     memstick_new_req(card->host);
0825     wait_for_completion(&card->mrq_complete);
0826     return card->current_mrq.error;
0827 }
0828 
0829 static int mspro_block_set_interface(struct memstick_dev *card,
0830                      unsigned char sys_reg)
0831 {
0832     struct memstick_host *host = card->host;
0833     struct mspro_block_data *msb = memstick_get_drvdata(card);
0834     struct mspro_param_register param = {
0835         .system = sys_reg,
0836         .data_count = 0,
0837         .data_address = 0,
0838         .tpc_param = 0
0839     };
0840 
0841     card->next_request = h_mspro_block_req_init;
0842     msb->mrq_handler = h_mspro_block_default;
0843     memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
0844               sizeof(param));
0845     memstick_new_req(host);
0846     wait_for_completion(&card->mrq_complete);
0847     return card->current_mrq.error;
0848 }
0849 
0850 static int mspro_block_switch_interface(struct memstick_dev *card)
0851 {
0852     struct memstick_host *host = card->host;
0853     struct mspro_block_data *msb = memstick_get_drvdata(card);
0854     int rc = 0;
0855 
0856 try_again:
0857     if (msb->caps & MEMSTICK_CAP_PAR4)
0858         rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR4);
0859     else
0860         return 0;
0861 
0862     if (rc) {
0863         printk(KERN_WARNING
0864                "%s: could not switch to 4-bit mode, error %d\n",
0865                dev_name(&card->dev), rc);
0866         return 0;
0867     }
0868 
0869     msb->system = MEMSTICK_SYS_PAR4;
0870     host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
0871     printk(KERN_INFO "%s: switching to 4-bit parallel mode\n",
0872            dev_name(&card->dev));
0873 
0874     if (msb->caps & MEMSTICK_CAP_PAR8) {
0875         rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8);
0876 
0877         if (!rc) {
0878             msb->system = MEMSTICK_SYS_PAR8;
0879             host->set_param(host, MEMSTICK_INTERFACE,
0880                     MEMSTICK_PAR8);
0881             printk(KERN_INFO
0882                    "%s: switching to 8-bit parallel mode\n",
0883                    dev_name(&card->dev));
0884         } else
0885             printk(KERN_WARNING
0886                    "%s: could not switch to 8-bit mode, error %d\n",
0887                    dev_name(&card->dev), rc);
0888     }
0889 
0890     card->next_request = h_mspro_block_req_init;
0891     msb->mrq_handler = h_mspro_block_default;
0892     memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
0893     memstick_new_req(card->host);
0894     wait_for_completion(&card->mrq_complete);
0895     rc = card->current_mrq.error;
0896 
0897     if (rc) {
0898         printk(KERN_WARNING
0899                "%s: interface error, trying to fall back to serial\n",
0900                dev_name(&card->dev));
0901         msb->system = MEMSTICK_SYS_SERIAL;
0902         host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
0903         msleep(10);
0904         host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
0905         host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
0906 
0907         rc = memstick_set_rw_addr(card);
0908         if (!rc)
0909             rc = mspro_block_set_interface(card, msb->system);
0910 
0911         if (!rc) {
0912             msleep(150);
0913             rc = mspro_block_wait_for_ced(card);
0914             if (rc)
0915                 return rc;
0916 
0917             if (msb->caps & MEMSTICK_CAP_PAR8) {
0918                 msb->caps &= ~MEMSTICK_CAP_PAR8;
0919                 goto try_again;
0920             }
0921         }
0922     }
0923     return rc;
0924 }
0925 
0926 /* Memory allocated for attributes by this function should be freed by
0927  * mspro_block_data_clear, no matter if the initialization process succeeded
0928  * or failed.
0929  */
0930 static int mspro_block_read_attributes(struct memstick_dev *card)
0931 {
0932     struct mspro_block_data *msb = memstick_get_drvdata(card);
0933     struct mspro_attribute *attr = NULL;
0934     struct mspro_sys_attr *s_attr = NULL;
0935     unsigned char *buffer = NULL;
0936     int cnt, rc, attr_count;
0937     /* While normally physical device offsets, represented here by
0938      * attr_offset and attr_len will be of large numeric types, we can be
0939      * sure, that attributes are close enough to the beginning of the
0940      * device, to save ourselves some trouble.
0941      */
0942     unsigned int addr, attr_offset = 0, attr_len = msb->page_size;
0943 
0944     attr = kmalloc(msb->page_size, GFP_KERNEL);
0945     if (!attr)
0946         return -ENOMEM;
0947 
0948     sg_init_one(&msb->req_sg[0], attr, msb->page_size);
0949     msb->seg_count = 1;
0950     msb->current_seg = 0;
0951     msb->current_page = 0;
0952     msb->data_dir = READ;
0953     msb->transfer_cmd = MSPRO_CMD_READ_ATRB;
0954 
0955     msb->setup_transfer(card, attr_offset, attr_len);
0956 
0957     memstick_new_req(card->host);
0958     wait_for_completion(&card->mrq_complete);
0959     if (card->current_mrq.error) {
0960         rc = card->current_mrq.error;
0961         goto out_free_attr;
0962     }
0963 
0964     if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) {
0965         printk(KERN_ERR "%s: unrecognized device signature %x\n",
0966                dev_name(&card->dev), be16_to_cpu(attr->signature));
0967         rc = -ENODEV;
0968         goto out_free_attr;
0969     }
0970 
0971     if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) {
0972         printk(KERN_WARNING "%s: way too many attribute entries\n",
0973                dev_name(&card->dev));
0974         attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES;
0975     } else
0976         attr_count = attr->count;
0977 
0978     msb->attr_group.attrs = kcalloc(attr_count + 1,
0979                     sizeof(*msb->attr_group.attrs),
0980                     GFP_KERNEL);
0981     if (!msb->attr_group.attrs) {
0982         rc = -ENOMEM;
0983         goto out_free_attr;
0984     }
0985     msb->attr_group.name = "media_attributes";
0986 
0987     buffer = kmemdup(attr, attr_len, GFP_KERNEL);
0988     if (!buffer) {
0989         rc = -ENOMEM;
0990         goto out_free_attr;
0991     }
0992 
0993     for (cnt = 0; cnt < attr_count; ++cnt) {
0994         s_attr = kzalloc(sizeof(struct mspro_sys_attr), GFP_KERNEL);
0995         if (!s_attr) {
0996             rc = -ENOMEM;
0997             goto out_free_buffer;
0998         }
0999 
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     put_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_mq_free_tag_set(&msb->tag_set);
1298     msb->queue = NULL;
1299 
1300     sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
1301 
1302     mutex_lock(&mspro_block_disk_lock);
1303     mspro_block_data_clear(msb);
1304     mutex_unlock(&mspro_block_disk_lock);
1305 
1306     put_disk(msb->disk);
1307     memstick_set_drvdata(card, NULL);
1308 }
1309 
1310 #ifdef CONFIG_PM
1311 
1312 static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state)
1313 {
1314     struct mspro_block_data *msb = memstick_get_drvdata(card);
1315     unsigned long flags;
1316 
1317     blk_mq_stop_hw_queues(msb->queue);
1318 
1319     spin_lock_irqsave(&msb->q_lock, flags);
1320     msb->active = 0;
1321     spin_unlock_irqrestore(&msb->q_lock, flags);
1322 
1323     return 0;
1324 }
1325 
1326 static int mspro_block_resume(struct memstick_dev *card)
1327 {
1328     struct mspro_block_data *msb = memstick_get_drvdata(card);
1329     int rc = 0;
1330 
1331 #ifdef CONFIG_MEMSTICK_UNSAFE_RESUME
1332 
1333     struct mspro_block_data *new_msb;
1334     struct memstick_host *host = card->host;
1335     struct mspro_sys_attr *s_attr, *r_attr;
1336     unsigned char cnt;
1337 
1338     mutex_lock(&host->lock);
1339     new_msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
1340     if (!new_msb) {
1341         rc = -ENOMEM;
1342         goto out_unlock;
1343     }
1344 
1345     new_msb->card = card;
1346     memstick_set_drvdata(card, new_msb);
1347     rc = mspro_block_init_card(card);
1348     if (rc)
1349         goto out_free;
1350 
1351     for (cnt = 0; new_msb->attr_group.attrs[cnt]
1352               && msb->attr_group.attrs[cnt]; ++cnt) {
1353         s_attr = mspro_from_sysfs_attr(new_msb->attr_group.attrs[cnt]);
1354         r_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[cnt]);
1355 
1356         if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO
1357             && r_attr->id == s_attr->id) {
1358             if (memcmp(s_attr->data, r_attr->data, s_attr->size))
1359                 break;
1360 
1361             msb->active = 1;
1362             break;
1363         }
1364     }
1365 
1366 out_free:
1367     memstick_set_drvdata(card, msb);
1368     mspro_block_data_clear(new_msb);
1369     kfree(new_msb);
1370 out_unlock:
1371     mutex_unlock(&host->lock);
1372 
1373 #endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */
1374 
1375     blk_mq_start_hw_queues(msb->queue);
1376     return rc;
1377 }
1378 
1379 #else
1380 
1381 #define mspro_block_suspend NULL
1382 #define mspro_block_resume NULL
1383 
1384 #endif /* CONFIG_PM */
1385 
1386 static struct memstick_device_id mspro_block_id_tbl[] = {
1387     {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_PRO, MEMSTICK_CATEGORY_STORAGE_DUO,
1388      MEMSTICK_CLASS_DUO},
1389     {}
1390 };
1391 
1392 
1393 static struct memstick_driver mspro_block_driver = {
1394     .driver = {
1395         .name  = DRIVER_NAME,
1396         .owner = THIS_MODULE
1397     },
1398     .id_table = mspro_block_id_tbl,
1399     .probe    = mspro_block_probe,
1400     .remove   = mspro_block_remove,
1401     .suspend  = mspro_block_suspend,
1402     .resume   = mspro_block_resume
1403 };
1404 
1405 static int __init mspro_block_init(void)
1406 {
1407     int rc = -ENOMEM;
1408 
1409     rc = register_blkdev(major, DRIVER_NAME);
1410     if (rc < 0) {
1411         printk(KERN_ERR DRIVER_NAME ": failed to register "
1412                "major %d, error %d\n", major, rc);
1413         return rc;
1414     }
1415     if (!major)
1416         major = rc;
1417 
1418     rc = memstick_register_driver(&mspro_block_driver);
1419     if (rc)
1420         unregister_blkdev(major, DRIVER_NAME);
1421     return rc;
1422 }
1423 
1424 static void __exit mspro_block_exit(void)
1425 {
1426     memstick_unregister_driver(&mspro_block_driver);
1427     unregister_blkdev(major, DRIVER_NAME);
1428     idr_destroy(&mspro_block_disk_idr);
1429 }
1430 
1431 module_init(mspro_block_init);
1432 module_exit(mspro_block_exit);
1433 
1434 MODULE_LICENSE("GPL");
1435 MODULE_AUTHOR("Alex Dubov");
1436 MODULE_DESCRIPTION("Sony MemoryStickPro block device driver");
1437 MODULE_DEVICE_TABLE(memstick, mspro_block_id_tbl);
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!