12874c5fdSThomas Gleixner // SPDX-License-Identifier: GPL-2.0-or-later
28852ecd9SLaurent Vivier /*
38852ecd9SLaurent Vivier * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
48852ecd9SLaurent Vivier *
58852ecd9SLaurent Vivier * Copyright (C) 2004,2008 Laurent Vivier <Laurent@lvivier.info>
68852ecd9SLaurent Vivier *
78852ecd9SLaurent Vivier * based on Alastair Bridgewater SWIM analysis, 2001
88852ecd9SLaurent Vivier * based on SWIM3 driver (c) Paul Mackerras, 1996
98852ecd9SLaurent Vivier * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
108852ecd9SLaurent Vivier *
118852ecd9SLaurent Vivier * 2004-08-21 (lv) - Initial implementation
128852ecd9SLaurent Vivier * 2008-10-30 (lv) - Port to 2.6
138852ecd9SLaurent Vivier */
148852ecd9SLaurent Vivier
158852ecd9SLaurent Vivier #include <linux/module.h>
168852ecd9SLaurent Vivier #include <linux/fd.h>
175a0e3ad6STejun Heo #include <linux/slab.h>
18e3896d77SOmar Sandoval #include <linux/blk-mq.h>
19b81e0c23SChristoph Hellwig #include <linux/major.h>
202a48fc0aSArnd Bergmann #include <linux/mutex.h>
218852ecd9SLaurent Vivier #include <linux/hdreg.h>
228852ecd9SLaurent Vivier #include <linux/kernel.h>
238852ecd9SLaurent Vivier #include <linux/delay.h>
248852ecd9SLaurent Vivier #include <linux/platform_device.h>
258852ecd9SLaurent Vivier
268852ecd9SLaurent Vivier #include <asm/mac_via.h>
278852ecd9SLaurent Vivier
288852ecd9SLaurent Vivier #define CARDNAME "swim"
298852ecd9SLaurent Vivier
308852ecd9SLaurent Vivier struct sector_header {
318852ecd9SLaurent Vivier unsigned char side;
328852ecd9SLaurent Vivier unsigned char track;
338852ecd9SLaurent Vivier unsigned char sector;
348852ecd9SLaurent Vivier unsigned char size;
358852ecd9SLaurent Vivier unsigned char crc0;
368852ecd9SLaurent Vivier unsigned char crc1;
378852ecd9SLaurent Vivier } __attribute__((packed));
388852ecd9SLaurent Vivier
398852ecd9SLaurent Vivier #define DRIVER_VERSION "Version 0.2 (2008-10-30)"
408852ecd9SLaurent Vivier
418852ecd9SLaurent Vivier #define REG(x) unsigned char x, x ## _pad[0x200 - 1];
428852ecd9SLaurent Vivier
438852ecd9SLaurent Vivier struct swim {
448852ecd9SLaurent Vivier REG(write_data)
458852ecd9SLaurent Vivier REG(write_mark)
468852ecd9SLaurent Vivier REG(write_CRC)
478852ecd9SLaurent Vivier REG(write_parameter)
488852ecd9SLaurent Vivier REG(write_phase)
498852ecd9SLaurent Vivier REG(write_setup)
508852ecd9SLaurent Vivier REG(write_mode0)
518852ecd9SLaurent Vivier REG(write_mode1)
528852ecd9SLaurent Vivier
538852ecd9SLaurent Vivier REG(read_data)
548852ecd9SLaurent Vivier REG(read_mark)
558852ecd9SLaurent Vivier REG(read_error)
568852ecd9SLaurent Vivier REG(read_parameter)
578852ecd9SLaurent Vivier REG(read_phase)
588852ecd9SLaurent Vivier REG(read_setup)
598852ecd9SLaurent Vivier REG(read_status)
608852ecd9SLaurent Vivier REG(read_handshake)
618852ecd9SLaurent Vivier } __attribute__((packed));
628852ecd9SLaurent Vivier
638852ecd9SLaurent Vivier #define swim_write(base, reg, v) out_8(&(base)->write_##reg, (v))
648852ecd9SLaurent Vivier #define swim_read(base, reg) in_8(&(base)->read_##reg)
658852ecd9SLaurent Vivier
668852ecd9SLaurent Vivier /* IWM registers */
678852ecd9SLaurent Vivier
688852ecd9SLaurent Vivier struct iwm {
698852ecd9SLaurent Vivier REG(ph0L)
708852ecd9SLaurent Vivier REG(ph0H)
718852ecd9SLaurent Vivier REG(ph1L)
728852ecd9SLaurent Vivier REG(ph1H)
738852ecd9SLaurent Vivier REG(ph2L)
748852ecd9SLaurent Vivier REG(ph2H)
758852ecd9SLaurent Vivier REG(ph3L)
768852ecd9SLaurent Vivier REG(ph3H)
778852ecd9SLaurent Vivier REG(mtrOff)
788852ecd9SLaurent Vivier REG(mtrOn)
798852ecd9SLaurent Vivier REG(intDrive)
808852ecd9SLaurent Vivier REG(extDrive)
818852ecd9SLaurent Vivier REG(q6L)
828852ecd9SLaurent Vivier REG(q6H)
838852ecd9SLaurent Vivier REG(q7L)
848852ecd9SLaurent Vivier REG(q7H)
858852ecd9SLaurent Vivier } __attribute__((packed));
868852ecd9SLaurent Vivier
878852ecd9SLaurent Vivier #define iwm_write(base, reg, v) out_8(&(base)->reg, (v))
888852ecd9SLaurent Vivier #define iwm_read(base, reg) in_8(&(base)->reg)
898852ecd9SLaurent Vivier
908852ecd9SLaurent Vivier /* bits in phase register */
918852ecd9SLaurent Vivier
928852ecd9SLaurent Vivier #define SEEK_POSITIVE 0x070
938852ecd9SLaurent Vivier #define SEEK_NEGATIVE 0x074
948852ecd9SLaurent Vivier #define STEP 0x071
958852ecd9SLaurent Vivier #define MOTOR_ON 0x072
968852ecd9SLaurent Vivier #define MOTOR_OFF 0x076
978852ecd9SLaurent Vivier #define INDEX 0x073
988852ecd9SLaurent Vivier #define EJECT 0x077
998852ecd9SLaurent Vivier #define SETMFM 0x171
1008852ecd9SLaurent Vivier #define SETGCR 0x175
1018852ecd9SLaurent Vivier
1028852ecd9SLaurent Vivier #define RELAX 0x033
1038852ecd9SLaurent Vivier #define LSTRB 0x008
1048852ecd9SLaurent Vivier
1058852ecd9SLaurent Vivier #define CA_MASK 0x077
1068852ecd9SLaurent Vivier
1078852ecd9SLaurent Vivier /* Select values for swim_select and swim_readbit */
1088852ecd9SLaurent Vivier
1098852ecd9SLaurent Vivier #define READ_DATA_0 0x074
11056a1c5eeSFinn Thain #define ONEMEG_DRIVE 0x075
1118852ecd9SLaurent Vivier #define SINGLE_SIDED 0x076
1128852ecd9SLaurent Vivier #define DRIVE_PRESENT 0x077
1138852ecd9SLaurent Vivier #define DISK_IN 0x170
1148852ecd9SLaurent Vivier #define WRITE_PROT 0x171
1158852ecd9SLaurent Vivier #define TRACK_ZERO 0x172
1168852ecd9SLaurent Vivier #define TACHO 0x173
1178852ecd9SLaurent Vivier #define READ_DATA_1 0x174
11856a1c5eeSFinn Thain #define GCR_MODE 0x175
1198852ecd9SLaurent Vivier #define SEEK_COMPLETE 0x176
12056a1c5eeSFinn Thain #define TWOMEG_MEDIA 0x177
1218852ecd9SLaurent Vivier
1228852ecd9SLaurent Vivier /* Bits in handshake register */
1238852ecd9SLaurent Vivier
1248852ecd9SLaurent Vivier #define MARK_BYTE 0x01
1258852ecd9SLaurent Vivier #define CRC_ZERO 0x02
1268852ecd9SLaurent Vivier #define RDDATA 0x04
1278852ecd9SLaurent Vivier #define SENSE 0x08
1288852ecd9SLaurent Vivier #define MOTEN 0x10
1298852ecd9SLaurent Vivier #define ERROR 0x20
1308852ecd9SLaurent Vivier #define DAT2BYTE 0x40
1318852ecd9SLaurent Vivier #define DAT1BYTE 0x80
1328852ecd9SLaurent Vivier
1338852ecd9SLaurent Vivier /* bits in setup register */
1348852ecd9SLaurent Vivier
1358852ecd9SLaurent Vivier #define S_INV_WDATA 0x01
1368852ecd9SLaurent Vivier #define S_3_5_SELECT 0x02
1378852ecd9SLaurent Vivier #define S_GCR 0x04
1388852ecd9SLaurent Vivier #define S_FCLK_DIV2 0x08
1398852ecd9SLaurent Vivier #define S_ERROR_CORR 0x10
1408852ecd9SLaurent Vivier #define S_IBM_DRIVE 0x20
1418852ecd9SLaurent Vivier #define S_GCR_WRITE 0x40
1428852ecd9SLaurent Vivier #define S_TIMEOUT 0x80
1438852ecd9SLaurent Vivier
1448852ecd9SLaurent Vivier /* bits in mode register */
1458852ecd9SLaurent Vivier
1468852ecd9SLaurent Vivier #define CLFIFO 0x01
1478852ecd9SLaurent Vivier #define ENBL1 0x02
1488852ecd9SLaurent Vivier #define ENBL2 0x04
1498852ecd9SLaurent Vivier #define ACTION 0x08
1508852ecd9SLaurent Vivier #define WRITE_MODE 0x10
1518852ecd9SLaurent Vivier #define HEDSEL 0x20
1528852ecd9SLaurent Vivier #define MOTON 0x80
1538852ecd9SLaurent Vivier
1548852ecd9SLaurent Vivier /*----------------------------------------------------------------------------*/
1558852ecd9SLaurent Vivier
1568852ecd9SLaurent Vivier enum drive_location {
1578852ecd9SLaurent Vivier INTERNAL_DRIVE = 0x02,
1588852ecd9SLaurent Vivier EXTERNAL_DRIVE = 0x04,
1598852ecd9SLaurent Vivier };
1608852ecd9SLaurent Vivier
1618852ecd9SLaurent Vivier enum media_type {
1628852ecd9SLaurent Vivier DD_MEDIA,
1638852ecd9SLaurent Vivier HD_MEDIA,
1648852ecd9SLaurent Vivier };
1658852ecd9SLaurent Vivier
1668852ecd9SLaurent Vivier struct floppy_state {
1678852ecd9SLaurent Vivier
1688852ecd9SLaurent Vivier /* physical properties */
1698852ecd9SLaurent Vivier
1708852ecd9SLaurent Vivier enum drive_location location; /* internal or external drive */
1718852ecd9SLaurent Vivier int head_number; /* single- or double-sided drive */
1728852ecd9SLaurent Vivier
1738852ecd9SLaurent Vivier /* media */
1748852ecd9SLaurent Vivier
1758852ecd9SLaurent Vivier int disk_in;
1768852ecd9SLaurent Vivier int ejected;
1778852ecd9SLaurent Vivier enum media_type type;
1788852ecd9SLaurent Vivier int write_protected;
1798852ecd9SLaurent Vivier
1808852ecd9SLaurent Vivier int total_secs;
1818852ecd9SLaurent Vivier int secpercyl;
1828852ecd9SLaurent Vivier int secpertrack;
1838852ecd9SLaurent Vivier
1848852ecd9SLaurent Vivier /* in-use information */
1858852ecd9SLaurent Vivier
1868852ecd9SLaurent Vivier int track;
1878852ecd9SLaurent Vivier int ref_count;
1889ef41effSLuis Chamberlain bool registered;
1898852ecd9SLaurent Vivier
1908852ecd9SLaurent Vivier struct gendisk *disk;
191e3896d77SOmar Sandoval struct blk_mq_tag_set tag_set;
1928852ecd9SLaurent Vivier
1938852ecd9SLaurent Vivier /* parent controller */
1948852ecd9SLaurent Vivier
1958852ecd9SLaurent Vivier struct swim_priv *swd;
1968852ecd9SLaurent Vivier };
1978852ecd9SLaurent Vivier
1988852ecd9SLaurent Vivier enum motor_action {
1998852ecd9SLaurent Vivier OFF,
2008852ecd9SLaurent Vivier ON,
2018852ecd9SLaurent Vivier };
2028852ecd9SLaurent Vivier
2038852ecd9SLaurent Vivier enum head {
2048852ecd9SLaurent Vivier LOWER_HEAD = 0,
2058852ecd9SLaurent Vivier UPPER_HEAD = 1,
2068852ecd9SLaurent Vivier };
2078852ecd9SLaurent Vivier
2088852ecd9SLaurent Vivier #define FD_MAX_UNIT 2
2098852ecd9SLaurent Vivier
2108852ecd9SLaurent Vivier struct swim_priv {
2118852ecd9SLaurent Vivier struct swim __iomem *base;
2128852ecd9SLaurent Vivier spinlock_t lock;
2138852ecd9SLaurent Vivier int floppy_count;
2148852ecd9SLaurent Vivier struct floppy_state unit[FD_MAX_UNIT];
2158852ecd9SLaurent Vivier };
2168852ecd9SLaurent Vivier
2178852ecd9SLaurent Vivier extern int swim_read_sector_header(struct swim __iomem *base,
2188852ecd9SLaurent Vivier struct sector_header *header);
2198852ecd9SLaurent Vivier extern int swim_read_sector_data(struct swim __iomem *base,
2208852ecd9SLaurent Vivier unsigned char *data);
2218852ecd9SLaurent Vivier
2222a48fc0aSArnd Bergmann static DEFINE_MUTEX(swim_mutex);
set_swim_mode(struct swim __iomem * base,int enable)2238852ecd9SLaurent Vivier static inline void set_swim_mode(struct swim __iomem *base, int enable)
2248852ecd9SLaurent Vivier {
2258852ecd9SLaurent Vivier struct iwm __iomem *iwm_base;
2268852ecd9SLaurent Vivier unsigned long flags;
2278852ecd9SLaurent Vivier
2288852ecd9SLaurent Vivier if (!enable) {
2298852ecd9SLaurent Vivier swim_write(base, mode0, 0xf8);
2308852ecd9SLaurent Vivier return;
2318852ecd9SLaurent Vivier }
2328852ecd9SLaurent Vivier
2338852ecd9SLaurent Vivier iwm_base = (struct iwm __iomem *)base;
2348852ecd9SLaurent Vivier local_irq_save(flags);
2358852ecd9SLaurent Vivier
2368852ecd9SLaurent Vivier iwm_read(iwm_base, q7L);
2378852ecd9SLaurent Vivier iwm_read(iwm_base, mtrOff);
2388852ecd9SLaurent Vivier iwm_read(iwm_base, q6H);
2398852ecd9SLaurent Vivier
2408852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57);
2418852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x17);
2428852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57);
2438852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57);
2448852ecd9SLaurent Vivier
2458852ecd9SLaurent Vivier local_irq_restore(flags);
2468852ecd9SLaurent Vivier }
2478852ecd9SLaurent Vivier
get_swim_mode(struct swim __iomem * base)2488852ecd9SLaurent Vivier static inline int get_swim_mode(struct swim __iomem *base)
2498852ecd9SLaurent Vivier {
2508852ecd9SLaurent Vivier unsigned long flags;
2518852ecd9SLaurent Vivier
2528852ecd9SLaurent Vivier local_irq_save(flags);
2538852ecd9SLaurent Vivier
2548852ecd9SLaurent Vivier swim_write(base, phase, 0xf5);
2558852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf5)
2568852ecd9SLaurent Vivier goto is_iwm;
2578852ecd9SLaurent Vivier swim_write(base, phase, 0xf6);
2588852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf6)
2598852ecd9SLaurent Vivier goto is_iwm;
2608852ecd9SLaurent Vivier swim_write(base, phase, 0xf7);
2618852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf7)
2628852ecd9SLaurent Vivier goto is_iwm;
2638852ecd9SLaurent Vivier local_irq_restore(flags);
2648852ecd9SLaurent Vivier return 1;
2658852ecd9SLaurent Vivier is_iwm:
2668852ecd9SLaurent Vivier local_irq_restore(flags);
2678852ecd9SLaurent Vivier return 0;
2688852ecd9SLaurent Vivier }
2698852ecd9SLaurent Vivier
swim_select(struct swim __iomem * base,int sel)2708852ecd9SLaurent Vivier static inline void swim_select(struct swim __iomem *base, int sel)
2718852ecd9SLaurent Vivier {
2728852ecd9SLaurent Vivier swim_write(base, phase, RELAX);
2738852ecd9SLaurent Vivier
2748852ecd9SLaurent Vivier via1_set_head(sel & 0x100);
2758852ecd9SLaurent Vivier
2768852ecd9SLaurent Vivier swim_write(base, phase, sel & CA_MASK);
2778852ecd9SLaurent Vivier }
2788852ecd9SLaurent Vivier
swim_action(struct swim __iomem * base,int action)2798852ecd9SLaurent Vivier static inline void swim_action(struct swim __iomem *base, int action)
2808852ecd9SLaurent Vivier {
2818852ecd9SLaurent Vivier unsigned long flags;
2828852ecd9SLaurent Vivier
2838852ecd9SLaurent Vivier local_irq_save(flags);
2848852ecd9SLaurent Vivier
2858852ecd9SLaurent Vivier swim_select(base, action);
2868852ecd9SLaurent Vivier udelay(1);
2878852ecd9SLaurent Vivier swim_write(base, phase, (LSTRB<<4) | LSTRB);
2888852ecd9SLaurent Vivier udelay(1);
2898852ecd9SLaurent Vivier swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F));
2908852ecd9SLaurent Vivier udelay(1);
2918852ecd9SLaurent Vivier
2928852ecd9SLaurent Vivier local_irq_restore(flags);
2938852ecd9SLaurent Vivier }
2948852ecd9SLaurent Vivier
swim_readbit(struct swim __iomem * base,int bit)2958852ecd9SLaurent Vivier static inline int swim_readbit(struct swim __iomem *base, int bit)
2968852ecd9SLaurent Vivier {
2978852ecd9SLaurent Vivier int stat;
2988852ecd9SLaurent Vivier
2998852ecd9SLaurent Vivier swim_select(base, bit);
3008852ecd9SLaurent Vivier
3018852ecd9SLaurent Vivier udelay(10);
3028852ecd9SLaurent Vivier
3038852ecd9SLaurent Vivier stat = swim_read(base, handshake);
3048852ecd9SLaurent Vivier
3058852ecd9SLaurent Vivier return (stat & SENSE) == 0;
3068852ecd9SLaurent Vivier }
3078852ecd9SLaurent Vivier
swim_drive(struct swim __iomem * base,enum drive_location location)3088852ecd9SLaurent Vivier static inline void swim_drive(struct swim __iomem *base,
3098852ecd9SLaurent Vivier enum drive_location location)
3108852ecd9SLaurent Vivier {
3118852ecd9SLaurent Vivier if (location == INTERNAL_DRIVE) {
3128852ecd9SLaurent Vivier swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
3138852ecd9SLaurent Vivier swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
3148852ecd9SLaurent Vivier } else if (location == EXTERNAL_DRIVE) {
3158852ecd9SLaurent Vivier swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
3168852ecd9SLaurent Vivier swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
3178852ecd9SLaurent Vivier }
3188852ecd9SLaurent Vivier }
3198852ecd9SLaurent Vivier
swim_motor(struct swim __iomem * base,enum motor_action action)3208852ecd9SLaurent Vivier static inline void swim_motor(struct swim __iomem *base,
3218852ecd9SLaurent Vivier enum motor_action action)
3228852ecd9SLaurent Vivier {
3238852ecd9SLaurent Vivier if (action == ON) {
3248852ecd9SLaurent Vivier int i;
3258852ecd9SLaurent Vivier
3268852ecd9SLaurent Vivier swim_action(base, MOTOR_ON);
3278852ecd9SLaurent Vivier
3288852ecd9SLaurent Vivier for (i = 0; i < 2*HZ; i++) {
3298852ecd9SLaurent Vivier swim_select(base, RELAX);
3308852ecd9SLaurent Vivier if (swim_readbit(base, MOTOR_ON))
3318852ecd9SLaurent Vivier break;
332c6516565SXu Wang set_current_state(TASK_INTERRUPTIBLE);
3338852ecd9SLaurent Vivier schedule_timeout(1);
3348852ecd9SLaurent Vivier }
3358852ecd9SLaurent Vivier } else if (action == OFF) {
3368852ecd9SLaurent Vivier swim_action(base, MOTOR_OFF);
3378852ecd9SLaurent Vivier swim_select(base, RELAX);
3388852ecd9SLaurent Vivier }
3398852ecd9SLaurent Vivier }
3408852ecd9SLaurent Vivier
swim_eject(struct swim __iomem * base)3418852ecd9SLaurent Vivier static inline void swim_eject(struct swim __iomem *base)
3428852ecd9SLaurent Vivier {
3438852ecd9SLaurent Vivier int i;
3448852ecd9SLaurent Vivier
3458852ecd9SLaurent Vivier swim_action(base, EJECT);
3468852ecd9SLaurent Vivier
3478852ecd9SLaurent Vivier for (i = 0; i < 2*HZ; i++) {
3488852ecd9SLaurent Vivier swim_select(base, RELAX);
3498852ecd9SLaurent Vivier if (!swim_readbit(base, DISK_IN))
3508852ecd9SLaurent Vivier break;
351c6516565SXu Wang set_current_state(TASK_INTERRUPTIBLE);
3528852ecd9SLaurent Vivier schedule_timeout(1);
3538852ecd9SLaurent Vivier }
3548852ecd9SLaurent Vivier swim_select(base, RELAX);
3558852ecd9SLaurent Vivier }
3568852ecd9SLaurent Vivier
swim_head(struct swim __iomem * base,enum head head)3578852ecd9SLaurent Vivier static inline void swim_head(struct swim __iomem *base, enum head head)
3588852ecd9SLaurent Vivier {
3598852ecd9SLaurent Vivier /* wait drive is ready */
3608852ecd9SLaurent Vivier
3618852ecd9SLaurent Vivier if (head == UPPER_HEAD)
3628852ecd9SLaurent Vivier swim_select(base, READ_DATA_1);
3638852ecd9SLaurent Vivier else if (head == LOWER_HEAD)
3648852ecd9SLaurent Vivier swim_select(base, READ_DATA_0);
3658852ecd9SLaurent Vivier }
3668852ecd9SLaurent Vivier
swim_step(struct swim __iomem * base)3678852ecd9SLaurent Vivier static inline int swim_step(struct swim __iomem *base)
3688852ecd9SLaurent Vivier {
3698852ecd9SLaurent Vivier int wait;
3708852ecd9SLaurent Vivier
3718852ecd9SLaurent Vivier swim_action(base, STEP);
3728852ecd9SLaurent Vivier
3738852ecd9SLaurent Vivier for (wait = 0; wait < HZ; wait++) {
3748852ecd9SLaurent Vivier
375c6516565SXu Wang set_current_state(TASK_INTERRUPTIBLE);
3768852ecd9SLaurent Vivier schedule_timeout(1);
3778852ecd9SLaurent Vivier
3788852ecd9SLaurent Vivier swim_select(base, RELAX);
3798852ecd9SLaurent Vivier if (!swim_readbit(base, STEP))
3808852ecd9SLaurent Vivier return 0;
3818852ecd9SLaurent Vivier }
3828852ecd9SLaurent Vivier return -1;
3838852ecd9SLaurent Vivier }
3848852ecd9SLaurent Vivier
swim_track00(struct swim __iomem * base)3858852ecd9SLaurent Vivier static inline int swim_track00(struct swim __iomem *base)
3868852ecd9SLaurent Vivier {
3878852ecd9SLaurent Vivier int try;
3888852ecd9SLaurent Vivier
3898852ecd9SLaurent Vivier swim_action(base, SEEK_NEGATIVE);
3908852ecd9SLaurent Vivier
3918852ecd9SLaurent Vivier for (try = 0; try < 100; try++) {
3928852ecd9SLaurent Vivier
3938852ecd9SLaurent Vivier swim_select(base, RELAX);
3948852ecd9SLaurent Vivier if (swim_readbit(base, TRACK_ZERO))
3958852ecd9SLaurent Vivier break;
3968852ecd9SLaurent Vivier
3978852ecd9SLaurent Vivier if (swim_step(base))
3988852ecd9SLaurent Vivier return -1;
3998852ecd9SLaurent Vivier }
4008852ecd9SLaurent Vivier
4018852ecd9SLaurent Vivier if (swim_readbit(base, TRACK_ZERO))
4028852ecd9SLaurent Vivier return 0;
4038852ecd9SLaurent Vivier
4048852ecd9SLaurent Vivier return -1;
4058852ecd9SLaurent Vivier }
4068852ecd9SLaurent Vivier
swim_seek(struct swim __iomem * base,int step)4078852ecd9SLaurent Vivier static inline int swim_seek(struct swim __iomem *base, int step)
4088852ecd9SLaurent Vivier {
4098852ecd9SLaurent Vivier if (step == 0)
4108852ecd9SLaurent Vivier return 0;
4118852ecd9SLaurent Vivier
4128852ecd9SLaurent Vivier if (step < 0) {
4138852ecd9SLaurent Vivier swim_action(base, SEEK_NEGATIVE);
4148852ecd9SLaurent Vivier step = -step;
4158852ecd9SLaurent Vivier } else
4168852ecd9SLaurent Vivier swim_action(base, SEEK_POSITIVE);
4178852ecd9SLaurent Vivier
4188852ecd9SLaurent Vivier for ( ; step > 0; step--) {
4198852ecd9SLaurent Vivier if (swim_step(base))
4208852ecd9SLaurent Vivier return -1;
4218852ecd9SLaurent Vivier }
4228852ecd9SLaurent Vivier
4238852ecd9SLaurent Vivier return 0;
4248852ecd9SLaurent Vivier }
4258852ecd9SLaurent Vivier
swim_track(struct floppy_state * fs,int track)4268852ecd9SLaurent Vivier static inline int swim_track(struct floppy_state *fs, int track)
4278852ecd9SLaurent Vivier {
4288852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
4298852ecd9SLaurent Vivier int ret;
4308852ecd9SLaurent Vivier
4318852ecd9SLaurent Vivier ret = swim_seek(base, track - fs->track);
4328852ecd9SLaurent Vivier
4338852ecd9SLaurent Vivier if (ret == 0)
4348852ecd9SLaurent Vivier fs->track = track;
4358852ecd9SLaurent Vivier else {
4368852ecd9SLaurent Vivier swim_track00(base);
4378852ecd9SLaurent Vivier fs->track = 0;
4388852ecd9SLaurent Vivier }
4398852ecd9SLaurent Vivier
4408852ecd9SLaurent Vivier return ret;
4418852ecd9SLaurent Vivier }
4428852ecd9SLaurent Vivier
floppy_eject(struct floppy_state * fs)4438852ecd9SLaurent Vivier static int floppy_eject(struct floppy_state *fs)
4448852ecd9SLaurent Vivier {
4458852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
4468852ecd9SLaurent Vivier
4478852ecd9SLaurent Vivier swim_drive(base, fs->location);
4488852ecd9SLaurent Vivier swim_motor(base, OFF);
4498852ecd9SLaurent Vivier swim_eject(base);
4508852ecd9SLaurent Vivier
4518852ecd9SLaurent Vivier fs->disk_in = 0;
4528852ecd9SLaurent Vivier fs->ejected = 1;
4538852ecd9SLaurent Vivier
4548852ecd9SLaurent Vivier return 0;
4558852ecd9SLaurent Vivier }
4568852ecd9SLaurent Vivier
swim_read_sector(struct floppy_state * fs,int side,int track,int sector,unsigned char * buffer)4578852ecd9SLaurent Vivier static inline int swim_read_sector(struct floppy_state *fs,
4588852ecd9SLaurent Vivier int side, int track,
4598852ecd9SLaurent Vivier int sector, unsigned char *buffer)
4608852ecd9SLaurent Vivier {
4618852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
4628852ecd9SLaurent Vivier unsigned long flags;
4638852ecd9SLaurent Vivier struct sector_header header;
4648852ecd9SLaurent Vivier int ret = -1;
4658852ecd9SLaurent Vivier short i;
4668852ecd9SLaurent Vivier
4678852ecd9SLaurent Vivier swim_track(fs, track);
4688852ecd9SLaurent Vivier
4698852ecd9SLaurent Vivier swim_write(base, mode1, MOTON);
4708852ecd9SLaurent Vivier swim_head(base, side);
4718852ecd9SLaurent Vivier swim_write(base, mode0, side);
4728852ecd9SLaurent Vivier
4738852ecd9SLaurent Vivier local_irq_save(flags);
4748852ecd9SLaurent Vivier for (i = 0; i < 36; i++) {
4758852ecd9SLaurent Vivier ret = swim_read_sector_header(base, &header);
4768852ecd9SLaurent Vivier if (!ret && (header.sector == sector)) {
4778852ecd9SLaurent Vivier /* found */
4788852ecd9SLaurent Vivier
4798852ecd9SLaurent Vivier ret = swim_read_sector_data(base, buffer);
4808852ecd9SLaurent Vivier break;
4818852ecd9SLaurent Vivier }
4828852ecd9SLaurent Vivier }
4838852ecd9SLaurent Vivier local_irq_restore(flags);
4848852ecd9SLaurent Vivier
4858852ecd9SLaurent Vivier swim_write(base, mode0, MOTON);
4868852ecd9SLaurent Vivier
4878852ecd9SLaurent Vivier if ((header.side != side) || (header.track != track) ||
4888852ecd9SLaurent Vivier (header.sector != sector))
4898852ecd9SLaurent Vivier return 0;
4908852ecd9SLaurent Vivier
4918852ecd9SLaurent Vivier return ret;
4928852ecd9SLaurent Vivier }
4938852ecd9SLaurent Vivier
floppy_read_sectors(struct floppy_state * fs,int req_sector,int sectors_nb,unsigned char * buffer)4942a842acaSChristoph Hellwig static blk_status_t floppy_read_sectors(struct floppy_state *fs,
4958852ecd9SLaurent Vivier int req_sector, int sectors_nb,
4968852ecd9SLaurent Vivier unsigned char *buffer)
4978852ecd9SLaurent Vivier {
4988852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
4998852ecd9SLaurent Vivier int ret;
5008852ecd9SLaurent Vivier int side, track, sector;
5018852ecd9SLaurent Vivier int i, try;
5028852ecd9SLaurent Vivier
5038852ecd9SLaurent Vivier
5048852ecd9SLaurent Vivier swim_drive(base, fs->location);
5058852ecd9SLaurent Vivier for (i = req_sector; i < req_sector + sectors_nb; i++) {
5068852ecd9SLaurent Vivier int x;
5078852ecd9SLaurent Vivier track = i / fs->secpercyl;
5088852ecd9SLaurent Vivier x = i % fs->secpercyl;
5098852ecd9SLaurent Vivier side = x / fs->secpertrack;
5108852ecd9SLaurent Vivier sector = x % fs->secpertrack + 1;
5118852ecd9SLaurent Vivier
5128852ecd9SLaurent Vivier try = 5;
5138852ecd9SLaurent Vivier do {
5148852ecd9SLaurent Vivier ret = swim_read_sector(fs, side, track, sector,
5158852ecd9SLaurent Vivier buffer);
5168852ecd9SLaurent Vivier if (try-- == 0)
5172a842acaSChristoph Hellwig return BLK_STS_IOERR;
5188852ecd9SLaurent Vivier } while (ret != 512);
5198852ecd9SLaurent Vivier
5208852ecd9SLaurent Vivier buffer += ret;
5218852ecd9SLaurent Vivier }
5228852ecd9SLaurent Vivier
5238852ecd9SLaurent Vivier return 0;
5248852ecd9SLaurent Vivier }
5258852ecd9SLaurent Vivier
swim_queue_rq(struct blk_mq_hw_ctx * hctx,const struct blk_mq_queue_data * bd)526e3896d77SOmar Sandoval static blk_status_t swim_queue_rq(struct blk_mq_hw_ctx *hctx,
527e3896d77SOmar Sandoval const struct blk_mq_queue_data *bd)
5288852ecd9SLaurent Vivier {
529e3896d77SOmar Sandoval struct floppy_state *fs = hctx->queue->queuedata;
530e3896d77SOmar Sandoval struct swim_priv *swd = fs->swd;
531e3896d77SOmar Sandoval struct request *req = bd->rq;
532e3896d77SOmar Sandoval blk_status_t err;
533e3896d77SOmar Sandoval
534e3896d77SOmar Sandoval if (!spin_trylock_irq(&swd->lock))
535e3896d77SOmar Sandoval return BLK_STS_DEV_RESOURCE;
536e3896d77SOmar Sandoval
537e3896d77SOmar Sandoval blk_mq_start_request(req);
538e3896d77SOmar Sandoval
539e3896d77SOmar Sandoval if (!fs->disk_in || rq_data_dir(req) == WRITE) {
540e3896d77SOmar Sandoval err = BLK_STS_IOERR;
541e3896d77SOmar Sandoval goto out;
542e3896d77SOmar Sandoval }
543103db8b2SOmar Sandoval
544103db8b2SOmar Sandoval do {
54506b0608eSTejun Heo err = floppy_read_sectors(fs, blk_rq_pos(req),
54683096ebfSTejun Heo blk_rq_cur_sectors(req),
547b4f42e28SJens Axboe bio_data(req->bio));
548e3896d77SOmar Sandoval } while (blk_update_request(req, err, blk_rq_cur_bytes(req)));
549e3896d77SOmar Sandoval __blk_mq_end_request(req, err);
550e3896d77SOmar Sandoval
551e3896d77SOmar Sandoval err = BLK_STS_OK;
552e3896d77SOmar Sandoval out:
553e3896d77SOmar Sandoval spin_unlock_irq(&swd->lock);
554e3896d77SOmar Sandoval return err;
555e3896d77SOmar Sandoval
5568852ecd9SLaurent Vivier }
5578852ecd9SLaurent Vivier
5588852ecd9SLaurent Vivier static struct floppy_struct floppy_type[4] = {
5598852ecd9SLaurent Vivier { 0, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing */
5608852ecd9SLaurent Vivier { 720, 9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
5618852ecd9SLaurent Vivier { 1440, 9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5" */
5628852ecd9SLaurent Vivier { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5" */
5638852ecd9SLaurent Vivier };
5648852ecd9SLaurent Vivier
get_floppy_geometry(struct floppy_state * fs,int type,struct floppy_struct ** g)5658852ecd9SLaurent Vivier static int get_floppy_geometry(struct floppy_state *fs, int type,
5668852ecd9SLaurent Vivier struct floppy_struct **g)
5678852ecd9SLaurent Vivier {
5688852ecd9SLaurent Vivier if (type >= ARRAY_SIZE(floppy_type))
5698852ecd9SLaurent Vivier return -EINVAL;
5708852ecd9SLaurent Vivier
5718852ecd9SLaurent Vivier if (type)
5728852ecd9SLaurent Vivier *g = &floppy_type[type];
5738852ecd9SLaurent Vivier else if (fs->type == HD_MEDIA) /* High-Density media */
5748852ecd9SLaurent Vivier *g = &floppy_type[3];
5758852ecd9SLaurent Vivier else if (fs->head_number == 2) /* double-sided */
5768852ecd9SLaurent Vivier *g = &floppy_type[2];
5778852ecd9SLaurent Vivier else
5788852ecd9SLaurent Vivier *g = &floppy_type[1];
5798852ecd9SLaurent Vivier
5808852ecd9SLaurent Vivier return 0;
5818852ecd9SLaurent Vivier }
5828852ecd9SLaurent Vivier
setup_medium(struct floppy_state * fs)5838852ecd9SLaurent Vivier static void setup_medium(struct floppy_state *fs)
5848852ecd9SLaurent Vivier {
5858852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
5868852ecd9SLaurent Vivier
5878852ecd9SLaurent Vivier if (swim_readbit(base, DISK_IN)) {
5888852ecd9SLaurent Vivier struct floppy_struct *g;
5898852ecd9SLaurent Vivier fs->disk_in = 1;
5908852ecd9SLaurent Vivier fs->write_protected = swim_readbit(base, WRITE_PROT);
5918852ecd9SLaurent Vivier
5928852ecd9SLaurent Vivier if (swim_track00(base))
5938852ecd9SLaurent Vivier printk(KERN_ERR
5948852ecd9SLaurent Vivier "SWIM: cannot move floppy head to track 0\n");
5958852ecd9SLaurent Vivier
5968852ecd9SLaurent Vivier swim_track00(base);
5978852ecd9SLaurent Vivier
5985a13388dSFinn Thain fs->type = swim_readbit(base, TWOMEG_MEDIA) ?
5995a13388dSFinn Thain HD_MEDIA : DD_MEDIA;
6005a13388dSFinn Thain fs->head_number = swim_readbit(base, SINGLE_SIDED) ? 1 : 2;
6018852ecd9SLaurent Vivier get_floppy_geometry(fs, 0, &g);
6028852ecd9SLaurent Vivier fs->total_secs = g->size;
6038852ecd9SLaurent Vivier fs->secpercyl = g->head * g->sect;
6048852ecd9SLaurent Vivier fs->secpertrack = g->sect;
6058852ecd9SLaurent Vivier fs->track = 0;
6068852ecd9SLaurent Vivier } else {
6078852ecd9SLaurent Vivier fs->disk_in = 0;
6088852ecd9SLaurent Vivier }
6098852ecd9SLaurent Vivier }
6108852ecd9SLaurent Vivier
floppy_open(struct gendisk * disk,blk_mode_t mode)61105bdb996SChristoph Hellwig static int floppy_open(struct gendisk *disk, blk_mode_t mode)
6128852ecd9SLaurent Vivier {
613d32e2bf8SChristoph Hellwig struct floppy_state *fs = disk->private_data;
6148852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
6158852ecd9SLaurent Vivier int err;
6168852ecd9SLaurent Vivier
61705bdb996SChristoph Hellwig if (fs->ref_count == -1 || (fs->ref_count && mode & BLK_OPEN_EXCL))
6188852ecd9SLaurent Vivier return -EBUSY;
61905bdb996SChristoph Hellwig if (mode & BLK_OPEN_EXCL)
6208852ecd9SLaurent Vivier fs->ref_count = -1;
6218852ecd9SLaurent Vivier else
6228852ecd9SLaurent Vivier fs->ref_count++;
6238852ecd9SLaurent Vivier swim_write(base, setup, S_IBM_DRIVE | S_FCLK_DIV2);
6248852ecd9SLaurent Vivier udelay(10);
625b3906535SFinn Thain swim_drive(base, fs->location);
6268852ecd9SLaurent Vivier swim_motor(base, ON);
6278852ecd9SLaurent Vivier swim_action(base, SETMFM);
6288852ecd9SLaurent Vivier if (fs->ejected)
6298852ecd9SLaurent Vivier setup_medium(fs);
6308852ecd9SLaurent Vivier if (!fs->disk_in) {
6318852ecd9SLaurent Vivier err = -ENXIO;
6328852ecd9SLaurent Vivier goto out;
6338852ecd9SLaurent Vivier }
6348852ecd9SLaurent Vivier
6355a13388dSFinn Thain set_capacity(fs->disk, fs->total_secs);
6365a13388dSFinn Thain
63705bdb996SChristoph Hellwig if (mode & BLK_OPEN_NDELAY)
6388852ecd9SLaurent Vivier return 0;
6398852ecd9SLaurent Vivier
64005bdb996SChristoph Hellwig if (mode & (BLK_OPEN_READ | BLK_OPEN_WRITE)) {
641d32e2bf8SChristoph Hellwig if (disk_check_media_change(disk) && fs->disk_in)
642433d21ceSChristoph Hellwig fs->ejected = 0;
643*9a7933f3SChristoph Hellwig if ((mode & BLK_OPEN_WRITE) && fs->write_protected) {
6448852ecd9SLaurent Vivier err = -EROFS;
6458852ecd9SLaurent Vivier goto out;
6468852ecd9SLaurent Vivier }
6478852ecd9SLaurent Vivier }
6488852ecd9SLaurent Vivier return 0;
6498852ecd9SLaurent Vivier out:
6508852ecd9SLaurent Vivier if (fs->ref_count < 0)
6518852ecd9SLaurent Vivier fs->ref_count = 0;
6528852ecd9SLaurent Vivier else if (fs->ref_count > 0)
6538852ecd9SLaurent Vivier --fs->ref_count;
6548852ecd9SLaurent Vivier
6558852ecd9SLaurent Vivier if (fs->ref_count == 0)
6568852ecd9SLaurent Vivier swim_motor(base, OFF);
6578852ecd9SLaurent Vivier return err;
6588852ecd9SLaurent Vivier }
6598852ecd9SLaurent Vivier
floppy_unlocked_open(struct gendisk * disk,blk_mode_t mode)66005bdb996SChristoph Hellwig static int floppy_unlocked_open(struct gendisk *disk, blk_mode_t mode)
6616e9624b8SArnd Bergmann {
6626e9624b8SArnd Bergmann int ret;
6636e9624b8SArnd Bergmann
6642a48fc0aSArnd Bergmann mutex_lock(&swim_mutex);
665d32e2bf8SChristoph Hellwig ret = floppy_open(disk, mode);
6662a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex);
6676e9624b8SArnd Bergmann
6686e9624b8SArnd Bergmann return ret;
6696e9624b8SArnd Bergmann }
6706e9624b8SArnd Bergmann
floppy_release(struct gendisk * disk)671ae220766SChristoph Hellwig static void floppy_release(struct gendisk *disk)
6728852ecd9SLaurent Vivier {
6738852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data;
6748852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base;
6758852ecd9SLaurent Vivier
6762a48fc0aSArnd Bergmann mutex_lock(&swim_mutex);
6778852ecd9SLaurent Vivier if (fs->ref_count < 0)
6788852ecd9SLaurent Vivier fs->ref_count = 0;
6798852ecd9SLaurent Vivier else if (fs->ref_count > 0)
6808852ecd9SLaurent Vivier --fs->ref_count;
6818852ecd9SLaurent Vivier
6828852ecd9SLaurent Vivier if (fs->ref_count == 0)
6838852ecd9SLaurent Vivier swim_motor(base, OFF);
6842a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex);
6858852ecd9SLaurent Vivier }
6868852ecd9SLaurent Vivier
floppy_ioctl(struct block_device * bdev,blk_mode_t mode,unsigned int cmd,unsigned long param)68705bdb996SChristoph Hellwig static int floppy_ioctl(struct block_device *bdev, blk_mode_t mode,
6888852ecd9SLaurent Vivier unsigned int cmd, unsigned long param)
6898852ecd9SLaurent Vivier {
6908852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data;
6918852ecd9SLaurent Vivier int err;
6928852ecd9SLaurent Vivier
6938852ecd9SLaurent Vivier if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
6948852ecd9SLaurent Vivier return -EPERM;
6958852ecd9SLaurent Vivier
6968852ecd9SLaurent Vivier switch (cmd) {
6978852ecd9SLaurent Vivier case FDEJECT:
6988852ecd9SLaurent Vivier if (fs->ref_count != 1)
6998852ecd9SLaurent Vivier return -EBUSY;
7002a48fc0aSArnd Bergmann mutex_lock(&swim_mutex);
7018852ecd9SLaurent Vivier err = floppy_eject(fs);
7022a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex);
7038852ecd9SLaurent Vivier return err;
7048852ecd9SLaurent Vivier
7058852ecd9SLaurent Vivier case FDGETPRM:
7068852ecd9SLaurent Vivier if (copy_to_user((void __user *) param, (void *) &floppy_type,
7078852ecd9SLaurent Vivier sizeof(struct floppy_struct)))
7088852ecd9SLaurent Vivier return -EFAULT;
7098852ecd9SLaurent Vivier return 0;
7108852ecd9SLaurent Vivier }
7118e2ab5a4SFinn Thain return -ENOTTY;
7128e2ab5a4SFinn Thain }
7138852ecd9SLaurent Vivier
floppy_getgeo(struct block_device * bdev,struct hd_geometry * geo)7148852ecd9SLaurent Vivier static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo)
7158852ecd9SLaurent Vivier {
7168852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data;
7178852ecd9SLaurent Vivier struct floppy_struct *g;
7188852ecd9SLaurent Vivier int ret;
7198852ecd9SLaurent Vivier
7208852ecd9SLaurent Vivier ret = get_floppy_geometry(fs, 0, &g);
7218852ecd9SLaurent Vivier if (ret)
7228852ecd9SLaurent Vivier return ret;
7238852ecd9SLaurent Vivier
7248852ecd9SLaurent Vivier geo->heads = g->head;
7258852ecd9SLaurent Vivier geo->sectors = g->sect;
7268852ecd9SLaurent Vivier geo->cylinders = g->track;
7278852ecd9SLaurent Vivier
7288852ecd9SLaurent Vivier return 0;
7298852ecd9SLaurent Vivier }
7308852ecd9SLaurent Vivier
floppy_check_events(struct gendisk * disk,unsigned int clearing)7314bbde777STejun Heo static unsigned int floppy_check_events(struct gendisk *disk,
7324bbde777STejun Heo unsigned int clearing)
7338852ecd9SLaurent Vivier {
7348852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data;
7358852ecd9SLaurent Vivier
7364bbde777STejun Heo return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
7378852ecd9SLaurent Vivier }
7388852ecd9SLaurent Vivier
73983d5cde4SAlexey Dobriyan static const struct block_device_operations floppy_fops = {
7408852ecd9SLaurent Vivier .owner = THIS_MODULE,
7416e9624b8SArnd Bergmann .open = floppy_unlocked_open,
7428852ecd9SLaurent Vivier .release = floppy_release,
7438a6cfeb6SArnd Bergmann .ioctl = floppy_ioctl,
7448852ecd9SLaurent Vivier .getgeo = floppy_getgeo,
7454bbde777STejun Heo .check_events = floppy_check_events,
7468852ecd9SLaurent Vivier };
7478852ecd9SLaurent Vivier
swim_add_floppy(struct swim_priv * swd,enum drive_location location)7488d85fce7SGreg Kroah-Hartman static int swim_add_floppy(struct swim_priv *swd, enum drive_location location)
7498852ecd9SLaurent Vivier {
7508852ecd9SLaurent Vivier struct floppy_state *fs = &swd->unit[swd->floppy_count];
7518852ecd9SLaurent Vivier struct swim __iomem *base = swd->base;
7528852ecd9SLaurent Vivier
7538852ecd9SLaurent Vivier fs->location = location;
7548852ecd9SLaurent Vivier
7558852ecd9SLaurent Vivier swim_drive(base, location);
7568852ecd9SLaurent Vivier
7578852ecd9SLaurent Vivier swim_motor(base, OFF);
7588852ecd9SLaurent Vivier
7595a13388dSFinn Thain fs->type = HD_MEDIA;
7608852ecd9SLaurent Vivier fs->head_number = 2;
7615a13388dSFinn Thain
7628852ecd9SLaurent Vivier fs->ref_count = 0;
7638852ecd9SLaurent Vivier fs->ejected = 1;
7648852ecd9SLaurent Vivier
7658852ecd9SLaurent Vivier swd->floppy_count++;
7668852ecd9SLaurent Vivier
7678852ecd9SLaurent Vivier return 0;
7688852ecd9SLaurent Vivier }
7698852ecd9SLaurent Vivier
770e3896d77SOmar Sandoval static const struct blk_mq_ops swim_mq_ops = {
771e3896d77SOmar Sandoval .queue_rq = swim_queue_rq,
772e3896d77SOmar Sandoval };
773e3896d77SOmar Sandoval
swim_cleanup_floppy_disk(struct floppy_state * fs)7744e9abe72SLuis Chamberlain static void swim_cleanup_floppy_disk(struct floppy_state *fs)
7754e9abe72SLuis Chamberlain {
7764e9abe72SLuis Chamberlain struct gendisk *disk = fs->disk;
7774e9abe72SLuis Chamberlain
7784e9abe72SLuis Chamberlain if (!disk)
7794e9abe72SLuis Chamberlain return;
7804e9abe72SLuis Chamberlain
7819ef41effSLuis Chamberlain if (fs->registered)
7829ef41effSLuis Chamberlain del_gendisk(fs->disk);
7839ef41effSLuis Chamberlain
7848b9ab626SChristoph Hellwig put_disk(disk);
7854e9abe72SLuis Chamberlain blk_mq_free_tag_set(&fs->tag_set);
7864e9abe72SLuis Chamberlain }
7874e9abe72SLuis Chamberlain
swim_floppy_init(struct swim_priv * swd)7888d85fce7SGreg Kroah-Hartman static int swim_floppy_init(struct swim_priv *swd)
7898852ecd9SLaurent Vivier {
7908852ecd9SLaurent Vivier int err;
7918852ecd9SLaurent Vivier int drive;
7928852ecd9SLaurent Vivier struct swim __iomem *base = swd->base;
7938852ecd9SLaurent Vivier
7948852ecd9SLaurent Vivier /* scan floppy drives */
7958852ecd9SLaurent Vivier
7968852ecd9SLaurent Vivier swim_drive(base, INTERNAL_DRIVE);
7978a500df6SFinn Thain if (swim_readbit(base, DRIVE_PRESENT) &&
7988a500df6SFinn Thain !swim_readbit(base, ONEMEG_DRIVE))
7998852ecd9SLaurent Vivier swim_add_floppy(swd, INTERNAL_DRIVE);
8008852ecd9SLaurent Vivier swim_drive(base, EXTERNAL_DRIVE);
8018a500df6SFinn Thain if (swim_readbit(base, DRIVE_PRESENT) &&
8028a500df6SFinn Thain !swim_readbit(base, ONEMEG_DRIVE))
8038852ecd9SLaurent Vivier swim_add_floppy(swd, EXTERNAL_DRIVE);
8048852ecd9SLaurent Vivier
8058852ecd9SLaurent Vivier /* register floppy drives */
8068852ecd9SLaurent Vivier
8078852ecd9SLaurent Vivier err = register_blkdev(FLOPPY_MAJOR, "fd");
8088852ecd9SLaurent Vivier if (err) {
8098852ecd9SLaurent Vivier printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
8108852ecd9SLaurent Vivier FLOPPY_MAJOR);
8118852ecd9SLaurent Vivier return -EBUSY;
8128852ecd9SLaurent Vivier }
8138852ecd9SLaurent Vivier
814103db8b2SOmar Sandoval spin_lock_init(&swd->lock);
815103db8b2SOmar Sandoval
8168852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) {
81751fbfedfSChristoph Hellwig err = blk_mq_alloc_sq_tag_set(&swd->unit[drive].tag_set,
81851fbfedfSChristoph Hellwig &swim_mq_ops, 2, BLK_MQ_F_SHOULD_MERGE);
81951fbfedfSChristoph Hellwig if (err)
82051fbfedfSChristoph Hellwig goto exit_put_disks;
821e3896d77SOmar Sandoval
82251fbfedfSChristoph Hellwig swd->unit[drive].disk =
82351fbfedfSChristoph Hellwig blk_mq_alloc_disk(&swd->unit[drive].tag_set,
82451fbfedfSChristoph Hellwig &swd->unit[drive]);
82551fbfedfSChristoph Hellwig if (IS_ERR(swd->unit[drive].disk)) {
82651fbfedfSChristoph Hellwig blk_mq_free_tag_set(&swd->unit[drive].tag_set);
82751fbfedfSChristoph Hellwig err = PTR_ERR(swd->unit[drive].disk);
8288852ecd9SLaurent Vivier goto exit_put_disks;
8298852ecd9SLaurent Vivier }
830e3896d77SOmar Sandoval
831103db8b2SOmar Sandoval swd->unit[drive].swd = swd;
832103db8b2SOmar Sandoval }
8338852ecd9SLaurent Vivier
8348852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) {
8358852ecd9SLaurent Vivier swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
8368852ecd9SLaurent Vivier swd->unit[drive].disk->major = FLOPPY_MAJOR;
8378852ecd9SLaurent Vivier swd->unit[drive].disk->first_minor = drive;
83851fbfedfSChristoph Hellwig swd->unit[drive].disk->minors = 1;
8398852ecd9SLaurent Vivier sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
8408852ecd9SLaurent Vivier swd->unit[drive].disk->fops = &floppy_fops;
8411ebe2e5fSChristoph Hellwig swd->unit[drive].disk->flags |= GENHD_FL_NO_PART;
842773008f6SMartin Wilck swd->unit[drive].disk->events = DISK_EVENT_MEDIA_CHANGE;
8438852ecd9SLaurent Vivier swd->unit[drive].disk->private_data = &swd->unit[drive];
8448852ecd9SLaurent Vivier set_capacity(swd->unit[drive].disk, 2880);
845625a28a7SLuis Chamberlain err = add_disk(swd->unit[drive].disk);
846625a28a7SLuis Chamberlain if (err)
847625a28a7SLuis Chamberlain goto exit_put_disks;
8489ef41effSLuis Chamberlain swd->unit[drive].registered = true;
8498852ecd9SLaurent Vivier }
8508852ecd9SLaurent Vivier
8518852ecd9SLaurent Vivier return 0;
8528852ecd9SLaurent Vivier
8538852ecd9SLaurent Vivier exit_put_disks:
8548852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd");
8551448a2a5SOmar Sandoval do {
8564e9abe72SLuis Chamberlain swim_cleanup_floppy_disk(&swd->unit[drive]);
8571448a2a5SOmar Sandoval } while (drive--);
8588852ecd9SLaurent Vivier return err;
8598852ecd9SLaurent Vivier }
8608852ecd9SLaurent Vivier
swim_probe(struct platform_device * dev)8618d85fce7SGreg Kroah-Hartman static int swim_probe(struct platform_device *dev)
8628852ecd9SLaurent Vivier {
8638852ecd9SLaurent Vivier struct resource *res;
8648852ecd9SLaurent Vivier struct swim __iomem *swim_base;
8658852ecd9SLaurent Vivier struct swim_priv *swd;
8668852ecd9SLaurent Vivier int ret;
8678852ecd9SLaurent Vivier
8682724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0);
8698852ecd9SLaurent Vivier if (!res) {
8708852ecd9SLaurent Vivier ret = -ENODEV;
8718852ecd9SLaurent Vivier goto out;
8728852ecd9SLaurent Vivier }
8738852ecd9SLaurent Vivier
8748852ecd9SLaurent Vivier if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
8758852ecd9SLaurent Vivier ret = -EBUSY;
8768852ecd9SLaurent Vivier goto out;
8778852ecd9SLaurent Vivier }
8788852ecd9SLaurent Vivier
879b64576cbSFinn Thain swim_base = (struct swim __iomem *)res->start;
8808852ecd9SLaurent Vivier if (!swim_base) {
881957d6bf6SJoe Perches ret = -ENOMEM;
8828852ecd9SLaurent Vivier goto out_release_io;
8838852ecd9SLaurent Vivier }
8848852ecd9SLaurent Vivier
8858852ecd9SLaurent Vivier /* probe device */
8868852ecd9SLaurent Vivier
8878852ecd9SLaurent Vivier set_swim_mode(swim_base, 1);
8888852ecd9SLaurent Vivier if (!get_swim_mode(swim_base)) {
8898852ecd9SLaurent Vivier printk(KERN_INFO "SWIM device not found !\n");
8908852ecd9SLaurent Vivier ret = -ENODEV;
891b64576cbSFinn Thain goto out_release_io;
8928852ecd9SLaurent Vivier }
8938852ecd9SLaurent Vivier
8948852ecd9SLaurent Vivier /* set platform driver data */
8958852ecd9SLaurent Vivier
8968852ecd9SLaurent Vivier swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL);
8978852ecd9SLaurent Vivier if (!swd) {
8988852ecd9SLaurent Vivier ret = -ENOMEM;
899b64576cbSFinn Thain goto out_release_io;
9008852ecd9SLaurent Vivier }
9018852ecd9SLaurent Vivier platform_set_drvdata(dev, swd);
9028852ecd9SLaurent Vivier
9038852ecd9SLaurent Vivier swd->base = swim_base;
9048852ecd9SLaurent Vivier
9058852ecd9SLaurent Vivier ret = swim_floppy_init(swd);
9068852ecd9SLaurent Vivier if (ret)
9078852ecd9SLaurent Vivier goto out_kfree;
9088852ecd9SLaurent Vivier
9098852ecd9SLaurent Vivier return 0;
9108852ecd9SLaurent Vivier
9118852ecd9SLaurent Vivier out_kfree:
9128852ecd9SLaurent Vivier kfree(swd);
9138852ecd9SLaurent Vivier out_release_io:
9148852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res));
9158852ecd9SLaurent Vivier out:
9168852ecd9SLaurent Vivier return ret;
9178852ecd9SLaurent Vivier }
9188852ecd9SLaurent Vivier
swim_remove(struct platform_device * dev)9198d85fce7SGreg Kroah-Hartman static int swim_remove(struct platform_device *dev)
9208852ecd9SLaurent Vivier {
9218852ecd9SLaurent Vivier struct swim_priv *swd = platform_get_drvdata(dev);
9228852ecd9SLaurent Vivier int drive;
9238852ecd9SLaurent Vivier struct resource *res;
9248852ecd9SLaurent Vivier
9259ef41effSLuis Chamberlain for (drive = 0; drive < swd->floppy_count; drive++)
9264e9abe72SLuis Chamberlain swim_cleanup_floppy_disk(&swd->unit[drive]);
9278852ecd9SLaurent Vivier
9288852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd");
9298852ecd9SLaurent Vivier
9308852ecd9SLaurent Vivier /* eject floppies */
9318852ecd9SLaurent Vivier
9328852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++)
9338852ecd9SLaurent Vivier floppy_eject(&swd->unit[drive]);
9348852ecd9SLaurent Vivier
9352724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0);
9368852ecd9SLaurent Vivier if (res)
9378852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res));
9388852ecd9SLaurent Vivier
9398852ecd9SLaurent Vivier kfree(swd);
9408852ecd9SLaurent Vivier
9418852ecd9SLaurent Vivier return 0;
9428852ecd9SLaurent Vivier }
9438852ecd9SLaurent Vivier
9448852ecd9SLaurent Vivier static struct platform_driver swim_driver = {
9458852ecd9SLaurent Vivier .probe = swim_probe,
9468d85fce7SGreg Kroah-Hartman .remove = swim_remove,
9478852ecd9SLaurent Vivier .driver = {
9488852ecd9SLaurent Vivier .name = CARDNAME,
9498852ecd9SLaurent Vivier },
9508852ecd9SLaurent Vivier };
9518852ecd9SLaurent Vivier
swim_init(void)9528852ecd9SLaurent Vivier static int __init swim_init(void)
9538852ecd9SLaurent Vivier {
9548852ecd9SLaurent Vivier printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
9558852ecd9SLaurent Vivier
9568852ecd9SLaurent Vivier return platform_driver_register(&swim_driver);
9578852ecd9SLaurent Vivier }
9588852ecd9SLaurent Vivier module_init(swim_init);
9598852ecd9SLaurent Vivier
swim_exit(void)9608852ecd9SLaurent Vivier static void __exit swim_exit(void)
9618852ecd9SLaurent Vivier {
9628852ecd9SLaurent Vivier platform_driver_unregister(&swim_driver);
9638852ecd9SLaurent Vivier }
9648852ecd9SLaurent Vivier module_exit(swim_exit);
9658852ecd9SLaurent Vivier
9668852ecd9SLaurent Vivier MODULE_DESCRIPTION("Driver for SWIM floppy controller");
9678852ecd9SLaurent Vivier MODULE_LICENSE("GPL");
9688852ecd9SLaurent Vivier MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>");
9698852ecd9SLaurent Vivier MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);
970