fdcdrv.c

cpc-1631的BSP包for VxWorks操作系统

LOCAL STATUS fdcWrite
    (
    register FDC_DEV *pDev,		/* pointer to device descriptor */
    register UINT startBlk,		/* starting block to write */
    register UINT numBlks,		/* number of blocks to write */
    register char *pBuf 		/* pointer to buffer of send data */
    )
    {
    register UINT localStatus;	/* local status variable */

    FDC_CMDPCKT fdcCmdPacket;	/* command packet */

    /*
     * verify the request against the logical block parameters (all
     * or nothing)
     */

    if ((startBlk + pDev->blockOffset + numBlks) > pDev->blockTotal) 
	{
        return (ERROR);
	}

    /* check for a NOP request */

    if (!numBlks) 
	{
        errno = S_ioLib_DEVICE_ERROR;
        return (ERROR);
	}

    /* build command packet */

    fdcCmdPacket.command = FDC_WRITOP;
    fdcCmdPacket.status = 0;
    fdcCmdPacket.memaddr = (UINT)pBuf;
    fdcCmdPacket.blcknum = startBlk + pDev->blockOffset;
    fdcCmdPacket.nblcks = numBlks;
    fdcCmdPacket.tdflg = 0;
    fdcCmdPacket.aescount = 0;

    /* take ownership, call driver, release ownership */

    semTake(fdcDrvSemId, WAIT_FOREVER);
    localStatus = fdcDrvMain(pDev, (FDC_CMDPCKT *)&fdcCmdPacket);
    semGive(fdcDrvSemId);

    return ((localStatus ? ERROR : OK));
    }

/*******************************************************************************
*
* fdcIoctl - fdc i/o control
*
* This function's purpose is to perform the specified I/O control
* operation.
*
* RETURNS: OK, or ERROR if ioctl request failed
*/

LOCAL STATUS fdcIoctl
    (
    register FDC_DEV *pDev,	/* pointer to device descriptor */
    register int funcCode,	/* ioctl() function code */
    register int arg 		/* function-specific argument */
    )
    {
    register UINT localStatus;	/* local status variable */

    FDC_CMDPCKT fdcCmdPacket;	/* command packet */

    switch (funcCode) 
	{
        case FIODISKFORMAT:

	    /* build command packet */

	    fdcCmdPacket.command = FDC_FRMTOP;
	    fdcCmdPacket.status = 0;
	    fdcCmdPacket.memaddr = 0;
	    fdcCmdPacket.blcknum = 0;
	    fdcCmdPacket.nblcks = 0;
	    fdcCmdPacket.tdflg = 'D';
	    fdcCmdPacket.aescount = 0;

	    /* take ownership, call driver, release ownership */

	    semTake(fdcDrvSemId, WAIT_FOREVER);
	    localStatus = fdcDrvMain(pDev, (FDC_CMDPCKT *)&fdcCmdPacket);
	    semGive(fdcDrvSemId);

	    break;
        default:
	    errno = S_ioLib_UNKNOWN_REQUEST;
	    localStatus = ERROR;
	    break;
	}

    return ((localStatus ? ERROR : OK));
    }

/*******************************************************************************
*
* fdcStatusChk - fdc status check
*
* This function's purpose is to perform a status check on the
* specified device.  The status check basically checks to see
* if the disk has been changed.
*
* RETURNS: OK, or ERROR if check fails
*/

LOCAL STATUS fdcStatusChk
    (
    register FDC_DEV *pDev		/* pointer to device descriptor */
    )
    {
    register STATUS lstatus;	/* local error status */
    register FDC *pFdc;		/* FDC registers pointer */

    FDC_CMDPCKT fdcCmdPacket;	/* command packet */
    FDC_CRDATA fdc_crdata;

    /*
     * initialize pointer to FDC registers
     * initialize local error status variable
     */

    pFdc = (FDC *)fdcDrvBaseAddress;
    lstatus = OK;

    /* take ownership */

    semTake(fdcDrvSemId, WAIT_FOREVER);

    /* motor on */

    pFdc->dor |= (pDev->driveNumber | (0x10 << pDev->driveNumber));
    EIEIO_SYNC;

    /*
     * query the digital input register for a disk change status, if
     * a disk change has occurred, set the "ready change" flag
     */

    if (pFdc->dir_ccr & FDC_DIR_PS2_DSKCHG) 
	{
   
        /* set the "ready change" flag */

        pDev->fdcBlockDev.bd_readyChanged = TRUE;

        /* build command packet */

        fdcCmdPacket.command = FDC_CHCKOP;
        fdcCmdPacket.status = 0;
        fdcCmdPacket.memaddr = 0;
        fdcCmdPacket.blcknum = 0;
        fdcCmdPacket.nblcks = 0;
        fdcCmdPacket.tdflg = 0;
        fdcCmdPacket.aescount = 0;

        /* initialize interrupt handler arguments data structure */

        fdcDrvIArgs.pDev = pDev;
        fdcDrvIArgs.pCmd = (FDC_CMDPCKT *)&fdcCmdPacket;
        fdcDrvIArgs.pFdc = pFdc;
        fdcDrvIArgs.cr_data_p = (FDC_CRDATA *)&fdc_crdata;

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

        if (fdcCheck(pDev, (FDC_DEV *)&fdcCmdPacket, pFdc, fdc_crdata, 1)) 
	    {
	    lstatus = ERROR;
            }
   	}

    /* motor off */

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

    /* release ownership */

    semGive(fdcDrvSemId);

    return (lstatus);
    }

