isocdata.c

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

 0x0060, 0x0061, 0x0062, 0x20c3, 0x0064, 0x0065, 0x0066, 0x20cb, 0x0068, 0x0069, 0x006a, 0x20d3, 0x006c, 0x006d, 0x006e, 0x20db,
 0x0070, 0x0071, 0x0072, 0x20e3, 0x0074, 0x0075, 0x0076, 0x20eb, 0x0078, 0x0079, 0x007a, 0x20f3, 0x207c, 0x207d, 0x20be, 0x40fb,
 0x0480, 0x0481, 0x0482, 0x2503, 0x0484, 0x0485, 0x0486, 0x250b, 0x0488, 0x0489, 0x048a, 0x2513, 0x048c, 0x048d, 0x048e, 0x251b,
 0x0490, 0x0491, 0x0492, 0x2523, 0x0494, 0x0495, 0x0496, 0x252b, 0x0498, 0x0499, 0x049a, 0x2533, 0x049c, 0x049d, 0x049e, 0x253b,
 0x04a0, 0x04a1, 0x04a2, 0x2543, 0x04a4, 0x04a5, 0x04a6, 0x254b, 0x04a8, 0x04a9, 0x04aa, 0x2553, 0x04ac, 0x04ad, 0x04ae, 0x255b,
 0x04b0, 0x04b1, 0x04b2, 0x2563, 0x04b4, 0x04b5, 0x04b6, 0x256b, 0x04b8, 0x04b9, 0x04ba, 0x2573, 0x04bc, 0x04bd, 0x253e, 0x257b,
 0x08c0, 0x08c1, 0x08c2, 0x2983, 0x08c4, 0x08c5, 0x08c6, 0x298b, 0x08c8, 0x08c9, 0x08ca, 0x2993, 0x08cc, 0x08cd, 0x08ce, 0x299b,
 0x08d0, 0x08d1, 0x08d2, 0x29a3, 0x08d4, 0x08d5, 0x08d6, 0x29ab, 0x08d8, 0x08d9, 0x08da, 0x29b3, 0x08dc, 0x08dd, 0x08de, 0x29bb,
 0x0ce0, 0x0ce1, 0x0ce2, 0x2dc3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dcb, 0x0ce8, 0x0ce9, 0x0cea, 0x2dd3, 0x0cec, 0x0ced, 0x0cee, 0x2ddb,
 0x10f0, 0x10f1, 0x10f2, 0x31e3, 0x10f4, 0x10f5, 0x10f6, 0x31eb, 0x20f8, 0x20f9, 0x20fa, 0x41f3, 0x257c, 0x257d, 0x29be, 0x46fb,

