isocdata.c

来自「LINUX 2.6.17.4的源码」· C语言 代码 · 共 1,011 行 · 第 1/3 页

	 */
	inputstate = bcs->inputstate;
	seqlen = ubc->seqlen;
	inbyte = ubc->inbyte;
	inbits = ubc->inbits;

	/* bit unstuffing a byte a time
	 * Take your time to understand this; it's straightforward but tedious.
	 * The "bitcounts" lookup table is used to speed up the counting of
	 * leading and trailing '1' bits.
	 */
	while (count--) {
		unsigned char c = *src++;
		unsigned char tabentry = bitcounts[c];
		unsigned lead1 = tabentry & 0x0f;
		unsigned trail1 = (tabentry >> 4) & 0x0f;

		seqlen += lead1;

		if (unlikely(inputstate & INS_flag_hunt)) {
			if (c == PPP_FLAG) {
				/* flag-in-one */
				inputstate &= ~(INS_flag_hunt | INS_have_data);
				inbyte = 0;
				inbits = 0;
			} else if (seqlen == 6 && trail1 != 7) {
				/* flag completed & not followed by abort */
				inputstate &= ~(INS_flag_hunt | INS_have_data);
				inbyte = c >> (lead1 + 1);
				inbits = 7 - lead1;
				if (trail1 >= 8) {
					/* interior stuffing: omitting the MSB handles most cases */
					inbits--;
					/* correct the incorrectly handled cases individually */
					switch (c) {
					case 0xbe:
						inbyte = 0x3f;
						break;
					}
				}
			}
			/* else: continue flag-hunting */
		} else if (likely(seqlen < 5 && trail1 < 7)) {
			/* streamlined case: 8 data bits, no stuffing */
			inbyte |= c << inbits;
			hdlc_putbyte(inbyte & 0xff, bcs);
			inputstate |= INS_have_data;
			inbyte >>= 8;
			/* inbits unchanged */
		} else if (likely(seqlen == 6 && inbits == 7 - lead1 &&
				  trail1 + 1 == inbits &&
				  !(inputstate & INS_have_data))) {
			/* streamlined case: flag idle - state unchanged */
		} else if (unlikely(seqlen > 6)) {
			/* abort sequence */
			ubc->aborts++;
			hdlc_flush(bcs);
			inputstate |= INS_flag_hunt;
		} else if (seqlen == 6) {
			/* closing flag, including (6 - lead1) '1's and one '0' from inbits */
			if (inbits > 7 - lead1) {
				hdlc_frag(bcs, inbits + lead1 - 7);
				inputstate &= ~INS_have_data;
			} else {
				if (inbits < 7 - lead1)
					ubc->stolen0s ++;
				if (inputstate & INS_have_data) {
					hdlc_done(bcs);
					inputstate &= ~INS_have_data;
				}
			}

			if (c == PPP_FLAG) {
				/* complete flag, LSB overlaps preceding flag */
				ubc->shared0s ++;
				inbits = 0;
				inbyte = 0;
			} else if (trail1 != 7) {
				/* remaining bits */
				inbyte = c >> (lead1 + 1);
				inbits = 7 - lead1;
				if (trail1 >= 8) {
					/* interior stuffing: omitting the MSB handles most cases */
					inbits--;
					/* correct the incorrectly handled cases individually */
					switch (c) {
					case 0xbe:
						inbyte = 0x3f;
						break;
					}
				}
			} else {
				/* abort sequence follows, skb already empty anyway */
				ubc->aborts++;
				inputstate |= INS_flag_hunt;
			}
		} else { /* (seqlen < 6) && (seqlen == 5 || trail1 >= 7) */

			if (c == PPP_FLAG) {
				/* complete flag */
				if (seqlen == 5)
					ubc->stolen0s++;
				if (inbits) {
					hdlc_frag(bcs, inbits);
					inbits = 0;
					inbyte = 0;
				} else if (inputstate & INS_have_data)
					hdlc_done(bcs);
				inputstate &= ~INS_have_data;
			} else if (trail1 == 7) {
				/* abort sequence */
				ubc->aborts++;
				hdlc_flush(bcs);
				inputstate |= INS_flag_hunt;
			} else {
				/* stuffed data */
				if (trail1 < 7) { /* => seqlen == 5 */
					/* stuff bit at position lead1, no interior stuffing */
					unsigned char mask = (1 << lead1) - 1;
					c = (c & mask) | ((c & ~mask) >> 1);
					inbyte |= c << inbits;
					inbits += 7;
				} else if (seqlen < 5) { /* trail1 >= 8 */
					/* interior stuffing: omitting the MSB handles most cases */
					/* correct the incorrectly handled cases individually */
					switch (c) {
					case 0xbe:
						c = 0x7e;
						break;
					}
					inbyte |= c << inbits;
					inbits += 7;
				} else { /* seqlen == 5 && trail1 >= 8 */

					/* stuff bit at lead1 *and* interior stuffing */
					switch (c) {	/* unstuff individually */
					case 0x7d:
						c = 0x3f;
						break;
					case 0xbe:
						c = 0x3f;
						break;
					case 0x3e:
						c = 0x1f;
						break;
					case 0x7c:
						c = 0x3e;
						break;
					}
					inbyte |= c << inbits;
					inbits += 6;
				}
				if (inbits >= 8) {
					inbits -= 8;
					hdlc_putbyte(inbyte & 0xff, bcs);
					inputstate |= INS_have_data;
					inbyte >>= 8;
				}
			}
		}
		seqlen = trail1 & 7;
	}

	/* save new state */
	bcs->inputstate = inputstate;
	ubc->seqlen = seqlen;
	ubc->inbyte = inbyte;
	ubc->inbits = inbits;
}

