fdcdrv.c

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

		     		pFdc->fifo = 0x00;
		     		EIEIO_SYNC;
		     		isaDmaStop(fdcDrvDmaChannel);
		     		x_count = 0;
		    		break;
		  		}
	       		    }
	    		}
	 	    } 
		else 
		    {

	    	    /*
	    	     * transfer data to or from the data FIFO, this is
	    	     * the execution phase
	    	     */

	    	    if (pCmd->command == FDC_READOP) 
	   	        {
	       		x_count = fdcRxdEp(pFdc, bufadd, d_count);
	    		} 
		    else 
			{
	       		x_count = fdcTxdEp(pFdc, bufadd, d_count);
	    		}
	 	    }
      		}

	/*
	 * precondition the results phase status bytes, this is done
       	 * in the event the FDC timeouts while retrieving the status
       	 * data bytes from the results phase
       	 */

	pd_p = (UCHAR *)&cr_data_p->r_data.r_justdata.databytes[0];
      	pd_p[0] = 0xC0 | (pos.headno << 2) | pDev->driveNumber;
      	pd_p[1] = 0x14;
      	pd_p[2] = 0x00;


      	/* read data from data FIFO, results phase */

      	fdcStat(pFdc, pd_p, sizeof(cr_data_p->r_data.r_readdata));

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

	statusx = pd_p[0];
	if ((statusx & 0xC0) != 0x00) 
	    {
	    switch (statusx & 0xC0) 
		{
	    	case 0x40:
	            statusx = pd_p[1];
	            if (statusx == 0x80) 
			{
		  	statusx = pd_p[2];
		  	if (statusx == 0x00) 
			    {
		     	    break;
		  	    }
	       		}
	    	case 0x80:
	    	case 0xC0:
	       	    lstatus = (UINT)-1;
	            break;
	 	}
	    if (lstatus) 
		{
	    	pCmd->status = ((pCmd->command == FDC_READOP) ?
		 		FDC_ERROR_READ : FDC_ERROR_WRITE);
	        fdcSetAES(pCmd, pd_p, 3);
	        break;
	 	}
	    }

	/*
      	 * verify the expected data count with the actual data count,
      	 * if they do not match, post the appropriate error status
      	 */

      	if (d_count != x_count) 
	    {
	    pCmd->e_count = d_count;
	    pCmd->a_count = x_count;
	    pCmd->status = FDC_ERROR_DATACOUNT;
	    lstatus = (UINT)-1;
	    break;
            }

	/* copy internal buffer to caller's buffer */

      	if (fdcDrvDmaChannel != (UINT)-1) 
	    {
	    if (pCmd->command == FDC_READOP) 
		{
		cacheInvalidate(DATA_CACHE, (void *)pDev->dmaBuffer, d_count);
	        memcpy((void *) bufadd, (void *) pDev->dmaBuffer, d_count);
	        }
      	    }

	/*
      	 * update the logical sector number
      	 * update the accumulated data count
      	 * update the buffer address
      	 */

      	pos.lsector += (d_count / pDev->fdcType.sectorsize);
      	m_count -= d_count;
      	bufadd += d_count;
	}

    /* motor off */

    pFdc->dor &= (FDC_DOR_DMAEN|FDC_DOR_RESET);

    return (lstatus);
    }