// previous 1s = 4:
 0x0000, 0x2001, 0x0002, 0x2005, 0x0004, 0x2009, 0x0006, 0x200d, 0x0008, 0x2011, 0x000a, 0x2015, 0x000c, 0x2019, 0x000e, 0x201d,
 0x0010, 0x2021, 0x0012, 0x2025, 0x0014, 0x2029, 0x0016, 0x202d, 0x0018, 0x2031, 0x001a, 0x2035, 0x001c, 0x2039, 0x001e, 0x203d,
 0x0020, 0x2041, 0x0022, 0x2045, 0x0024, 0x2049, 0x0026, 0x204d, 0x0028, 0x2051, 0x002a, 0x2055, 0x002c, 0x2059, 0x002e, 0x205d,
 0x0030, 0x2061, 0x0032, 0x2065, 0x0034, 0x2069, 0x0036, 0x206d, 0x0038, 0x2071, 0x003a, 0x2075, 0x003c, 0x2079, 0x203e, 0x407d,
 0x0040, 0x2081, 0x0042, 0x2085, 0x0044, 0x2089, 0x0046, 0x208d, 0x0048, 0x2091, 0x004a, 0x2095, 0x004c, 0x2099, 0x004e, 0x209d,
 0x0050, 0x20a1, 0x0052, 0x20a5, 0x0054, 0x20a9, 0x0056, 0x20ad, 0x0058, 0x20b1, 0x005a, 0x20b5, 0x005c, 0x20b9, 0x005e, 0x20bd,
 0x0060, 0x20c1, 0x0062, 0x20c5, 0x0064, 0x20c9, 0x0066, 0x20cd, 0x0068, 0x20d1, 0x006a, 0x20d5, 0x006c, 0x20d9, 0x006e, 0x20dd,
 0x0070, 0x20e1, 0x0072, 0x20e5, 0x0074, 0x20e9, 0x0076, 0x20ed, 0x0078, 0x20f1, 0x007a, 0x20f5, 0x207c, 0x40f9, 0x20be, 0x417d,
 0x0480, 0x2501, 0x0482, 0x2505, 0x0484, 0x2509, 0x0486, 0x250d, 0x0488, 0x2511, 0x048a, 0x2515, 0x048c, 0x2519, 0x048e, 0x251d,
 0x0490, 0x2521, 0x0492, 0x2525, 0x0494, 0x2529, 0x0496, 0x252d, 0x0498, 0x2531, 0x049a, 0x2535, 0x049c, 0x2539, 0x049e, 0x253d,
 0x04a0, 0x2541, 0x04a2, 0x2545, 0x04a4, 0x2549, 0x04a6, 0x254d, 0x04a8, 0x2551, 0x04aa, 0x2555, 0x04ac, 0x2559, 0x04ae, 0x255d,
 0x04b0, 0x2561, 0x04b2, 0x2565, 0x04b4, 0x2569, 0x04b6, 0x256d, 0x04b8, 0x2571, 0x04ba, 0x2575, 0x04bc, 0x2579, 0x253e, 0x467d,
 0x08c0, 0x2981, 0x08c2, 0x2985, 0x08c4, 0x2989, 0x08c6, 0x298d, 0x08c8, 0x2991, 0x08ca, 0x2995, 0x08cc, 0x2999, 0x08ce, 0x299d,
 0x08d0, 0x29a1, 0x08d2, 0x29a5, 0x08d4, 0x29a9, 0x08d6, 0x29ad, 0x08d8, 0x29b1, 0x08da, 0x29b5, 0x08dc, 0x29b9, 0x08de, 0x29bd,
 0x0ce0, 0x2dc1, 0x0ce2, 0x2dc5, 0x0ce4, 0x2dc9, 0x0ce6, 0x2dcd, 0x0ce8, 0x2dd1, 0x0cea, 0x2dd5, 0x0cec, 0x2dd9, 0x0cee, 0x2ddd,
 0x10f0, 0x31e1, 0x10f2, 0x31e5, 0x10f4, 0x31e9, 0x10f6, 0x31ed, 0x20f8, 0x41f1, 0x20fa, 0x41f5, 0x257c, 0x46f9, 0x29be, 0x4b7d
};

/* hdlc_bitstuff_byte
 * perform HDLC bitstuffing for one input byte (8 bits, LSB first)
 * parameters:
 *	cin	input byte
 *	ones	number of trailing '1' bits in result before this step
 *	iwb	pointer to output buffer structure (write semaphore must be held)
 * return value:
 *	number of trailing '1' bits in result after this step
 */

static inline int hdlc_bitstuff_byte(struct isowbuf_t *iwb, unsigned char cin,
				     int ones)
{
	u16 stuff;
	int shiftinc, newones;

	/* get stuffing information for input byte
	 * value: bit  9.. 0 = result bits
	 *        bit 12..10 = number of trailing '1' bits in result
	 *        bit 14..13 = number of bits added by stuffing
	 */
	stuff = stufftab[256 * ones + cin];
	shiftinc = (stuff >> 13) & 3;
	newones = (stuff >> 10) & 7;
	stuff &= 0x3ff;

	/* append stuffed byte to output stream */
	isowbuf_putbits(iwb, stuff, 8 + shiftinc);
	return newones;
}

/* hdlc_buildframe
 * Perform HDLC framing with bitstuffing on a byte buffer
 * The input buffer is regarded as a sequence of bits, starting with the least
 * significant bit of the first byte and ending with the most significant bit
 * of the last byte. A 16 bit FCS is appended as defined by RFC 1662.
 * Whenever five consecutive '1' bits appear in the resulting bit sequence, a
 * '0' bit is inserted after them.
 * The resulting bit string and a closing flag pattern (PPP_FLAG, '01111110')
 * are appended to the output buffer starting at the given bit position, which
 * is assumed to already contain a leading flag.
 * The output buffer must have sufficient length; count + count/5 + 6 bytes
 * starting at *out are safe and are verified to be present.
 * parameters:
 *	in	input buffer
 *	count	number of bytes in input buffer
 *	iwb	pointer to output buffer structure (write semaphore must be held)
 * return value:
 *	position of end of packet in output buffer on success,
 *	-EAGAIN if write semaphore busy or buffer full
 */

