atadrv.c

用于EQUATOR处理器上的FAT32文件系统（vxWorks5.5）

	retryRW1 = 0;
	retryRW0 = 0;
	while (ataRW(pDev->ctrl, pDev->drive, cylinder, head, sector, 
	       pBuf, nSecs, direction) != OK)
	    {
	    if (++retryRW0 > ataRetry)
		{
	        (void)ataCmd (pDev->ctrl, pDev->drive, ATA_CMD_RECALIB, NULL,
			      NULL);
	        if (++retryRW1 > ataRetry)
		    goto done;
	        retrySeek = 0;
	        while (ataCmd (pDev->ctrl, pDev->drive, ATA_CMD_SEEK, cylinder,
			       head) != OK)
	            if (++retrySeek > ataRetry)
		        goto done;
	        retryRW0 = 0;
		}
	    }

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

    status = OK;

done:
    if (status == ERROR)
        (void)errnoSet (S_ioLib_DEVICE_ERROR);
    semGive (&pCtrl->muteSem);
    return (status);
    }

/*******************************************************************************
*
* ataIntr - ATA/IDE controller interrupt handler.
*
* RETURNS: N/A
*/

LOCAL void ataIntr
    (
    int ctrl
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];

    pCtrl->intCount++;
    pCtrl->intStatus = ATA_IO_BYTE_READ (pCtrl->status);
    semGive (&pCtrl->syncSem);
    }

/*******************************************************************************
*
* ataWdog - ATA/IDE controller watchdog handler.
*
* RETURNS: N/A
*/

LOCAL void ataWdog
    (
    int ctrl
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];

    pCtrl->wdgOkay = FALSE;
    }

/*******************************************************************************
*
* ataWait - wait the drive ready
*
* Wait the drive ready
*
* RETURNS: OK, ERROR if the drive didn't become ready in certain period of time.
*/

LOCAL void ataWait
    (
    int ctrl,
    int request
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];

#ifdef	ATA_DEBUG
    printErr ("ataWait: ctrl=%d  request=0x%x\n", ctrl, request);
#endif	/* ATA_DEBUG */

    switch (request)
	{
	case ATA_STAT_READY:
	    wdStart (pCtrl->wdgId, (sysClkRateGet() * pCtrl->wdgTimeout), 
		     (FUNCPTR)ataWdog, ctrl);
            while ((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_BUSY) && 
		   (pCtrl->wdgOkay))
	        ;
            while (((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_READY) == 0)
                   && (pCtrl->wdgOkay))
	        ;
	    wdCancel (pCtrl->wdgId);

            if (!pCtrl->wdgOkay)
	        {
	        pCtrl->wdgOkay = TRUE;
	        (void) ataInit (ctrl);
	        }
	    break;

	case ATA_STAT_BUSY:
            while (ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_BUSY)
		;
	    break;

	case ATA_STAT_DRQ:
            while ((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_DRQ) == 0)
	        ;
	    break;

	case ATA_STAT_SEEKCMPLT:
            while ((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_SEEKCMPLT) == 0)
	        ;
	    break;
	}

#ifdef	ATA_DEBUG
    printErr ("ataWait end:\n");
#endif	/* ATA_DEBUG */
    }

