nec765fd.c

软驱通用分区管理

		    goto doneRW;
		retrySeek = 0;
		while (fdSeek(pFdDev->drive, cylinder, head) != OK)
	    	    if (++retrySeek > fdRetry)
			goto doneRW;
		retryRW0 = 0;
		}
	    }

        startBlk += nSecs;
        pBuf	 += pBlkDev->bd_bytesPerBlk * nSecs;
	}

    status = OK;

doneRW:
    if (rwStatus == FD_UNFORMATED)
        (void)errnoSet (S_ioLib_UNFORMATED);
    else if (rwStatus == FD_WRITE_PROTECTED)
	{
        pBlkDev->bd_mode = O_RDONLY;
        (void)errnoSet (S_ioLib_WRITE_PROTECTED);
	}
    else if (rwStatus == FD_DISK_NOT_PRESENT)
        (void)errnoSet (S_ioLib_DISK_NOT_PRESENT);
    else if (rwStatus == ERROR)
        (void)errnoSet (S_ioLib_DEVICE_ERROR);

    semGive (&fdMuteSem);
    wdStart (fdWid, (sysClkRateGet() * fdWdSec), (FUNCPTR)fdDriveRelease, 0);
    return (status);
    }

/*******************************************************************************
*
* fdInit - init a floppy disk controller
*
* This routine initializes a floppy disk controller.
*
* RETURNS: N/A
*/

LOCAL void fdInit (void)
    {
    int ix;

    /* reset the chip */

    sysOutByte (FD_REG_OUTPUT, (FD_DOR_RESET | FD_DOR_DMA_DISABLE));
    taskDelay (sysClkRateGet() >> 1);

    sysOutByte (FD_REG_OUTPUT, (FD_DOR_CLEAR_RESET | FD_DOR_DMA_ENABLE));
    taskDelay (sysClkRateGet() >> 1);
    
    sysOutByte (FD_REG_CONFIG, 0);

    if (semTake (&fdSyncSem, sysClkRateGet() * fdSemSec) != OK)
	return;

    for (ix = 0; ix < FD_MAX_DRIVES; ix++)
	(void) fdIntSense (0);

#ifdef	FD_DEBUG
    printErr ("fdInit\n");
#endif	/* FD_DEBUG */
    }

/*******************************************************************************
*
* fdIntr - Floppy controller interrupt handler.
*
* RETURNS: N/A
*/

LOCAL void fdIntr
    (
    int ctrl
    )
    {
    fdIntCount++;	/* XXX */
    semGive (&fdSyncSem);
    }

/*******************************************************************************
*
* fdDriveSelect - select and turn on the specified drive.
*
* Select and turn on the specified drive.
*
* RETURNS: N/A
*/

LOCAL void fdDriveSelect
    (
    int fdType,
    int drive
    )
    {
    FD_TYPE *pType = &fdTypes[fdType];
    UCHAR command[12];

    /* turn on the motor */

    sysOutByte (FD_REG_OUTPUT, fdDORvalues[drive]);
    sysDelay ();

    /* set data rate */

    sysOutByte (FD_REG_CONFIG, pType->dataRate);
    sysDelay ();

    command[0] = FD_CMD_SPECIFY;
    command[1] = (pType->stepRate << 4) | pType->headUnload;
    command[2] = pType->headLoad << 1;

    fdCmdSend (command, FD_CMD_LEN_SPECIFY);
    sysDelay ();

#ifdef	FD_DEBUG
    printErr ("fdDriveSelect\n");
#endif	/* FD_DEBUG */
    }

/*******************************************************************************
*
* fdDriveRelease - release and turn off the specified drive.
*
* Release and turn off the specified drive.
*
* RETURNS: N/A
*/

LOCAL void fdDriveRelease (void)
    {

    /* turn off the motor */

    sysOutByte (FD_REG_OUTPUT, (FD_DOR_CLEAR_RESET | FD_DOR_DMA_ENABLE));
    sysDelay ();
    }

