pf.c

上传linux-jx2410的源代码

#define LUN             (0x20*PF.lun)
#define DRIVE           (0xa0+0x10*PF.drive)

static int pf_wait( int unit, int go, int stop, char * fun, char * msg )

{       int j, r, e, s, p;

        j = 0;
        while ((((r=RR(1,6))&go)||(stop&&(!(r&stop))))&&(j++<PF_SPIN))
                udelay(PF_SPIN_DEL);

        if ((r&(STAT_ERR&stop))||(j>=PF_SPIN)) {
           s = RR(0,7);
           e = RR(0,1);
           p = RR(0,2);
           if (j >= PF_SPIN) e |= 0x100;
           if (fun) printk("%s: %s %s: alt=0x%x stat=0x%x err=0x%x"
                           " loop=%d phase=%d\n",
                            PF.name,fun,msg,r,s,e,j,p);
           return (e<<8)+s;
        }
        return 0;
}

static int pf_command( int unit, char * cmd, int dlen, char * fun )

{       pi_connect(PI);

        WR(0,6,DRIVE);

        if (pf_wait(unit,STAT_BUSY|STAT_DRQ,0,fun,"before command")) {
                pi_disconnect(PI);
                return -1;
        }

        WR(0,4,dlen % 256);
        WR(0,5,dlen / 256);
        WR(0,7,0xa0);          /* ATAPI packet command */

        if (pf_wait(unit,STAT_BUSY,STAT_DRQ,fun,"command DRQ")) {
                pi_disconnect(PI);
                return -1;
        }

        if (RR(0,2) != 1) {
           printk("%s: %s: command phase error\n",PF.name,fun);
           pi_disconnect(PI);
           return -1;
        }

        pi_write_block(PI,cmd,12);

        return 0;
}

static int pf_completion( int unit, char * buf, char * fun )

{       int r, s, n;

        r = pf_wait(unit,STAT_BUSY,STAT_DRQ|STAT_READY|STAT_ERR,
			fun,"completion");

        if ((RR(0,2)&2) && (RR(0,7)&STAT_DRQ)) { 
                n = (((RR(0,4)+256*RR(0,5))+3)&0xfffc);
                pi_read_block(PI,buf,n);
        }

        s = pf_wait(unit,STAT_BUSY,STAT_READY|STAT_ERR,fun,"data done");

        pi_disconnect(PI); 

        return (r?r:s);
}

static void pf_req_sense( int unit, int quiet )

{       char    rs_cmd[12] = { ATAPI_REQ_SENSE,LUN,0,0,16,0,0,0,0,0,0,0 };
        char    buf[16];
        int     r;

        r = pf_command(unit,rs_cmd,16,"Request sense");
        mdelay(1);
        if (!r) pf_completion(unit,buf,"Request sense");

        if ((!r)&&(!quiet)) 
                printk("%s: Sense key: %x, ASC: %x, ASQ: %x\n",
                       PF.name,buf[2]&0xf,buf[12],buf[13]);
}

static int pf_atapi( int unit, char * cmd, int dlen, char * buf, char * fun )

{       int r;

        r = pf_command(unit,cmd,dlen,fun);
        mdelay(1);
        if (!r) r = pf_completion(unit,buf,fun);
        if (r) pf_req_sense(unit,!fun);
        
        return r;
}

#define DBMSG(msg)      ((verbose>1)?(msg):NULL)

static void pf_lock(int unit, int func)

{	char	lo_cmd[12] = { ATAPI_LOCK,LUN,0,0,func,0,0,0,0,0,0,0 };

        pf_atapi(unit,lo_cmd,0,pf_scratch,func?"unlock":"lock");
}


static void pf_eject( int unit )

{	char	ej_cmd[12] = { ATAPI_DOOR,LUN,0,0,2,0,0,0,0,0,0,0 };

	pf_lock(unit,0);
	pf_atapi(unit,ej_cmd,0,pf_scratch,"eject");
}

#define PF_RESET_TMO   30              /* in tenths of a second */

static void pf_sleep( int cs )

{       current->state = TASK_INTERRUPTIBLE;
        schedule_timeout(cs);
}


static int pf_reset( int unit )

/* the ATAPI standard actually specifies the contents of all 7 registers
   after a reset, but the specification is ambiguous concerning the last
   two bytes, and different drives interpret the standard differently.
*/

