zatm.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 1,628 行 · 第 1/3 页

	shift = (1-(vcc->vci & 1)) << 4;
	zpokel(zatm_dev,(zpeekl(zatm_dev,pos) & ~(0xffff << shift)) |
	    ((zatm_vcc->rx_chan | uPD98401_RXLT_ENBL) << shift),pos);
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	return 0;
}


static void close_rx(struct atm_vcc *vcc)
{
	struct zatm_dev *zatm_dev;
	struct zatm_vcc *zatm_vcc;
	unsigned long flags;
	int pos,shift;

	zatm_vcc = ZATM_VCC(vcc);
	zatm_dev = ZATM_DEV(vcc->dev);
	if (!zatm_vcc->rx_chan) return;
	DPRINTK("close_rx\n");
	/* disable receiver */
	if (vcc->vpi != ATM_VPI_UNSPEC && vcc->vci != ATM_VCI_UNSPEC) {
		spin_lock_irqsave(&zatm_dev->lock, flags);
		pos = vcc->vci >> 1;
		shift = (1-(vcc->vci & 1)) << 4;
		zpokel(zatm_dev,zpeekl(zatm_dev,pos) & ~(0xffff << shift),pos);
		zwait;
		zout(uPD98401_NOP,CMR);
		zwait;
		zout(uPD98401_NOP,CMR);
		spin_unlock_irqrestore(&zatm_dev->lock, flags);
	}
	spin_lock_irqsave(&zatm_dev->lock, flags);
	zwait;
	zout(uPD98401_DEACT_CHAN | uPD98401_CHAN_RT | (zatm_vcc->rx_chan <<
	    uPD98401_CHAN_ADDR_SHIFT),CMR);
	zwait;
	udelay(10); /* why oh why ... ? */
	zout(uPD98401_CLOSE_CHAN | uPD98401_CHAN_RT | (zatm_vcc->rx_chan <<
	    uPD98401_CHAN_ADDR_SHIFT),CMR);
	zwait;
	if (!(zin(CMR) & uPD98401_CHAN_ADDR))
		printk(KERN_CRIT DEV_LABEL "(itf %d): can't close RX channel "
		    "%d\n",vcc->dev->number,zatm_vcc->rx_chan);
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	zatm_dev->rx_map[zatm_vcc->rx_chan] = NULL;
	zatm_vcc->rx_chan = 0;
	unuse_pool(vcc->dev,zatm_vcc->pool);
}


static int start_rx(struct atm_dev *dev)
{
	struct zatm_dev *zatm_dev;
	int size,i;

DPRINTK("start_rx\n");
	zatm_dev = ZATM_DEV(dev);
	size = sizeof(struct atm_vcc *)*zatm_dev->chans;
	zatm_dev->rx_map = (struct atm_vcc **) kmalloc(size,GFP_KERNEL);
	if (!zatm_dev->rx_map) return -ENOMEM;
	memset(zatm_dev->rx_map,0,size);
	/* set VPI/VCI split (use all VCIs and give what's left to VPIs) */
	zpokel(zatm_dev,(1 << dev->ci_range.vci_bits)-1,uPD98401_VRR);
	/* prepare free buffer pools */
	for (i = 0; i <= ZATM_LAST_POOL; i++) {
		zatm_dev->pool_info[i].ref_count = 0;
		zatm_dev->pool_info[i].rqa_count = 0;
		zatm_dev->pool_info[i].rqu_count = 0;
		zatm_dev->pool_info[i].low_water = LOW_MARK;
		zatm_dev->pool_info[i].high_water = HIGH_MARK;
		zatm_dev->pool_info[i].offset = 0;
		zatm_dev->pool_info[i].next_off = 0;
		zatm_dev->pool_info[i].next_cnt = 0;
		zatm_dev->pool_info[i].next_thres = OFF_CNG_THRES;
	}
	return 0;
}


/*----------------------------------- TX ------------------------------------*/