/*******************************************************************************
*
* fdIntSense - get information concerning the last drive interrupt
*
* Get information concerning the last drive interrupt
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS fdIntSense
    (
    int seekEnd
    )
    {
    UCHAR rValue;
    UCHAR command[12];
    UCHAR results[12];

    command[0] = FD_CMD_SENSEINT;

#ifdef	FD_DEBUG
    {
    int ix;

    printErr ("fdIntSense: ");
    for (ix = 0; ix < FD_CMD_LEN_SENSEINT; ix++)
        printErr ("0x%x ", command[ix]);
    printErr (" intCnt=%d\n", fdIntCount);
    }
#endif	/* FD_DEBUG */

    fdCmdSend (command, FD_CMD_LEN_SENSEINT);

    rValue = fdResultPhase (results, TRUE, 2);

    if ((rValue == 0) && (seekEnd == 0) &&
	(((results[0] & 0xc0) == 0x00) || ((results[0] & 0xc0) == 0xc0)))
	return (OK);
    else if ((rValue == 0) && (seekEnd == 1) && ((results[0] & 0xe0) == 0x20))
	return (OK);

#ifdef	FD_DEBUG
    printErr ("fdIntSense: rValue=0x%x r0=0x%x 0x%x\n",
              rValue, results[0], results[1]);
#endif	/* FD_DEBUG */

    return (ERROR);
    }

/*******************************************************************************
*
* fdRecalib - recalibrate the drive
*
* Recalibrate the drive
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS fdRecalib
    (
    int drive
    )
    {
    int ix;
    UCHAR rValue[2];
    UCHAR command[12];

    command[0] = FD_CMD_RECALIBRATE;
    command[1] = drive & 0x03;

#ifdef	FD_DEBUG
    {
    printErr ("fdRecalib: ");
    for (ix = 0; ix < FD_CMD_LEN_RECALIBRATE; ix++)
        printErr ("0x%x ", command[ix]);
    printErr (" intCnt=%d\n", fdIntCount);
    }
#endif	/* FD_DEBUG */

    for (ix = 0; ix < 2; ix++)
	{
        fdCmdSend (command, FD_CMD_LEN_RECALIBRATE);

        if (semTake (&fdSyncSem, sysClkRateGet() * fdSemSec) != OK)
	    return (ERROR);

        rValue[ix] = fdIntSense (1);
	}

    if ((rValue[0] == OK) && (rValue[1] == OK))
	return (OK);

#ifdef	FD_DEBUG
    printErr ("fdRecalib: rValue=0x%x 0x%x\n", rValue[0], rValue[1]);
#endif	/* FD_DEBUG */

    return (ERROR);
    }

/*******************************************************************************
*
* fdSeek - seek the drive heads to the specified cylinder
*
* Seek the drive heads to the specified cylinder
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS fdSeek
    (
    int drive,
    int cylinder,
    int head
    )
    {
    UCHAR rValue;
    UCHAR command[12];

    command[0] = FD_CMD_SEEK;
    command[1] = (head << 2) | (drive & 0x03);
    command[2] = cylinder;

#ifdef	FD_DEBUG
    {
    int ix;

    printErr ("fdSeek: ");
    for (ix = 0; ix < FD_CMD_LEN_SEEK; ix++)
        printErr ("0x%x ", command[ix]);
    printErr (" intCnt=%d\n", fdIntCount);
    }
#endif	/* FD_DEBUG */

    fdCmdSend (command, FD_CMD_LEN_SEEK);

    if (semTake (&fdSyncSem, sysClkRateGet() * fdSemSec) != OK)
	return (ERROR);

    rValue = fdIntSense (1);

    if (rValue == OK)
	return (OK);

#ifdef	FD_DEBUG
    printErr ("fdSeek: rValue=0x%x\n", rValue);
#endif	/* FD_DEBUG */

    return (ERROR);
    }

