dvb_net.c

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

/* 
 * dvb_net.c
 *
 * Copyright (C) 2001 Convergence integrated media GmbH
 *                    Ralph Metzler <[e-mail]>
 * Copyright (C) 2002 Ralph Metzler <[e-mail]>
 *
 * ULE Decapsulation code:
 * Copyright (C) 2003 gcs - Global Communication & Services GmbH.
 *                and Institute for Computer Sciences
 *                    Salzburg University.
 *                    Hilmar Linder <[e-mail]>
 *                and Wolfram Stering <[e-mail]>
 *
 * ULE Decaps according to draft-fair-ipdvb-ule-01.txt.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
 */

#include <linux/kernel.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/dvb/net.h>
#include <linux/uio.h>
#include <asm/uaccess.h>
#include <linux/crc32.h>

#include "dvb_demux.h"
#include "dvb_net.h"
#include "dvb_functions.h"


static inline __u32 iov_crc32( __u32 c, struct kvec *iov, unsigned int cnt )
{
	unsigned int j;
	for (j = 0; j < cnt; j++)
		c = crc32_be( c, iov[j].iov_base, iov[j].iov_len );
	return c;
}


#if 1
#define dprintk(x...) printk(x)
#else
#define dprintk(x...)
#endif


#define DVB_NET_MULTICAST_MAX 10

#define isprint(c)	((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9'))

static void hexdump( const unsigned char *buf, unsigned short len )
{
	char str[80], octet[10];
	int ofs, i, l;

	for (ofs = 0; ofs < len; ofs += 16) {
		sprintf( str, "%03d: ", ofs );

		for (i = 0; i < 16; i++) {
			if ((i + ofs) < len)
				sprintf( octet, "%02x ", buf[ofs + i] );
			else
				strcpy( octet, "   " );

			strcat( str, octet );
		}
		strcat( str, "  " );
		l = strlen( str );

		for (i = 0; (i < 16) && ((i + ofs) < len); i++)
			str[l++] = isprint( buf[ofs + i] ) ? buf[ofs + i] : '.';

		str[l] = '\0';
		printk( KERN_WARNING "%s\n", str );
	}
}


struct dvb_net_priv {
	int in_use;
        struct net_device_stats stats;
        char name[6];
	u16 pid;
	struct dvb_net *host;
        struct dmx_demux *demux;
	struct dmx_section_feed *secfeed;
	struct dmx_section_filter *secfilter;
	struct dmx_ts_feed *tsfeed;
	int multi_num;
	struct dmx_section_filter *multi_secfilter[DVB_NET_MULTICAST_MAX];
	unsigned char multi_macs[DVB_NET_MULTICAST_MAX][6];
	int rx_mode;
#define RX_MODE_UNI 0
#define RX_MODE_MULTI 1
#define RX_MODE_ALL_MULTI 2
#define RX_MODE_PROMISC 3
	struct work_struct set_multicast_list_wq;
	struct work_struct restart_net_feed_wq;
	unsigned char feedtype;
	int need_pusi;
	unsigned char tscc;			/* TS continuity counter after sync. */
	struct sk_buff *ule_skb;
	unsigned short ule_sndu_len;
	unsigned short ule_sndu_type;
	unsigned char ule_sndu_type_1;
	unsigned char ule_dbit;			/* whether the DestMAC address present
						 * bit is set or not. */
	unsigned char ule_ethhdr_complete;	/* whether we have completed the Ethernet
						 * header for the current ULE SNDU. */
	int ule_sndu_remain;
};


/**
 *	Determine the packet's protocol ID. The rule here is that we 
 *	assume 802.3 if the type field is short enough to be a length.
 *	This is normal practice and works for any 'now in use' protocol.
 *
 *  stolen from eth.c out of the linux kernel, hacked for dvb-device
 *  by Michael Holzt <[e-mail]>
 */
static unsigned short dvb_net_eth_type_trans(struct sk_buff *skb,
				      struct net_device *dev)
{
	struct ethhdr *eth;
	unsigned char *rawp;
	
	skb->mac.raw=skb->data;
	skb_pull(skb,dev->hard_header_len);
	eth = eth_hdr(skb);
	
	if (*eth->h_dest & 1) {
		if(memcmp(eth->h_dest,dev->broadcast, ETH_ALEN)==0)
			skb->pkt_type=PACKET_BROADCAST;
		else
			skb->pkt_type=PACKET_MULTICAST;
	}
	
	if (ntohs(eth->h_proto) >= 1536)
		return eth->h_proto;
		
	rawp = skb->data;
	