static int do_tx(struct sk_buff *skb)
{
	struct atm_vcc *vcc;
	struct zatm_dev *zatm_dev;
	struct zatm_vcc *zatm_vcc;
	u32 *dsc;
	unsigned long flags;

	EVENT("do_tx\n",0,0);
	DPRINTK("sending skb %p\n",skb);
	vcc = ATM_SKB(skb)->vcc;
	zatm_dev = ZATM_DEV(vcc->dev);
	zatm_vcc = ZATM_VCC(vcc);
	EVENT("iovcnt=%d\n",skb_shinfo(skb)->nr_frags,0);
	spin_lock_irqsave(&zatm_dev->lock, flags);
	if (!skb_shinfo(skb)->nr_frags) {
		if (zatm_vcc->txing == RING_ENTRIES-1) {
			spin_unlock_irqrestore(&zatm_dev->lock, flags);
			return RING_BUSY;
		}
		zatm_vcc->txing++;
		dsc = zatm_vcc->ring+zatm_vcc->ring_curr;
		zatm_vcc->ring_curr = (zatm_vcc->ring_curr+RING_WORDS) &
		    (RING_ENTRIES*RING_WORDS-1);
		dsc[1] = 0;
		dsc[2] = skb->len;
		dsc[3] = virt_to_bus(skb->data);
		mb();
		dsc[0] = uPD98401_TXPD_V | uPD98401_TXPD_DP | uPD98401_TXPD_SM
		    | (vcc->qos.aal == ATM_AAL5 ? uPD98401_TXPD_AAL5 : 0 |
		    (ATM_SKB(skb)->atm_options & ATM_ATMOPT_CLP ?
		    uPD98401_CLPM_1 : uPD98401_CLPM_0));
		EVENT("dsc (0x%lx)\n",(unsigned long) dsc,0);
	}
	else {
printk("NONONONOO!!!!\n");
		dsc = NULL;
#if 0
		u32 *put;
		int i;

		dsc = (u32 *) kmalloc(uPD98401_TXPD_SIZE*2+
		    uPD98401_TXBD_SIZE*ATM_SKB(skb)->iovcnt,GFP_ATOMIC);
		if (!dsc) {
			if (vcc->pop) vcc->pop(vcc,skb);
			else dev_kfree_skb_irq(skb);
			return -EAGAIN;
		}
		/* @@@ should check alignment */
		put = dsc+8;
		dsc[0] = uPD98401_TXPD_V | uPD98401_TXPD_DP |
		    (vcc->aal == ATM_AAL5 ? uPD98401_TXPD_AAL5 : 0 |
		    (ATM_SKB(skb)->atm_options & ATM_ATMOPT_CLP ?
		    uPD98401_CLPM_1 : uPD98401_CLPM_0));
		dsc[1] = 0;
		dsc[2] = ATM_SKB(skb)->iovcnt*uPD98401_TXBD_SIZE;
		dsc[3] = virt_to_bus(put);
		for (i = 0; i < ATM_SKB(skb)->iovcnt; i++) {
			*put++ = ((struct iovec *) skb->data)[i].iov_len;
			*put++ = virt_to_bus(((struct iovec *)
			    skb->data)[i].iov_base);
		}
		put[-2] |= uPD98401_TXBD_LAST;
#endif
	}
	ZATM_PRV_DSC(skb) = dsc;
	skb_queue_tail(&zatm_vcc->tx_queue,skb);
	DPRINTK("QRP=0x%08lx\n",zpeekl(zatm_dev,zatm_vcc->tx_chan*VC_SIZE/4+
	  uPD98401_TXVC_QRP));
	zwait;
	zout(uPD98401_TX_READY | (zatm_vcc->tx_chan <<
	    uPD98401_CHAN_ADDR_SHIFT),CMR);
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	EVENT("done\n",0,0);
	return 0;
}


