drivers/block/swim.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
0004  *
0005  * Copyright (C) 2004,2008 Laurent Vivier <Laurent@lvivier.info>
0006  *
0007  * based on Alastair Bridgewater SWIM analysis, 2001
0008  * based on SWIM3 driver (c) Paul Mackerras, 1996
0009  * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
0010  *
0011  * 2004-08-21 (lv) - Initial implementation
0012  * 2008-10-30 (lv) - Port to 2.6
0013  */
0014
0015 #include <linux/module.h>
0016 #include <linux/fd.h>
0017 #include <linux/slab.h>
0018 #include <linux/blk-mq.h>
0019 #include <linux/major.h>
0020 #include <linux/mutex.h>
0021 #include <linux/hdreg.h>
0022 #include <linux/kernel.h>
0023 #include <linux/delay.h>
0024 #include <linux/platform_device.h>
0025
0026 #include <asm/mac_via.h>
0027
0028 #define CARDNAME "swim"
0029
0030 struct sector_header {
0031     unsigned char side;
0032     unsigned char track;
0033     unsigned char sector;
0034     unsigned char size;
0035     unsigned char crc0;
0036     unsigned char crc1;
0037 } __attribute__((packed));
0038
0039 #define DRIVER_VERSION "Version 0.2 (2008-10-30)"
0040
0041 #define REG(x)  unsigned char x, x ## _pad[0x200 - 1];
0042
0043 struct swim {
0044     REG(write_data)
0045     REG(write_mark)
0046     REG(write_CRC)
0047     REG(write_parameter)
0048     REG(write_phase)
0049     REG(write_setup)
0050     REG(write_mode0)
0051     REG(write_mode1)
0052
0053     REG(read_data)
0054     REG(read_mark)
0055     REG(read_error)
0056     REG(read_parameter)
0057     REG(read_phase)
0058     REG(read_setup)
0059     REG(read_status)
0060     REG(read_handshake)
0061 } __attribute__((packed));
0062
0063 #define swim_write(base, reg, v)    out_8(&(base)->write_##reg, (v))
0064 #define swim_read(base, reg)        in_8(&(base)->read_##reg)
0065
0066 /* IWM registers */
0067
0068 struct iwm {
0069     REG(ph0L)
0070     REG(ph0H)
0071     REG(ph1L)
0072     REG(ph1H)
0073     REG(ph2L)
0074     REG(ph2H)
0075     REG(ph3L)
0076     REG(ph3H)
0077     REG(mtrOff)
0078     REG(mtrOn)
0079     REG(intDrive)
0080     REG(extDrive)
0081     REG(q6L)
0082     REG(q6H)
0083     REG(q7L)
0084     REG(q7H)
0085 } __attribute__((packed));
0086
0087 #define iwm_write(base, reg, v)     out_8(&(base)->reg, (v))
0088 #define iwm_read(base, reg)     in_8(&(base)->reg)
0089
0090 /* bits in phase register */
0091
0092 #define SEEK_POSITIVE   0x070
0093 #define SEEK_NEGATIVE   0x074
0094 #define STEP        0x071
0095 #define MOTOR_ON    0x072
0096 #define MOTOR_OFF   0x076
0097 #define INDEX       0x073
0098 #define EJECT       0x077
0099 #define SETMFM      0x171
0100 #define SETGCR      0x175
0101
0102 #define RELAX       0x033
0103 #define LSTRB       0x008
0104
0105 #define CA_MASK     0x077
0106
0107 /* Select values for swim_select and swim_readbit */
0108
0109 #define READ_DATA_0 0x074
0110 #define ONEMEG_DRIVE    0x075
0111 #define SINGLE_SIDED    0x076
0112 #define DRIVE_PRESENT   0x077
0113 #define DISK_IN     0x170
0114 #define WRITE_PROT  0x171
0115 #define TRACK_ZERO  0x172
0116 #define TACHO       0x173
0117 #define READ_DATA_1 0x174
0118 #define GCR_MODE    0x175
0119 #define SEEK_COMPLETE   0x176
0120 #define TWOMEG_MEDIA    0x177
0121
0122 /* Bits in handshake register */
0123
0124 #define MARK_BYTE   0x01
0125 #define CRC_ZERO    0x02
0126 #define RDDATA      0x04
0127 #define SENSE       0x08
0128 #define MOTEN       0x10
0129 #define ERROR       0x20
0130 #define DAT2BYTE    0x40
0131 #define DAT1BYTE    0x80
0132
0133 /* bits in setup register */
0134
0135 #define S_INV_WDATA 0x01
0136 #define S_3_5_SELECT    0x02
0137 #define S_GCR       0x04
0138 #define S_FCLK_DIV2 0x08
0139 #define S_ERROR_CORR    0x10
0140 #define S_IBM_DRIVE 0x20
0141 #define S_GCR_WRITE 0x40
0142 #define S_TIMEOUT   0x80
0143
0144 /* bits in mode register */
0145
0146 #define CLFIFO      0x01
0147 #define ENBL1       0x02
0148 #define ENBL2       0x04
0149 #define ACTION      0x08
0150 #define WRITE_MODE  0x10
0151 #define HEDSEL      0x20
0152 #define MOTON       0x80
0153
0154 /*----------------------------------------------------------------------------*/
0155
0156 enum drive_location {
0157     INTERNAL_DRIVE = 0x02,
0158     EXTERNAL_DRIVE = 0x04,
0159 };
0160
0161 enum media_type {
0162     DD_MEDIA,
0163     HD_MEDIA,
0164 };
0165
0166 struct floppy_state {
0167
0168     /* physical properties */
0169
0170     enum drive_location location;   /* internal or external drive */
0171     int      head_number;   /* single- or double-sided drive */
0172
0173     /* media */
0174
0175     int      disk_in;
0176     int      ejected;
0177     enum media_type  type;
0178     int      write_protected;
0179
0180     int      total_secs;
0181     int      secpercyl;
0182     int      secpertrack;
0183
0184     /* in-use information */
0185
0186     int     track;
0187     int     ref_count;
0188     bool registered;
0189
0190     struct gendisk *disk;
0191     struct blk_mq_tag_set tag_set;
0192
0193     /* parent controller */
0194
0195     struct swim_priv *swd;
0196 };
0197
0198 enum motor_action {
0199     OFF,
0200     ON,
0201 };
0202
0203 enum head {
0204     LOWER_HEAD = 0,
0205     UPPER_HEAD = 1,
0206 };
0207
0208 #define FD_MAX_UNIT 2
0209
0210 struct swim_priv {
0211     struct swim __iomem *base;
0212     spinlock_t lock;
0213     int floppy_count;
0214     struct floppy_state unit[FD_MAX_UNIT];
0215 };
0216
0217 extern int swim_read_sector_header(struct swim __iomem *base,
0218                    struct sector_header *header);
0219 extern int swim_read_sector_data(struct swim __iomem *base,
0220                  unsigned char *data);
0221
0222 static DEFINE_MUTEX(swim_mutex);
0223 static inline void set_swim_mode(struct swim __iomem *base, int enable)
0224 {
0225     struct iwm __iomem *iwm_base;
0226     unsigned long flags;
0227
0228     if (!enable) {
0229         swim_write(base, mode0, 0xf8);
0230         return;
0231     }
0232
0233     iwm_base = (struct iwm __iomem *)base;
0234     local_irq_save(flags);
0235
0236     iwm_read(iwm_base, q7L);
0237     iwm_read(iwm_base, mtrOff);
0238     iwm_read(iwm_base, q6H);
0239
0240     iwm_write(iwm_base, q7H, 0x57);
0241     iwm_write(iwm_base, q7H, 0x17);
0242     iwm_write(iwm_base, q7H, 0x57);
0243     iwm_write(iwm_base, q7H, 0x57);
0244
0245     local_irq_restore(flags);
0246 }
0247
0248 static inline int get_swim_mode(struct swim __iomem *base)
0249 {
0250     unsigned long flags;
0251
0252     local_irq_save(flags);
0253
0254     swim_write(base, phase, 0xf5);
0255     if (swim_read(base, phase) != 0xf5)
0256         goto is_iwm;
0257     swim_write(base, phase, 0xf6);
0258     if (swim_read(base, phase) != 0xf6)
0259         goto is_iwm;
0260     swim_write(base, phase, 0xf7);
0261     if (swim_read(base, phase) != 0xf7)
0262         goto is_iwm;
0263     local_irq_restore(flags);
0264     return 1;
0265 is_iwm:
0266     local_irq_restore(flags);
0267     return 0;
0268 }
0269
0270 static inline void swim_select(struct swim __iomem *base, int sel)
0271 {
0272     swim_write(base, phase, RELAX);
0273
0274     via1_set_head(sel & 0x100);
0275
0276     swim_write(base, phase, sel & CA_MASK);
0277 }
0278
0279 static inline void swim_action(struct swim __iomem *base, int action)
0280 {
0281     unsigned long flags;
0282
0283     local_irq_save(flags);
0284
0285     swim_select(base, action);
0286     udelay(1);
0287     swim_write(base, phase, (LSTRB<<4) | LSTRB);
0288     udelay(1);
0289     swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F));
0290     udelay(1);
0291
0292     local_irq_restore(flags);
0293 }
0294
0295 static inline int swim_readbit(struct swim __iomem *base, int bit)
0296 {
0297     int stat;
0298
0299     swim_select(base, bit);
0300
0301     udelay(10);
0302
0303     stat = swim_read(base, handshake);
0304
0305     return (stat & SENSE) == 0;
0306 }
0307
0308 static inline void swim_drive(struct swim __iomem *base,
0309                   enum drive_location location)
0310 {
0311     if (location == INTERNAL_DRIVE) {
0312         swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
0313         swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
0314     } else if (location == EXTERNAL_DRIVE) {
0315         swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
0316         swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
0317     }
0318 }
0319
0320 static inline void swim_motor(struct swim __iomem *base,
0321                   enum motor_action action)
0322 {
0323     if (action == ON) {
0324         int i;
0325
0326         swim_action(base, MOTOR_ON);
0327
0328         for (i = 0; i < 2*HZ; i++) {
0329             swim_select(base, RELAX);
0330             if (swim_readbit(base, MOTOR_ON))
0331                 break;
0332             set_current_state(TASK_INTERRUPTIBLE);
0333             schedule_timeout(1);
0334         }
0335     } else if (action == OFF) {
0336         swim_action(base, MOTOR_OFF);
0337         swim_select(base, RELAX);
0338     }
0339 }
0340
0341 static inline void swim_eject(struct swim __iomem *base)
0342 {
0343     int i;
0344
0345     swim_action(base, EJECT);
0346
0347     for (i = 0; i < 2*HZ; i++) {
0348         swim_select(base, RELAX);
0349         if (!swim_readbit(base, DISK_IN))
0350             break;
0351         set_current_state(TASK_INTERRUPTIBLE);
0352         schedule_timeout(1);
0353     }
0354     swim_select(base, RELAX);
0355 }
0356
0357 static inline void swim_head(struct swim __iomem *base, enum head head)
0358 {
0359     /* wait drive is ready */
0360
0361     if (head == UPPER_HEAD)
0362         swim_select(base, READ_DATA_1);
0363     else if (head == LOWER_HEAD)
0364         swim_select(base, READ_DATA_0);
0365 }
0366
0367 static inline int swim_step(struct swim __iomem *base)
0368 {
0369     int wait;
0370
0371     swim_action(base, STEP);
0372
0373     for (wait = 0; wait < HZ; wait++) {
0374
0375         set_current_state(TASK_INTERRUPTIBLE);
0376         schedule_timeout(1);
0377
0378         swim_select(base, RELAX);
0379         if (!swim_readbit(base, STEP))
0380             return 0;
0381     }
0382     return -1;
0383 }
0384
0385 static inline int swim_track00(struct swim __iomem *base)
0386 {
0387     int try;
0388
0389     swim_action(base, SEEK_NEGATIVE);
0390
0391     for (try = 0; try < 100; try++) {
0392
0393         swim_select(base, RELAX);
0394         if (swim_readbit(base, TRACK_ZERO))
0395             break;
0396
0397         if (swim_step(base))
0398             return -1;
0399     }
0400
0401     if (swim_readbit(base, TRACK_ZERO))
0402         return 0;
0403
0404     return -1;
0405 }
0406
0407 static inline int swim_seek(struct swim __iomem *base, int step)
0408 {
0409     if (step == 0)
0410         return 0;
0411
0412     if (step < 0) {
0413         swim_action(base, SEEK_NEGATIVE);
0414         step = -step;
0415     } else
0416         swim_action(base, SEEK_POSITIVE);
0417
0418     for ( ; step > 0; step--) {
0419         if (swim_step(base))
0420             return -1;
0421     }
0422
0423     return 0;
0424 }
0425
0426 static inline int swim_track(struct floppy_state *fs,  int track)
0427 {
0428     struct swim __iomem *base = fs->swd->base;
0429     int ret;
0430
0431     ret = swim_seek(base, track - fs->track);
0432
0433     if (ret == 0)
0434         fs->track = track;
0435     else {
0436         swim_track00(base);
0437         fs->track = 0;
0438     }
0439
0440     return ret;
0441 }
0442
0443 static int floppy_eject(struct floppy_state *fs)
0444 {
0445     struct swim __iomem *base = fs->swd->base;
0446
0447     swim_drive(base, fs->location);
0448     swim_motor(base, OFF);
0449     swim_eject(base);
0450
0451     fs->disk_in = 0;
0452     fs->ejected = 1;
0453
0454     return 0;
0455 }
0456
0457 static inline int swim_read_sector(struct floppy_state *fs,
0458                    int side, int track,
0459                    int sector, unsigned char *buffer)
0460 {
0461     struct swim __iomem *base = fs->swd->base;
0462     unsigned long flags;
0463     struct sector_header header;
0464     int ret = -1;
0465     short i;
0466
0467     swim_track(fs, track);
0468
0469     swim_write(base, mode1, MOTON);
0470     swim_head(base, side);
0471     swim_write(base, mode0, side);
0472
0473     local_irq_save(flags);
0474     for (i = 0; i < 36; i++) {
0475         ret = swim_read_sector_header(base, &header);
0476         if (!ret && (header.sector == sector)) {
0477             /* found */
0478
0479             ret = swim_read_sector_data(base, buffer);
0480             break;
0481         }
0482     }
0483     local_irq_restore(flags);
0484
0485     swim_write(base, mode0, MOTON);
0486
0487     if ((header.side != side)  || (header.track != track) ||
0488          (header.sector != sector))
0489         return 0;
0490
0491     return ret;
0492 }
0493
0494 static blk_status_t floppy_read_sectors(struct floppy_state *fs,
0495                    int req_sector, int sectors_nb,
0496                    unsigned char *buffer)
0497 {
0498     struct swim __iomem *base = fs->swd->base;
0499     int ret;
0500     int side, track, sector;
0501     int i, try;
0502
0503
0504     swim_drive(base, fs->location);
0505     for (i = req_sector; i < req_sector + sectors_nb; i++) {
0506         int x;
0507         track = i / fs->secpercyl;
0508         x = i % fs->secpercyl;
0509         side = x / fs->secpertrack;
0510         sector = x % fs->secpertrack + 1;
0511
0512         try = 5;
0513         do {
0514             ret = swim_read_sector(fs, side, track, sector,
0515                         buffer);
0516             if (try-- == 0)
0517                 return BLK_STS_IOERR;
0518         } while (ret != 512);
0519
0520         buffer += ret;
0521     }
0522
0523     return 0;
0524 }
0525
0526 static blk_status_t swim_queue_rq(struct blk_mq_hw_ctx *hctx,
0527                   const struct blk_mq_queue_data *bd)
0528 {
0529     struct floppy_state *fs = hctx->queue->queuedata;
0530     struct swim_priv *swd = fs->swd;
0531     struct request *req = bd->rq;
0532     blk_status_t err;
0533
0534     if (!spin_trylock_irq(&swd->lock))
0535         return BLK_STS_DEV_RESOURCE;
0536
0537     blk_mq_start_request(req);
0538
0539     if (!fs->disk_in || rq_data_dir(req) == WRITE) {
0540         err = BLK_STS_IOERR;
0541         goto out;
0542     }
0543
0544     do {
0545         err = floppy_read_sectors(fs, blk_rq_pos(req),
0546                       blk_rq_cur_sectors(req),
0547                       bio_data(req->bio));
0548     } while (blk_update_request(req, err, blk_rq_cur_bytes(req)));
0549     __blk_mq_end_request(req, err);
0550
0551     err = BLK_STS_OK;
0552 out:
0553     spin_unlock_irq(&swd->lock);
0554     return err;
0555
0556 }
0557
0558 static struct floppy_struct floppy_type[4] = {
0559     {    0,  0, 0,  0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing   */
0560     {  720,  9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
0561     { 1440,  9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5"   */
0562     { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5"  */
0563 };
0564
0565 static int get_floppy_geometry(struct floppy_state *fs, int type,
0566                    struct floppy_struct **g)
0567 {
0568     if (type >= ARRAY_SIZE(floppy_type))
0569         return -EINVAL;
0570
0571     if (type)
0572         *g = &floppy_type[type];
0573     else if (fs->type == HD_MEDIA) /* High-Density media */
0574         *g = &floppy_type[3];
0575     else if (fs->head_number == 2) /* double-sided */
0576         *g = &floppy_type[2];
0577     else
0578         *g = &floppy_type[1];
0579
0580     return 0;
0581 }
0582
0583 static void setup_medium(struct floppy_state *fs)
0584 {
0585     struct swim __iomem *base = fs->swd->base;
0586
0587     if (swim_readbit(base, DISK_IN)) {
0588         struct floppy_struct *g;
0589         fs->disk_in = 1;
0590         fs->write_protected = swim_readbit(base, WRITE_PROT);
0591
0592         if (swim_track00(base))
0593             printk(KERN_ERR
0594                 "SWIM: cannot move floppy head to track 0\n");
0595
0596         swim_track00(base);
0597
0598         fs->type = swim_readbit(base, TWOMEG_MEDIA) ?
0599             HD_MEDIA : DD_MEDIA;
0600         fs->head_number = swim_readbit(base, SINGLE_SIDED) ? 1 : 2;
0601         get_floppy_geometry(fs, 0, &g);
0602         fs->total_secs = g->size;
0603         fs->secpercyl = g->head * g->sect;
0604         fs->secpertrack = g->sect;
0605         fs->track = 0;
0606     } else {
0607         fs->disk_in = 0;
0608     }
0609 }
0610
0611 static int floppy_open(struct block_device *bdev, fmode_t mode)
0612 {
0613     struct floppy_state *fs = bdev->bd_disk->private_data;
0614     struct swim __iomem *base = fs->swd->base;
0615     int err;
0616
0617     if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL))
0618         return -EBUSY;
0619
0620     if (mode & FMODE_EXCL)
0621         fs->ref_count = -1;
0622     else
0623         fs->ref_count++;
0624
0625     swim_write(base, setup, S_IBM_DRIVE  | S_FCLK_DIV2);
0626     udelay(10);
0627     swim_drive(base, fs->location);
0628     swim_motor(base, ON);
0629     swim_action(base, SETMFM);
0630     if (fs->ejected)
0631         setup_medium(fs);
0632     if (!fs->disk_in) {
0633         err = -ENXIO;
0634         goto out;
0635     }
0636
0637     set_capacity(fs->disk, fs->total_secs);
0638
0639     if (mode & FMODE_NDELAY)
0640         return 0;
0641
0642     if (mode & (FMODE_READ|FMODE_WRITE)) {
0643         if (bdev_check_media_change(bdev) && fs->disk_in)
0644             fs->ejected = 0;
0645         if ((mode & FMODE_WRITE) && fs->write_protected) {
0646             err = -EROFS;
0647             goto out;
0648         }
0649     }
0650     return 0;
0651 out:
0652     if (fs->ref_count < 0)
0653         fs->ref_count = 0;
0654     else if (fs->ref_count > 0)
0655         --fs->ref_count;
0656
0657     if (fs->ref_count == 0)
0658         swim_motor(base, OFF);
0659     return err;
0660 }
0661
0662 static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
0663 {
0664     int ret;
0665
0666     mutex_lock(&swim_mutex);
0667     ret = floppy_open(bdev, mode);
0668     mutex_unlock(&swim_mutex);
0669
0670     return ret;
0671 }
0672
0673 static void floppy_release(struct gendisk *disk, fmode_t mode)
0674 {
0675     struct floppy_state *fs = disk->private_data;
0676     struct swim __iomem *base = fs->swd->base;
0677
0678     mutex_lock(&swim_mutex);
0679     if (fs->ref_count < 0)
0680         fs->ref_count = 0;
0681     else if (fs->ref_count > 0)
0682         --fs->ref_count;
0683
0684     if (fs->ref_count == 0)
0685         swim_motor(base, OFF);
0686     mutex_unlock(&swim_mutex);
0687 }
0688
0689 static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
0690             unsigned int cmd, unsigned long param)
0691 {
0692     struct floppy_state *fs = bdev->bd_disk->private_data;
0693     int err;
0694
0695     if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
0696             return -EPERM;
0697
0698     switch (cmd) {
0699     case FDEJECT:
0700         if (fs->ref_count != 1)
0701             return -EBUSY;
0702         mutex_lock(&swim_mutex);
0703         err = floppy_eject(fs);
0704         mutex_unlock(&swim_mutex);
0705         return err;
0706
0707     case FDGETPRM:
0708         if (copy_to_user((void __user *) param, (void *) &floppy_type,
0709                  sizeof(struct floppy_struct)))
0710             return -EFAULT;
0711         return 0;
0712     }
0713     return -ENOTTY;
0714 }
0715
0716 static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo)
0717 {
0718     struct floppy_state *fs = bdev->bd_disk->private_data;
0719     struct floppy_struct *g;
0720     int ret;
0721
0722     ret = get_floppy_geometry(fs, 0, &g);
0723     if (ret)
0724         return ret;
0725
0726     geo->heads = g->head;
0727     geo->sectors = g->sect;
0728     geo->cylinders = g->track;
0729
0730     return 0;
0731 }
0732
0733 static unsigned int floppy_check_events(struct gendisk *disk,
0734                     unsigned int clearing)
0735 {
0736     struct floppy_state *fs = disk->private_data;
0737
0738     return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
0739 }
0740
0741 static const struct block_device_operations floppy_fops = {
0742     .owner       = THIS_MODULE,
0743     .open        = floppy_unlocked_open,
0744     .release     = floppy_release,
0745     .ioctl       = floppy_ioctl,
0746     .getgeo      = floppy_getgeo,
0747     .check_events    = floppy_check_events,
0748 };
0749
0750 static int swim_add_floppy(struct swim_priv *swd, enum drive_location location)
0751 {
0752     struct floppy_state *fs = &swd->unit[swd->floppy_count];
0753     struct swim __iomem *base = swd->base;
0754
0755     fs->location = location;
0756
0757     swim_drive(base, location);
0758
0759     swim_motor(base, OFF);
0760
0761     fs->type = HD_MEDIA;
0762     fs->head_number = 2;
0763
0764     fs->ref_count = 0;
0765     fs->ejected = 1;
0766
0767     swd->floppy_count++;
0768
0769     return 0;
0770 }
0771
0772 static const struct blk_mq_ops swim_mq_ops = {
0773     .queue_rq = swim_queue_rq,
0774 };
0775
0776 static void swim_cleanup_floppy_disk(struct floppy_state *fs)
0777 {
0778     struct gendisk *disk = fs->disk;
0779
0780     if (!disk)
0781         return;
0782
0783     if (fs->registered)
0784         del_gendisk(fs->disk);
0785
0786     put_disk(disk);
0787     blk_mq_free_tag_set(&fs->tag_set);
0788 }
0789
0790 static int swim_floppy_init(struct swim_priv *swd)
0791 {
0792     int err;
0793     int drive;
0794     struct swim __iomem *base = swd->base;
0795
0796     /* scan floppy drives */
0797
0798     swim_drive(base, INTERNAL_DRIVE);
0799     if (swim_readbit(base, DRIVE_PRESENT) &&
0800         !swim_readbit(base, ONEMEG_DRIVE))
0801         swim_add_floppy(swd, INTERNAL_DRIVE);
0802     swim_drive(base, EXTERNAL_DRIVE);
0803     if (swim_readbit(base, DRIVE_PRESENT) &&
0804         !swim_readbit(base, ONEMEG_DRIVE))
0805         swim_add_floppy(swd, EXTERNAL_DRIVE);
0806
0807     /* register floppy drives */
0808
0809     err = register_blkdev(FLOPPY_MAJOR, "fd");
0810     if (err) {
0811         printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
0812                FLOPPY_MAJOR);
0813         return -EBUSY;
0814     }
0815
0816     spin_lock_init(&swd->lock);
0817
0818     for (drive = 0; drive < swd->floppy_count; drive++) {
0819         err = blk_mq_alloc_sq_tag_set(&swd->unit[drive].tag_set,
0820                 &swim_mq_ops, 2, BLK_MQ_F_SHOULD_MERGE);
0821         if (err)
0822             goto exit_put_disks;
0823
0824         swd->unit[drive].disk =
0825             blk_mq_alloc_disk(&swd->unit[drive].tag_set,
0826                       &swd->unit[drive]);
0827         if (IS_ERR(swd->unit[drive].disk)) {
0828             blk_mq_free_tag_set(&swd->unit[drive].tag_set);
0829             err = PTR_ERR(swd->unit[drive].disk);
0830             goto exit_put_disks;
0831         }
0832
0833         swd->unit[drive].swd = swd;
0834     }
0835
0836     for (drive = 0; drive < swd->floppy_count; drive++) {
0837         swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
0838         swd->unit[drive].disk->major = FLOPPY_MAJOR;
0839         swd->unit[drive].disk->first_minor = drive;
0840         swd->unit[drive].disk->minors = 1;
0841         sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
0842         swd->unit[drive].disk->fops = &floppy_fops;
0843         swd->unit[drive].disk->flags |= GENHD_FL_NO_PART;
0844         swd->unit[drive].disk->events = DISK_EVENT_MEDIA_CHANGE;
0845         swd->unit[drive].disk->private_data = &swd->unit[drive];
0846         set_capacity(swd->unit[drive].disk, 2880);
0847         err = add_disk(swd->unit[drive].disk);
0848         if (err)
0849             goto exit_put_disks;
0850         swd->unit[drive].registered = true;
0851     }
0852
0853     return 0;
0854
0855 exit_put_disks:
0856     unregister_blkdev(FLOPPY_MAJOR, "fd");
0857     do {
0858         swim_cleanup_floppy_disk(&swd->unit[drive]);
0859     } while (drive--);
0860     return err;
0861 }
0862
0863 static int swim_probe(struct platform_device *dev)
0864 {
0865     struct resource *res;
0866     struct swim __iomem *swim_base;
0867     struct swim_priv *swd;
0868     int ret;
0869
0870     res = platform_get_resource(dev, IORESOURCE_MEM, 0);
0871     if (!res) {
0872         ret = -ENODEV;
0873         goto out;
0874     }
0875
0876     if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
0877         ret = -EBUSY;
0878         goto out;
0879     }
0880
0881     swim_base = (struct swim __iomem *)res->start;
0882     if (!swim_base) {
0883         ret = -ENOMEM;
0884         goto out_release_io;
0885     }
0886
0887     /* probe device */
0888
0889     set_swim_mode(swim_base, 1);
0890     if (!get_swim_mode(swim_base)) {
0891         printk(KERN_INFO "SWIM device not found !\n");
0892         ret = -ENODEV;
0893         goto out_release_io;
0894     }
0895
0896     /* set platform driver data */
0897
0898     swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL);
0899     if (!swd) {
0900         ret = -ENOMEM;
0901         goto out_release_io;
0902     }
0903     platform_set_drvdata(dev, swd);
0904
0905     swd->base = swim_base;
0906
0907     ret = swim_floppy_init(swd);
0908     if (ret)
0909         goto out_kfree;
0910
0911     return 0;
0912
0913 out_kfree:
0914     kfree(swd);
0915 out_release_io:
0916     release_mem_region(res->start, resource_size(res));
0917 out:
0918     return ret;
0919 }
0920
0921 static int swim_remove(struct platform_device *dev)
0922 {
0923     struct swim_priv *swd = platform_get_drvdata(dev);
0924     int drive;
0925     struct resource *res;
0926
0927     for (drive = 0; drive < swd->floppy_count; drive++)
0928         swim_cleanup_floppy_disk(&swd->unit[drive]);
0929
0930     unregister_blkdev(FLOPPY_MAJOR, "fd");
0931
0932     /* eject floppies */
0933
0934     for (drive = 0; drive < swd->floppy_count; drive++)
0935         floppy_eject(&swd->unit[drive]);
0936
0937     res = platform_get_resource(dev, IORESOURCE_MEM, 0);
0938     if (res)
0939         release_mem_region(res->start, resource_size(res));
0940
0941     kfree(swd);
0942
0943     return 0;
0944 }
0945
0946 static struct platform_driver swim_driver = {
0947     .probe  = swim_probe,
0948     .remove = swim_remove,
0949     .driver   = {
0950         .name   = CARDNAME,
0951     },
0952 };
0953
0954 static int __init swim_init(void)
0955 {
0956     printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
0957
0958     return platform_driver_register(&swim_driver);
0959 }
0960 module_init(swim_init);
0961
0962 static void __exit swim_exit(void)
0963 {
0964     platform_driver_unregister(&swim_driver);
0965 }
0966 module_exit(swim_exit);
0967
0968 MODULE_DESCRIPTION("Driver for SWIM floppy controller");
0969 MODULE_LICENSE("GPL");
0970 MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>");
0971 MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);