ppc64: Re-enable SF bit after returning from ELF binary

[openbios.git] / drivers / floppy.c

#include "config.h"
#include "libopenbios/bindings.h"
#include "kernel/kernel.h"
#include "libc/byteorder.h"
#include "libc/vsprintf.h"

#include "drivers/drivers.h"

#include "timer.h"

/* DECLARE data structures for the nodes.  */
DECLARE_UNNAMED_NODE( ob_floppy, INSTALL_OPEN, 2*sizeof(int) );

#ifdef CONFIG_DEBUG_FLOPPY
#define printk_info printk
#define printk_debug printk
#else
#define printk_info(x ...)
#define printk_debug(x ...)
#endif
#define printk_err printk

#define FD_DRIVE 0


#define FD_STATUS_A     (0)             /* Status register A */
#define FD_STATUS_B     (1)             /* Status register B */
#define FD_DOR          (2)             /* Digital Output Register */
#define FD_TDR          (3)             /* Tape Drive Register */
#define FD_STATUS       (4)             /* Main Status Register */
#define FD_DSR          (4)             /* Data Rate Select Register (old) */
#define FD_DATA         (5)             /* Data Transfer (FIFO) register */
#define FD_DIR          (7)             /* Digital Input Register (read) */
#define FD_DCR          (7)             /* Diskette Control Register (write)*/

/* Bit of FD_STATUS_A */
#define STA_INT_PENDING 0x80            /* Interrupt Pending */

/* DOR */
#define DOR_DRIVE0      0x00
#define DOR_DRIVE1      0x01
#define DOR_DRIVE2      0x02
#define DOR_DRIVE3      0x03
#define DOR_DRIVE_MASK  0x03
#define DOR_NO_RESET    0x04
#define DOR_DMA_EN      0x08
#define DOR_MOT_EN0     0x10
#define DOR_MOT_EN1     0x20
#define DOR_MOT_EN2     0x40
#define DOR_MOT_EN3     0x80

/* Bits of main status register */
#define STATUS_BUSYMASK 0x0F            /* drive busy mask */
#define STATUS_BUSY     0x10            /* FDC busy */
#define STATUS_NON_DMA  0x20            /* 0- DMA mode */
#define STATUS_DIR      0x40            /* 0- cpu->fdc */
#define STATUS_READY    0x80            /* Data reg ready */

/* Bits of FD_ST0 */
#define ST0_DS          0x03            /* drive select mask */
#define ST0_HA          0x04            /* Head (Address) */
#define ST0_NR          0x08            /* Not Ready */
#define ST0_ECE         0x10            /* Equipment check error */
#define ST0_SE          0x20            /* Seek end */
#define ST0_INTR        0xC0            /* Interrupt code mask */
#define ST0_INTR_OK             (0 << 6)
#define ST0_INTR_ERROR          (1 << 6)
#define ST0_INTR_INVALID        (2 << 6)
#define ST0_INTR_POLL_ERROR     (3 << 6)

/* Bits of FD_ST1 */
#define ST1_MAM         0x01            /* Missing Address Mark */
#define ST1_WP          0x02            /* Write Protect */
#define ST1_ND          0x04            /* No Data - unreadable */
#define ST1_OR          0x10            /* OverRun */
#define ST1_CRC         0x20            /* CRC error in data or addr */
#define ST1_EOC         0x80            /* End Of Cylinder */

/* Bits of FD_ST2 */
#define ST2_MAM         0x01            /* Missing Address Mark (again) */
#define ST2_BC          0x02            /* Bad Cylinder */
#define ST2_SNS         0x04            /* Scan Not Satisfied */
#define ST2_SEH         0x08            /* Scan Equal Hit */
#define ST2_WC          0x10            /* Wrong Cylinder */
#define ST2_CRC         0x20            /* CRC error in data field */
#define ST2_CM          0x40            /* Control Mark = deleted */

/* Bits of FD_ST3 */
#define ST3_HA          0x04            /* Head (Address) */
#define ST3_DS          0x08            /* drive is double-sided */
#define ST3_TZ          0x10            /* Track Zero signal (1=track 0) */
#define ST3_RY          0x20            /* drive is ready */
#define ST3_WP          0x40            /* Write Protect */
#define ST3_FT          0x80            /* Drive Fault */

/* Values for FD_COMMAND */
#define FD_RECALIBRATE          0x07    /* move to track 0 */
#define FD_SEEK                 0x0F    /* seek track */
#define FD_READ                 0xA6    /* read with MT, SKip deleted */
#define FD_WRITE                0xC5    /* write with MT, MFM */
#define FD_SENSEI               0x08    /* Sense Interrupt Status */
#define FD_SPECIFY              0x03    /* specify HUT etc */
#define FD_FORMAT               0x4D    /* format one track */
#define FD_VERSION              0x10    /* get version code */
#define FD_CONFIGURE            0x13    /* configure FIFO operation */
#define FD_PERPENDICULAR        0x12    /* perpendicular r/w mode */
#define FD_GETSTATUS            0x04    /* read ST3 */
#define FD_DUMPREGS             0x0E    /* dump the contents of the fdc regs */
#define FD_READID               0xEA    /* prints the header of a sector */
#define FD_UNLOCK               0x14    /* Fifo config unlock */
#define FD_LOCK                 0x94    /* Fifo config lock */
#define FD_RSEEK_OUT            0x8f    /* seek out (i.e. to lower tracks) */
#define FD_RSEEK_IN             0xcf    /* seek in (i.e. to higher tracks) */