static inline void dequeue_tx(struct atm_vcc *vcc)
{
	struct zatm_vcc *zatm_vcc;
	struct sk_buff *skb;

	EVENT("dequeue_tx\n",0,0);
	zatm_vcc = ZATM_VCC(vcc);
	skb = skb_dequeue(&zatm_vcc->tx_queue);
	if (!skb) {
		printk(KERN_CRIT DEV_LABEL "(itf %d): dequeue_tx but not "
		    "txing\n",vcc->dev->number);
		return;
	}
#if 0 /* @@@ would fail on CLP */
if (*ZATM_PRV_DSC(skb) != (uPD98401_TXPD_V | uPD98401_TXPD_DP |
  uPD98401_TXPD_SM | uPD98401_TXPD_AAL5)) printk("@#*$!!!!  (%08x)\n",
  *ZATM_PRV_DSC(skb));
#endif
	*ZATM_PRV_DSC(skb) = 0; /* mark as invalid */
	zatm_vcc->txing--;
	if (vcc->pop) vcc->pop(vcc,skb);
	else dev_kfree_skb_irq(skb);
	while ((skb = skb_dequeue(&zatm_vcc->backlog)))
		if (do_tx(skb) == RING_BUSY) {
			skb_queue_head(&zatm_vcc->backlog,skb);
			break;
		}
	atomic_inc(&vcc->stats->tx);
	wake_up(&zatm_vcc->tx_wait);
}


static void poll_tx(struct atm_dev *dev,int mbx)
{
	struct zatm_dev *zatm_dev;
	unsigned long pos;
	u32 x;

	EVENT("poll_tx\n",0,0);
	zatm_dev = ZATM_DEV(dev);
	pos = (zatm_dev->mbx_start[mbx] & ~0xffffUL) | zin(MTA(mbx));
	while (x = zin(MWA(mbx)), (pos & 0xffff) != x) {
		int chan;

#if 1
		u32 data,*addr;

		EVENT("MBX: host 0x%lx, nic 0x%x\n",pos,x);
		addr = (u32 *) pos;
		data = *addr;
		chan = (data & uPD98401_TXI_CONN) >> uPD98401_TXI_CONN_SHIFT;
		EVENT("addr = 0x%lx, data = 0x%08x,",(unsigned long) addr,
		    data);
		EVENT("chan = %d\n",chan,0);
#else
NO !
		chan = (zatm_dev->mbx_start[mbx][pos >> 2] & uPD98401_TXI_CONN)
		>> uPD98401_TXI_CONN_SHIFT;
#endif
		if (chan < zatm_dev->chans && zatm_dev->tx_map[chan])
			dequeue_tx(zatm_dev->tx_map[chan]);
		else {
			printk(KERN_CRIT DEV_LABEL "(itf %d): TX indication "
			    "for non-existing channel %d\n",dev->number,chan);
			event_dump();
		}
		if (((pos += 4) & 0xffff) == zatm_dev->mbx_end[mbx])
			pos = zatm_dev->mbx_start[mbx];
	}
	zout(pos & 0xffff,MTA(mbx));
}


/*
 * BUG BUG BUG: Doesn't handle "new-style" rate specification yet.
 */

