firestream.c

Linux内核源代码 为压缩文件 是<<Linux内核>>一书中的源代码

			/* check msb that we are discarding */
			if (man & (1<<(32-9-1))) {
				man = (man>>(32-9)) + 1;
				if (man == (1<<9)) {
					/* no need to check for round up outside of range */
					man = 0;
					exp += 1;
				}
			} else {
				man = (man>>(32-9));
			}
			break;
		}
		}
    
	} else {
		/* zero rate - not representable */
    
		if (r == ROUND_DOWN) {
			return -EINVAL;
		} else {
			exp = 0;
			man = 0;
		}
	}
  
	fs_dprintk (FS_DEBUG_QOS, "rate: man=%u, exp=%hu", man, exp);
  
	if (bits)
		*bits = /* (1<<14) | */ (exp<<9) | man;
  
	if (actual)
		*actual = (exp >= 9)
			? (1 << exp) + (man << (exp-9))
			: (1 << exp) + ((man + (1<<(9-exp-1))) >> (9-exp));
  
	return 0;
}




/* FireStream access routines */
/* For DEEP-DOWN debugging these can be rigged to intercept accesses to
   certain registers or to just log all accesses. */

static inline void write_fs (struct fs_dev *dev, int offset, u32 val)
{
	writel (val, dev->base + offset);
}


static inline u32  read_fs (struct fs_dev *dev, int offset)
{
	return readl (dev->base + offset);
}



static inline struct FS_QENTRY *get_qentry (struct fs_dev *dev, struct queue *q)
{
	return bus_to_virt (read_fs (dev, Q_WP(q->offset)) & Q_ADDR_MASK);
}


static void submit_qentry (struct fs_dev *dev, struct queue *q, struct FS_QENTRY *qe)
{
	u32 wp;
	struct FS_QENTRY *cqe;

	/* XXX Sanity check: the write pointer can be checked to be 
	   still the same as the value passed as qe... -- REW */
	/*  udelay (5); */
	while ((wp = read_fs (dev, Q_WP (q->offset))) & Q_FULL) {
		fs_dprintk (FS_DEBUG_TXQ, "Found queue at %x full. Waiting.\n", 
			    q->offset);
		schedule ();
	}

	wp &= ~0xf;
	cqe = bus_to_virt (wp);
	if (qe != cqe) {
		fs_dprintk (FS_DEBUG_TXQ, "q mismatch! %p %p\n", qe, cqe);
	}

	write_fs (dev, Q_WP(q->offset), Q_INCWRAP);

	{
		static int c;
		if (!(c++ % 100))
			{
				int rp, wp;
				rp =  read_fs (dev, Q_RP(q->offset));
				wp =  read_fs (dev, Q_WP(q->offset));
				fs_dprintk (FS_DEBUG_TXQ, "q at %d: %x-%x: %x entries.\n", 
					    q->offset, rp, wp, wp-rp);
			}
	}
}

#ifdef DEBUG_EXTRA
static struct FS_QENTRY pq[60];
static int qp;

static struct FS_BPENTRY dq[60];
static int qd;
static void *da[60];
#endif 

static void submit_queue (struct fs_dev *dev, struct queue *q, 
			  u32 cmd, u32 p1, u32 p2, u32 p3)
{
	struct FS_QENTRY *qe;

	qe = get_qentry (dev, q);
	qe->cmd = cmd;
	qe->p0 = p1;
	qe->p1 = p2;
	qe->p2 = p3;
	submit_qentry (dev,  q, qe);

#ifdef DEBUG_EXTRA
	pq[qp].cmd = cmd;
	pq[qp].p0 = p1;
	pq[qp].p1 = p2;
	pq[qp].p2 = p3;
	qp++;
	if (qp >= 60) qp = 0;
#endif
}

/* Test the "other" way one day... -- REW */
#if 1
#define submit_command submit_queue
#else

static void submit_command (struct fs_dev *dev, struct queue *q, 
			    u32 cmd, u32 p1, u32 p2, u32 p3)
{
	write_fs (dev, CMDR0, cmd);
	write_fs (dev, CMDR1, p1);
	write_fs (dev, CMDR2, p2);
	write_fs (dev, CMDR3, p3);
}
#endif