/* the following commands are new in the 82078. They are not used in the
 * floppy driver, except the first three. These commands may be useful for apps
 * which use the FDRAWCMD interface. For doc, get the 82078 spec sheets at
 * http://www-techdoc.intel.com/docs/periph/fd_contr/datasheets/ */

#define FD_PARTID               0x18    /* part id ("extended" version cmd) */
#define FD_SAVE                 0x2e    /* save fdc regs for later restore */
#define FD_DRIVESPEC            0x8e    /* drive specification: Access to the
                                         * 2 Mbps data transfer rate for tape
                                         * drives */

#define FD_RESTORE              0x4e    /* later restore */
#define FD_POWERDOWN            0x27    /* configure FDC's powersave features */
#define FD_FORMAT_N_WRITE       0xef    /* format and write in one go. */
#define FD_OPTION               0x33    /* ISO format (which is a clean way to
                                         * pack more sectors on a track) */

/* FDC version return types */
#define FDC_NONE        0x00
#define FDC_UNKNOWN     0x10    /* DO NOT USE THIS TYPE EXCEPT IF IDENTIFICATION
                                   FAILS EARLY */
#define FDC_8272A       0x20    /* Intel 8272a, NEC 765 */
#define FDC_765ED       0x30    /* Non-Intel 1MB-compatible FDC, can't detect */
#define FDC_82072       0x40    /* Intel 82072; 8272a + FIFO + DUMPREGS */
#define FDC_82072A      0x45    /* 82072A (on Sparcs) */
#define FDC_82077_ORIG  0x51    /* Original version of 82077AA, sans LOCK */
#define FDC_82077       0x52    /* 82077AA-1 */
#define FDC_82078_UNKN  0x5f    /* Unknown 82078 variant */
#define FDC_82078       0x60    /* 44pin 82078 or 64pin 82078SL */
#define FDC_82078_1     0x61    /* 82078-1 (2Mbps fdc) */
#define FDC_S82078B     0x62    /* S82078B (first seen on Adaptec AVA-2825 VLB
                                 * SCSI/EIDE/Floppy controller) */
#define FDC_87306       0x63    /* National Semiconductor PC 87306 */

/*
 * Beware: the fdc type list is roughly sorted by increasing features.
 * Presence of features is tested by comparing the FDC version id with the
 * "oldest" version that has the needed feature.
 * If during FDC detection, an obscure test fails late in the sequence, don't
 * assign FDC_UNKNOWN. Else the FDC will be treated as a dumb 8272a, or worse.
 * This is especially true if the tests are unneeded.
 */

/* Parameters for a 1.44 3.5" disk */
#define DISK_H1440_SIZE       2880
#define DISK_H1440_SECT       18
#define DISK_H1440_HEAD       2
#define DISK_H1440_TRACK      80
#define DISK_H1440_STRETCH    0
#define DISK_H1440_GAP        0x1B
#define DISK_H1440_RATE       0x00
#define DISK_H1440_SPEC1      0xCF
#define DISK_H1440_FMT_GAP    0x6C

/* Parameters for a 1.44 3.5" drive */
#define DRIVE_H1440_MAX_DTR          500
#define DRIVE_H1440_HLT              16   /* ms */
#define DRIVE_H1440_HUT              16   /* ms */
#define DRIVE_H1440_SRT              4000 /* us */
#define DRIVE_H1440_SPINUP           400  /* ms */
#define DRIVE_H1440_SPINDOWN         3000 /* ms */
#define DRIVE_H1440_SPINDOWN_OFFSET  10
#define DRIVE_H1440_SELECT_DELAY     20  /* ms */
#define DRIVE_H1440_RPS              5
#define DRIVE_H1440_TRACKS           83
#define DRIVE_H1440_TIMEOUT          3000 /* ms */
#define DRIVE_H1440_INTERLEAVE_SECT  20

/* Floppy drive configuration */
#define FIFO_DEPTH            10
#define USE_IMPLIED_SEEK      0
#define USE_FIFO              1
#define FIFO_THRESHOLD        10
#define TRACK_PRECOMPENSATION 0

#define SLOW_FLOPPY 0

#define FD_RESET_DELAY 20 /* microseconds */

/*
 * FDC state
 */
static struct drive_state {
        unsigned track;
} drive_state[1];

static struct floppy_fdc_state {
        int in_sync;
        int spec1;              /* spec1 value last used */
        int spec2;              /* spec2 value last used */
        int dtr;
        unsigned char dor;
        unsigned char version;  /* FDC version code */
        void (*fdc_outb)(unsigned char data, unsigned long port);
        unsigned char (*fdc_inb)(unsigned long port);
        unsigned long io_base;
        unsigned long mmio_base;
} fdc_state;

/* Synchronization of FDC access. */
#define FD_COMMAND_NONE -1
#define FD_COMMAND_ERROR 2
#define FD_COMMAND_OKAY 3