static int alloc_shaper(struct atm_dev *dev,int *pcr,int min,int max,int ubr)
{
	struct zatm_dev *zatm_dev;
	unsigned long flags;
	unsigned long i,m,c;
	int shaper;

	DPRINTK("alloc_shaper (min = %d, max = %d)\n",min,max);
	zatm_dev = ZATM_DEV(dev);
	if (!zatm_dev->free_shapers) return -EAGAIN;
	for (shaper = 0; !((zatm_dev->free_shapers >> shaper) & 1); shaper++);
	zatm_dev->free_shapers &= ~1 << shaper;
	if (ubr) {
		c = 5;
		i = m = 1;
		zatm_dev->ubr_ref_cnt++;
		zatm_dev->ubr = shaper;
	}
	else {
		if (min) {
			if (min <= 255) {
				i = min;
				m = ATM_OC3_PCR;
			}
			else {
				i = 255;
				m = ATM_OC3_PCR*255/min;
			}
		}
		else {
			if (max > zatm_dev->tx_bw) max = zatm_dev->tx_bw;
			if (max <= 255) {
				i = max;
				m = ATM_OC3_PCR;
			}
			else {
				i = 255;
				m = (ATM_OC3_PCR*255+max-1)/max;
			}
		}
		if (i > m) {
			printk(KERN_CRIT DEV_LABEL "shaper algorithm botched "
			    "[%d,%d] -> i=%ld,m=%ld\n",min,max,i,m);
			m = i;
		}
		*pcr = i*ATM_OC3_PCR/m;
		c = 20; /* @@@ should use max_cdv ! */
		if ((min && *pcr < min) || (max && *pcr > max)) return -EINVAL;
		if (zatm_dev->tx_bw < *pcr) return -EAGAIN;
		zatm_dev->tx_bw -= *pcr;
	}
	spin_lock_irqsave(&zatm_dev->lock, flags);
	DPRINTK("i = %d, m = %d, PCR = %d\n",i,m,*pcr);
	zpokel(zatm_dev,(i << uPD98401_IM_I_SHIFT) | m,uPD98401_IM(shaper));
	zpokel(zatm_dev,c << uPD98401_PC_C_SHIFT,uPD98401_PC(shaper));
	zpokel(zatm_dev,0,uPD98401_X(shaper));
	zpokel(zatm_dev,0,uPD98401_Y(shaper));
	zpokel(zatm_dev,uPD98401_PS_E,uPD98401_PS(shaper));
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	return shaper;
}


static void dealloc_shaper(struct atm_dev *dev,int shaper)
{
	struct zatm_dev *zatm_dev;
	unsigned long flags;

	zatm_dev = ZATM_DEV(dev);
	if (shaper == zatm_dev->ubr) {
		if (--zatm_dev->ubr_ref_cnt) return;
		zatm_dev->ubr = -1;
	}
	spin_lock_irqsave(&zatm_dev->lock, flags);
	zpokel(zatm_dev,zpeekl(zatm_dev,uPD98401_PS(shaper)) & ~uPD98401_PS_E,
	    uPD98401_PS(shaper));
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	zatm_dev->free_shapers |= 1 << shaper;
}


static void close_tx(struct atm_vcc *vcc)
{
	struct zatm_dev *zatm_dev;
	struct zatm_vcc *zatm_vcc;
	unsigned long flags;
	int chan;
struct sk_buff *skb;
int once = 1;

	zatm_vcc = ZATM_VCC(vcc);
	zatm_dev = ZATM_DEV(vcc->dev);
	chan = zatm_vcc->tx_chan;
	if (!chan) return;
	DPRINTK("close_tx\n");
	while (skb_peek(&zatm_vcc->backlog)) {
if (once) {
printk("waiting for backlog to drain ...\n");
event_dump();
once = 0;
}
		sleep_on(&zatm_vcc->tx_wait);
	}
once = 1;
	while ((skb = skb_peek(&zatm_vcc->tx_queue))) {
if (once) {
printk("waiting for TX queue to drain ... %p\n",skb);
event_dump();
once = 0;
}
		DPRINTK("waiting for TX queue to drain ... %p\n",skb);
		sleep_on(&zatm_vcc->tx_wait);
	}
	spin_lock_irqsave(&zatm_dev->lock, flags);
#if 0
	zwait;
	zout(uPD98401_DEACT_CHAN | (chan << uPD98401_CHAN_ADDR_SHIFT),CMR);
#endif
	zwait;
	zout(uPD98401_CLOSE_CHAN | (chan << uPD98401_CHAN_ADDR_SHIFT),CMR);
	zwait;
	if (!(zin(CMR) & uPD98401_CHAN_ADDR))
		printk(KERN_CRIT DEV_LABEL "(itf %d): can't close TX channel "
		    "%d\n",vcc->dev->number,chan);
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	zatm_vcc->tx_chan = 0;
	zatm_dev->tx_map[chan] = NULL;
	if (zatm_vcc->shaper != zatm_dev->ubr) {
		zatm_dev->tx_bw += vcc->qos.txtp.min_pcr;
		dealloc_shaper(vcc->dev,zatm_vcc->shaper);
	}
	if (zatm_vcc->ring) kfree(zatm_vcc->ring);
}