/*******************************************************************************
*
* fdcDrvMain - fdc driver main entry point
*
* This function's purpose is the main entry point into the FDC
* Floppy Disk I/O driver.
* 
* RETURNS: 0 if OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcDrvMain
    (
    register FDC_DEV *pDev,	/* device descriptor pointer */
    register FDC_CMDPCKT *pCmd 	/* command packet pointer */
    )
    {
    register UINT lstatus;	/* local error status */

    FDC_CRDATA fdc_crdata;

    /* setup local error status (error condition) */

    lstatus = (UINT)-1;

    /* initialize interrupt handler arguments data structure */

    fdcDrvIArgs.pDev = pDev;
    fdcDrvIArgs.pCmd = pCmd;
    fdcDrvIArgs.pFdc = (FDC *)fdcDrvBaseAddress;
    fdcDrvIArgs.cr_data_p = (FDC_CRDATA *)&fdc_crdata;

    /* perform the requested action */

    switch (pCmd->command) 
	{
        case FDC_READOP:
        case FDC_WRITOP:
	    /*
	     * sanity check of block size/number of blocks/physical
	     * sector size
	     */

	    if ((pCmd->command == FDC_READOP) || (pCmd->command == FDC_WRITOP)) 
		{
	        if (fdcXfrcheck(pDev, pCmd, 0)) 
		    {
	            break;
	    	    }
	 	}
	    if (!(fdcXfer(pDev, pCmd, (FDC *)fdcDrvBaseAddress, fdc_crdata))) 
		{
	        lstatus = 0;
	 	}
	    break;

	case FDC_FRMTOP:
	    if (!(fdcFormat(pDev, pCmd, (FDC *)fdcDrvBaseAddress, fdc_crdata)))
		{
	    	lstatus = 0;
	 	}
	    break;
	}
    if (lstatus) 
	errno = S_ioLib_DEVICE_ERROR;

    return (lstatus);
    }

/*******************************************************************************
*
* fdcXfer - read/write transfer
*
* This function's purpose is to perform the specified data
* transfers with the specified device.  The data requests
* are blocked to a track size, only a one track can be
* read/written at a time.
*
* RETURNS: 0 if OK, non-zero for error condition which means the status
*  word in the command packet is set.
*/

LOCAL UINT fdcXfer
    (
    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 bufadd;		/* starting address of data transfer */
    register UINT t_count, m_count;	/* data count variables */
    register UINT d_count, x_count;	/* data count variables */
    register UINT l_count;		/* loop count variable */
    register UINT lstatus;		/* local status flag */
    register UCHAR statusx;		/* status register copy */
    register UINT e_sector;		/* ending sector numbner */
    register UCHAR *pd_p;		/* phase data pointer */

    struct fdc_pos pos;

    /* setup the return status */

    lstatus = 0;

    /*
     * calculate starting logical sector number of data
     * transfer, this is only done once
     */

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

    /*
     * calculate the number of bytes in a track, this is the
     * maximum number of bytes that can be transferred at one
     * given time
     *
     * calculate the number of bytes that the user specified
     */

    t_count = pDev->fdcType.sectorsize * pDev->fdcType.sectorstrack;
    m_count = pCmd->nblcks * pDev->blockSize;

    /* check the sanity of number of blocks requested */

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

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

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

    /*
     * setup starting address of data transfer
     * read/write data tracks loop
     */

    bufadd = pCmd->memaddr;
    for (l_count = 0; m_count; l_count++) 
	{
   
      	/* calculate floppy disk address from logical sector number */

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

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

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

      	/*
         * calculate the size (number of bytes) for this transfer
         * calculate the ending sector number for this transfer
         */

      	if (pos.sectorno > 1) 
	    {
	    d_count = 
		((pDev->fdcType.sectorstrack - 
		 (pos.sectorno - 1)) * pDev->fdcType.sectorsize);
	    if (d_count >= m_count) 
		{
	    	d_count = m_count;
	    	e_sector = 
		    ((d_count / pDev->fdcType.sectorsize) - 1) + pos.sectorno;
	 	} 
	    else 
		{
	        e_sector = pDev->fdcType.sectorstrack;
	 	}
      	    } 
	else 
	    {
	    if (m_count >= t_count) 
		{
	        d_count = t_count;
	 	} 
	    else 
		{
	        d_count = m_count;
		}
	   e_sector = d_count / pDev->fdcType.sectorsize;
      	   }

      	/* setup the "read-data" or "write-data" 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 | 
		  ((pCmd->command == FDC_READOP) ? 
		   FDC_CS_READDATA : FDC_CS_WRITEDATA);
      
        pd_p[1] = pDev->driveNumber | (pos.headno ? FDC_ST0_HDS : 0);
        pd_p[2] = pos.cylndrno;
        pd_p[3] = pos.headno;
        pd_p[4] = pos.sectorno;
        pd_p[5] = fdcSCode(pDev->fdcType.sectorsize);
        pd_p[6] = (UCHAR)e_sector;
        pd_p[7] = (UCHAR)pDev->fdcType.gaprw;
        pd_p[8] = 0xFF;

        /* setup/start DMA controller */

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

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

	    if (pCmd->command == FDC_WRITOP) 
		{
	        memcpy((void *) pDev->dmaBuffer, (void *) bufadd, d_count);
	        cacheFlush(DATA_CACHE, (void *)pDev->dmaBuffer, d_count);
	 	}

	    /* initialize DMA controller for DMA data transfer */

	    isaDmaStart( fdcDrvDmaChannel,
		         I8237_MODE_TM_DEMAND,
		         ((pCmd->command == FDC_READOP) ? 
			  I8237_MODE_TT_WRITE : I8237_MODE_TT_READ),
		         pDev->dmaBuffer, d_count );
	    }

	/* issue the "read-data" or the "write-data" command */

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

      	/* 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)) 
				{