/*******************************************************************************
*
* fdcFormat - format track/disk
*
* This function's purpose is to format the specified number
* of tracks (i.e., one or all) of the specified device.  The
* format is specified by the configuration parameters.
*
* RETURNS: 0 if OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcFormat
    (
    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 d_count, x_count;	/* data count variables */
    register UINT n_tracks;		/* number of track variable */
    register UINT lstatus;		/* local status flag */
    register UCHAR statusx;		/* status register copy */
    register UCHAR *pd_p;		/* phase data pointer */

    struct fdc_pos pos;

    UCHAR addressfieldbytes[64][4];	/* address field bytes array */

    /* setup the return status */

    lstatus = 0;

    /* setup the number of tracks to format variable */

    if (pCmd->tdflg == 'T') 
	{

        /* calculate logical sector number of selected track */

        pos.lsector =
        fdc_clsn( pCmd->blcknum, pDev->fdcType.sectorsize,
	          pDev->blockSize, pDev->fdcType.sectorstrack, 1 );

        /* take sector number to a track boundary */

        pos.lsector = 
	(pos.lsector / pDev->fdcType.sectorstrack) * pDev->fdcType.sectorstrack;

        /* check the sanity of logical sector number */

        if (pos.lsector >=
	    (pDev->fdcType.sectorstrack * pDev->fdcType.numberofheads * 
             pDev->fdcType.numberoftracks)) 
	    {
	    pCmd->status = FDC_ERROR_ILLDREQ;
	    return ((UINT)-1);
            }

        n_tracks = 1;
	} 
    else 
	{
        pos.lsector = 0;
        n_tracks = pDev->fdcType.numberoftracks * pDev->fdcType.numberofheads;
        }

    /* check state (take to known state) */

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


    /* format tracks loop */

    for (; n_tracks; n_tracks--, pos.lsector += pDev->fdcType.sectorstrack) 
	{

        /* calculate floppy disk address from logical sector number */

        fdcCP( pDev->fdcType.numberofheads,
	       pDev->fdcType.sectorstrack,
	       (struct fdc_pos *)&pos );

        /*
         * seek to track to format
         *
         * if the seek to track call fails, post error status and
         * terminate for loop
         */

        if (fdcSeek(pDev, pCmd, pFdc, cr_data_p, pos.headno, pos.cylndrno)) 
	    {
	    lstatus = (UINT)-1;
	    break;
            }

        /* setup the "format-track" 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_CP_MFM | FDC_CS_FORMATTRACK;
        pd_p[1] = pDev->driveNumber | (pos.headno ? FDC_ST0_HDS : 0);
        pd_p[2] = fdcSCode(pDev->fdcType.sectorsize);
        pd_p[3] = pDev->fdcType.sectorstrack;
        pd_p[4] = (UCHAR)pDev->fdcType.gapformat;
        pd_p[5] = 0xF6;

        /* setup the "format-track" data phase (address field bytes) */

        d_count = pDev->fdcType.sectorstrack;
        for (x_count = 0; x_count < d_count; x_count++) 
	    {
	    addressfieldbytes[x_count][0] = pos.cylndrno;
	    addressfieldbytes[x_count][1] = pos.headno;
	    addressfieldbytes[x_count][2] = pos.sectorno + x_count;
	    addressfieldbytes[x_count][3] = pd_p[2];
	    }
        d_count *= 4;


        /* setup/start the DMA controller */

        if (fdcDrvDmaChannel != (UINT)-1) 
	    {

	    /* copy format-data buffer to internal buffer */

	    memcpy((void *) pDev->dmaBuffer, (UCHAR *)&addressfieldbytes[0], d_count);
	    cacheFlush(DATA_CACHE, (void *)pDev->dmaBuffer, d_count);


	    /* initialize DMA controller for DMA data transfer */

	    isaDmaStart( fdcDrvDmaChannel,
		         I8237_MODE_TM_DEMAND,
		         I8237_MODE_TT_READ,
		         pDev->dmaBuffer, d_count );
	    }


        /* issue the "format-track" command */

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

        /* wait for interrupt, or poll for completion */

        if (fdcDrvIntVector != (UINT)-1) 
	    {
	    if (semTake(pDev->intSemId, pDev->sysClkRate * 2) == ERROR) 
		{
	        pFdc->fifo = 0x00;
	        EIEIO_SYNC;
	        isaDmaStop(fdcDrvDmaChannel);
	        x_count = 0;
		} 
	    else 
		{
	        x_count = d_count;
	        }
	    } 
	else 
	    {

	    /* poll DMA controller for completion (terminal count) */

	    if (fdcDrvDmaChannel != (UINT)-1) 
		{
	        for (x_count = 0;; x_count++) 
		    {

		    /* sleep for a bit (10ms) */

	            fdcDelay(10);

		    /* retrieve DMA controller status */

	            if ((statusx = (UCHAR)isaDmaStatus(fdcDrvDmaChannel)) == 1)
			{
		  	x_count = d_count;
		  	break;
	       		} 
		    else 
			{
		  	if (x_count >= (2000/10)) 
			    {
		     	    pFdc->fifo = 0x00;
		     	    EIEIO_SYNC;
		     	    isaDmaStop(fdcDrvDmaChannel);
		     	    x_count = 0;
		     	    break;
		  	    }
	       	   	}
	    	    }
	 	} 
	    else 
		{

	        /* 
		 * transfer data to the data FIFO, 
		 * this is the execution phase 
		 */

	        d_count = pDev->fdcType.sectorstrack * 4;
	        x_count = 
		    fdcTxdEp(pFdc, (UCHAR *)&addressfieldbytes[0], d_count);
		}
	    }

        /*
         * precondition the results phase status bytes, this is done
         * in the event the FDC timeouts while retrieving the status
         * data bytes from the results phase
         */

        pd_p = (UCHAR *)&cr_data_p->r_data.r_justdata.databytes[0];
        pd_p[0] = 0xC0 | (pos.headno << 2) | pDev->driveNumber;
        pd_p[1] = 0x14;
        pd_p[2] = 0x00;


        /* read data from data FIFO, results phase */

        fdcStat(pFdc, pd_p, sizeof(cr_data_p->r_data.r_formattrack));

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

        statusx = pd_p[0];
        if ((statusx & 0xC0) != 0x00) 
	    {
	    switch (statusx & 0xC0) 
		{
	        case 0x40:
	            statusx = pd_p[1];
	            if (statusx == 0x80) 
			{
		        statusx = pd_p[2];
		        if (statusx == 0x00) 
			    {
		     	    break;
		  	    }
	       		}
	    	case 0x80:
	    	case 0xC0:
	            lstatus = (UINT)-1;
	            break;
	 	}
	    if (lstatus) 
		{
		pCmd->status = FDC_ERROR_FORMAT;
		fdcSetAES(pCmd, pd_p, 3);
		break;
		}
	    }

        /*
         * verify the expected data count with the actual data count,
         * if they do not match, post the appropriate error status
         */

        if (d_count != x_count) 
	    {
	    pCmd->e_count = d_count;
	    pCmd->a_count = x_count;
	    pCmd->status = FDC_ERROR_DATACOUNT;
	    lstatus = (UINT)-1;
	    break;
            }
	}


    /* motor off */

    pFdc->dor &= (FDC_DOR_DMAEN|FDC_DOR_RESET);

    return (lstatus);
    }