/*******************************************************************************
*
* fdRW - read/write a number of sectors on the current track
*
* Read/write a number of sectors on the current track
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS fdRW
    (
    int fdType,
    int drive,
    int cylinder,
    int head,
    int sector,
    void *pBuf,
    int nSecs,
    int direction
    )
    {
    FD_TYPE *pType = &fdTypes[fdType];
    UCHAR rValue;
    char fd_mt = 0;
    unsigned nBytes = nSecs * (128 << pType->secSize);
    UCHAR command[12];
    UCHAR results[12];

    if (pType->heads > 1)
	fd_mt = 1;

    /* setup DMA controller */

    if (direction == O_RDONLY)
	{
        if (dmaSetup (O_RDONLY, (void *)sysFdBuf, nBytes, FD_DMA_CHAN) != OK)
	    return (ERROR);
        command[0] = FD_CMD_READ | (fd_mt << 7) | (pType->mfm << 6) |
			 (pType->sk << 5);
	}
    else
	{
        if (dmaSetup (O_WRONLY, (void *)sysFdBuf, nBytes, FD_DMA_CHAN) != OK)
	    return (ERROR);
        command[0] = FD_CMD_WRITE | (fd_mt << 7) | (pType->mfm << 6);
	bcopy ((char *)pBuf, (char *)sysFdBuf, nBytes);
	}

    command[1] = (head << 2) | (drive & 0x03);
    command[2] = cylinder;
    command[3] = head;
    command[4] = sector;
    command[5] = pType->secSize;
    command[6] = pType->sectorsTrack;
    command[7] = pType->gap1;
    command[8] = 0xff;

#ifdef	FD_DEBUG
    {
    int ix;

    printErr ("fdRW %c : ", (direction == O_RDONLY) ? 'R' : 'W');
    for (ix = 0; ix < FD_CMD_LEN_RW; ix++)
        printErr ("0x%x ", command[ix]);
    printErr (" intCnt=%d\n", fdIntCount);
    }
#endif	/* FD_DEBUG */

    fdCmdSend (command, FD_CMD_LEN_RW);

    if (semTake (&fdSyncSem, sysClkRateGet() * fdSemSec) != OK)
	return (FD_DISK_NOT_PRESENT);

    rValue = fdResultPhase (results, FALSE, 7);

    if (rValue == 0)
	{
	if ((results[0] & 0xc0) == 0x00)
	    {
            if (direction == O_RDONLY)
	        bcopy ((char *)sysFdBuf, (char *)pBuf, nBytes);
	    return (OK);
	    }
	else
	    {
	    if ((results[1] & 0x04) == 0x04)
		return (FD_UNFORMATED);
	    else if ((results[1] & 0x02) == 0x02)
		return (FD_WRITE_PROTECTED);
	    else
		return (ERROR);
	    }
	}

#ifdef	FD_DEBUG
    printErr ("fdRW %c : ", (direction == O_RDONLY) ? 'R' : 'W');
    printErr ("rValue=0x%x r0=0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
               rValue, results[0], results[1], results[2],
               results[3], results[4], results[5], results[6]);
#endif	/* FD_DEBUG */

    return (ERROR);
    }

/*******************************************************************************
*
* fdFormat - format the current track
*
* format the current track
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS fdFormat
    (
    int fdType,
    int drive,
    int cylinder,
    int head,
    int interleave
    )
    {
    FD_TYPE *pType = &fdTypes[fdType];
    int ix;
    int sector;
    UCHAR rValue;
    UCHAR command[12];
    UCHAR results[12];
    char *pBuf = (char *)sysFdBuf;

    sector = 1;
    for (ix = 0; ix < pType->sectorsTrack; ix++)
	{
	*pBuf++ = (char)cylinder;
	*pBuf++ = (char)head;
	*pBuf++ = (char)sector;
	*pBuf++ = (char)pType->secSize;
	sector += 1;			/* XXX no interleave yet */
	}

    /* setup DMA controller */

    if (dmaSetup (O_WRONLY, (void *)sysFdBuf, pType->sectorsTrack * 4,
		  FD_DMA_CHAN) != OK)
	return (ERROR);

    command[0] = FD_CMD_FORMAT | (pType->mfm << 6);
    command[1] = (head << 2) | (drive & 0x03);
    command[2] = pType->secSize;
    command[3] = pType->sectorsTrack;
    command[4] = pType->gap2;
    command[5] = 0xff;

