fdcdrv.c

来自「cpc-1631的BSP包for VxWorks操作系统」· C语言 代码 · 共 2,400 行 · 第 1/5 页

	    if (semTake(pDev->intSemId, pDev->sysClkRate * 4) == ERROR) 
		{
	        pCmd->status = FDC_ERROR_SEEK;
	        fdcSetAES(pCmd, pd_p, 2);
	        lstatus = (UINT)-1;
	        break;
		}
	    } 
	else 
	    {
	    fdcDelay(200);
	    }

        /* setup the "sense-interrupt" command structure */

        pd_p = (UCHAR *)&cr_data_p->c_data.c_justdata.databytes[0];
        bzero((UCHAR *)cr_data_p, sizeof(FDC_CRDATA));
        pd_p[0] = FDC_CS_SENSEINT;

        /* issue the "sense-interrupt" command */

        fdcExeCmd(pFdc, pd_p, sizeof(cr_data_p->c_data.c_senseinterrupt));

        /* read data (1 byte only) from data FIFO, results phase */

        pd_p = (UCHAR *)&cr_data_p->r_data.r_justdata.databytes[0];
        fdcStat(pFdc, pd_p, 1);

	/*
         * check for invalid command status, this would be the case
         * if no interrupt was pending, if not invalid, read second
         * byte of sense interrupt result data
         */

	statusx = pd_p[0];
	if ((statusx & 0xC0) != 0x80) 
	    {

	    /* read data (1 byte only) from data FIFO, results phase */

	    fdcStat(pFdc, pd_p + 1, 1);

	    /*
	     * check for errors, if so, set up the additional error
	     * status data
	     */

	    if ((statusx & 0x03) == pDev->driveNumber) 
		{
	        if (statusx & 0x20) 
		    {
	            if ((statusx & 0xC0) != 0x00) 
			{
		        pCmd->status = FDC_ERROR_SEEK;
		        fdcSetAES(pCmd, pd_p, 2);
		        lstatus = (UINT)-1;
			}
	            break;
		    }
		}
	    }
	}

    return (lstatus);
    }

/*******************************************************************************
*
* fdcCheck - check state
* 
* This function's purpose is to check the state of the FDC and
* the state of the specified floppy diskette drive.
*
* RETURNS: 0 if OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcCheck
    (
    register FDC_DEV *pDev,		/* device descriptor pointer */
    register FDC_CMDPCKT *pCmd,		/* command packet pointer */
    register FDC *pFdc,			/* FDC registers pointer */
    register FDC_CRDATA *cr_data_p,	/* command/result data pointer */
    register UINT diskChange		/* disk change clear flag */
    )
    {
    register UINT lstatus;		/* local status flag */

    /* setup the return status */

    lstatus = 0;

    for (; !lstatus;) 
	{

        /* if some outstanding status is present, reset the FDC */

        if (pFdc->msr_dsr != FDC_MSR_RQM) 
	    {
	     EIEIO_SYNC;
	     pFdc->dor = 0x00;
	     EIEIO_SYNC;
	    }


        /* deassert S/W reset bit in DOR, all motors off, no DMA */

        if (!(pFdc->dor & FDC_DOR_RESET)) 
	    {
	    EIEIO_SYNC;
	    if (fdcDrvDmaChannel != (UINT)-1) 
		{
	        pFdc->dor = (FDC_DOR_RESET|FDC_DOR_DMAEN);
		} 
	    else 
		{
	        pFdc->dor = (FDC_DOR_RESET);
		}
	    EIEIO_SYNC;


	    /* clear reset state of FDC */

	    if (fdcClearReset(pDev, pCmd, pFdc, cr_data_p)) 
		{
	        lstatus = (UINT)-1;
	        continue;
		}


	    /* initialize DMA controller */

	    if (fdcDrvDmaChannel != (UINT)-1) 
		{
	        isaDmaInit();
		}
	    }

        /*
         * motor on
         * initialize DATA RATE and PRECOMP (%000 specifies default)
         * clear local error status flag
         */

        pFdc->dor |= (pDev->driveNumber | (0x10 << pDev->driveNumber));
        pFdc->msr_dsr = fdcDRCode(pDev);
        pFdc->dir_ccr = fdcDRCode(pDev);

        /*
         * initialize the FDC with the operating parameters of
         * the attached disk
         *
         * if the initialization calls fails, post error status
         */

        if (fdcInit(pDev, pFdc, cr_data_p)) 
	    {
	    lstatus = (UINT)-1;
	    continue;
	    }

        /*
         * query the digital input register for a disk change status, if
         * a disk change has occurred, seek to head zero, track zero
         *
         * if the seek to track call fails, post error status
         */

	if ((pFdc->dir_ccr & FDC_DIR_PS2_DSKCHG) ||
	  (pDev->fdcBlockDev.bd_readyChanged == TRUE)) 
	    {
	    if (fdcSeek(pDev, pCmd, pFdc, cr_data_p, 0, 0)) 
		{
	        lstatus = (UINT)-1;
	        continue;
		}
	    }

	/* clear disk change status (if directed) */

	if (diskChange) 
	    {

	    /* seek to head zero, track zero */

	    if (fdcSeek(pDev, pCmd, pFdc, cr_data_p, 0, 0)) 
		{
	        lstatus = (UINT)-1;
	        continue;
		}


	    /* seek to head zero, track 8 */

	    if (fdcSeek(pDev, pCmd, pFdc, cr_data_p, 0, 8)) 
		{
	        lstatus = (UINT)-1;
	        continue;
		}


	    /* seek to head zero, track zero */

	    if (fdcSeek(pDev, pCmd, pFdc, cr_data_p, 0, 0)) 
		{
	        lstatus = (UINT)-1;
	        continue;
		}
	    }

        break;
	}

    return (lstatus);
    }