/*******************************************************************************
*
* fdcSeek - seek to track
* 
* This function's purpose is to seek to the specified track.
* The case of Head Zero and Track Zero, a recalibrate operation
* will be performed.
* 
* RETURNS: 0 if OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcSeek
    (
    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 headno,		/* head number to seek to */
    register UINT trackno 		/* track number to seek to */
    )
    {
    register UINT lstatus;		/* local status flag */
    register UCHAR statusx;		/* status register copy */
    register UCHAR *pd_p;		/* phase data pointer */

    /* setup the return status */

    lstatus = 0;

    /*
     * if the Head Zero and Track Zero case is true, issue a
     * recalibrate, else issue a seek command
     */

    if ((headno == 0) && (trackno == 0)) {

      /* setup the "recalibrate" 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_RECALIBRATE;
      pd_p[1] = pDev->driveNumber;


      /* issue the "recalibrate" command */

      fdcExeCmd(pFdc, pd_p, sizeof(cr_data_p->c_data.c_recalibrate));
    } else {

      /* setup the "seek" 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_SEEK;
      pd_p[1] = pDev->driveNumber | (headno ? FDC_ST0_HDS : 0);
      pd_p[2] = trackno;


      /* issue the "seek" command */

      fdcExeCmd(pFdc, pd_p, sizeof(cr_data_p->c_data.c_seek) - 1);
   }


    /* wait for interrupt from the selected drive */

    for (;;) 
	{

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

        if (fdcDrvIntVector != (UINT)-1) 
	    {