{	int	i, k, flg;
	int	expect[5] = {1,1,1,0x14,0xeb};

	pi_connect(PI);
	WR(0,6,DRIVE);
	WR(0,7,8);

	pf_sleep(20*HZ/1000);

        k = 0;
        while ((k++ < PF_RESET_TMO) && (RR(1,6)&STAT_BUSY))
                pf_sleep(HZ/10);

	flg = 1;
	for(i=0;i<5;i++) flg &= (RR(0,i+1) == expect[i]);

	if (verbose) {
		printk("%s: Reset (%d) signature = ",PF.name,k);
		for (i=0;i<5;i++) printk("%3x",RR(0,i+1));
		if (!flg) printk(" (incorrect)");
		printk("\n");
	}
	
	pi_disconnect(PI);
	return flg-1;	
}

static void pf_mode_sense( int unit )

{       char    ms_cmd[12] = { ATAPI_MODE_SENSE,LUN,0,0,0,0,0,0,8,0,0,0};
	char	buf[8];

        pf_atapi(unit,ms_cmd,8,buf,DBMSG("mode sense"));
	PF.media_status = PF_RW;
	if (buf[3] & 0x80) PF.media_status = PF_RO;
}

static void xs( char *buf, char *targ, int offs, int len )

{	int	j,k,l;

	j=0; l=0;
	for (k=0;k<len;k++) 
	   if((buf[k+offs]!=0x20)||(buf[k+offs]!=l))
		l=targ[j++]=buf[k+offs];
	if (l==0x20) j--;
	targ[j]=0;
}

static int xl( char *buf, int offs )

{	int	v,k;

	v=0; 
	for(k=0;k<4;k++) v=v*256+(buf[k+offs]&0xff);
	return v;
}

static void pf_get_capacity( int unit )

{	char    rc_cmd[12] = { ATAPI_CAPACITY,LUN,0,0,0,0,0,0,0,0,0,0};
	char	buf[8];
        int 	bs;

	if (pf_atapi(unit,rc_cmd,8,buf,DBMSG("get capacity"))) {
		PF.media_status = PF_NM;
		return;
	}
	PF.capacity = xl(buf,0) + 1;  
	bs = xl(buf,4);
	if (bs != 512) {
		PF.capacity = 0;
		if (verbose) printk("%s: Drive %d, LUN %d,"
		       		    " unsupported block size %d\n",
			            PF.name,PF.drive,PF.lun,bs);
		}
}

static int pf_identify( int unit )

{	int 	dt, s;
	char	*ms[2] = {"master","slave"};
	char	mf[10], id[18];
	char    id_cmd[12] = { ATAPI_IDENTIFY,LUN,0,0,36,0,0,0,0,0,0,0};
	char	buf[36];

        s = pf_atapi(unit,id_cmd,36,buf,"identify");
	if (s) return -1;

	dt = buf[0] & 0x1f;
	if ((dt != 0) && (dt != 7)) {
	  	if (verbose) 
		   printk("%s: Drive %d, LUN %d, unsupported type %d\n",
				PF.name,PF.drive,PF.lun,dt);
	  	return -1;
       	}

	xs(buf,mf,8,8);
	xs(buf,id,16,16);

	PF.removable = (buf[1] & 0x80);

	pf_mode_sense(unit);
	pf_mode_sense(unit);
	pf_mode_sense(unit);

	pf_get_capacity(unit);

        printk("%s: %s %s, %s LUN %d, type %d",
		PF.name,mf,id,ms[PF.drive],PF.lun,dt);
        if (PF.removable) printk(", removable");
        if (PF.media_status == PF_NM) 
                printk(", no media\n");
        else {  if (PF.media_status == PF_RO) printk(", RO");
                printk(", %d blocks\n",PF.capacity);
        }

	return 0;
}

static int pf_probe( int unit )

/*	returns  0, with id set if drive is detected
	        -1, if drive detection failed
*/

{	if (PF.drive == -1) {
	   for (PF.drive=0;PF.drive<=1;PF.drive++)
		if (!pf_reset(unit)) {
		   if (PF.lun != -1) return pf_identify(unit);
		   else for (PF.lun=0;PF.lun<8;PF.lun++) 
                           if (!pf_identify(unit)) return 0;
		}
	} else {
	   if (pf_reset(unit)) return -1;
	   if (PF.lun != -1) return pf_identify(unit);
	   for (PF.lun=0;PF.lun<8;PF.lun++) 
	      if (!pf_identify(unit)) return 0;
	}
        return -1; 
}