/*******************************************************************************
*
* ataInit - init a ATA/IDE disk controller
*
* This routine initializes a ATA/IDE disk controller.
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS ataInit
    (
    int ctrl
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];
    int ix;

#ifdef	ATA_DEBUG
    printErr ("ataInit: ctrl=%d\n", ctrl);
#endif	/* ATA_DEBUG */

    ATA_IO_BYTE_WRITE (pCtrl->dControl,
                       ATA_CTL_4BIT | ATA_CTL_RST | ATA_CTL_IDS);
    for (ix = 0; ix < 100; ix++)
        sysDelay ();

    ATA_IO_BYTE_WRITE (pCtrl->dControl, ATA_CTL_4BIT | ATA_CTL_IDS);
    for (ix = 0; ix < 100; ix++)
        sysDelay ();

    pCtrl->wdgOkay = TRUE;

    /* start the ata  watchdog */

    wdStart (pCtrl->wdgId, (sysClkRateGet() * pCtrl->wdgTimeout),
	     (FUNCPTR)ataWdog, ctrl);

    while ((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_BUSY) &&
           (pCtrl->wdgOkay))
	;

    ATA_IO_BYTE_WRITE (pCtrl->dControl, ATA_CTL_4BIT);
   for (ix = 0; ix < 100; ix++) 
        sysDelay ();

    while (((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_READY) == 0) &&
	   (pCtrl->wdgOkay))
	;

    wdCancel (pCtrl->wdgId);

    if (!pCtrl->wdgOkay)
        {
#ifdef	ATA_DEBUG
	printErr ("ataInit error:\n");
#endif	/* ATA_DEBUG */
        pCtrl->wdgOkay = TRUE;
        return (ERROR);
        }
#ifdef  ATA_DEBUG
        printErr ("ataInit Calling sysAtaInit (if present):\n");
#endif  /* ATA_DEBUG */

    /* Call out to bsp specific setup routine */
    SYS_ATA_INIT_RTN (ctrl);

#ifdef  ATA_DEBUG
        printErr ("ataInit sysAtaInit returned:\n");
#endif  /* ATA_DEBUG */

#ifdef	ATA_DEBUG
    printErr ("ataInit end:\n");
#endif	/* ATA_DEBUG */

    return (OK);
    }

/*******************************************************************************
*
* ataCmd - issue non data command
*
* Issue a non data command.  Non data commands have the same protocol.
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS ataCmd
    (
    int ctrl,
    int drive,
    int cmd,
    int arg0,
    int arg1
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];
    ATA_TYPE *pType	= &ataTypes[ctrl][drive];
    BOOL retry		= TRUE;
    int retryCount	= 0;
    int semStatus;

#ifdef	ATA_DEBUG
    printErr ("ataCmd: ctrl=%d drive=%d cmd=0x%x\n", ctrl, drive, cmd);
#endif	/* ATA_DEBUG */

    while (retry)
	{
        ataWait (ctrl, ATA_STAT_READY);

	switch (cmd)
	    {
	    case ATA_CMD_DIAGNOSE:
        	ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4));
		break;
	    case ATA_CMD_INITP:
		ATA_IO_BYTE_WRITE (pCtrl->cylLo, pType->cylinders);
		ATA_IO_BYTE_WRITE (pCtrl->cylHi, pType->cylinders >> 8);
		ATA_IO_BYTE_WRITE (pCtrl->seccnt, pType->sectors);
		ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4) | 
			    ((pType->heads - 1) & 0x0f));
		break;
	    case ATA_CMD_RECALIB:
		ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4));
		break;
	    case ATA_CMD_SEEK:
		ATA_IO_BYTE_WRITE (pCtrl->cylLo, arg0);
		ATA_IO_BYTE_WRITE (pCtrl->cylHi, arg0>>8);
		ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4) |
			    (arg1 & 0xf));
		break;
	    case ATA_CMD_SET_FEATURE:
        	ATA_IO_BYTE_WRITE (pCtrl->seccnt, arg1);
        	ATA_IO_BYTE_WRITE (pCtrl->feature, arg0);
        	ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4));
		break;
	    case ATA_CMD_SET_MULTI:
        	ATA_IO_BYTE_WRITE (pCtrl->seccnt, arg0);
        	ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4));
		break;
	    }

        ATA_IO_BYTE_WRITE (pCtrl->command, cmd);
        semStatus = semTake (&pCtrl->syncSem, 
			     sysClkRateGet() * pCtrl->semTimeout);

        if ((pCtrl->intStatus & ATA_STAT_ERR) || (semStatus == ERROR))
	    {
#ifdef	ATA_DEBUG
            int error = ATA_IO_BYTE_READ(pCtrl->error);
	    printErr ("ataCmd err: status=0x%x semStatus=%d err=0x%x\n", 
		      pCtrl->intStatus, semStatus, error);
#endif	/* ATA_DEBUG */
	    if (++retryCount > ataRetry)
	        return (ERROR);
	    }
	else
	    retry = FALSE;
	}

	switch (cmd)
	    {
	    case ATA_CMD_SEEK:
		ataWait (ctrl, ATA_STAT_SEEKCMPLT);
		break;
	    }