/*
 * globals used by 'result()'
 */
#define MAX_REPLIES 16

static void show_floppy(void);
static void floppy_reset(void);

/*
 * IO port operations
 */
static unsigned char
ob_fdc_inb(unsigned long port)
{
    return inb(fdc_state.io_base + port);
}

static void
ob_fdc_outb(unsigned char data, unsigned long port)
{
    outb(data, fdc_state.io_base + port);
}

/*
 * MMIO operations
 */
static unsigned char
ob_fdc_mmio_readb(unsigned long port)
{
    return *(unsigned char *)(fdc_state.mmio_base + port);
}

static void
ob_fdc_mmio_writeb(unsigned char data, unsigned long port)
{
    *(unsigned char *)(fdc_state.mmio_base + port) = data;
}

static int set_dor(char mask, char data)
{
        unsigned char newdor,olddor;

        olddor = fdc_state.dor;
        newdor =  (olddor & mask) | data;
        if (newdor != olddor){
                fdc_state.dor = newdor;
                fdc_state.fdc_outb(newdor, FD_DOR);
        }
        return olddor;
}

/* waits until the fdc becomes ready */
static int wait_til_ready(void)
{
        int counter, status;
        for (counter = 0; counter < 10000; counter++) {
                status = fdc_state.fdc_inb(FD_STATUS);
                if (status & STATUS_READY) {
                        return status;
                }
        }
        printk_debug("Getstatus times out (%x)\n", status);
        show_floppy();
        return -3;
}


/* sends a command byte to the fdc */
static int output_byte(unsigned char byte)
{
        int status;

        if ((status = wait_til_ready()) < 0)
                return status;
        if ((status & (STATUS_READY|STATUS_DIR|STATUS_NON_DMA)) == STATUS_READY){
                fdc_state.fdc_outb(byte,FD_DATA);
                return 0;
        }
        printk_debug("Unable to send byte %x to FDC_STATE. Status=%x\n",
                byte, status);

        show_floppy();
        return -2;
}

/* gets the response from the fdc */
static int result(unsigned char *reply_buffer, int max_replies)
{
        int i, status=0;

        for(i=0; i < max_replies; i++) {
                if ((status = wait_til_ready()) < 0)
                        break;
                status &= STATUS_DIR|STATUS_READY|STATUS_BUSY|STATUS_NON_DMA;
                if ((status & ~STATUS_BUSY) == STATUS_READY){
                        return i;
                }
                if (status == (STATUS_DIR|STATUS_READY|STATUS_BUSY))
                        reply_buffer[i] = fdc_state.fdc_inb(FD_DATA);
                else
                        break;
        }
        if (i == max_replies)
                return i;
        printk_debug("get result error. Last status=%x Read bytes=%d\n",
                status, i);
        show_floppy();
        return -1;
}
#define MORE_OUTPUT -2
/* does the fdc need more output? */
static int need_more_output(void)
{
        unsigned char reply_buffer[MAX_REPLIES];
        int status;
        if ((status = wait_til_ready()) < 0)
                return -1;
        if ((status & (STATUS_READY|STATUS_DIR|STATUS_NON_DMA)) == STATUS_READY)
                return MORE_OUTPUT;
        return result(reply_buffer, MAX_REPLIES);
}

static int output_command(unsigned char *cmd, int count)
{
        int i, status;
        for(i = 0; i < count; i++) {
                if ((status = output_byte(cmd[i])) < 0) {
                        printk_err("full command not acceppted, status =%x\n",
                                status);
                        return -1;
                }
        }
        return 0;
}

static int output_new_command(unsigned char *cmd, int count)
{
        int i, status;
        if ((status = output_byte(cmd[0])) < 0)
                return -1;
        if (need_more_output() != MORE_OUTPUT)
                return -1;
        for(i = 1; i < count; i++) {
                if ((status = output_byte(cmd[i])) < 0) {
                        printk_err("full new command not acceppted, status =%d\n",
                                status);
                        return -1;
                }
        }
        return 0;
}


/* Collect pending interrupt status */
static unsigned char collect_interrupt(void)
{
        unsigned char pcn = 0xff;
        unsigned char reply_buffer[MAX_REPLIES];
        int nr;
#ifdef CONFIG_DEBUG_FLOPPY
        int i, status;
#endif
        nr = result(reply_buffer, MAX_REPLIES);
        if (nr != 0) {
                printk_debug("SENSEI\n");
        }
        else {
                int max_sensei = 4;
                do {
                        if (output_byte(FD_SENSEI) < 0)
                                break;
                        nr = result(reply_buffer, MAX_REPLIES);
                        if (nr == 2) {
                                pcn = reply_buffer[1];
                                printk_debug("SENSEI %02x %02x\n",
                                        reply_buffer[0], reply_buffer[1]);
                        }
                        max_sensei--;
                }while(((reply_buffer[0] & 0x83) != FD_DRIVE) && (nr == 2) && max_sensei);
#ifdef CONFIG_DEBUG_FLOPPY
                status = fdc_state.fdc_inb(FD_STATUS);
                printk_debug("status = %x, reply_buffer=", status);
                for(i = 0; i < nr; i++) {
                        printk_debug(" %x",
                                reply_buffer[i]);
                }
                printk_debug("\n");
#else
                fdc_state.fdc_inb(FD_STATUS);
#endif
        }

        return pcn;
}