static inline int hdlc_buildframe(struct isowbuf_t *iwb,
				  unsigned char *in, int count)
{
	int ones;
	u16 fcs;
	int end;
	unsigned char c;

	if (isowbuf_freebytes(iwb) < count + count / 5 + 6 ||
	    !isowbuf_startwrite(iwb)) {
		gig_dbg(DEBUG_ISO, "%s: %d bytes free -> -EAGAIN",
			__func__, isowbuf_freebytes(iwb));
		return -EAGAIN;
	}

	dump_bytes(DEBUG_STREAM, "snd data", in, count);

	/* bitstuff and checksum input data */
	fcs = PPP_INITFCS;
	ones = 0;
	while (count-- > 0) {
		c = *in++;
		ones = hdlc_bitstuff_byte(iwb, c, ones);
		fcs = crc_ccitt_byte(fcs, c);
	}

	/* bitstuff and append FCS (complemented, least significant byte first) */
	fcs ^= 0xffff;
	ones = hdlc_bitstuff_byte(iwb, fcs & 0x00ff, ones);
	ones = hdlc_bitstuff_byte(iwb, (fcs >> 8) & 0x00ff, ones);

	/* put closing flag and repeat byte for flag idle */
	isowbuf_putflag(iwb);
	end = isowbuf_donewrite(iwb);
	dump_bytes(DEBUG_STREAM_DUMP, "isowbuf", iwb->data, end + 1);
	return end;
}

/* trans_buildframe
 * Append a block of 'transparent' data to the output buffer,
 * inverting the bytes.
 * The output buffer must have sufficient length; count bytes
 * starting at *out are safe and are verified to be present.
 * parameters:
 *	in	input buffer
 *	count	number of bytes in input buffer
 *	iwb	pointer to output buffer structure (write semaphore must be held)
 * return value:
 *	position of end of packet in output buffer on success,
 *	-EAGAIN if write semaphore busy or buffer full
 */

static inline int trans_buildframe(struct isowbuf_t *iwb,
				   unsigned char *in, int count)
{
	int write;
	unsigned char c;

	if (unlikely(count <= 0))
		return atomic_read(&iwb->write); /* better ideas? */

	if (isowbuf_freebytes(iwb) < count ||
	    !isowbuf_startwrite(iwb)) {
		gig_dbg(DEBUG_ISO, "can't put %d bytes", count);
		return -EAGAIN;
	}

	gig_dbg(DEBUG_STREAM, "put %d bytes", count);
	write = atomic_read(&iwb->write);
	do {
		c = gigaset_invtab[*in++];
		iwb->data[write++] = c;
		write %= BAS_OUTBUFSIZE;
	} while (--count > 0);
	atomic_set(&iwb->write, write);
	iwb->idle = c;

	return isowbuf_donewrite(iwb);
}

int gigaset_isoc_buildframe(struct bc_state *bcs, unsigned char *in, int len)
{
	int result;

	switch (bcs->proto2) {
	case ISDN_PROTO_L2_HDLC:
		result = hdlc_buildframe(bcs->hw.bas->isooutbuf, in, len);
		gig_dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d",
			__func__, len, result);
		break;
	default:			/* assume transparent */
		result = trans_buildframe(bcs->hw.bas->isooutbuf, in, len);
		gig_dbg(DEBUG_ISO, "%s: %d bytes trans -> %d",
			__func__, len, result);
	}
	return result;
}

/* hdlc_putbyte
 * append byte c to current skb of B channel structure *bcs, updating fcs
 */
static inline void hdlc_putbyte(unsigned char c, struct bc_state *bcs)
{
	bcs->fcs = crc_ccitt_byte(bcs->fcs, c);
	if (unlikely(bcs->skb == NULL)) {
		/* skipping */
		return;
	}
	if (unlikely(bcs->skb->len == SBUFSIZE)) {
		dev_warn(bcs->cs->dev, "received oversized packet discarded\n");
		bcs->hw.bas->giants++;
		dev_kfree_skb_any(bcs->skb);
		bcs->skb = NULL;
		return;
	}
	*__skb_put(bcs->skb, 1) = c;
}

/* hdlc_flush
 * drop partial HDLC data packet
 */