	/**
	 *	This is a magic hack to spot IPX packets. Older Novell breaks
	 *	the protocol design and runs IPX over 802.3 without an 802.2 LLC
	 *	layer. We look for FFFF which isn't a used 802.2 SSAP/DSAP. This
	 *	won't work for fault tolerant netware but does for the rest.
	 */
	if (*(unsigned short *)rawp == 0xFFFF)
		return htons(ETH_P_802_3);
		
	/**
	 *	Real 802.2 LLC
	 */
	return htons(ETH_P_802_2);
}

#define TS_SZ	188
#define TS_SYNC	0x47
#define TS_TEI	0x80
#define TS_PUSI	0x40
#define TS_AF_A	0x20
#define TS_AF_D	0x10

#define ULE_TEST	0
#define ULE_BRIDGED	1
#define ULE_LLC		2

static inline void reset_ule( struct dvb_net_priv *p )
{
	p->ule_skb = NULL;
	p->ule_sndu_len = 0;
	p->ule_sndu_type = 0;
	p->ule_sndu_type_1 = 0;
	p->ule_sndu_remain = 0;
	p->ule_dbit = 0xFF;
	p->ule_ethhdr_complete = 0;
}

static const char eth_dest_addr[] = { 0x0b, 0x0a, 0x09, 0x08, 0x04, 0x03 };

