floppy.c~

The main purpose of this project is to add a new scheduling algorithm to GeekOS and to implement a s

 */
static void Floppy_Out(uchar_t val)
{
    Wait_For_MRQ(FDC_STATUS_READY_WRITE);
    Out_Byte(FDC_DATA_REG, val);
}

/*
 * Wait for the controller to issue an interrupt.
 * Must be called with interrupts disabled.
 */
static void Wait_For_Interrupt(void)
{
    KASSERT(!Interrupts_Enabled());

    /* Spin wait */

    s_interruptOccurred = 0;
    Enable_Interrupts();
    while (!s_interruptOccurred) {
	/* FIXME: Could sleep here */
    }
    Disable_Interrupts();
}

static void Sense_Interrupt_Status(uchar_t* st0, uchar_t *pcn)
{
    Floppy_Out(FDC_COMMAND_SENSE_INT_STATUS);
    *st0 = Floppy_In();
    *pcn = Floppy_In();
}

/*
 * Calibrate the given drive.
 */
static bool Calibrate(int drive)
{
    int numAttempts = 4;
    bool success = false;
    uchar_t st0, pcn;

    KASSERT(!Interrupts_Enabled());

    while (numAttempts-- > 0) {
	/* Issue the calibrate command */
	Floppy_Out(FDC_COMMAND_CALIBRATE);
	Floppy_Out((uchar_t) drive);
	Wait_For_Interrupt();

	/* Check interrupt status, to see if calibrate succeeded */
	Sense_Interrupt_Status(&st0, &pcn);
	Debug("Calibrate: st0=%02x, pcn=%02x\n", st0, pcn);
	if (st0 & FDC_ST0_SEEK_END) {
	    success = true;
	    break;
	}
    }

    Debug("Drive %d calibration %s\n", drive, success?"succeeded":"failed");
    return success;
}

static void Start_Motor(int drive)
{
    Out_Byte(FDC_DOR_REG,
	FDC_DOR_MOTOR(drive) | FDC_DOR_DMA_ENABLE | FDC_DOR_RESET_DISABLE | FDC_DOR_DRIVE_SELECT(0));
}

static void Stop_Motor(int drive)
{
    Out_Byte(FDC_DOR_REG,
	FDC_DOR_DMA_ENABLE | FDC_DOR_RESET_DISABLE | FDC_DOR_DRIVE_SELECT(0));
}

/*
 * Reset and calibrate the controller.
 * Return true is successful, false otherwise.
 */
static bool Reset_Controller(void)
{
    /* Reset */
    Out_Byte(FDC_DOR_REG, 0);
    /*Micro_Delay(1000); */

    /*
     * Enable fd0
     * TODO: we might want to support drives other than 0 eventually
     */
    Start_Motor(0);

    return Calibrate(0);
}

static bool Floppy_Seek(int drive, int cylinder, int head)
{
    uchar_t st0, pcn;
    int numAttempts = 4;
    bool success = false;

    Debug("Floppy_Seek(%d,%d,%d)\n", drive, cylinder, head);

    while (numAttempts-- > 0) {
	Start_Motor(drive);
	/*Micro_Delay(1000); */

	Disable_Interrupts();

	Floppy_Out(FDC_COMMAND_SEEK);
	Floppy_Out((head << 2) | (drive & 3));
	Floppy_Out(cylinder & 0xFF);

	Debug("Seek: waiting for interrupt\n");
	Wait_For_Interrupt();
	Debug("Seek: got interrupt\n");

	Enable_Interrupts();

	Stop_Motor(drive);

	Sense_Interrupt_Status(&st0, &pcn);
	if (st0 & FDC_ST0_SEEK_END) {
	    /* Make sure we arrived at the desired cylinder */
	    if (pcn != cylinder) {
		Debug("Seek arrived at wrong cylinder\n");
	    } else {
		Debug("Seek complete!\n");
		success = true;
		break;
	    }
	}
    }

    return success;
}

