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); 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 611*d32e2bf8SChristoph Hellwig static int floppy_open(struct gendisk *disk, fmode_t mode) 6128852ecd9SLaurent Vivier { 613*d32e2bf8SChristoph Hellwig struct floppy_state *fs = disk->private_data; 6148852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 6158852ecd9SLaurent Vivier int err; 6168852ecd9SLaurent Vivier 6178852ecd9SLaurent Vivier if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL)) 6188852ecd9SLaurent Vivier return -EBUSY; 6198852ecd9SLaurent Vivier 6208852ecd9SLaurent Vivier if (mode & FMODE_EXCL) 6218852ecd9SLaurent Vivier fs->ref_count = -1; 6228852ecd9SLaurent Vivier else 6238852ecd9SLaurent Vivier fs->ref_count++; 6248852ecd9SLaurent Vivier 6258852ecd9SLaurent Vivier swim_write(base, setup, S_IBM_DRIVE | S_FCLK_DIV2); 6268852ecd9SLaurent Vivier udelay(10); 627b3906535SFinn Thain swim_drive(base, fs->location); 6288852ecd9SLaurent Vivier swim_motor(base, ON); 6298852ecd9SLaurent Vivier swim_action(base, SETMFM); 6308852ecd9SLaurent Vivier if (fs->ejected) 6318852ecd9SLaurent Vivier setup_medium(fs); 6328852ecd9SLaurent Vivier if (!fs->disk_in) { 6338852ecd9SLaurent Vivier err = -ENXIO; 6348852ecd9SLaurent Vivier goto out; 6358852ecd9SLaurent Vivier } 6368852ecd9SLaurent Vivier 6375a13388dSFinn Thain set_capacity(fs->disk, fs->total_secs); 6385a13388dSFinn Thain 6398852ecd9SLaurent Vivier if (mode & FMODE_NDELAY) 6408852ecd9SLaurent Vivier return 0; 6418852ecd9SLaurent Vivier 6428852ecd9SLaurent Vivier if (mode & (FMODE_READ|FMODE_WRITE)) { 643*d32e2bf8SChristoph Hellwig if (disk_check_media_change(disk) && fs->disk_in) 644433d21ceSChristoph Hellwig fs->ejected = 0; 6458852ecd9SLaurent Vivier if ((mode & FMODE_WRITE) && fs->write_protected) { 6468852ecd9SLaurent Vivier err = -EROFS; 6478852ecd9SLaurent Vivier goto out; 6488852ecd9SLaurent Vivier } 6498852ecd9SLaurent Vivier } 6508852ecd9SLaurent Vivier return 0; 6518852ecd9SLaurent Vivier out: 6528852ecd9SLaurent Vivier if (fs->ref_count < 0) 6538852ecd9SLaurent Vivier fs->ref_count = 0; 6548852ecd9SLaurent Vivier else if (fs->ref_count > 0) 6558852ecd9SLaurent Vivier --fs->ref_count; 6568852ecd9SLaurent Vivier 6578852ecd9SLaurent Vivier if (fs->ref_count == 0) 6588852ecd9SLaurent Vivier swim_motor(base, OFF); 6598852ecd9SLaurent Vivier return err; 6608852ecd9SLaurent Vivier } 6618852ecd9SLaurent Vivier 662*d32e2bf8SChristoph Hellwig static int floppy_unlocked_open(struct gendisk *disk, fmode_t mode) 6636e9624b8SArnd Bergmann { 6646e9624b8SArnd Bergmann int ret; 6656e9624b8SArnd Bergmann 6662a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 667*d32e2bf8SChristoph Hellwig ret = floppy_open(disk, mode); 6682a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 6696e9624b8SArnd Bergmann 6706e9624b8SArnd Bergmann return ret; 6716e9624b8SArnd Bergmann } 6726e9624b8SArnd Bergmann 673db2a144bSAl Viro static void floppy_release(struct gendisk *disk, fmode_t mode) 6748852ecd9SLaurent Vivier { 6758852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 6768852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 6778852ecd9SLaurent Vivier 6782a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 6798852ecd9SLaurent Vivier if (fs->ref_count < 0) 6808852ecd9SLaurent Vivier fs->ref_count = 0; 6818852ecd9SLaurent Vivier else if (fs->ref_count > 0) 6828852ecd9SLaurent Vivier --fs->ref_count; 6838852ecd9SLaurent Vivier 6848852ecd9SLaurent Vivier if (fs->ref_count == 0) 6858852ecd9SLaurent Vivier swim_motor(base, OFF); 6862a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 6878852ecd9SLaurent Vivier } 6888852ecd9SLaurent Vivier 6898852ecd9SLaurent Vivier static int floppy_ioctl(struct block_device *bdev, fmode_t mode, 6908852ecd9SLaurent Vivier unsigned int cmd, unsigned long param) 6918852ecd9SLaurent Vivier { 6928852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 6938852ecd9SLaurent Vivier int err; 6948852ecd9SLaurent Vivier 6958852ecd9SLaurent Vivier if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN)) 6968852ecd9SLaurent Vivier return -EPERM; 6978852ecd9SLaurent Vivier 6988852ecd9SLaurent Vivier switch (cmd) { 6998852ecd9SLaurent Vivier case FDEJECT: 7008852ecd9SLaurent Vivier if (fs->ref_count != 1) 7018852ecd9SLaurent Vivier return -EBUSY; 7022a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 7038852ecd9SLaurent Vivier err = floppy_eject(fs); 7042a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 7058852ecd9SLaurent Vivier return err; 7068852ecd9SLaurent Vivier 7078852ecd9SLaurent Vivier case FDGETPRM: 7088852ecd9SLaurent Vivier if (copy_to_user((void __user *) param, (void *) &floppy_type, 7098852ecd9SLaurent Vivier sizeof(struct floppy_struct))) 7108852ecd9SLaurent Vivier return -EFAULT; 7118852ecd9SLaurent Vivier return 0; 7128852ecd9SLaurent Vivier } 7138e2ab5a4SFinn Thain return -ENOTTY; 7148e2ab5a4SFinn Thain } 7158852ecd9SLaurent Vivier 7168852ecd9SLaurent Vivier static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo) 7178852ecd9SLaurent Vivier { 7188852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 7198852ecd9SLaurent Vivier struct floppy_struct *g; 7208852ecd9SLaurent Vivier int ret; 7218852ecd9SLaurent Vivier 7228852ecd9SLaurent Vivier ret = get_floppy_geometry(fs, 0, &g); 7238852ecd9SLaurent Vivier if (ret) 7248852ecd9SLaurent Vivier return ret; 7258852ecd9SLaurent Vivier 7268852ecd9SLaurent Vivier geo->heads = g->head; 7278852ecd9SLaurent Vivier geo->sectors = g->sect; 7288852ecd9SLaurent Vivier geo->cylinders = g->track; 7298852ecd9SLaurent Vivier 7308852ecd9SLaurent Vivier return 0; 7318852ecd9SLaurent Vivier } 7328852ecd9SLaurent Vivier 7334bbde777STejun Heo static unsigned int floppy_check_events(struct gendisk *disk, 7344bbde777STejun Heo unsigned int clearing) 7358852ecd9SLaurent Vivier { 7368852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 7378852ecd9SLaurent Vivier 7384bbde777STejun Heo return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0; 7398852ecd9SLaurent Vivier } 7408852ecd9SLaurent Vivier 74183d5cde4SAlexey Dobriyan static const struct block_device_operations floppy_fops = { 7428852ecd9SLaurent Vivier .owner = THIS_MODULE, 7436e9624b8SArnd Bergmann .open = floppy_unlocked_open, 7448852ecd9SLaurent Vivier .release = floppy_release, 7458a6cfeb6SArnd Bergmann .ioctl = floppy_ioctl, 7468852ecd9SLaurent Vivier .getgeo = floppy_getgeo, 7474bbde777STejun Heo .check_events = floppy_check_events, 7488852ecd9SLaurent Vivier }; 7498852ecd9SLaurent Vivier 7508d85fce7SGreg Kroah-Hartman static int swim_add_floppy(struct swim_priv *swd, enum drive_location location) 7518852ecd9SLaurent Vivier { 7528852ecd9SLaurent Vivier struct floppy_state *fs = &swd->unit[swd->floppy_count]; 7538852ecd9SLaurent Vivier struct swim __iomem *base = swd->base; 7548852ecd9SLaurent Vivier 7558852ecd9SLaurent Vivier fs->location = location; 7568852ecd9SLaurent Vivier 7578852ecd9SLaurent Vivier swim_drive(base, location); 7588852ecd9SLaurent Vivier 7598852ecd9SLaurent Vivier swim_motor(base, OFF); 7608852ecd9SLaurent Vivier 7615a13388dSFinn Thain fs->type = HD_MEDIA; 7628852ecd9SLaurent Vivier fs->head_number = 2; 7635a13388dSFinn Thain 7648852ecd9SLaurent Vivier fs->ref_count = 0; 7658852ecd9SLaurent Vivier fs->ejected = 1; 7668852ecd9SLaurent Vivier 7678852ecd9SLaurent Vivier swd->floppy_count++; 7688852ecd9SLaurent Vivier 7698852ecd9SLaurent Vivier return 0; 7708852ecd9SLaurent Vivier } 7718852ecd9SLaurent Vivier 772e3896d77SOmar Sandoval static const struct blk_mq_ops swim_mq_ops = { 773e3896d77SOmar Sandoval .queue_rq = swim_queue_rq, 774e3896d77SOmar Sandoval }; 775e3896d77SOmar Sandoval 7764e9abe72SLuis Chamberlain static void swim_cleanup_floppy_disk(struct floppy_state *fs) 7774e9abe72SLuis Chamberlain { 7784e9abe72SLuis Chamberlain struct gendisk *disk = fs->disk; 7794e9abe72SLuis Chamberlain 7804e9abe72SLuis Chamberlain if (!disk) 7814e9abe72SLuis Chamberlain return; 7824e9abe72SLuis Chamberlain 7839ef41effSLuis Chamberlain if (fs->registered) 7849ef41effSLuis Chamberlain del_gendisk(fs->disk); 7859ef41effSLuis Chamberlain 7868b9ab626SChristoph Hellwig put_disk(disk); 7874e9abe72SLuis Chamberlain blk_mq_free_tag_set(&fs->tag_set); 7884e9abe72SLuis Chamberlain } 7894e9abe72SLuis Chamberlain 7908d85fce7SGreg Kroah-Hartman static int swim_floppy_init(struct swim_priv *swd) 7918852ecd9SLaurent Vivier { 7928852ecd9SLaurent Vivier int err; 7938852ecd9SLaurent Vivier int drive; 7948852ecd9SLaurent Vivier struct swim __iomem *base = swd->base; 7958852ecd9SLaurent Vivier 7968852ecd9SLaurent Vivier /* scan floppy drives */ 7978852ecd9SLaurent Vivier 7988852ecd9SLaurent Vivier swim_drive(base, INTERNAL_DRIVE); 7998a500df6SFinn Thain if (swim_readbit(base, DRIVE_PRESENT) && 8008a500df6SFinn Thain !swim_readbit(base, ONEMEG_DRIVE)) 8018852ecd9SLaurent Vivier swim_add_floppy(swd, INTERNAL_DRIVE); 8028852ecd9SLaurent Vivier swim_drive(base, EXTERNAL_DRIVE); 8038a500df6SFinn Thain if (swim_readbit(base, DRIVE_PRESENT) && 8048a500df6SFinn Thain !swim_readbit(base, ONEMEG_DRIVE)) 8058852ecd9SLaurent Vivier swim_add_floppy(swd, EXTERNAL_DRIVE); 8068852ecd9SLaurent Vivier 8078852ecd9SLaurent Vivier /* register floppy drives */ 8088852ecd9SLaurent Vivier 8098852ecd9SLaurent Vivier err = register_blkdev(FLOPPY_MAJOR, "fd"); 8108852ecd9SLaurent Vivier if (err) { 8118852ecd9SLaurent Vivier printk(KERN_ERR "Unable to get major %d for SWIM floppy\n", 8128852ecd9SLaurent Vivier FLOPPY_MAJOR); 8138852ecd9SLaurent Vivier return -EBUSY; 8148852ecd9SLaurent Vivier } 8158852ecd9SLaurent Vivier 816103db8b2SOmar Sandoval spin_lock_init(&swd->lock); 817103db8b2SOmar Sandoval 8188852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 81951fbfedfSChristoph Hellwig err = blk_mq_alloc_sq_tag_set(&swd->unit[drive].tag_set, 82051fbfedfSChristoph Hellwig &swim_mq_ops, 2, BLK_MQ_F_SHOULD_MERGE); 82151fbfedfSChristoph Hellwig if (err) 82251fbfedfSChristoph Hellwig goto exit_put_disks; 823e3896d77SOmar Sandoval 82451fbfedfSChristoph Hellwig swd->unit[drive].disk = 82551fbfedfSChristoph Hellwig blk_mq_alloc_disk(&swd->unit[drive].tag_set, 82651fbfedfSChristoph Hellwig &swd->unit[drive]); 82751fbfedfSChristoph Hellwig if (IS_ERR(swd->unit[drive].disk)) { 82851fbfedfSChristoph Hellwig blk_mq_free_tag_set(&swd->unit[drive].tag_set); 82951fbfedfSChristoph Hellwig err = PTR_ERR(swd->unit[drive].disk); 8308852ecd9SLaurent Vivier goto exit_put_disks; 8318852ecd9SLaurent Vivier } 832e3896d77SOmar Sandoval 833103db8b2SOmar Sandoval swd->unit[drive].swd = swd; 834103db8b2SOmar Sandoval } 8358852ecd9SLaurent Vivier 8368852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 8378852ecd9SLaurent Vivier swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE; 8388852ecd9SLaurent Vivier swd->unit[drive].disk->major = FLOPPY_MAJOR; 8398852ecd9SLaurent Vivier swd->unit[drive].disk->first_minor = drive; 84051fbfedfSChristoph Hellwig swd->unit[drive].disk->minors = 1; 8418852ecd9SLaurent Vivier sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive); 8428852ecd9SLaurent Vivier swd->unit[drive].disk->fops = &floppy_fops; 8431ebe2e5fSChristoph Hellwig swd->unit[drive].disk->flags |= GENHD_FL_NO_PART; 844773008f6SMartin Wilck swd->unit[drive].disk->events = DISK_EVENT_MEDIA_CHANGE; 8458852ecd9SLaurent Vivier swd->unit[drive].disk->private_data = &swd->unit[drive]; 8468852ecd9SLaurent Vivier set_capacity(swd->unit[drive].disk, 2880); 847625a28a7SLuis Chamberlain err = add_disk(swd->unit[drive].disk); 848625a28a7SLuis Chamberlain if (err) 849625a28a7SLuis Chamberlain goto exit_put_disks; 8509ef41effSLuis Chamberlain swd->unit[drive].registered = true; 8518852ecd9SLaurent Vivier } 8528852ecd9SLaurent Vivier 8538852ecd9SLaurent Vivier return 0; 8548852ecd9SLaurent Vivier 8558852ecd9SLaurent Vivier exit_put_disks: 8568852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd"); 8571448a2a5SOmar Sandoval do { 8584e9abe72SLuis Chamberlain swim_cleanup_floppy_disk(&swd->unit[drive]); 8591448a2a5SOmar Sandoval } while (drive--); 8608852ecd9SLaurent Vivier return err; 8618852ecd9SLaurent Vivier } 8628852ecd9SLaurent Vivier 8638d85fce7SGreg Kroah-Hartman static int swim_probe(struct platform_device *dev) 8648852ecd9SLaurent Vivier { 8658852ecd9SLaurent Vivier struct resource *res; 8668852ecd9SLaurent Vivier struct swim __iomem *swim_base; 8678852ecd9SLaurent Vivier struct swim_priv *swd; 8688852ecd9SLaurent Vivier int ret; 8698852ecd9SLaurent Vivier 8702724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0); 8718852ecd9SLaurent Vivier if (!res) { 8728852ecd9SLaurent Vivier ret = -ENODEV; 8738852ecd9SLaurent Vivier goto out; 8748852ecd9SLaurent Vivier } 8758852ecd9SLaurent Vivier 8768852ecd9SLaurent Vivier if (!request_mem_region(res->start, resource_size(res), CARDNAME)) { 8778852ecd9SLaurent Vivier ret = -EBUSY; 8788852ecd9SLaurent Vivier goto out; 8798852ecd9SLaurent Vivier } 8808852ecd9SLaurent Vivier 881b64576cbSFinn Thain swim_base = (struct swim __iomem *)res->start; 8828852ecd9SLaurent Vivier if (!swim_base) { 883957d6bf6SJoe Perches ret = -ENOMEM; 8848852ecd9SLaurent Vivier goto out_release_io; 8858852ecd9SLaurent Vivier } 8868852ecd9SLaurent Vivier 8878852ecd9SLaurent Vivier /* probe device */ 8888852ecd9SLaurent Vivier 8898852ecd9SLaurent Vivier set_swim_mode(swim_base, 1); 8908852ecd9SLaurent Vivier if (!get_swim_mode(swim_base)) { 8918852ecd9SLaurent Vivier printk(KERN_INFO "SWIM device not found !\n"); 8928852ecd9SLaurent Vivier ret = -ENODEV; 893b64576cbSFinn Thain goto out_release_io; 8948852ecd9SLaurent Vivier } 8958852ecd9SLaurent Vivier 8968852ecd9SLaurent Vivier /* set platform driver data */ 8978852ecd9SLaurent Vivier 8988852ecd9SLaurent Vivier swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL); 8998852ecd9SLaurent Vivier if (!swd) { 9008852ecd9SLaurent Vivier ret = -ENOMEM; 901b64576cbSFinn Thain goto out_release_io; 9028852ecd9SLaurent Vivier } 9038852ecd9SLaurent Vivier platform_set_drvdata(dev, swd); 9048852ecd9SLaurent Vivier 9058852ecd9SLaurent Vivier swd->base = swim_base; 9068852ecd9SLaurent Vivier 9078852ecd9SLaurent Vivier ret = swim_floppy_init(swd); 9088852ecd9SLaurent Vivier if (ret) 9098852ecd9SLaurent Vivier goto out_kfree; 9108852ecd9SLaurent Vivier 9118852ecd9SLaurent Vivier return 0; 9128852ecd9SLaurent Vivier 9138852ecd9SLaurent Vivier out_kfree: 9148852ecd9SLaurent Vivier kfree(swd); 9158852ecd9SLaurent Vivier out_release_io: 9168852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res)); 9178852ecd9SLaurent Vivier out: 9188852ecd9SLaurent Vivier return ret; 9198852ecd9SLaurent Vivier } 9208852ecd9SLaurent Vivier 9218d85fce7SGreg Kroah-Hartman static int swim_remove(struct platform_device *dev) 9228852ecd9SLaurent Vivier { 9238852ecd9SLaurent Vivier struct swim_priv *swd = platform_get_drvdata(dev); 9248852ecd9SLaurent Vivier int drive; 9258852ecd9SLaurent Vivier struct resource *res; 9268852ecd9SLaurent Vivier 9279ef41effSLuis Chamberlain for (drive = 0; drive < swd->floppy_count; drive++) 9284e9abe72SLuis Chamberlain swim_cleanup_floppy_disk(&swd->unit[drive]); 9298852ecd9SLaurent Vivier 9308852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd"); 9318852ecd9SLaurent Vivier 9328852ecd9SLaurent Vivier /* eject floppies */ 9338852ecd9SLaurent Vivier 9348852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) 9358852ecd9SLaurent Vivier floppy_eject(&swd->unit[drive]); 9368852ecd9SLaurent Vivier 9372724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0); 9388852ecd9SLaurent Vivier if (res) 9398852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res)); 9408852ecd9SLaurent Vivier 9418852ecd9SLaurent Vivier kfree(swd); 9428852ecd9SLaurent Vivier 9438852ecd9SLaurent Vivier return 0; 9448852ecd9SLaurent Vivier } 9458852ecd9SLaurent Vivier 9468852ecd9SLaurent Vivier static struct platform_driver swim_driver = { 9478852ecd9SLaurent Vivier .probe = swim_probe, 9488d85fce7SGreg Kroah-Hartman .remove = swim_remove, 9498852ecd9SLaurent Vivier .driver = { 9508852ecd9SLaurent Vivier .name = CARDNAME, 9518852ecd9SLaurent Vivier }, 9528852ecd9SLaurent Vivier }; 9538852ecd9SLaurent Vivier 9548852ecd9SLaurent Vivier static int __init swim_init(void) 9558852ecd9SLaurent Vivier { 9568852ecd9SLaurent Vivier printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION); 9578852ecd9SLaurent Vivier 9588852ecd9SLaurent Vivier return platform_driver_register(&swim_driver); 9598852ecd9SLaurent Vivier } 9608852ecd9SLaurent Vivier module_init(swim_init); 9618852ecd9SLaurent Vivier 9628852ecd9SLaurent Vivier static void __exit swim_exit(void) 9638852ecd9SLaurent Vivier { 9648852ecd9SLaurent Vivier platform_driver_unregister(&swim_driver); 9658852ecd9SLaurent Vivier } 9668852ecd9SLaurent Vivier module_exit(swim_exit); 9678852ecd9SLaurent Vivier 9688852ecd9SLaurent Vivier MODULE_DESCRIPTION("Driver for SWIM floppy controller"); 9698852ecd9SLaurent Vivier MODULE_LICENSE("GPL"); 9708852ecd9SLaurent Vivier MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>"); 9718852ecd9SLaurent Vivier MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR); 972