Re: [PATCH 3/6] m68k: mac - Add SWIM floppy support

From: Geert Uytterhoeven
Date: Wed Mar 25 2009 - 10:56:14 EST

Next message: Thierry Reding: "[PATCH v2] net: Add support for the OpenCores 10/100 Mbps Ethernet MAC."
Previous message: Thierry Reding: "Re: [PATCH] net: Add support for the OpenCores 10/100 MbpsEthernet MAC."
Next in thread: Jens Axboe: "Re: [PATCH 3/6] m68k: mac - Add SWIM floppy support"
Messages sorted by: [ date ] [ thread ] [ subject ] [ author ]

Hi Jens,

Is this OK for you to go in through the m68k tree?

Thanks!

On Sun, Mar 1, 2009 at 10:21, Geert Uytterhoeven <geert@xxxxxxxxxxxxxx> wrote:
> From: Laurent Vivier <laurent@xxxxxxxxxxxx>
>
> It allows to read data from a floppy, but not to write to, and to eject the
> floppy (useful on our Mac without eject button).
>
> Signed-off-by: Laurent Vivier <Laurent@xxxxxxxxxxxx>
> Signed-off-by: Geert Uytterhoeven <geert@xxxxxxxxxxxxxx>
> Cc: Jens Axboe <axboe@xxxxxxxxx>
> ---
> Âarch/m68k/mac/config.c Â | Â 44 ++
> Âarch/m68k/mac/via.c Â Â Â| Â Â9 +
> Âdrivers/block/Kconfig Â Â| Â Â7 +
> Âdrivers/block/Makefile Â | Â Â3 +
> Âdrivers/block/swim.c Â Â | Â995 ++++++++++++++++++++++++++++++++++++++++++++++
> Âdrivers/block/swim_asm.S | Â247 ++++++++++++
> Â6 files changed, 1305 insertions(+), 0 deletions(-)
> Âcreate mode 100644 drivers/block/swim.c
> Âcreate mode 100644 drivers/block/swim_asm.S
>
> diff --git a/arch/m68k/mac/config.c b/arch/m68k/mac/config.c
> index 3a1c0b2..be01798 100644
> --- a/arch/m68k/mac/config.c
> +++ b/arch/m68k/mac/config.c
> @@ -22,6 +22,7 @@
> Â/* keyb */
> Â#include <linux/init.h>
> Â#include <linux/vt_kern.h>
> +#include <linux/platform_device.h>
>
> Â#define BOOTINFO_COMPAT_1_0
> Â#include <asm/setup.h>
> @@ -43,6 +44,10 @@
> Â#include <asm/mac_oss.h>
> Â#include <asm/mac_psc.h>
>
> +/* platform device info */
> +
> +#define SWIM_IO_SIZE 0x2000 Â Â/* SWIM IO resource size */
> +
> Â/* Mac bootinfo struct */
>
> Âstruct mac_booter_data mac_bi_data;
> @@ -870,3 +875,42 @@ static void mac_get_model(char *str)
> Â Â Â Âstrcpy(str, "Macintosh ");
> Â Â Â Âstrcat(str, macintosh_config->name);
> Â}
> +
> +static struct resource swim_resources[1];
> +
> +static struct platform_device swim_device = {
> + Â Â Â .name Â Â Â Â Â = "swim",
> + Â Â Â .id Â Â Â Â Â Â = -1,
> + Â Â Â .num_resources Â= ARRAY_SIZE(swim_resources),
> + Â Â Â .resource Â Â Â = swim_resources,
> +};
> +
> +static struct platform_device *mac_platform_devices[] __initdata = {
> + Â Â Â &swim_device
> +};
> +
> +int __init mac_platform_init(void)
> +{
> + Â Â Â u8 *swim_base;
> +
> + Â Â Â switch (macintosh_config->floppy_type) {
> + Â Â Â case MAC_FLOPPY_SWIM_ADDR1:
> + Â Â Â Â Â Â Â swim_base = (u8 *)(VIA1_BASE + 0x1E000);
> + Â Â Â Â Â Â Â break;
> + Â Â Â case MAC_FLOPPY_SWIM_ADDR2:
> + Â Â Â Â Â Â Â swim_base = (u8 *)(VIA1_BASE + 0x16000);
> + Â Â Â Â Â Â Â break;
> + Â Â Â default:
> + Â Â Â Â Â Â Â return 0;
> + Â Â Â }
> +
> + Â Â Â swim_resources[0].name = "swim-regs";
> + Â Â Â swim_resources[0].start = (resource_size_t)swim_base;
> + Â Â Â swim_resources[0].end = (resource_size_t)(swim_base + SWIM_IO_SIZE);
> + Â Â Â swim_resources[0].flags = IORESOURCE_MEM;
> +
> + Â Â Â return platform_add_devices(mac_platform_devices,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â ARRAY_SIZE(mac_platform_devices));
> +}
> +
> +arch_initcall(mac_platform_init);
> diff --git a/arch/m68k/mac/via.c b/arch/m68k/mac/via.c
> index 7d97ba5..11bce3c 100644
> --- a/arch/m68k/mac/via.c
> +++ b/arch/m68k/mac/via.c
> @@ -645,3 +645,12 @@ int via_irq_pending(int irq)
> Â Â Â Â}
> Â Â Â Âreturn 0;
> Â}
> +
> +void via1_set_head(int head)
> +{
> + Â Â Â if (head == 0)
> + Â Â Â Â Â Â Â via1[vBufA] &= ~VIA1A_vHeadSel;
> + Â Â Â else
> + Â Â Â Â Â Â Â via1[vBufA] |= VIA1A_vHeadSel;
> +}
> +EXPORT_SYMBOL(via1_set_head);
> diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
> index 0344a8a..e7b8aa0 100644
> --- a/drivers/block/Kconfig
> +++ b/drivers/block/Kconfig
> @@ -45,6 +45,13 @@ config MAC_FLOPPY
> Â Â Â Â ÂIf you have a SWIM-3 (Super Woz Integrated Machine 3; from Apple)
> Â Â Â Â Âfloppy controller, say Y here. Most commonly found in PowerMacs.
>
> +config BLK_DEV_SWIM
> + Â Â Â tristate "Support for SWIM Macintosh floppy"
> + Â Â Â depends on M68K && MAC
> + Â Â Â help
> + Â Â Â Â You should select this option if you want floppy support
> + Â Â Â Â and you don't have a II, IIfx, Q900, Q950 or AV series.
> +
> Âconfig AMIGA_Z2RAM
> Â Â Â Âtristate "Amiga Zorro II ramdisk support"
> Â Â Â Âdepends on ZORRO
> diff --git a/drivers/block/Makefile b/drivers/block/Makefile
> index 204332b..b32b7f9 100644
> --- a/drivers/block/Makefile
> +++ b/drivers/block/Makefile
> @@ -6,6 +6,7 @@
> Â#
>
> Âobj-$(CONFIG_MAC_FLOPPY) Â Â Â += swim3.o
> +obj-$(CONFIG_BLK_DEV_SWIM) Â Â += swim_mod.o
> Âobj-$(CONFIG_BLK_DEV_FD) Â Â Â += floppy.o
> Âobj-$(CONFIG_AMIGA_FLOPPY) Â Â += amiflop.o
> Âobj-$(CONFIG_PS3_DISK) Â Â Â Â += ps3disk.o
> @@ -32,3 +33,5 @@ obj-$(CONFIG_BLK_DEV_UB) Â Â Â+= ub.o
> Âobj-$(CONFIG_BLK_DEV_HD) Â Â Â += hd.o
>
> Âobj-$(CONFIG_XEN_BLKDEV_FRONTEND) Â Â Â+= xen-blkfront.o
> +
> +swim_mod-objs Â:= swim.o swim_asm.o
> diff --git a/drivers/block/swim.c b/drivers/block/swim.c
> new file mode 100644
> index 0000000..d22cc38
> --- /dev/null
> +++ b/drivers/block/swim.c
> @@ -0,0 +1,995 @@
> +/*
> + * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
> + *
> + * Copyright (C) 2004,2008 Laurent Vivier <Laurent@xxxxxxxxxxxx>
> + *
> + * based on Alastair Bridgewater SWIM analysis, 2001
> + * based on SWIM3 driver (c) Paul Mackerras, 1996
> + * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License
> + * as published by the Free Software Foundation; either version
> + * 2 of the License, or (at your option) any later version.
> + *
> + * 2004-08-21 (lv) - Initial implementation
> + * 2008-10-30 (lv) - Port to 2.6
> + */
> +
> +#include <linux/module.h>
> +#include <linux/fd.h>
> +#include <linux/blkdev.h>
> +#include <linux/hdreg.h>
> +#include <linux/kernel.h>
> +#include <linux/delay.h>
> +#include <linux/platform_device.h>
> +
> +#include <asm/macintosh.h>
> +#include <asm/mac_via.h>
> +
> +#define CARDNAME "swim"
> +
> +struct sector_header {
> + Â Â Â unsigned char side;
> + Â Â Â unsigned char track;
> + Â Â Â unsigned char sector;
> + Â Â Â unsigned char size;
> + Â Â Â unsigned char crc0;
> + Â Â Â unsigned char crc1;
> +} __attribute__((packed));
> +
> +#define DRIVER_VERSION "Version 0.2 (2008-10-30)"
> +
> +#define REG(x) unsigned char x, x ## _pad[0x200 - 1];
> +
> +struct swim {
> + Â Â Â REG(write_data)
> + Â Â Â REG(write_mark)
> + Â Â Â REG(write_CRC)
> + Â Â Â REG(write_parameter)
> + Â Â Â REG(write_phase)
> + Â Â Â REG(write_setup)
> + Â Â Â REG(write_mode0)
> + Â Â Â REG(write_mode1)
> +
> + Â Â Â REG(read_data)
> + Â Â Â REG(read_mark)
> + Â Â Â REG(read_error)
> + Â Â Â REG(read_parameter)
> + Â Â Â REG(read_phase)
> + Â Â Â REG(read_setup)
> + Â Â Â REG(read_status)
> + Â Â Â REG(read_handshake)
> +} __attribute__((packed));
> +
> +#define swim_write(base, reg, v) Â Â Â out_8(&(base)->write_##reg, (v))
> +#define swim_read(base, reg) Â Â Â Â Â in_8(&(base)->read_##reg)
> +
> +/* IWM registers */
> +
> +struct iwm {
> + Â Â Â REG(ph0L)
> + Â Â Â REG(ph0H)
> + Â Â Â REG(ph1L)
> + Â Â Â REG(ph1H)
> + Â Â Â REG(ph2L)
> + Â Â Â REG(ph2H)
> + Â Â Â REG(ph3L)
> + Â Â Â REG(ph3H)
> + Â Â Â REG(mtrOff)
> + Â Â Â REG(mtrOn)
> + Â Â Â REG(intDrive)
> + Â Â Â REG(extDrive)
> + Â Â Â REG(q6L)
> + Â Â Â REG(q6H)
> + Â Â Â REG(q7L)
> + Â Â Â REG(q7H)
> +} __attribute__((packed));
> +
> +#define iwm_write(base, reg, v) Â Â Â Âout_8(&(base)->reg, (v))
> +#define iwm_read(base, reg) Â Â Â Â Â Âin_8(&(base)->reg)
> +
> +/* bits in phase register */
> +
> +#define SEEK_POSITIVE Â0x070
> +#define SEEK_NEGATIVE Â0x074
> +#define STEP Â Â Â Â Â 0x071
> +#define MOTOR_ON Â Â Â 0x072
> +#define MOTOR_OFF Â Â Â0x076
> +#define INDEX Â Â Â Â Â0x073
> +#define EJECT Â Â Â Â Â0x077
> +#define SETMFM Â Â Â Â 0x171
> +#define SETGCR Â Â Â Â 0x175
> +
> +#define RELAX Â Â Â Â Â0x033
> +#define LSTRB Â Â Â Â Â0x008
> +
> +#define CA_MASK Â Â Â Â Â Â Â Â0x077
> +
> +/* Select values for swim_select and swim_readbit */
> +
> +#define READ_DATA_0 Â Â0x074
> +#define TWOMEG_DRIVE Â 0x075
> +#define SINGLE_SIDED Â 0x076
> +#define DRIVE_PRESENT Â0x077
> +#define DISK_IN Â Â Â Â Â Â Â Â0x170
> +#define WRITE_PROT Â Â 0x171
> +#define TRACK_ZERO Â Â 0x172
> +#define TACHO Â Â Â Â Â0x173
> +#define READ_DATA_1 Â Â0x174
> +#define MFM_MODE Â Â Â 0x175
> +#define SEEK_COMPLETE Â0x176
> +#define ONEMEG_MEDIA Â 0x177
> +
> +/* Bits in handshake register */
> +
> +#define MARK_BYTE Â Â Â0x01
> +#define CRC_ZERO Â Â Â 0x02
> +#define RDDATA Â Â Â Â 0x04
> +#define SENSE Â Â Â Â Â0x08
> +#define MOTEN Â Â Â Â Â0x10
> +#define ERROR Â Â Â Â Â0x20
> +#define DAT2BYTE Â Â Â 0x40
> +#define DAT1BYTE Â Â Â 0x80
> +
> +/* bits in setup register */
> +
> +#define S_INV_WDATA Â Â0x01
> +#define S_3_5_SELECT Â 0x02
> +#define S_GCR Â Â Â Â Â0x04
> +#define S_FCLK_DIV2 Â Â0x08
> +#define S_ERROR_CORR Â 0x10
> +#define S_IBM_DRIVE Â Â0x20
> +#define S_GCR_WRITE Â Â0x40
> +#define S_TIMEOUT Â Â Â0x80
> +
> +/* bits in mode register */
> +
> +#define CLFIFO Â Â Â Â 0x01
> +#define ENBL1 Â Â Â Â Â0x02
> +#define ENBL2 Â Â Â Â Â0x04
> +#define ACTION Â Â Â Â 0x08
> +#define WRITE_MODE Â Â 0x10
> +#define HEDSEL Â Â Â Â 0x20
> +#define MOTON Â Â Â Â Â0x80
> +
> +/*----------------------------------------------------------------------------*/
> +
> +enum drive_location {
> + Â Â Â INTERNAL_DRIVE = 0x02,
> + Â Â Â EXTERNAL_DRIVE = 0x04,
> +};
> +
> +enum media_type {
> + Â Â Â DD_MEDIA,
> + Â Â Â HD_MEDIA,
> +};
> +
> +struct floppy_state {
> +
> + Â Â Â /* physical properties */
> +
> + Â Â Â enum drive_location location; Â /* internal or external drive */
> + Â Â Â int Â Â Â Â Â Â Âhead_number; Â /* single- or double-sided drive */
> +
> + Â Â Â /* media */
> +
> + Â Â Â int Â Â Â Â Â Â Âdisk_in;
> + Â Â Â int Â Â Â Â Â Â Âejected;
> + Â Â Â enum media_type Âtype;
> + Â Â Â int Â Â Â Â Â Â Âwrite_protected;
> +
> + Â Â Â int Â Â Â Â Â Â Âtotal_secs;
> + Â Â Â int Â Â Â Â Â Â Âsecpercyl;
> + Â Â Â int Â Â Â Â Â Â Âsecpertrack;
> +
> + Â Â Â /* in-use information */
> +
> + Â Â Â int Â Â Â Â Â Â track;
> + Â Â Â int Â Â Â Â Â Â ref_count;
> +
> + Â Â Â struct gendisk *disk;
> +
> + Â Â Â /* parent controller */
> +
> + Â Â Â struct swim_priv *swd;
> +};
> +
> +enum motor_action {
> + Â Â Â OFF,
> + Â Â Â ON,
> +};
> +
> +enum head {
> + Â Â Â LOWER_HEAD = 0,
> + Â Â Â UPPER_HEAD = 1,
> +};
> +
> +#define FD_MAX_UNIT Â Â2
> +
> +struct swim_priv {
> + Â Â Â struct swim __iomem *base;
> + Â Â Â spinlock_t lock;
> + Â Â Â struct request_queue *queue;
> + Â Â Â int floppy_count;
> + Â Â Â struct floppy_state unit[FD_MAX_UNIT];
> +};
> +
> +extern int swim_read_sector_header(struct swim __iomem *base,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âstruct sector_header *header);
> +extern int swim_read_sector_data(struct swim __iomem *base,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âunsigned char *data);
> +
> +static inline void set_swim_mode(struct swim __iomem *base, int enable)
> +{
> + Â Â Â struct iwm __iomem *iwm_base;
> + Â Â Â unsigned long flags;
> +
> + Â Â Â if (!enable) {
> + Â Â Â Â Â Â Â swim_write(base, mode0, 0xf8);
> + Â Â Â Â Â Â Â return;
> + Â Â Â }
> +
> + Â Â Â iwm_base = (struct iwm __iomem *)base;
> + Â Â Â local_irq_save(flags);
> +
> + Â Â Â iwm_read(iwm_base, q7L);
> + Â Â Â iwm_read(iwm_base, mtrOff);
> + Â Â Â iwm_read(iwm_base, q6H);
> +
> + Â Â Â iwm_write(iwm_base, q7H, 0x57);
> + Â Â Â iwm_write(iwm_base, q7H, 0x17);
> + Â Â Â iwm_write(iwm_base, q7H, 0x57);
> + Â Â Â iwm_write(iwm_base, q7H, 0x57);
> +
> + Â Â Â local_irq_restore(flags);
> +}
> +
> +static inline int get_swim_mode(struct swim __iomem *base)
> +{
> + Â Â Â unsigned long flags;
> +
> + Â Â Â local_irq_save(flags);
> +
> + Â Â Â swim_write(base, phase, 0xf5);
> + Â Â Â if (swim_read(base, phase) != 0xf5)
> + Â Â Â Â Â Â Â goto is_iwm;
> + Â Â Â swim_write(base, phase, 0xf6);
> + Â Â Â if (swim_read(base, phase) != 0xf6)
> + Â Â Â Â Â Â Â goto is_iwm;
> + Â Â Â swim_write(base, phase, 0xf7);
> + Â Â Â if (swim_read(base, phase) != 0xf7)
> + Â Â Â Â Â Â Â goto is_iwm;
> + Â Â Â local_irq_restore(flags);
> + Â Â Â return 1;
> +is_iwm:
> + Â Â Â local_irq_restore(flags);
> + Â Â Â return 0;
> +}
> +
> +static inline void swim_select(struct swim __iomem *base, int sel)
> +{
> + Â Â Â swim_write(base, phase, RELAX);
> +
> + Â Â Â via1_set_head(sel & 0x100);
> +
> + Â Â Â swim_write(base, phase, sel & CA_MASK);
> +}
> +
> +static inline void swim_action(struct swim __iomem *base, int action)
> +{
> + Â Â Â unsigned long flags;
> +
> + Â Â Â local_irq_save(flags);
> +
> + Â Â Â swim_select(base, action);
> + Â Â Â udelay(1);
> + Â Â Â swim_write(base, phase, (LSTRB<<4) | LSTRB);
> + Â Â Â udelay(1);
> + Â Â Â swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F));
> + Â Â Â udelay(1);
> +
> + Â Â Â local_irq_restore(flags);
> +}
> +
> +static inline int swim_readbit(struct swim __iomem *base, int bit)
> +{
> + Â Â Â int stat;
> +
> + Â Â Â swim_select(base, bit);
> +
> + Â Â Â udelay(10);
> +
> + Â Â Â stat = swim_read(base, handshake);
> +
> + Â Â Â return (stat & SENSE) == 0;
> +}
> +
> +static inline void swim_drive(struct swim __iomem *base,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â enum drive_location location)
> +{
> + Â Â Â if (location == INTERNAL_DRIVE) {
> + Â Â Â Â Â Â Â swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
> + Â Â Â Â Â Â Â swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
> + Â Â Â } else if (location == EXTERNAL_DRIVE) {
> + Â Â Â Â Â Â Â swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
> + Â Â Â Â Â Â Â swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
> + Â Â Â }
> +}
> +
> +static inline void swim_motor(struct swim __iomem *base,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â enum motor_action action)
> +{
> + Â Â Â if (action == ON) {
> + Â Â Â Â Â Â Â int i;
> +
> + Â Â Â Â Â Â Â swim_action(base, MOTOR_ON);
> +
> + Â Â Â Â Â Â Â for (i = 0; i < 2*HZ; i++) {
> + Â Â Â Â Â Â Â Â Â Â Â swim_select(base, RELAX);
> + Â Â Â Â Â Â Â Â Â Â Â if (swim_readbit(base, MOTOR_ON))
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â break;
> + Â Â Â Â Â Â Â Â Â Â Â current->state = TASK_INTERRUPTIBLE;
> + Â Â Â Â Â Â Â Â Â Â Â schedule_timeout(1);
> + Â Â Â Â Â Â Â }
> + Â Â Â } else if (action == OFF) {
> + Â Â Â Â Â Â Â swim_action(base, MOTOR_OFF);
> + Â Â Â Â Â Â Â swim_select(base, RELAX);
> + Â Â Â }
> +}
> +
> +static inline void swim_eject(struct swim __iomem *base)
> +{
> + Â Â Â int i;
> +
> + Â Â Â swim_action(base, EJECT);
> +
> + Â Â Â for (i = 0; i < 2*HZ; i++) {
> + Â Â Â Â Â Â Â swim_select(base, RELAX);
> + Â Â Â Â Â Â Â if (!swim_readbit(base, DISK_IN))
> + Â Â Â Â Â Â Â Â Â Â Â break;
> + Â Â Â Â Â Â Â current->state = TASK_INTERRUPTIBLE;
> + Â Â Â Â Â Â Â schedule_timeout(1);
> + Â Â Â }
> + Â Â Â swim_select(base, RELAX);
> +}
> +
> +static inline void swim_head(struct swim __iomem *base, enum head head)
> +{
> + Â Â Â /* wait drive is ready */
> +
> + Â Â Â if (head == UPPER_HEAD)
> + Â Â Â Â Â Â Â swim_select(base, READ_DATA_1);
> + Â Â Â else if (head == LOWER_HEAD)
> + Â Â Â Â Â Â Â swim_select(base, READ_DATA_0);
> +}
> +
> +static inline int swim_step(struct swim __iomem *base)
> +{
> + Â Â Â int wait;
> +
> + Â Â Â swim_action(base, STEP);
> +
> + Â Â Â for (wait = 0; wait < HZ; wait++) {
> +
> + Â Â Â Â Â Â Â current->state = TASK_INTERRUPTIBLE;
> + Â Â Â Â Â Â Â schedule_timeout(1);
> +
> + Â Â Â Â Â Â Â swim_select(base, RELAX);
> + Â Â Â Â Â Â Â if (!swim_readbit(base, STEP))
> + Â Â Â Â Â Â Â Â Â Â Â return 0;
> + Â Â Â }
> + Â Â Â return -1;
> +}
> +
> +static inline int swim_track00(struct swim __iomem *base)
> +{
> + Â Â Â int try;
> +
> + Â Â Â swim_action(base, SEEK_NEGATIVE);
> +
> + Â Â Â for (try = 0; try < 100; try++) {
> +
> + Â Â Â Â Â Â Â swim_select(base, RELAX);
> + Â Â Â Â Â Â Â if (swim_readbit(base, TRACK_ZERO))
> + Â Â Â Â Â Â Â Â Â Â Â break;
> +
> + Â Â Â Â Â Â Â if (swim_step(base))
> + Â Â Â Â Â Â Â Â Â Â Â return -1;
> + Â Â Â }
> +
> + Â Â Â if (swim_readbit(base, TRACK_ZERO))
> + Â Â Â Â Â Â Â return 0;
> +
> + Â Â Â return -1;
> +}
> +
> +static inline int swim_seek(struct swim __iomem *base, int step)
> +{
> + Â Â Â if (step == 0)
> + Â Â Â Â Â Â Â return 0;
> +
> + Â Â Â if (step < 0) {
> + Â Â Â Â Â Â Â swim_action(base, SEEK_NEGATIVE);
> + Â Â Â Â Â Â Â step = -step;
> + Â Â Â } else
> + Â Â Â Â Â Â Â swim_action(base, SEEK_POSITIVE);
> +
> + Â Â Â for ( ; step > 0; step--) {
> + Â Â Â Â Â Â Â if (swim_step(base))
> + Â Â Â Â Â Â Â Â Â Â Â return -1;
> + Â Â Â }
> +
> + Â Â Â return 0;
> +}
> +
> +static inline int swim_track(struct floppy_state *fs, Âint track)
> +{
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> + Â Â Â int ret;
> +
> + Â Â Â ret = swim_seek(base, track - fs->track);
> +
> + Â Â Â if (ret == 0)
> + Â Â Â Â Â Â Â fs->track = track;
> + Â Â Â else {
> + Â Â Â Â Â Â Â swim_track00(base);
> + Â Â Â Â Â Â Â fs->track = 0;
> + Â Â Â }
> +
> + Â Â Â return ret;
> +}
> +
> +static int floppy_eject(struct floppy_state *fs)
> +{
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> +
> + Â Â Â swim_drive(base, fs->location);
> + Â Â Â swim_motor(base, OFF);
> + Â Â Â swim_eject(base);
> +
> + Â Â Â fs->disk_in = 0;
> + Â Â Â fs->ejected = 1;
> +
> + Â Â Â return 0;
> +}
> +
> +static inline int swim_read_sector(struct floppy_state *fs,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âint side, int track,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âint sector, unsigned char *buffer)
> +{
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> + Â Â Â unsigned long flags;
> + Â Â Â struct sector_header header;
> + Â Â Â int ret = -1;
> + Â Â Â short i;
> +
> + Â Â Â swim_track(fs, track);
> +
> + Â Â Â swim_write(base, mode1, MOTON);
> + Â Â Â swim_head(base, side);
> + Â Â Â swim_write(base, mode0, side);
> +
> + Â Â Â local_irq_save(flags);
> + Â Â Â for (i = 0; i < 36; i++) {
> + Â Â Â Â Â Â Â ret = swim_read_sector_header(base, &header);
> + Â Â Â Â Â Â Â if (!ret && (header.sector == sector)) {
> + Â Â Â Â Â Â Â Â Â Â Â /* found */
> +
> + Â Â Â Â Â Â Â Â Â Â Â ret = swim_read_sector_data(base, buffer);
> + Â Â Â Â Â Â Â Â Â Â Â break;
> + Â Â Â Â Â Â Â }
> + Â Â Â }
> + Â Â Â local_irq_restore(flags);
> +
> + Â Â Â swim_write(base, mode0, MOTON);
> +
> + Â Â Â if ((header.side != side) Â|| (header.track != track) ||
> + Â Â Â Â Â Â(header.sector != sector))
> + Â Â Â Â Â Â Â return 0;
> +
> + Â Â Â return ret;
> +}
> +
> +static int floppy_read_sectors(struct floppy_state *fs,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âint req_sector, int sectors_nb,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âunsigned char *buffer)
> +{
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> + Â Â Â int ret;
> + Â Â Â int side, track, sector;
> + Â Â Â int i, try;
> +
> +
> + Â Â Â swim_drive(base, fs->location);
> + Â Â Â for (i = req_sector; i < req_sector + sectors_nb; i++) {
> + Â Â Â Â Â Â Â int x;
> + Â Â Â Â Â Â Â track = i / fs->secpercyl;
> + Â Â Â Â Â Â Â x = i % fs->secpercyl;
> + Â Â Â Â Â Â Â side = x / fs->secpertrack;
> + Â Â Â Â Â Â Â sector = x % fs->secpertrack + 1;
> +
> + Â Â Â Â Â Â Â try = 5;
> + Â Â Â Â Â Â Â do {
> + Â Â Â Â Â Â Â Â Â Â Â ret = swim_read_sector(fs, side, track, sector,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â buffer);
> + Â Â Â Â Â Â Â Â Â Â Â if (try-- == 0)
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â return -1;
> + Â Â Â Â Â Â Â } while (ret != 512);
> +
> + Â Â Â Â Â Â Â buffer += ret;
> + Â Â Â }
> +
> + Â Â Â return 0;
> +}
> +
> +static void redo_fd_request(struct request_queue *q)
> +{
> + Â Â Â struct request *req;
> + Â Â Â struct floppy_state *fs;
> +
> + Â Â Â while ((req = elv_next_request(q))) {
> +
> + Â Â Â Â Â Â Â fs = req->rq_disk->private_data;
> + Â Â Â Â Â Â Â if (req->sector < 0 || req->sector >= fs->total_secs) {
> + Â Â Â Â Â Â Â Â Â Â Â end_request(req, 0);
> + Â Â Â Â Â Â Â Â Â Â Â continue;
> + Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â if (req->current_nr_sectors == 0) {
> + Â Â Â Â Â Â Â Â Â Â Â end_request(req, 1);
> + Â Â Â Â Â Â Â Â Â Â Â continue;
> + Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â if (!fs->disk_in) {
> + Â Â Â Â Â Â Â Â Â Â Â end_request(req, 0);
> + Â Â Â Â Â Â Â Â Â Â Â continue;
> + Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â if (rq_data_dir(req) == WRITE) {
> + Â Â Â Â Â Â Â Â Â Â Â if (fs->write_protected) {
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â end_request(req, 0);
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â continue;
> + Â Â Â Â Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â switch (rq_data_dir(req)) {
> + Â Â Â Â Â Â Â case WRITE:
> + Â Â Â Â Â Â Â Â Â Â Â /* NOT IMPLEMENTED */
> + Â Â Â Â Â Â Â Â Â Â Â end_request(req, 0);
> + Â Â Â Â Â Â Â Â Â Â Â break;
> + Â Â Â Â Â Â Â case READ:
> + Â Â Â Â Â Â Â Â Â Â Â if (floppy_read_sectors(fs, req->sector,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â req->current_nr_sectors,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â req->buffer)) {
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â end_request(req, 0);
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â continue;
> + Â Â Â Â Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â Â Â Â Â req->nr_sectors -= req->current_nr_sectors;
> + Â Â Â Â Â Â Â Â Â Â Â req->sector += req->current_nr_sectors;
> + Â Â Â Â Â Â Â Â Â Â Â req->buffer += req->current_nr_sectors * 512;
> + Â Â Â Â Â Â Â Â Â Â Â end_request(req, 1);
> + Â Â Â Â Â Â Â Â Â Â Â break;
> + Â Â Â Â Â Â Â }
> + Â Â Â }
> +}
> +
> +static void do_fd_request(struct request_queue *q)
> +{
> + Â Â Â redo_fd_request(q);
> +}
> +
> +static struct floppy_struct floppy_type[4] = {
> + Â Â Â { Â Â0, Â0, 0, Â0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing Â */
> + Â Â Â { Â720, Â9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
> + Â Â Â { 1440, Â9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5" Â */
> + Â Â Â { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5" Â*/
> +};
> +
> +static int get_floppy_geometry(struct floppy_state *fs, int type,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âstruct floppy_struct **g)
> +{
> + Â Â Â if (type >= ARRAY_SIZE(floppy_type))
> + Â Â Â Â Â Â Â return -EINVAL;
> +
> + Â Â Â if (type)
> + Â Â Â Â Â Â Â *g = &floppy_type[type];
> + Â Â Â else if (fs->type == HD_MEDIA) /* High-Density media */
> + Â Â Â Â Â Â Â *g = &floppy_type[3];
> + Â Â Â else if (fs->head_number == 2) /* double-sided */
> + Â Â Â Â Â Â Â *g = &floppy_type[2];
> + Â Â Â else
> + Â Â Â Â Â Â Â *g = &floppy_type[1];
> +
> + Â Â Â return 0;
> +}
> +
> +static void setup_medium(struct floppy_state *fs)
> +{
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> +
> + Â Â Â if (swim_readbit(base, DISK_IN)) {
> + Â Â Â Â Â Â Â struct floppy_struct *g;
> + Â Â Â Â Â Â Â fs->disk_in = 1;
> + Â Â Â Â Â Â Â fs->write_protected = swim_readbit(base, WRITE_PROT);
> + Â Â Â Â Â Â Â fs->type = swim_readbit(base, ONEMEG_MEDIA);
> +
> + Â Â Â Â Â Â Â if (swim_track00(base))
> + Â Â Â Â Â Â Â Â Â Â Â printk(KERN_ERR
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â "SWIM: cannot move floppy head to track 0\n");
> +
> + Â Â Â Â Â Â Â swim_track00(base);
> +
> + Â Â Â Â Â Â Â get_floppy_geometry(fs, 0, &g);
> + Â Â Â Â Â Â Â fs->total_secs = g->size;
> + Â Â Â Â Â Â Â fs->secpercyl = g->head * g->sect;
> + Â Â Â Â Â Â Â fs->secpertrack = g->sect;
> + Â Â Â Â Â Â Â fs->track = 0;
> + Â Â Â } else {
> + Â Â Â Â Â Â Â fs->disk_in = 0;
> + Â Â Â }
> +}
> +
> +static int floppy_open(struct block_device *bdev, fmode_t mode)
> +{
> + Â Â Â struct floppy_state *fs = bdev->bd_disk->private_data;
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> + Â Â Â int err;
> +
> + Â Â Â if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL))
> + Â Â Â Â Â Â Â return -EBUSY;
> +
> + Â Â Â if (mode & FMODE_EXCL)
> + Â Â Â Â Â Â Â fs->ref_count = -1;
> + Â Â Â else
> + Â Â Â Â Â Â Â fs->ref_count++;
> +
> + Â Â Â swim_write(base, setup, S_IBM_DRIVE Â| S_FCLK_DIV2);
> + Â Â Â udelay(10);
> + Â Â Â swim_drive(base, INTERNAL_DRIVE);
> + Â Â Â swim_motor(base, ON);
> + Â Â Â swim_action(base, SETMFM);
> + Â Â Â if (fs->ejected)
> + Â Â Â Â Â Â Â setup_medium(fs);
> + Â Â Â if (!fs->disk_in) {
> + Â Â Â Â Â Â Â err = -ENXIO;
> + Â Â Â Â Â Â Â goto out;
> + Â Â Â }
> +
> + Â Â Â if (mode & FMODE_NDELAY)
> + Â Â Â Â Â Â Â return 0;
> +
> + Â Â Â if (mode & (FMODE_READ|FMODE_WRITE)) {
> + Â Â Â Â Â Â Â check_disk_change(bdev);
> + Â Â Â Â Â Â Â if ((mode & FMODE_WRITE) && fs->write_protected) {
> + Â Â Â Â Â Â Â Â Â Â Â err = -EROFS;
> + Â Â Â Â Â Â Â Â Â Â Â goto out;
> + Â Â Â Â Â Â Â }
> + Â Â Â }
> + Â Â Â return 0;
> +out:
> + Â Â Â if (fs->ref_count < 0)
> + Â Â Â Â Â Â Â fs->ref_count = 0;
> + Â Â Â else if (fs->ref_count > 0)
> + Â Â Â Â Â Â Â --fs->ref_count;
> +
> + Â Â Â if (fs->ref_count == 0)
> + Â Â Â Â Â Â Â swim_motor(base, OFF);
> + Â Â Â return err;
> +}
> +
> +static int floppy_release(struct gendisk *disk, fmode_t mode)
> +{
> + Â Â Â struct floppy_state *fs = disk->private_data;
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> +
> + Â Â Â if (fs->ref_count < 0)
> + Â Â Â Â Â Â Â fs->ref_count = 0;
> + Â Â Â else if (fs->ref_count > 0)
> + Â Â Â Â Â Â Â --fs->ref_count;
> +
> + Â Â Â if (fs->ref_count == 0)
> + Â Â Â Â Â Â Â swim_motor(base, OFF);
> +
> + Â Â Â return 0;
> +}
> +
> +static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
> + Â Â Â Â Â Â Â Â Â Â Â unsigned int cmd, unsigned long param)
> +{
> + Â Â Â struct floppy_state *fs = bdev->bd_disk->private_data;
> + Â Â Â int err;
> +
> + Â Â Â if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
> + Â Â Â Â Â Â Â Â Â Â Â return -EPERM;
> +
> + Â Â Â switch (cmd) {
> + Â Â Â case FDEJECT:
> + Â Â Â Â Â Â Â if (fs->ref_count != 1)
> + Â Â Â Â Â Â Â Â Â Â Â return -EBUSY;
> + Â Â Â Â Â Â Â err = floppy_eject(fs);
> + Â Â Â Â Â Â Â return err;
> +
> + Â Â Â case FDGETPRM:
> + Â Â Â Â Â Â Â if (copy_to_user((void __user *) param, (void *) &floppy_type,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âsizeof(struct floppy_struct)))
> + Â Â Â Â Â Â Â Â Â Â Â return -EFAULT;
> + Â Â Â Â Â Â Â break;
> +
> + Â Â Â default:
> + Â Â Â Â Â Â Â printk(KERN_DEBUG "SWIM floppy_ioctl: unknown cmd %d\n",
> + Â Â Â Â Â Â Â Â Â Â Âcmd);
> + Â Â Â Â Â Â Â return -ENOSYS;
> + Â Â Â }
> + Â Â Â return 0;
> +}
> +
> +static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo)
> +{
> + Â Â Â struct floppy_state *fs = bdev->bd_disk->private_data;
> + Â Â Â struct floppy_struct *g;
> + Â Â Â int ret;
> +
> + Â Â Â ret = get_floppy_geometry(fs, 0, &g);
> + Â Â Â if (ret)
> + Â Â Â Â Â Â Â return ret;
> +
> + Â Â Â geo->heads = g->head;
> + Â Â Â geo->sectors = g->sect;
> + Â Â Â geo->cylinders = g->track;
> +
> + Â Â Â return 0;
> +}
> +
> +static int floppy_check_change(struct gendisk *disk)
> +{
> + Â Â Â struct floppy_state *fs = disk->private_data;
> +
> + Â Â Â return fs->ejected;
> +}
> +
> +static int floppy_revalidate(struct gendisk *disk)
> +{
> + Â Â Â struct floppy_state *fs = disk->private_data;
> + Â Â Â struct swim __iomem *base = fs->swd->base;
> +
> + Â Â Â swim_drive(base, fs->location);
> +
> + Â Â Â if (fs->ejected)
> + Â Â Â Â Â Â Â setup_medium(fs);
> +
> + Â Â Â if (!fs->disk_in)
> + Â Â Â Â Â Â Â swim_motor(base, OFF);
> + Â Â Â else
> + Â Â Â Â Â Â Â fs->ejected = 0;
> +
> + Â Â Â return !fs->disk_in;
> +}
> +
> +static struct block_device_operations floppy_fops = {
> + Â Â Â .owner Â Â Â Â Â = THIS_MODULE,
> + Â Â Â .open Â Â Â Â Â Â= floppy_open,
> + Â Â Â .release Â Â Â Â = floppy_release,
> + Â Â Â .locked_ioctl Â Â= floppy_ioctl,
> + Â Â Â .getgeo Â Â Â Â Â= floppy_getgeo,
> + Â Â Â .media_changed Â = floppy_check_change,
> + Â Â Â .revalidate_disk = floppy_revalidate,
> +};
> +
> +static struct kobject *floppy_find(dev_t dev, int *part, void *data)
> +{
> + Â Â Â struct swim_priv *swd = data;
> + Â Â Â int drive = (*part & 3);
> +
> + Â Â Â if (drive > swd->floppy_count)
> + Â Â Â Â Â Â Â return NULL;
> +
> + Â Â Â *part = 0;
> + Â Â Â return get_disk(swd->unit[drive].disk);
> +}
> +
> +static int __devinit swim_add_floppy(struct swim_priv *swd,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Â Âenum drive_location location)
> +{
> + Â Â Â struct floppy_state *fs = &swd->unit[swd->floppy_count];
> + Â Â Â struct swim __iomem *base = swd->base;
> +
> + Â Â Â fs->location = location;
> +
> + Â Â Â swim_drive(base, location);
> +
> + Â Â Â swim_motor(base, OFF);
> +
> + Â Â Â if (swim_readbit(base, SINGLE_SIDED))
> + Â Â Â Â Â Â Â fs->head_number = 1;
> + Â Â Â else
> + Â Â Â Â Â Â Â fs->head_number = 2;
> + Â Â Â fs->ref_count = 0;
> + Â Â Â fs->ejected = 1;
> +
> + Â Â Â swd->floppy_count++;
> +
> + Â Â Â return 0;
> +}
> +
> +static int __devinit swim_floppy_init(struct swim_priv *swd)
> +{
> + Â Â Â int err;
> + Â Â Â int drive;
> + Â Â Â struct swim __iomem *base = swd->base;
> +
> + Â Â Â /* scan floppy drives */
> +
> + Â Â Â swim_drive(base, INTERNAL_DRIVE);
> + Â Â Â if (swim_readbit(base, DRIVE_PRESENT))
> + Â Â Â Â Â Â Â swim_add_floppy(swd, INTERNAL_DRIVE);
> + Â Â Â swim_drive(base, EXTERNAL_DRIVE);
> + Â Â Â if (swim_readbit(base, DRIVE_PRESENT))
> + Â Â Â Â Â Â Â swim_add_floppy(swd, EXTERNAL_DRIVE);
> +
> + Â Â Â /* register floppy drives */
> +
> + Â Â Â err = register_blkdev(FLOPPY_MAJOR, "fd");
> + Â Â Â if (err) {
> + Â Â Â Â Â Â Â printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
> + Â Â Â Â Â Â Â Â Â Â ÂFLOPPY_MAJOR);
> + Â Â Â Â Â Â Â return -EBUSY;
> + Â Â Â }
> +
> + Â Â Â for (drive = 0; drive < swd->floppy_count; drive++) {
> + Â Â Â Â Â Â Â swd->unit[drive].disk = alloc_disk(1);
> + Â Â Â Â Â Â Â if (swd->unit[drive].disk == NULL) {
> + Â Â Â Â Â Â Â Â Â Â Â err = -ENOMEM;
> + Â Â Â Â Â Â Â Â Â Â Â goto exit_put_disks;
> + Â Â Â Â Â Â Â }
> + Â Â Â Â Â Â Â swd->unit[drive].swd = swd;
> + Â Â Â }
> +
> + Â Â Â swd->queue = blk_init_queue(do_fd_request, &swd->lock);
> + Â Â Â if (!swd->queue) {
> + Â Â Â Â Â Â Â err = -ENOMEM;
> + Â Â Â Â Â Â Â goto exit_put_disks;
> + Â Â Â }
> +
> + Â Â Â for (drive = 0; drive < swd->floppy_count; drive++) {
> + Â Â Â Â Â Â Â swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
> + Â Â Â Â Â Â Â swd->unit[drive].disk->major = FLOPPY_MAJOR;
> + Â Â Â Â Â Â Â swd->unit[drive].disk->first_minor = drive;
> + Â Â Â Â Â Â Â sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
> + Â Â Â Â Â Â Â swd->unit[drive].disk->fops = &floppy_fops;
> + Â Â Â Â Â Â Â swd->unit[drive].disk->private_data = &swd->unit[drive];
> + Â Â Â Â Â Â Â swd->unit[drive].disk->queue = swd->queue;
> + Â Â Â Â Â Â Â set_capacity(swd->unit[drive].disk, 2880);
> + Â Â Â Â Â Â Â add_disk(swd->unit[drive].disk);
> + Â Â Â }
> +
> + Â Â Â blk_register_region(MKDEV(FLOPPY_MAJOR, 0), 256, THIS_MODULE,
> + Â Â Â Â Â Â Â Â Â Â Â Â Â floppy_find, NULL, swd);
> +
> + Â Â Â return 0;
> +
> +exit_put_disks:
> + Â Â Â unregister_blkdev(FLOPPY_MAJOR, "fd");
> + Â Â Â while (drive--)
> + Â Â Â Â Â Â Â put_disk(swd->unit[drive].disk);
> + Â Â Â return err;
> +}
> +
> +static int __devinit swim_probe(struct platform_device *dev)
> +{
> + Â Â Â struct resource *res;
> + Â Â Â struct swim __iomem *swim_base;
> + Â Â Â struct swim_priv *swd;
> + Â Â Â int ret;
> +
> + Â Â Â res = platform_get_resource_byname(dev, IORESOURCE_MEM, "swim-regs");
> + Â Â Â if (!res) {
> + Â Â Â Â Â Â Â ret = -ENODEV;
> + Â Â Â Â Â Â Â goto out;
> + Â Â Â }
> +
> + Â Â Â if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
> + Â Â Â Â Â Â Â ret = -EBUSY;
> + Â Â Â Â Â Â Â goto out;
> + Â Â Â }
> +
> + Â Â Â swim_base = ioremap(res->start, resource_size(res));
> + Â Â Â if (!swim_base) {
> + Â Â Â Â Â Â Â return -ENOMEM;
> + Â Â Â Â Â Â Â goto out_release_io;
> + Â Â Â }
> +
> + Â Â Â /* probe device */
> +
> + Â Â Â set_swim_mode(swim_base, 1);
> + Â Â Â if (!get_swim_mode(swim_base)) {
> + Â Â Â Â Â Â Â printk(KERN_INFO "SWIM device not found !\n");
> + Â Â Â Â Â Â Â ret = -ENODEV;
> + Â Â Â Â Â Â Â goto out_iounmap;
> + Â Â Â }
> +
> + Â Â Â /* set platform driver data */
> +
> + Â Â Â swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL);
> + Â Â Â if (!swd) {
> + Â Â Â Â Â Â Â ret = -ENOMEM;
> + Â Â Â Â Â Â Â goto out_iounmap;
> + Â Â Â }
> + Â Â Â platform_set_drvdata(dev, swd);
> +
> + Â Â Â swd->base = swim_base;
> +
> + Â Â Â ret = swim_floppy_init(swd);
> + Â Â Â if (ret)
> + Â Â Â Â Â Â Â goto out_kfree;
> +
> + Â Â Â return 0;
> +
> +out_kfree:
> + Â Â Â platform_set_drvdata(dev, NULL);
> + Â Â Â kfree(swd);
> +out_iounmap:
> + Â Â Â iounmap(swim_base);
> +out_release_io:
> + Â Â Â release_mem_region(res->start, resource_size(res));
> +out:
> + Â Â Â return ret;
> +}
> +
> +static int __devexit swim_remove(struct platform_device *dev)
> +{
> + Â Â Â struct swim_priv *swd = platform_get_drvdata(dev);
> + Â Â Â int drive;
> + Â Â Â struct resource *res;
> +
> + Â Â Â blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
> +
> + Â Â Â for (drive = 0; drive < swd->floppy_count; drive++) {
> + Â Â Â Â Â Â Â del_gendisk(swd->unit[drive].disk);
> + Â Â Â Â Â Â Â put_disk(swd->unit[drive].disk);
> + Â Â Â }
> +
> + Â Â Â unregister_blkdev(FLOPPY_MAJOR, "fd");
> +
> + Â Â Â blk_cleanup_queue(swd->queue);
> +
> + Â Â Â /* eject floppies */
> +
> + Â Â Â for (drive = 0; drive < swd->floppy_count; drive++)
> + Â Â Â Â Â Â Â floppy_eject(&swd->unit[drive]);
> +
> + Â Â Â iounmap(swd->base);
> +
> + Â Â Â res = platform_get_resource_byname(dev, IORESOURCE_MEM, "swim-regs");
> + Â Â Â if (res)
> + Â Â Â Â Â Â Â release_mem_region(res->start, resource_size(res));
> +
> + Â Â Â platform_set_drvdata(dev, NULL);
> + Â Â Â kfree(swd);
> +
> + Â Â Â return 0;
> +}
> +
> +static struct platform_driver swim_driver = {
> + Â Â Â .probe Â= swim_probe,
> + Â Â Â .remove = __devexit_p(swim_remove),
> + Â Â Â .driver Â = {
> + Â Â Â Â Â Â Â .name Â = CARDNAME,
> + Â Â Â Â Â Â Â .owner Â= THIS_MODULE,
> + Â Â Â },
> +};
> +
> +static int __init swim_init(void)
> +{
> + Â Â Â printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
> +
> + Â Â Â return platform_driver_register(&swim_driver);
> +}
> +module_init(swim_init);
> +
> +static void __exit swim_exit(void)
> +{
> + Â Â Â platform_driver_unregister(&swim_driver);
> +}
> +module_exit(swim_exit);
> +
> +MODULE_DESCRIPTION("Driver for SWIM floppy controller");
> +MODULE_LICENSE("GPL");
> +MODULE_AUTHOR("Laurent Vivier <laurent@xxxxxxxxxxxx>");
> +MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);
> diff --git a/drivers/block/swim_asm.S b/drivers/block/swim_asm.S
> new file mode 100644
> index 0000000..c966820
> --- /dev/null
> +++ b/drivers/block/swim_asm.S
> @@ -0,0 +1,247 @@
> +/*
> + * low-level functions for the SWIM floppy controller
> + *
> + * needs assembly language because is very timing dependent
> + * this controller exists only on macintosh 680x0 based
> + *
> + * Copyright (C) 2004,2008 Laurent Vivier <Laurent@xxxxxxxxxxxx>
> + *
> + * based on Alastair Bridgewater SWIM analysis, 2001
> + * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License
> + * as published by the Free Software Foundation; either version
> + * 2 of the License, or (at your option) any later version.
> + *
> + * 2004-08-21 (lv) - Initial implementation
> + * 2008-11-05 (lv) - add get_swim_mode
> + */
> +
> + Â Â Â .equ Â Âwrite_data, Â Â 0x0000
> + Â Â Â .equ Â Âwrite_mark, Â Â 0x0200
> + Â Â Â .equ Â Âwrite_CRC, Â Â Â0x0400
> + Â Â Â .equ Â Âwrite_parameter,0x0600
> + Â Â Â .equ Â Âwrite_phase, Â Â0x0800
> + Â Â Â .equ Â Âwrite_setup, Â Â0x0a00
> + Â Â Â .equ Â Âwrite_mode0, Â Â0x0c00
> + Â Â Â .equ Â Âwrite_mode1, Â Â0x0e00
> + Â Â Â .equ Â Âread_data, Â Â Â0x1000
> + Â Â Â .equ Â Âread_mark, Â Â Â0x1200
> + Â Â Â .equ Â Âread_error, Â Â 0x1400
> + Â Â Â .equ Â Âread_parameter, 0x1600
> + Â Â Â .equ Â Âread_phase, Â Â 0x1800
> + Â Â Â .equ Â Âread_setup, Â Â 0x1a00
> + Â Â Â .equ Â Âread_status, Â Â0x1c00
> + Â Â Â .equ Â Âread_handshake, 0x1e00
> +
> + Â Â Â .equ Â Âo_side, 0
> + Â Â Â .equ Â Âo_track, 1
> + Â Â Â .equ Â Âo_sector, 2
> + Â Â Â .equ Â Âo_size, 3
> + Â Â Â .equ Â Âo_crc0, 4
> + Â Â Â .equ Â Âo_crc1, 5
> +
> + Â Â Â .equ Â Âseek_time, 30000
> + Â Â Â .equ Â Âmax_retry, 40
> + Â Â Â .equ Â Âsector_size, 512
> +
> + Â Â Â .global swim_read_sector_header
> +swim_read_sector_header:
> + Â Â Â link Â Â%a6, #0
> + Â Â Â moveml Â%d1-%d5/%a0-%a4,%sp@-
> + Â Â Â movel Â %a6@(0x0c), %a4
> + Â Â Â bsr Â Â mfm_read_addrmark
> + Â Â Â moveml Â%sp@+, %d1-%d5/%a0-%a4
> + Â Â Â unlk Â Â%a6
> + Â Â Â rts
> +
> +sector_address_mark:
> + Â Â Â .byte Â 0xa1, 0xa1, 0xa1, 0xfe
> +sector_data_mark:
> + Â Â Â .byte Â 0xa1, 0xa1, 0xa1, 0xfb
> +
> +mfm_read_addrmark:
> + Â Â Â movel Â %a6@(0x08), %a3
> + Â Â Â lea Â Â %a3@(read_handshake), %a2
> + Â Â Â lea Â Â %a3@(read_mark), %a3
> + Â Â Â moveq Â #-1, %d0
> + Â Â Â movew Â #seek_time, %d2
> +
> +wait_header_init:
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> + Â Â Â moveb Â #0x18, %a3@(write_mode0 - read_mark)
> + Â Â Â moveb Â #0x01, %a3@(write_mode1 - read_mark)
> + Â Â Â moveb Â #0x01, %a3@(write_mode0 - read_mark)
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> + Â Â Â moveb Â #0x08, %a3@(write_mode1 - read_mark)
> +
> + Â Â Â lea Â Â sector_address_mark, %a0
> + Â Â Â moveq Â #3, %d1
> +
> +wait_addr_mark_byte:
> +
> + Â Â Â tstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, wait_addr_mark_byte
> + Â Â Â bpl Â Â header_exit
> +
> + Â Â Â moveb Â %a3@, %d3
> + Â Â Â cmpb Â Â%a0@+, %d3
> + Â Â Â dbne Â Â%d1, wait_addr_mark_byte
> + Â Â Â bne Â Â wait_header_init
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +amark0: Â Â Â Âtstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, amark0
> + Â Â Â bpl Â Â signal_nonyb
> +
> + Â Â Â moveb Â %a3@, %a4@(o_track)
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +amark1: Â Â Â Âtstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, amark1
> + Â Â Â bpl Â Â signal_nonyb
> +
> + Â Â Â moveb Â %a3@, %a4@(o_side)
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +amark2: Â Â Â Âtstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, amark2
> + Â Â Â bpl Â Â signal_nonyb
> +
> + Â Â Â moveb Â %a3@, %a4@(o_sector)
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +amark3: Â Â Â Âtstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, amark3
> + Â Â Â bpl Â Â signal_nonyb
> +
> + Â Â Â moveb Â %a3@, %a4@(o_size)
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +crc0: Âtstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, crc0
> + Â Â Â bpl Â Â signal_nonyb
> +
> + Â Â Â moveb Â %a3@, %a4@(o_crc0)
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +crc1: Âtstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, crc1
> + Â Â Â bpl Â Â signal_nonyb
> +
> + Â Â Â moveb Â %a3@, %a4@(o_crc1)
> +
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> +
> +header_exit:
> + Â Â Â moveq Â #0, %d0
> + Â Â Â moveb Â #0x18, %a3@(write_mode0 - read_mark)
> + Â Â Â rts
> +signal_nonyb:
> + Â Â Â moveq Â #-1, %d0
> + Â Â Â moveb Â #0x18, %a3@(write_mode0 - read_mark)
> + Â Â Â rts
> +
> + Â Â Â .global swim_read_sector_data
> +swim_read_sector_data:
> + Â Â Â link Â Â%a6, #0
> + Â Â Â moveml Â%d1-%d5/%a0-%a5,%sp@-
> + Â Â Â movel Â %a6@(0x0c), %a4
> + Â Â Â bsr Â Â mfm_read_data
> + Â Â Â moveml Â%sp@+, %d1-%d5/%a0-%a5
> + Â Â Â unlk Â Â%a6
> + Â Â Â rts
> +
> +mfm_read_data:
> + Â Â Â movel Â %a6@(0x08), %a3
> + Â Â Â lea Â Â %a3@(read_handshake), %a2
> + Â Â Â lea Â Â %a3@(read_data), %a5
> + Â Â Â lea Â Â %a3@(read_mark), %a3
> + Â Â Â movew Â #seek_time, %d2
> +
> +wait_data_init:
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> + Â Â Â moveb Â #0x18, %a3@(write_mode0 - read_mark)
> + Â Â Â moveb Â #0x01, %a3@(write_mode1 - read_mark)
> + Â Â Â moveb Â #0x01, %a3@(write_mode0 - read_mark)
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> + Â Â Â moveb Â #0x08, %a3@(write_mode1 - read_mark)
> +
> + Â Â Â lea Â Â sector_data_mark, %a0
> + Â Â Â moveq Â #3, %d1
> +
> + Â Â Â /* wait data address mark */
> +
> +wait_data_mark_byte:
> +
> + Â Â Â tstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, wait_data_mark_byte
> + Â Â Â bpl Â Â data_exit
> +
> + Â Â Â moveb Â %a3@, %d3
> + Â Â Â cmpb Â Â%a0@+, %d3
> + Â Â Â dbne Â Â%d1, wait_data_mark_byte
> + Â Â Â bne Â Â wait_data_init
> +
> + Â Â Â /* read data */
> +
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> +
> + Â Â Â movel Â #sector_size-1, %d4 Â Â Â Â Â Â /* sector size */
> +read_new_data:
> + Â Â Â movew Â #max_retry, %d2
> +read_data_loop:
> + Â Â Â moveb Â %a2@, %d5
> + Â Â Â andb Â Â#0xc0, %d5
> + Â Â Â dbne Â Â%d2, read_data_loop
> + Â Â Â beq Â Â data_exit
> + Â Â Â moveb Â %a5@, %a4@+
> + Â Â Â andb Â Â#0x40, %d5
> + Â Â Â dbne Â Â%d4, read_new_data
> + Â Â Â beq Â Â exit_loop
> + Â Â Â moveb Â %a5@, %a4@+
> + Â Â Â dbra Â Â%d4, read_new_data
> +exit_loop:
> +
> + Â Â Â /* read CRC */
> +
> + Â Â Â movew Â #max_retry, %d2
> +data_crc0:
> +
> + Â Â Â tstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, data_crc0
> + Â Â Â bpl Â Â data_exit
> +
> + Â Â Â moveb Â %a3@, %d5
> +
> + Â Â Â moveq Â #max_retry, %d2
> +
> +data_crc1:
> +
> + Â Â Â tstb Â Â%a2@
> + Â Â Â dbmi Â Â%d2, data_crc1
> + Â Â Â bpl Â Â data_exit
> +
> + Â Â Â moveb Â %a3@, %d5
> +
> + Â Â Â tstb Â Â%a3@(read_error - read_mark)
> +
> + Â Â Â moveb Â #0x18, %a3@(write_mode0 - read_mark)
> +
> + Â Â Â /* return number of bytes read */
> +
> + Â Â Â movel Â #sector_size, %d0
> + Â Â Â addw Â Â#1, %d4
> + Â Â Â subl Â Â%d4, %d0
> + Â Â Â rts
> +data_exit:
> + Â Â Â moveb Â #0x18, %a3@(write_mode0 - read_mark)
> + Â Â Â moveq Â #-1, %d0
> + Â Â Â rts
> --
> 1.6.1.3
>
>

--
Gr{oetje,eeting}s,

Geert

--
Geert Uytterhoeven -- There's lots of Linux beyond ia32 -- geert@xxxxxxxxxxxxxx

In personal conversations with technical people, I call myself a hacker. But
when I'm talking to journalists I just say "programmer" or something like that.
-- Linus Torvalds
N‹§²æìr¸›yúèšØb²X¬¶ÇvØ^–)Þ{.nÇ+‰·¥Š{±‘êçzX§¶›¡Ü}©ž²ÆzÚ&j:+v‰¨¾«‘êçzZ+€Ê+zf£¢·hšˆ§~††Ûiÿûàz¹®w¥¢¸?™¨èÚ&¢)ßf”ù^jÇy§m…á@A«a¶Úÿ0¶ìh®å’i

Next message: Thierry Reding: "[PATCH v2] net: Add support for the OpenCores 10/100 Mbps Ethernet MAC."
Previous message: Thierry Reding: "Re: [PATCH] net: Add support for the OpenCores 10/100 MbpsEthernet MAC."
Next in thread: Jens Axboe: "Re: [PATCH 3/6] m68k: mac - Add SWIM floppy support"
Messages sorted by: [ date ] [ thread ] [ subject ] [ author ]