static int open_tx_first(struct atm_vcc *vcc)
{
	struct zatm_dev *zatm_dev;
	struct zatm_vcc *zatm_vcc;
	unsigned long flags;
	u32 *loop;
	unsigned short chan;
	int pcr,unlimited;

	DPRINTK("open_tx_first\n");
	zatm_dev = ZATM_DEV(vcc->dev);
	zatm_vcc = ZATM_VCC(vcc);
	zatm_vcc->tx_chan = 0;
	if (vcc->qos.txtp.traffic_class == ATM_NONE) return 0;
	spin_lock_irqsave(&zatm_dev->lock, flags);
	zwait;
	zout(uPD98401_OPEN_CHAN,CMR);
	zwait;
	DPRINTK("0x%x 0x%x\n",zin(CMR),zin(CER));
	chan = (zin(CMR) & uPD98401_CHAN_ADDR) >> uPD98401_CHAN_ADDR_SHIFT;
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	DPRINTK("chan is %d\n",chan);
	if (!chan) return -EAGAIN;
	unlimited = vcc->qos.txtp.traffic_class == ATM_UBR &&
	    (!vcc->qos.txtp.max_pcr || vcc->qos.txtp.max_pcr == ATM_MAX_PCR ||
	    vcc->qos.txtp.max_pcr >= ATM_OC3_PCR);
	if (unlimited && zatm_dev->ubr != -1) zatm_vcc->shaper = zatm_dev->ubr;
	else {
		if (unlimited) vcc->qos.txtp.max_sdu = ATM_MAX_AAL5_PDU;
		if ((zatm_vcc->shaper = alloc_shaper(vcc->dev,&pcr,
		    vcc->qos.txtp.min_pcr,vcc->qos.txtp.max_pcr,unlimited))
		    < 0) {
			close_tx(vcc);
			return zatm_vcc->shaper;
		}
		if (pcr > ATM_OC3_PCR) pcr = ATM_OC3_PCR;
		vcc->qos.txtp.min_pcr = vcc->qos.txtp.max_pcr = pcr;
	}
	zatm_vcc->tx_chan = chan;
	skb_queue_head_init(&zatm_vcc->tx_queue);
	init_waitqueue_head(&zatm_vcc->tx_wait);
	/* initialize ring */
	zatm_vcc->ring = kmalloc(RING_SIZE,GFP_KERNEL);
	if (!zatm_vcc->ring) return -ENOMEM;
	memset(zatm_vcc->ring,0,RING_SIZE);
	loop = zatm_vcc->ring+RING_ENTRIES*RING_WORDS;
	loop[0] = uPD98401_TXPD_V;
	loop[1] = loop[2] = 0;
	loop[3] = virt_to_bus(zatm_vcc->ring);
	zatm_vcc->ring_curr = 0;
	zatm_vcc->txing = 0;
	skb_queue_head_init(&zatm_vcc->backlog);
	zpokel(zatm_dev,virt_to_bus(zatm_vcc->ring),
	    chan*VC_SIZE/4+uPD98401_TXVC_QRP);
	return 0;
}


static int open_tx_second(struct atm_vcc *vcc)
{
	struct zatm_dev *zatm_dev;
	struct zatm_vcc *zatm_vcc;
	unsigned long flags;

	DPRINTK("open_tx_second\n");
	zatm_dev = ZATM_DEV(vcc->dev);
	zatm_vcc = ZATM_VCC(vcc);
	if (!zatm_vcc->tx_chan) return 0;
	/* set up VC descriptor */
	spin_lock_irqsave(&zatm_dev->lock, flags);
	zpokel(zatm_dev,0,zatm_vcc->tx_chan*VC_SIZE/4);
	zpokel(zatm_dev,uPD98401_TXVC_L | (zatm_vcc->shaper <<
	    uPD98401_TXVC_SHP_SHIFT) | (vcc->vpi << uPD98401_TXVC_VPI_SHIFT) |
	    vcc->vci,zatm_vcc->tx_chan*VC_SIZE/4+1);
	zpokel(zatm_dev,0,zatm_vcc->tx_chan*VC_SIZE/4+2);
	spin_unlock_irqrestore(&zatm_dev->lock, flags);
	zatm_dev->tx_map[zatm_vcc->tx_chan] = vcc;
	return 0;
}


