18852ecd9SLaurent Vivier /* 28852ecd9SLaurent Vivier * Driver for SWIM (Sander Woz Integrated Machine) floppy controller 38852ecd9SLaurent Vivier * 48852ecd9SLaurent Vivier * Copyright (C) 2004,2008 Laurent Vivier <Laurent@lvivier.info> 58852ecd9SLaurent Vivier * 68852ecd9SLaurent Vivier * based on Alastair Bridgewater SWIM analysis, 2001 78852ecd9SLaurent Vivier * based on SWIM3 driver (c) Paul Mackerras, 1996 88852ecd9SLaurent Vivier * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath. 98852ecd9SLaurent Vivier * 108852ecd9SLaurent Vivier * This program is free software; you can redistribute it and/or 118852ecd9SLaurent Vivier * modify it under the terms of the GNU General Public License 128852ecd9SLaurent Vivier * as published by the Free Software Foundation; either version 138852ecd9SLaurent Vivier * 2 of the License, or (at your option) any later version. 148852ecd9SLaurent Vivier * 158852ecd9SLaurent Vivier * 2004-08-21 (lv) - Initial implementation 168852ecd9SLaurent Vivier * 2008-10-30 (lv) - Port to 2.6 178852ecd9SLaurent Vivier */ 188852ecd9SLaurent Vivier 198852ecd9SLaurent Vivier #include <linux/module.h> 208852ecd9SLaurent Vivier #include <linux/fd.h> 215a0e3ad6STejun Heo #include <linux/slab.h> 228852ecd9SLaurent Vivier #include <linux/blkdev.h> 232a48fc0aSArnd Bergmann #include <linux/mutex.h> 248852ecd9SLaurent Vivier #include <linux/hdreg.h> 258852ecd9SLaurent Vivier #include <linux/kernel.h> 268852ecd9SLaurent Vivier #include <linux/delay.h> 278852ecd9SLaurent Vivier #include <linux/platform_device.h> 288852ecd9SLaurent Vivier 298852ecd9SLaurent Vivier #include <asm/mac_via.h> 308852ecd9SLaurent Vivier 318852ecd9SLaurent Vivier #define CARDNAME "swim" 328852ecd9SLaurent Vivier 338852ecd9SLaurent Vivier struct sector_header { 348852ecd9SLaurent Vivier unsigned char side; 358852ecd9SLaurent Vivier unsigned char track; 368852ecd9SLaurent Vivier unsigned char sector; 378852ecd9SLaurent Vivier unsigned char size; 388852ecd9SLaurent Vivier unsigned char crc0; 398852ecd9SLaurent Vivier unsigned char crc1; 408852ecd9SLaurent Vivier } __attribute__((packed)); 418852ecd9SLaurent Vivier 428852ecd9SLaurent Vivier #define DRIVER_VERSION "Version 0.2 (2008-10-30)" 438852ecd9SLaurent Vivier 448852ecd9SLaurent Vivier #define REG(x) unsigned char x, x ## _pad[0x200 - 1]; 458852ecd9SLaurent Vivier 468852ecd9SLaurent Vivier struct swim { 478852ecd9SLaurent Vivier REG(write_data) 488852ecd9SLaurent Vivier REG(write_mark) 498852ecd9SLaurent Vivier REG(write_CRC) 508852ecd9SLaurent Vivier REG(write_parameter) 518852ecd9SLaurent Vivier REG(write_phase) 528852ecd9SLaurent Vivier REG(write_setup) 538852ecd9SLaurent Vivier REG(write_mode0) 548852ecd9SLaurent Vivier REG(write_mode1) 558852ecd9SLaurent Vivier 568852ecd9SLaurent Vivier REG(read_data) 578852ecd9SLaurent Vivier REG(read_mark) 588852ecd9SLaurent Vivier REG(read_error) 598852ecd9SLaurent Vivier REG(read_parameter) 608852ecd9SLaurent Vivier REG(read_phase) 618852ecd9SLaurent Vivier REG(read_setup) 628852ecd9SLaurent Vivier REG(read_status) 638852ecd9SLaurent Vivier REG(read_handshake) 648852ecd9SLaurent Vivier } __attribute__((packed)); 658852ecd9SLaurent Vivier 668852ecd9SLaurent Vivier #define swim_write(base, reg, v) out_8(&(base)->write_##reg, (v)) 678852ecd9SLaurent Vivier #define swim_read(base, reg) in_8(&(base)->read_##reg) 688852ecd9SLaurent Vivier 698852ecd9SLaurent Vivier /* IWM registers */ 708852ecd9SLaurent Vivier 718852ecd9SLaurent Vivier struct iwm { 728852ecd9SLaurent Vivier REG(ph0L) 738852ecd9SLaurent Vivier REG(ph0H) 748852ecd9SLaurent Vivier REG(ph1L) 758852ecd9SLaurent Vivier REG(ph1H) 768852ecd9SLaurent Vivier REG(ph2L) 778852ecd9SLaurent Vivier REG(ph2H) 788852ecd9SLaurent Vivier REG(ph3L) 798852ecd9SLaurent Vivier REG(ph3H) 808852ecd9SLaurent Vivier REG(mtrOff) 818852ecd9SLaurent Vivier REG(mtrOn) 828852ecd9SLaurent Vivier REG(intDrive) 838852ecd9SLaurent Vivier REG(extDrive) 848852ecd9SLaurent Vivier REG(q6L) 858852ecd9SLaurent Vivier REG(q6H) 868852ecd9SLaurent Vivier REG(q7L) 878852ecd9SLaurent Vivier REG(q7H) 888852ecd9SLaurent Vivier } __attribute__((packed)); 898852ecd9SLaurent Vivier 908852ecd9SLaurent Vivier #define iwm_write(base, reg, v) out_8(&(base)->reg, (v)) 918852ecd9SLaurent Vivier #define iwm_read(base, reg) in_8(&(base)->reg) 928852ecd9SLaurent Vivier 938852ecd9SLaurent Vivier /* bits in phase register */ 948852ecd9SLaurent Vivier 958852ecd9SLaurent Vivier #define SEEK_POSITIVE 0x070 968852ecd9SLaurent Vivier #define SEEK_NEGATIVE 0x074 978852ecd9SLaurent Vivier #define STEP 0x071 988852ecd9SLaurent Vivier #define MOTOR_ON 0x072 998852ecd9SLaurent Vivier #define MOTOR_OFF 0x076 1008852ecd9SLaurent Vivier #define INDEX 0x073 1018852ecd9SLaurent Vivier #define EJECT 0x077 1028852ecd9SLaurent Vivier #define SETMFM 0x171 1038852ecd9SLaurent Vivier #define SETGCR 0x175 1048852ecd9SLaurent Vivier 1058852ecd9SLaurent Vivier #define RELAX 0x033 1068852ecd9SLaurent Vivier #define LSTRB 0x008 1078852ecd9SLaurent Vivier 1088852ecd9SLaurent Vivier #define CA_MASK 0x077 1098852ecd9SLaurent Vivier 1108852ecd9SLaurent Vivier /* Select values for swim_select and swim_readbit */ 1118852ecd9SLaurent Vivier 1128852ecd9SLaurent Vivier #define READ_DATA_0 0x074 1138852ecd9SLaurent Vivier #define TWOMEG_DRIVE 0x075 1148852ecd9SLaurent Vivier #define SINGLE_SIDED 0x076 1158852ecd9SLaurent Vivier #define DRIVE_PRESENT 0x077 1168852ecd9SLaurent Vivier #define DISK_IN 0x170 1178852ecd9SLaurent Vivier #define WRITE_PROT 0x171 1188852ecd9SLaurent Vivier #define TRACK_ZERO 0x172 1198852ecd9SLaurent Vivier #define TACHO 0x173 1208852ecd9SLaurent Vivier #define READ_DATA_1 0x174 1218852ecd9SLaurent Vivier #define MFM_MODE 0x175 1228852ecd9SLaurent Vivier #define SEEK_COMPLETE 0x176 1238852ecd9SLaurent Vivier #define ONEMEG_MEDIA 0x177 1248852ecd9SLaurent Vivier 1258852ecd9SLaurent Vivier /* Bits in handshake register */ 1268852ecd9SLaurent Vivier 1278852ecd9SLaurent Vivier #define MARK_BYTE 0x01 1288852ecd9SLaurent Vivier #define CRC_ZERO 0x02 1298852ecd9SLaurent Vivier #define RDDATA 0x04 1308852ecd9SLaurent Vivier #define SENSE 0x08 1318852ecd9SLaurent Vivier #define MOTEN 0x10 1328852ecd9SLaurent Vivier #define ERROR 0x20 1338852ecd9SLaurent Vivier #define DAT2BYTE 0x40 1348852ecd9SLaurent Vivier #define DAT1BYTE 0x80 1358852ecd9SLaurent Vivier 1368852ecd9SLaurent Vivier /* bits in setup register */ 1378852ecd9SLaurent Vivier 1388852ecd9SLaurent Vivier #define S_INV_WDATA 0x01 1398852ecd9SLaurent Vivier #define S_3_5_SELECT 0x02 1408852ecd9SLaurent Vivier #define S_GCR 0x04 1418852ecd9SLaurent Vivier #define S_FCLK_DIV2 0x08 1428852ecd9SLaurent Vivier #define S_ERROR_CORR 0x10 1438852ecd9SLaurent Vivier #define S_IBM_DRIVE 0x20 1448852ecd9SLaurent Vivier #define S_GCR_WRITE 0x40 1458852ecd9SLaurent Vivier #define S_TIMEOUT 0x80 1468852ecd9SLaurent Vivier 1478852ecd9SLaurent Vivier /* bits in mode register */ 1488852ecd9SLaurent Vivier 1498852ecd9SLaurent Vivier #define CLFIFO 0x01 1508852ecd9SLaurent Vivier #define ENBL1 0x02 1518852ecd9SLaurent Vivier #define ENBL2 0x04 1528852ecd9SLaurent Vivier #define ACTION 0x08 1538852ecd9SLaurent Vivier #define WRITE_MODE 0x10 1548852ecd9SLaurent Vivier #define HEDSEL 0x20 1558852ecd9SLaurent Vivier #define MOTON 0x80 1568852ecd9SLaurent Vivier 1578852ecd9SLaurent Vivier /*----------------------------------------------------------------------------*/ 1588852ecd9SLaurent Vivier 1598852ecd9SLaurent Vivier enum drive_location { 1608852ecd9SLaurent Vivier INTERNAL_DRIVE = 0x02, 1618852ecd9SLaurent Vivier EXTERNAL_DRIVE = 0x04, 1628852ecd9SLaurent Vivier }; 1638852ecd9SLaurent Vivier 1648852ecd9SLaurent Vivier enum media_type { 1658852ecd9SLaurent Vivier DD_MEDIA, 1668852ecd9SLaurent Vivier HD_MEDIA, 1678852ecd9SLaurent Vivier }; 1688852ecd9SLaurent Vivier 1698852ecd9SLaurent Vivier struct floppy_state { 1708852ecd9SLaurent Vivier 1718852ecd9SLaurent Vivier /* physical properties */ 1728852ecd9SLaurent Vivier 1738852ecd9SLaurent Vivier enum drive_location location; /* internal or external drive */ 1748852ecd9SLaurent Vivier int head_number; /* single- or double-sided drive */ 1758852ecd9SLaurent Vivier 1768852ecd9SLaurent Vivier /* media */ 1778852ecd9SLaurent Vivier 1788852ecd9SLaurent Vivier int disk_in; 1798852ecd9SLaurent Vivier int ejected; 1808852ecd9SLaurent Vivier enum media_type type; 1818852ecd9SLaurent Vivier int write_protected; 1828852ecd9SLaurent Vivier 1838852ecd9SLaurent Vivier int total_secs; 1848852ecd9SLaurent Vivier int secpercyl; 1858852ecd9SLaurent Vivier int secpertrack; 1868852ecd9SLaurent Vivier 1878852ecd9SLaurent Vivier /* in-use information */ 1888852ecd9SLaurent Vivier 1898852ecd9SLaurent Vivier int track; 1908852ecd9SLaurent Vivier int ref_count; 1918852ecd9SLaurent Vivier 1928852ecd9SLaurent Vivier struct gendisk *disk; 1938852ecd9SLaurent Vivier 1948852ecd9SLaurent Vivier /* parent controller */ 1958852ecd9SLaurent Vivier 1968852ecd9SLaurent Vivier struct swim_priv *swd; 1978852ecd9SLaurent Vivier }; 1988852ecd9SLaurent Vivier 1998852ecd9SLaurent Vivier enum motor_action { 2008852ecd9SLaurent Vivier OFF, 2018852ecd9SLaurent Vivier ON, 2028852ecd9SLaurent Vivier }; 2038852ecd9SLaurent Vivier 2048852ecd9SLaurent Vivier enum head { 2058852ecd9SLaurent Vivier LOWER_HEAD = 0, 2068852ecd9SLaurent Vivier UPPER_HEAD = 1, 2078852ecd9SLaurent Vivier }; 2088852ecd9SLaurent Vivier 2098852ecd9SLaurent Vivier #define FD_MAX_UNIT 2 2108852ecd9SLaurent Vivier 2118852ecd9SLaurent Vivier struct swim_priv { 2128852ecd9SLaurent Vivier struct swim __iomem *base; 2138852ecd9SLaurent Vivier spinlock_t lock; 2148852ecd9SLaurent Vivier struct request_queue *queue; 2158852ecd9SLaurent Vivier int floppy_count; 2168852ecd9SLaurent Vivier struct floppy_state unit[FD_MAX_UNIT]; 2178852ecd9SLaurent Vivier }; 2188852ecd9SLaurent Vivier 2198852ecd9SLaurent Vivier extern int swim_read_sector_header(struct swim __iomem *base, 2208852ecd9SLaurent Vivier struct sector_header *header); 2218852ecd9SLaurent Vivier extern int swim_read_sector_data(struct swim __iomem *base, 2228852ecd9SLaurent Vivier unsigned char *data); 2238852ecd9SLaurent Vivier 2242a48fc0aSArnd Bergmann static DEFINE_MUTEX(swim_mutex); 2258852ecd9SLaurent Vivier static inline void set_swim_mode(struct swim __iomem *base, int enable) 2268852ecd9SLaurent Vivier { 2278852ecd9SLaurent Vivier struct iwm __iomem *iwm_base; 2288852ecd9SLaurent Vivier unsigned long flags; 2298852ecd9SLaurent Vivier 2308852ecd9SLaurent Vivier if (!enable) { 2318852ecd9SLaurent Vivier swim_write(base, mode0, 0xf8); 2328852ecd9SLaurent Vivier return; 2338852ecd9SLaurent Vivier } 2348852ecd9SLaurent Vivier 2358852ecd9SLaurent Vivier iwm_base = (struct iwm __iomem *)base; 2368852ecd9SLaurent Vivier local_irq_save(flags); 2378852ecd9SLaurent Vivier 2388852ecd9SLaurent Vivier iwm_read(iwm_base, q7L); 2398852ecd9SLaurent Vivier iwm_read(iwm_base, mtrOff); 2408852ecd9SLaurent Vivier iwm_read(iwm_base, q6H); 2418852ecd9SLaurent Vivier 2428852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57); 2438852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x17); 2448852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57); 2458852ecd9SLaurent Vivier iwm_write(iwm_base, q7H, 0x57); 2468852ecd9SLaurent Vivier 2478852ecd9SLaurent Vivier local_irq_restore(flags); 2488852ecd9SLaurent Vivier } 2498852ecd9SLaurent Vivier 2508852ecd9SLaurent Vivier static inline int get_swim_mode(struct swim __iomem *base) 2518852ecd9SLaurent Vivier { 2528852ecd9SLaurent Vivier unsigned long flags; 2538852ecd9SLaurent Vivier 2548852ecd9SLaurent Vivier local_irq_save(flags); 2558852ecd9SLaurent Vivier 2568852ecd9SLaurent Vivier swim_write(base, phase, 0xf5); 2578852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf5) 2588852ecd9SLaurent Vivier goto is_iwm; 2598852ecd9SLaurent Vivier swim_write(base, phase, 0xf6); 2608852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf6) 2618852ecd9SLaurent Vivier goto is_iwm; 2628852ecd9SLaurent Vivier swim_write(base, phase, 0xf7); 2638852ecd9SLaurent Vivier if (swim_read(base, phase) != 0xf7) 2648852ecd9SLaurent Vivier goto is_iwm; 2658852ecd9SLaurent Vivier local_irq_restore(flags); 2668852ecd9SLaurent Vivier return 1; 2678852ecd9SLaurent Vivier is_iwm: 2688852ecd9SLaurent Vivier local_irq_restore(flags); 2698852ecd9SLaurent Vivier return 0; 2708852ecd9SLaurent Vivier } 2718852ecd9SLaurent Vivier 2728852ecd9SLaurent Vivier static inline void swim_select(struct swim __iomem *base, int sel) 2738852ecd9SLaurent Vivier { 2748852ecd9SLaurent Vivier swim_write(base, phase, RELAX); 2758852ecd9SLaurent Vivier 2768852ecd9SLaurent Vivier via1_set_head(sel & 0x100); 2778852ecd9SLaurent Vivier 2788852ecd9SLaurent Vivier swim_write(base, phase, sel & CA_MASK); 2798852ecd9SLaurent Vivier } 2808852ecd9SLaurent Vivier 2818852ecd9SLaurent Vivier static inline void swim_action(struct swim __iomem *base, int action) 2828852ecd9SLaurent Vivier { 2838852ecd9SLaurent Vivier unsigned long flags; 2848852ecd9SLaurent Vivier 2858852ecd9SLaurent Vivier local_irq_save(flags); 2868852ecd9SLaurent Vivier 2878852ecd9SLaurent Vivier swim_select(base, action); 2888852ecd9SLaurent Vivier udelay(1); 2898852ecd9SLaurent Vivier swim_write(base, phase, (LSTRB<<4) | LSTRB); 2908852ecd9SLaurent Vivier udelay(1); 2918852ecd9SLaurent Vivier swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F)); 2928852ecd9SLaurent Vivier udelay(1); 2938852ecd9SLaurent Vivier 2948852ecd9SLaurent Vivier local_irq_restore(flags); 2958852ecd9SLaurent Vivier } 2968852ecd9SLaurent Vivier 2978852ecd9SLaurent Vivier static inline int swim_readbit(struct swim __iomem *base, int bit) 2988852ecd9SLaurent Vivier { 2998852ecd9SLaurent Vivier int stat; 3008852ecd9SLaurent Vivier 3018852ecd9SLaurent Vivier swim_select(base, bit); 3028852ecd9SLaurent Vivier 3038852ecd9SLaurent Vivier udelay(10); 3048852ecd9SLaurent Vivier 3058852ecd9SLaurent Vivier stat = swim_read(base, handshake); 3068852ecd9SLaurent Vivier 3078852ecd9SLaurent Vivier return (stat & SENSE) == 0; 3088852ecd9SLaurent Vivier } 3098852ecd9SLaurent Vivier 3108852ecd9SLaurent Vivier static inline void swim_drive(struct swim __iomem *base, 3118852ecd9SLaurent Vivier enum drive_location location) 3128852ecd9SLaurent Vivier { 3138852ecd9SLaurent Vivier if (location == INTERNAL_DRIVE) { 3148852ecd9SLaurent Vivier swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */ 3158852ecd9SLaurent Vivier swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */ 3168852ecd9SLaurent Vivier } else if (location == EXTERNAL_DRIVE) { 3178852ecd9SLaurent Vivier swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */ 3188852ecd9SLaurent Vivier swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */ 3198852ecd9SLaurent Vivier } 3208852ecd9SLaurent Vivier } 3218852ecd9SLaurent Vivier 3228852ecd9SLaurent Vivier static inline void swim_motor(struct swim __iomem *base, 3238852ecd9SLaurent Vivier enum motor_action action) 3248852ecd9SLaurent Vivier { 3258852ecd9SLaurent Vivier if (action == ON) { 3268852ecd9SLaurent Vivier int i; 3278852ecd9SLaurent Vivier 3288852ecd9SLaurent Vivier swim_action(base, MOTOR_ON); 3298852ecd9SLaurent Vivier 3308852ecd9SLaurent Vivier for (i = 0; i < 2*HZ; i++) { 3318852ecd9SLaurent Vivier swim_select(base, RELAX); 3328852ecd9SLaurent Vivier if (swim_readbit(base, MOTOR_ON)) 3338852ecd9SLaurent Vivier break; 3348852ecd9SLaurent Vivier current->state = TASK_INTERRUPTIBLE; 3358852ecd9SLaurent Vivier schedule_timeout(1); 3368852ecd9SLaurent Vivier } 3378852ecd9SLaurent Vivier } else if (action == OFF) { 3388852ecd9SLaurent Vivier swim_action(base, MOTOR_OFF); 3398852ecd9SLaurent Vivier swim_select(base, RELAX); 3408852ecd9SLaurent Vivier } 3418852ecd9SLaurent Vivier } 3428852ecd9SLaurent Vivier 3438852ecd9SLaurent Vivier static inline void swim_eject(struct swim __iomem *base) 3448852ecd9SLaurent Vivier { 3458852ecd9SLaurent Vivier int i; 3468852ecd9SLaurent Vivier 3478852ecd9SLaurent Vivier swim_action(base, EJECT); 3488852ecd9SLaurent Vivier 3498852ecd9SLaurent Vivier for (i = 0; i < 2*HZ; i++) { 3508852ecd9SLaurent Vivier swim_select(base, RELAX); 3518852ecd9SLaurent Vivier if (!swim_readbit(base, DISK_IN)) 3528852ecd9SLaurent Vivier break; 3538852ecd9SLaurent Vivier current->state = TASK_INTERRUPTIBLE; 3548852ecd9SLaurent Vivier schedule_timeout(1); 3558852ecd9SLaurent Vivier } 3568852ecd9SLaurent Vivier swim_select(base, RELAX); 3578852ecd9SLaurent Vivier } 3588852ecd9SLaurent Vivier 3598852ecd9SLaurent Vivier static inline void swim_head(struct swim __iomem *base, enum head head) 3608852ecd9SLaurent Vivier { 3618852ecd9SLaurent Vivier /* wait drive is ready */ 3628852ecd9SLaurent Vivier 3638852ecd9SLaurent Vivier if (head == UPPER_HEAD) 3648852ecd9SLaurent Vivier swim_select(base, READ_DATA_1); 3658852ecd9SLaurent Vivier else if (head == LOWER_HEAD) 3668852ecd9SLaurent Vivier swim_select(base, READ_DATA_0); 3678852ecd9SLaurent Vivier } 3688852ecd9SLaurent Vivier 3698852ecd9SLaurent Vivier static inline int swim_step(struct swim __iomem *base) 3708852ecd9SLaurent Vivier { 3718852ecd9SLaurent Vivier int wait; 3728852ecd9SLaurent Vivier 3738852ecd9SLaurent Vivier swim_action(base, STEP); 3748852ecd9SLaurent Vivier 3758852ecd9SLaurent Vivier for (wait = 0; wait < HZ; wait++) { 3768852ecd9SLaurent Vivier 3778852ecd9SLaurent Vivier current->state = TASK_INTERRUPTIBLE; 3788852ecd9SLaurent Vivier schedule_timeout(1); 3798852ecd9SLaurent Vivier 3808852ecd9SLaurent Vivier swim_select(base, RELAX); 3818852ecd9SLaurent Vivier if (!swim_readbit(base, STEP)) 3828852ecd9SLaurent Vivier return 0; 3838852ecd9SLaurent Vivier } 3848852ecd9SLaurent Vivier return -1; 3858852ecd9SLaurent Vivier } 3868852ecd9SLaurent Vivier 3878852ecd9SLaurent Vivier static inline int swim_track00(struct swim __iomem *base) 3888852ecd9SLaurent Vivier { 3898852ecd9SLaurent Vivier int try; 3908852ecd9SLaurent Vivier 3918852ecd9SLaurent Vivier swim_action(base, SEEK_NEGATIVE); 3928852ecd9SLaurent Vivier 3938852ecd9SLaurent Vivier for (try = 0; try < 100; try++) { 3948852ecd9SLaurent Vivier 3958852ecd9SLaurent Vivier swim_select(base, RELAX); 3968852ecd9SLaurent Vivier if (swim_readbit(base, TRACK_ZERO)) 3978852ecd9SLaurent Vivier break; 3988852ecd9SLaurent Vivier 3998852ecd9SLaurent Vivier if (swim_step(base)) 4008852ecd9SLaurent Vivier return -1; 4018852ecd9SLaurent Vivier } 4028852ecd9SLaurent Vivier 4038852ecd9SLaurent Vivier if (swim_readbit(base, TRACK_ZERO)) 4048852ecd9SLaurent Vivier return 0; 4058852ecd9SLaurent Vivier 4068852ecd9SLaurent Vivier return -1; 4078852ecd9SLaurent Vivier } 4088852ecd9SLaurent Vivier 4098852ecd9SLaurent Vivier static inline int swim_seek(struct swim __iomem *base, int step) 4108852ecd9SLaurent Vivier { 4118852ecd9SLaurent Vivier if (step == 0) 4128852ecd9SLaurent Vivier return 0; 4138852ecd9SLaurent Vivier 4148852ecd9SLaurent Vivier if (step < 0) { 4158852ecd9SLaurent Vivier swim_action(base, SEEK_NEGATIVE); 4168852ecd9SLaurent Vivier step = -step; 4178852ecd9SLaurent Vivier } else 4188852ecd9SLaurent Vivier swim_action(base, SEEK_POSITIVE); 4198852ecd9SLaurent Vivier 4208852ecd9SLaurent Vivier for ( ; step > 0; step--) { 4218852ecd9SLaurent Vivier if (swim_step(base)) 4228852ecd9SLaurent Vivier return -1; 4238852ecd9SLaurent Vivier } 4248852ecd9SLaurent Vivier 4258852ecd9SLaurent Vivier return 0; 4268852ecd9SLaurent Vivier } 4278852ecd9SLaurent Vivier 4288852ecd9SLaurent Vivier static inline int swim_track(struct floppy_state *fs, int track) 4298852ecd9SLaurent Vivier { 4308852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4318852ecd9SLaurent Vivier int ret; 4328852ecd9SLaurent Vivier 4338852ecd9SLaurent Vivier ret = swim_seek(base, track - fs->track); 4348852ecd9SLaurent Vivier 4358852ecd9SLaurent Vivier if (ret == 0) 4368852ecd9SLaurent Vivier fs->track = track; 4378852ecd9SLaurent Vivier else { 4388852ecd9SLaurent Vivier swim_track00(base); 4398852ecd9SLaurent Vivier fs->track = 0; 4408852ecd9SLaurent Vivier } 4418852ecd9SLaurent Vivier 4428852ecd9SLaurent Vivier return ret; 4438852ecd9SLaurent Vivier } 4448852ecd9SLaurent Vivier 4458852ecd9SLaurent Vivier static int floppy_eject(struct floppy_state *fs) 4468852ecd9SLaurent Vivier { 4478852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4488852ecd9SLaurent Vivier 4498852ecd9SLaurent Vivier swim_drive(base, fs->location); 4508852ecd9SLaurent Vivier swim_motor(base, OFF); 4518852ecd9SLaurent Vivier swim_eject(base); 4528852ecd9SLaurent Vivier 4538852ecd9SLaurent Vivier fs->disk_in = 0; 4548852ecd9SLaurent Vivier fs->ejected = 1; 4558852ecd9SLaurent Vivier 4568852ecd9SLaurent Vivier return 0; 4578852ecd9SLaurent Vivier } 4588852ecd9SLaurent Vivier 4598852ecd9SLaurent Vivier static inline int swim_read_sector(struct floppy_state *fs, 4608852ecd9SLaurent Vivier int side, int track, 4618852ecd9SLaurent Vivier int sector, unsigned char *buffer) 4628852ecd9SLaurent Vivier { 4638852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 4648852ecd9SLaurent Vivier unsigned long flags; 4658852ecd9SLaurent Vivier struct sector_header header; 4668852ecd9SLaurent Vivier int ret = -1; 4678852ecd9SLaurent Vivier short i; 4688852ecd9SLaurent Vivier 4698852ecd9SLaurent Vivier swim_track(fs, track); 4708852ecd9SLaurent Vivier 4718852ecd9SLaurent Vivier swim_write(base, mode1, MOTON); 4728852ecd9SLaurent Vivier swim_head(base, side); 4738852ecd9SLaurent Vivier swim_write(base, mode0, side); 4748852ecd9SLaurent Vivier 4758852ecd9SLaurent Vivier local_irq_save(flags); 4768852ecd9SLaurent Vivier for (i = 0; i < 36; i++) { 4778852ecd9SLaurent Vivier ret = swim_read_sector_header(base, &header); 4788852ecd9SLaurent Vivier if (!ret && (header.sector == sector)) { 4798852ecd9SLaurent Vivier /* found */ 4808852ecd9SLaurent Vivier 4818852ecd9SLaurent Vivier ret = swim_read_sector_data(base, buffer); 4828852ecd9SLaurent Vivier break; 4838852ecd9SLaurent Vivier } 4848852ecd9SLaurent Vivier } 4858852ecd9SLaurent Vivier local_irq_restore(flags); 4868852ecd9SLaurent Vivier 4878852ecd9SLaurent Vivier swim_write(base, mode0, MOTON); 4888852ecd9SLaurent Vivier 4898852ecd9SLaurent Vivier if ((header.side != side) || (header.track != track) || 4908852ecd9SLaurent Vivier (header.sector != sector)) 4918852ecd9SLaurent Vivier return 0; 4928852ecd9SLaurent Vivier 4938852ecd9SLaurent Vivier return ret; 4948852ecd9SLaurent Vivier } 4958852ecd9SLaurent Vivier 4968852ecd9SLaurent Vivier static int floppy_read_sectors(struct floppy_state *fs, 4978852ecd9SLaurent Vivier int req_sector, int sectors_nb, 4988852ecd9SLaurent Vivier unsigned char *buffer) 4998852ecd9SLaurent Vivier { 5008852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 5018852ecd9SLaurent Vivier int ret; 5028852ecd9SLaurent Vivier int side, track, sector; 5038852ecd9SLaurent Vivier int i, try; 5048852ecd9SLaurent Vivier 5058852ecd9SLaurent Vivier 5068852ecd9SLaurent Vivier swim_drive(base, fs->location); 5078852ecd9SLaurent Vivier for (i = req_sector; i < req_sector + sectors_nb; i++) { 5088852ecd9SLaurent Vivier int x; 5098852ecd9SLaurent Vivier track = i / fs->secpercyl; 5108852ecd9SLaurent Vivier x = i % fs->secpercyl; 5118852ecd9SLaurent Vivier side = x / fs->secpertrack; 5128852ecd9SLaurent Vivier sector = x % fs->secpertrack + 1; 5138852ecd9SLaurent Vivier 5148852ecd9SLaurent Vivier try = 5; 5158852ecd9SLaurent Vivier do { 5168852ecd9SLaurent Vivier ret = swim_read_sector(fs, side, track, sector, 5178852ecd9SLaurent Vivier buffer); 5188852ecd9SLaurent Vivier if (try-- == 0) 51906b0608eSTejun Heo return -EIO; 5208852ecd9SLaurent Vivier } while (ret != 512); 5218852ecd9SLaurent Vivier 5228852ecd9SLaurent Vivier buffer += ret; 5238852ecd9SLaurent Vivier } 5248852ecd9SLaurent Vivier 5258852ecd9SLaurent Vivier return 0; 5268852ecd9SLaurent Vivier } 5278852ecd9SLaurent Vivier 5288852ecd9SLaurent Vivier static void redo_fd_request(struct request_queue *q) 5298852ecd9SLaurent Vivier { 5308852ecd9SLaurent Vivier struct request *req; 5318852ecd9SLaurent Vivier struct floppy_state *fs; 5328852ecd9SLaurent Vivier 5339934c8c0STejun Heo req = blk_fetch_request(q); 53406b0608eSTejun Heo while (req) { 53506b0608eSTejun Heo int err = -EIO; 5368852ecd9SLaurent Vivier 5378852ecd9SLaurent Vivier fs = req->rq_disk->private_data; 53806b0608eSTejun Heo if (blk_rq_pos(req) >= fs->total_secs) 53906b0608eSTejun Heo goto done; 54006b0608eSTejun Heo if (!fs->disk_in) 54106b0608eSTejun Heo goto done; 54206b0608eSTejun Heo if (rq_data_dir(req) == WRITE && fs->write_protected) 54306b0608eSTejun Heo goto done; 54406b0608eSTejun Heo 5458852ecd9SLaurent Vivier switch (rq_data_dir(req)) { 5468852ecd9SLaurent Vivier case WRITE: 5478852ecd9SLaurent Vivier /* NOT IMPLEMENTED */ 5488852ecd9SLaurent Vivier break; 5498852ecd9SLaurent Vivier case READ: 55006b0608eSTejun Heo err = floppy_read_sectors(fs, blk_rq_pos(req), 55183096ebfSTejun Heo blk_rq_cur_sectors(req), 55206b0608eSTejun Heo req->buffer); 5538852ecd9SLaurent Vivier break; 5548852ecd9SLaurent Vivier } 55506b0608eSTejun Heo done: 5569934c8c0STejun Heo if (!__blk_end_request_cur(req, err)) 5579934c8c0STejun Heo req = blk_fetch_request(q); 5588852ecd9SLaurent Vivier } 5598852ecd9SLaurent Vivier } 5608852ecd9SLaurent Vivier 5618852ecd9SLaurent Vivier static void do_fd_request(struct request_queue *q) 5628852ecd9SLaurent Vivier { 5638852ecd9SLaurent Vivier redo_fd_request(q); 5648852ecd9SLaurent Vivier } 5658852ecd9SLaurent Vivier 5668852ecd9SLaurent Vivier static struct floppy_struct floppy_type[4] = { 5678852ecd9SLaurent Vivier { 0, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing */ 5688852ecd9SLaurent Vivier { 720, 9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/ 5698852ecd9SLaurent Vivier { 1440, 9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5" */ 5708852ecd9SLaurent Vivier { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5" */ 5718852ecd9SLaurent Vivier }; 5728852ecd9SLaurent Vivier 5738852ecd9SLaurent Vivier static int get_floppy_geometry(struct floppy_state *fs, int type, 5748852ecd9SLaurent Vivier struct floppy_struct **g) 5758852ecd9SLaurent Vivier { 5768852ecd9SLaurent Vivier if (type >= ARRAY_SIZE(floppy_type)) 5778852ecd9SLaurent Vivier return -EINVAL; 5788852ecd9SLaurent Vivier 5798852ecd9SLaurent Vivier if (type) 5808852ecd9SLaurent Vivier *g = &floppy_type[type]; 5818852ecd9SLaurent Vivier else if (fs->type == HD_MEDIA) /* High-Density media */ 5828852ecd9SLaurent Vivier *g = &floppy_type[3]; 5838852ecd9SLaurent Vivier else if (fs->head_number == 2) /* double-sided */ 5848852ecd9SLaurent Vivier *g = &floppy_type[2]; 5858852ecd9SLaurent Vivier else 5868852ecd9SLaurent Vivier *g = &floppy_type[1]; 5878852ecd9SLaurent Vivier 5888852ecd9SLaurent Vivier return 0; 5898852ecd9SLaurent Vivier } 5908852ecd9SLaurent Vivier 5918852ecd9SLaurent Vivier static void setup_medium(struct floppy_state *fs) 5928852ecd9SLaurent Vivier { 5938852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 5948852ecd9SLaurent Vivier 5958852ecd9SLaurent Vivier if (swim_readbit(base, DISK_IN)) { 5968852ecd9SLaurent Vivier struct floppy_struct *g; 5978852ecd9SLaurent Vivier fs->disk_in = 1; 5988852ecd9SLaurent Vivier fs->write_protected = swim_readbit(base, WRITE_PROT); 5998852ecd9SLaurent Vivier fs->type = swim_readbit(base, ONEMEG_MEDIA); 6008852ecd9SLaurent Vivier 6018852ecd9SLaurent Vivier if (swim_track00(base)) 6028852ecd9SLaurent Vivier printk(KERN_ERR 6038852ecd9SLaurent Vivier "SWIM: cannot move floppy head to track 0\n"); 6048852ecd9SLaurent Vivier 6058852ecd9SLaurent Vivier swim_track00(base); 6068852ecd9SLaurent Vivier 6078852ecd9SLaurent Vivier get_floppy_geometry(fs, 0, &g); 6088852ecd9SLaurent Vivier fs->total_secs = g->size; 6098852ecd9SLaurent Vivier fs->secpercyl = g->head * g->sect; 6108852ecd9SLaurent Vivier fs->secpertrack = g->sect; 6118852ecd9SLaurent Vivier fs->track = 0; 6128852ecd9SLaurent Vivier } else { 6138852ecd9SLaurent Vivier fs->disk_in = 0; 6148852ecd9SLaurent Vivier } 6158852ecd9SLaurent Vivier } 6168852ecd9SLaurent Vivier 6178852ecd9SLaurent Vivier static int floppy_open(struct block_device *bdev, fmode_t mode) 6188852ecd9SLaurent Vivier { 6198852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 6208852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 6218852ecd9SLaurent Vivier int err; 6228852ecd9SLaurent Vivier 6238852ecd9SLaurent Vivier if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL)) 6248852ecd9SLaurent Vivier return -EBUSY; 6258852ecd9SLaurent Vivier 6268852ecd9SLaurent Vivier if (mode & FMODE_EXCL) 6278852ecd9SLaurent Vivier fs->ref_count = -1; 6288852ecd9SLaurent Vivier else 6298852ecd9SLaurent Vivier fs->ref_count++; 6308852ecd9SLaurent Vivier 6318852ecd9SLaurent Vivier swim_write(base, setup, S_IBM_DRIVE | S_FCLK_DIV2); 6328852ecd9SLaurent Vivier udelay(10); 6338852ecd9SLaurent Vivier swim_drive(base, INTERNAL_DRIVE); 6348852ecd9SLaurent Vivier swim_motor(base, ON); 6358852ecd9SLaurent Vivier swim_action(base, SETMFM); 6368852ecd9SLaurent Vivier if (fs->ejected) 6378852ecd9SLaurent Vivier setup_medium(fs); 6388852ecd9SLaurent Vivier if (!fs->disk_in) { 6398852ecd9SLaurent Vivier err = -ENXIO; 6408852ecd9SLaurent Vivier goto out; 6418852ecd9SLaurent Vivier } 6428852ecd9SLaurent Vivier 6438852ecd9SLaurent Vivier if (mode & FMODE_NDELAY) 6448852ecd9SLaurent Vivier return 0; 6458852ecd9SLaurent Vivier 6468852ecd9SLaurent Vivier if (mode & (FMODE_READ|FMODE_WRITE)) { 6478852ecd9SLaurent Vivier check_disk_change(bdev); 6488852ecd9SLaurent Vivier if ((mode & FMODE_WRITE) && fs->write_protected) { 6498852ecd9SLaurent Vivier err = -EROFS; 6508852ecd9SLaurent Vivier goto out; 6518852ecd9SLaurent Vivier } 6528852ecd9SLaurent Vivier } 6538852ecd9SLaurent Vivier return 0; 6548852ecd9SLaurent Vivier out: 6558852ecd9SLaurent Vivier if (fs->ref_count < 0) 6568852ecd9SLaurent Vivier fs->ref_count = 0; 6578852ecd9SLaurent Vivier else if (fs->ref_count > 0) 6588852ecd9SLaurent Vivier --fs->ref_count; 6598852ecd9SLaurent Vivier 6608852ecd9SLaurent Vivier if (fs->ref_count == 0) 6618852ecd9SLaurent Vivier swim_motor(base, OFF); 6628852ecd9SLaurent Vivier return err; 6638852ecd9SLaurent Vivier } 6648852ecd9SLaurent Vivier 6656e9624b8SArnd Bergmann static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode) 6666e9624b8SArnd Bergmann { 6676e9624b8SArnd Bergmann int ret; 6686e9624b8SArnd Bergmann 6692a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 6706e9624b8SArnd Bergmann ret = floppy_open(bdev, mode); 6712a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 6726e9624b8SArnd Bergmann 6736e9624b8SArnd Bergmann return ret; 6746e9624b8SArnd Bergmann } 6756e9624b8SArnd Bergmann 676db2a144bSAl Viro static void floppy_release(struct gendisk *disk, fmode_t mode) 6778852ecd9SLaurent Vivier { 6788852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 6798852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 6808852ecd9SLaurent Vivier 6812a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 6828852ecd9SLaurent Vivier if (fs->ref_count < 0) 6838852ecd9SLaurent Vivier fs->ref_count = 0; 6848852ecd9SLaurent Vivier else if (fs->ref_count > 0) 6858852ecd9SLaurent Vivier --fs->ref_count; 6868852ecd9SLaurent Vivier 6878852ecd9SLaurent Vivier if (fs->ref_count == 0) 6888852ecd9SLaurent Vivier swim_motor(base, OFF); 6892a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 6908852ecd9SLaurent Vivier } 6918852ecd9SLaurent Vivier 6928852ecd9SLaurent Vivier static int floppy_ioctl(struct block_device *bdev, fmode_t mode, 6938852ecd9SLaurent Vivier unsigned int cmd, unsigned long param) 6948852ecd9SLaurent Vivier { 6958852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 6968852ecd9SLaurent Vivier int err; 6978852ecd9SLaurent Vivier 6988852ecd9SLaurent Vivier if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN)) 6998852ecd9SLaurent Vivier return -EPERM; 7008852ecd9SLaurent Vivier 7018852ecd9SLaurent Vivier switch (cmd) { 7028852ecd9SLaurent Vivier case FDEJECT: 7038852ecd9SLaurent Vivier if (fs->ref_count != 1) 7048852ecd9SLaurent Vivier return -EBUSY; 7052a48fc0aSArnd Bergmann mutex_lock(&swim_mutex); 7068852ecd9SLaurent Vivier err = floppy_eject(fs); 7072a48fc0aSArnd Bergmann mutex_unlock(&swim_mutex); 7088852ecd9SLaurent Vivier return err; 7098852ecd9SLaurent Vivier 7108852ecd9SLaurent Vivier case FDGETPRM: 7118852ecd9SLaurent Vivier if (copy_to_user((void __user *) param, (void *) &floppy_type, 7128852ecd9SLaurent Vivier sizeof(struct floppy_struct))) 7138852ecd9SLaurent Vivier return -EFAULT; 7148852ecd9SLaurent Vivier break; 7158852ecd9SLaurent Vivier 7168852ecd9SLaurent Vivier default: 7178852ecd9SLaurent Vivier printk(KERN_DEBUG "SWIM floppy_ioctl: unknown cmd %d\n", 7188852ecd9SLaurent Vivier cmd); 7198852ecd9SLaurent Vivier return -ENOSYS; 7208852ecd9SLaurent Vivier } 7218852ecd9SLaurent Vivier return 0; 7228852ecd9SLaurent Vivier } 7238852ecd9SLaurent Vivier 7248852ecd9SLaurent Vivier static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo) 7258852ecd9SLaurent Vivier { 7268852ecd9SLaurent Vivier struct floppy_state *fs = bdev->bd_disk->private_data; 7278852ecd9SLaurent Vivier struct floppy_struct *g; 7288852ecd9SLaurent Vivier int ret; 7298852ecd9SLaurent Vivier 7308852ecd9SLaurent Vivier ret = get_floppy_geometry(fs, 0, &g); 7318852ecd9SLaurent Vivier if (ret) 7328852ecd9SLaurent Vivier return ret; 7338852ecd9SLaurent Vivier 7348852ecd9SLaurent Vivier geo->heads = g->head; 7358852ecd9SLaurent Vivier geo->sectors = g->sect; 7368852ecd9SLaurent Vivier geo->cylinders = g->track; 7378852ecd9SLaurent Vivier 7388852ecd9SLaurent Vivier return 0; 7398852ecd9SLaurent Vivier } 7408852ecd9SLaurent Vivier 7414bbde777STejun Heo static unsigned int floppy_check_events(struct gendisk *disk, 7424bbde777STejun Heo unsigned int clearing) 7438852ecd9SLaurent Vivier { 7448852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 7458852ecd9SLaurent Vivier 7464bbde777STejun Heo return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0; 7478852ecd9SLaurent Vivier } 7488852ecd9SLaurent Vivier 7498852ecd9SLaurent Vivier static int floppy_revalidate(struct gendisk *disk) 7508852ecd9SLaurent Vivier { 7518852ecd9SLaurent Vivier struct floppy_state *fs = disk->private_data; 7528852ecd9SLaurent Vivier struct swim __iomem *base = fs->swd->base; 7538852ecd9SLaurent Vivier 7548852ecd9SLaurent Vivier swim_drive(base, fs->location); 7558852ecd9SLaurent Vivier 7568852ecd9SLaurent Vivier if (fs->ejected) 7578852ecd9SLaurent Vivier setup_medium(fs); 7588852ecd9SLaurent Vivier 7598852ecd9SLaurent Vivier if (!fs->disk_in) 7608852ecd9SLaurent Vivier swim_motor(base, OFF); 7618852ecd9SLaurent Vivier else 7628852ecd9SLaurent Vivier fs->ejected = 0; 7638852ecd9SLaurent Vivier 7648852ecd9SLaurent Vivier return !fs->disk_in; 7658852ecd9SLaurent Vivier } 7668852ecd9SLaurent Vivier 76783d5cde4SAlexey Dobriyan static const struct block_device_operations floppy_fops = { 7688852ecd9SLaurent Vivier .owner = THIS_MODULE, 7696e9624b8SArnd Bergmann .open = floppy_unlocked_open, 7708852ecd9SLaurent Vivier .release = floppy_release, 7718a6cfeb6SArnd Bergmann .ioctl = floppy_ioctl, 7728852ecd9SLaurent Vivier .getgeo = floppy_getgeo, 7734bbde777STejun Heo .check_events = floppy_check_events, 7748852ecd9SLaurent Vivier .revalidate_disk = floppy_revalidate, 7758852ecd9SLaurent Vivier }; 7768852ecd9SLaurent Vivier 7778852ecd9SLaurent Vivier static struct kobject *floppy_find(dev_t dev, int *part, void *data) 7788852ecd9SLaurent Vivier { 7798852ecd9SLaurent Vivier struct swim_priv *swd = data; 7808852ecd9SLaurent Vivier int drive = (*part & 3); 7818852ecd9SLaurent Vivier 7828852ecd9SLaurent Vivier if (drive > swd->floppy_count) 7838852ecd9SLaurent Vivier return NULL; 7848852ecd9SLaurent Vivier 7858852ecd9SLaurent Vivier *part = 0; 7868852ecd9SLaurent Vivier return get_disk(swd->unit[drive].disk); 7878852ecd9SLaurent Vivier } 7888852ecd9SLaurent Vivier 7898d85fce7SGreg Kroah-Hartman static int swim_add_floppy(struct swim_priv *swd, enum drive_location location) 7908852ecd9SLaurent Vivier { 7918852ecd9SLaurent Vivier struct floppy_state *fs = &swd->unit[swd->floppy_count]; 7928852ecd9SLaurent Vivier struct swim __iomem *base = swd->base; 7938852ecd9SLaurent Vivier 7948852ecd9SLaurent Vivier fs->location = location; 7958852ecd9SLaurent Vivier 7968852ecd9SLaurent Vivier swim_drive(base, location); 7978852ecd9SLaurent Vivier 7988852ecd9SLaurent Vivier swim_motor(base, OFF); 7998852ecd9SLaurent Vivier 8008852ecd9SLaurent Vivier if (swim_readbit(base, SINGLE_SIDED)) 8018852ecd9SLaurent Vivier fs->head_number = 1; 8028852ecd9SLaurent Vivier else 8038852ecd9SLaurent Vivier fs->head_number = 2; 8048852ecd9SLaurent Vivier fs->ref_count = 0; 8058852ecd9SLaurent Vivier fs->ejected = 1; 8068852ecd9SLaurent Vivier 8078852ecd9SLaurent Vivier swd->floppy_count++; 8088852ecd9SLaurent Vivier 8098852ecd9SLaurent Vivier return 0; 8108852ecd9SLaurent Vivier } 8118852ecd9SLaurent Vivier 8128d85fce7SGreg Kroah-Hartman static int swim_floppy_init(struct swim_priv *swd) 8138852ecd9SLaurent Vivier { 8148852ecd9SLaurent Vivier int err; 8158852ecd9SLaurent Vivier int drive; 8168852ecd9SLaurent Vivier struct swim __iomem *base = swd->base; 8178852ecd9SLaurent Vivier 8188852ecd9SLaurent Vivier /* scan floppy drives */ 8198852ecd9SLaurent Vivier 8208852ecd9SLaurent Vivier swim_drive(base, INTERNAL_DRIVE); 8218852ecd9SLaurent Vivier if (swim_readbit(base, DRIVE_PRESENT)) 8228852ecd9SLaurent Vivier swim_add_floppy(swd, INTERNAL_DRIVE); 8238852ecd9SLaurent Vivier swim_drive(base, EXTERNAL_DRIVE); 8248852ecd9SLaurent Vivier if (swim_readbit(base, DRIVE_PRESENT)) 8258852ecd9SLaurent Vivier swim_add_floppy(swd, EXTERNAL_DRIVE); 8268852ecd9SLaurent Vivier 8278852ecd9SLaurent Vivier /* register floppy drives */ 8288852ecd9SLaurent Vivier 8298852ecd9SLaurent Vivier err = register_blkdev(FLOPPY_MAJOR, "fd"); 8308852ecd9SLaurent Vivier if (err) { 8318852ecd9SLaurent Vivier printk(KERN_ERR "Unable to get major %d for SWIM floppy\n", 8328852ecd9SLaurent Vivier FLOPPY_MAJOR); 8338852ecd9SLaurent Vivier return -EBUSY; 8348852ecd9SLaurent Vivier } 8358852ecd9SLaurent Vivier 8368852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 8378852ecd9SLaurent Vivier swd->unit[drive].disk = alloc_disk(1); 8388852ecd9SLaurent Vivier if (swd->unit[drive].disk == NULL) { 8398852ecd9SLaurent Vivier err = -ENOMEM; 8408852ecd9SLaurent Vivier goto exit_put_disks; 8418852ecd9SLaurent Vivier } 8428852ecd9SLaurent Vivier swd->unit[drive].swd = swd; 8438852ecd9SLaurent Vivier } 8448852ecd9SLaurent Vivier 845243eeb78SJean Delvare spin_lock_init(&swd->lock); 8468852ecd9SLaurent Vivier swd->queue = blk_init_queue(do_fd_request, &swd->lock); 8478852ecd9SLaurent Vivier if (!swd->queue) { 8488852ecd9SLaurent Vivier err = -ENOMEM; 8498852ecd9SLaurent Vivier goto exit_put_disks; 8508852ecd9SLaurent Vivier } 8518852ecd9SLaurent Vivier 8528852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 8538852ecd9SLaurent Vivier swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE; 8548852ecd9SLaurent Vivier swd->unit[drive].disk->major = FLOPPY_MAJOR; 8558852ecd9SLaurent Vivier swd->unit[drive].disk->first_minor = drive; 8568852ecd9SLaurent Vivier sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive); 8578852ecd9SLaurent Vivier swd->unit[drive].disk->fops = &floppy_fops; 8588852ecd9SLaurent Vivier swd->unit[drive].disk->private_data = &swd->unit[drive]; 8598852ecd9SLaurent Vivier swd->unit[drive].disk->queue = swd->queue; 8608852ecd9SLaurent Vivier set_capacity(swd->unit[drive].disk, 2880); 8618852ecd9SLaurent Vivier add_disk(swd->unit[drive].disk); 8628852ecd9SLaurent Vivier } 8638852ecd9SLaurent Vivier 8648852ecd9SLaurent Vivier blk_register_region(MKDEV(FLOPPY_MAJOR, 0), 256, THIS_MODULE, 8658852ecd9SLaurent Vivier floppy_find, NULL, swd); 8668852ecd9SLaurent Vivier 8678852ecd9SLaurent Vivier return 0; 8688852ecd9SLaurent Vivier 8698852ecd9SLaurent Vivier exit_put_disks: 8708852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd"); 8718852ecd9SLaurent Vivier while (drive--) 8728852ecd9SLaurent Vivier put_disk(swd->unit[drive].disk); 8738852ecd9SLaurent Vivier return err; 8748852ecd9SLaurent Vivier } 8758852ecd9SLaurent Vivier 8768d85fce7SGreg Kroah-Hartman static int swim_probe(struct platform_device *dev) 8778852ecd9SLaurent Vivier { 8788852ecd9SLaurent Vivier struct resource *res; 8798852ecd9SLaurent Vivier struct swim __iomem *swim_base; 8808852ecd9SLaurent Vivier struct swim_priv *swd; 8818852ecd9SLaurent Vivier int ret; 8828852ecd9SLaurent Vivier 8832724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0); 8848852ecd9SLaurent Vivier if (!res) { 8858852ecd9SLaurent Vivier ret = -ENODEV; 8868852ecd9SLaurent Vivier goto out; 8878852ecd9SLaurent Vivier } 8888852ecd9SLaurent Vivier 8898852ecd9SLaurent Vivier if (!request_mem_region(res->start, resource_size(res), CARDNAME)) { 8908852ecd9SLaurent Vivier ret = -EBUSY; 8918852ecd9SLaurent Vivier goto out; 8928852ecd9SLaurent Vivier } 8938852ecd9SLaurent Vivier 8948852ecd9SLaurent Vivier swim_base = ioremap(res->start, resource_size(res)); 8958852ecd9SLaurent Vivier if (!swim_base) { 896957d6bf6SJoe Perches ret = -ENOMEM; 8978852ecd9SLaurent Vivier goto out_release_io; 8988852ecd9SLaurent Vivier } 8998852ecd9SLaurent Vivier 9008852ecd9SLaurent Vivier /* probe device */ 9018852ecd9SLaurent Vivier 9028852ecd9SLaurent Vivier set_swim_mode(swim_base, 1); 9038852ecd9SLaurent Vivier if (!get_swim_mode(swim_base)) { 9048852ecd9SLaurent Vivier printk(KERN_INFO "SWIM device not found !\n"); 9058852ecd9SLaurent Vivier ret = -ENODEV; 9068852ecd9SLaurent Vivier goto out_iounmap; 9078852ecd9SLaurent Vivier } 9088852ecd9SLaurent Vivier 9098852ecd9SLaurent Vivier /* set platform driver data */ 9108852ecd9SLaurent Vivier 9118852ecd9SLaurent Vivier swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL); 9128852ecd9SLaurent Vivier if (!swd) { 9138852ecd9SLaurent Vivier ret = -ENOMEM; 9148852ecd9SLaurent Vivier goto out_iounmap; 9158852ecd9SLaurent Vivier } 9168852ecd9SLaurent Vivier platform_set_drvdata(dev, swd); 9178852ecd9SLaurent Vivier 9188852ecd9SLaurent Vivier swd->base = swim_base; 9198852ecd9SLaurent Vivier 9208852ecd9SLaurent Vivier ret = swim_floppy_init(swd); 9218852ecd9SLaurent Vivier if (ret) 9228852ecd9SLaurent Vivier goto out_kfree; 9238852ecd9SLaurent Vivier 9248852ecd9SLaurent Vivier return 0; 9258852ecd9SLaurent Vivier 9268852ecd9SLaurent Vivier out_kfree: 9278852ecd9SLaurent Vivier platform_set_drvdata(dev, NULL); 9288852ecd9SLaurent Vivier kfree(swd); 9298852ecd9SLaurent Vivier out_iounmap: 9308852ecd9SLaurent Vivier iounmap(swim_base); 9318852ecd9SLaurent Vivier out_release_io: 9328852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res)); 9338852ecd9SLaurent Vivier out: 9348852ecd9SLaurent Vivier return ret; 9358852ecd9SLaurent Vivier } 9368852ecd9SLaurent Vivier 9378d85fce7SGreg Kroah-Hartman static int swim_remove(struct platform_device *dev) 9388852ecd9SLaurent Vivier { 9398852ecd9SLaurent Vivier struct swim_priv *swd = platform_get_drvdata(dev); 9408852ecd9SLaurent Vivier int drive; 9418852ecd9SLaurent Vivier struct resource *res; 9428852ecd9SLaurent Vivier 9438852ecd9SLaurent Vivier blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256); 9448852ecd9SLaurent Vivier 9458852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) { 9468852ecd9SLaurent Vivier del_gendisk(swd->unit[drive].disk); 9478852ecd9SLaurent Vivier put_disk(swd->unit[drive].disk); 9488852ecd9SLaurent Vivier } 9498852ecd9SLaurent Vivier 9508852ecd9SLaurent Vivier unregister_blkdev(FLOPPY_MAJOR, "fd"); 9518852ecd9SLaurent Vivier 9528852ecd9SLaurent Vivier blk_cleanup_queue(swd->queue); 9538852ecd9SLaurent Vivier 9548852ecd9SLaurent Vivier /* eject floppies */ 9558852ecd9SLaurent Vivier 9568852ecd9SLaurent Vivier for (drive = 0; drive < swd->floppy_count; drive++) 9578852ecd9SLaurent Vivier floppy_eject(&swd->unit[drive]); 9588852ecd9SLaurent Vivier 9598852ecd9SLaurent Vivier iounmap(swd->base); 9608852ecd9SLaurent Vivier 9612724daf4SFinn Thain res = platform_get_resource(dev, IORESOURCE_MEM, 0); 9628852ecd9SLaurent Vivier if (res) 9638852ecd9SLaurent Vivier release_mem_region(res->start, resource_size(res)); 9648852ecd9SLaurent Vivier 9658852ecd9SLaurent Vivier platform_set_drvdata(dev, NULL); 9668852ecd9SLaurent Vivier kfree(swd); 9678852ecd9SLaurent Vivier 9688852ecd9SLaurent Vivier return 0; 9698852ecd9SLaurent Vivier } 9708852ecd9SLaurent Vivier 9718852ecd9SLaurent Vivier static struct platform_driver swim_driver = { 9728852ecd9SLaurent Vivier .probe = swim_probe, 9738d85fce7SGreg Kroah-Hartman .remove = swim_remove, 9748852ecd9SLaurent Vivier .driver = { 9758852ecd9SLaurent Vivier .name = CARDNAME, 9768852ecd9SLaurent Vivier .owner = THIS_MODULE, 9778852ecd9SLaurent Vivier }, 9788852ecd9SLaurent Vivier }; 9798852ecd9SLaurent Vivier 9808852ecd9SLaurent Vivier static int __init swim_init(void) 9818852ecd9SLaurent Vivier { 9828852ecd9SLaurent Vivier printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION); 9838852ecd9SLaurent Vivier 9848852ecd9SLaurent Vivier return platform_driver_register(&swim_driver); 9858852ecd9SLaurent Vivier } 9868852ecd9SLaurent Vivier module_init(swim_init); 9878852ecd9SLaurent Vivier 9888852ecd9SLaurent Vivier static void __exit swim_exit(void) 9898852ecd9SLaurent Vivier { 9908852ecd9SLaurent Vivier platform_driver_unregister(&swim_driver); 9918852ecd9SLaurent Vivier } 9928852ecd9SLaurent Vivier module_exit(swim_exit); 9938852ecd9SLaurent Vivier 9948852ecd9SLaurent Vivier MODULE_DESCRIPTION("Driver for SWIM floppy controller"); 9958852ecd9SLaurent Vivier MODULE_LICENSE("GPL"); 9968852ecd9SLaurent Vivier MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>"); 9978852ecd9SLaurent Vivier MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR); 998