static inline void hdlc_flush(struct bc_state *bcs)
{
	/* clear skb or allocate new if not skipping */
	if (likely(bcs->skb != NULL))
		skb_trim(bcs->skb, 0);
	else if (!bcs->ignore) {
		if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
			skb_reserve(bcs->skb, HW_HDR_LEN);
		else
			dev_err(bcs->cs->dev, "could not allocate skb\n");
	}

	/* reset packet state */
	bcs->fcs = PPP_INITFCS;
}

/* hdlc_done
 * process completed HDLC data packet
 */
static inline void hdlc_done(struct bc_state *bcs)
{
	struct sk_buff *procskb;

	if (unlikely(bcs->ignore)) {
		bcs->ignore--;
		hdlc_flush(bcs);
		return;
	}

	if ((procskb = bcs->skb) == NULL) {
		/* previous error */
		gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
		gigaset_rcv_error(NULL, bcs->cs, bcs);
	} else if (procskb->len < 2) {
		dev_notice(bcs->cs->dev, "received short frame (%d octets)\n",
			   procskb->len);
		bcs->hw.bas->runts++;
		gigaset_rcv_error(procskb, bcs->cs, bcs);
	} else if (bcs->fcs != PPP_GOODFCS) {
		dev_notice(bcs->cs->dev, "frame check error (0x%04x)\n",
			   bcs->fcs);
		bcs->hw.bas->fcserrs++;
		gigaset_rcv_error(procskb, bcs->cs, bcs);
	} else {
		procskb->len -= 2;		/* subtract FCS */
		procskb->tail -= 2;
		gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)",
			__func__, procskb->len);
		dump_bytes(DEBUG_STREAM,
			   "rcv data", procskb->data, procskb->len);
		bcs->hw.bas->goodbytes += procskb->len;
		gigaset_rcv_skb(procskb, bcs->cs, bcs);
	}

	if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
		skb_reserve(bcs->skb, HW_HDR_LEN);
	else
		dev_err(bcs->cs->dev, "could not allocate skb\n");
	bcs->fcs = PPP_INITFCS;
}

/* hdlc_frag
 * drop HDLC data packet with non-integral last byte
 */
static inline void hdlc_frag(struct bc_state *bcs, unsigned inbits)
{
	if (unlikely(bcs->ignore)) {
		bcs->ignore--;
		hdlc_flush(bcs);
		return;
	}

	dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
	bcs->hw.bas->alignerrs++;
	gigaset_rcv_error(bcs->skb, bcs->cs, bcs);

	if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
		skb_reserve(bcs->skb, HW_HDR_LEN);
	else
		dev_err(bcs->cs->dev, "could not allocate skb\n");
	bcs->fcs = PPP_INITFCS;
}

/* bit counts lookup table for HDLC bit unstuffing
 * index: input byte
 * value: bit 0..3 = number of consecutive '1' bits starting from LSB
 *        bit 4..6 = number of consecutive '1' bits starting from MSB
 *		     (replacing 8 by 7 to make it fit; the algorithm won't care)
 *        bit 7 set if there are 5 or more "interior" consecutive '1' bits
 */
static unsigned char bitcounts[256] = {
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x80, 0x06,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
  0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x80, 0x81, 0x80, 0x07,
  0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
  0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x15,
  0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
  0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x90, 0x16,
  0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x24,
  0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x25,
  0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x33, 0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x34,
  0x40, 0x41, 0x40, 0x42, 0x40, 0x41, 0x40, 0x43, 0x50, 0x51, 0x50, 0x52, 0x60, 0x61, 0x70, 0x78
};

/* hdlc_unpack
 * perform HDLC frame processing (bit unstuffing, flag detection, FCS calculation)
 * on a sequence of received data bytes (8 bits each, LSB first)
 * pass on successfully received, complete frames as SKBs via gigaset_rcv_skb
 * notify of errors via gigaset_rcv_error
 * 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 hdlc_unpack(unsigned char *src, unsigned count,
			       struct bc_state *bcs)
{
	struct bas_bc_state *ubc = bcs->hw.bas;
	int inputstate;
	unsigned seqlen, inbyte, inbits;

	/* load previous state:
	 * inputstate = set of flag bits:
	 * - INS_flag_hunt: no complete opening flag received since connection setup or last abort
	 * - INS_have_data: at least one complete data byte received since last flag
	 * seqlen = number of consecutive '1' bits in last 7 input stream bits (0..7)
	 * inbyte = accumulated partial data byte (if !INS_flag_hunt)
	 * inbits = number of valid bits in inbyte, starting at LSB (0..6)