/* selects the fdc and drive, and enables the fdc's input/dma, and it's motor. */
static void set_drive(int drive)
{
        int fdc = (drive >> 2) & 1;
        int status;
        unsigned new_dor;
        if (drive > 3) {
                printk_err("bad drive value\n");
                return;
        }
        if (fdc != 0) {
                printk_err("bad fdc value\n");
                return;
        }
        drive &= 3;
#if 0
        new_dor = 8; /* Enable the controller */
#else
        new_dor = 0; /* Don't enable DMA on the controller */
#endif
        new_dor |= (1 << (drive + 4)); /* Spinup the selected drive */
        new_dor |= drive; /* Select the drive for commands as well */
        set_dor(0xc, new_dor);

        mdelay(DRIVE_H1440_SPINUP);

        status = fdc_state.fdc_inb(FD_STATUS);
        printk_debug("set_drive status = %02x, new_dor = %02x\n",
                status, new_dor);
        if (status != STATUS_READY) {
                printk_err("set_drive bad status\n");
        }
}


/* Disable the motor for a given floppy drive */
static void floppy_motor_off(int drive)
{
        unsigned mask;
        printk_debug("floppy_motor_off\n");
        /* fix the number of drives */
        drive &= 3;
        /* Clear the bit for the drive we care about */
        mask = 0xff;
        mask &= ~(1 << (drive +4));
        /* Now clear the bit in the Digital Output Register */
        set_dor(mask, 0);
}

/* Set the FDC's data transfer rate on behalf of the specified drive.
 * NOTE: with 82072/82077 FDCs, changing the data rate requires a reissue
 * of the specify command (i.e. using the fdc_specify function).
 */
static void fdc_dtr(unsigned rate)
{
        rate &= 3;
        /* If data rate not already set to desired value, set it. */
        if (fdc_state.in_sync && (rate == fdc_state.dtr))
                return;

        /* Set dtr */
        fdc_state.fdc_outb(rate, FD_DCR);

        /* TODO: some FDC/drive combinations (C&T 82C711 with TEAC 1.2MB)
         * need a stabilization period of several milliseconds to be
         * enforced after data rate changes before R/W operations.
         * Pause 5 msec to avoid trouble. (Needs to be 2 jiffies)
         */
        fdc_state.dtr = rate & 3;
        mdelay(5);
} /* fdc_dtr */

static int fdc_configure(int use_implied_seek, int use_fifo,
        unsigned fifo_threshold, unsigned precompensation)
{
        unsigned config_bits;
        unsigned char cmd[4];
        /* 0 EIS EFIFO POLL FIFOOTHR[4] */

        /* santize parameters */
        config_bits = fifo_threshold & 0xf;
        config_bits |= (1 << 4); /* Always disable background floppy poll */
        config_bits |= (!use_fifo) << 5;
        config_bits |= (!!use_implied_seek) << 6;

        precompensation &= 0xff; /* pre-compensation from track 0 upwards */

        cmd[0] = FD_CONFIGURE;
        cmd[1] = 0;
        cmd[2] = config_bits;
        cmd[3] = precompensation;

        /* Turn on FIFO */
        if (output_new_command(cmd, 4) < 0)
                return 0;
        return 1;
}

#define NOMINAL_DTR 500
/* Issue a "SPECIFY" command to set the step rate time, head unload time,
 * head load time, and DMA disable flag to values needed by floppy.
 *
 * The value "dtr" is the data transfer rate in Kbps.  It is needed
 * to account for the data rate-based scaling done by the 82072 and 82077
 * FDC types.  This parameter is ignored for other types of FDCs (i.e.
 * 8272a).
 *
 * Note that changing the data transfer rate has a (probably deleterious)
 * effect on the parameters subject to scaling for 82072/82077 FDCs, so
 * fdc_specify is called again after each data transfer rate
 * change.
 *
 * srt: 1000 to 16000 in microseconds
 * hut: 16 to 240 milliseconds
 * hlt: 2 to 254 milliseconds
 *
 * These values are rounded up to the next highest available delay time.
 */