static int start_tx(struct atm_dev *dev)
{
	struct zatm_dev *zatm_dev;
	int i;

	DPRINTK("start_tx\n");
	zatm_dev = ZATM_DEV(dev);
	zatm_dev->tx_map = (struct atm_vcc **) kmalloc(sizeof(struct atm_vcc *)*
	    zatm_dev->chans,GFP_KERNEL);
	if (!zatm_dev->tx_map) return -ENOMEM;
	zatm_dev->tx_bw = ATM_OC3_PCR;
	zatm_dev->free_shapers = (1 << NR_SHAPERS)-1;
	zatm_dev->ubr = -1;
	zatm_dev->ubr_ref_cnt = 0;
	/* initialize shapers */
	for (i = 0; i < NR_SHAPERS; i++) zpokel(zatm_dev,0,uPD98401_PS(i));
	return 0;
}


/*------------------------------- interrupts --------------------------------*/


static irqreturn_t zatm_int(int irq,void *dev_id,struct pt_regs *regs)
{
	struct atm_dev *dev;
	struct zatm_dev *zatm_dev;
	u32 reason;
	int handled = 0;

	dev = dev_id;
	zatm_dev = ZATM_DEV(dev);
	while ((reason = zin(GSR))) {
		handled = 1;
		EVENT("reason 0x%x\n",reason,0);
		if (reason & uPD98401_INT_PI) {
			EVENT("PHY int\n",0,0);
			dev->phy->interrupt(dev);
		}
		if (reason & uPD98401_INT_RQA) {
			unsigned long pools;
			int i;

			pools = zin(RQA);
			EVENT("RQA (0x%08x)\n",pools,0);
			for (i = 0; pools; i++) {
				if (pools & 1) {
					refill_pool(dev,i);
					zatm_dev->pool_info[i].rqa_count++;
				}
				pools >>= 1;
			}
		}
		if (reason & uPD98401_INT_RQU) {
			unsigned long pools;
			int i;
			pools = zin(RQU);
			printk(KERN_WARNING DEV_LABEL "(itf %d): RQU 0x%08lx\n",
			    dev->number,pools);
			event_dump();
			for (i = 0; pools; i++) {
				if (pools & 1) {
					refill_pool(dev,i);
					zatm_dev->pool_info[i].rqu_count++;
				}
				pools >>= 1;
			}
		}
		/* don't handle RD */
		if (reason & uPD98401_INT_SPE)
			printk(KERN_ALERT DEV_LABEL "(itf %d): system parity "
			    "error at 0x%08x\n",dev->number,zin(ADDR));
		if (reason & uPD98401_INT_CPE)
			printk(KERN_ALERT DEV_LABEL "(itf %d): control memory "
			    "parity error at 0x%08x\n",dev->number,zin(ADDR));
		if (reason & uPD98401_INT_SBE) {
			printk(KERN_ALERT DEV_LABEL "(itf %d): system bus "
			    "error at 0x%08x\n",dev->number,zin(ADDR));
			event_dump();
		}
		/* don't handle IND */
		if (reason & uPD98401_INT_MF) {
			printk(KERN_CRIT DEV_LABEL "(itf %d): mailbox full "
			    "(0x%x)\n",dev->number,(reason & uPD98401_INT_MF)
			    >> uPD98401_INT_MF_SHIFT);
			event_dump();
			    /* @@@ should try to recover */
		}
		if (reason & uPD98401_INT_MM) {
			if (reason & 1) poll_rx(dev,0);