static int Floppy_Transfer(int direction, int driveNum, int blockNum, char *buf)
{
    struct Floppy_Drive *drive = &s_driveTable[driveNum];
    struct Floppy_Parameters *params = drive->params;
    int cylinder, head, sector;
    enum DMA_Direction dmaDirection =
	direction == FLOPPY_READ ? DMA_READ : DMA_WRITE;
    uchar_t command;
    uchar_t st0, st1, st2;
    int result = -1;

    KASSERT(driveNum == 0);  /* FIXME */
    KASSERT(direction == FLOPPY_READ || direction == FLOPPY_WRITE);
    KASSERT(params != 0);

    LBA_To_CHS(&s_driveTable[driveNum], blockNum, &cylinder, &head, &sector);

    if (!Floppy_Seek(driveNum, cylinder, head))
	return -1;

    Disable_Interrupts();

    /* Set up DMA for transfer */
    Setup_DMA(dmaDirection, FDC_DMA, s_transferBuf, SECTOR_SIZE);

    /* Turn the floppy motor on */
    Start_Motor(driveNum);

    /*
     * According to The Undocumented PC, we should wait 8 millis
     * before attempting a read or write.
     */
    Micro_Delay(8000);

    if (direction == FLOPPY_READ)
	command = FDC_COMMAND_READ_SECTOR | FDC_MFM | FDC_SKIP_DELETED;
    else
	command = FDC_COMMAND_WRITE_SECTOR | FDC_MFM;
 
    /* Issue the command */
    Floppy_Out(command);
    Floppy_Out((head << 2) | (driveNum & 3));
    Floppy_Out(cylinder);
    Floppy_Out(head);
    Floppy_Out(sector);
    Floppy_Out(params->sectorSizeCode);
    Floppy_Out(params->sectors);
    Floppy_Out(params->gapLengthCode);
    Floppy_Out(0xFF);  /* DTL */

    /* Controller will issue an interrupt when the command is complete */
    Wait_For_Interrupt();
    Debug("Floppy_Transfer: received interrupt!\n");

    /* Read results */
    st0 = Floppy_In();
    st1 = Floppy_In();
    st2 = Floppy_In();
    Floppy_In();  /* cylinder */
    Floppy_In();  /* head */
    Floppy_In();  /* sector number */
    Floppy_In();  /* sector size */

    Stop_Motor(driveNum);

    if (FDC_ST0_IS_SUCCESS(st0)) {
	Debug("Floppy_Transfer: successful transfer!\n");
	result = 0;
    }

    Enable_Interrupts();

    /*STOP(); */
    return result;
}

static int Floppy_Read(int driveNum, int blockNum, char *buffer)
{
    int rc;

    Debug("Floppy_Read(%d,%d,%x)\n", driveNum, blockNum, buffer);

#ifndef NDEBUG
    memset(buffer, (char) 0xcd, SECTOR_SIZE);
    memset(s_transferBuf, (char) 0xcd, SECTOR_SIZE);
#endif

    rc = Floppy_Transfer(FLOPPY_READ, driveNum, blockNum, buffer);

    if (rc == 0) {
	/*
	 * Successful transfer!
	 * Copy data from transfer buffer into caller's buffer.
	 */
	memcpy(buffer, s_transferBuf, SECTOR_SIZE);
    }

    return rc;
}

static int Floppy_Write(int driveNum, int blockNum, char *buffer)
{
    Debug("Floppy_Write(%d,%d,%x)\n", driveNum, blockNum, buffer);

    memcpy(s_transferBuf, buffer, SECTOR_SIZE);
    return Floppy_Transfer(FLOPPY_WRITE, driveNum, blockNum, buffer);
}

/*
 * This is the thread which processes floppy I/O requests.
 */
static void Floppy_Request_Thread(ulong_t arg)
{
    int rc;

    Debug("FRQ: Floppy request thread starting...\n");

    for (;;) {
	struct Block_Request *request;

	/* Wait for an I/O request to arrive */
	Debug("FRQ: Request thread waiting for a request\n");
	request = Dequeue_Request(&s_floppyRequestQueue, &s_floppyWaitQueue);
	Debug("FRQ: Got a floppy request [@%x]\n", request);
	KASSERT(request->type == BLOCK_READ || request->type == BLOCK_WRITE);

	/* Perform the I/O. */
	if (request->type == BLOCK_READ)
	    rc = Floppy_Read(request->dev->unit, request->blockNum, request->buf);
	else
	    rc = Floppy_Write(request->dev->unit, request->blockNum, request->buf);

	/* Notify the requesting thread of the outcome of the I/O. */
	Debug("FRQ: Notifying requesting thread...\n");
	Notify_Request_Completion(request, rc == 0 ? COMPLETED : ERROR, rc);
	Debug("FRQ: Completed floppy request\n");
    }
}

/* ----------------------------------------------------------------------
 * Public functions
 * ---------------------------------------------------------------------- */

/*
 * Initialize the floppy controller.
 */
void Init_Floppy(void)
{
    uchar_t floppyByte;
    bool ready = false;
    bool good;

    Print("Initializing floppy controller...\n");

    /* Allocate memory for DMA transfers */
    s_transferBuf = (uchar_t*) Alloc_Page();

    /* Use CMOS to get floppy configuration */
    Out_Byte(CMOS_OUT, CMOS_FLOPPY_INDEX);
    floppyByte = In_Byte(CMOS_IN);
    Setup_Drive_Parameters(0, (floppyByte >> 4) & 0xF);
    Setup_Drive_Parameters(1, floppyByte & 0xF);

    /* Install floppy interrupt handler */
    Install_IRQ(FDC_IRQ, &Floppy_Interrupt_Handler);
    Enable_IRQ(FDC_IRQ);

    /* Reset and calibrate the controller. */
    Disable_Interrupts();
    good = Reset_Controller();
    Enable_Interrupts();
    if (!good) {
	Print("  Failed to reset controller!\n");
	goto done;
    }

    /* Reserve DMA channel 2. */
    if (!Reserve_DMA(FDC_DMA)) {
	Print("  Failed to reserve DMA channel\n");
	goto done;
    }

    /*
     * Driver is now ready for requests.
     * Start the request processing thread.
     */
    ready = true;
    Start_Kernel_Thread(Floppy_Request_Thread, 0, PRIORITY_NORMAL, true);

done:
    if (!ready)
	Print("  Floppy controller initialization FAILED\n");
}