#ifdef	FD_DEBUG
    {
    printErr ("fdFormat: ");
    for (ix = 0; ix < FD_CMD_LEN_FORMAT; ix++)
        printErr ("0x%x ", command[ix]);
    printErr (" intCnt=%d\n", fdIntCount);
    }
#endif	/* FD_DEBUG */

    fdCmdSend (command, FD_CMD_LEN_FORMAT);

    if (semTake (&fdSyncSem, sysClkRateGet() * fdSemSec) != OK)
	return (ERROR);

    rValue = fdResultPhase (results, FALSE, 7);

    if ((rValue == 0) && ((results[0] & 0xc0) == 0x00))
	return (OK);

#ifdef	FD_DEBUG
    printErr ("fdFmt: rValue=0x%x r0=0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
              rValue, results[0], results[1], results[2],
              results[3], results[4], results[5], results[6]);
#endif	/* FD_DEBUG */

    return (ERROR);
    }

/*******************************************************************************
*
* fdCmdSend - send commands to the chip
*
* Send commands to the chip
*
* RETURNS: 0, -1 or -2 if the command didn't succeed.
*/

LOCAL int fdCmdSend
    (
    UCHAR *pCommand,
    int nBytes
    )
    {
    int timeout;
    int ix;

    /* send the command to the FDC */
    for (ix = 0; ix < nBytes; ix++)
	{

	/* wait for the RQM flag */

	timeout = 0;
	while ((sysInByte (FD_REG_STATUS) & FD_MSR_RQM) != FD_MSR_RQM)
	    {
	    if (++timeout > fdTimeout)
		return (-1);
	    }
	
	/* check that the FDC is ready for input */

	if ((sysInByte (FD_REG_STATUS) & FD_MSR_DIRECTION) != 0)
	    return (-2);
	
	sysOutByte (FD_REG_DATA, (*pCommand++));
	sysDelay ();
	}
    
    return (0);
    }

/*******************************************************************************
*
* fdResultPhase - get results from the chip
*
* Get results from the chip
*
* RETURNS: 0, -1 -2 -3 -4 if the command didn't succeed.
*/

LOCAL int fdResultPhase
    (
    UCHAR *pResults,
    BOOL immediate,
    int nBytes
    )
    {
    int ix;
    int timeout;

    /* input results from the FDC */

    if (immediate || (sysInByte (FD_REG_STATUS) & FD_MSR_FD_BUSY) != 0)
	{

	/* operation is completed, get FDC result bytes */

	for (ix = 0; ix < nBytes; ix++)
	    {

	    /* wait for the RQM flag */

	    timeout = 0;
	    while ((sysInByte (FD_REG_STATUS) & FD_MSR_RQM) != FD_MSR_RQM)
	        {
	        if (++timeout > fdTimeout)
		    return (-1);
	        }
	
	    /* check that the FDC is ready for output */

	    if ((sysInByte (FD_REG_STATUS) & FD_MSR_DIRECTION) == 0)
	        return (-2);
	    
	    pResults[ix] = sysInByte (FD_REG_DATA);
	    sysDelay ();
	    }
	
	/* make sure that FDC has completed sending data */

	timeout = 0;
	while ((sysInByte (FD_REG_STATUS) & FD_MSR_FD_BUSY) != 0)
	    {
	    if (++timeout > fdTimeout)
	        return (-3);
	    }

#ifdef	FD_DEBUG
	{
	printErr ("fdResultPhase: ");
	for (ix = 0; ix < nBytes; ix++)
	    printErr ("0x%x ", pResults[ix]);
	printErr ("\n");
	}
#endif	/* FD_DEBUG */

	}
    else
	{

	/* catch all hidden interrupts */

	ix = 0;
	while (fdIntSense(0) != OK)
	    {
	    if (++ix > fdRetry)
		return (-4);
	    }
	}

    return (0);
    }