static void process_return_queue (struct fs_dev *dev, struct queue *q)
{
	long rq;
	struct FS_QENTRY *qe;
	void *tc;
  
	while (!((rq = read_fs (dev, Q_RP(q->offset))) & Q_EMPTY)) {
		fs_dprintk (FS_DEBUG_QUEUE, "reaping return queue entry at %lx\n", rq); 
		qe = bus_to_virt (rq);
    
		fs_dprintk (FS_DEBUG_QUEUE, "queue entry: %08x %08x %08x %08x. (%d)\n", 
			    qe->cmd, qe->p0, qe->p1, qe->p2, STATUS_CODE (qe));

		switch (STATUS_CODE (qe)) {
		case 5:
			tc = bus_to_virt (qe->p0);
			fs_dprintk (FS_DEBUG_ALLOC, "Free tc: %p\n", tc);
			kfree (tc);
			break;
		}
    
		write_fs (dev, Q_RP(q->offset), Q_INCWRAP);
	}
}


static void process_txdone_queue (struct fs_dev *dev, struct queue *q)
{
	long rq;
	long tmp;
	struct FS_QENTRY *qe;
	struct sk_buff *skb;
	struct FS_BPENTRY *td;

	while (!((rq = read_fs (dev, Q_RP(q->offset))) & Q_EMPTY)) {
		fs_dprintk (FS_DEBUG_QUEUE, "reaping txdone entry at %lx\n", rq); 
		qe = bus_to_virt (rq);
    
		fs_dprintk (FS_DEBUG_QUEUE, "queue entry: %08x %08x %08x %08x: %d\n", 
			    qe->cmd, qe->p0, qe->p1, qe->p2, STATUS_CODE (qe));

		if (STATUS_CODE (qe) != 2)
			fs_dprintk (FS_DEBUG_TXMEM, "queue entry: %08x %08x %08x %08x: %d\n", 
				    qe->cmd, qe->p0, qe->p1, qe->p2, STATUS_CODE (qe));


		switch (STATUS_CODE (qe)) {
		case 0x02:
			/* Process a real txdone entry. */
			tmp = qe->p0;
			if (tmp & 0x0f)
				printk (KERN_WARNING "td not aligned: %ld\n", tmp);
			tmp &= ~0x0f;
			td = bus_to_virt (tmp);

			fs_dprintk (FS_DEBUG_QUEUE, "Pool entry: %08x %08x %08x %08x %p.\n", 
				    td->flags, td->next, td->bsa, td->aal_bufsize, td->skb );
      
			skb = td->skb;
			if (skb == FS_VCC (ATM_SKB(skb)->vcc)->last_skb) {
				wake_up_interruptible (& FS_VCC (ATM_SKB(skb)->vcc)->close_wait);
				FS_VCC (ATM_SKB(skb)->vcc)->last_skb = NULL;
			}
			td->dev->ntxpckts--;

			{
				static int c=0;
	
				if (!(c++ % 100)) {
					fs_dprintk (FS_DEBUG_QSIZE, "[%d]", td->dev->ntxpckts);
				}
			}

			atomic_inc(&ATM_SKB(skb)->vcc->stats->tx);

			fs_dprintk (FS_DEBUG_TXMEM, "i");
			fs_dprintk (FS_DEBUG_ALLOC, "Free t-skb: %p\n", skb);
			fs_kfree_skb (skb);

			fs_dprintk (FS_DEBUG_ALLOC, "Free trans-d: %p\n", td); 
			memset (td, 0x12, sizeof (struct FS_BPENTRY));
			kfree (td);
			break;
		default:
			/* Here we get the tx purge inhibit command ... */
			/* Action, I believe, is "don't do anything". -- REW */
		}
    
		write_fs (dev, Q_RP(q->offset), Q_INCWRAP);
	}
}