/* trans_receive
 * pass on received USB frame transparently as SKB via gigaset_rcv_skb
 * invert bytes
 * tally frames, errors etc. in BC structure counters
 * parameters:
 *	src	received data
 *	count	number of received bytes
 *	bcs	receiving B channel structure
 */
static inline void trans_receive(unsigned char *src, unsigned count,
				 struct bc_state *bcs)
{
	struct sk_buff *skb;
	int dobytes;
	unsigned char *dst;

	if (unlikely(bcs->ignore)) {
		bcs->ignore--;
		hdlc_flush(bcs);
		return;
	}
	if (unlikely((skb = bcs->skb) == NULL)) {
		bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
		if (!skb) {
			dev_err(bcs->cs->dev, "could not allocate skb\n");
			return;
		}
		skb_reserve(skb, HW_HDR_LEN);
	}
	bcs->hw.bas->goodbytes += skb->len;
	dobytes = TRANSBUFSIZE - skb->len;
	while (count > 0) {
		dst = skb_put(skb, count < dobytes ? count : dobytes);
		while (count > 0 && dobytes > 0) {
			*dst++ = gigaset_invtab[*src++];
			count--;
			dobytes--;
		}
		if (dobytes == 0) {
			gigaset_rcv_skb(skb, bcs->cs, bcs);
			bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
			if (!skb) {
				dev_err(bcs->cs->dev,
					"could not allocate skb\n");
				return;
			}
			skb_reserve(bcs->skb, HW_HDR_LEN);
			dobytes = TRANSBUFSIZE;
		}
	}
}

void gigaset_isoc_receive(unsigned char *src, unsigned count, struct bc_state *bcs)
{
	switch (bcs->proto2) {
	case ISDN_PROTO_L2_HDLC:
		hdlc_unpack(src, count, bcs);
		break;
	default:		/* assume transparent */
		trans_receive(src, count, bcs);
	}
}

/* == data input =========================================================== */

static void cmd_loop(unsigned char *src, int numbytes, struct inbuf_t *inbuf)
{
	struct cardstate *cs = inbuf->cs;
	unsigned cbytes      = cs->cbytes;

	while (numbytes--) {
		/* copy next character, check for end of line */
		switch (cs->respdata[cbytes] = *src++) {
		case '\r':
		case '\n':
			/* end of line */
			gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
				__func__, cbytes);
			cs->cbytes = cbytes;
			gigaset_handle_modem_response(cs);
			cbytes = 0;
			break;
		default:
			/* advance in line buffer, checking for overflow */
			if (cbytes < MAX_RESP_SIZE - 1)
				cbytes++;
			else
				dev_warn(cs->dev, "response too large\n");
		}
	}

	/* save state */
	cs->cbytes = cbytes;
}


/* process a block of data received through the control channel
 */
void gigaset_isoc_input(struct inbuf_t *inbuf)
{
	struct cardstate *cs = inbuf->cs;
	unsigned tail, head, numbytes;
	unsigned char *src;

	head = atomic_read(&inbuf->head);
	while (head != (tail = atomic_read(&inbuf->tail))) {
		gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
		if (head > tail)
			tail = RBUFSIZE;
		src = inbuf->data + head;
		numbytes = tail - head;
		gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);

		if (atomic_read(&cs->mstate) == MS_LOCKED) {
			gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
					   numbytes, src);
			gigaset_if_receive(inbuf->cs, src, numbytes);
		} else {
			gigaset_dbg_buffer(DEBUG_CMD, "received response",
					   numbytes, src);
			cmd_loop(src, numbytes, inbuf);
		}

		head += numbytes;
		if (head == RBUFSIZE)
			head = 0;
		gig_dbg(DEBUG_INTR, "setting head to %u", head);
		atomic_set(&inbuf->head, head);
	}
}


/* == data output ========================================================== */

/* gigaset_send_skb
 * called by common.c to queue an skb for sending
 * and start transmission if necessary
 * parameters:
 *	B Channel control structure
 *	skb
 * return value:
 *	number of bytes accepted for sending
 *	(skb->len if ok, 0 if out of buffer space)
 *	or error code (< 0, eg. -EINVAL)
 */
int gigaset_isoc_send_skb(struct bc_state *bcs, struct sk_buff *skb)
{
	int len = skb->len;
	unsigned long flags;

	spin_lock_irqsave(&bcs->cs->lock, flags);
	if (!bcs->cs->connected) {
		spin_unlock_irqrestore(&bcs->cs->lock, flags);
		return -ENODEV;
	}

	skb_queue_tail(&bcs->squeue, skb);
	gig_dbg(DEBUG_ISO, "%s: skb queued, qlen=%d",
		__func__, skb_queue_len(&bcs->squeue));

	/* tasklet submits URB if necessary */
	tasklet_schedule(&bcs->hw.bas->sent_tasklet);
	spin_unlock_irqrestore(&bcs->cs->lock, flags);

	return len;	/* ok so far */
}