/*
    JPC: A x86 PC Hardware Emulator for a pure Java Virtual Machine
    Release Version 2.0

    A project from the Physics Dept, The University of Oxford

    Copyright (C) 2007 Isis Innovation Limited

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License version 2 as published by
    the Free Software Foundation.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 
    Details (including contact information) can be found at: 

    www.physics.ox.ac.uk/jpc
*/

package org.jpc.emulator.peripheral;

import org.jpc.emulator.motherboard.*;
import org.jpc.support.*;
import org.jpc.emulator.*;

public class FloppyController implements IOPortCapable, DMATransferCapable, TimerResponsive, HardwareComponent {
    /* Will always be a fixed parameter for us */
    private static final int FDRIVE_SECTOR_LEN = 512;
    private static final int FDRIVE_SECTOR_SC = 2; // Sector size code

    /* Floppy disk drive emulation */
    public static final int FDRIVE_DISK_288   = 0x01; // 2.88 MB disk
    public static final int FDRIVE_DISK_144   = 0x02; // 1.44 MB disk
    public static final int FDRIVE_DISK_720   = 0x03; // 720 kB disk
    public static final int FDRIVE_DISK_USER  = 0x04; // User defined geometry
    public static final int FDRIVE_DISK_NONE  = 0x05; // No disk

    public static final int FDRIVE_DRV_144  = 0x00;   // 1.44 MB 3"5 drive
    public static final int FDRIVE_DRV_288  = 0x01;   // 2.88 MB 3"5 drive
    public static final int FDRIVE_DRV_120  = 0x02;   // 1.2  MB 5"25 drive
    public static final int FDRIVE_DRV_NONE = 0x03;   // No drive connected

    private static final int FDRIVE_MOTOR_ON   = 0x01; // motor on/off
    private static final int FDRIVE_REVALIDATE = 0x02; // Revalidated

    private static final int FDISK_DBL_SIDES  = 0x01;

    private static final int FDRIVE_CTRL_ACTIVE = 0x01; /* XXX: suppress that */
    private static final int FDRIVE_CTRL_RESET  = 0x02;
    private static final int FDRIVE_CTRL_SLEEP  = 0x04; /* XXX: suppress that */
    private static final int FDRIVE_CTRL_BUSY   = 0x08; /* dma transfer in progress */
    private static final int FDRIVE_CTRL_INTR   = 0x10;

    private static final int FDRIVE_DIRECTION_WRITE   = 0;
    private static final int FDRIVE_DIRECTION_READ    = 1;
    private static final int FDRIVE_DIRECTION_SCANE   = 2;
    private static final int FDRIVE_DIRECTION_SCANL   = 3;
    private static final int FDRIVE_DIRECTION_SCANH   = 4;

    private static final int FDRIVE_STATE_CMD    = 0x00;
    private static final int FDRIVE_STATE_STATUS = 0x01;
    private static final int FDRIVE_STATE_DATA   = 0x02;
    private static final int FDRIVE_STATE_STATE  = 0x03;
    private static final int FDRIVE_STATE_MULTI  = 0x10;
    private static final int FDRIVE_STATE_SEEK   = 0x20;
    private static final int FDRIVE_STATE_FORMAT = 0x40;

    private byte version;
    private int irqLevel;
    private int dmaChannel;
    private int ioBase;

    private Timer resultTimer;
    private Clock clock;

    private byte state;
    private boolean dmaEnabled;
    private byte currentDrive;
    private byte bootSelect;

    /* Command FIFO */
    private byte[] fifo;
    private int dataOffset;
    private int dataLength;
    private byte dataState;
    private byte dataDirection;
    private byte intStatus;
    private byte eot; // last wanted sector

    /* State kept only to be returned back */
    /* Timers state */
    private byte timer0;
    private byte timer1;
    /* precompensation */
    private byte preCompensationTrack;
    private byte config;
    private byte lock;
    /* Power down config */
    private byte pwrd;
    /* Drives */
    FloppyDrive[] drives;

    InterruptController irqDevice;
    DMAController dma;

    public FloppyController()
    {
	ioportRegistered = false;
	this.fifo = new byte[FDRIVE_SECTOR_LEN];

	this.version = (byte)0x90; /* Intel 82078 Controller */
	this.irqLevel = 6; //Hard coded, but could be passed
	this.ioBase = 0x3f0; //Hard coded, but could be passed
	this.config = (byte)0x60; /* Implicit Seek, polling & fifo enabled */
	
	//Go look in FDCInit in the Motherboard
	this.dmaChannel = 2;

	this.drives = new FloppyDrive[2];
	
	this.state = FDRIVE_CTRL_ACTIVE;       	
    }

    public void timerCallback()
    {
	this.stopTransfer((byte)0x00, (byte)0x00, (byte)0x00);
    }

    public int getDriveType(int number)
    {
	return this.drives[number].drive;
    }

    public int[] ioPortsRequested()
    {
	return new int[]{ioBase + 1, ioBase + 2, ioBase + 3,
			 ioBase + 4, ioBase + 5, ioBase + 7};
    }