static void process_incoming (struct fs_dev *dev, struct queue *q)
{
	long rq;
	struct FS_QENTRY *qe;
	struct FS_BPENTRY *pe;    
	struct sk_buff *skb;
	unsigned int channo;
	struct atm_vcc *atm_vcc;

	while (!((rq = read_fs (dev, Q_RP(q->offset))) & Q_EMPTY)) {
		fs_dprintk (FS_DEBUG_QUEUE, "reaping incoming queue entry at %lx\n", rq); 
		qe = bus_to_virt (rq);
    
		fs_dprintk (FS_DEBUG_QUEUE, "queue entry: %08x %08x %08x %08x.  ", 
			    qe->cmd, qe->p0, qe->p1, qe->p2);

		fs_dprintk (FS_DEBUG_QUEUE, "-> %x: %s\n", 
			    STATUS_CODE (qe), 
			    res_strings[STATUS_CODE(qe)]);

		pe = bus_to_virt (qe->p0);
		fs_dprintk (FS_DEBUG_QUEUE, "Pool entry: %08x %08x %08x %08x %p %p.\n", 
			    pe->flags, pe->next, pe->bsa, pe->aal_bufsize, 
			    pe->skb, pe->fp);
      
		channo = qe->cmd & 0xffff;

		if (channo < dev->nchannels)
			atm_vcc = dev->atm_vccs[channo];
		else
			atm_vcc = NULL;

		/* Single buffer packet */
		switch (STATUS_CODE (qe)) {
		case 0x2:/* Packet received OK.... */
			if (atm_vcc) {
				skb = pe->skb;
				pe->fp->n--;
#if 0
				fs_dprintk (FS_DEBUG_QUEUE, "Got skb: %p\n", skb);
				if (FS_DEBUG_QUEUE & fs_debug) my_hd (bus_to_virt (pe->bsa), 0x20);
#endif
				skb_put (skb, qe->p1 & 0xffff); 
				ATM_SKB(skb)->vcc = atm_vcc;
				atomic_inc(&atm_vcc->stats->rx);
				skb->stamp = xtime;
				fs_dprintk (FS_DEBUG_ALLOC, "Free rec-skb: %p (pushed)\n", skb);
				atm_vcc->push (atm_vcc, skb);
				fs_dprintk (FS_DEBUG_ALLOC, "Free rec-d: %p\n", pe);
				kfree (pe);
			} else {
				printk (KERN_ERR "Got a receive on a non-open channel %d.\n", channo);
			}
			break;
		case 0x17:/* AAL 5 CRC32 error. IFF the length field is nonzero, a buffer
			     has been consumed and needs to be processed. -- REW */
			if (qe->p1 & 0xffff) {
				pe = bus_to_virt (qe->p0);
				pe->fp->n--;
				fs_dprintk (FS_DEBUG_ALLOC, "Free rec-skb: %p\n", pe->skb);
				dev_kfree_skb_any (pe->skb);
				fs_dprintk (FS_DEBUG_ALLOC, "Free rec-d: %p\n", pe);
				kfree (pe);
			}
			if (atm_vcc)
				atomic_inc(&atm_vcc->stats->rx_drop);
			break;
		case 0x1f: /*  Reassembly abort: no buffers. */
			/* Silently increment error counter. */
			if (atm_vcc)
				atomic_inc(&atm_vcc->stats->rx_drop);
			break;
		default: /* Hmm. Haven't written the code to handle the others yet... -- REW */
			printk (KERN_WARNING "Don't know what to do with RX status %x: %s.\n", 
				STATUS_CODE(qe), res_strings[STATUS_CODE (qe)]);
		}
		write_fs (dev, Q_RP(q->offset), Q_INCWRAP);
	}
}



#define DO_DIRECTION(tp) ((tp)->traffic_class != ATM_NONE)

