atadrv.c

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

		     (FUNCPTR)ataWdog, ctrl);
            while ((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_BUSY) && 
		   (pCtrl->wdgOkay))
            taskDelay(0);

            while ((ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_DRQ) 
                   && (pCtrl->wdgOkay))
	                   taskDelay(0);

	    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 &&
                   ATA_IO_BYTE_READ (pCtrl->aStatus) & ATA_STAT_BUSY)
	        printErr("waiting for not busy and DRQ 0x%x\n"
			          ,ATA_IO_BYTE_READ (pCtrl->aStatus));
	    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.
*/

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);

    ATA_IO_BYTE_WRITE (pCtrl->dControl,
                       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);
   ATA_IO_BYTE_WRITE (pCtrl->dControl,  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);
     ATA_IO_BYTE_WRITE (pCtrl->dControl, 0);
   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 */

    //
    // reset causes interrupt 
    //
    semTake (&pCtrl->syncSem, NO_WAIT);


    /* 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 arg0=0x%x arg1=0x%x\n", ctrl, drive, cmd,arg0,arg1);
	printErr ("drive << 4, 0x%x\n",drive << 4);
#endif	/* ATA_DEBUG */

    while (retry)
	{
        ataWait (ctrl, ATA_STAT_READY);
        ATA_IO_BYTE_WRITE (pCtrl->sdh,  (drive << 4));
        taskDelay(0);
        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);
			int IdeStatus = sysInByte(busMasterRegs[ctrl] + 2);
			char picstatus=0;
			UINT32 intStatus0 = ReadRegisterU32(0x4060028);
		    sysOutByte(0xa0,3);
			picstatus = sysInByte(0xa0);
	    printErr ("ataCmd err: status=0x%x semStatus=%d err=0x%x,IdeStatus=0x%x,pic=0x%x,RomInt=0x%x\n", 
		      pCtrl->intStatus, semStatus, error,IdeStatus,picstatus,intStatus0);
		    sysOutByte(0xa0,3);
			picstatus = sysInByte(0xa0);
#endif	/* ATA_DEBUG */
	    if (++retryCount > ataRetry)
	        return (ERROR);
	    }
	else
	    retry = FALSE;
	}

//    ATA_IO_BYTE_READ (pCtrl->status);

	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);
	printErr ("drive << 4,0x%x\n",(drive<<4));
#endif	/* ATA_DEBUG */

    while (retry)
	{
        ataWait (ctrl, ATA_STAT_READY);
        ATA_IO_BYTE_WRITE (pCtrl->sdh, (drive << 4));

        taskDelay(0);
        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;
        }
//    ATA_IO_BYTE_READ (pCtrl->status);

    ataWait (ctrl, ATA_STAT_DRQ);

    ATA_IO_NWORD_READ_SWAP (pCtrl->data, (UINT16 *)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;
    UINT16 *pBuf;


    //
    // If dma enabled then override this function
    //
    if (pDrive->okDma)
      return ataDmaRW(ctrl,drive, cylinder, head, sector,buffer,nSecs,direction);


#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);
    
    ATA_IO_BYTE_WRITE (pCtrl->sdh,
                           (drive << 4));

    ataWait (ctrl, ATA_STAT_READY);

    pBuf = (UINT16 *)buffer;

    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);

    taskDelay(0); 
    ataWait (ctrl, ATA_STAT_DRQ);

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

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

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

	           goto errorRW;
            }
        

	        pBuf     += nWords;
	        nSectors -= block;
            if (nSectors > 0) { 
             ataWait (ctrl, ATA_STAT_DRQ);

            }
 
//            ATA_IO_BYTE_READ (pCtrl->status);        

            }
	}
    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_DRQ);

//        ATA_IO_BYTE_READ (pCtrl->status);


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