#ifdef	ATA_DEBUG
    printErr ("ataCmd end:\n");
#endif	/* ATA_DEBUG */

    return (OK);
    }

/*******************************************************************************
*
* ataPread - Read drive parameters
*
* Read drive parameters.
*
* RETURNS: OK, ERROR if the command didn't succeed.
*/

LOCAL STATUS ataPread
    (
    int ctrl,
    int drive,
    void *buffer
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];
    BOOL retry		= TRUE;
    int retryCount	= 0;
    int semStatus;

#ifdef	ATA_DEBUG
    printErr ("ataPread: ctrl=%d drive=%d\n", ctrl, drive);
#endif	/* ATA_DEBUG */

    while (retry)
	{
        ataWait (ctrl, ATA_STAT_READY);

        ATA_IO_BYTE_WRITE (pCtrl->sdh, ATA_SDH_IBM | (drive << 4));
        ATA_IO_BYTE_WRITE (pCtrl->command, ATA_CMD_READP);
        semStatus = semTake (&pCtrl->syncSem, 
			     sysClkRateGet() * pCtrl->semTimeout);

        if ((pCtrl->intStatus & ATA_STAT_ERR) || (semStatus == ERROR))
	    {
#ifdef	ATA_DEBUG
	    int error	= ATA_IO_BYTE_READ (pCtrl->error);
	    int status	= ATA_IO_BYTE_READ (pCtrl->aStatus);
            printErr ("ataPread err: stat=0x%x 0x%x semStatus=%d err=0x%x\n",
	              pCtrl->intStatus, status, semStatus, error);
#endif	/* ATA_DEBUG */
	    if (++retryCount > ataRetry)
	        return (ERROR);
	    }
        else
	    retry = FALSE;
        }

    ataWait (ctrl, ATA_STAT_DRQ);

    ATA_IO_NWORD_READ_SWAP (pCtrl->data, (short *)buffer, 256);

#ifdef	ATA_DEBUG
    printErr ("ataPread end:\n");
#endif	/* ATA_DEBUG */

    return (OK);
    }

/*******************************************************************************
*
* ataRW - 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 ataRW
    (
    int ctrl,
    int drive,
    int cylinder,
    int head,
    int sector,
    void *buffer,
    int nSecs,
    int direction
    )
    {
    ATA_CTRL *pCtrl	= &ataCtrl[ctrl];
    ATA_DRIVE *pDrive	= &pCtrl->drive[drive];
    ATA_TYPE *pType	= &ataTypes[ctrl][drive];
    int retryCount	= 0;
    int block		= 1;
    int nSectors	= nSecs;
    int nWords;
    int semStatus;
    short *pBuf;

#ifdef	ATA_DEBUG
    printErr ("ataRW: ctrl=%d drive=%d c=%d h=%d s=%d buf=0x%x n=%d dir=%d\n",
              ctrl, drive, cylinder, head, sector, 
	      (int)buffer, nSecs, direction);
#endif	/* ATA_DEBUG */