static int fs_open(struct atm_vcc *atm_vcc, short vpi, int vci)
{
	struct fs_dev *dev;
	struct fs_vcc *vcc;
	struct fs_transmit_config *tc;
	struct atm_trafprm * txtp;
	struct atm_trafprm * rxtp;
	/*  struct fs_receive_config *rc;*/
	/*  struct FS_QENTRY *qe; */
	int error;
	int bfp;
	int to;
	unsigned short tmc0;

	func_enter ();

	dev = FS_DEV(atm_vcc->dev);
	fs_dprintk (FS_DEBUG_OPEN, "fs: open on dev: %p, vcc at %p\n", 
		    dev, atm_vcc);

	error = atm_find_ci(atm_vcc, &vpi, &vci);
	if (error) {
		fs_dprintk (FS_DEBUG_OPEN, "fs: find_ci failed.\n");
		return error;
	}

	atm_vcc->vpi = vpi;
	atm_vcc->vci = vci;
	if (vci != ATM_VPI_UNSPEC && vpi != ATM_VCI_UNSPEC)
		set_bit(ATM_VF_ADDR, &atm_vcc->flags);

	if (atm_vcc->qos.aal != ATM_AAL5) return -EINVAL; /* XXX AAL0 */

	fs_dprintk (FS_DEBUG_OPEN, "fs: (itf %d): open %d.%d\n", 
		    atm_vcc->dev->number, atm_vcc->vpi, atm_vcc->vci);	

	/* XXX handle qos parameters (rate limiting) ? */

	vcc = kmalloc(sizeof(struct fs_vcc), GFP_KERNEL);
	fs_dprintk (FS_DEBUG_ALLOC, "Alloc VCC: %p(%d)\n", vcc, sizeof(struct fs_vcc));
	if (!vcc) {
		clear_bit(ATM_VF_ADDR, &atm_vcc->flags);
		return -ENOMEM;
	}
  
	atm_vcc->dev_data = vcc;
	vcc->last_skb = NULL;

	init_waitqueue_head (&vcc->close_wait);

	txtp = &atm_vcc->qos.txtp;
	rxtp = &atm_vcc->qos.rxtp;

	if (!test_bit(ATM_VF_PARTIAL, &atm_vcc->flags)) {
		if (IS_FS50(dev)) {
			/* Increment the channel numer: take a free one next time.  */
			for (to=33;to;to--, dev->channo++) {
				/* If we need to do RX, AND the RX is inuse, try the next */
				if (DO_DIRECTION(rxtp) && dev->atm_vccs[dev->channo])
					continue;
				/* If we need to do TX, AND the TX is inuse, try the next */
				if (DO_DIRECTION(txtp) && test_bit (dev->channo, dev->tx_inuse))
					continue;
				/* Ok, both are free! (or not needed) */
				break;
			}
			if (!to) {
				printk ("No more free channels for FS50..\n");
				return -EBUSY;
			}
			vcc->channo = dev->channo;
			dev->channo &= dev->channel_mask;
      
		} else {
			vcc->channo = (vpi << FS155_VCI_BITS) | (vci);
			if (((DO_DIRECTION(rxtp) && dev->atm_vccs[vcc->channo])) ||
			    ( DO_DIRECTION(txtp) && test_bit (vcc->channo, dev->tx_inuse))) {
				printk ("Channel is in use for FS155.\n");
				return -EBUSY;
			}
		}
		fs_dprintk (FS_DEBUG_OPEN, "OK. Allocated channel %x(%d).\n", 
			    vcc->channo, vcc->channo);
	}

	if (DO_DIRECTION (txtp)) {
		tc = kmalloc (sizeof (struct fs_transmit_config), GFP_KERNEL);
		fs_dprintk (FS_DEBUG_ALLOC, "Alloc tc: %p(%d)\n", 
			    tc, sizeof (struct fs_transmit_config));
		if (!tc) {
			fs_dprintk (FS_DEBUG_OPEN, "fs: can't alloc transmit_config.\n");
			return -ENOMEM;
		}

		/* Allocate the "open" entry from the high priority txq. This makes
		   it most likely that the chip will notice it. It also prevents us
		   from having to wait for completion. On the other hand, we may
		   need to wait for completion anyway, to see if it completed
		   succesfully. */

		tc->flags = 0
			| TC_FLAGS_AAL5
			| TC_FLAGS_PACKET  /* ??? */
			| TC_FLAGS_TYPE_CBR
			| TC_FLAGS_CAL0;

		/* Docs are vague about this atm_hdr field. By the way, the FS
		 * chip makes odd errors if lower bits are set.... -- REW */
		tc->atm_hdr =  (vpi << 20) | (vci << 4); 
		{
			int pcr = atm_pcr_goal (txtp);

			fs_dprintk (FS_DEBUG_OPEN, "pcr = %d.\n", pcr);

			/* XXX Hmm. officially we're only allowed to do this if rounding 
			   is round_down -- REW */
			if (IS_FS50(dev)) {
				if (pcr > 51840000/53/8)  pcr = 51840000/53/8;
			} else {
				if (pcr > 155520000/53/8) pcr = 155520000/53/8;
			}
			if (!pcr) {
				/* no rate cap */
				tmc0 = IS_FS50(dev)?0x61BE:0x64c9; /* Just copied over the bits from Fujitsu -- REW */
			} else {
				int r;
				if (pcr < 0) {
					r = ROUND_DOWN;
					pcr = -pcr;
				} else {
					r = ROUND_UP;
				}
				error = make_rate (pcr, r, &tmc0, 0);
			}
			fs_dprintk (FS_DEBUG_OPEN, "pcr = %d.\n", pcr);
		}
      
		tc->TMC[0] = tmc0 | 0x4000;
		tc->TMC[1] = 0; /* Unused */
		tc->TMC[2] = 0; /* Unused */
		tc->TMC[3] = 0; /* Unused */
    
		tc->spec = 0;    /* UTOPIA address, UDF, HEC: Unused -> 0 */
		tc->rtag[0] = 0; /* What should I do with routing tags??? 
				    -- Not used -- AS -- Thanks -- REW*/
		tc->rtag[1] = 0;
		tc->rtag[2] = 0;

		if (fs_debug & FS_DEBUG_OPEN) {
			fs_dprintk (FS_DEBUG_OPEN, "TX config record:\n");
			my_hd (tc, sizeof (*tc));
		}

		/* We now use the "submit_command" function to submit commands to
		   the firestream. There is a define up near the definition of
		   that routine that switches this routine between immediate write
		   to the immediate comamnd registers and queuing the commands in
		   the HPTXQ for execution. This last technique might be more
		   efficient if we know we're going to submit a whole lot of
		   commands in one go, but this driver is not setup to be able to
		   use such a construct. So it probably doen't matter much right
		   now. -- REW */
    
		/* The command is IMMediate and INQueue. The parameters are out-of-line.. */
		submit_command (dev, &dev->hp_txq, 
				QE_CMD_CONFIG_TX | QE_CMD_IMM_INQ | vcc->channo,
				virt_to_bus (tc), 0, 0);

		submit_command (dev, &dev->hp_txq, 
				QE_CMD_TX_EN | QE_CMD_IMM_INQ | vcc->channo,
				0, 0, 0);
		set_bit (vcc->channo, dev->tx_inuse);
	}

	if (DO_DIRECTION (rxtp)) {
		dev->atm_vccs[vcc->channo] = atm_vcc;

		for (bfp = 0;bfp < FS_NR_FREE_POOLS; bfp++)
			if (atm_vcc->qos.rxtp.max_sdu <= dev->rx_fp[bfp].bufsize) break;
    
		if (bfp >= FS_NR_FREE_POOLS) {
			fs_dprintk (FS_DEBUG_OPEN, "No free pool fits sdu: %d.\n", 
				    atm_vcc->qos.rxtp.max_sdu);
			/* XXX Cleanup? -- Would just calling fs_close work??? -- REW */

			/* XXX clear tx inuse. Close TX part? */
			dev->atm_vccs[vcc->channo] = NULL;
			kfree (vcc);
			return -EINVAL;
		}

		submit_command (dev, &dev->hp_txq, 
				QE_CMD_CONFIG_RX | QE_CMD_IMM_INQ | vcc->channo,
				RC_FLAGS_AAL5 | 
				RC_FLAGS_BFPS_BFP * bfp |
				RC_FLAGS_RXBM_PSB, 0, 0);
    
		if (IS_FS50 (dev)) {