    public int ioPortReadByte(int address)
    {
	switch(address & 0x07) {
	case 0x01:
	    return readStatusB();
	case 0x02:
	    return readDOR();
	case 0x03:
	    return readTape();
	case 0x04:
	    return readMainStatus();
	case 0x05:
	    return readData();
	case 0x07:
	    return readDirection();
	default:
	    return 0xff;
	}
    }
    public int ioPortReadWord(int address)
    {
	return (this.ioPortReadByte(address) & 0xff) |
	    ((this.ioPortReadByte(address + 1) << 8) & 0xff00);
    }
    public int ioPortReadLong(int address)
    {
	return (this.ioPortReadWord(address) & 0xffff) |
	    ((this.ioPortReadWord(address + 2) << 16) & 0xffff0000);
    }

    public void ioPortWriteByte(int address, int data)
    {
	switch(address & 0x07) {
	case 0x02:
	    this.writeDOR(data);
	    break;
	case 0x03:
	    this.writeTape(data);
	    break;
	case 0x04:
	    this.writeRate(data);
	    break;
	case 0x05:
	    this.writeData(data);
	    break;
	default:
	    break;
	}
    }
    public void ioPortWriteWord(int address, int data)
    {
	this.ioPortWriteByte(address, data & 0xff);
	this.ioPortWriteByte(address + 1, (data >>> 8) & 0xff);
    }
    public void ioPortWriteLong(int address, int data)
    {
	this.ioPortWriteWord(address, data & 0xffff);
	this.ioPortWriteWord(address + 2, (data >>> 16) & 0xffff);
    }

    private void reset(boolean doIRQ)
    {
	this.resetIRQ();
	this.currentDrive = 0;
	this.dataOffset = 0;
	this.dataLength = 0;
	this.dataState = FDRIVE_STATE_CMD;
	this.dataDirection = FDRIVE_DIRECTION_WRITE;
	for (int i = 0; i < 2; i++)
	    drives[i].reset();
	this.resetFIFO();
	if (doIRQ)
	    this.raiseIRQ((byte)0xc0);
    }

    private void raiseIRQ(byte status)
    {
	if (~(this.state & FDRIVE_CTRL_INTR) != 0) {
	    this.irqDevice.setIRQ(this.irqLevel, 1);
	    this.state |= FDRIVE_CTRL_INTR;
	}
	this.intStatus = status;
    }

    private void resetFIFO()
    {
	this.dataDirection = FDRIVE_DIRECTION_WRITE;
	this.dataOffset = 0;
	this.dataState = (byte)((this.dataState & ~FDRIVE_STATE_STATE) | FDRIVE_STATE_CMD);
    }

    private void resetIRQ()
    {
	this.irqDevice.setIRQ(this.irqLevel, 0);
	this.state &= ~FDRIVE_CTRL_INTR;
    }

    private int readStatusB()
    {
	return 0;
    }

    private int readDOR()
    {
	int retval = 0;
	
	/* Drive motors state indicators */
	if ((this.getDrive(0).driveFlags & FDRIVE_MOTOR_ON) != 0) 
	    retval |= 1 << 5;
	if ((this.getDrive(1).driveFlags & FDRIVE_MOTOR_ON) != 0) 
	    retval |= 1 << 4;
	/* DMA enable */
	retval |= this.dmaEnabled ?  1 << 3 : 0;
	/* Reset indicator */
	retval |= (this.state & FDRIVE_CTRL_RESET) == 0 ? 1 << 2 : 0;
	/* Selected drive */
	retval |= this.currentDrive;
	
	return retval;
    }

    private int readTape()
    {
	/* Disk boot selection indicator */
	return this.bootSelect << 2;
	/* Tape indicators: never allowed */
    }

    private int readMainStatus()
    {
	int retval = 0;
	
	this.state &= ~(FDRIVE_CTRL_SLEEP | FDRIVE_CTRL_RESET);
	if ((this.state & FDRIVE_CTRL_BUSY) == 0) {
	    /* Data transfer allowed */
	    retval |= 0x80;
	    /* Data transfer direction indicator */
	    if (this.dataDirection == FDRIVE_DIRECTION_READ)
		retval |= 0x40;
	}
	/* Should handle 0x20 for SPECIFY command */
	/* Command busy indicator */
	if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_DATA ||
	    (this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_STATUS)
	    retval |= 0x10;
	
	return retval;
    }
    
    private int readData()
    {
	FloppyDrive drive;

	drive = this.getCurrentDrive();
	this.state &= ~FDRIVE_CTRL_SLEEP;
	if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_CMD) {
	    System.err.println("fdc >> can't read data in CMD state");
	    return 0;
	}