retryRW:
    ataWait (ctrl, ATA_STAT_READY);

    pBuf = (short *)buffer;
    ATA_IO_BYTE_WRITE (pCtrl->feature, pType->precomp);
    ATA_IO_BYTE_WRITE (pCtrl->seccnt, nSecs);
    ATA_IO_BYTE_WRITE (pCtrl->sector, sector);
    ATA_IO_BYTE_WRITE (pCtrl->cylLo, cylinder);
    ATA_IO_BYTE_WRITE (pCtrl->cylHi, cylinder>>8);

    if (pDrive->okLba)
        ATA_IO_BYTE_WRITE (pCtrl->sdh,
                           ATA_SDH_LBA | (drive << 4) | (head & 0xf));
    else
        ATA_IO_BYTE_WRITE (pCtrl->sdh,
                           ATA_SDH_IBM | (drive << 4) | (head & 0xf));

    if (pDrive->rwPio == ATA_PIO_MULTI)
	block = pDrive->multiSecs;

    nWords = (pType->bytes * block) >> 1;

    if (direction == O_WRONLY)
	{
        if (pDrive->rwPio == ATA_PIO_MULTI)
	    ATA_IO_BYTE_WRITE (pCtrl->command, ATA_CMD_WRITE_MULTI);
	else
	    ATA_IO_BYTE_WRITE (pCtrl->command, ATA_CMD_WRITE);

	while (nSectors > 0)
	    {
	    if ((pDrive->rwPio == ATA_PIO_MULTI) && (nSectors < block))
		{
		block = nSectors;
		nWords = (pType->bytes * block) >> 1;
		}

            ataWait (ctrl, ATA_STAT_BUSY);
            ataWait (ctrl, ATA_STAT_DRQ);

	    if (pDrive->rwBits == ATA_BITS_16)
	        ATA_IO_NWORD_WRITE (pCtrl->data, pBuf, nWords);
	    else
	        ATA_IO_NLONG_WRITE (pCtrl->data, (long *)pBuf, nWords >> 1);

            semStatus = semTake (&pCtrl->syncSem, 
				 sysClkRateGet() * pCtrl->semTimeout);
    	    if ((pCtrl->intStatus & ATA_STAT_ERR) || (semStatus == ERROR))
	        goto errorRW;

	    pBuf     += nWords;
	    nSectors -= block;
            }
	}
    else
	{
        if (pDrive->rwPio == ATA_PIO_MULTI)
	    ATA_IO_BYTE_WRITE (pCtrl->command, ATA_CMD_READ_MULTI);
	else
	    ATA_IO_BYTE_WRITE (pCtrl->command, ATA_CMD_READ);

	while (nSectors > 0)
	    {
	    if ((pDrive->rwPio == ATA_PIO_MULTI) && (nSectors < block))
		{
		block = nSectors;
		nWords = (pType->bytes * block) >> 1;
		}

            semStatus = semTake (&pCtrl->syncSem, 
				 sysClkRateGet() * pCtrl->semTimeout);
    	    if ((pCtrl->intStatus & ATA_STAT_ERR) || (semStatus == ERROR))
	        goto errorRW;

            ataWait (ctrl, ATA_STAT_BUSY); /* wait for slow disk */
            ataWait (ctrl, ATA_STAT_DRQ);

	    if (pDrive->rwBits == ATA_BITS_16)
	        ATA_IO_NWORD_READ (pCtrl->data, pBuf, nWords);
	    else
	        ATA_IO_NLONG_READ (pCtrl->data, (long *)pBuf, nWords >> 1);

	    pBuf     += nWords;
	    nSectors -= block;
	    }
	}

#ifdef	ATA_DEBUG
    printErr ("ataRW: end\n");
#endif	/* ATA_DEBUG */

    return (OK);

errorRW:
#ifdef	ATA_DEBUG
    {
    int error	= ATA_IO_BYTE_READ (pCtrl->error);
    int status	= ATA_IO_BYTE_READ (pCtrl->aStatus);
    printErr ("ataRW err: stat=0x%x 0x%x semStatus=%d error=0x%x\n",
	      pCtrl->intStatus, status, semStatus, error);
    }
#endif	/* ATA_DEBUG */
    if (++retryCount < ataRetry)
	goto retryRW;
    return (ERROR);
    }