static int pf_detect( void )

{	int	k, unit;

	printk("%s: %s version %s, major %d, cluster %d, nice %d\n",
		name,name,PF_VERSION,major,cluster,nice);

	k = 0;
	if (pf_drive_count == 0) {
	    unit = 0;
	    if (pi_init(PI,1,-1,-1,-1,-1,-1,pf_scratch,
                        PI_PF,verbose,PF.name)) {
	        if (!pf_probe(unit)) {
			PF.present = 1;
			k++;
	        } else pi_release(PI);
	    }

	} else for (unit=0;unit<PF_UNITS;unit++) if (DU[D_PRT])
	    if (pi_init(PI,0,DU[D_PRT],DU[D_MOD],DU[D_UNI],
			DU[D_PRO],DU[D_DLY],pf_scratch,PI_PF,verbose,
			PF.name)) { 
                if (!pf_probe(unit)) {
                        PF.present = 1;
                        k++;
                } else pi_release(PI);
            }

	if (k) return 0;

	printk("%s: No ATAPI disk detected\n",name);
	return -1;
}

/* The i/o request engine */

static int pf_start( int unit, int cmd, int b, int c )

{	int	i;
	char	io_cmd[12] = {cmd,LUN,0,0,0,0,0,0,0,0,0,0};

	for(i=0;i<4;i++) { 
	   io_cmd[5-i] = b & 0xff;
	   b = b >> 8;
	}
	
	io_cmd[8] = c & 0xff;
	io_cmd[7] = (c >> 8) & 0xff;

	i = pf_command(unit,io_cmd,c*512,"start i/o");

        mdelay(1);

	return i;	
}

static int pf_ready( void )

{	int	unit = pf_unit;

	return (((RR(1,6)&(STAT_BUSY|pf_mask)) == pf_mask));
}

static void do_pf_request (request_queue_t * q)

{       struct buffer_head * bh;
	int unit;

        if (pf_busy) return;
repeat:
        if (QUEUE_EMPTY || (CURRENT->rq_status == RQ_INACTIVE)) return;
        INIT_REQUEST;

        pf_unit = unit = DEVICE_NR(CURRENT->rq_dev);
        pf_block = CURRENT->sector;
        pf_run = CURRENT->nr_sectors;
        pf_count = CURRENT->current_nr_sectors;

	bh = CURRENT->bh;

        if ((pf_unit >= PF_UNITS) || (pf_block+pf_count > PF.capacity)) {
                end_request(0);
                goto repeat;
        }

	pf_cmd = CURRENT->cmd;
        pf_buf = CURRENT->buffer;
        pf_retries = 0;

	pf_busy = 1;
        if (pf_cmd == READ) pi_do_claimed(PI,do_pf_read);
        else if (pf_cmd == WRITE) pi_do_claimed(PI,do_pf_write);
        else {  pf_busy = 0;
		end_request(0);
                goto repeat;
        }
}

static void pf_next_buf( int unit )

{	long	saved_flags;

	spin_lock_irqsave(&io_request_lock,saved_flags);
	end_request(1);
	if (!pf_run) { spin_unlock_irqrestore(&io_request_lock,saved_flags);
		       return; 
	}
	
/* paranoia */

	if (QUEUE_EMPTY ||
	    (CURRENT->cmd != pf_cmd) ||
	    (DEVICE_NR(CURRENT->rq_dev) != pf_unit) ||
	    (CURRENT->rq_status == RQ_INACTIVE) ||
	    (CURRENT->sector != pf_block)) 
		printk("%s: OUCH: request list changed unexpectedly\n",
			PF.name);

	pf_count = CURRENT->current_nr_sectors;
	pf_buf = CURRENT->buffer;
	spin_unlock_irqrestore(&io_request_lock,saved_flags);
}

static void do_pf_read( void )

/* detach from the calling context - in case the spinlock is held */

{	ps_set_intr(do_pf_read_start,0,0,nice);
}

static void do_pf_read_start( void )