static void fdc_specify(
        unsigned head_load_time, unsigned head_unload_time, unsigned step_rate)
{
        unsigned char cmd[3];
        unsigned long srt, hlt, hut;
        unsigned long dtr = NOMINAL_DTR;
        unsigned long scale_dtr = NOMINAL_DTR;
        int hlt_max_code = 0x7f;
        int hut_max_code = 0xf;

        printk_debug("fdc_specify\n");

        switch (DISK_H1440_RATE & 0x03) {
                case 3:
                        dtr = 1000;
                        break;
                case 1:
                        dtr = 300;
                        if (fdc_state.version >= FDC_82078) {
                                /* chose the default rate table, not the one
                                 * where 1 = 2 Mbps */
                                cmd[0] = FD_DRIVESPEC;
                                cmd[1] = FD_DRIVE & 3;
                                cmd[2] = 0xc0;
                                output_new_command(cmd,3);
                                /* FIXME how do I handle errors here? */
                        }
                        break;
                case 2:
                        dtr = 250;
                        break;
        }


        if (fdc_state.version >= FDC_82072) {
                scale_dtr = dtr;
                hlt_max_code = 0x00; /* 0==256msec*dtr0/dtr (not linear!) */
                hut_max_code = 0x0; /* 0==256msec*dtr0/dtr (not linear!) */
        }

        /* Convert step rate from microseconds to milliseconds and 4 bits */
        srt = 16 - (step_rate*scale_dtr/1000 + NOMINAL_DTR - 1)/NOMINAL_DTR;
        if (SLOW_FLOPPY) {
                srt = srt / 4;
        }
        if (srt > 0xf) {
                srt = 0xf;
        }

        hlt = (head_load_time*scale_dtr/2 + NOMINAL_DTR - 1)/NOMINAL_DTR;
        if (hlt < 0x01)
                hlt = 0x01;
        else if (hlt > 0x7f)
                hlt = hlt_max_code;

        hut = (head_unload_time*scale_dtr/16 + NOMINAL_DTR - 1)/NOMINAL_DTR;
        if (hut < 0x1)
                hut = 0x1;
        else if (hut > 0xf)
                hut = hut_max_code;

        cmd[0] = FD_SPECIFY;
        cmd[1] = (srt << 4) | hut;
        cmd[2] = (hlt << 1) | 1; /* Always disable DMA */

        /* If these parameters did not change, just return with success */
        if (!fdc_state.in_sync || fdc_state.spec1 != cmd[1] || fdc_state.spec2 != cmd[2]) {
                /* Go ahead and set spec1 and spec2 */
                output_command(cmd, 3);
                /* FIXME how do I handle errors here... */
                printk_info("FD_SPECIFY(%02x, %02x)\n", cmd[1], cmd[2]);
        }
} /* fdc_specify */


/*
 * reset is done by pulling bit 2 of DOR low for a while (old FDCs),
 * or by setting the self clearing bit 7 of STATUS (newer FDCs)
 */
static void reset_fdc(void)
{
        unsigned char reply[MAX_REPLIES];

        fdc_state.in_sync = 0;

        /* Pseudo-DMA may intercept 'reset finished' interrupt.  */
        /* Irrelevant for systems with true DMA (i386).          */

        if (fdc_state.version >= FDC_82072A)
                fdc_state.fdc_outb(0x80 | (fdc_state.dtr &3), FD_DSR);
        else {
                fdc_state.fdc_outb(fdc_state.dor & ~DOR_NO_RESET, FD_DOR);
                udelay(FD_RESET_DELAY);
                fdc_state.fdc_outb(fdc_state.dor, FD_DOR);
        }
        result(reply, MAX_REPLIES);
}



static void show_floppy(void)
{

        printk_debug("\n");
        printk_debug("floppy driver state\n");
        printk_debug("-------------------\n");

        printk_debug("fdc_bytes: %02x %02x xx %02x %02x %02x xx %02x\n",
                     fdc_state.fdc_inb(FD_STATUS_A),
                     fdc_state.fdc_inb(FD_STATUS_B),
                     fdc_state.fdc_inb(FD_TDR),
                     fdc_state.fdc_inb(FD_STATUS),
                     fdc_state.fdc_inb(FD_DATA),
                     fdc_state.fdc_inb(FD_DIR));

        printk_debug("status=%x\n", fdc_state.fdc_inb(FD_STATUS));
        printk_debug("\n");
}

static void floppy_recalibrate(void)
{
        unsigned char cmd[2];
        unsigned char reply[MAX_REPLIES];
        int nr, success;
        success = 0;
        do {
                printk_debug("floppy_recalibrate\n");
                /* Send the recalibrate command to the controller.
                 * We don't have interrupts or anything we can poll
                 * so we have to guess when it is done.
                 */
                cmd[0] = FD_RECALIBRATE;
                cmd[1] = 0;
                if (output_command(cmd, 2) < 0)
                        continue;

                /* Sleep for the maximum time the recalibrate command
                 * can run.
                 */
                mdelay(80*DRIVE_H1440_SRT/1000);

                /* Now call FD_SENSEI to end the command
                 * and collect up the reply.
                 */
                if (output_byte(FD_SENSEI) < 0)
                        continue;
                nr = result(reply, MAX_REPLIES);

                /* Now see if we have succeeded in our seek */
                success =
                        /* We have the right size result */
                        (nr == 2) &&
                        /* The command didn't terminate in error */
                        ((reply[0] & ST0_INTR) == ST0_INTR_OK) &&
                        /* We finished a seek */
                        (reply[0] & ST0_SE) &&
                        /* We are at cylinder 0 */
                        (reply[1] == 0);
        } while(!success);
        /* Remember we are at track 0 */
        drive_state[FD_DRIVE].track = 0;
}