/*******************************************************************************
* fdcClearReset - clear reset state
*
* This function's purpose is to clear the 4 internal disk ready
* line change interrupts.  These interrupts are only present
* following a hardware/software reset.
*
* RETURNS: 0 if OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcClearReset
    (
    register FDC_DEV *pDev,		/* device descriptor pointer */
    register FDC_CMDPCKT *pCmd,		/* command packet pointer */
    register FDC *pFdc,			/* FDC registers pointer */
    register FDC_CRDATA *cr_data_p	/* command/result data pointer */
    )
    {
    register UINT lstatus;		/* local status flag */
    register UCHAR statusx;		/* status register copy */
    register UCHAR *pd_p;		/* phase data pointer */
    register UINT driveno;		/* drive number */


    /* setup the return status */

    lstatus = 0;

    /*
     * delay for a bit, the device must enter the idle
     * phase, accessing the device (FDC) will prohibit
     * the idle phase from being entered
     */

    fdcDelay(2000);

    /* wait for interrupt from the selected drive */

    for (driveno = 0; driveno < FDC_NDRIVES;) 
	{

        /* setup the "sense-interrupt" command structure */

        pd_p = (UCHAR *)&cr_data_p->c_data.c_justdata.databytes[0];
        bzero((UCHAR *)cr_data_p, sizeof(FDC_CRDATA));
        pd_p[0] = FDC_CS_SENSEINT;

        /* issue the "sense-interrupt" command */

        fdcExeCmd(pFdc, pd_p, sizeof(cr_data_p->c_data.c_senseinterrupt));

        /* read data (1 byte only) from data FIFO, results phase */

        pd_p = (UCHAR *)&cr_data_p->r_data.r_justdata.databytes[0];
        fdcStat(pFdc, pd_p, 1);

        /*
         * check for invalid command status, this would be the case
         * if no interrupt was pending, if not invalid, read second
         * byte of sense interrupt result data
         */

        statusx = pd_p[0];
        if ((statusx & 0xC0) != 0x80) 
	    {

	    /* read data (1 byte only) from data FIFO, results phase */

	    fdcStat(pFdc, pd_p + 1, 1);

	    /*
	     * check for errors, if so, set up the additional error
	     * status data
	     */

	    if ((statusx & 0x03) != driveno) 
		{
	        lstatus = (UINT)-1;
	        } 
	    else 
		{
	        if ((statusx & 0xC0) != 0xC0) 
		    {
	            lstatus = (UINT)-1;
	    	    }
		}
	    if (lstatus == (UINT)-1) 
		{
	        pCmd->status = FDC_ERROR_CLEARRESET;
	        fdcSetAES(pCmd, pd_p, 2);
	        break;
	        } 
	    else 
		{
		driveno++;
	        }
	    }

        /* delay a bit, wait for interrupt status */

        fdcDelay(200);
	}

    return (lstatus);
    }

