nec765fd.c

VxWorks系统下软盘驱动器驱动的源代码。

LOCAL STATUS fdReset
    (
    FD_DEV *pFdDev
    )
    {
    
    fdInit ();

    fdDriveSelect (pFdDev->fdType, pFdDev->drive);
    (void) fdRecalib (pFdDev->drive);
    fdDriveRelease ();

    return (OK);
    }

/*******************************************************************************
*
* fdIoctl - do device specific control function
*
* This routine is called when the file system cannot handle an ioctl()
* function.
*
* RETURNS:  OK or ERROR.
*/

LOCAL STATUS fdIoctl
    (
    FD_DEV *pFdDev,
    int function,
    int arg
    )
    {
    FD_TYPE *pType = &fdTypes[pFdDev->fdType];
    FAST int status = ERROR;
    FAST int cylinder;
    FAST int head;
    int retryCount;

    wdCancel (fdWid);
    semTake (&fdMuteSem, WAIT_FOREVER);

    switch (function)
	{
	case FIODISKFORMAT:
            fdDriveSelect (pFdDev->fdType, pFdDev->drive);
	    (void) fdRecalib (pFdDev->drive);

	    for (cylinder = 0; cylinder < pType->cylinders; cylinder++)
	        for (head = 0; head < pType->heads; head++)
		    {
		    retryCount = 0;
		    while (fdSeek(pFdDev->drive, cylinder, head) != OK)
	    	        if (++retryCount > fdRetry)
			    {
	    		    (void) errnoSet (S_ioLib_DEVICE_ERROR);
			    goto doneIoctl;
			    }
		    retryCount = 0;
		    while (fdFormat(pFdDev->fdType, pFdDev->drive, 
			   cylinder, head, arg) != OK)
	    	        if (++retryCount > fdRetry)
			    {
	    		    (void) errnoSet (S_ioLib_DEVICE_ERROR);
			    goto doneIoctl;
			    }
		    }
	    status = OK;
	    break;

	default:
	    (void) errnoSet (S_ioLib_UNKNOWN_REQUEST);
	}

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

/*******************************************************************************
*
* fdStatusChk - check a status of a floppy disk
*
* This routine checks for a disk change on devices.
*
* RETURNS: OK.
*/

LOCAL STATUS fdStatusChk
    (
    FD_DEV *pFdDev
    )
    {
    FD_TYPE *pType = &fdTypes[pFdDev->fdType];

    wdCancel (fdWid);
    semTake (&fdMuteSem, WAIT_FOREVER);

    sysOutByte (FD_REG_OUTPUT, fdDORvalues[pFdDev->drive]); 
    sysDelay ();

    /* 
     * Bit 7 is set when a diskette is changed.
     * To clear the bit, we need to perform a seek.  
     */

    if (sysInByte (FD_REG_INPUT) & 0x80)
	{
	pFdDev->blkDev.bd_readyChanged	= TRUE;

	/* do seek to clear DCHG bit in FD_REG_INPUT */

	if (++fdCylinder >= pType->cylinders)
	    fdCylinder = 1;

        fdDriveSelect (pFdDev->fdType, pFdDev->drive);
	(void) fdSeek(pFdDev->drive, fdCylinder, 0);

        /* set the bd_mode per the WP tab */

        pFdDev->blkDev.bd_mode =
			(fdDriveIsWP(pFdDev->drive))? O_RDONLY : O_RDWR;
	}
    
    semGive (&fdMuteSem);
    wdStart (fdWid, (sysClkRateGet() * fdWdSec), 
		(FUNCPTR)fdDriveRelease, 0);
    return (OK);
    }

/*******************************************************************************
*
* fdBlkRW - read or write sectors to a floppy disk.
*
* Read or write sectors to a floppy disk.
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS fdBlkRW
    (
    FD_DEV *pFdDev,
    int startBlk,
    int nBlks,
    char *pBuf,
    int direction
    )
    {
    BLK_DEV *pBlkDev	= &pFdDev->blkDev;
    FD_TYPE *pType	= &fdTypes[pFdDev->fdType];
    int status		= ERROR;
    int rwStatus;
    int head;
    int cylinder;
    int sector;
    int ix;
    int nSecs;
    int retryRW0;
    int retryRW1;
    int retrySeek;

    wdCancel (fdWid);
    semTake (&fdMuteSem, WAIT_FOREVER);

    startBlk += pFdDev->blkOffset;

    /* recalibrate is necessary before the first access */

    fdDriveSelect (pFdDev->fdType, pFdDev->drive);

    if (fdAccess[pFdDev->drive] == 0)
	{
	(void) fdRecalib (pFdDev->drive);
        fdAccess[pFdDev->drive] = 1;
	}

    for (ix = 0; ix < nBlks; ix += nSecs)
	{
	cylinder = startBlk / (pType->sectorsTrack * pType->heads);
	sector   = startBlk % (pType->sectorsTrack * pType->heads);
	head     = sector / pType->sectorsTrack;
	sector   = sector % pType->sectorsTrack + 1;

	fdCylinder = cylinder;		/* remember it for fdStatusChk() */

	retrySeek = 0;
	while (fdSeek(pFdDev->drive, cylinder, head) != OK)
	    if (++retrySeek > fdRetry)
		goto doneRW;
	
	nSecs = min (nBlks - ix, pType->sectorsTrack - sector + 1);
        while ((pBlkDev->bd_bytesPerBlk * nSecs) > sysFdBufSize)
	    nSecs -= 1;

	retryRW1 = 0;
	retryRW0 = 0;
	while ((rwStatus = fdRW (pFdDev->fdType, pFdDev->drive, cylinder, head, 
				 sector, pBuf, nSecs, direction)) != OK)
	    {
	    if ((rwStatus == FD_UNFORMATED) ||
	    	(rwStatus == FD_WRITE_PROTECTED) ||
	    	(rwStatus == FD_DISK_NOT_PRESENT))
		retryRW0 = fdRetry;

	    if (++retryRW0 > fdRetry)
		{
	        (void) fdRecalib (pFdDev->drive);
	        if (++retryRW1 > fdRetry)
		    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 */