linux/drivers/block/ataflop.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 *  drivers/block/ataflop.c
   4 *
   5 *  Copyright (C) 1993  Greg Harp
   6 *  Atari Support by Bjoern Brauel, Roman Hodek
   7 *
   8 *  Big cleanup Sep 11..14 1994 Roman Hodek:
   9 *   - Driver now works interrupt driven
  10 *   - Support for two drives; should work, but I cannot test that :-(
  11 *   - Reading is done in whole tracks and buffered to speed up things
  12 *   - Disk change detection and drive deselecting after motor-off
  13 *     similar to TOS
  14 *   - Autodetection of disk format (DD/HD); untested yet, because I
  15 *     don't have an HD drive :-(
  16 *
  17 *  Fixes Nov 13 1994 Martin Schaller:
  18 *   - Autodetection works now
  19 *   - Support for 5 1/4'' disks
  20 *   - Removed drive type (unknown on atari)
  21 *   - Do seeks with 8 Mhz
  22 *
  23 *  Changes by Andreas Schwab:
  24 *   - After errors in multiple read mode try again reading single sectors
  25 *  (Feb 1995):
  26 *   - Clean up error handling
  27 *   - Set blk_size for proper size checking
  28 *   - Initialize track register when testing presence of floppy
  29 *   - Implement some ioctl's
  30 *
  31 *  Changes by Torsten Lang:
  32 *   - When probing the floppies we should add the FDCCMDADD_H flag since
  33 *     the FDC will otherwise wait forever when no disk is inserted...
  34 *
  35 * ++ Freddi Aschwanden (fa) 20.9.95 fixes for medusa:
  36 *  - MFPDELAY() after each FDC access -> atari 
  37 *  - more/other disk formats
  38 *  - DMA to the block buffer directly if we have a 32bit DMA
  39 *  - for medusa, the step rate is always 3ms
  40 *  - on medusa, use only cache_push()
  41 * Roman:
  42 *  - Make disk format numbering independent from minors
  43 *  - Let user set max. supported drive type (speeds up format
  44 *    detection, saves buffer space)
  45 *
  46 * Roman 10/15/95:
  47 *  - implement some more ioctls
  48 *  - disk formatting
  49 *  
  50 * Andreas 95/12/12:
  51 *  - increase gap size at start of track for HD/ED disks
  52 *
  53 * Michael (MSch) 11/07/96:
  54 *  - implemented FDSETPRM and FDDEFPRM ioctl
  55 *
  56 * Andreas (97/03/19):
  57 *  - implemented missing BLK* ioctls
  58 *
  59 *  Things left to do:
  60 *   - Formatting
  61 *   - Maybe a better strategy for disk change detection (does anyone
  62 *     know one?)
  63 */
  64
  65#include <linux/module.h>
  66
  67#include <linux/fd.h>
  68#include <linux/delay.h>
  69#include <linux/init.h>
  70#include <linux/blk-mq.h>
  71#include <linux/mutex.h>
  72#include <linux/completion.h>
  73#include <linux/wait.h>
  74
  75#include <asm/atariints.h>
  76#include <asm/atari_stdma.h>
  77#include <asm/atari_stram.h>
  78
  79#define FD_MAX_UNITS 2
  80
  81#undef DEBUG
  82
  83static DEFINE_MUTEX(ataflop_mutex);
  84static struct request *fd_request;
  85
  86/*
  87 * WD1772 stuff
  88 */
  89
  90/* register codes */
  91
  92#define FDCSELREG_STP   (0x80)   /* command/status register */
  93#define FDCSELREG_TRA   (0x82)   /* track register */
  94#define FDCSELREG_SEC   (0x84)   /* sector register */
  95#define FDCSELREG_DTA   (0x86)   /* data register */
  96
  97/* register names for FDC_READ/WRITE macros */
  98
  99#define FDCREG_CMD              0
 100#define FDCREG_STATUS   0
 101#define FDCREG_TRACK    2
 102#define FDCREG_SECTOR   4
 103#define FDCREG_DATA             6
 104
 105/* command opcodes */
 106
 107#define FDCCMD_RESTORE  (0x00)   /*  -                   */
 108#define FDCCMD_SEEK     (0x10)   /*   |                  */
 109#define FDCCMD_STEP     (0x20)   /*   |  TYP 1 Commands  */
 110#define FDCCMD_STIN     (0x40)   /*   |                  */
 111#define FDCCMD_STOT     (0x60)   /*  -                   */
 112#define FDCCMD_RDSEC    (0x80)   /*  -   TYP 2 Commands  */
 113#define FDCCMD_WRSEC    (0xa0)   /*  -          "        */
 114#define FDCCMD_RDADR    (0xc0)   /*  -                   */
 115#define FDCCMD_RDTRA    (0xe0)   /*   |  TYP 3 Commands  */
 116#define FDCCMD_WRTRA    (0xf0)   /*  -                   */
 117#define FDCCMD_FORCI    (0xd0)   /*  -   TYP 4 Command   */
 118
 119/* command modifier bits */
 120
 121#define FDCCMDADD_SR6   (0x00)   /* step rate settings */
 122#define FDCCMDADD_SR12  (0x01)
 123#define FDCCMDADD_SR2   (0x02)
 124#define FDCCMDADD_SR3   (0x03)
 125#define FDCCMDADD_V     (0x04)   /* verify */
 126#define FDCCMDADD_H     (0x08)   /* wait for spin-up */
 127#define FDCCMDADD_U     (0x10)   /* update track register */
 128#define FDCCMDADD_M     (0x10)   /* multiple sector access */
 129#define FDCCMDADD_E     (0x04)   /* head settling flag */
 130#define FDCCMDADD_P     (0x02)   /* precompensation off */
 131#define FDCCMDADD_A0    (0x01)   /* DAM flag */
 132
 133/* status register bits */
 134
 135#define FDCSTAT_MOTORON (0x80)   /* motor on */
 136#define FDCSTAT_WPROT   (0x40)   /* write protected (FDCCMD_WR*) */
 137#define FDCSTAT_SPINUP  (0x20)   /* motor speed stable (Type I) */
 138#define FDCSTAT_DELDAM  (0x20)   /* sector has deleted DAM (Type II+III) */
 139#define FDCSTAT_RECNF   (0x10)   /* record not found */
 140#define FDCSTAT_CRC             (0x08)   /* CRC error */
 141#define FDCSTAT_TR00    (0x04)   /* Track 00 flag (Type I) */
 142#define FDCSTAT_LOST    (0x04)   /* Lost Data (Type II+III) */
 143#define FDCSTAT_IDX             (0x02)   /* Index status (Type I) */
 144#define FDCSTAT_DRQ             (0x02)   /* DRQ status (Type II+III) */
 145#define FDCSTAT_BUSY    (0x01)   /* FDC is busy */
 146
 147
 148/* PSG Port A Bit Nr 0 .. Side Sel .. 0 -> Side 1  1 -> Side 2 */
 149#define DSKSIDE     (0x01)
 150
 151#define DSKDRVNONE  (0x06)
 152#define DSKDRV0     (0x02)
 153#define DSKDRV1     (0x04)
 154
 155/* step rates */
 156#define FDCSTEP_6       0x00
 157#define FDCSTEP_12      0x01
 158#define FDCSTEP_2       0x02
 159#define FDCSTEP_3       0x03
 160
 161struct atari_format_descr {
 162        int track;              /* to be formatted */
 163        int head;               /*   ""     ""     */
 164        int sect_offset;        /* offset of first sector */
 165};
 166
 167/* Disk types: DD, HD, ED */
 168static struct atari_disk_type {
 169        const char      *name;
 170        unsigned        spt;            /* sectors per track */
 171        unsigned        blocks;         /* total number of blocks */
 172        unsigned        fdc_speed;      /* fdc_speed setting */
 173        unsigned        stretch;        /* track doubling ? */
 174} atari_disk_type[] = {
 175        { "d360",  9, 720, 0, 0},       /*  0: 360kB diskette */
 176        { "D360",  9, 720, 0, 1},       /*  1: 360kb in 720k or 1.2MB drive */
 177        { "D720",  9,1440, 0, 0},       /*  2: 720kb in 720k or 1.2MB drive */
 178        { "D820", 10,1640, 0, 0},       /*  3: DD disk with 82 tracks/10 sectors */
 179/* formats above are probed for type DD */
 180#define MAX_TYPE_DD 3
 181        { "h1200",15,2400, 3, 0},       /*  4: 1.2MB diskette */
 182        { "H1440",18,2880, 3, 0},       /*  5: 1.4 MB diskette (HD) */
 183        { "H1640",20,3280, 3, 0},       /*  6: 1.64MB diskette (fat HD) 82 tr 20 sec */
 184/* formats above are probed for types DD and HD */
 185#define MAX_TYPE_HD 6
 186        { "E2880",36,5760, 3, 0},       /*  7: 2.8 MB diskette (ED) */
 187        { "E3280",40,6560, 3, 0},       /*  8: 3.2 MB diskette (fat ED) 82 tr 40 sec */
 188/* formats above are probed for types DD, HD and ED */
 189#define MAX_TYPE_ED 8
 190/* types below are never autoprobed */
 191        { "H1680",21,3360, 3, 0},       /*  9: 1.68MB diskette (fat HD) 80 tr 21 sec */
 192        { "h410",10,820, 0, 1},         /* 10: 410k diskette 41 tr 10 sec, stretch */
 193        { "h1476",18,2952, 3, 0},       /* 11: 1.48MB diskette 82 tr 18 sec */
 194        { "H1722",21,3444, 3, 0},       /* 12: 1.72MB diskette 82 tr 21 sec */
 195        { "h420",10,840, 0, 1},         /* 13: 420k diskette 42 tr 10 sec, stretch */
 196        { "H830",10,1660, 0, 0},        /* 14: 820k diskette 83 tr 10 sec */
 197        { "h1494",18,2952, 3, 0},       /* 15: 1.49MB diskette 83 tr 18 sec */
 198        { "H1743",21,3486, 3, 0},       /* 16: 1.74MB diskette 83 tr 21 sec */
 199        { "h880",11,1760, 0, 0},        /* 17: 880k diskette 80 tr 11 sec */
 200        { "D1040",13,2080, 0, 0},       /* 18: 1.04MB diskette 80 tr 13 sec */
 201        { "D1120",14,2240, 0, 0},       /* 19: 1.12MB diskette 80 tr 14 sec */
 202        { "h1600",20,3200, 3, 0},       /* 20: 1.60MB diskette 80 tr 20 sec */
 203        { "H1760",22,3520, 3, 0},       /* 21: 1.76MB diskette 80 tr 22 sec */
 204        { "H1920",24,3840, 3, 0},       /* 22: 1.92MB diskette 80 tr 24 sec */
 205        { "E3200",40,6400, 3, 0},       /* 23: 3.2MB diskette 80 tr 40 sec */
 206        { "E3520",44,7040, 3, 0},       /* 24: 3.52MB diskette 80 tr 44 sec */
 207        { "E3840",48,7680, 3, 0},       /* 25: 3.84MB diskette 80 tr 48 sec */
 208        { "H1840",23,3680, 3, 0},       /* 26: 1.84MB diskette 80 tr 23 sec */
 209        { "D800",10,1600, 0, 0},        /* 27: 800k diskette 80 tr 10 sec */
 210};
 211
 212static int StartDiskType[] = {
 213        MAX_TYPE_DD,
 214        MAX_TYPE_HD,
 215        MAX_TYPE_ED
 216};
 217
 218#define TYPE_DD         0
 219#define TYPE_HD         1
 220#define TYPE_ED         2
 221
 222static int DriveType = TYPE_HD;
 223
 224static DEFINE_SPINLOCK(ataflop_lock);
 225
 226/* Array for translating minors into disk formats */
 227static struct {
 228        int      index;
 229        unsigned drive_types;
 230} minor2disktype[] = {
 231        {  0, TYPE_DD },        /*  1: d360 */
 232        {  4, TYPE_HD },        /*  2: h1200 */
 233        {  1, TYPE_DD },        /*  3: D360 */
 234        {  2, TYPE_DD },        /*  4: D720 */
 235        {  1, TYPE_DD },        /*  5: h360 = D360 */
 236        {  2, TYPE_DD },        /*  6: h720 = D720 */
 237        {  5, TYPE_HD },        /*  7: H1440 */
 238        {  7, TYPE_ED },        /*  8: E2880 */
 239/* some PC formats :-) */
 240        {  8, TYPE_ED },        /*  9: E3280    <- was "CompaQ" == E2880 for PC */
 241        {  5, TYPE_HD },        /* 10: h1440 = H1440 */
 242        {  9, TYPE_HD },        /* 11: H1680 */
 243        { 10, TYPE_DD },        /* 12: h410  */
 244        {  3, TYPE_DD },        /* 13: H820     <- == D820, 82x10 */
 245        { 11, TYPE_HD },        /* 14: h1476 */
 246        { 12, TYPE_HD },        /* 15: H1722 */
 247        { 13, TYPE_DD },        /* 16: h420  */
 248        { 14, TYPE_DD },        /* 17: H830  */
 249        { 15, TYPE_HD },        /* 18: h1494 */
 250        { 16, TYPE_HD },        /* 19: H1743 */
 251        { 17, TYPE_DD },        /* 20: h880  */
 252        { 18, TYPE_DD },        /* 21: D1040 */
 253        { 19, TYPE_DD },        /* 22: D1120 */
 254        { 20, TYPE_HD },        /* 23: h1600 */
 255        { 21, TYPE_HD },        /* 24: H1760 */
 256        { 22, TYPE_HD },        /* 25: H1920 */
 257        { 23, TYPE_ED },        /* 26: E3200 */
 258        { 24, TYPE_ED },        /* 27: E3520 */
 259        { 25, TYPE_ED },        /* 28: E3840 */
 260        { 26, TYPE_HD },        /* 29: H1840 */
 261        { 27, TYPE_DD },        /* 30: D800  */
 262        {  6, TYPE_HD },        /* 31: H1640    <- was H1600 == h1600 for PC */
 263};
 264
 265#define NUM_DISK_MINORS ARRAY_SIZE(minor2disktype)
 266
 267/*
 268 * Maximum disk size (in kilobytes). This default is used whenever the
 269 * current disk size is unknown.
 270 */
 271#define MAX_DISK_SIZE 3280
 272
 273/*
 274 * MSch: User-provided type information. 'drive' points to
 275 * the respective entry of this array. Set by FDSETPRM ioctls.
 276 */
 277static struct atari_disk_type user_params[FD_MAX_UNITS];
 278
 279/*
 280 * User-provided permanent type information. 'drive' points to
 281 * the respective entry of this array.  Set by FDDEFPRM ioctls, 
 282 * restored upon disk change by floppy_revalidate() if valid (as seen by
 283 * default_params[].blocks > 0 - a bit in unit[].flags might be used for this?)
 284 */
 285static struct atari_disk_type default_params[FD_MAX_UNITS];
 286
 287/* current info on each unit */
 288static struct atari_floppy_struct {
 289        int connected;                          /* !=0 : drive is connected */
 290        int autoprobe;                          /* !=0 : do autoprobe       */
 291
 292        struct atari_disk_type  *disktype;      /* current type of disk */
 293
 294        int track;              /* current head position or -1 if
 295                                   unknown */
 296        unsigned int steprate;  /* steprate setting */
 297        unsigned int wpstat;    /* current state of WP signal (for
 298                                   disk change detection) */
 299        int flags;              /* flags */
 300        struct gendisk *disk[NUM_DISK_MINORS];
 301        int ref;
 302        int type;
 303        struct blk_mq_tag_set tag_set;
 304} unit[FD_MAX_UNITS];
 305
 306#define UD      unit[drive]
 307#define UDT     unit[drive].disktype
 308#define SUD     unit[SelectedDrive]
 309#define SUDT    unit[SelectedDrive].disktype
 310
 311
 312#define FDC_READ(reg) ({                        \
 313    /* unsigned long __flags; */                \
 314    unsigned short __val;                       \
 315    /* local_irq_save(__flags); */              \
 316    dma_wd.dma_mode_status = 0x80 | (reg);      \
 317    udelay(25);                                 \
 318    __val = dma_wd.fdc_acces_seccount;          \
 319    MFPDELAY();                                 \
 320    /* local_irq_restore(__flags); */           \
 321    __val & 0xff;                               \
 322})
 323
 324#define FDC_WRITE(reg,val)                      \
 325    do {                                        \
 326        /* unsigned long __flags; */            \
 327        /* local_irq_save(__flags); */          \
 328        dma_wd.dma_mode_status = 0x80 | (reg);  \
 329        udelay(25);                             \
 330        dma_wd.fdc_acces_seccount = (val);      \
 331        MFPDELAY();                             \
 332        /* local_irq_restore(__flags); */       \
 333    } while(0)
 334
 335
 336/* Buffering variables:
 337 * First, there is a DMA buffer in ST-RAM that is used for floppy DMA
 338 * operations. Second, a track buffer is used to cache a whole track
 339 * of the disk to save read operations. These are two separate buffers
 340 * because that allows write operations without clearing the track buffer.
 341 */
 342
 343static int MaxSectors[] = {
 344        11, 22, 44
 345};
 346static int BufferSize[] = {
 347        15*512, 30*512, 60*512
 348};
 349
 350#define BUFFER_SIZE     (BufferSize[DriveType])
 351
 352unsigned char *DMABuffer;                         /* buffer for writes */
 353static unsigned long PhysDMABuffer;   /* physical address */
 354
 355static int UseTrackbuffer = -1;           /* Do track buffering? */
 356module_param(UseTrackbuffer, int, 0);
 357
 358unsigned char *TrackBuffer;                       /* buffer for reads */
 359static unsigned long PhysTrackBuffer; /* physical address */
 360static int BufferDrive, BufferSide, BufferTrack;
 361static int read_track;          /* non-zero if we are reading whole tracks */
 362
 363#define SECTOR_BUFFER(sec)      (TrackBuffer + ((sec)-1)*512)
 364#define IS_BUFFERED(drive,side,track) \
 365    (BufferDrive == (drive) && BufferSide == (side) && BufferTrack == (track))
 366
 367/*
 368 * These are global variables, as that's the easiest way to give
 369 * information to interrupts. They are the data used for the current
 370 * request.
 371 */
 372static int SelectedDrive = 0;
 373static int ReqCmd, ReqBlock;
 374static int ReqSide, ReqTrack, ReqSector, ReqCnt;
 375static int HeadSettleFlag = 0;
 376static unsigned char *ReqData, *ReqBuffer;
 377static int MotorOn = 0, MotorOffTrys;
 378static int IsFormatting = 0, FormatError;
 379
 380static int UserSteprate[FD_MAX_UNITS] = { -1, -1 };
 381module_param_array(UserSteprate, int, NULL, 0);
 382
 383static DECLARE_COMPLETION(format_wait);
 384
 385static unsigned long changed_floppies = 0xff, fake_change = 0;
 386#define CHECK_CHANGE_DELAY      HZ/2
 387
 388#define FD_MOTOR_OFF_DELAY      (3*HZ)
 389#define FD_MOTOR_OFF_MAXTRY     (10*20)
 390
 391#define FLOPPY_TIMEOUT          (6*HZ)
 392#define RECALIBRATE_ERRORS      4       /* After this many errors the drive
 393                                         * will be recalibrated. */
 394#define MAX_ERRORS              8       /* After this many errors the driver
 395                                         * will give up. */
 396
 397
 398/*
 399 * The driver is trying to determine the correct media format
 400 * while Probing is set. fd_rwsec_done() clears it after a
 401 * successful access.
 402 */
 403static int Probing = 0;
 404
 405/* This flag is set when a dummy seek is necessary to make the WP
 406 * status bit accessible.
 407 */
 408static int NeedSeek = 0;
 409
 410
 411#ifdef DEBUG
 412#define DPRINT(a)       printk a
 413#else
 414#define DPRINT(a)
 415#endif
 416
 417/***************************** Prototypes *****************************/
 418
 419static void fd_select_side( int side );
 420static void fd_select_drive( int drive );
 421static void fd_deselect( void );
 422static void fd_motor_off_timer(struct timer_list *unused);
 423static void check_change(struct timer_list *unused);
 424static irqreturn_t floppy_irq (int irq, void *dummy);
 425static void fd_error( void );
 426static int do_format(int drive, int type, struct atari_format_descr *desc);
 427static void do_fd_action( int drive );
 428static void fd_calibrate( void );
 429static void fd_calibrate_done( int status );
 430static void fd_seek( void );
 431static void fd_seek_done( int status );
 432static void fd_rwsec( void );
 433static void fd_readtrack_check(struct timer_list *unused);
 434static void fd_rwsec_done( int status );
 435static void fd_rwsec_done1(int status);
 436static void fd_writetrack( void );
 437static void fd_writetrack_done( int status );
 438static void fd_times_out(struct timer_list *unused);
 439static void finish_fdc( void );
 440static void finish_fdc_done( int dummy );
 441static void setup_req_params( int drive );
 442static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 443                     cmd, unsigned long param);
 444static void fd_probe( int drive );
 445static int fd_test_drive_present( int drive );
 446static void config_types( void );
 447static int floppy_open(struct block_device *bdev, fmode_t mode);
 448static void floppy_release(struct gendisk *disk, fmode_t mode);
 449
 450/************************* End of Prototypes **************************/
 451
 452static DEFINE_TIMER(motor_off_timer, fd_motor_off_timer);
 453static DEFINE_TIMER(readtrack_timer, fd_readtrack_check);
 454static DEFINE_TIMER(timeout_timer, fd_times_out);
 455static DEFINE_TIMER(fd_timer, check_change);
 456        
 457static void fd_end_request_cur(blk_status_t err)
 458{
 459        if (!blk_update_request(fd_request, err,
 460                                blk_rq_cur_bytes(fd_request))) {
 461                __blk_mq_end_request(fd_request, err);
 462                fd_request = NULL;
 463        }
 464}
 465
 466static inline void start_motor_off_timer(void)
 467{
 468        mod_timer(&motor_off_timer, jiffies + FD_MOTOR_OFF_DELAY);
 469        MotorOffTrys = 0;
 470}
 471
 472static inline void start_check_change_timer( void )
 473{
 474        mod_timer(&fd_timer, jiffies + CHECK_CHANGE_DELAY);
 475}
 476
 477static inline void start_timeout(void)
 478{
 479        mod_timer(&timeout_timer, jiffies + FLOPPY_TIMEOUT);
 480}
 481
 482static inline void stop_timeout(void)
 483{
 484        del_timer(&timeout_timer);
 485}
 486
 487/* Select the side to use. */
 488
 489static void fd_select_side( int side )
 490{
 491        unsigned long flags;
 492
 493        /* protect against various other ints mucking around with the PSG */
 494        local_irq_save(flags);
 495  
 496        sound_ym.rd_data_reg_sel = 14; /* Select PSG Port A */
 497        sound_ym.wd_data = (side == 0) ? sound_ym.rd_data_reg_sel | 0x01 :
 498                                         sound_ym.rd_data_reg_sel & 0xfe;
 499
 500        local_irq_restore(flags);
 501}
 502
 503
 504/* Select a drive, update the FDC's track register and set the correct
 505 * clock speed for this disk's type.
 506 */
 507
 508static void fd_select_drive( int drive )
 509{
 510        unsigned long flags;
 511        unsigned char tmp;
 512  
 513        if (drive == SelectedDrive)
 514          return;
 515
 516        /* protect against various other ints mucking around with the PSG */
 517        local_irq_save(flags);
 518        sound_ym.rd_data_reg_sel = 14; /* Select PSG Port A */
 519        tmp = sound_ym.rd_data_reg_sel;
 520        sound_ym.wd_data = (tmp | DSKDRVNONE) & ~(drive == 0 ? DSKDRV0 : DSKDRV1);
 521        atari_dont_touch_floppy_select = 1;
 522        local_irq_restore(flags);
 523
 524        /* restore track register to saved value */
 525        FDC_WRITE( FDCREG_TRACK, UD.track );
 526        udelay(25);
 527
 528        /* select 8/16 MHz */
 529        if (UDT)
 530                if (ATARIHW_PRESENT(FDCSPEED))
 531                        dma_wd.fdc_speed = UDT->fdc_speed;
 532        
 533        SelectedDrive = drive;
 534}
 535
 536
 537/* Deselect both drives. */
 538
 539static void fd_deselect( void )
 540{
 541        unsigned long flags;
 542
 543        /* protect against various other ints mucking around with the PSG */
 544        local_irq_save(flags);
 545        atari_dont_touch_floppy_select = 0;
 546        sound_ym.rd_data_reg_sel=14;    /* Select PSG Port A */
 547        sound_ym.wd_data = (sound_ym.rd_data_reg_sel |
 548                            (MACH_IS_FALCON ? 3 : 7)); /* no drives selected */
 549        /* On Falcon, the drive B select line is used on the printer port, so
 550         * leave it alone... */
 551        SelectedDrive = -1;
 552        local_irq_restore(flags);
 553}
 554
 555
 556/* This timer function deselects the drives when the FDC switched the
 557 * motor off. The deselection cannot happen earlier because the FDC
 558 * counts the index signals, which arrive only if one drive is selected.
 559 */
 560
 561static void fd_motor_off_timer(struct timer_list *unused)
 562{
 563        unsigned char status;
 564
 565        if (SelectedDrive < 0)
 566                /* no drive selected, needn't deselect anyone */
 567                return;
 568
 569        if (stdma_islocked())
 570                goto retry;
 571
 572        status = FDC_READ( FDCREG_STATUS );
 573
 574        if (!(status & 0x80)) {
 575                /* motor already turned off by FDC -> deselect drives */
 576                MotorOn = 0;
 577                fd_deselect();
 578                return;
 579        }
 580        /* not yet off, try again */
 581
 582  retry:
 583        /* Test again later; if tested too often, it seems there is no disk
 584         * in the drive and the FDC will leave the motor on forever (or,
 585         * at least until a disk is inserted). So we'll test only twice
 586         * per second from then on...
 587         */
 588        mod_timer(&motor_off_timer,
 589                  jiffies + (MotorOffTrys++ < FD_MOTOR_OFF_MAXTRY ? HZ/20 : HZ/2));
 590}
 591
 592
 593/* This function is repeatedly called to detect disk changes (as good
 594 * as possible) and keep track of the current state of the write protection.
 595 */
 596
 597static void check_change(struct timer_list *unused)
 598{
 599        static int    drive = 0;
 600
 601        unsigned long flags;
 602        unsigned char old_porta;
 603        int                       stat;
 604
 605        if (++drive > 1 || !UD.connected)
 606                drive = 0;
 607
 608        /* protect against various other ints mucking around with the PSG */
 609        local_irq_save(flags);
 610
 611        if (!stdma_islocked()) {
 612                sound_ym.rd_data_reg_sel = 14;
 613                old_porta = sound_ym.rd_data_reg_sel;
 614                sound_ym.wd_data = (old_porta | DSKDRVNONE) &
 615                                       ~(drive == 0 ? DSKDRV0 : DSKDRV1);
 616                stat = !!(FDC_READ( FDCREG_STATUS ) & FDCSTAT_WPROT);
 617                sound_ym.wd_data = old_porta;
 618
 619                if (stat != UD.wpstat) {
 620                        DPRINT(( "wpstat[%d] = %d\n", drive, stat ));
 621                        UD.wpstat = stat;
 622                        set_bit (drive, &changed_floppies);
 623                }
 624        }
 625        local_irq_restore(flags);
 626
 627        start_check_change_timer();
 628}
 629
 630 
 631/* Handling of the Head Settling Flag: This flag should be set after each
 632 * seek operation, because we don't use seeks with verify.
 633 */
 634
 635static inline void set_head_settle_flag(void)
 636{
 637        HeadSettleFlag = FDCCMDADD_E;
 638}
 639
 640static inline int get_head_settle_flag(void)
 641{
 642        int     tmp = HeadSettleFlag;
 643        HeadSettleFlag = 0;
 644        return( tmp );
 645}
 646
 647static inline void copy_buffer(void *from, void *to)
 648{
 649        ulong *p1 = (ulong *)from, *p2 = (ulong *)to;
 650        int cnt;
 651
 652        for (cnt = 512/4; cnt; cnt--)
 653                *p2++ = *p1++;
 654}
 655
 656  
 657  
 658
 659/* General Interrupt Handling */
 660
 661static void (*FloppyIRQHandler)( int status ) = NULL;
 662
 663static irqreturn_t floppy_irq (int irq, void *dummy)
 664{
 665        unsigned char status;
 666        void (*handler)( int );
 667
 668        handler = xchg(&FloppyIRQHandler, NULL);
 669
 670        if (handler) {
 671                nop();
 672                status = FDC_READ( FDCREG_STATUS );
 673                DPRINT(("FDC irq, status = %02x handler = %08lx\n",status,(unsigned long)handler));
 674                handler( status );
 675        }
 676        else {
 677                DPRINT(("FDC irq, no handler\n"));
 678        }
 679        return IRQ_HANDLED;
 680}
 681
 682
 683/* Error handling: If some error happened, retry some times, then
 684 * recalibrate, then try again, and fail after MAX_ERRORS.
 685 */
 686
 687static void fd_error( void )
 688{
 689        if (IsFormatting) {
 690                IsFormatting = 0;
 691                FormatError = 1;
 692                complete(&format_wait);
 693                return;
 694        }
 695
 696        if (!fd_request)
 697                return;
 698
 699        fd_request->error_count++;
 700        if (fd_request->error_count >= MAX_ERRORS) {
 701                printk(KERN_ERR "fd%d: too many errors.\n", SelectedDrive );
 702                fd_end_request_cur(BLK_STS_IOERR);
 703        }
 704        else if (fd_request->error_count == RECALIBRATE_ERRORS) {
 705                printk(KERN_WARNING "fd%d: recalibrating\n", SelectedDrive );
 706                if (SelectedDrive != -1)
 707                        SUD.track = -1;
 708        }
 709}
 710
 711
 712
 713#define SET_IRQ_HANDLER(proc) do { FloppyIRQHandler = (proc); } while(0)
 714
 715
 716/* ---------- Formatting ---------- */
 717
 718#define FILL(n,val)             \
 719    do {                        \
 720        memset( p, val, n );    \
 721        p += n;                 \
 722    } while(0)
 723
 724static int do_format(int drive, int type, struct atari_format_descr *desc)
 725{
 726        struct request_queue *q;
 727        unsigned char   *p;
 728        int sect, nsect;
 729        unsigned long   flags;
 730        int ret;
 731
 732        if (type) {
 733                type--;
 734                if (type >= NUM_DISK_MINORS ||
 735                    minor2disktype[type].drive_types > DriveType)
 736                        return -EINVAL;
 737        }
 738
 739        q = unit[drive].disk[type]->queue;
 740        blk_mq_freeze_queue(q);
 741        blk_mq_quiesce_queue(q);
 742
 743        local_irq_save(flags);
 744        stdma_lock(floppy_irq, NULL);
 745        atari_turnon_irq( IRQ_MFP_FDC ); /* should be already, just to be sure */
 746        local_irq_restore(flags);
 747
 748        if (type) {
 749                type = minor2disktype[type].index;
 750                UDT = &atari_disk_type[type];
 751        }
 752
 753        if (!UDT || desc->track >= UDT->blocks/UDT->spt/2 || desc->head >= 2) {
 754                ret = -EINVAL;
 755                goto out;
 756        }
 757
 758        nsect = UDT->spt;
 759        p = TrackBuffer;
 760        /* The track buffer is used for the raw track data, so its
 761           contents become invalid! */
 762        BufferDrive = -1;
 763        /* stop deselect timer */
 764        del_timer( &motor_off_timer );
 765
 766        FILL( 60 * (nsect / 9), 0x4e );
 767        for( sect = 0; sect < nsect; ++sect ) {
 768                FILL( 12, 0 );
 769                FILL( 3, 0xf5 );
 770                *p++ = 0xfe;
 771                *p++ = desc->track;
 772                *p++ = desc->head;
 773                *p++ = (nsect + sect - desc->sect_offset) % nsect + 1;
 774                *p++ = 2;
 775                *p++ = 0xf7;
 776                FILL( 22, 0x4e );
 777                FILL( 12, 0 );
 778                FILL( 3, 0xf5 );
 779                *p++ = 0xfb;
 780                FILL( 512, 0xe5 );
 781                *p++ = 0xf7;
 782                FILL( 40, 0x4e );
 783        }
 784        FILL( TrackBuffer+BUFFER_SIZE-p, 0x4e );
 785
 786        IsFormatting = 1;
 787        FormatError = 0;
 788        ReqTrack = desc->track;
 789        ReqSide  = desc->head;
 790        do_fd_action( drive );
 791
 792        wait_for_completion(&format_wait);
 793
 794        ret = FormatError ? -EIO : 0;
 795out:
 796        blk_mq_unquiesce_queue(q);
 797        blk_mq_unfreeze_queue(q);
 798        return ret;
 799}
 800
 801
 802/* do_fd_action() is the general procedure for a fd request: All
 803 * required parameter settings (drive select, side select, track
 804 * position) are checked and set if needed. For each of these
 805 * parameters and the actual reading or writing exist two functions:
 806 * one that starts the setting (or skips it if possible) and one
 807 * callback for the "done" interrupt. Each done func calls the next
 808 * set function to propagate the request down to fd_rwsec_done().
 809 */
 810
 811static void do_fd_action( int drive )
 812{
 813        DPRINT(("do_fd_action\n"));
 814        
 815        if (UseTrackbuffer && !IsFormatting) {
 816        repeat:
 817            if (IS_BUFFERED( drive, ReqSide, ReqTrack )) {
 818                if (ReqCmd == READ) {
 819                    copy_buffer( SECTOR_BUFFER(ReqSector), ReqData );
 820                    if (++ReqCnt < blk_rq_cur_sectors(fd_request)) {
 821                        /* read next sector */
 822                        setup_req_params( drive );
 823                        goto repeat;
 824                    }
 825                    else {
 826                        /* all sectors finished */
 827                        fd_end_request_cur(BLK_STS_OK);
 828                        return;
 829                    }
 830                }
 831                else {
 832                    /* cmd == WRITE, pay attention to track buffer
 833                     * consistency! */
 834                    copy_buffer( ReqData, SECTOR_BUFFER(ReqSector) );
 835                }
 836            }
 837        }
 838
 839        if (SelectedDrive != drive)
 840                fd_select_drive( drive );
 841    
 842        if (UD.track == -1)
 843                fd_calibrate();
 844        else if (UD.track != ReqTrack << UDT->stretch)
 845                fd_seek();
 846        else if (IsFormatting)
 847                fd_writetrack();
 848        else
 849                fd_rwsec();
 850}
 851
 852
 853/* Seek to track 0 if the current track is unknown */
 854
 855static void fd_calibrate( void )
 856{
 857        if (SUD.track >= 0) {
 858                fd_calibrate_done( 0 );
 859                return;
 860        }
 861
 862        if (ATARIHW_PRESENT(FDCSPEED))
 863                dma_wd.fdc_speed = 0;   /* always seek with 8 Mhz */
 864        DPRINT(("fd_calibrate\n"));
 865        SET_IRQ_HANDLER( fd_calibrate_done );
 866        /* we can't verify, since the speed may be incorrect */
 867        FDC_WRITE( FDCREG_CMD, FDCCMD_RESTORE | SUD.steprate );
 868
 869        NeedSeek = 1;
 870        MotorOn = 1;
 871        start_timeout();
 872        /* wait for IRQ */
 873}
 874
 875
 876static void fd_calibrate_done( int status )
 877{
 878        DPRINT(("fd_calibrate_done()\n"));
 879        stop_timeout();
 880    
 881        /* set the correct speed now */
 882        if (ATARIHW_PRESENT(FDCSPEED))
 883                dma_wd.fdc_speed = SUDT->fdc_speed;
 884        if (status & FDCSTAT_RECNF) {
 885                printk(KERN_ERR "fd%d: restore failed\n", SelectedDrive );
 886                fd_error();
 887        }
 888        else {
 889                SUD.track = 0;
 890                fd_seek();
 891        }
 892}
 893  
 894  
 895/* Seek the drive to the requested track. The drive must have been
 896 * calibrated at some point before this.
 897 */
 898  
 899static void fd_seek( void )
 900{
 901        if (SUD.track == ReqTrack << SUDT->stretch) {
 902                fd_seek_done( 0 );
 903                return;
 904        }
 905
 906        if (ATARIHW_PRESENT(FDCSPEED)) {
 907                dma_wd.fdc_speed = 0;   /* always seek witch 8 Mhz */
 908                MFPDELAY();
 909        }
 910
 911        DPRINT(("fd_seek() to track %d\n",ReqTrack));
 912        FDC_WRITE( FDCREG_DATA, ReqTrack << SUDT->stretch);
 913        udelay(25);
 914        SET_IRQ_HANDLER( fd_seek_done );
 915        FDC_WRITE( FDCREG_CMD, FDCCMD_SEEK | SUD.steprate );
 916
 917        MotorOn = 1;
 918        set_head_settle_flag();
 919        start_timeout();
 920        /* wait for IRQ */
 921}
 922
 923
 924static void fd_seek_done( int status )
 925{
 926        DPRINT(("fd_seek_done()\n"));
 927        stop_timeout();
 928        
 929        /* set the correct speed */
 930        if (ATARIHW_PRESENT(FDCSPEED))
 931                dma_wd.fdc_speed = SUDT->fdc_speed;
 932        if (status & FDCSTAT_RECNF) {
 933                printk(KERN_ERR "fd%d: seek error (to track %d)\n",
 934                                SelectedDrive, ReqTrack );
 935                /* we don't know exactly which track we are on now! */
 936                SUD.track = -1;
 937                fd_error();
 938        }
 939        else {
 940                SUD.track = ReqTrack << SUDT->stretch;
 941                NeedSeek = 0;
 942                if (IsFormatting)
 943                        fd_writetrack();
 944                else
 945                        fd_rwsec();
 946        }
 947}
 948
 949
 950/* This does the actual reading/writing after positioning the head
 951 * over the correct track.
 952 */
 953
 954static int MultReadInProgress = 0;
 955
 956
 957static void fd_rwsec( void )
 958{
 959        unsigned long paddr, flags;
 960        unsigned int  rwflag, old_motoron;
 961        unsigned int track;
 962        
 963        DPRINT(("fd_rwsec(), Sec=%d, Access=%c\n",ReqSector, ReqCmd == WRITE ? 'w' : 'r' ));
 964        if (ReqCmd == WRITE) {
 965                if (ATARIHW_PRESENT(EXTD_DMA)) {
 966                        paddr = virt_to_phys(ReqData);
 967                }
 968                else {
 969                        copy_buffer( ReqData, DMABuffer );
 970                        paddr = PhysDMABuffer;
 971                }
 972                dma_cache_maintenance( paddr, 512, 1 );
 973                rwflag = 0x100;
 974        }
 975        else {
 976                if (read_track)
 977                        paddr = PhysTrackBuffer;
 978                else
 979                        paddr = ATARIHW_PRESENT(EXTD_DMA) ? 
 980                                virt_to_phys(ReqData) : PhysDMABuffer;
 981                rwflag = 0;
 982        }
 983
 984        fd_select_side( ReqSide );
 985  
 986        /* Start sector of this operation */
 987        FDC_WRITE( FDCREG_SECTOR, read_track ? 1 : ReqSector );
 988        MFPDELAY();
 989        /* Cheat for track if stretch != 0 */
 990        if (SUDT->stretch) {
 991                track = FDC_READ( FDCREG_TRACK);
 992                MFPDELAY();
 993                FDC_WRITE( FDCREG_TRACK, track >> SUDT->stretch);
 994        }
 995        udelay(25);
 996  
 997        /* Setup DMA */
 998        local_irq_save(flags);
 999        dma_wd.dma_lo = (unsigned char)paddr;
1000        MFPDELAY();
1001        paddr >>= 8;
1002        dma_wd.dma_md = (unsigned char)paddr;
1003        MFPDELAY();
1004        paddr >>= 8;
1005        if (ATARIHW_PRESENT(EXTD_DMA))
1006                st_dma_ext_dmahi = (unsigned short)paddr;
1007        else
1008                dma_wd.dma_hi = (unsigned char)paddr;
1009        MFPDELAY();
1010        local_irq_restore(flags);
1011  
1012        /* Clear FIFO and switch DMA to correct mode */  
1013        dma_wd.dma_mode_status = 0x90 | rwflag;  
1014        MFPDELAY();
1015        dma_wd.dma_mode_status = 0x90 | (rwflag ^ 0x100);  
1016        MFPDELAY();
1017        dma_wd.dma_mode_status = 0x90 | rwflag;
1018        MFPDELAY();
1019  
1020        /* How many sectors for DMA */
1021        dma_wd.fdc_acces_seccount = read_track ? SUDT->spt : 1;
1022  
1023        udelay(25);  
1024  
1025        /* Start operation */
1026        dma_wd.dma_mode_status = FDCSELREG_STP | rwflag;
1027        udelay(25);
1028        SET_IRQ_HANDLER( fd_rwsec_done );
1029        dma_wd.fdc_acces_seccount =
1030          (get_head_settle_flag() |
1031           (rwflag ? FDCCMD_WRSEC : (FDCCMD_RDSEC | (read_track ? FDCCMDADD_M : 0))));
1032
1033        old_motoron = MotorOn;
1034        MotorOn = 1;
1035        NeedSeek = 1;
1036        /* wait for interrupt */
1037
1038        if (read_track) {
1039                /* If reading a whole track, wait about one disk rotation and
1040                 * then check if all sectors are read. The FDC will even
1041                 * search for the first non-existent sector and need 1 sec to
1042                 * recognise that it isn't present :-(
1043                 */
1044                MultReadInProgress = 1;
1045                mod_timer(&readtrack_timer,
1046                          /* 1 rot. + 5 rot.s if motor was off  */
1047                          jiffies + HZ/5 + (old_motoron ? 0 : HZ));
1048        }
1049        start_timeout();
1050}
1051
1052    
1053static void fd_readtrack_check(struct timer_list *unused)
1054{
1055        unsigned long flags, addr, addr2;
1056
1057        local_irq_save(flags);
1058
1059        if (!MultReadInProgress) {
1060                /* This prevents a race condition that could arise if the
1061                 * interrupt is triggered while the calling of this timer
1062                 * callback function takes place. The IRQ function then has
1063                 * already cleared 'MultReadInProgress'  when flow of control
1064                 * gets here.
1065                 */
1066                local_irq_restore(flags);
1067                return;
1068        }
1069
1070        /* get the current DMA address */
1071        /* ++ f.a. read twice to avoid being fooled by switcher */
1072        addr = 0;
1073        do {
1074                addr2 = addr;
1075                addr = dma_wd.dma_lo & 0xff;
1076                MFPDELAY();
1077                addr |= (dma_wd.dma_md & 0xff) << 8;
1078                MFPDELAY();
1079                if (ATARIHW_PRESENT( EXTD_DMA ))
1080                        addr |= (st_dma_ext_dmahi & 0xffff) << 16;
1081                else
1082                        addr |= (dma_wd.dma_hi & 0xff) << 16;
1083                MFPDELAY();
1084        } while(addr != addr2);
1085  
1086        if (addr >= PhysTrackBuffer + SUDT->spt*512) {
1087                /* already read enough data, force an FDC interrupt to stop
1088                 * the read operation
1089                 */
1090                SET_IRQ_HANDLER( NULL );
1091                MultReadInProgress = 0;
1092                local_irq_restore(flags);
1093                DPRINT(("fd_readtrack_check(): done\n"));
1094                FDC_WRITE( FDCREG_CMD, FDCCMD_FORCI );
1095                udelay(25);
1096
1097                /* No error until now -- the FDC would have interrupted
1098                 * otherwise!
1099                 */
1100                fd_rwsec_done1(0);
1101        }
1102        else {
1103                /* not yet finished, wait another tenth rotation */
1104                local_irq_restore(flags);
1105                DPRINT(("fd_readtrack_check(): not yet finished\n"));
1106                mod_timer(&readtrack_timer, jiffies + HZ/5/10);
1107        }
1108}
1109
1110
1111static void fd_rwsec_done( int status )
1112{
1113        DPRINT(("fd_rwsec_done()\n"));
1114
1115        if (read_track) {
1116                del_timer(&readtrack_timer);
1117                if (!MultReadInProgress)
1118                        return;
1119                MultReadInProgress = 0;
1120        }
1121        fd_rwsec_done1(status);
1122}
1123
1124static void fd_rwsec_done1(int status)
1125{
1126        unsigned int track;
1127
1128        stop_timeout();
1129        
1130        /* Correct the track if stretch != 0 */
1131        if (SUDT->stretch) {
1132                track = FDC_READ( FDCREG_TRACK);
1133                MFPDELAY();
1134                FDC_WRITE( FDCREG_TRACK, track << SUDT->stretch);
1135        }
1136
1137        if (!UseTrackbuffer) {
1138                dma_wd.dma_mode_status = 0x90;
1139                MFPDELAY();
1140                if (!(dma_wd.dma_mode_status & 0x01)) {
1141                        printk(KERN_ERR "fd%d: DMA error\n", SelectedDrive );
1142                        goto err_end;
1143                }
1144        }
1145        MFPDELAY();
1146
1147        if (ReqCmd == WRITE && (status & FDCSTAT_WPROT)) {
1148                printk(KERN_NOTICE "fd%d: is write protected\n", SelectedDrive );
1149                goto err_end;
1150        }       
1151        if ((status & FDCSTAT_RECNF) &&
1152            /* RECNF is no error after a multiple read when the FDC
1153               searched for a non-existent sector! */
1154            !(read_track && FDC_READ(FDCREG_SECTOR) > SUDT->spt)) {
1155                if (Probing) {
1156                        if (SUDT > atari_disk_type) {
1157                            if (SUDT[-1].blocks > ReqBlock) {
1158                                /* try another disk type */
1159                                SUDT--;
1160                                set_capacity(unit[SelectedDrive].disk[0],
1161                                                        SUDT->blocks);
1162                            } else
1163                                Probing = 0;
1164                        }
1165                        else {
1166                                if (SUD.flags & FTD_MSG)
1167                                        printk(KERN_INFO "fd%d: Auto-detected floppy type %s\n",
1168                                               SelectedDrive, SUDT->name );
1169                                Probing=0;
1170                        }
1171                } else {        
1172/* record not found, but not probing. Maybe stretch wrong ? Restart probing */
1173                        if (SUD.autoprobe) {
1174                                SUDT = atari_disk_type + StartDiskType[DriveType];
1175                                set_capacity(unit[SelectedDrive].disk[0],
1176                                                        SUDT->blocks);
1177                                Probing = 1;
1178                        }
1179                }
1180                if (Probing) {
1181                        if (ATARIHW_PRESENT(FDCSPEED)) {
1182                                dma_wd.fdc_speed = SUDT->fdc_speed;
1183                                MFPDELAY();
1184                        }
1185                        setup_req_params( SelectedDrive );
1186                        BufferDrive = -1;
1187                        do_fd_action( SelectedDrive );
1188                        return;
1189                }
1190
1191                printk(KERN_ERR "fd%d: sector %d not found (side %d, track %d)\n",
1192                       SelectedDrive, FDC_READ (FDCREG_SECTOR), ReqSide, ReqTrack );
1193                goto err_end;
1194        }
1195        if (status & FDCSTAT_CRC) {
1196                printk(KERN_ERR "fd%d: CRC error (side %d, track %d, sector %d)\n",
1197                       SelectedDrive, ReqSide, ReqTrack, FDC_READ (FDCREG_SECTOR) );
1198                goto err_end;
1199        }
1200        if (status & FDCSTAT_LOST) {
1201                printk(KERN_ERR "fd%d: lost data (side %d, track %d, sector %d)\n",
1202                       SelectedDrive, ReqSide, ReqTrack, FDC_READ (FDCREG_SECTOR) );
1203                goto err_end;
1204        }
1205
1206        Probing = 0;
1207        
1208        if (ReqCmd == READ) {
1209                if (!read_track) {
1210                        void *addr;
1211                        addr = ATARIHW_PRESENT( EXTD_DMA ) ? ReqData : DMABuffer;
1212                        dma_cache_maintenance( virt_to_phys(addr), 512, 0 );
1213                        if (!ATARIHW_PRESENT( EXTD_DMA ))
1214                                copy_buffer (addr, ReqData);
1215                } else {
1216                        dma_cache_maintenance( PhysTrackBuffer, MaxSectors[DriveType] * 512, 0 );
1217                        BufferDrive = SelectedDrive;
1218                        BufferSide  = ReqSide;
1219                        BufferTrack = ReqTrack;
1220                        copy_buffer (SECTOR_BUFFER (ReqSector), ReqData);
1221                }
1222        }
1223  
1224        if (++ReqCnt < blk_rq_cur_sectors(fd_request)) {
1225                /* read next sector */
1226                setup_req_params( SelectedDrive );
1227                do_fd_action( SelectedDrive );
1228        }
1229        else {
1230                /* all sectors finished */
1231                fd_end_request_cur(BLK_STS_OK);
1232        }
1233        return;
1234  
1235  err_end:
1236        BufferDrive = -1;
1237        fd_error();
1238}
1239
1240
1241static void fd_writetrack( void )
1242{
1243        unsigned long paddr, flags;
1244        unsigned int track;
1245        
1246        DPRINT(("fd_writetrack() Tr=%d Si=%d\n", ReqTrack, ReqSide ));
1247
1248        paddr = PhysTrackBuffer;
1249        dma_cache_maintenance( paddr, BUFFER_SIZE, 1 );
1250
1251        fd_select_side( ReqSide );
1252  
1253        /* Cheat for track if stretch != 0 */
1254        if (SUDT->stretch) {
1255                track = FDC_READ( FDCREG_TRACK);
1256                MFPDELAY();
1257                FDC_WRITE(FDCREG_TRACK,track >> SUDT->stretch);
1258        }
1259        udelay(40);
1260  
1261        /* Setup DMA */
1262        local_irq_save(flags);
1263        dma_wd.dma_lo = (unsigned char)paddr;
1264        MFPDELAY();
1265        paddr >>= 8;
1266        dma_wd.dma_md = (unsigned char)paddr;
1267        MFPDELAY();
1268        paddr >>= 8;
1269        if (ATARIHW_PRESENT( EXTD_DMA ))
1270                st_dma_ext_dmahi = (unsigned short)paddr;
1271        else
1272                dma_wd.dma_hi = (unsigned char)paddr;
1273        MFPDELAY();
1274        local_irq_restore(flags);
1275  
1276        /* Clear FIFO and switch DMA to correct mode */  
1277        dma_wd.dma_mode_status = 0x190;  
1278        MFPDELAY();
1279        dma_wd.dma_mode_status = 0x90;  
1280        MFPDELAY();
1281        dma_wd.dma_mode_status = 0x190;
1282        MFPDELAY();
1283  
1284        /* How many sectors for DMA */
1285        dma_wd.fdc_acces_seccount = BUFFER_SIZE/512;
1286        udelay(40);  
1287  
1288        /* Start operation */
1289        dma_wd.dma_mode_status = FDCSELREG_STP | 0x100;
1290        udelay(40);
1291        SET_IRQ_HANDLER( fd_writetrack_done );
1292        dma_wd.fdc_acces_seccount = FDCCMD_WRTRA | get_head_settle_flag(); 
1293
1294        MotorOn = 1;
1295        start_timeout();
1296        /* wait for interrupt */
1297}
1298
1299
1300static void fd_writetrack_done( int status )
1301{
1302        DPRINT(("fd_writetrack_done()\n"));
1303
1304        stop_timeout();
1305
1306        if (status & FDCSTAT_WPROT) {
1307                printk(KERN_NOTICE "fd%d: is write protected\n", SelectedDrive );
1308                goto err_end;
1309        }       
1310        if (status & FDCSTAT_LOST) {
1311                printk(KERN_ERR "fd%d: lost data (side %d, track %d)\n",
1312                                SelectedDrive, ReqSide, ReqTrack );
1313                goto err_end;
1314        }
1315
1316        complete(&format_wait);
1317        return;
1318
1319  err_end:
1320        fd_error();
1321}
1322
1323static void fd_times_out(struct timer_list *unused)
1324{
1325        atari_disable_irq( IRQ_MFP_FDC );
1326        if (!FloppyIRQHandler) goto end; /* int occurred after timer was fired, but
1327                                          * before we came here... */
1328
1329        SET_IRQ_HANDLER( NULL );
1330        /* If the timeout occurred while the readtrack_check timer was
1331         * active, we need to cancel it, else bad things will happen */
1332        if (UseTrackbuffer)
1333                del_timer( &readtrack_timer );
1334        FDC_WRITE( FDCREG_CMD, FDCCMD_FORCI );
1335        udelay( 25 );
1336        
1337        printk(KERN_ERR "floppy timeout\n" );
1338        fd_error();
1339  end:
1340        atari_enable_irq( IRQ_MFP_FDC );
1341}
1342
1343
1344/* The (noop) seek operation here is needed to make the WP bit in the
1345 * FDC status register accessible for check_change. If the last disk
1346 * operation would have been a RDSEC, this bit would always read as 0
1347 * no matter what :-( To save time, the seek goes to the track we're
1348 * already on.
1349 */
1350
1351static void finish_fdc( void )
1352{
1353        if (!NeedSeek) {
1354                finish_fdc_done( 0 );
1355        }
1356        else {
1357                DPRINT(("finish_fdc: dummy seek started\n"));
1358                FDC_WRITE (FDCREG_DATA, SUD.track);
1359                SET_IRQ_HANDLER( finish_fdc_done );
1360                FDC_WRITE (FDCREG_CMD, FDCCMD_SEEK);
1361                MotorOn = 1;
1362                start_timeout();
1363                /* we must wait for the IRQ here, because the ST-DMA
1364                   is released immediately afterwards and the interrupt
1365                   may be delivered to the wrong driver. */
1366          }
1367}
1368
1369
1370static void finish_fdc_done( int dummy )
1371{
1372        unsigned long flags;
1373
1374        DPRINT(("finish_fdc_done entered\n"));
1375        stop_timeout();
1376        NeedSeek = 0;
1377
1378        if (timer_pending(&fd_timer) && time_before(fd_timer.expires, jiffies + 5))
1379                /* If the check for a disk change is done too early after this
1380                 * last seek command, the WP bit still reads wrong :-((
1381                 */
1382                mod_timer(&fd_timer, jiffies + 5);
1383        else
1384                start_check_change_timer();
1385        start_motor_off_timer();
1386
1387        local_irq_save(flags);
1388        stdma_release();
1389        local_irq_restore(flags);
1390
1391        DPRINT(("finish_fdc() finished\n"));
1392}
1393
1394/* The detection of disk changes is a dark chapter in Atari history :-(
1395 * Because the "Drive ready" signal isn't present in the Atari
1396 * hardware, one has to rely on the "Write Protect". This works fine,
1397 * as long as no write protected disks are used. TOS solves this
1398 * problem by introducing tri-state logic ("maybe changed") and
1399 * looking at the serial number in block 0. This isn't possible for
1400 * Linux, since the floppy driver can't make assumptions about the
1401 * filesystem used on the disk and thus the contents of block 0. I've
1402 * chosen the method to always say "The disk was changed" if it is
1403 * unsure whether it was. This implies that every open or mount
1404 * invalidates the disk buffers if you work with write protected
1405 * disks. But at least this is better than working with incorrect data
1406 * due to unrecognised disk changes.
1407 */
1408
1409static unsigned int floppy_check_events(struct gendisk *disk,
1410                                        unsigned int clearing)
1411{
1412        struct atari_floppy_struct *p = disk->private_data;
1413        unsigned int drive = p - unit;
1414        if (test_bit (drive, &fake_change)) {
1415                /* simulated change (e.g. after formatting) */
1416                return DISK_EVENT_MEDIA_CHANGE;
1417        }
1418        if (test_bit (drive, &changed_floppies)) {
1419                /* surely changed (the WP signal changed at least once) */
1420                return DISK_EVENT_MEDIA_CHANGE;
1421        }
1422        if (UD.wpstat) {
1423                /* WP is on -> could be changed: to be sure, buffers should be
1424                 * invalidated...
1425                 */
1426                return DISK_EVENT_MEDIA_CHANGE;
1427        }
1428
1429        return 0;
1430}
1431
1432static int floppy_revalidate(struct gendisk *disk)
1433{
1434        struct atari_floppy_struct *p = disk->private_data;
1435        unsigned int drive = p - unit;
1436
1437        if (test_bit(drive, &changed_floppies) ||
1438            test_bit(drive, &fake_change) ||
1439            p->disktype == 0) {
1440                if (UD.flags & FTD_MSG)
1441                        printk(KERN_ERR "floppy: clear format %p!\n", UDT);
1442                BufferDrive = -1;
1443                clear_bit(drive, &fake_change);
1444                clear_bit(drive, &changed_floppies);
1445                /* MSch: clearing geometry makes sense only for autoprobe
1446                   formats, for 'permanent user-defined' parameter:
1447                   restore default_params[] here if flagged valid! */
1448                if (default_params[drive].blocks == 0)
1449                        UDT = NULL;
1450                else
1451                        UDT = &default_params[drive];
1452        }
1453        return 0;
1454}
1455
1456
1457/* This sets up the global variables describing the current request. */
1458
1459static void setup_req_params( int drive )
1460{
1461        int block = ReqBlock + ReqCnt;
1462
1463        ReqTrack = block / UDT->spt;
1464        ReqSector = block - ReqTrack * UDT->spt + 1;
1465        ReqSide = ReqTrack & 1;
1466        ReqTrack >>= 1;
1467        ReqData = ReqBuffer + 512 * ReqCnt;
1468
1469        if (UseTrackbuffer)
1470                read_track = (ReqCmd == READ && fd_request->error_count == 0);
1471        else
1472                read_track = 0;
1473
1474        DPRINT(("Request params: Si=%d Tr=%d Se=%d Data=%08lx\n",ReqSide,
1475                        ReqTrack, ReqSector, (unsigned long)ReqData ));
1476}
1477
1478static void ataflop_commit_rqs(struct blk_mq_hw_ctx *hctx)
1479{
1480        spin_lock_irq(&ataflop_lock);
1481        atari_disable_irq(IRQ_MFP_FDC);
1482        finish_fdc();
1483        atari_enable_irq(IRQ_MFP_FDC);
1484        spin_unlock_irq(&ataflop_lock);
1485}
1486
1487static blk_status_t ataflop_queue_rq(struct blk_mq_hw_ctx *hctx,
1488                                     const struct blk_mq_queue_data *bd)
1489{
1490        struct atari_floppy_struct *floppy = bd->rq->rq_disk->private_data;
1491        int drive = floppy - unit;
1492        int type = floppy->type;
1493
1494        spin_lock_irq(&ataflop_lock);
1495        if (fd_request) {
1496                spin_unlock_irq(&ataflop_lock);
1497                return BLK_STS_DEV_RESOURCE;
1498        }
1499        if (!stdma_try_lock(floppy_irq, NULL))  {
1500                spin_unlock_irq(&ataflop_lock);
1501                return BLK_STS_RESOURCE;
1502        }
1503        fd_request = bd->rq;
1504        blk_mq_start_request(fd_request);
1505
1506        atari_disable_irq( IRQ_MFP_FDC );
1507
1508        IsFormatting = 0;
1509
1510        if (!UD.connected) {
1511                /* drive not connected */
1512                printk(KERN_ERR "Unknown Device: fd%d\n", drive );
1513                fd_end_request_cur(BLK_STS_IOERR);
1514                goto out;
1515        }
1516                
1517        if (type == 0) {
1518                if (!UDT) {
1519                        Probing = 1;
1520                        UDT = atari_disk_type + StartDiskType[DriveType];
1521                        set_capacity(bd->rq->rq_disk, UDT->blocks);
1522                        UD.autoprobe = 1;
1523                }
1524        } 
1525        else {
1526                /* user supplied disk type */
1527                if (--type >= NUM_DISK_MINORS) {
1528                        printk(KERN_WARNING "fd%d: invalid disk format", drive );
1529                        fd_end_request_cur(BLK_STS_IOERR);
1530                        goto out;
1531                }
1532                if (minor2disktype[type].drive_types > DriveType)  {
1533                        printk(KERN_WARNING "fd%d: unsupported disk format", drive );
1534                        fd_end_request_cur(BLK_STS_IOERR);
1535                        goto out;
1536                }
1537                type = minor2disktype[type].index;
1538                UDT = &atari_disk_type[type];
1539                set_capacity(bd->rq->rq_disk, UDT->blocks);
1540                UD.autoprobe = 0;
1541        }
1542
1543        /* stop deselect timer */
1544        del_timer( &motor_off_timer );
1545                
1546        ReqCnt = 0;
1547        ReqCmd = rq_data_dir(fd_request);
1548        ReqBlock = blk_rq_pos(fd_request);
1549        ReqBuffer = bio_data(fd_request->bio);
1550        setup_req_params( drive );
1551        do_fd_action( drive );
1552
1553        if (bd->last)
1554                finish_fdc();
1555        atari_enable_irq( IRQ_MFP_FDC );
1556
1557out:
1558        spin_unlock_irq(&ataflop_lock);
1559        return BLK_STS_OK;
1560}
1561
1562static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode,
1563                    unsigned int cmd, unsigned long param)
1564{
1565        struct gendisk *disk = bdev->bd_disk;
1566        struct atari_floppy_struct *floppy = disk->private_data;
1567        int drive = floppy - unit;
1568        int type = floppy->type;
1569        struct atari_format_descr fmt_desc;
1570        struct atari_disk_type *dtp;
1571        struct floppy_struct getprm;
1572        int settype;
1573        struct floppy_struct setprm;
1574        void __user *argp = (void __user *)param;
1575
1576        switch (cmd) {
1577        case FDGETPRM:
1578                if (type) {
1579                        if (--type >= NUM_DISK_MINORS)
1580                                return -ENODEV;
1581                        if (minor2disktype[type].drive_types > DriveType)
1582                                return -ENODEV;
1583                        type = minor2disktype[type].index;
1584                        dtp = &atari_disk_type[type];
1585                        if (UD.flags & FTD_MSG)
1586                            printk (KERN_ERR "floppy%d: found dtp %p name %s!\n",
1587                                drive, dtp, dtp->name);
1588                }
1589                else {
1590                        if (!UDT)
1591                                return -ENXIO;
1592                        else
1593                                dtp = UDT;
1594                }
1595                memset((void *)&getprm, 0, sizeof(getprm));
1596                getprm.size = dtp->blocks;
1597                getprm.sect = dtp->spt;
1598                getprm.head = 2;
1599                getprm.track = dtp->blocks/dtp->spt/2;
1600                getprm.stretch = dtp->stretch;
1601                if (copy_to_user(argp, &getprm, sizeof(getprm)))
1602                        return -EFAULT;
1603                return 0;
1604        }
1605        switch (cmd) {
1606        case FDSETPRM:
1607        case FDDEFPRM:
1608                /* 
1609                 * MSch 7/96: simple 'set geometry' case: just set the
1610                 * 'default' device params (minor == 0).
1611                 * Currently, the drive geometry is cleared after each
1612                 * disk change and subsequent revalidate()! simple
1613                 * implementation of FDDEFPRM: save geometry from a
1614                 * FDDEFPRM call and restore it in floppy_revalidate() !
1615                 */
1616
1617                /* get the parameters from user space */
1618                if (floppy->ref != 1 && floppy->ref != -1)
1619                        return -EBUSY;
1620                if (copy_from_user(&setprm, argp, sizeof(setprm)))
1621                        return -EFAULT;
1622                /* 
1623                 * first of all: check for floppy change and revalidate, 
1624                 * or the next access will revalidate - and clear UDT :-(
1625                 */
1626
1627                if (floppy_check_events(disk, 0))
1628                        floppy_revalidate(disk);
1629
1630                if (UD.flags & FTD_MSG)
1631                    printk (KERN_INFO "floppy%d: setting size %d spt %d str %d!\n",
1632                        drive, setprm.size, setprm.sect, setprm.stretch);
1633
1634                /* what if type > 0 here? Overwrite specified entry ? */
1635                if (type) {
1636                        /* refuse to re-set a predefined type for now */
1637                        return -EINVAL;
1638                }
1639
1640                /* 
1641                 * type == 0: first look for a matching entry in the type list,
1642                 * and set the UD.disktype field to use the perdefined entry.
1643                 * TODO: add user-defined format to head of autoprobe list ? 
1644                 * Useful to include the user-type for future autodetection!
1645                 */
1646
1647                for (settype = 0; settype < NUM_DISK_MINORS; settype++) {
1648                        int setidx = 0;
1649                        if (minor2disktype[settype].drive_types > DriveType) {
1650                                /* skip this one, invalid for drive ... */
1651                                continue;
1652                        }
1653                        setidx = minor2disktype[settype].index;
1654                        dtp = &atari_disk_type[setidx];
1655
1656                        /* found matching entry ?? */
1657                        if (   dtp->blocks  == setprm.size 
1658                            && dtp->spt     == setprm.sect
1659                            && dtp->stretch == setprm.stretch ) {
1660                                if (UD.flags & FTD_MSG)
1661                                    printk (KERN_INFO "floppy%d: setting %s %p!\n",
1662                                        drive, dtp->name, dtp);
1663                                UDT = dtp;
1664                                set_capacity(disk, UDT->blocks);
1665
1666                                if (cmd == FDDEFPRM) {
1667                                  /* save settings as permanent default type */
1668                                  default_params[drive].name    = dtp->name;
1669                                  default_params[drive].spt     = dtp->spt;
1670                                  default_params[drive].blocks  = dtp->blocks;
1671                                  default_params[drive].fdc_speed = dtp->fdc_speed;
1672                                  default_params[drive].stretch = dtp->stretch;
1673                                }
1674                                
1675                                return 0;
1676                        }
1677
1678                }
1679
1680                /* no matching disk type found above - setting user_params */
1681
1682                if (cmd == FDDEFPRM) {
1683                        /* set permanent type */
1684                        dtp = &default_params[drive];
1685                } else
1686                        /* set user type (reset by disk change!) */
1687                        dtp = &user_params[drive];
1688
1689                dtp->name   = "user format";
1690                dtp->blocks = setprm.size;
1691                dtp->spt    = setprm.sect;
1692                if (setprm.sect > 14) 
1693                        dtp->fdc_speed = 3;
1694                else
1695                        dtp->fdc_speed = 0;
1696                dtp->stretch = setprm.stretch;
1697
1698                if (UD.flags & FTD_MSG)
1699                        printk (KERN_INFO "floppy%d: blk %d spt %d str %d!\n",
1700                                drive, dtp->blocks, dtp->spt, dtp->stretch);
1701
1702                /* sanity check */
1703                if (setprm.track != dtp->blocks/dtp->spt/2 ||
1704                    setprm.head != 2)
1705                        return -EINVAL;
1706
1707                UDT = dtp;
1708                set_capacity(disk, UDT->blocks);
1709
1710                return 0;
1711        case FDMSGON:
1712                UD.flags |= FTD_MSG;
1713                return 0;
1714        case FDMSGOFF:
1715                UD.flags &= ~FTD_MSG;
1716                return 0;
1717        case FDSETEMSGTRESH:
1718                return -EINVAL;
1719        case FDFMTBEG:
1720                return 0;
1721        case FDFMTTRK:
1722                if (floppy->ref != 1 && floppy->ref != -1)
1723                        return -EBUSY;
1724                if (copy_from_user(&fmt_desc, argp, sizeof(fmt_desc)))
1725                        return -EFAULT;
1726                return do_format(drive, type, &fmt_desc);
1727        case FDCLRPRM:
1728                UDT = NULL;
1729                /* MSch: invalidate default_params */
1730                default_params[drive].blocks  = 0;
1731                set_capacity(disk, MAX_DISK_SIZE * 2);
1732                fallthrough;
1733        case FDFMTEND:
1734        case FDFLUSH:
1735                /* invalidate the buffer track to force a reread */
1736                BufferDrive = -1;
1737                set_bit(drive, &fake_change);
1738                if (bdev_check_media_change(bdev))
1739                        floppy_revalidate(bdev->bd_disk);
1740                return 0;
1741        default:
1742                return -EINVAL;
1743        }
1744}
1745
1746static int fd_ioctl(struct block_device *bdev, fmode_t mode,
1747                             unsigned int cmd, unsigned long arg)
1748{
1749        int ret;
1750
1751        mutex_lock(&ataflop_mutex);
1752        ret = fd_locked_ioctl(bdev, mode, cmd, arg);
1753        mutex_unlock(&ataflop_mutex);
1754
1755        return ret;
1756}
1757
1758/* Initialize the 'unit' variable for drive 'drive' */
1759
1760static void __init fd_probe( int drive )
1761{
1762        UD.connected = 0;
1763        UDT  = NULL;
1764
1765        if (!fd_test_drive_present( drive ))
1766                return;
1767
1768        UD.connected = 1;
1769        UD.track     = 0;
1770        switch( UserSteprate[drive] ) {
1771        case 2:
1772                UD.steprate = FDCSTEP_2;
1773                break;
1774        case 3:
1775                UD.steprate = FDCSTEP_3;
1776                break;
1777        case 6:
1778                UD.steprate = FDCSTEP_6;
1779                break;
1780        case 12:
1781                UD.steprate = FDCSTEP_12;
1782                break;
1783        default: /* should be -1 for "not set by user" */
1784                if (ATARIHW_PRESENT( FDCSPEED ) || MACH_IS_MEDUSA)
1785                        UD.steprate = FDCSTEP_3;
1786                else
1787                        UD.steprate = FDCSTEP_6;
1788                break;
1789        }
1790        MotorOn = 1;    /* from probe restore operation! */
1791}
1792
1793
1794/* This function tests the physical presence of a floppy drive (not
1795 * whether a disk is inserted). This is done by issuing a restore
1796 * command, waiting max. 2 seconds (that should be enough to move the
1797 * head across the whole disk) and looking at the state of the "TR00"
1798 * signal. This should now be raised if there is a drive connected
1799 * (and there is no hardware failure :-) Otherwise, the drive is
1800 * declared absent.
1801 */
1802
1803static int __init fd_test_drive_present( int drive )
1804{
1805        unsigned long timeout;
1806        unsigned char status;
1807        int ok;
1808        
1809        if (drive >= (MACH_IS_FALCON ? 1 : 2)) return( 0 );
1810        fd_select_drive( drive );
1811
1812        /* disable interrupt temporarily */
1813        atari_turnoff_irq( IRQ_MFP_FDC );
1814        FDC_WRITE (FDCREG_TRACK, 0xff00);
1815        FDC_WRITE( FDCREG_CMD, FDCCMD_RESTORE | FDCCMDADD_H | FDCSTEP_6 );
1816
1817        timeout = jiffies + 2*HZ+HZ/2;
1818        while (time_before(jiffies, timeout))
1819                if (!(st_mfp.par_dt_reg & 0x20))
1820                        break;
1821
1822        status = FDC_READ( FDCREG_STATUS );
1823        ok = (status & FDCSTAT_TR00) != 0;
1824
1825        /* force interrupt to abort restore operation (FDC would try
1826         * about 50 seconds!) */
1827        FDC_WRITE( FDCREG_CMD, FDCCMD_FORCI );
1828        udelay(500);
1829        status = FDC_READ( FDCREG_STATUS );
1830        udelay(20);
1831
1832        if (ok) {
1833                /* dummy seek command to make WP bit accessible */
1834                FDC_WRITE( FDCREG_DATA, 0 );
1835                FDC_WRITE( FDCREG_CMD, FDCCMD_SEEK );
1836                while( st_mfp.par_dt_reg & 0x20 )
1837                        ;
1838                status = FDC_READ( FDCREG_STATUS );
1839        }
1840
1841        atari_turnon_irq( IRQ_MFP_FDC );
1842        return( ok );
1843}
1844
1845
1846/* Look how many and which kind of drives are connected. If there are
1847 * floppies, additionally start the disk-change and motor-off timers.
1848 */
1849
1850static void __init config_types( void )
1851{
1852        int drive, cnt = 0;
1853
1854        /* for probing drives, set the FDC speed to 8 MHz */
1855        if (ATARIHW_PRESENT(FDCSPEED))
1856                dma_wd.fdc_speed = 0;
1857
1858        printk(KERN_INFO "Probing floppy drive(s):\n");
1859        for( drive = 0; drive < FD_MAX_UNITS; drive++ ) {
1860                fd_probe( drive );
1861                if (UD.connected) {
1862                        printk(KERN_INFO "fd%d\n", drive);
1863                        ++cnt;
1864                }
1865        }
1866
1867        if (FDC_READ( FDCREG_STATUS ) & FDCSTAT_BUSY) {
1868                /* If FDC is still busy from probing, give it another FORCI
1869                 * command to abort the operation. If this isn't done, the FDC
1870                 * will interrupt later and its IRQ line stays low, because
1871                 * the status register isn't read. And this will block any
1872                 * interrupts on this IRQ line :-(
1873                 */
1874                FDC_WRITE( FDCREG_CMD, FDCCMD_FORCI );
1875                udelay(500);
1876                FDC_READ( FDCREG_STATUS );
1877                udelay(20);
1878        }
1879        
1880        if (cnt > 0) {
1881                start_motor_off_timer();
1882                if (cnt == 1) fd_select_drive( 0 );
1883                start_check_change_timer();
1884        }
1885}
1886
1887/*
1888 * floppy_open check for aliasing (/dev/fd0 can be the same as
1889 * /dev/PS0 etc), and disallows simultaneous access to the same
1890 * drive with different device numbers.
1891 */
1892
1893static int floppy_open(struct block_device *bdev, fmode_t mode)
1894{
1895        struct atari_floppy_struct *p = bdev->bd_disk->private_data;
1896        int type  = MINOR(bdev->bd_dev) >> 2;
1897
1898        DPRINT(("fd_open: type=%d\n",type));
1899        if (p->ref && p->type != type)
1900                return -EBUSY;
1901
1902        if (p->ref == -1 || (p->ref && mode & FMODE_EXCL))
1903                return -EBUSY;
1904
1905        if (mode & FMODE_EXCL)
1906                p->ref = -1;
1907        else
1908                p->ref++;
1909
1910        p->type = type;
1911
1912        if (mode & FMODE_NDELAY)
1913                return 0;
1914
1915        if (mode & (FMODE_READ|FMODE_WRITE)) {
1916                if (bdev_check_media_change(bdev))
1917                        floppy_revalidate(bdev->bd_disk);
1918                if (mode & FMODE_WRITE) {
1919                        if (p->wpstat) {
1920                                if (p->ref < 0)
1921                                        p->ref = 0;
1922                                else
1923                                        p->ref--;
1924                                return -EROFS;
1925                        }
1926                }
1927        }
1928        return 0;
1929}
1930
1931static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
1932{
1933        int ret;
1934
1935        mutex_lock(&ataflop_mutex);
1936        ret = floppy_open(bdev, mode);
1937        mutex_unlock(&ataflop_mutex);
1938
1939        return ret;
1940}
1941
1942static void floppy_release(struct gendisk *disk, fmode_t mode)
1943{
1944        struct atari_floppy_struct *p = disk->private_data;
1945        mutex_lock(&ataflop_mutex);
1946        if (p->ref < 0)
1947                p->ref = 0;
1948        else if (!p->ref--) {
1949                printk(KERN_ERR "floppy_release with fd_ref == 0");
1950                p->ref = 0;
1951        }
1952        mutex_unlock(&ataflop_mutex);
1953}
1954
1955static const struct block_device_operations floppy_fops = {
1956        .owner          = THIS_MODULE,
1957        .open           = floppy_unlocked_open,
1958        .release        = floppy_release,
1959        .ioctl          = fd_ioctl,
1960        .check_events   = floppy_check_events,
1961};
1962
1963static const struct blk_mq_ops ataflop_mq_ops = {
1964        .queue_rq = ataflop_queue_rq,
1965        .commit_rqs = ataflop_commit_rqs,
1966};
1967
1968static int ataflop_alloc_disk(unsigned int drive, unsigned int type)
1969{
1970        struct gendisk *disk;
1971        int ret;
1972
1973        disk = alloc_disk(1);
1974        if (!disk)
1975                return -ENOMEM;
1976
1977        disk->queue = blk_mq_init_queue(&unit[drive].tag_set);
1978        if (IS_ERR(disk->queue)) {
1979                ret = PTR_ERR(disk->queue);
1980                disk->queue = NULL;
1981                put_disk(disk);
1982                return ret;
1983        }
1984
1985        disk->major = FLOPPY_MAJOR;
1986        disk->first_minor = drive + (type << 2);
1987        sprintf(disk->disk_name, "fd%d", drive);
1988        disk->fops = &floppy_fops;
1989        disk->events = DISK_EVENT_MEDIA_CHANGE;
1990        disk->private_data = &unit[drive];
1991        set_capacity(disk, MAX_DISK_SIZE * 2);
1992
1993        unit[drive].disk[type] = disk;
1994        return 0;
1995}
1996
1997static DEFINE_MUTEX(ataflop_probe_lock);
1998
1999static void ataflop_probe(dev_t dev)
2000{
2001        int drive = MINOR(dev) & 3;
2002        int type  = MINOR(dev) >> 2;
2003
2004        if (type)
2005                type--;
2006
2007        if (drive >= FD_MAX_UNITS || type >= NUM_DISK_MINORS)
2008                return;
2009        mutex_lock(&ataflop_probe_lock);
2010        if (!unit[drive].disk[type]) {
2011                if (ataflop_alloc_disk(drive, type) == 0)
2012                        add_disk(unit[drive].disk[type]);
2013        }
2014        mutex_unlock(&ataflop_probe_lock);
2015}
2016
2017static int __init atari_floppy_init (void)
2018{
2019        int i;
2020        int ret;
2021
2022        if (!MACH_IS_ATARI)
2023                /* Amiga, Mac, ... don't have Atari-compatible floppy :-) */
2024                return -ENODEV;
2025
2026        mutex_lock(&ataflop_probe_lock);
2027        ret = __register_blkdev(FLOPPY_MAJOR, "fd", ataflop_probe);
2028        if (ret)
2029                goto out_unlock;
2030
2031        for (i = 0; i < FD_MAX_UNITS; i++) {
2032                memset(&unit[i].tag_set, 0, sizeof(unit[i].tag_set));
2033                unit[i].tag_set.ops = &ataflop_mq_ops;
2034                unit[i].tag_set.nr_hw_queues = 1;
2035                unit[i].tag_set.nr_maps = 1;
2036                unit[i].tag_set.queue_depth = 2;
2037                unit[i].tag_set.numa_node = NUMA_NO_NODE;
2038                unit[i].tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
2039                ret = blk_mq_alloc_tag_set(&unit[i].tag_set);
2040                if (ret)
2041                        goto err;
2042
2043                ret = ataflop_alloc_disk(i, 0);
2044                if (ret) {
2045                        blk_mq_free_tag_set(&unit[i].tag_set);
2046                        goto err;
2047                }
2048        }
2049
2050        if (UseTrackbuffer < 0)
2051                /* not set by user -> use default: for now, we turn
2052                   track buffering off for all Medusas, though it
2053                   could be used with ones that have a counter
2054                   card. But the test is too hard :-( */
2055                UseTrackbuffer = !MACH_IS_MEDUSA;
2056
2057        /* initialize variables */
2058        SelectedDrive = -1;
2059        BufferDrive = -1;
2060
2061        DMABuffer = atari_stram_alloc(BUFFER_SIZE+512, "ataflop");
2062        if (!DMABuffer) {
2063                printk(KERN_ERR "atari_floppy_init: cannot get dma buffer\n");
2064                ret = -ENOMEM;
2065                goto err;
2066        }
2067        TrackBuffer = DMABuffer + 512;
2068        PhysDMABuffer = atari_stram_to_phys(DMABuffer);
2069        PhysTrackBuffer = virt_to_phys(TrackBuffer);
2070        BufferDrive = BufferSide = BufferTrack = -1;
2071
2072        for (i = 0; i < FD_MAX_UNITS; i++) {
2073                unit[i].track = -1;
2074                unit[i].flags = 0;
2075                add_disk(unit[i].disk[0]);
2076        }
2077
2078        printk(KERN_INFO "Atari floppy driver: max. %cD, %strack buffering\n",
2079               DriveType == 0 ? 'D' : DriveType == 1 ? 'H' : 'E',
2080               UseTrackbuffer ? "" : "no ");
2081        config_types();
2082
2083        return 0;
2084
2085err:
2086        while (--i >= 0) {
2087                blk_cleanup_queue(unit[i].disk[0]->queue);
2088                put_disk(unit[i].disk[0]);
2089                blk_mq_free_tag_set(&unit[i].tag_set);
2090        }
2091
2092        unregister_blkdev(FLOPPY_MAJOR, "fd");
2093out_unlock:
2094        mutex_unlock(&ataflop_probe_lock);
2095        return ret;
2096}
2097
2098#ifndef MODULE
2099static int __init atari_floppy_setup(char *str)
2100{
2101        int ints[3 + FD_MAX_UNITS];
2102        int i;
2103
2104        if (!MACH_IS_ATARI)
2105                return 0;
2106
2107        str = get_options(str, 3 + FD_MAX_UNITS, ints);
2108        
2109        if (ints[0] < 1) {
2110                printk(KERN_ERR "ataflop_setup: no arguments!\n" );
2111                return 0;
2112        }
2113        else if (ints[0] > 2+FD_MAX_UNITS) {
2114                printk(KERN_ERR "ataflop_setup: too many arguments\n" );
2115        }
2116
2117        if (ints[1] < 0 || ints[1] > 2)
2118                printk(KERN_ERR "ataflop_setup: bad drive type\n" );
2119        else
2120                DriveType = ints[1];
2121
2122        if (ints[0] >= 2)
2123                UseTrackbuffer = (ints[2] > 0);
2124
2125        for( i = 3; i <= ints[0] && i-3 < FD_MAX_UNITS; ++i ) {
2126                if (ints[i] != 2 && ints[i] != 3 && ints[i] != 6 && ints[i] != 12)
2127                        printk(KERN_ERR "ataflop_setup: bad steprate\n" );
2128                else
2129                        UserSteprate[i-3] = ints[i];
2130        }
2131        return 1;
2132}
2133
2134__setup("floppy=", atari_floppy_setup);
2135#endif
2136
2137static void __exit atari_floppy_exit(void)
2138{
2139        int i, type;
2140
2141        for (i = 0; i < FD_MAX_UNITS; i++) {
2142                for (type = 0; type < NUM_DISK_MINORS; type++) {
2143                        if (!unit[i].disk[type])
2144                                continue;
2145                        del_gendisk(unit[i].disk[type]);
2146                        blk_cleanup_queue(unit[i].disk[type]->queue);
2147                        put_disk(unit[i].disk[type]);
2148                }
2149                blk_mq_free_tag_set(&unit[i].tag_set);
2150        }
2151        unregister_blkdev(FLOPPY_MAJOR, "fd");
2152
2153        del_timer_sync(&fd_timer);
2154        atari_stram_free( DMABuffer );
2155}
2156
2157module_init(atari_floppy_init)
2158module_exit(atari_floppy_exit)
2159
2160MODULE_LICENSE("GPL");
2161