/*******************************************************************************
*
* fdcInit - initialize operating parameters
*
* This function's purpose is to initialize the FDC with
* the operating parameters of the attached floppy disk
* drive.  These operating parameters are specified by the
* user's configuration parameters, as well as defaults
* determined by this driver.
*
* RETURNS: OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcInit
    (
    register FDC_DEV *pDev,		/* device descriptor pointer */
    register FDC *pFdc,			/* FDC registers pointer */
    register FDC_CRDATA *cr_data_p 	/* command/result data pointer */
    )
    {

    /*
     * issue the "mode" command
     *
     * fields that are set zero specifies to use the defaults
     * DENSEL set to default (%11)
     * HEAD-SETTLE set to default code (8)
     */

    bzero((UCHAR *)cr_data_p, sizeof(FDC_CRDATA));
    cr_data_p->c_data.c_mode.opcode = FDC_CS_MODE;
    cr_data_p->c_data.c_mode.cbyte1 = FDC_CP_TMR | 0x2;
    cr_data_p->c_data.c_mode.cbyte2 = 0x00;
    cr_data_p->c_data.c_mode.cbyte3 = 0xC8;
    cr_data_p->c_data.c_mode.cbyte4 = 0x00;
#ifdef FDC_ENHANCED
    fdcExeCmd( pFdc,
	     (UCHAR *)&cr_data_p->c_data.c_justdata.databytes[0],
	     sizeof(cr_data_p->c_data.c_mode)
	   );
#endif

    /*
     * issue the "specify" command
     *
     * fields that are set zero specifies to use the defaults
     * STEP-RATE set to maximum
     * MOTOR-OFF-TIME set to maximum
     * MOTOR-ON-TIME set to a code of 8,
     * (256ms for 1mb/500kbs, 512ms for 250kbs)
     */

    bzero((UCHAR *)cr_data_p, sizeof(FDC_CRDATA));
    cr_data_p->c_data.c_specify.opcode = FDC_CS_SPECIFY;
    cr_data_p->c_data.c_specify.cbyte1 = 0x00;	/* step-rate/motor-off-time */
    cr_data_p->c_data.c_specify.cbyte2 = 0x10;	/* motor-on-time */
    if (fdcDrvDmaChannel == (UINT)-1) 
	{
        cr_data_p->c_data.c_specify.cbyte2 |= FDC_CP_DMA;
        }
    fdcExeCmd( pFdc,
	     (UCHAR *)&cr_data_p->c_data.c_justdata.databytes[0],
	     sizeof(cr_data_p->c_data.c_specify)
	   );


    /* issue the "configure" command */

    bzero((UCHAR *)cr_data_p, sizeof(FDC_CRDATA));
    cr_data_p->c_data.c_configure.opcode = FDC_CS_CONFIGURE;
    cr_data_p->c_data.c_configure.cbyte1 = 0x00;
#ifdef FDC_ENHANCED
    cr_data_p->c_data.c_configure.cbyte2 = (FDC_CP_EIS) | 0x0F;
#else
    cr_data_p->c_data.c_configure.cbyte2 = (FDC_CP_EIS);
#endif
    cr_data_p->c_data.c_configure.pretrk = 0;
    fdcExeCmd( pFdc,
	     (UCHAR *)&cr_data_p->c_data.c_justdata.databytes[0],
	     sizeof(cr_data_p->c_data.c_configure)
	   );

    return (OK);
    }

/*******************************************************************************
*
* fdc_clsn - calculate logical sector number
*
* This function's purpose is to calculate the logical sector
* number from the logical block parameters.
*
* RETURNS: -1 on ERROR, or the logical sector number
*/

LOCAL UINT fdc_clsn
    (
    register UINT bno,		/* block number */
    register UINT ssiz,		/* sector size */
    register UINT bsiz,		/* block size */
    register UINT sectrk,	/* sectors per track */
    register UINT equalinall	/* equal in all */
    )
    {
    UINT ls = (UINT)-1;

    if (ssiz == bsiz ) 
        {
        if (equalinall) 
	    {
	    ls = bno;
	    return (ls);
	    }
	else 
	    {
	    ls = bno * 2;
	    if (ls >= sectrk)
	        ls = bno + (sectrk / 2);
	    return (ls);
	    }
	}

    if (ssiz < bsiz ) 
	{
        if (equalinall) 
	    {
	    ls = bno * (bsiz / ssiz);
	    return (ls);
	    } 
	else 
	    {
	    ls = bno * 2 * (bsiz / ssiz);
	    if (ls >= sectrk)
	        ls = ((bsiz / ssiz) * bno) + (sectrk / 2);
	    return (ls);
	    }
	}

    if (ssiz > bsiz ) 
	{
        if (equalinall) 
	    {
	    ls = bno / (ssiz / bsiz);
	    return (ls);
	    } 
	else 
	    {
	    ls = bno / ((ssiz / bsiz) / 2);
	    if (ls >= sectrk)
	        ls = (bno / (ssiz / bsiz)) + (sectrk / 2);
	    return (ls);
	    }
	}
    return (ls);
    }