linux/drivers/block/swim.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-or-later
   2/*
   3 * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
   4 *
   5 * Copyright (C) 2004,2008 Laurent Vivier <Laurent@lvivier.info>
   6 *
   7 * based on Alastair Bridgewater SWIM analysis, 2001
   8 * based on SWIM3 driver (c) Paul Mackerras, 1996
   9 * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
  10 *
  11 * 2004-08-21 (lv) - Initial implementation
  12 * 2008-10-30 (lv) - Port to 2.6
  13 */
  14
  15#include <linux/module.h>
  16#include <linux/fd.h>
  17#include <linux/slab.h>
  18#include <linux/blk-mq.h>
  19#include <linux/mutex.h>
  20#include <linux/hdreg.h>
  21#include <linux/kernel.h>
  22#include <linux/delay.h>
  23#include <linux/platform_device.h>
  24
  25#include <asm/mac_via.h>
  26
  27#define CARDNAME "swim"
  28
  29struct sector_header {
  30        unsigned char side;
  31        unsigned char track;
  32        unsigned char sector;
  33        unsigned char size;
  34        unsigned char crc0;
  35        unsigned char crc1;
  36} __attribute__((packed));
  37
  38#define DRIVER_VERSION "Version 0.2 (2008-10-30)"
  39
  40#define REG(x)  unsigned char x, x ## _pad[0x200 - 1];
  41
  42struct swim {
  43        REG(write_data)
  44        REG(write_mark)
  45        REG(write_CRC)
  46        REG(write_parameter)
  47        REG(write_phase)
  48        REG(write_setup)
  49        REG(write_mode0)
  50        REG(write_mode1)
  51
  52        REG(read_data)
  53        REG(read_mark)
  54        REG(read_error)
  55        REG(read_parameter)
  56        REG(read_phase)
  57        REG(read_setup)
  58        REG(read_status)
  59        REG(read_handshake)
  60} __attribute__((packed));
  61
  62#define swim_write(base, reg, v)        out_8(&(base)->write_##reg, (v))
  63#define swim_read(base, reg)            in_8(&(base)->read_##reg)
  64
  65/* IWM registers */
  66
  67struct iwm {
  68        REG(ph0L)
  69        REG(ph0H)
  70        REG(ph1L)
  71        REG(ph1H)
  72        REG(ph2L)
  73        REG(ph2H)
  74        REG(ph3L)
  75        REG(ph3H)
  76        REG(mtrOff)
  77        REG(mtrOn)
  78        REG(intDrive)
  79        REG(extDrive)
  80        REG(q6L)
  81        REG(q6H)
  82        REG(q7L)
  83        REG(q7H)
  84} __attribute__((packed));
  85
  86#define iwm_write(base, reg, v)         out_8(&(base)->reg, (v))
  87#define iwm_read(base, reg)             in_8(&(base)->reg)
  88
  89/* bits in phase register */
  90
  91#define SEEK_POSITIVE   0x070
  92#define SEEK_NEGATIVE   0x074
  93#define STEP            0x071
  94#define MOTOR_ON        0x072
  95#define MOTOR_OFF       0x076
  96#define INDEX           0x073
  97#define EJECT           0x077
  98#define SETMFM          0x171
  99#define SETGCR          0x175
 100
 101#define RELAX           0x033
 102#define LSTRB           0x008
 103
 104#define CA_MASK         0x077
 105
 106/* Select values for swim_select and swim_readbit */
 107
 108#define READ_DATA_0     0x074
 109#define ONEMEG_DRIVE    0x075
 110#define SINGLE_SIDED    0x076
 111#define DRIVE_PRESENT   0x077
 112#define DISK_IN         0x170
 113#define WRITE_PROT      0x171
 114#define TRACK_ZERO      0x172
 115#define TACHO           0x173
 116#define READ_DATA_1     0x174
 117#define GCR_MODE        0x175
 118#define SEEK_COMPLETE   0x176
 119#define TWOMEG_MEDIA    0x177
 120
 121/* Bits in handshake register */
 122
 123#define MARK_BYTE       0x01
 124#define CRC_ZERO        0x02
 125#define RDDATA          0x04
 126#define SENSE           0x08
 127#define MOTEN           0x10
 128#define ERROR           0x20
 129#define DAT2BYTE        0x40
 130#define DAT1BYTE        0x80
 131
 132/* bits in setup register */
 133
 134#define S_INV_WDATA     0x01
 135#define S_3_5_SELECT    0x02
 136#define S_GCR           0x04
 137#define S_FCLK_DIV2     0x08
 138#define S_ERROR_CORR    0x10
 139#define S_IBM_DRIVE     0x20
 140#define S_GCR_WRITE     0x40
 141#define S_TIMEOUT       0x80
 142
 143/* bits in mode register */
 144
 145#define CLFIFO          0x01
 146#define ENBL1           0x02
 147#define ENBL2           0x04
 148#define ACTION          0x08
 149#define WRITE_MODE      0x10
 150#define HEDSEL          0x20
 151#define MOTON           0x80
 152
 153/*----------------------------------------------------------------------------*/
 154
 155enum drive_location {
 156        INTERNAL_DRIVE = 0x02,
 157        EXTERNAL_DRIVE = 0x04,
 158};
 159
 160enum media_type {
 161        DD_MEDIA,
 162        HD_MEDIA,
 163};
 164
 165struct floppy_state {
 166
 167        /* physical properties */
 168
 169        enum drive_location location;   /* internal or external drive */
 170        int              head_number;   /* single- or double-sided drive */
 171
 172        /* media */
 173
 174        int              disk_in;
 175        int              ejected;
 176        enum media_type  type;
 177        int              write_protected;
 178
 179        int              total_secs;
 180        int              secpercyl;
 181        int              secpertrack;
 182
 183        /* in-use information */
 184
 185        int             track;
 186        int             ref_count;
 187
 188        struct gendisk *disk;
 189        struct blk_mq_tag_set tag_set;
 190
 191        /* parent controller */
 192
 193        struct swim_priv *swd;
 194};
 195
 196enum motor_action {
 197        OFF,
 198        ON,
 199};
 200
 201enum head {
 202        LOWER_HEAD = 0,
 203        UPPER_HEAD = 1,
 204};
 205
 206#define FD_MAX_UNIT     2
 207
 208struct swim_priv {
 209        struct swim __iomem *base;
 210        spinlock_t lock;
 211        int floppy_count;
 212        struct floppy_state unit[FD_MAX_UNIT];
 213};
 214
 215extern int swim_read_sector_header(struct swim __iomem *base,
 216                                   struct sector_header *header);
 217extern int swim_read_sector_data(struct swim __iomem *base,
 218                                 unsigned char *data);
 219
 220static DEFINE_MUTEX(swim_mutex);
 221static inline void set_swim_mode(struct swim __iomem *base, int enable)
 222{
 223        struct iwm __iomem *iwm_base;
 224        unsigned long flags;
 225
 226        if (!enable) {
 227                swim_write(base, mode0, 0xf8);
 228                return;
 229        }
 230
 231        iwm_base = (struct iwm __iomem *)base;
 232        local_irq_save(flags);
 233
 234        iwm_read(iwm_base, q7L);
 235        iwm_read(iwm_base, mtrOff);
 236        iwm_read(iwm_base, q6H);
 237
 238        iwm_write(iwm_base, q7H, 0x57);
 239        iwm_write(iwm_base, q7H, 0x17);
 240        iwm_write(iwm_base, q7H, 0x57);
 241        iwm_write(iwm_base, q7H, 0x57);
 242
 243        local_irq_restore(flags);
 244}
 245
 246static inline int get_swim_mode(struct swim __iomem *base)
 247{
 248        unsigned long flags;
 249
 250        local_irq_save(flags);
 251
 252        swim_write(base, phase, 0xf5);
 253        if (swim_read(base, phase) != 0xf5)
 254                goto is_iwm;
 255        swim_write(base, phase, 0xf6);
 256        if (swim_read(base, phase) != 0xf6)
 257                goto is_iwm;
 258        swim_write(base, phase, 0xf7);
 259        if (swim_read(base, phase) != 0xf7)
 260                goto is_iwm;
 261        local_irq_restore(flags);
 262        return 1;
 263is_iwm:
 264        local_irq_restore(flags);
 265        return 0;
 266}
 267
 268static inline void swim_select(struct swim __iomem *base, int sel)
 269{
 270        swim_write(base, phase, RELAX);
 271
 272        via1_set_head(sel & 0x100);
 273
 274        swim_write(base, phase, sel & CA_MASK);
 275}
 276
 277static inline void swim_action(struct swim __iomem *base, int action)
 278{
 279        unsigned long flags;
 280
 281        local_irq_save(flags);
 282
 283        swim_select(base, action);
 284        udelay(1);
 285        swim_write(base, phase, (LSTRB<<4) | LSTRB);
 286        udelay(1);
 287        swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F));
 288        udelay(1);
 289
 290        local_irq_restore(flags);
 291}
 292
 293static inline int swim_readbit(struct swim __iomem *base, int bit)
 294{
 295        int stat;
 296
 297        swim_select(base, bit);
 298
 299        udelay(10);
 300
 301        stat = swim_read(base, handshake);
 302
 303        return (stat & SENSE) == 0;
 304}
 305
 306static inline void swim_drive(struct swim __iomem *base,
 307                              enum drive_location location)
 308{
 309        if (location == INTERNAL_DRIVE) {
 310                swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
 311                swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
 312        } else if (location == EXTERNAL_DRIVE) {
 313                swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
 314                swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
 315        }
 316}
 317
 318static inline void swim_motor(struct swim __iomem *base,
 319                              enum motor_action action)
 320{
 321        if (action == ON) {
 322                int i;
 323
 324                swim_action(base, MOTOR_ON);
 325
 326                for (i = 0; i < 2*HZ; i++) {
 327                        swim_select(base, RELAX);
 328                        if (swim_readbit(base, MOTOR_ON))
 329                                break;
 330                        set_current_state(TASK_INTERRUPTIBLE);
 331                        schedule_timeout(1);
 332                }
 333        } else if (action == OFF) {
 334                swim_action(base, MOTOR_OFF);
 335                swim_select(base, RELAX);
 336        }
 337}
 338
 339static inline void swim_eject(struct swim __iomem *base)
 340{
 341        int i;
 342
 343        swim_action(base, EJECT);
 344
 345        for (i = 0; i < 2*HZ; i++) {
 346                swim_select(base, RELAX);
 347                if (!swim_readbit(base, DISK_IN))
 348                        break;
 349                set_current_state(TASK_INTERRUPTIBLE);
 350                schedule_timeout(1);
 351        }
 352        swim_select(base, RELAX);
 353}
 354
 355static inline void swim_head(struct swim __iomem *base, enum head head)
 356{
 357        /* wait drive is ready */
 358
 359        if (head == UPPER_HEAD)
 360                swim_select(base, READ_DATA_1);
 361        else if (head == LOWER_HEAD)
 362                swim_select(base, READ_DATA_0);
 363}
 364
 365static inline int swim_step(struct swim __iomem *base)
 366{
 367        int wait;
 368
 369        swim_action(base, STEP);
 370
 371        for (wait = 0; wait < HZ; wait++) {
 372
 373                set_current_state(TASK_INTERRUPTIBLE);
 374                schedule_timeout(1);
 375
 376                swim_select(base, RELAX);
 377                if (!swim_readbit(base, STEP))
 378                        return 0;
 379        }
 380        return -1;
 381}
 382
 383static inline int swim_track00(struct swim __iomem *base)
 384{
 385        int try;
 386
 387        swim_action(base, SEEK_NEGATIVE);
 388
 389        for (try = 0; try < 100; try++) {
 390
 391                swim_select(base, RELAX);
 392                if (swim_readbit(base, TRACK_ZERO))
 393                        break;
 394
 395                if (swim_step(base))
 396                        return -1;
 397        }
 398
 399        if (swim_readbit(base, TRACK_ZERO))
 400                return 0;
 401
 402        return -1;
 403}
 404
 405static inline int swim_seek(struct swim __iomem *base, int step)
 406{
 407        if (step == 0)
 408                return 0;
 409
 410        if (step < 0) {
 411                swim_action(base, SEEK_NEGATIVE);
 412                step = -step;
 413        } else
 414                swim_action(base, SEEK_POSITIVE);
 415
 416        for ( ; step > 0; step--) {
 417                if (swim_step(base))
 418                        return -1;
 419        }
 420
 421        return 0;
 422}
 423
 424static inline int swim_track(struct floppy_state *fs,  int track)
 425{
 426        struct swim __iomem *base = fs->swd->base;
 427        int ret;
 428
 429        ret = swim_seek(base, track - fs->track);
 430
 431        if (ret == 0)
 432                fs->track = track;
 433        else {
 434                swim_track00(base);
 435                fs->track = 0;
 436        }
 437
 438        return ret;
 439}
 440
 441static int floppy_eject(struct floppy_state *fs)
 442{
 443        struct swim __iomem *base = fs->swd->base;
 444
 445        swim_drive(base, fs->location);
 446        swim_motor(base, OFF);
 447        swim_eject(base);
 448
 449        fs->disk_in = 0;
 450        fs->ejected = 1;
 451
 452        return 0;
 453}
 454
 455static inline int swim_read_sector(struct floppy_state *fs,
 456                                   int side, int track,
 457                                   int sector, unsigned char *buffer)
 458{
 459        struct swim __iomem *base = fs->swd->base;
 460        unsigned long flags;
 461        struct sector_header header;
 462        int ret = -1;
 463        short i;
 464
 465        swim_track(fs, track);
 466
 467        swim_write(base, mode1, MOTON);
 468        swim_head(base, side);
 469        swim_write(base, mode0, side);
 470
 471        local_irq_save(flags);
 472        for (i = 0; i < 36; i++) {
 473                ret = swim_read_sector_header(base, &header);
 474                if (!ret && (header.sector == sector)) {
 475                        /* found */
 476
 477                        ret = swim_read_sector_data(base, buffer);
 478                        break;
 479                }
 480        }
 481        local_irq_restore(flags);
 482
 483        swim_write(base, mode0, MOTON);
 484
 485        if ((header.side != side)  || (header.track != track) ||
 486             (header.sector != sector))
 487                return 0;
 488
 489        return ret;
 490}
 491
 492static blk_status_t floppy_read_sectors(struct floppy_state *fs,
 493                               int req_sector, int sectors_nb,
 494                               unsigned char *buffer)
 495{
 496        struct swim __iomem *base = fs->swd->base;
 497        int ret;
 498        int side, track, sector;
 499        int i, try;
 500
 501
 502        swim_drive(base, fs->location);
 503        for (i = req_sector; i < req_sector + sectors_nb; i++) {
 504                int x;
 505                track = i / fs->secpercyl;
 506                x = i % fs->secpercyl;
 507                side = x / fs->secpertrack;
 508                sector = x % fs->secpertrack + 1;
 509
 510                try = 5;
 511                do {
 512                        ret = swim_read_sector(fs, side, track, sector,
 513                                                buffer);
 514                        if (try-- == 0)
 515                                return BLK_STS_IOERR;
 516                } while (ret != 512);
 517
 518                buffer += ret;
 519        }
 520
 521        return 0;
 522}
 523
 524static blk_status_t swim_queue_rq(struct blk_mq_hw_ctx *hctx,
 525                                  const struct blk_mq_queue_data *bd)
 526{
 527        struct floppy_state *fs = hctx->queue->queuedata;
 528        struct swim_priv *swd = fs->swd;
 529        struct request *req = bd->rq;
 530        blk_status_t err;
 531
 532        if (!spin_trylock_irq(&swd->lock))
 533                return BLK_STS_DEV_RESOURCE;
 534
 535        blk_mq_start_request(req);
 536
 537        if (!fs->disk_in || rq_data_dir(req) == WRITE) {
 538                err = BLK_STS_IOERR;
 539                goto out;
 540        }
 541
 542        do {
 543                err = floppy_read_sectors(fs, blk_rq_pos(req),
 544                                          blk_rq_cur_sectors(req),
 545                                          bio_data(req->bio));
 546        } while (blk_update_request(req, err, blk_rq_cur_bytes(req)));
 547        __blk_mq_end_request(req, err);
 548
 549        err = BLK_STS_OK;
 550out:
 551        spin_unlock_irq(&swd->lock);
 552        return err;
 553
 554}
 555
 556static struct floppy_struct floppy_type[4] = {
 557        {    0,  0, 0,  0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing   */
 558        {  720,  9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
 559        { 1440,  9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5"   */
 560        { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5"  */
 561};
 562
 563static int get_floppy_geometry(struct floppy_state *fs, int type,
 564                               struct floppy_struct **g)
 565{
 566        if (type >= ARRAY_SIZE(floppy_type))
 567                return -EINVAL;
 568
 569        if (type)
 570                *g = &floppy_type[type];
 571        else if (fs->type == HD_MEDIA) /* High-Density media */
 572                *g = &floppy_type[3];
 573        else if (fs->head_number == 2) /* double-sided */
 574                *g = &floppy_type[2];
 575        else
 576                *g = &floppy_type[1];
 577
 578        return 0;
 579}
 580
 581static void setup_medium(struct floppy_state *fs)
 582{
 583        struct swim __iomem *base = fs->swd->base;
 584
 585        if (swim_readbit(base, DISK_IN)) {
 586                struct floppy_struct *g;
 587                fs->disk_in = 1;
 588                fs->write_protected = swim_readbit(base, WRITE_PROT);
 589
 590                if (swim_track00(base))
 591                        printk(KERN_ERR
 592                                "SWIM: cannot move floppy head to track 0\n");
 593
 594                swim_track00(base);
 595
 596                fs->type = swim_readbit(base, TWOMEG_MEDIA) ?
 597                        HD_MEDIA : DD_MEDIA;
 598                fs->head_number = swim_readbit(base, SINGLE_SIDED) ? 1 : 2;
 599                get_floppy_geometry(fs, 0, &g);
 600                fs->total_secs = g->size;
 601                fs->secpercyl = g->head * g->sect;
 602                fs->secpertrack = g->sect;
 603                fs->track = 0;
 604        } else {
 605                fs->disk_in = 0;
 606        }
 607}
 608
 609static int floppy_open(struct block_device *bdev, fmode_t mode)
 610{
 611        struct floppy_state *fs = bdev->bd_disk->private_data;
 612        struct swim __iomem *base = fs->swd->base;
 613        int err;
 614
 615        if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL))
 616                return -EBUSY;
 617
 618        if (mode & FMODE_EXCL)
 619                fs->ref_count = -1;
 620        else
 621                fs->ref_count++;
 622
 623        swim_write(base, setup, S_IBM_DRIVE  | S_FCLK_DIV2);
 624        udelay(10);
 625        swim_drive(base, fs->location);
 626        swim_motor(base, ON);
 627        swim_action(base, SETMFM);
 628        if (fs->ejected)
 629                setup_medium(fs);
 630        if (!fs->disk_in) {
 631                err = -ENXIO;
 632                goto out;
 633        }
 634
 635        set_capacity(fs->disk, fs->total_secs);
 636
 637        if (mode & FMODE_NDELAY)
 638                return 0;
 639
 640        if (mode & (FMODE_READ|FMODE_WRITE)) {
 641                if (bdev_check_media_change(bdev) && fs->disk_in)
 642                        fs->ejected = 0;
 643                if ((mode & FMODE_WRITE) && fs->write_protected) {
 644                        err = -EROFS;
 645                        goto out;
 646                }
 647        }
 648        return 0;
 649out:
 650        if (fs->ref_count < 0)
 651                fs->ref_count = 0;
 652        else if (fs->ref_count > 0)
 653                --fs->ref_count;
 654
 655        if (fs->ref_count == 0)
 656                swim_motor(base, OFF);
 657        return err;
 658}
 659
 660static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
 661{
 662        int ret;
 663
 664        mutex_lock(&swim_mutex);
 665        ret = floppy_open(bdev, mode);
 666        mutex_unlock(&swim_mutex);
 667
 668        return ret;
 669}
 670
 671static void floppy_release(struct gendisk *disk, fmode_t mode)
 672{
 673        struct floppy_state *fs = disk->private_data;
 674        struct swim __iomem *base = fs->swd->base;
 675
 676        mutex_lock(&swim_mutex);
 677        if (fs->ref_count < 0)
 678                fs->ref_count = 0;
 679        else if (fs->ref_count > 0)
 680                --fs->ref_count;
 681
 682        if (fs->ref_count == 0)
 683                swim_motor(base, OFF);
 684        mutex_unlock(&swim_mutex);
 685}
 686
 687static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
 688                        unsigned int cmd, unsigned long param)
 689{
 690        struct floppy_state *fs = bdev->bd_disk->private_data;
 691        int err;
 692
 693        if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
 694                        return -EPERM;
 695
 696        switch (cmd) {
 697        case FDEJECT:
 698                if (fs->ref_count != 1)
 699                        return -EBUSY;
 700                mutex_lock(&swim_mutex);
 701                err = floppy_eject(fs);
 702                mutex_unlock(&swim_mutex);
 703                return err;
 704
 705        case FDGETPRM:
 706                if (copy_to_user((void __user *) param, (void *) &floppy_type,
 707                                 sizeof(struct floppy_struct)))
 708                        return -EFAULT;
 709                return 0;
 710        }
 711        return -ENOTTY;
 712}
 713
 714static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo)
 715{
 716        struct floppy_state *fs = bdev->bd_disk->private_data;
 717        struct floppy_struct *g;
 718        int ret;
 719
 720        ret = get_floppy_geometry(fs, 0, &g);
 721        if (ret)
 722                return ret;
 723
 724        geo->heads = g->head;
 725        geo->sectors = g->sect;
 726        geo->cylinders = g->track;
 727
 728        return 0;
 729}
 730
 731static unsigned int floppy_check_events(struct gendisk *disk,
 732                                        unsigned int clearing)
 733{
 734        struct floppy_state *fs = disk->private_data;
 735
 736        return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
 737}
 738
 739static const struct block_device_operations floppy_fops = {
 740        .owner           = THIS_MODULE,
 741        .open            = floppy_unlocked_open,
 742        .release         = floppy_release,
 743        .ioctl           = floppy_ioctl,
 744        .getgeo          = floppy_getgeo,
 745        .check_events    = floppy_check_events,
 746};
 747
 748static struct kobject *floppy_find(dev_t dev, int *part, void *data)
 749{
 750        struct swim_priv *swd = data;
 751        int drive = (*part & 3);
 752
 753        if (drive >= swd->floppy_count)
 754                return NULL;
 755
 756        *part = 0;
 757        return get_disk_and_module(swd->unit[drive].disk);
 758}
 759
 760static int swim_add_floppy(struct swim_priv *swd, enum drive_location location)
 761{
 762        struct floppy_state *fs = &swd->unit[swd->floppy_count];
 763        struct swim __iomem *base = swd->base;
 764
 765        fs->location = location;
 766
 767        swim_drive(base, location);
 768
 769        swim_motor(base, OFF);
 770
 771        fs->type = HD_MEDIA;
 772        fs->head_number = 2;
 773
 774        fs->ref_count = 0;
 775        fs->ejected = 1;
 776
 777        swd->floppy_count++;
 778
 779        return 0;
 780}
 781
 782static const struct blk_mq_ops swim_mq_ops = {
 783        .queue_rq = swim_queue_rq,
 784};
 785
 786static int swim_floppy_init(struct swim_priv *swd)
 787{
 788        int err;
 789        int drive;
 790        struct swim __iomem *base = swd->base;
 791
 792        /* scan floppy drives */
 793
 794        swim_drive(base, INTERNAL_DRIVE);
 795        if (swim_readbit(base, DRIVE_PRESENT) &&
 796            !swim_readbit(base, ONEMEG_DRIVE))
 797                swim_add_floppy(swd, INTERNAL_DRIVE);
 798        swim_drive(base, EXTERNAL_DRIVE);
 799        if (swim_readbit(base, DRIVE_PRESENT) &&
 800            !swim_readbit(base, ONEMEG_DRIVE))
 801                swim_add_floppy(swd, EXTERNAL_DRIVE);
 802
 803        /* register floppy drives */
 804
 805        err = register_blkdev(FLOPPY_MAJOR, "fd");
 806        if (err) {
 807                printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
 808                       FLOPPY_MAJOR);
 809                return -EBUSY;
 810        }
 811
 812        spin_lock_init(&swd->lock);
 813
 814        for (drive = 0; drive < swd->floppy_count; drive++) {
 815                struct request_queue *q;
 816
 817                swd->unit[drive].disk = alloc_disk(1);
 818                if (swd->unit[drive].disk == NULL) {
 819                        err = -ENOMEM;
 820                        goto exit_put_disks;
 821                }
 822
 823                q = blk_mq_init_sq_queue(&swd->unit[drive].tag_set, &swim_mq_ops,
 824                                                2, BLK_MQ_F_SHOULD_MERGE);
 825                if (IS_ERR(q)) {
 826                        err = PTR_ERR(q);
 827                        goto exit_put_disks;
 828                }
 829
 830                swd->unit[drive].disk->queue = q;
 831                blk_queue_bounce_limit(swd->unit[drive].disk->queue,
 832                                BLK_BOUNCE_HIGH);
 833                swd->unit[drive].disk->queue->queuedata = &swd->unit[drive];
 834                swd->unit[drive].swd = swd;
 835        }
 836
 837        for (drive = 0; drive < swd->floppy_count; drive++) {
 838                swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
 839                swd->unit[drive].disk->major = FLOPPY_MAJOR;
 840                swd->unit[drive].disk->first_minor = drive;
 841                sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
 842                swd->unit[drive].disk->fops = &floppy_fops;
 843                swd->unit[drive].disk->events = DISK_EVENT_MEDIA_CHANGE;
 844                swd->unit[drive].disk->private_data = &swd->unit[drive];
 845                set_capacity(swd->unit[drive].disk, 2880);
 846                add_disk(swd->unit[drive].disk);
 847        }
 848
 849        blk_register_region(MKDEV(FLOPPY_MAJOR, 0), 256, THIS_MODULE,
 850                            floppy_find, NULL, swd);
 851
 852        return 0;
 853
 854exit_put_disks:
 855        unregister_blkdev(FLOPPY_MAJOR, "fd");
 856        do {
 857                struct gendisk *disk = swd->unit[drive].disk;
 858
 859                if (disk) {
 860                        if (disk->queue) {
 861                                blk_cleanup_queue(disk->queue);
 862                                disk->queue = NULL;
 863                        }
 864                        blk_mq_free_tag_set(&swd->unit[drive].tag_set);
 865                        put_disk(disk);
 866                }
 867        } while (drive--);
 868        return err;
 869}
 870
 871static int swim_probe(struct platform_device *dev)
 872{
 873        struct resource *res;
 874        struct swim __iomem *swim_base;
 875        struct swim_priv *swd;
 876        int ret;
 877
 878        res = platform_get_resource(dev, IORESOURCE_MEM, 0);
 879        if (!res) {
 880                ret = -ENODEV;
 881                goto out;
 882        }
 883
 884        if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
 885                ret = -EBUSY;
 886                goto out;
 887        }
 888
 889        swim_base = (struct swim __iomem *)res->start;
 890        if (!swim_base) {
 891                ret = -ENOMEM;
 892                goto out_release_io;
 893        }
 894
 895        /* probe device */
 896
 897        set_swim_mode(swim_base, 1);
 898        if (!get_swim_mode(swim_base)) {
 899                printk(KERN_INFO "SWIM device not found !\n");
 900                ret = -ENODEV;
 901                goto out_release_io;
 902        }
 903
 904        /* set platform driver data */
 905
 906        swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL);
 907        if (!swd) {
 908                ret = -ENOMEM;
 909                goto out_release_io;
 910        }
 911        platform_set_drvdata(dev, swd);
 912
 913        swd->base = swim_base;
 914
 915        ret = swim_floppy_init(swd);
 916        if (ret)
 917                goto out_kfree;
 918
 919        return 0;
 920
 921out_kfree:
 922        kfree(swd);
 923out_release_io:
 924        release_mem_region(res->start, resource_size(res));
 925out:
 926        return ret;
 927}
 928
 929static int swim_remove(struct platform_device *dev)
 930{
 931        struct swim_priv *swd = platform_get_drvdata(dev);
 932        int drive;
 933        struct resource *res;
 934
 935        blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
 936
 937        for (drive = 0; drive < swd->floppy_count; drive++) {
 938                del_gendisk(swd->unit[drive].disk);
 939                blk_cleanup_queue(swd->unit[drive].disk->queue);
 940                blk_mq_free_tag_set(&swd->unit[drive].tag_set);
 941                put_disk(swd->unit[drive].disk);
 942        }
 943
 944        unregister_blkdev(FLOPPY_MAJOR, "fd");
 945
 946        /* eject floppies */
 947
 948        for (drive = 0; drive < swd->floppy_count; drive++)
 949                floppy_eject(&swd->unit[drive]);
 950
 951        res = platform_get_resource(dev, IORESOURCE_MEM, 0);
 952        if (res)
 953                release_mem_region(res->start, resource_size(res));
 954
 955        kfree(swd);
 956
 957        return 0;
 958}
 959
 960static struct platform_driver swim_driver = {
 961        .probe  = swim_probe,
 962        .remove = swim_remove,
 963        .driver   = {
 964                .name   = CARDNAME,
 965        },
 966};
 967
 968static int __init swim_init(void)
 969{
 970        printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
 971
 972        return platform_driver_register(&swim_driver);
 973}
 974module_init(swim_init);
 975
 976static void __exit swim_exit(void)
 977{
 978        platform_driver_unregister(&swim_driver);
 979}
 980module_exit(swim_exit);
 981
 982MODULE_DESCRIPTION("Driver for SWIM floppy controller");
 983MODULE_LICENSE("GPL");
 984MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>");
 985MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);
 986