static int floppy_seek(unsigned track)
{
        unsigned char cmd[3];
        unsigned char reply[MAX_REPLIES];
        int nr, success;
        unsigned distance, old_track;

        /* Look up the old track and see if we need to
         * do anything.
         */
        old_track = drive_state[FD_DRIVE].track;
        if (old_track == track) {
                return 1;
        }

        /* Compute the distance we are about to move,
         * We need to know this so we know how long to sleep...
         */
        distance = (old_track > track)?(old_track - track):(track - old_track);
        distance += 1;


        /* Send the seek command to the controller.
         * We don't have interrupts or anything we can poll
         * so we have to guess when it is done.
         */
        cmd[0] = FD_SEEK;
        cmd[1] = FD_DRIVE;
        cmd[2] = track;
        if (output_command(cmd, 3) < 0)
                return 0;

        /* Sleep for the time it takes to step throuhg distance tracks.
         */
        mdelay(distance*DRIVE_H1440_SRT/1000);

        /* Now call FD_SENSEI to end the command
         * and collect up the reply.
         */
        cmd[0] = FD_SENSEI;
        if (output_command(cmd, 1) < 0)
                return 0;
        nr = result(reply, MAX_REPLIES);

        /* Now see if we have succeeded in our seek */
        success =
                /* We have the right size result */
                (nr == 2) &&
                /* The command didn't terminate in error */
                ((reply[0] & ST0_INTR) == ST0_INTR_OK) &&
                /* We finished a seek */
                (reply[0] & ST0_SE) &&
                /* We are at cylinder 0 */
                (reply[1] == track);
        if (success)
                drive_state[FD_DRIVE].track = track;
        else {
                printk_debug("seek failed\n");
                printk_debug("nr = %d\n", nr);
                printk_debug("ST0 = %02x\n", reply[0]);
                printk_debug("PCN = %02x\n", reply[1]);
                printk_debug("status = %d\n", fdc_state.fdc_inb(FD_STATUS));
        }
        return success;
}

static int read_ok(unsigned head)
{
        unsigned char results[7];
        int result_ok;
        int nr;

        /* read back the read results */
        nr = result(results, 7);

        /* Now see if they say we are o.k. */
        result_ok = 0;
        /* Are my result bytes o.k.? */
        if (nr == 7) {
                /* Are we o.k. */
                if ((results[0] & ST0_INTR) == ST0_INTR_OK) {
                        result_ok = 1;
                }
                /* Or did we get just an overflow error */
                else if (((results[0] & ST0_INTR) == ST0_INTR_ERROR) &&
                        (results[1]== ST1_OR) &&
                        (results[2] == 0)) {
                        result_ok = 1;
                }
                /* Verify the reply had the correct head */
                if (((results[0] & ST0_HA) >> 2) != head) {
                        result_ok = 0;
                }
                /* Verify the reply had the correct drive */
                if (((results[0] & ST0_DS) != FD_DRIVE)) {
                        result_ok = 0;
                }
        }
        if (!result_ok) {
                printk_debug("result_bytes = %d\n", nr);
                printk_debug("ST0 = %02x\n", results[0]);
                printk_debug("ST1 = %02x\n", results[1]);
                printk_debug("ST2 = %02x\n", results[2]);
                printk_debug("  C = %02x\n", results[3]);
                printk_debug("  H = %02x\n", results[4]);
                printk_debug("  R = %02x\n", results[5]);
                printk_debug("  N = %02x\n", results[6]);
        }
        return result_ok;
}

static int floppy_read_sectors(
        char *dest, unsigned byte_offset, unsigned length,
        unsigned sector, unsigned head, unsigned track)
{
        /* MT  == Multitrack */
        /* MFM == MFM or FM Mode */
        /* SK  == Skip deleted data addres Mark */
        /* HDS == Head number select */
        /* DS0 == Disk Drive Select 0 */
        /* DS1 == Disk Drive Select 1 */
        /* C   == Cylinder number 0 - 255 */
        /* H   == Head number */
        /* R   == Record */
        /* N   == The number of data bytes written in a sector */
        /* EOT == End of Track */
        /* GPL == Gap Length */
        /* DTL == Data Length */
        /* MT MFM  SK  0 1 1   0   0 */
        /* 0  0    0   0 0 HDS DS1 DS0 */
        /* C, H, R, N, EOT, GPL, DTL */

        int i, status, result_ok;
        int max_bytes, bytes_read;
        int ret;
        unsigned char cmd[9];
        unsigned end_offset;

        end_offset = byte_offset + length;
        max_bytes = 512*(DISK_H1440_SECT - sector + 1);

        if (byte_offset >= max_bytes) {
                return 0;
        }
        cmd[0] = FD_READ | (((DISK_H1440_HEAD ==2)?1:0) << 6);
        cmd[1] = (head << 2) | FD_DRIVE;
        cmd[2] = track;
        cmd[3] = head;
        cmd[4] = sector;
        cmd[5] = 2; /* 2^N *128 == Sector size.  Hard coded to 512 bytes */
        cmd[6] = DISK_H1440_SECT;
        cmd[7] = DISK_H1440_GAP;
        cmd[8] = 0xff;

        /* Output the command bytes */
        if (output_command(cmd, 9) < 0)
                return -1;

        /* The execution stage begins when STATUS_READY&STATUS_NON_DMA is set */
        do {
                status = fdc_state.fdc_inb(FD_STATUS);
                status &= STATUS_READY | STATUS_NON_DMA;
        } while(status != (STATUS_READY|STATUS_NON_DMA));

        for(i = 0; i < max_bytes; i++) {
                unsigned char byte;
                if ((status = wait_til_ready()) < 0) {
                        break;
                }
                status &= STATUS_READY|STATUS_DIR|STATUS_NON_DMA;
                if (status != (STATUS_READY|STATUS_DIR|STATUS_NON_DMA)) {
                        break;
                }
                byte = fdc_state.fdc_inb(FD_DATA);
                if ((i >= byte_offset) && (i < end_offset)) {
                        dest[i - byte_offset] = byte;
                }
        }
        bytes_read = i;

        /* The result stage begins when STATUS_NON_DMA is cleared */
        while((status = fdc_state.fdc_inb(FD_STATUS)) & STATUS_NON_DMA) {
                /* We get extra bytes in the fifo  past
                 * the end of the sector and drop them on the floor.
                 * Otherwise the fifo is polluted.
                 */
                fdc_state.fdc_inb(FD_DATA);
        }
        /* Did I get an error? */
        result_ok = read_ok(head);
        /* Did I read enough bytes? */
        ret = -1;
        if (result_ok && (bytes_read == max_bytes)) {
                ret = bytes_read - byte_offset;
                if (ret > length) {
                        ret = length;
                }
        }

        if (ret < 0) {
                printk_debug("ret = %d\n", ret);
                printk_debug("bytes_read = %d\n", bytes_read);
                printk_debug("status = %x\n", status);
        }
        return ret;
}