	int length;
	int offset = this.dataOffset;
	if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_DATA) {
	    offset %= FDRIVE_SECTOR_LEN;
	    if (offset == 0) {
		length = this.dataLength - this.dataOffset;
		if (length > FDRIVE_SECTOR_LEN)
		    length = FDRIVE_SECTOR_LEN;
		drive.read(drive.currentSector(), this.fifo, length);
	    }
	}
	int retval = this.fifo[offset];
	if (++this.dataOffset == this.dataLength) {
	    this.dataOffset = 0;
	    /* Switch from transfer mode to status mode
	     * then from status mode to command mode
	     */
	    if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_DATA) {
		this.stopTransfer((byte)0x20, (byte)0x00, (byte)0x00);
	    } else {
		this.resetFIFO();
		this.resetIRQ();
	    }
	}
	
	return retval;
    }

    private int readDirection()
    {
	int retval = 0;
	if (((this.getDrive(0).driveFlags & FDRIVE_REVALIDATE) != 0)
	    || ((this.getDrive(1).driveFlags & FDRIVE_REVALIDATE) != 0))
	    retval |= 0x80;

	this.getDrive(0).driveFlags &= ~FDRIVE_REVALIDATE;
	this.getDrive(1).driveFlags &= ~FDRIVE_REVALIDATE;
	
	return retval;
    }

    private void writeDOR(int data)
    {
	/* Reset mode */
	if ((this.state & FDRIVE_CTRL_RESET) != 0) {
	    if ((data & 0x04) == 0) {
		return;
	    }
	}
	/* Drive motors state indicators */
	if ((data & 0x20) != 0)
	    this.getDrive(1).start();
	else
	    this.getDrive(1).stop();
	if ((data & 0x10) != 0) {
	    this.getDrive(0).start();
	} else
	    this.getDrive(0).stop();
	/* DMA enable */

	/* Reset */
	if ((data & 0x04) == 0) {
	    if ((this.state & FDRIVE_CTRL_RESET) == 0) {
		this.state |= FDRIVE_CTRL_RESET;
	    }
	} else {
	    if ((this.state & FDRIVE_CTRL_RESET) != 0) {
		this.reset(true);
		this.state &= ~(FDRIVE_CTRL_RESET | FDRIVE_CTRL_SLEEP);
	    }
	}
	/* Selected drive */
	this.currentDrive = (byte)(data & 1);
    }

    private void writeTape(int data)
    {
	/* Reset mode */
	if ((this.state & FDRIVE_CTRL_RESET) != 0) {
	    return;
	}
	/* Disk boot selection indicator */
	this.bootSelect = (byte)((data >>> 2) & 1);
	/* Tape indicators: never allow */
    }

    private void writeRate(int data)
    {
	/* Reset mode */
	if ((this.state & FDRIVE_CTRL_RESET) != 0) {
            return;
        }
	/* Reset: autoclear */
	if ((data & 0x80) != 0) {
	    this.state |= FDRIVE_CTRL_RESET;
	    this.reset(true);
	    this.state &= ~FDRIVE_CTRL_RESET;
	}
	if ((data & 0x40) != 0) {
	    this.state |= FDRIVE_CTRL_SLEEP;
	    this.reset(true);
	}
	//this.precomp = (data >>> 2) & 0x07;
    }
    private void writeData(int data)
    {
	FloppyDrive drive = this.getCurrentDrive();

	/* Reset Mode */
	if ((this.state & FDRIVE_CTRL_RESET) != 0) {
	    System.err.println("fdc >> floppy controller in RESET state!");
	    return;
	}
	this.state &= ~FDRIVE_CTRL_SLEEP;
	if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_STATUS) {
	    System.err.println("fdc >> can't write data in status mode");
	    return;
	}
	/* Is it write command time? */
	if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_DATA) {
	    /* FIFO data write */
	    this.fifo[this.dataOffset++] = (byte)data;
	    if (this.dataOffset % FDRIVE_SECTOR_LEN == (FDRIVE_SECTOR_LEN -1) ||
		this.dataOffset == this.dataLength) {
		drive.write(drive.currentSector(),
				   this.fifo, FDRIVE_SECTOR_LEN);
	    }
	    /* Switch from transfer mode to status mode
	     * then from status mode to command mode
	     */
	    if ((this.dataState & FDRIVE_STATE_STATE) == FDRIVE_STATE_DATA)
		this.stopTransfer((byte)0x20, (byte)0x00, (byte)0x00);
	    return;
	}
	if (this.dataOffset == 0) {
	    /* Command */
	    switch (data & 0x5f) {
	    case 0x46:
	    case 0x4c:
	    case 0x50:
	    case 0x56:
	    case 0x59:
	    case 0x5d:
		this.dataLength = 9;
		this.enqueue(data);
		return;
	    default:
		break;
	    }
	    switch (data & 0x7f) {
	    case 0x45:
	    case 0x49:
		this.dataLength = 9;
		this.enqueue(data);
		return;
	    default:
		break;
	    }
	    switch (data) {
	    case 0x03:
	    case 0x0f:
		this.dataLength = 3;
		this.enqueue(data);
		return;
	    case 0x04:
	    case 0x07:
	    case 0x12:
	    case 0x33:
	    case 0x4a:
		this.dataLength = 2;
		this.enqueue(data);
		return;
	    case 0x08:
		this.fifo[0] = (byte)(0x20 | (drive.head << 2) 
				      | (this.currentDrive));
		this.fifo[1] = drive.track;
		this.setFIFO(2, false);
		this.resetIRQ();
		this.intStatus = (byte)0xc0;
		return;
	    case 0x0e:
		/* Drives position */
		this.fifo[0] = getDrive(0).track;
		this.fifo[1] = getDrive(1).track;
		this.fifo[2] = 0;
		this.fifo[3] = 0;
		/* timers */
		this.fifo[4] = this.timer0;
		this.fifo[5] = this.dmaEnabled ?
		    (byte)(this.timer1 << 1) : (byte)0;
		this.fifo[6] = drive.lastSector;
		this.fifo[7] = (byte)((this.lock << 7) |
				      (drive.perpendicular << 2));
		this.fifo[8] = this.config;
		this.fifo[9] = this.preCompensationTrack;
		this.setFIFO(10, false);
		return;
	    case 0x10:
		this.fifo[0] = this.version;
		this.setFIFO(1,true);
		return;
	    case 0x13:
		this.dataLength = 4;
		this.enqueue(data);
		return;
	    case 0x14:
		this.lock = 0;
		this.fifo[0] = 0;
		this.setFIFO(1, false);
		return;
	    case 0x17:
	    case 0x8f:
	    case 0xcf:
		this.dataLength = 3;
		this.enqueue(data);
		return;
	    case 0x18:
		this.fifo[0] = 0x41; /* Stepping 1 */
		this.setFIFO(1, false);
		return;
	    case 0x2c:
		this.fifo[0] = 0;
		this.fifo[1] = 0;
		this.fifo[2] = getDrive(0).track;
		this.fifo[3] = getDrive(1).track;
		this.fifo[4] = 0;
		this.fifo[5] = 0;
		this.fifo[6] = this.timer0;
		this.fifo[7] = this.timer1;
		this.fifo[8] = drive.lastSector;
		this.fifo[9] = (byte)((this.lock << 7) |
				      (drive.perpendicular << 2));
		this.fifo[10] = this.config;
		this.fifo[11] = this.preCompensationTrack;
		this.fifo[12] = this.pwrd;
		this.fifo[13] = 0;
		this.fifo[14] = 0;
		this.setFIFO(15, true);
		return;
	    case 0x42:
		this.dataLength = 9;
		this.enqueue(data);
		return;
	    case 0x4c:
		this.dataLength = 18;
		this.enqueue(data);
		return;
	    case 0x4d:
	    case 0x8e:
		this.dataLength = 6;
		this.enqueue(data);
		return;
	    case 0x94:
		this.lock = 1;
		this.fifo[0] = 0x10;
		this.setFIFO(1, true);
		return;
	    case 0xcd:
		this.dataLength = 11;
		this.enqueue(data);
		return;
	    default:
		/* Unknown command */
		this.unimplemented();
		return;
	    }
	}
	this.enqueue(data);
    }

    private void enqueue(int data)
    {
	FloppyDrive drive = null;

	this.fifo[this.dataOffset] = (byte)data;
	if (++this.dataOffset == this.dataLength) {
	    if ((this.dataState & FDRIVE_STATE_FORMAT) != 0) {
		this.formatSector();
		return;
	    }
	    switch (this.fifo[0] & 0x1f) {
	    case 0x06:
		this.startTransfer(FDRIVE_DIRECTION_READ);
		return;
	    case 0x0c:
		this.startTransferDelete(FDRIVE_DIRECTION_READ);
		return;
	    case 0x16:
		this.stopTransfer((byte)0x20, (byte)0x00, (byte)0x00);
		return;
	    case 0x10:
		this.startTransfer(FDRIVE_DIRECTION_SCANE);
		return;
	    case 0x19:
		this.startTransfer(FDRIVE_DIRECTION_SCANL);
		return;
	    case 0x1d:
		this.startTransfer(FDRIVE_DIRECTION_SCANH);
		return;
	    default:
		break;
	    }
	    switch (this.fifo[0] & 0x3f) {
	    case 0x05:
		this.startTransfer(FDRIVE_DIRECTION_WRITE);
		return;
	    case 0x09:
		this.startTransferDelete(FDRIVE_DIRECTION_WRITE);
		return;
	    default:
		break;
	    }
	    switch (this.fifo[0]) {
	    case 0x03:
		this.timer0 = (byte)((this.fifo[1] >>> 4) & 0xf);
		this.timer1 = (byte)(this.fifo[2] >>> 1);
		this.dmaEnabled = ((this.fifo[2] & 1) != 1);
		this.resetFIFO();
		break;
	    case 0x04:
		this.currentDrive = (byte)(this.fifo[1] & 1);
		drive = this.getCurrentDrive();
		drive.head = (byte)((this.fifo[1] >>> 2) & 1);
		this.fifo[0] = (byte)((drive.readOnly << 6) |
				      (drive.track == 0 ? 0x10 : 0x00) |
				      (drive.head << 2) |
				      this.currentDrive |
				      0x28);
		this.setFIFO(1, false);
		break;
	    case 0x07:
		this.currentDrive = (byte)(this.fifo[1] & 1);
		drive = getCurrentDrive();
		drive.recalibrate();
		this.resetFIFO();
		this.raiseIRQ((byte)0x20);
		break;
	    case 0x0f:
		this.currentDrive = (byte)(this.fifo[1] & 1);
		drive = this.getCurrentDrive();
		drive.start();
		if (this.fifo[2] <= drive.track)
		    drive.direction = 1;
		else
		    drive.direction = 0;
		this.resetFIFO();
		if (this.fifo[2] > drive.maxTrack) {
		    this.raiseIRQ((byte)0x60);
		} else {
		    drive.track = this.fifo[2];
		    this.raiseIRQ((byte)0x20);
		}
		break;
	    case 0x12:
		if ((this.fifo[1] & 0x80) != 0)
		    drive.perpendicular = (byte)(this.fifo[1] & 0x7);
		/* No result back */
		this.resetFIFO();
		break;
	    case 0x13:
		this.config = this.fifo[2];
		this.preCompensationTrack =  this.fifo[3];
		/* No result back */
		this.resetFIFO();
		break;
	    case 0x17:
		this.pwrd = this.fifo[1];
		this.fifo[0] = this.fifo[1];
		this.setFIFO(1, true);
		break;
	    case 0x33:
		/* No result back */
		this.resetFIFO();
		break;
	    case 0x42:
		System.err.println("fdc >> treat READ_TRACK command");
		this.startTransfer(FDRIVE_DIRECTION_READ);
		break;
	    case 0x4A:
		/* XXX: should set main status register to busy */
		drive.head = (byte)((this.fifo[1] >>> 2) & 1);
		resultTimer.setExpiry(clock.getTime() + (clock.getTickRate()/50));
		break;
	    case 0x4C:
		/* RESTORE */
		/* Drives position */
		this.getDrive(0).track = this.fifo[3];
		this.getDrive(1).track = this.fifo[4];
		/* timers */
		this.timer0 = this.fifo[7];
		this.timer1 = this.fifo[8];
		drive.lastSector = this.fifo[9];
		this.lock = (byte)(this.fifo[10] >>> 7);
		drive.perpendicular = (byte)((this.fifo[10] >>> 2) & 0xF);
		this.config = this.fifo[11];
		this.preCompensationTrack = this.fifo[12];
		this.pwrd = this.fifo[13];
		this.resetFIFO();
		break;
	    case 0x4D:
		/* FORMAT_TRACK */
		this.currentDrive = (byte)(this.fifo[1] & 1);
		drive = this.getCurrentDrive();
		this.dataState |= FDRIVE_STATE_FORMAT;
		if ((this.fifo[0] & 0x80) != 0)
		    this.dataState |= FDRIVE_STATE_MULTI;
		else
		    this.dataState &= ~FDRIVE_STATE_MULTI;
		this.dataState &= ~FDRIVE_STATE_SEEK;
		drive.bps = this.fifo[2] > 7 ?
		    (short)16384 : (short)(128 << this.fifo[2]);
		drive.lastSector = this.fifo[3];
		
		/* Bochs BIOS is buggy and don't send format informations
		 * for each sector. So, pretend all's done right now...
		 */
		this.dataState &= ~FDRIVE_STATE_FORMAT;
		this.stopTransfer((byte)0x00, (byte)0x00, (byte)0x00);
		break;
	    case (byte)0x8E:
		/* DRIVE_SPECIFICATION_COMMAND */
		if ((this.fifo[this.dataOffset - 1] & 0x80) != 0) {
		    /* Command parameters done */
		    if ((this.fifo[this.dataOffset - 1] & 0x40) != 0) {
			this.fifo[0] = this.fifo[1];
			this.fifo[2] = 0;
			this.fifo[3] = 0;
			this.setFIFO(4, true);
		    } else {
			this.resetFIFO();
		    }
		} else if (this.dataLength > 7) {
		    /* ERROR */
		    this.fifo[0] = (byte)(0x80
					  | (drive.head << 2)
					  | this.currentDrive);
		    this.setFIFO(1, true);
		}
		break;
	    case (byte)0x8F:
		/* RELATIVE_SEEK_OUT */
		this.currentDrive = (byte)(this.fifo[1] & 1);
		drive = getCurrentDrive();
		drive.start();
		drive.direction = 0;
		if (this.fifo[2] + drive.track >= drive.maxTrack) {
		    drive.track = (byte)(drive.maxTrack - 1);
		} else {
		    drive.track += this.fifo[2];
		}
		this.resetFIFO();
		this.raiseIRQ((byte)0x20);
		break;
	    case (byte)0xCD:
		/* FORMAT_AND_WRITE */
		System.err.println("fdc >> treat FORMAT_AND_WRITE command");
		this.unimplemented();
		break;
	    case (byte)0xCF:
		/* RELATIVE_SEEK_IN */
		this.currentDrive = (byte)(this.fifo[1] & 1);
		drive = getCurrentDrive();
		drive.start();
		drive.direction = 1;
		if (this.fifo[2] > drive.track) {
		    drive.track = 0;
		} else {
		    drive.track -= this.fifo[2];
		}
		this.resetFIFO();
		/* Raise Interrupt */
		this.raiseIRQ((byte)0x20);
		break;
	    }
	}
    }

    private void setFIFO(int fifoLength, boolean doIRQ)
    {
	this.dataDirection = FDRIVE_DIRECTION_READ;
	this.dataLength = fifoLength;
	this.dataOffset = 0;
	this.dataState = (byte)((this.dataState & ~FDRIVE_STATE_STATE) | FDRIVE_STATE_STATUS);
	if (doIRQ)
	    this.raiseIRQ((byte)0x00);
    }

    private FloppyDrive getCurrentDrive()
    {
	return getDrive(this.currentDrive);
    }
    
    private FloppyDrive getDrive(int driveNumber)
    {
	return this.drives[driveNumber - this.bootSelect];
    }

    private void unimplemented()
    {
	this.fifo[0] = (byte)0x80;
	this.setFIFO(1, false);
    }

    private void startTransfer(int direction)
    {
	FloppyDrive drive;
	this.currentDrive = (byte)(this.fifo[1] & 1);
	drive = this.getCurrentDrive();
	byte kt = this.fifo[2];
	byte kh = this.fifo[3];
	byte ks = this.fifo[4];
	boolean didSeek = false;
	switch (drive.seek(kh, kt, ks, drive.lastSector)) {
	case 2:
	    /* sect too big */
	    this.stopTransfer((byte)0x40, (byte)0x00, (byte)0x00);
	    this.fifo[3] = kt;
	    this.fifo[4] = kh;
	    this.fifo[5] = ks;
	    return;
	case 3:
	    /* track too big */
	    this.stopTransfer((byte)0x40, (byte)0x80, (byte)0x00);
	    this.fifo[3] = kt;
	    this.fifo[4] = kh;
	    this.fifo[5] = ks;
	    return;
	case 4:
	    /* No seek enabled */
	    this.stopTransfer((byte)0x40, (byte)0x00, (byte)0x00);
	    this.fifo[3] = kt;
	    this.fifo[4] = kh;
	    this.fifo[5] = ks;
	    return;
	case 1:
	    didSeek = true;
	    break;
	default:
	    break;
	}

	this.dataDirection = (byte)direction;
	this.dataOffset = 0;
	this.dataState = (byte)((this.dataState & ~FDRIVE_STATE_STATE) | FDRIVE_STATE_DATA);

	if ((this.fifo[0] & 0x80) != 0)
	    this.dataState |= FDRIVE_STATE_MULTI;
	else
	    this.dataState &= ~FDRIVE_STATE_MULTI;
	if (didSeek)
	    this.dataState |= FDRIVE_STATE_SEEK;
	else 
	    this.dataState &= ~FDRIVE_STATE_SEEK;
	if (this.fifo[5] == 0x00) {
	    this.dataLength = this.fifo[8];
	} else {
	    this.dataLength = 128 << this.fifo[5];
	    int temp = drive.lastSector - ks + 1;
	    if ((this.fifo[0] & 0x80) != 0)
		temp += drive.lastSector;
	    this.dataLength *= temp;
	}
	this.eot = this.fifo[6];
	if (this.dmaEnabled) {
	    int dmaMode = 0;
	    dmaMode = dma.getChannelMode(dmaChannel & 3);
	    dmaMode = (dmaMode >>> 2) & 3;
	    if (((direction == FDRIVE_DIRECTION_SCANE || 
		  direction == FDRIVE_DIRECTION_SCANL ||
		  direction == FDRIVE_DIRECTION_SCANH) && dmaMode == 0) ||
		(direction == FDRIVE_DIRECTION_WRITE && dmaMode == 2) ||
		(direction == FDRIVE_DIRECTION_READ && dmaMode == 1)) {
		/* No access is allowed until DMA transfer has completed */
		this.state |= FDRIVE_CTRL_BUSY;
		/* Now, we just have to wait for the DMA controller to
		 * recall us...
		 */
		dma.holdDREQ(dmaChannel & 3);
		//dma.schedule(dmaChannel); In QEMU unlinks executing block and does DMA asap.
		return;
	    } else {
		System.err.println("fdc >> dma_mode=" + dmaMode + " direction="+ direction);
	    }
	}
	/* IO based transfer: calculate len */
	this.raiseIRQ((byte)0x00);
	return;
    }

    private void stopTransfer(byte status0, byte status1, byte status2)
    {
	FloppyDrive drive = this.getCurrentDrive();
	
	this.fifo[0] = (byte)(status0 | (drive.head << 2) | this.currentDrive);
	this.fifo[1] = status1;
	this.fifo[2] = status2;
	this.fifo[3] = drive.track;
	this.fifo[4] = drive.head;
	this.fifo[5] = drive.sector;
	this.fifo[6] = FDRIVE_SECTOR_SC;
	this.dataDirection = FDRIVE_DIRECTION_READ;
	if ((this.state & FDRIVE_CTRL_BUSY) != 0) {
	    dma.releaseDREQ(dmaChannel & 3);
	    this.state &= ~FDRIVE_CTRL_BUSY;
	}
	this.setFIFO(7, true);
    }

    private void startTransferDelete(int direction)
    {
	this.stopTransfer((byte)0x60, (byte)0x00, (byte)0x00);
    }

    private void formatSector()
    {
	System.err.println("Cannot Format Sector");
    }

    private int memcmp(byte[] a1, byte[] a2, int offset, int length)
    {
	for (int i = 0; i < length; i++) {
	    if (a1[i] - a2[i + offset] != 0)
		return a1[i] - a2[i + offset];
	}
	return 0;
    }
	   
    public int transferHandler(int nchan, int pos, int size)
    {
	byte status0 = 0x00, status1 = 0x00, status2 = 0x00;
	
	if ((this.state & FDRIVE_CTRL_BUSY) == 0) {
	    return 0;
	}

	FloppyDrive drive = getCurrentDrive();

	if ((this.dataDirection == FDRIVE_DIRECTION_SCANE) ||
	    (this.dataDirection == FDRIVE_DIRECTION_SCANL) ||
	    (this.dataDirection == FDRIVE_DIRECTION_SCANH))
	    status2 = 0x04;
	if (size > this.dataLength)
	    size = this.dataLength;
	if (drive.device == null) {
	    if (this.dataDirection == FDRIVE_DIRECTION_WRITE)
		this.stopTransfer((byte)0x60, (byte)0x00, (byte)0x00);
	    else
		this.stopTransfer((byte)0x40, (byte)0x00, (byte)0x00);
	    return 0;
	}

	int relativeOffset = this.dataOffset % FDRIVE_SECTOR_LEN;
	int startOffset;
	for (startOffset = this.dataOffset; this.dataOffset < size;) {
	    int length = size - this.dataOffset;
	    if (length + relativeOffset > FDRIVE_SECTOR_LEN)
		length = FDRIVE_SECTOR_LEN - relativeOffset;
	    if ((this.dataDirection != FDRIVE_DIRECTION_WRITE) ||
		(length < FDRIVE_SECTOR_LEN) || (relativeOffset != 0)) {
		/* READ & SCAN commands and realign to a sector for WRITE */
		if (drive.read(drive.currentSector(), this.fifo, 1) < 0) {
		    /* Sure, image size is too small... */
		    for (int i = 0; i < Math.min(this.fifo.length, FDRIVE_SECTOR_LEN); i++)
			this.fifo[i] = (byte)0x00;
		}
	    }
	    switch (this.dataDirection) {
	    case FDRIVE_DIRECTION_READ:
		/* READ commands */
		dma.writeMemory (nchan, this.fifo, relativeOffset,
				 this.dataOffset, length);
		break;
	    case FDRIVE_DIRECTION_WRITE:
		/* WRITE commands */
		dma.readMemory (nchan, this.fifo, relativeOffset,
				this.dataOffset, length);
		if (drive.write(drive.currentSector(), this.fifo, 1) < 0) {
		    this.stopTransfer((byte)0x60, (byte)0x00, (byte)0x00);
		    return length;
		}
		break;
	    default:
		/* SCAN commands */
		{
		    byte[] tempBuffer = new byte[FDRIVE_SECTOR_LEN];
		    dma.readMemory (nchan, tempBuffer, 0,
				    this.dataOffset, length);
		    int ret = memcmp(tempBuffer, this.fifo, relativeOffset, length);
		    if (ret == 0) {
			status2 = 0x08;
			length = this.dataOffset - startOffset;
			if (this.dataDirection == FDRIVE_DIRECTION_SCANE ||
			    this.dataDirection == FDRIVE_DIRECTION_SCANL ||
			    this.dataDirection == FDRIVE_DIRECTION_SCANH)
			    status2 = 0x08;
			if ((this.dataState & FDRIVE_STATE_SEEK) != 0)
			    status0 |= 0x20;
			this.dataLength -= length;
			//    if (this.dataLength == 0)
			this.stopTransfer(status0, status1, status2);
			return length;

		    }
		    if ((ret < 0 && this.dataDirection == FDRIVE_DIRECTION_SCANL) ||
			(ret > 0 && this.dataDirection == FDRIVE_DIRECTION_SCANH)) {
			status2 = 0x00;
			
			length = this.dataOffset - startOffset;
			if (this.dataDirection == FDRIVE_DIRECTION_SCANE ||
			    this.dataDirection == FDRIVE_DIRECTION_SCANL ||
			    this.dataDirection == FDRIVE_DIRECTION_SCANH)
			    status2 = 0x08;
			if ((this.dataState & FDRIVE_STATE_SEEK) != 0)
			    status0 |= 0x20;
			this.dataLength -= length;
			//    if (this.dataLength == 0)
			this.stopTransfer(status0, status1, status2);
			
			return length;
		    }
		}
		break;
	    }
	    this.dataOffset += length;
	    relativeOffset = this.dataOffset % FDRIVE_SECTOR_LEN;
	    if (relativeOffset == 0) {
		/* Seek to next sector */
		/* XXX: drive.sect >= drive.last_sect should be an
		   error in fact */
		if ((drive.sector >= drive.lastSector) ||
		    (drive.sector == this.eot)) {
		    drive.sector = 1;
		    if ((this.dataState & FDRIVE_STATE_MULTI) != 0) {
			if ((drive.head == 0) &&
			    ((drive.flags & FDISK_DBL_SIDES) != 0)) {	
			    drive.head = 1;
			} else {
			    drive.head = 0;
			    drive.track++;
			    if ((drive.flags & FDISK_DBL_SIDES) == 0)
				break;
			}
		    } else {
			drive.track++;
			break;
		    }
		} else {
		    drive.sector++;
		}
	    }
	}

	int length = this.dataOffset - startOffset;
	if (this.dataDirection == FDRIVE_DIRECTION_SCANE ||
	    this.dataDirection == FDRIVE_DIRECTION_SCANL ||
	    this.dataDirection == FDRIVE_DIRECTION_SCANH)
	    status2 = 0x08;
	if ((this.dataState & FDRIVE_STATE_SEEK) != 0)
	    status0 |= 0x20;
	this.dataLength -= length;
	//    if (this.dataLength == 0)
	this.stopTransfer(status0, status1, status2);
	
	return length;
    }
    
    
    class FloppyDrive
    {
	BlockDevice device;
	int drive;
	int driveFlags;
	byte perpendicular;
	byte head;
	byte track;
	byte sector;
	byte direction;
	byte readWrite;
	int flags;
	byte lastSector;
	byte maxTrack;
	short bps;
	byte readOnly;

	FloppyFormat format;
	
	public FloppyDrive(BlockDevice device)
	{
	    this.device = device;
	    this.drive = FDRIVE_DRV_NONE;
	    this.driveFlags = 0;
	    this.perpendicular = 0;
	    this.lastSector = 0;
	    this.maxTrack = 0;
	}

	public void start()
	{
	    this.driveFlags |= FDRIVE_MOTOR_ON;
	}
	public void stop()
	{
	    this.driveFlags &= ~FDRIVE_MOTOR_ON;
	}
	public int seek(byte head, byte track, byte sector, int enableSeek)
	{
	    if ((track > this.maxTrack)	|| (head != 0 && (this.flags & FDISK_DBL_SIDES) == 0)) {
		return 2;
	    }
	    if (sector > this.lastSector) {
		return 3;
	    }
	    int fileSector = this.calculateSector(head, track, sector, this.lastSector);
	    int ret = 0;
	    if (fileSector != this.currentSector()) {
		if (enableSeek == 0) {
		    return 4;
		}

		this.head = head;
		if (this.track != track)
		    ret = 1;
		this.track = track;
		this.sector = sector;
	    }
	    return ret;
	}

	public int currentSector()
	{
	    return calculateSector(this.head, this.track, this.sector, this.lastSector);
	}

	private int calculateSector(byte head, byte track, byte sector, byte lastSector)
	{
	    return ((((0xff & track) * 2) + (0xff & head)) * (0xff & lastSector)) + (0xff & sector) - 1;
	}

	public void recalibrate()
	{
	    this.head = 0;
	    this.track = 0;
	    this.sector = 1;
	    this.direction = 1;
	    this.readWrite = 0;
	}
	public int read(int sector, byte[] buffer, int length)
	{
	    return device.read(0xffffffffl & sector, buffer, length);
	}
	public int write(int sector, byte[] buffer, int length)
	{
	    return device.write(0xffffffffl & sector, buffer, length);
	}

	public void reset()
	{
	    this.stop();
	    this.recalibrate();
	}

	public void revalidate()
	{
	    this.driveFlags &= ~FDRIVE_REVALIDATE;
	    if (device != null && device.inserted()) {
		format = FloppyFormat.findFormat(device.getTotalSectors(), this.drive);
		if (format.heads() == 1) {
		    this.flags &= ~FDISK_DBL_SIDES;
		} else {
		    this.flags |= FDISK_DBL_SIDES;
		}
		this.maxTrack = (byte)format.tracks();
		this.lastSector = (byte)format.sectors();
		this.readOnly = device.readOnly() ? (byte)0x1 : (byte)0x0;
		this.drive = format.drive();
	    } else {
		this.lastSector = 0;
		this.maxTrack = 0;
		this.flags &= ~FDISK_DBL_SIDES;
	    }
	    this.driveFlags |= FDRIVE_REVALIDATE;
	}

	public String toString()
	{
	    return (device == null) ? "<none>" : format.toString();
	}
    }

    private boolean ioportRegistered;

    public void reset()
    {
	irqDevice = null;
	clock = null;
	resultTimer = null;
	dma = null;
	//Really Empty?
	ioportRegistered = false;
	this.fifo = new byte[FDRIVE_SECTOR_LEN];
	this.config = (byte)0x60; /* Implicit Seek, polling & fifo enabled */
	this.drives = new FloppyDrive[2];	
	this.state = FDRIVE_CTRL_ACTIVE;       	
    }

    public boolean initialised()
    {
	return ((irqDevice != null) && (clock != null) && (dma != null) &&
		(drives[0] != null) && ioportRegistered);
    }

    public void acceptComponent(HardwareComponent component)
    {
	if ((component instanceof InterruptController)
	    && component.initialised())
	    irqDevice = (InterruptController)component;
	if ((component instanceof Clock)
	    && component.initialised()) {
	    clock = (Clock)component;
	    resultTimer = clock.newTimer(this);
	}
	if ((component instanceof IOPortHandler)
	    && component.initialised()) {
	    ((IOPortHandler)component).registerIOPortCapable(this);
	    ioportRegistered = true;
	}
	if ((component instanceof DMAController)
	    && component.initialised()) {
	    if (((DMAController)component).isFirst()) {
		if (dmaChannel != -1) {
		    dma = (DMAController)component;
		    dmaEnabled = true;
		    dma.registerChannel(dmaChannel & 3, this);
		}
	    }
	}
	if ((component instanceof DriveSet)
	    && component.initialised()) {
	    for (int i = 0; i < 2; i++) {
		drives[i] = new FloppyDrive(((DriveSet)component).getFloppyDrive(i));
		//Change CB (like in hard drive)
	    }
	}
	if (this.initialised()) {
	    this.reset(false);
	    for (int i = 0; i < 2; i++)
		if (drives[i] != null) this.drives[i].revalidate();
	}
    }

    public String toString()
    {
	return "Intel 82078 Floppy Controller [" + drives[0].toString() + ", "
	    + drives[1].toString() + "]";
    }
}
