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> 19*b81e0c23SChristoph 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; 1888852ecd9SLaurent Vivier 1898852ecd9SLaurent Vivier struct gendisk *disk; 190e3896d77SOmar Sandoval struct blk_mq_tag_set tag_set; 1918852ecd9SLaurent Vivier 1928852ecd9SLaurent Vivier /* parent controller */ 1938852ecd9SLaurent Vivier 1948852ecd9SLaurent Vivier struct swim_priv *swd; 1958852ecd9SLaurent Vivier }; 1968852ecd9SLaurent Vivier 1978852ecd9SLaurent Vivier enum motor_action { 1988852ecd9SLaurent Vivier OFF, 1998852ecd9SLaurent Vivier ON, 2008852ecd9SLaurent Vivier }; 2018852ecd9SLaurent Vivier 2028852ecd9SLaurent Vivier enum head { 2038852ecd9SLaurent Vivier LOWER_HEAD = 0, 2048852ecd9SLaurent Vivier UPPER_HEAD = 1, 2058852ecd9SLaurent Vivier }; 2068852ecd9SLaurent Vivier 2078852ecd9SLaurent Vivier #define FD_MAX_UNIT 2 2088852ecd9SLaurent Vivier 2098852ecd9SLaurent Vivier struct swim_priv { 2108852ecd9SLaurent Vivier struct swim __iomem *base; 2118852ecd9SLaurent Vivier spinlock_t lock; 2128852ecd9SLaurent Vivier int floppy_count; 2138852ecd9SLaurent Vivier struct floppy_state unit[FD_MAX_UNIT]; 2148852ecd9SLaurent Vivier }; 2158852ecd9SLaurent Vivier 2168852ecd9SLaurent Vivier extern int swim_read_sector_header(struct swim __iomem *base, 2178852ecd9SLaurent Vivier struct sector_header *header); 2188852ecd9SLaurent Vivier extern int swim_read_sector_data(struct swim __iomem *base, 2198852ecd9SLaurent Vivier unsigned char *data); 2208852ecd9SLaurent Vivier 2212a48fc0aSArnd Bergmann static DEFINE_MUTEX(swim_mutex); 2228852ecd9SLaurent Vivier static inline void set_swim_mode(struct swim __iomem *base, int enable) 2238852ecd9SLaurent Vivier { 2248852ecd9SLaurent Vivier struct iwm __iomem *iwm_base; 2258852ecd9SLaurent Vivier unsigned long flags; 2268852ecd9SLaurent Vivier 2278852ecd9SLaurent Vivier if (!enable) { 2288852ecd9SLaurent Vivier swim_write(base, mode0, 0xf8); 2298852ecd9SLaurent Vivier return; 2308852ecd9SLaurent Vivier } 2318852ecd9SLaurent Vivier 2328852ecd9SLaurent Vivier iwm_base = (struct iwm __iomem *)base; 2338852ecd9SLaurent Vivier local_irq_save(flags); 2348852ecd9SLaurent Vivier 2358852ecd9SLaurent Vivier iwm_read(iwm_base, q7L); 2368852ecd9SLaurent Vivier iwm_read(iwm_base, mtrOff); 2378852ecd9SLaurent Vivier iwm_read(iwm_base, q6H); 2388852ecd9SLaurent Vivier 2398852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57); 2408852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x17); 2418852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57); 2428852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57); 2438852ecd9SLaurent Vivier 2448852ecd9SLaurent Vivier local_irq_restore(flags); 2458852ecd9SLaurent Vivier } 2468852ecd9SLaurent Vivier 2478852ecd9SLaurent Vivier static inline int get_swim_mode(struct swim __iomem *base) 2488852ecd9SLaurent Vivier { 2498852ecd9SLaurent Vivier unsigned long flags; 2508852ecd9SLaurent Vivier 2518852ecd9SLaurent Vivier local_irq_save(flags); 2528852ecd9SLaurent Vivier 2538852ecd9SLaurent Vivier swim_write(base, phase, 0xf5); 2548852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf5) 2558852ecd9SLaurent Vivier goto is_iwm; 2568852ecd9SLaurent Vivier swim_write(base, phase, 0xf6); 2578852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf6) 2588852ecd9SLaurent Vivier goto is_iwm; 2598852ecd9SLaurent Vivier swim_write(base, phase, 0xf7); 2608852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf7) 2618852ecd9SLaurent Vivier goto is_iwm; 2628852ecd9SLaurent Vivier local_irq_restore(flags); 2638852ecd9SLaurent Vivier return 1; 2648852ecd9SLaurent Vivier is_iwm: 2658852ecd9SLaurent Vivier local_irq_restore(flags); 2668852ecd9SLaurent Vivier return 0; 2678852ecd9SLaurent Vivier } 2688852ecd9SLaurent Vivier 2698852ecd9SLaurent Vivier static inline void swim_select(struct swim __iomem *base, int sel) 2708852ecd9SLaurent Vivier { 2718852ecd9SLaurent Vivier swim_write(base, phase, RELAX); 2728852ecd9SLaurent Vivier 2738852ecd9SLaurent Vivier via1_set_head(sel & 0x100); 2748852ecd9SLaurent Vivier 2758852ecd9SLaurent Vivier swim_write(base, phase, sel & CA_MASK); 2768852ecd9SLaurent Vivier } 2778852ecd9SLaurent Vivier 2788852ecd9SLaurent Vivier static inline void swim_action(struct swim __iomem *base, int action) 2798852ecd9SLaurent Vivier { 2808852ecd9SLaurent Vivier unsigned long flags; 2818852ecd9SLaurent Vivier 2828852ecd9SLaurent Vivier local_irq_save(flags); 2838852ecd9SLaurent Vivier 2848852ecd9SLaurent Vivier swim_select(base, action); 2858852ecd9SLaurent Vivier udelay(1); 2868852ecd9SLaurent Vivier swim_write(base, phase, (LSTRB<<4) | LSTRB); 2878852ecd9SLaurent Vivier udelay(1); 2888852ecd9SLaurent Vivier swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F)); 2898852ecd9SLaurent Vivier udelay(1); 2908852ecd9SLaurent Vivier 2918852ecd9SLaurent Vivier local_irq_restore(flags); 2928852ecd9SLaurent Vivier } 2938852ecd9SLaurent Vivier 2948852ecd9SLaurent Vivier static inline int swim_readbit(struct swim __iomem *base, int bit) 2958852ecd9SLaurent Vivier { 2968852ecd9SLaurent Vivier int stat; 2978852ecd9SLaurent Vivier 2988852ecd9SLaurent Vivier swim_select(base, bit); 2998852ecd9SLaurent Vivier 3008852ecd9SLaurent Vivier udelay(10); 3018852ecd9SLaurent Vivier 3028852ecd9SLaurent Vivier stat = swim_read(base, handshake); 3038852ecd9SLaurent Vivier 3048852ecd9SLaurent Vivier return (stat & SENSE) == 0; 3058852ecd9SLaurent Vivier } 3068852ecd9SLaurent Vivier 3078852ecd9SLaurent Vivier static inline void swim_drive(struct swim __iomem *base, 3088852ecd9SLaurent Vivier enum drive_location location) 3098852ecd9SLaurent Vivier { 3108852ecd9SLaurent Vivier if (location == INTERNAL_DRIVE) { 3118852ecd9SLaurent Vivier swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */ 3128852ecd9SLaurent Vivier swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */ 3138852ecd9SLaurent Vivier } else if (location == EXTERNAL_DRIVE) { 3148852ecd9SLaurent Vivier swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */ 3158852ecd9SLaurent Vivier swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */ 3168852ecd9SLaurent Vivier } 3178852ecd9SLaurent Vivier } 3188852ecd9SLaurent Vivier 3198852ecd9SLaurent Vivier static inline void swim_motor(struct swim __iomem *base, 3208852ecd9SLaurent Vivier enum motor_action action) 3218852ecd9SLaurent Vivier { 3228852ecd9SLaurent Vivier if (action == ON) { 3238852ecd9SLaurent Vivier int i; 3248852ecd9SLaurent Vivier 3258852ecd9SLaurent Vivier swim_action(base, MOTOR_ON); 3268852ecd9SLaurent Vivier 3278852ecd9SLaurent Vivier for (i = 0; i < 2*HZ; i++) { 3288852ecd9SLaurent Vivier swim_select(base, RELAX); 3298852ecd9SLaurent Vivier if (swim_readbit(base, MOTOR_ON)) 3308852ecd9SLaurent Vivier break; 331c6516565SXu Wang set_current_state(TASK_INTERRUPTIBLE); 3328852ecd9SLaurent Vivier schedule_timeout(1); 3338852ecd9SLaurent Vivier } 3348852ecd9SLaurent Vivier } else if (action == OFF) { 3358852ecd9SLaurent Vivier swim_action(base, MOTOR_OFF); 3368852ecd9SLaurent Vivier swim_select(base, RELAX); 3378852ecd9SLaurent Vivier } 3388852ecd9SLaurent Vivier } 3398852ecd9SLaurent Vivier 3408852ecd9SLaurent Vivier static inline void swim_eject(struct swim __iomem *base) 3418852ecd9SLaurent Vivier { 3428852ecd9SLaurent Vivier int i; 3438852ecd9SLaurent Vivier 3448852ecd9SLaurent Vivier swim_action(base, EJECT); 3458852ecd9SLaurent Vivier 3468852ecd9SLaurent Vivier for (i = 0; i < 2*HZ; i++) { 3478852ecd9SLaurent Vivier swim_select(base, RELAX); 3488852ecd9SLaurent Vivier if (!swim_readbit(base, DISK_IN)) 3498852ecd9SLaurent Vivier break; 350c6516565SXu Wang set_current_state(TASK_INTERRUPTIBLE); 3518852ecd9SLaurent Vivier schedule_timeout(1); 3528852ecd9SLaurent Vivier } 3538852ecd9SLaurent Vivier swim_select(base, RELAX); 3548852ecd9SLaurent Vivier } 3558852ecd9SLaurent Vivier 3568852ecd9SLaurent Vivier static inline void swim_head(struct swim __iomem *base, enum head head) 3578852ecd9SLaurent Vivier { 3588852ecd9SLaurent Vivier /* wait drive is ready */ 3598852ecd9SLaurent Vivier 3608852ecd9SLaurent Vivier if (head == UPPER_HEAD) 3618852ecd9SLaurent Vivier swim_select(base, READ_DATA_1); 3628852ecd9SLaurent Vivier else if (head == LOWER_HEAD) 3638852ecd9SLaurent Vivier swim_select(base, READ_DATA_0); 3648852ecd9SLaurent Vivier } 3658852ecd9SLaurent Vivier 3668852ecd9SLaurent Vivier static inline int swim_step(struct swim __iomem *base) 3678852ecd9SLaurent Vivier { 3688852ecd9SLaurent Vivier int wait; 3698852ecd9SLaurent Vivier 3708852ecd9SLaurent Vivier swim_action(base, STEP); 3718852ecd9SLaurent Vivier 3728852ecd9SLaurent Vivier for (wait = 0; wait < HZ; wait++) { 3738852ecd9SLaurent Vivier 374c6516565SXu Wang set_current_state(TASK_INTERRUPTIBLE); 3758852ecd9SLaurent Vivier schedule_timeout(1); 3768852ecd9SLaurent Vivier 3778852ecd9SLaurent Vivier swim_select(base, RELAX); 3788852ecd9SLaurent Vivier if (!swim_readbit(base, STEP)) 3798852ecd9SLaurent Vivier return 0; 3808852ecd9SLaurent Vivier } 3818852ecd9SLaurent Vivier return -1; 3828852ecd9SLaurent Vivier } 3838852ecd9SLaurent Vivier 3848852ecd9SLaurent Vivier static inline int swim_track00(struct swim __iomem *base) 3858852ecd9SLaurent Vivier { 3868852ecd9SLaurent Vivier int try; 3878852ecd9SLaurent Vivier 3888852ecd9SLaurent Vivier swim_action(base, SEEK_NEGATIVE); 3898852ecd9SLaurent Vivier 3908852ecd9SLaurent Vivier for (try = 0; try < 100; try++) { 3918852ecd9SLaurent Vivier 3928852ecd9SLaurent Vivier swim_select(base, RELAX); 3938852ecd9SLaurent Vivier if (swim_readbit(base, TRACK_ZERO)) 3948852ecd9SLaurent Vivier break; 3958852ecd9SLaurent Vivier 3968852ecd9SLaurent Vivier if (swim_step(base)) 3978852ecd9SLaurent Vivier return -1; 3988852ecd9SLaurent Vivier } 3998852ecd9SLaurent Vivier 4008852ecd9SLaurent Vivier if (swim_readbit(base, TRACK_ZERO)) 4018852ecd9SLaurent Vivier return 0; 4028852ecd9SLaurent Vivier 4038852ecd9SLaurent Vivier return -1; 4048852ecd9SLaurent Vivier } 4058852ecd9SLaurent Vivier 4068852ecd9SLaurent Vivier static inline int swim_seek(struct swim __iomem *base, int step) 4078852ecd9SLaurent Vivier { 4088852ecd9SLaurent Vivier if (step == 0) 4098852ecd9SLaurent Vivier return 0; 4108852ecd9SLaurent Vivier 4118852ecd9SLaurent Vivier if (step < 0) { 4128852ecd9SLaurent Vivier swim_action(base, SEEK_NEGATIVE); 4138852ecd9SLaurent Vivier step = -step; 4148852ecd9SLaurent Vivier } else 4158852ecd9SLaurent Vivier swim_action(base, SEEK_POSITIVE); 4168852ecd9SLaurent Vivier 4178852ecd9SLaurent Vivier for ( ; step > 0; step--) { 4188852ecd9SLaurent Vivier if (swim_step(base)) 4198852ecd9SLaurent Vivier return -1; 4208852ecd9SLaurent Vivier } 4218852ecd9SLaurent Vivier 4228852ecd9SLaurent Vivier return 0; 4238852ecd9SLaurent Vivier } 4248852ecd9SLaurent Vivier 4258852ecd9SLaurent Vivier static inline int swim_track(struct floppy_state *fs, int track) 4268852ecd9SLaurent Vivier { 4278852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4288852ecd9SLaurent Vivier int ret; 4298852ecd9SLaurent Vivier 4308852ecd9SLaurent Vivier ret = swim_seek(base, track - fs->track); 4318852ecd9SLaurent Vivier 4328852ecd9SLaurent Vivier if (ret == 0) 4338852ecd9SLaurent Vivier fs->track = track; 4348852ecd9SLaurent Vivier else { 4358852ecd9SLaurent Vivier swim_track00(base); 4368852ecd9SLaurent Vivier fs->track = 0; 4378852ecd9SLaurent Vivier } 4388852ecd9SLaurent Vivier 4398852ecd9SLaurent Vivier return ret; 4408852ecd9SLaurent Vivier } 4418852ecd9SLaurent Vivier 4428852ecd9SLaurent Vivier static int floppy_eject(struct floppy_state *fs) 4438852ecd9SLaurent Vivier { 4448852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4458852ecd9SLaurent Vivier 4468852ecd9SLaurent Vivier swim_drive(base, fs->location); 4478852ecd9SLaurent Vivier swim_motor(base, OFF); 4488852ecd9SLaurent Vivier swim_eject(base); 4498852ecd9SLaurent Vivier 4508852ecd9SLaurent Vivier fs->disk_in = 0; 4518852ecd9SLaurent Vivier fs->ejected = 1; 4528852ecd9SLaurent Vivier 4538852ecd9SLaurent Vivier return 0; 4548852ecd9SLaurent Vivier } 4558852ecd9SLaurent Vivier 4568852ecd9SLaurent Vivier static inline int swim_read_sector(struct floppy_state *fs, 4578852ecd9SLaurent Vivier int side, int track, 4588852ecd9SLaurent Vivier int sector, unsigned char *buffer) 4598852ecd9SLaurent Vivier { 4608852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4618852ecd9SLaurent Vivier unsigned long flags; 4628852ecd9SLaurent Vivier struct sector_header header; 4638852ecd9SLaurent Vivier int ret = -1; 4648852ecd9SLaurent Vivier short i; 4658852ecd9SLaurent Vivier 4668852ecd9SLaurent Vivier swim_track(fs, track); 4678852ecd9SLaurent Vivier 4688852ecd9SLaurent Vivier swim_write(base, mode1, MOTON); 4698852ecd9SLaurent Vivier swim_head(base, side); 4708852ecd9SLaurent Vivier swim_write(base, mode0, side); 4718852ecd9SLaurent Vivier 4728852ecd9SLaurent Vivier local_irq_save(flags); 4738852ecd9SLaurent Vivier for (i = 0; i < 36; i++) { 4748852ecd9SLaurent Vivier ret = swim_read_sector_header(base, &header); 4758852ecd9SLaurent Vivier if (!ret && (header.sector == sector)) { 4768852ecd9SLaurent Vivier /* found */ 4778852ecd9SLaurent Vivier 4788852ecd9SLaurent Vivier ret = swim_read_sector_data(base, buffer); 4798852ecd9SLaurent Vivier break; 4808852ecd9SLaurent Vivier } 4818852ecd9SLaurent Vivier } 4828852ecd9SLaurent Vivier local_irq_restore(flags); 4838852ecd9SLaurent Vivier 4848852ecd9SLaurent Vivier swim_write(base, mode0, MOTON); 4858852ecd9SLaurent Vivier 4868852ecd9SLaurent Vivier if ((header.side != side) || (header.track != track) || 4878852ecd9SLaurent Vivier (header.sector != sector)) 4888852ecd9SLaurent Vivier return 0; 4898852ecd9SLaurent Vivier 4908852ecd9SLaurent Vivier return ret; 4918852ecd9SLaurent Vivier } 4928852ecd9SLaurent Vivier 4932a842acaSChristoph Hellwig static blk_status_t floppy_read_sectors(struct floppy_state *fs, 4948852ecd9SLaurent Vivier int req_sector, int sectors_nb, 4958852ecd9SLaurent Vivier unsigned char *buffer) 4968852ecd9SLaurent Vivier { 4978852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4988852ecd9SLaurent Vivier int ret; 4998852ecd9SLaurent Vivier int side, track, sector; 5008852ecd9SLaurent Vivier int i, try; 5018852ecd9SLaurent Vivier 5028852ecd9SLaurent Vivier 5038852ecd9SLaurent Vivier swim_drive(base, fs->location); 5048852ecd9SLaurent Vivier for (i = req_sector; i < req_sector + sectors_nb; i++) { 5058852ecd9SLaurent Vivier int x; 5068852ecd9SLaurent Vivier track = i / fs->secpercyl; 5078852ecd9SLaurent Vivier x = i % fs->secpercyl; 5088852ecd9SLaurent Vivier side = x / fs->secpertrack; 5098852ecd9SLaurent Vivier sector = x % fs->secpertrack + 1; 5108852ecd9SLaurent Vivier 5118852ecd9SLaurent Vivier try = 5; 5128852ecd9SLaurent Vivier do { 5138852ecd9SLaurent Vivier ret = swim_read_sector(fs, side, track, sector, 5148852ecd9SLaurent Vivier buffer); 5158852ecd9SLaurent Vivier if (try-- == 0) 5162a842acaSChristoph Hellwig return BLK_STS_IOERR; 5178852ecd9SLaurent Vivier } while (ret != 512); 5188852ecd9SLaurent Vivier 5198852ecd9SLaurent Vivier buffer += ret; 5208852ecd9SLaurent Vivier } 5218852ecd9SLaurent Vivier 5228852ecd9SLaurent Vivier return 0; 5238852ecd9SLaurent Vivier } 5248852ecd9SLaurent Vivier 525e3896d77SOmar Sandoval static blk_status_t swim_queue_rq(struct blk_mq_hw_ctx *hctx, 526e3896d77SOmar Sandoval const struct blk_mq_queue_data *bd) 5278852ecd9SLaurent Vivier { 528e3896d77SOmar Sandoval struct floppy_state *fs = hctx->queue->queuedata; 529e3896d77SOmar Sandoval struct swim_priv *swd = fs->swd; 530e3896d77SOmar Sandoval struct request *req = bd->rq; 531e3896d77SOmar Sandoval blk_status_t err; 532e3896d77SOmar Sandoval 533e3896d77SOmar Sandoval if (!spin_trylock_irq(&swd->lock)) 534e3896d77SOmar Sandoval return BLK_STS_DEV_RESOURCE; 535e3896d77SOmar Sandoval 536e3896d77SOmar Sandoval blk_mq_start_request(req); 537e3896d77SOmar Sandoval 538e3896d77SOmar Sandoval if (!fs->disk_in || rq_data_dir(req) == WRITE) { 539e3896d77SOmar Sandoval err = BLK_STS_IOERR; 540e3896d77SOmar Sandoval goto out; 541e3896d77SOmar Sandoval } 542103db8b2SOmar Sandoval 543103db8b2SOmar Sandoval do { 54406b0608eSTejun Heo err = floppy_read_sectors(fs, blk_rq_pos(req), 54583096ebfSTejun Heo blk_rq_cur_sectors(req), 546b4f42e28SJens Axboe bio_data(req->bio)); 547e3896d77SOmar Sandoval } while (blk_update_request(req, err, blk_rq_cur_bytes(req))); 548e3896d77SOmar Sandoval __blk_mq_end_request(req, err); 549e3896d77SOmar Sandoval 550e3896d77SOmar Sandoval err = BLK_STS_OK; 551e3896d77SOmar Sandoval out: 552e3896d77SOmar Sandoval spin_unlock_irq(&swd->lock); 553e3896d77SOmar Sandoval return err; 554e3896d77SOmar Sandoval 5558852ecd9SLaurent Vivier } 5568852ecd9SLaurent Vivier 5578852ecd9SLaurent Vivier static struct floppy_struct floppy_type[4] = { 5588852ecd9SLaurent Vivier { 0, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing */ 5598852ecd9SLaurent Vivier { 720, 9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/ 5608852ecd9SLaurent Vivier { 1440, 9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5" */ 5618852ecd9SLaurent Vivier { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5" */ 5628852ecd9SLaurent Vivier }; 5638852ecd9SLaurent Vivier 5648852ecd9SLaurent Vivier static int get_floppy_geometry(struct floppy_state *fs, int type, 5658852ecd9SLaurent Vivier struct floppy_struct **g) 5668852ecd9SLaurent Vivier { 5678852ecd9SLaurent Vivier if (type >= ARRAY_SIZE(floppy_type)) 5688852ecd9SLaurent Vivier return -EINVAL; 5698852ecd9SLaurent Vivier 5708852ecd9SLaurent Vivier if (type) 5718852ecd9SLaurent Vivier *g = &floppy_type[type]; 5728852ecd9SLaurent Vivier else if (fs->type == HD_MEDIA) /* High-Density media */ 5738852ecd9SLaurent Vivier *g = &floppy_type[3]; 5748852ecd9SLaurent Vivier else if (fs->head_number == 2) /* double-sided */ 5758852ecd9SLaurent Vivier *g = &floppy_type[2]; 5768852ecd9SLaurent Vivier else 5778852ecd9SLaurent Vivier *g = &floppy_type[1]; 5788852ecd9SLaurent Vivier 5798852ecd9SLaurent Vivier return 0; 5808852ecd9SLaurent Vivier } 5818852ecd9SLaurent Vivier 5828852ecd9SLaurent Vivier static void setup_medium(struct floppy_state *fs) 5838852ecd9SLaurent Vivier { 5848852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 5858852ecd9SLaurent Vivier 5868852ecd9SLaurent Vivier if (swim_readbit(base, DISK_IN)) { 5878852ecd9SLaurent Vivier struct floppy_struct *g; 5888852ecd9SLaurent Vivier fs->disk_in = 1; 5898852ecd9SLaurent Vivier fs->write_protected = swim_readbit(base, WRITE_PROT); 5908852ecd9SLaurent Vivier 5918852ecd9SLaurent Vivier if (swim_track00(base)) 5928852ecd9SLaurent Vivier printk(KERN_ERR 5938852ecd9SLaurent Vivier "SWIM: cannot move floppy head to track 0\n"); 5948852ecd9SLaurent Vivier 5958852ecd9SLaurent Vivier swim_track00(base); 5968852ecd9SLaurent Vivier 5975a13388dSFinn Thain fs->type = swim_readbit(base, TWOMEG_MEDIA) ? 5985a13388dSFinn Thain HD_MEDIA : DD_MEDIA; 5995a13388dSFinn Thain fs->head_number = swim_readbit(base, SINGLE_SIDED) ? 1 : 2; 6008852ecd9SLaurent Vivier get_floppy_geometry(fs, 0, &g); 6018852ecd9SLaurent Vivier fs->total_secs = g->size; 6028852ecd9SLaurent Vivier fs->secpercyl = g->head * g->sect; 6038852ecd9SLaurent Vivier fs->secpertrack = g->sect; 6048852ecd9SLaurent Vivier fs->track = 0; 6058852ecd9SLaurent Vivier } else { 6068852ecd9SLaurent Vivier fs->disk_in = 0; 6078852ecd9SLaurent Vivier } 6088852ecd9SLaurent Vivier } 6098852ecd9SLaurent Vivier 6108852ecd9SLaurent Vivier static int floppy_open(struct block_device *bdev, fmode_t mode) 6118852ecd9SLaurent Vivier { 6128852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 6138852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 6148852ecd9SLaurent Vivier int err; 6158852ecd9SLaurent Vivier 6168852ecd9SLaurent Vivier if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL)) 6178852ecd9SLaurent Vivier return -EBUSY; 6188852ecd9SLaurent Vivier 6198852ecd9SLaurent Vivier if (mode & FMODE_EXCL) 6208852ecd9SLaurent Vivier fs->ref_count = -1; 6218852ecd9SLaurent Vivier else 6228852ecd9SLaurent Vivier fs->ref_count++; 6238852ecd9SLaurent Vivier 6248852ecd9SLaurent Vivier swim_write(base, setup, S_IBM_DRIVE | S_FCLK_DIV2); 6258852ecd9SLaurent Vivier udelay(10); 626b3906535SFinn Thain swim_drive(base, fs->location); 6278852ecd9SLaurent Vivier swim_motor(base, ON); 6288852ecd9SLaurent Vivier swim_action(base, SETMFM); 6298852ecd9SLaurent Vivier if (fs->ejected) 6308852ecd9SLaurent Vivier setup_medium(fs); 6318852ecd9SLaurent Vivier if (!fs->disk_in) { 6328852ecd9SLaurent Vivier err = -ENXIO; 6338852ecd9SLaurent Vivier goto out; 6348852ecd9SLaurent Vivier } 6358852ecd9SLaurent Vivier 6365a13388dSFinn Thain set_capacity(fs->disk, fs->total_secs); 6375a13388dSFinn Thain 6388852ecd9SLaurent Vivier if (mode & FMODE_NDELAY) 6398852ecd9SLaurent Vivier return 0; 6408852ecd9SLaurent Vivier 6418852ecd9SLaurent Vivier if (mode & (FMODE_READ|FMODE_WRITE)) { 642433d21ceSChristoph Hellwig if (bdev_check_media_change(bdev) && fs->disk_in) 643433d21ceSChristoph Hellwig fs->ejected = 0; 6448852ecd9SLaurent Vivier if ((mode & FMODE_WRITE) && fs->write_protected) { 6458852ecd9SLaurent Vivier err = -EROFS; 6468852ecd9SLaurent Vivier goto out; 6478852ecd9SLaurent Vivier } 6488852ecd9SLaurent Vivier } 6498852ecd9SLaurent Vivier return 0; 6508852ecd9SLaurent Vivier out: 6518852ecd9SLaurent Vivier if (fs->ref_count < 0) 6528852ecd9SLaurent Vivier fs->ref_count = 0; 6538852ecd9SLaurent Vivier else if (fs->ref_count > 0) 6548852ecd9SLaurent Vivier --fs->ref_count; 6558852ecd9SLaurent Vivier 6568852ecd9SLaurent Vivier if (fs->ref_count == 0) 6578852ecd9SLaurent Vivier swim_motor(base, OFF); 6588852ecd9SLaurent Vivier return err; 6598852ecd9SLaurent Vivier } 6608852ecd9SLaurent Vivier 6616e9624b8SArnd Bergmann static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode) 6626e9624b8SArnd Bergmann { 6636e9624b8SArnd Bergmann int ret; 6646e9624b8SArnd Bergmann 6652a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 6666e9624b8SArnd Bergmann ret = floppy_open(bdev, mode); 6672a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 6686e9624b8SArnd Bergmann 6696e9624b8SArnd Bergmann return ret; 6706e9624b8SArnd Bergmann } 6716e9624b8SArnd Bergmann 672db2a144bSAl Viro static void floppy_release(struct gendisk *disk, fmode_t mode) 6738852ecd9SLaurent Vivier { 6748852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 6758852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 6768852ecd9SLaurent Vivier 6772a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 6788852ecd9SLaurent Vivier if (fs->ref_count < 0) 6798852ecd9SLaurent Vivier fs->ref_count = 0; 6808852ecd9SLaurent Vivier else if (fs->ref_count > 0) 6818852ecd9SLaurent Vivier --fs->ref_count; 6828852ecd9SLaurent Vivier 6838852ecd9SLaurent Vivier if (fs->ref_count == 0) 6848852ecd9SLaurent Vivier swim_motor(base, OFF); 6852a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 6868852ecd9SLaurent Vivier } 6878852ecd9SLaurent Vivier 6888852ecd9SLaurent Vivier static int floppy_ioctl(struct block_device *bdev, fmode_t mode, 6898852ecd9SLaurent Vivier unsigned int cmd, unsigned long param) 6908852ecd9SLaurent Vivier { 6918852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 6928852ecd9SLaurent Vivier int err; 6938852ecd9SLaurent Vivier 6948852ecd9SLaurent Vivier if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN)) 6958852ecd9SLaurent Vivier return -EPERM; 6968852ecd9SLaurent Vivier 6978852ecd9SLaurent Vivier switch (cmd) { 6988852ecd9SLaurent Vivier case FDEJECT: 6998852ecd9SLaurent Vivier if (fs->ref_count != 1) 7008852ecd9SLaurent Vivier return -EBUSY; 7012a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 7028852ecd9SLaurent Vivier err = floppy_eject(fs); 7032a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 7048852ecd9SLaurent Vivier return err; 7058852ecd9SLaurent Vivier 7068852ecd9SLaurent Vivier case FDGETPRM: 7078852ecd9SLaurent Vivier if (copy_to_user((void __user *) param, (void *) &floppy_type, 7088852ecd9SLaurent Vivier sizeof(struct floppy_struct))) 7098852ecd9SLaurent Vivier return -EFAULT; 7108852ecd9SLaurent Vivier return 0; 7118852ecd9SLaurent Vivier } 7128e2ab5a4SFinn Thain return -ENOTTY; 7138e2ab5a4SFinn Thain } 7148852ecd9SLaurent Vivier 7158852ecd9SLaurent Vivier static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo) 7168852ecd9SLaurent Vivier { 7178852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 7188852ecd9SLaurent Vivier struct floppy_struct *g; 7198852ecd9SLaurent Vivier int ret; 7208852ecd9SLaurent Vivier 7218852ecd9SLaurent Vivier ret = get_floppy_geometry(fs, 0, &g); 7228852ecd9SLaurent Vivier if (ret) 7238852ecd9SLaurent Vivier return ret; 7248852ecd9SLaurent Vivier 7258852ecd9SLaurent Vivier geo->heads = g->head; 7268852ecd9SLaurent Vivier geo->sectors = g->sect; 7278852ecd9SLaurent Vivier geo->cylinders = g->track; 7288852ecd9SLaurent Vivier 7298852ecd9SLaurent Vivier return 0; 7308852ecd9SLaurent Vivier } 7318852ecd9SLaurent Vivier 7324bbde777STejun Heo static unsigned int floppy_check_events(struct gendisk *disk, 7334bbde777STejun Heo unsigned int clearing) 7348852ecd9SLaurent Vivier { 7358852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 7368852ecd9SLaurent Vivier 7374bbde777STejun Heo return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0; 7388852ecd9SLaurent Vivier } 7398852ecd9SLaurent Vivier 74083d5cde4SAlexey Dobriyan static const struct block_device_operations floppy_fops = { 7418852ecd9SLaurent Vivier .owner = THIS_MODULE, 7426e9624b8SArnd Bergmann .open = floppy_unlocked_open, 7438852ecd9SLaurent Vivier .release = floppy_release, 7448a6cfeb6SArnd Bergmann .ioctl = floppy_ioctl, 7458852ecd9SLaurent Vivier .getgeo = floppy_getgeo, 7464bbde777STejun Heo .check_events = floppy_check_events, 7478852ecd9SLaurent Vivier }; 7488852ecd9SLaurent Vivier 7498d85fce7SGreg Kroah-Hartman static int swim_add_floppy(struct swim_priv *swd, enum drive_location location) 7508852ecd9SLaurent Vivier { 7518852ecd9SLaurent Vivier struct floppy_state *fs = &swd->unit[swd->floppy_count]; 7528852ecd9SLaurent Vivier struct swim __iomem *base = swd->base; 7538852ecd9SLaurent Vivier 7548852ecd9SLaurent Vivier fs->location = location; 7558852ecd9SLaurent Vivier 7568852ecd9SLaurent Vivier swim_drive(base, location); 7578852ecd9SLaurent Vivier 7588852ecd9SLaurent Vivier swim_motor(base, OFF); 7598852ecd9SLaurent Vivier 7605a13388dSFinn Thain fs->type = HD_MEDIA; 7618852ecd9SLaurent Vivier fs->head_number = 2; 7625a13388dSFinn Thain 7638852ecd9SLaurent Vivier fs->ref_count = 0; 7648852ecd9SLaurent Vivier fs->ejected = 1; 7658852ecd9SLaurent Vivier 7668852ecd9SLaurent Vivier swd->floppy_count++; 7678852ecd9SLaurent Vivier 7688852ecd9SLaurent Vivier return 0; 7698852ecd9SLaurent Vivier } 7708852ecd9SLaurent Vivier 771e3896d77SOmar Sandoval static const struct blk_mq_ops swim_mq_ops = { 772e3896d77SOmar Sandoval .queue_rq = swim_queue_rq, 773e3896d77SOmar Sandoval }; 774e3896d77SOmar Sandoval 7758d85fce7SGreg Kroah-Hartman static int swim_floppy_init(struct swim_priv *swd) 7768852ecd9SLaurent Vivier { 7778852ecd9SLaurent Vivier int err; 7788852ecd9SLaurent Vivier int drive; 7798852ecd9SLaurent Vivier struct swim __iomem *base = swd->base; 7808852ecd9SLaurent Vivier 7818852ecd9SLaurent Vivier /* scan floppy drives */ 7828852ecd9SLaurent Vivier 7838852ecd9SLaurent Vivier swim_drive(base, INTERNAL_DRIVE); 7848a500df6SFinn Thain if (swim_readbit(base, DRIVE_PRESENT) && 7858a500df6SFinn Thain !swim_readbit(base, ONEMEG_DRIVE)) 7868852ecd9SLaurent Vivier swim_add_floppy(swd, INTERNAL_DRIVE); 7878852ecd9SLaurent Vivier swim_drive(base, EXTERNAL_DRIVE); 7888a500df6SFinn Thain if (swim_readbit(base, DRIVE_PRESENT) && 7898a500df6SFinn Thain !swim_readbit(base, ONEMEG_DRIVE)) 7908852ecd9SLaurent Vivier swim_add_floppy(swd, EXTERNAL_DRIVE); 7918852ecd9SLaurent Vivier 7928852ecd9SLaurent Vivier /* register floppy drives */ 7938852ecd9SLaurent Vivier 7948852ecd9SLaurent Vivier err = register_blkdev(FLOPPY_MAJOR, "fd"); 7958852ecd9SLaurent Vivier if (err) { 7968852ecd9SLaurent Vivier printk(KERN_ERR "Unable to get major %d for SWIM floppy\n", 7978852ecd9SLaurent Vivier FLOPPY_MAJOR); 7988852ecd9SLaurent Vivier return -EBUSY; 7998852ecd9SLaurent Vivier } 8008852ecd9SLaurent Vivier 801103db8b2SOmar Sandoval spin_lock_init(&swd->lock); 802103db8b2SOmar Sandoval 8038852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 80451fbfedfSChristoph Hellwig err = blk_mq_alloc_sq_tag_set(&swd->unit[drive].tag_set, 80551fbfedfSChristoph Hellwig &swim_mq_ops, 2, BLK_MQ_F_SHOULD_MERGE); 80651fbfedfSChristoph Hellwig if (err) 80751fbfedfSChristoph Hellwig goto exit_put_disks; 808e3896d77SOmar Sandoval 80951fbfedfSChristoph Hellwig swd->unit[drive].disk = 81051fbfedfSChristoph Hellwig blk_mq_alloc_disk(&swd->unit[drive].tag_set, 81151fbfedfSChristoph Hellwig &swd->unit[drive]); 81251fbfedfSChristoph Hellwig if (IS_ERR(swd->unit[drive].disk)) { 81351fbfedfSChristoph Hellwig blk_mq_free_tag_set(&swd->unit[drive].tag_set); 81451fbfedfSChristoph Hellwig err = PTR_ERR(swd->unit[drive].disk); 8158852ecd9SLaurent Vivier goto exit_put_disks; 8168852ecd9SLaurent Vivier } 817e3896d77SOmar Sandoval 818103db8b2SOmar Sandoval swd->unit[drive].swd = swd; 819103db8b2SOmar Sandoval } 8208852ecd9SLaurent Vivier 8218852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 8228852ecd9SLaurent Vivier swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE; 8238852ecd9SLaurent Vivier swd->unit[drive].disk->major = FLOPPY_MAJOR; 8248852ecd9SLaurent Vivier swd->unit[drive].disk->first_minor = drive; 82551fbfedfSChristoph Hellwig swd->unit[drive].disk->minors = 1; 8268852ecd9SLaurent Vivier sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive); 8278852ecd9SLaurent Vivier swd->unit[drive].disk->fops = &floppy_fops; 828773008f6SMartin Wilck swd->unit[drive].disk->events = DISK_EVENT_MEDIA_CHANGE; 8298852ecd9SLaurent Vivier swd->unit[drive].disk->private_data = &swd->unit[drive]; 8308852ecd9SLaurent Vivier set_capacity(swd->unit[drive].disk, 2880); 8318852ecd9SLaurent Vivier add_disk(swd->unit[drive].disk); 8328852ecd9SLaurent Vivier } 8338852ecd9SLaurent Vivier 8348852ecd9SLaurent Vivier return 0; 8358852ecd9SLaurent Vivier 8368852ecd9SLaurent Vivier exit_put_disks: 8378852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd"); 8381448a2a5SOmar Sandoval do { 8391448a2a5SOmar Sandoval struct gendisk *disk = swd->unit[drive].disk; 8401448a2a5SOmar Sandoval 84151fbfedfSChristoph Hellwig if (!disk) 84251fbfedfSChristoph Hellwig continue; 84351fbfedfSChristoph Hellwig blk_cleanup_disk(disk); 844e3896d77SOmar Sandoval blk_mq_free_tag_set(&swd->unit[drive].tag_set); 8451448a2a5SOmar Sandoval } while (drive--); 8468852ecd9SLaurent Vivier return err; 8478852ecd9SLaurent Vivier } 8488852ecd9SLaurent Vivier 8498d85fce7SGreg Kroah-Hartman static int swim_probe(struct platform_device *dev) 8508852ecd9SLaurent Vivier { 8518852ecd9SLaurent Vivier struct resource *res; 8528852ecd9SLaurent Vivier struct swim __iomem *swim_base; 8538852ecd9SLaurent Vivier struct swim_priv *swd; 8548852ecd9SLaurent Vivier int ret; 8558852ecd9SLaurent Vivier 8562724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0); 8578852ecd9SLaurent Vivier if (!res) { 8588852ecd9SLaurent Vivier ret = -ENODEV; 8598852ecd9SLaurent Vivier goto out; 8608852ecd9SLaurent Vivier } 8618852ecd9SLaurent Vivier 8628852ecd9SLaurent Vivier if (!request_mem_region(res->start, resource_size(res), CARDNAME)) { 8638852ecd9SLaurent Vivier ret = -EBUSY; 8648852ecd9SLaurent Vivier goto out; 8658852ecd9SLaurent Vivier } 8668852ecd9SLaurent Vivier 867b64576cbSFinn Thain swim_base = (struct swim __iomem *)res->start; 8688852ecd9SLaurent Vivier if (!swim_base) { 869957d6bf6SJoe Perches ret = -ENOMEM; 8708852ecd9SLaurent Vivier goto out_release_io; 8718852ecd9SLaurent Vivier } 8728852ecd9SLaurent Vivier 8738852ecd9SLaurent Vivier /* probe device */ 8748852ecd9SLaurent Vivier 8758852ecd9SLaurent Vivier set_swim_mode(swim_base, 1); 8768852ecd9SLaurent Vivier if (!get_swim_mode(swim_base)) { 8778852ecd9SLaurent Vivier printk(KERN_INFO "SWIM device not found !\n"); 8788852ecd9SLaurent Vivier ret = -ENODEV; 879b64576cbSFinn Thain goto out_release_io; 8808852ecd9SLaurent Vivier } 8818852ecd9SLaurent Vivier 8828852ecd9SLaurent Vivier /* set platform driver data */ 8838852ecd9SLaurent Vivier 8848852ecd9SLaurent Vivier swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL); 8858852ecd9SLaurent Vivier if (!swd) { 8868852ecd9SLaurent Vivier ret = -ENOMEM; 887b64576cbSFinn Thain goto out_release_io; 8888852ecd9SLaurent Vivier } 8898852ecd9SLaurent Vivier platform_set_drvdata(dev, swd); 8908852ecd9SLaurent Vivier 8918852ecd9SLaurent Vivier swd->base = swim_base; 8928852ecd9SLaurent Vivier 8938852ecd9SLaurent Vivier ret = swim_floppy_init(swd); 8948852ecd9SLaurent Vivier if (ret) 8958852ecd9SLaurent Vivier goto out_kfree; 8968852ecd9SLaurent Vivier 8978852ecd9SLaurent Vivier return 0; 8988852ecd9SLaurent Vivier 8998852ecd9SLaurent Vivier out_kfree: 9008852ecd9SLaurent Vivier kfree(swd); 9018852ecd9SLaurent Vivier out_release_io: 9028852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res)); 9038852ecd9SLaurent Vivier out: 9048852ecd9SLaurent Vivier return ret; 9058852ecd9SLaurent Vivier } 9068852ecd9SLaurent Vivier 9078d85fce7SGreg Kroah-Hartman static int swim_remove(struct platform_device *dev) 9088852ecd9SLaurent Vivier { 9098852ecd9SLaurent Vivier struct swim_priv *swd = platform_get_drvdata(dev); 9108852ecd9SLaurent Vivier int drive; 9118852ecd9SLaurent Vivier struct resource *res; 9128852ecd9SLaurent Vivier 9138852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 9148852ecd9SLaurent Vivier del_gendisk(swd->unit[drive].disk); 915103db8b2SOmar Sandoval blk_cleanup_queue(swd->unit[drive].disk->queue); 916e3896d77SOmar Sandoval blk_mq_free_tag_set(&swd->unit[drive].tag_set); 9178852ecd9SLaurent Vivier put_disk(swd->unit[drive].disk); 9188852ecd9SLaurent Vivier } 9198852ecd9SLaurent Vivier 9208852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd"); 9218852ecd9SLaurent Vivier 9228852ecd9SLaurent Vivier /* eject floppies */ 9238852ecd9SLaurent Vivier 9248852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) 9258852ecd9SLaurent Vivier floppy_eject(&swd->unit[drive]); 9268852ecd9SLaurent Vivier 9272724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0); 9288852ecd9SLaurent Vivier if (res) 9298852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res)); 9308852ecd9SLaurent Vivier 9318852ecd9SLaurent Vivier kfree(swd); 9328852ecd9SLaurent Vivier 9338852ecd9SLaurent Vivier return 0; 9348852ecd9SLaurent Vivier } 9358852ecd9SLaurent Vivier 9368852ecd9SLaurent Vivier static struct platform_driver swim_driver = { 9378852ecd9SLaurent Vivier .probe = swim_probe, 9388d85fce7SGreg Kroah-Hartman .remove = swim_remove, 9398852ecd9SLaurent Vivier .driver = { 9408852ecd9SLaurent Vivier .name = CARDNAME, 9418852ecd9SLaurent Vivier }, 9428852ecd9SLaurent Vivier }; 9438852ecd9SLaurent Vivier 9448852ecd9SLaurent Vivier static int __init swim_init(void) 9458852ecd9SLaurent Vivier { 9468852ecd9SLaurent Vivier printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION); 9478852ecd9SLaurent Vivier 9488852ecd9SLaurent Vivier return platform_driver_register(&swim_driver); 9498852ecd9SLaurent Vivier } 9508852ecd9SLaurent Vivier module_init(swim_init); 9518852ecd9SLaurent Vivier 9528852ecd9SLaurent Vivier static void __exit swim_exit(void) 9538852ecd9SLaurent Vivier { 9548852ecd9SLaurent Vivier platform_driver_unregister(&swim_driver); 9558852ecd9SLaurent Vivier } 9568852ecd9SLaurent Vivier module_exit(swim_exit); 9578852ecd9SLaurent Vivier 9588852ecd9SLaurent Vivier MODULE_DESCRIPTION("Driver for SWIM floppy controller"); 9598852ecd9SLaurent Vivier MODULE_LICENSE("GPL"); 9608852ecd9SLaurent Vivier MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>"); 9618852ecd9SLaurent Vivier MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR); 962