{       int	unit = pf_unit;
	long	saved_flags;

	pf_busy = 1;

	if (pf_start(unit,ATAPI_READ_10,pf_block,pf_run)) {
                pi_disconnect(PI);
                if (pf_retries < PF_MAX_RETRIES) {
                        pf_retries++;
                        pi_do_claimed(PI,do_pf_read_start);
			return;
                }
		spin_lock_irqsave(&io_request_lock,saved_flags);
                end_request(0);
                pf_busy = 0;
		do_pf_request(NULL);
		spin_unlock_irqrestore(&io_request_lock,saved_flags);
                return;
        }
	pf_mask = STAT_DRQ;
        ps_set_intr(do_pf_read_drq,pf_ready,PF_TMO,nice);
}

static void do_pf_read_drq( void )

{       int	unit = pf_unit;
	long	saved_flags;
	
	while (1) {
            if (pf_wait(unit,STAT_BUSY,STAT_DRQ|STAT_ERR,
			"read block","completion") & STAT_ERR) {
                pi_disconnect(PI);
                if (pf_retries < PF_MAX_RETRIES) {
			pf_req_sense(unit,0);
                        pf_retries++;
                        pi_do_claimed(PI,do_pf_read_start);
                        return;
                }
		spin_lock_irqsave(&io_request_lock,saved_flags);
                end_request(0);
                pf_busy = 0;
		do_pf_request(NULL);
		spin_unlock_irqrestore(&io_request_lock,saved_flags);
                return;
            }
            pi_read_block(PI,pf_buf,512);
            pf_count--; pf_run--;
            pf_buf += 512;
	    pf_block++;
	    if (!pf_run) break;
	    if (!pf_count) pf_next_buf(unit);
        }
        pi_disconnect(PI);
	spin_lock_irqsave(&io_request_lock,saved_flags); 
        end_request(1);
        pf_busy = 0;
	do_pf_request(NULL);
	spin_unlock_irqrestore(&io_request_lock,saved_flags);
}

static void do_pf_write( void )

{	ps_set_intr(do_pf_write_start,0,0,nice);
}

static void do_pf_write_start( void )

{       int	unit = pf_unit;
	long	saved_flags;

	pf_busy = 1;

	if (pf_start(unit,ATAPI_WRITE_10,pf_block,pf_run)) {
                pi_disconnect(PI);
                if (pf_retries < PF_MAX_RETRIES) {
                        pf_retries++;
                        pi_do_claimed(PI,do_pf_write_start);
			return;
                }
		spin_lock_irqsave(&io_request_lock,saved_flags);
                end_request(0);
                pf_busy = 0;
		do_pf_request(NULL);
		spin_unlock_irqrestore(&io_request_lock,saved_flags);
                return;
        }

	while (1) {
            if (pf_wait(unit,STAT_BUSY,STAT_DRQ|STAT_ERR,
			"write block","data wait") & STAT_ERR) {
                pi_disconnect(PI);
                if (pf_retries < PF_MAX_RETRIES) {
                        pf_retries++;
                        pi_do_claimed(PI,do_pf_write_start);
                        return;
                }
		spin_lock_irqsave(&io_request_lock,saved_flags);
                end_request(0);
                pf_busy = 0;
		do_pf_request(NULL);
		spin_unlock_irqrestore(&io_request_lock,saved_flags);
                return;
            }
            pi_write_block(PI,pf_buf,512);
	    pf_count--; pf_run--;
	    pf_buf += 512;
	    pf_block++;
	    if (!pf_run) break;
	    if (!pf_count) pf_next_buf(unit);
	}
	pf_mask = 0;
        ps_set_intr(do_pf_write_done,pf_ready,PF_TMO,nice);
}

static void do_pf_write_done( void )

{       int	unit = pf_unit;
	long	saved_flags;

        if (pf_wait(unit,STAT_BUSY,0,"write block","done") & STAT_ERR) {
                pi_disconnect(PI);
                if (pf_retries < PF_MAX_RETRIES) {
                        pf_retries++;
			pi_do_claimed(PI,do_pf_write_start);
                        return;
                }
		spin_lock_irqsave(&io_request_lock,saved_flags);
                end_request(0);
                pf_busy = 0;
		do_pf_request(NULL);
		spin_unlock_irqrestore(&io_request_lock,saved_flags);
                return;
        }
        pi_disconnect(PI);
	spin_lock_irqsave(&io_request_lock,saved_flags);
        end_request(1);
        pf_busy = 0;
	do_pf_request(NULL);
	spin_unlock_irqrestore(&io_request_lock,saved_flags);
}

/* end of pf.c */

MODULE_LICENSE("GPL");