static int __floppy_read(char *dest, unsigned long offset, unsigned long length)
{
        unsigned head, track, sector, byte_offset, sector_offset;
        int ret;

        /* break the offset up into sectors and bytes */
        byte_offset = offset % 512;
        sector_offset = offset / 512;

        /* Find the disk block we are starting with... */
        sector = (sector_offset % DISK_H1440_SECT) + 1;
        head = (sector_offset / DISK_H1440_SECT) % DISK_H1440_HEAD;
        track = (sector_offset / (DISK_H1440_SECT *DISK_H1440_HEAD))% DISK_H1440_TRACK;

        /* First seek to our start track */
        if (!floppy_seek(track)) {
                return -1;
        }
        /* Then read the data */
        ret = floppy_read_sectors(dest, byte_offset, length, sector, head, track);
        if (ret >= 0) {
                return ret;
        }
        /* If we failed reset the fdc... */
        floppy_reset();
        return -1;
}

static int floppy_read(char *dest, unsigned long offset, unsigned long length)
{
        int fr_result, bytes_read;;

        printk_debug("floppy_read\n");
        bytes_read = 0;
        do {
                int max_errors = 3;
                do {
                        fr_result = __floppy_read(dest + bytes_read, offset,
                                                  length - bytes_read);
                        if (max_errors-- == 0) {
                                return (bytes_read)?bytes_read: -1;
                        }
                } while (fr_result <= 0);
                offset += fr_result;
                bytes_read += fr_result;
        } while(bytes_read < length);
        return bytes_read;
}

/* Determine the floppy disk controller type */
/* This routine was written by David C. Niemi */
static char get_fdc_version(void)
{
        int bytes, ret;
        unsigned char reply_buffer[MAX_REPLIES];

        ret = output_byte(FD_DUMPREGS); /* 82072 and better know DUMPREGS */
        if (ret < 0)
                return FDC_NONE;
        if ((bytes = result(reply_buffer, MAX_REPLIES)) <= 0x00)
                return FDC_NONE;        /* No FDC present ??? */
        if ((bytes==1) && (reply_buffer[0] == 0x80)){
                printk_info("FDC is an 8272A\n");
                return FDC_8272A;       /* 8272a/765 don't know DUMPREGS */
        }
        if (bytes != 10) {
                printk_debug("init: DUMPREGS: unexpected return of %d bytes.\n",
                        bytes);
                return FDC_UNKNOWN;
        }
        if (!fdc_configure(USE_IMPLIED_SEEK, USE_FIFO, FIFO_THRESHOLD,
                TRACK_PRECOMPENSATION)) {
                printk_info("FDC is an 82072\n");
                return FDC_82072;       /* 82072 doesn't know CONFIGURE */
        }

        output_byte(FD_PERPENDICULAR);
        if (need_more_output() == MORE_OUTPUT) {
                output_byte(0);
        } else {
                printk_info("FDC is an 82072A\n");
                return FDC_82072A;      /* 82072A as found on Sparcs. */
        }

        output_byte(FD_UNLOCK);
        bytes = result(reply_buffer, MAX_REPLIES);
        if ((bytes == 1) && (reply_buffer[0] == 0x80)){
                printk_info("FDC is a pre-1991 82077\n");
                return FDC_82077_ORIG;  /* Pre-1991 82077, doesn't know
                                         * LOCK/UNLOCK */
        }
        if ((bytes != 1) || (reply_buffer[0] != 0x00)) {
                printk_debug("FDC init: UNLOCK: unexpected return of %d bytes.\n",
                        bytes);
                return FDC_UNKNOWN;
        }
        output_byte(FD_PARTID);
        bytes = result(reply_buffer, MAX_REPLIES);
        if (bytes != 1) {
                printk_debug("FDC init: PARTID: unexpected return of %d bytes.\n",
                        bytes);
                return FDC_UNKNOWN;
        }
        if (reply_buffer[0] == 0x80) {
                printk_info("FDC is a post-1991 82077\n");
                return FDC_82077;       /* Revised 82077AA passes all the tests */
        }
        switch (reply_buffer[0] >> 5) {
        case 0x0:
                /* Either a 82078-1 or a 82078SL running at 5Volt */
                printk_info("FDC is an 82078.\n");
                return FDC_82078;
        case 0x1:
                printk_info("FDC is a 44pin 82078\n");
                return FDC_82078;
        case 0x2:
                printk_info("FDC is a S82078B\n");
                return FDC_S82078B;
        case 0x3:
                printk_info("FDC is a National Semiconductor PC87306\n");
                return FDC_87306;
        default:
                printk_info("FDC init: 82078 variant with unknown PARTID=%d.\n",
                        reply_buffer[0] >> 5);
                return FDC_82078_UNKN;
        }
} /* get_fdc_version */