static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
{
	struct dvb_net_priv *priv = (struct dvb_net_priv *)dev->priv;
	unsigned long skipped = 0L, skblen = 0L;
	u8 *ts, *ts_end, *from_where = NULL, ts_remain = 0, how_much = 0, new_ts = 1;
	struct ethhdr *ethh = NULL;
	unsigned int emergency_count = 0;

	if (dev == NULL) {
		printk( KERN_ERR "NO netdev struct!\n" );
		return;
	}

	for (ts = (char *)buf, ts_end = (char *)buf + buf_len; ts < ts_end; ) {

		if (emergency_count++ > 200) {
			/* Huh?? */
			hexdump(ts, TS_SZ);
			printk(KERN_WARNING "*** LOOP ALERT! ts %p ts_remain %u "
				"how_much %u, ule_skb %p, ule_len %u, ule_remain %u\n",
				ts, ts_remain, how_much, priv->ule_skb,
				priv->ule_sndu_len, priv->ule_sndu_remain);
			break;
		}

		if (new_ts) {
			if ((ts[0] != TS_SYNC) || (ts[1] & TS_TEI)) {
				printk(KERN_WARNING "Invalid TS cell: SYNC %#x, TEI %u.\n",
				       ts[0], ts[1] & TS_TEI >> 7);
				continue;
			}
			ts_remain = 184;
			from_where = ts + 4;
		}
		/* Synchronize on PUSI, if required. */
		if (priv->need_pusi) {
			if (ts[1] & TS_PUSI) {
				/* Find beginning of first ULE SNDU in current TS cell.
				 * priv->need_pusi = 0; */
				priv->tscc = ts[3] & 0x0F;
				/* There is a pointer field here. */
				if (ts[4] > ts_remain) {
					printk(KERN_ERR "Invalid ULE packet "
					       "(pointer field %d)\n", ts[4]);
					continue;
				}
				from_where = &ts[5] + ts[4];
				ts_remain -= 1 + ts[4];
				skipped = 0;
			} else {
				skipped++;
				continue;
			}
		}

		/* Check continuity counter. */
		if (new_ts) {
			if ((ts[3] & 0x0F) == priv->tscc)
				priv->tscc = (priv->tscc + 1) & 0x0F;
			else {
				/* TS discontinuity handling: */
				if (priv->ule_skb) {
					dev_kfree_skb( priv->ule_skb );
					/* Prepare for next SNDU. */
					reset_ule(priv);
					((struct dvb_net_priv *) dev->priv)->stats.rx_errors++;
					((struct dvb_net_priv *) dev->priv)->stats.rx_frame_errors++;
				}
				/* skip to next PUSI. */
				printk(KERN_WARNING "TS discontinuity: got %#x, "
				       "exptected %#x.\n", ts[3] & 0x0F, priv->tscc);
				priv->need_pusi = 1;
				continue;
			}
			/* If we still have an incomplete payload, but PUSI is
			 * set, some TS cells are missing.
			 * This is only possible here, if we missed exactly 16 TS
			 * cells (continuity counter). */
			if (ts[1] & TS_PUSI) {
				if (! priv->need_pusi) {
					/* printk(KERN_WARNING "Skipping pointer field %u.\n", *from_where); */
					if (*from_where > 181) {
						printk(KERN_WARNING "*** Invalid pointer "
						       "field: %u.  Current TS cell "
						       "follows:\n", *from_where);
						hexdump( ts, TS_SZ );
						printk(KERN_WARNING "-------------------\n");
					}
					/* Skip pointer field (we're processing a
					 * packed payload). */
					from_where += 1;
					ts_remain -= 1;
				} else
					priv->need_pusi = 0;

				if (priv->ule_sndu_remain > 183) {
					((struct dvb_net_priv *) dev->priv)->stats.rx_errors++;
					((struct dvb_net_priv *) dev->priv)->stats.rx_length_errors++;
					printk(KERN_WARNING "Expected %d more SNDU bytes, but "
					       "got PUSI.  Flushing incomplete payload.\n",
					       priv->ule_sndu_remain);
					dev_kfree_skb(priv->ule_skb);
					/* Prepare for next SNDU. */
					reset_ule(priv);
				}
			}
		}

		/* Check if new payload needs to be started. */
		if (priv->ule_skb == NULL) {
			/* Start a new payload w/ skb.
			 * Find ULE header.  It is only guaranteed that the
			 * length field (2 bytes) is contained in the current
			 * TS.
			 * Check ts_remain has to be >= 2 here. */
			if (ts_remain < 2) {
				printk(KERN_WARNING "Invalid payload packing: only %d "
				       "bytes left in TS.  Resyncing.\n", ts_remain);
				priv->ule_sndu_len = 0;
				priv->need_pusi = 1;
				continue;
			}

			if (! priv->ule_sndu_len) {
				priv->ule_sndu_len = from_where[0] << 8 | from_where[1];
				if (priv->ule_sndu_len & 0x8000) {
					/* D-Bit is set: no dest mac present. */
					priv->ule_sndu_len &= 0x7FFF;
					priv->ule_dbit = 1;
				} else
					priv->ule_dbit = 0;

				/* printk(KERN_WARNING "ULE D-Bit: %d, SNDU len %u.\n",
				          priv->ule_dbit, priv->ule_sndu_len); */

				if (priv->ule_sndu_len > 32763) {
					printk(KERN_WARNING "Invalid ULE SNDU length %u. "
					       "Resyncing.\n", priv->ule_sndu_len);
					hexdump(ts, TS_SZ);
					priv->ule_sndu_len = 0;
					priv->need_pusi = 1;
					new_ts = 1;
					ts += TS_SZ;
					continue;
				}
				ts_remain -= 2;	/* consume the 2 bytes SNDU length. */
				from_where += 2;
			}

			/*
			 * State of current TS:
			 *   ts_remain (remaining bytes in the current TS cell)
			 *   0	ule_type is not available now, we need the next TS cell
			 *   1	the first byte of the ule_type is present
			 * >=2	full ULE header present, maybe some payload data as well.
			 */
			switch (ts_remain) {
				case 1:
					priv->ule_sndu_type = from_where[0] << 8;
					priv->ule_sndu_type_1 = 1; /* first byte of ule_type is set. */
					/* ts_remain -= 1; from_where += 1;
					 *   here not necessary, because we continue. */
				case 0:
					new_ts = 1;
					ts += TS_SZ;
					continue;

				default: /* complete ULE header is present in current TS. */
					/* Extract ULE type field. */
					if (priv->ule_sndu_type_1) {
						priv->ule_sndu_type |= from_where[0];
						from_where += 1; /* points to payload start. */
						ts_remain -= 1;
					} else {
						/* Complete type is present in new TS. */
						priv->ule_sndu_type = from_where[0] << 8 | from_where[1];
						from_where += 2; /* points to payload start. */
						ts_remain -= 2;
					}
					break;
			}

			if (priv->ule_sndu_type == ULE_TEST) {
				/* Test SNDU, discarded by the receiver. */
				printk(KERN_WARNING "Discarding ULE Test SNDU (%d bytes). "
				       "Resyncing.\n", priv->ule_sndu_len);
				priv->ule_sndu_len = 0;
				priv->need_pusi = 1;
				continue;
			}

			skblen = priv->ule_sndu_len;	/* Including CRC32 */
			if (priv->ule_sndu_type != ULE_BRIDGED) {
				skblen += ETH_HLEN;
#if 1
				if (! priv->ule_dbit)
					skblen -= ETH_ALEN;
#endif
			}
			priv->ule_skb = dev_alloc_skb(skblen);
			if (priv->ule_skb == NULL) {
				printk(KERN_NOTICE "%s: Memory squeeze, dropping packet.\n",
				       dev->name);
				((struct dvb_net_priv *)dev->priv)->stats.rx_dropped++;
				return;
			}

#if 0
			if (priv->ule_sndu_type != ULE_BRIDGED) {
				// skb_reserve(priv->ule_skb, 2);    /* longword align L3 header */
				// Create Ethernet header.