static int floppy_init(unsigned long io_base, unsigned long mmio_base)
{
        printk_debug("floppy_init\n");
        fdc_state.in_sync = 0;
        fdc_state.spec1 = -1;
        fdc_state.spec2 = -1;
        fdc_state.dtr = -1;
        fdc_state.dor = DOR_NO_RESET;
        fdc_state.version = FDC_UNKNOWN;
        if (mmio_base) {
            fdc_state.fdc_inb = ob_fdc_mmio_readb;
            fdc_state.fdc_outb = ob_fdc_mmio_writeb;
        } else {
            fdc_state.fdc_inb = ob_fdc_inb;
            fdc_state.fdc_outb = ob_fdc_outb;
        }
        fdc_state.io_base = io_base;
        fdc_state.mmio_base = mmio_base;
        reset_fdc();
        /* Try to determine the floppy controller type */
        fdc_state.version = get_fdc_version();
        if (fdc_state.version == FDC_NONE) {
                return -1;
        }
        floppy_reset();
        printk_info("fdc_state.version = %04x\n", fdc_state.version);
        return 0;
}

static void floppy_reset(void)
{
        printk_debug("floppy_reset\n");
        floppy_motor_off(FD_DRIVE);
        reset_fdc();
        fdc_dtr(DISK_H1440_RATE);
        /* program data rate via ccr */
        collect_interrupt();
        fdc_configure(USE_IMPLIED_SEEK, USE_FIFO, FIFO_THRESHOLD,
                TRACK_PRECOMPENSATION);
        fdc_specify(DRIVE_H1440_HLT, DRIVE_H1440_HUT, DRIVE_H1440_SRT);
        set_drive(FD_DRIVE);
        floppy_recalibrate();
        fdc_state.in_sync = 1;
}

static void
ob_floppy_initialize(const char *path)
{
        int props[3];
        phandle_t ph = find_dev(path);

        set_property(ph, "device_type", "block", sizeof("block"));

        // Set dummy reg properties
        props[0] = __cpu_to_be32(0); props[1] = __cpu_to_be32(0); props[2] = __cpu_to_be32(0);
        set_property(ph, "reg", (char *)&props, 3*sizeof(int));

        fword("is-deblocker");
}


static void
ob_floppy_open(int *idx)
{
        int ret = 1;
        phandle_t ph;

        fword("my-unit");
        idx[0]=POP();

        fword("my-parent");
        fword("ihandle>phandle");
        ph=(phandle_t)POP();

        selfword("open-deblocker");

        /* interpose disk-label */
        ph = find_dev("/packages/disk-label");
        fword("my-args");
        PUSH_ph( ph );
        fword("interpose");

        RET ( -ret );
}

static void
ob_floppy_close(int *idx)
{
        selfword("close-deblocker");
}

static void
ob_floppy_read_blocks(int *idx)
{
        cell cnt = POP();
        ucell blk = POP();
        char *dest = (char*)POP();
        floppy_read(dest, blk*512, cnt*512);
        PUSH(cnt);
}


static void
ob_floppy_block_size(int *idx)
{
        PUSH(512);
}

static void
ob_floppy_max_transfer(int *idx)
{
        // Fixme
        PUSH(18 * 512);
}

NODE_METHODS(ob_floppy) = {
        { "open",               ob_floppy_open             },
        { "close",              ob_floppy_close            },
        { "read-blocks",        ob_floppy_read_blocks      },
        { "block-size",         ob_floppy_block_size       },
        { "max-transfer",       ob_floppy_max_transfer     },
};


int ob_floppy_init(const char *path, const char *dev_name,
                   unsigned long io_base, unsigned long mmio_base)
{
        char nodebuff[128];
        phandle_t aliases;

        snprintf(nodebuff, sizeof(nodebuff), "%s/%s", path, dev_name);
        if (!mmio_base) {
            REGISTER_NAMED_NODE(ob_floppy, nodebuff);
            ob_floppy_initialize(nodebuff);
        } else {
            // Already in tree and mapped
            REGISTER_NODE_METHODS(ob_floppy, nodebuff);
        }
        floppy_init(io_base, mmio_base);

        aliases = find_dev("/aliases");
        set_property(aliases, "floppy", nodebuff, strlen(nodebuff) + 1);

        return 0;
}