dvb_net.c

linux-2.6.15.6

		}

		/* Check if new payload needs to be started. */
		if (priv->ule_skb == NULL) {
			/* Start a new payload with 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) {
				/* Got at least two bytes, thus extrace the SNDU length. */
				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;

				if (priv->ule_sndu_len > 32763) {
					printk(KERN_WARNING "%lu: Invalid ULE SNDU length %u. "
					       "Resyncing.\n", priv->ts_count, priv->ule_sndu_len);
					priv->ule_sndu_len = 0;
					priv->need_pusi = 1;
					new_ts = 1;
					ts += TS_SZ;
					priv->ts_count++;
					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;
					/* Continue w/ next TS. */
				case 0:
					new_ts = 1;
					ts += TS_SZ;
					priv->ts_count++;
					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;
			}

			/* Allocate the skb (decoder target buffer) with the correct size, as follows:
			 * prepare for the largest case: bridged SNDU with MAC address (dbit = 0). */
			priv->ule_skb = dev_alloc_skb( priv->ule_sndu_len + ETH_HLEN + ETH_ALEN );
			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;
			}

			/* This includes the CRC32 _and_ dest mac, if !dbit. */
			priv->ule_sndu_remain = priv->ule_sndu_len;
			priv->ule_skb->dev = dev;
			/* Leave space for Ethernet or bridged SNDU header (eth hdr plus one MAC addr). */
			skb_reserve( priv->ule_skb, ETH_HLEN + ETH_ALEN );
		}

		/* Copy data into our current skb. */
		how_much = min(priv->ule_sndu_remain, (int)ts_remain);
		memcpy(skb_put(priv->ule_skb, how_much), from_where, how_much);
		priv->ule_sndu_remain -= how_much;
		ts_remain -= how_much;
		from_where += how_much;

		/* Check for complete payload. */
		if (priv->ule_sndu_remain <= 0) {
			/* Check CRC32, we've got it in our skb already. */
			unsigned short ulen = htons(priv->ule_sndu_len);
			unsigned short utype = htons(priv->ule_sndu_type);
			struct kvec iov[3] = {
				{ &ulen, sizeof ulen },
				{ &utype, sizeof utype },
				{ priv->ule_skb->data, priv->ule_skb->len - 4 }
			};
			unsigned long ule_crc = ~0L, expected_crc;
			if (priv->ule_dbit) {
				/* Set D-bit for CRC32 verification,
				 * if it was set originally. */
				ulen |= 0x0080;
			}

			ule_crc = iov_crc32(ule_crc, iov, 3);
			expected_crc = *((u8 *)priv->ule_skb->tail - 4) << 24 |
				       *((u8 *)priv->ule_skb->tail - 3) << 16 |
				       *((u8 *)priv->ule_skb->tail - 2) << 8 |
				       *((u8 *)priv->ule_skb->tail - 1);
			if (ule_crc != expected_crc) {
				printk(KERN_WARNING "%lu: CRC32 check FAILED: %#lx / %#lx, SNDU len %d type %#x, ts_remain %d, next 2: %x.\n",
				       priv->ts_count, ule_crc, expected_crc, priv->ule_sndu_len, priv->ule_sndu_type, ts_remain, ts_remain > 2 ? *(unsigned short *)from_where : 0);

#ifdef ULE_DEBUG
				hexdump( iov[0].iov_base, iov[0].iov_len );
				hexdump( iov[1].iov_base, iov[1].iov_len );
				hexdump( iov[2].iov_base, iov[2].iov_len );

				if (ule_where == ule_hist) {
					hexdump( &ule_hist[98*TS_SZ], TS_SZ );
					hexdump( &ule_hist[99*TS_SZ], TS_SZ );
				} else if (ule_where == &ule_hist[TS_SZ]) {
					hexdump( &ule_hist[99*TS_SZ], TS_SZ );
					hexdump( ule_hist, TS_SZ );
				} else {
					hexdump( ule_where - TS_SZ - TS_SZ, TS_SZ );
					hexdump( ule_where - TS_SZ, TS_SZ );
				}
				ule_dump = 1;
#endif

				((struct dvb_net_priv *) dev->priv)->stats.rx_errors++;
				((struct dvb_net_priv *) dev->priv)->stats.rx_crc_errors++;
				dev_kfree_skb(priv->ule_skb);
			} else {
				/* CRC32 verified OK. */
				/* Handle ULE Extension Headers. */
				if (priv->ule_sndu_type < 1536) {
					/* There is an extension header.  Handle it accordingly. */
					int l = handle_ule_extensions( priv );
					if (l < 0) {
						/* Mandatory extension header unknown or TEST SNDU.  Drop it. */
						// printk( KERN_WARNING "Dropping SNDU, extension headers.\n" );
						dev_kfree_skb( priv->ule_skb );
						goto sndu_done;
					}
					skb_pull( priv->ule_skb, l );
				}

				/* CRC32 was OK. Remove it from skb. */
				priv->ule_skb->tail -= 4;
				priv->ule_skb->len -= 4;

				/* Filter on receiver's destination MAC address, if present. */
				if (!priv->ule_dbit) {
					/* The destination MAC address is the next data in the skb. */
					if (memcmp( priv->ule_skb->data, dev->dev_addr, ETH_ALEN )) {
						/* MAC addresses don't match.  Drop SNDU. */
						// printk( KERN_WARNING "Dropping SNDU, MAC address.\n" );
						dev_kfree_skb( priv->ule_skb );
						goto sndu_done;
					}
					if (! priv->ule_bridged) {
						skb_push( priv->ule_skb, ETH_ALEN + 2 );
						ethh = (struct ethhdr *)priv->ule_skb->data;
						memcpy( ethh->h_dest, ethh->h_source, ETH_ALEN );
						memset( ethh->h_source, 0, ETH_ALEN );
						ethh->h_proto = htons( priv->ule_sndu_type );
					} else {
						/* Skip the Receiver destination MAC address. */
						skb_pull( priv->ule_skb, ETH_ALEN );
					}
				} else {
					if (! priv->ule_bridged) {
						skb_push( priv->ule_skb, ETH_HLEN );
						ethh = (struct ethhdr *)priv->ule_skb->data;
						memcpy( ethh->h_dest, dev->dev_addr, ETH_ALEN );
						memset( ethh->h_source, 0, ETH_ALEN );
						ethh->h_proto = htons( priv->ule_sndu_type );
					} else {
						/* skb is in correct state; nothing to do. */
					}
				}
				priv->ule_bridged = 0;

				/* Stuff into kernel's protocol stack. */
				priv->ule_skb->protocol = dvb_net_eth_type_trans(priv->ule_skb, dev);
				/* If D-bit is set (i.e. destination MAC address not present),
				 * receive the packet anyhow. */
				/* if (priv->ule_dbit && skb->pkt_type == PACKET_OTHERHOST)
					priv->ule_skb->pkt_type = PACKET_HOST; */
				((struct dvb_net_priv *) dev->priv)->stats.rx_packets++;
				((struct dvb_net_priv *) dev->priv)->stats.rx_bytes += priv->ule_skb->len;
				netif_rx(priv->ule_skb);
			}
			sndu_done:
			/* Prepare for next SNDU. */
			reset_ule(priv);
		}

		/* More data in current TS (look at the bytes following the CRC32)? */
		if (ts_remain >= 2 && *((unsigned short *)from_where) != 0xFFFF) {
			/* Next ULE SNDU starts right there. */
			new_ts = 0;
			priv->ule_skb = NULL;
			priv->ule_sndu_type_1 = 0;
			priv->ule_sndu_len = 0;
			// printk(KERN_WARNING "More data in current TS: [%#x %#x %#x %#x]\n",
			//	*(from_where + 0), *(from_where + 1),
			//	*(from_where + 2), *(from_where + 3));
			// printk(KERN_WARNING "ts @ %p, stopped @ %p:\n", ts, from_where + 0);
			// hexdump(ts, 188);
		} else {
			new_ts = 1;
			ts += TS_SZ;
			priv->ts_count++;
			if (priv->ule_skb == NULL) {
				priv->need_pusi = 1;
				priv->ule_sndu_type_1 = 0;
				priv->ule_sndu_len = 0;
			}
		}
	}	/* for all available TS cells */
}

static int dvb_net_ts_callback(const u8 *buffer1, size_t buffer1_len,
			       const u8 *buffer2, size_t buffer2_len,
			       struct dmx_ts_feed *feed, enum dmx_success success)
{
	struct net_device *dev = feed->priv;

	if (buffer2 != 0)
		printk(KERN_WARNING "buffer2 not 0: %p.\n", buffer2);
	if (buffer1_len > 32768)
		printk(KERN_WARNING "length > 32k: %zu.\n", buffer1_len);
	/* printk("TS callback: %u bytes, %u TS cells @ %p.\n",
		  buffer1_len, buffer1_len / TS_SZ, buffer1); */
	dvb_net_ule(dev, buffer1, buffer1_len);
	return 0;
}


static void dvb_net_sec(struct net_device *dev, u8 *pkt, int pkt_len)
{
	u8 *eth;
	struct sk_buff *skb;
	struct net_device_stats *stats = &(((struct dvb_net_priv *) dev->priv)->stats);
	int snap = 0;

	/* note: pkt_len includes a 32bit checksum */
	if (pkt_len < 16) {
		printk("%s: IP/MPE packet length = %d too small.\n",
			dev->name, pkt_len);
		stats->rx_errors++;
		stats->rx_length_errors++;
		return;
	}
/* it seems some ISPs manage to screw up here, so we have to
 * relax the error checks... */
#if 0
	if ((pkt[5] & 0xfd) != 0xc1) {
		/* drop scrambled or broken packets */
#else
	if ((pkt[5] & 0x3c) != 0x00) {
		/* drop scrambled */
#endif
		stats->rx_errors++;
		stats->rx_crc_errors++;
		return;
	}
	if (pkt[5] & 0x02) {
		/* handle LLC/SNAP, see rfc-1042 */
		if (pkt_len < 24 || memcmp(&pkt[12], "\xaa\xaa\x03\0\0\0", 6)) {
			stats->rx_dropped++;
			return;
		}
		snap = 8;
	}
	if (pkt[7]) {
		/* FIXME: assemble datagram from multiple sections */
		stats->rx_errors++;
		stats->rx_frame_errors++;
		return;
	}

	/* we have 14 byte ethernet header (ip header follows);
	 * 12 byte MPE header; 4 byte checksum; + 2 byte alignment, 8 byte LLC/SNAP
	 */
	if (!(skb = dev_alloc_skb(pkt_len - 4 - 12 + 14 + 2 - snap))) {
		//printk(KERN_NOTICE "%s: Memory squeeze, dropping packet.\n", dev->name);
		stats->rx_dropped++;
		return;
	}
	skb_reserve(skb, 2);    /* longword align L3 header */
	skb->dev = dev;

	/* copy L3 payload */
	eth = (u8 *) skb_put(skb, pkt_len - 12 - 4 + 14 - snap);
	memcpy(eth + 14, pkt + 12 + snap, pkt_len - 12 - 4 - snap);

	/* create ethernet header: */
	eth[0]=pkt[0x0b];
	eth[1]=pkt[0x0a];
	eth[2]=pkt[0x09];
	eth[3]=pkt[0x08];
	eth[4]=pkt[0x04];
	eth[5]=pkt[0x03];

	eth[6]=eth[7]=eth[8]=eth[9]=eth[10]=eth[11]=0;

	if (snap) {
		eth[12] = pkt[18];
		eth[13] = pkt[19];
	} else {
		/* protocol numbers are from rfc-1700 or
		 * http://www.iana.org/assignments/ethernet-numbers
		 */
		if (pkt[12] >> 4 == 6) { /* version field from IP header */
			eth[12] = 0x86;	/* IPv6 */
			eth[13] = 0xdd;
		} else {
			eth[12] = 0x08;	/* IPv4 */
			eth[13] = 0x00;
		}
	}

	skb->protocol = dvb_net_eth_type_trans(skb, dev);

	stats->rx_packets++;
	stats->rx_bytes+=skb->len;
	netif_rx(skb);
}

static int dvb_net_sec_callback(const u8 *buffer1, size_t buffer1_len,
		 const u8 *buffer2, size_t buffer2_len,
		 struct dmx_section_filter *filter,
		 enum dmx_success success)
{
	struct net_device *dev = filter->priv;

	/**
	 * we rely on the DVB API definition where exactly one complete
	 * section is delivered in buffer1
	 */
	dvb_net_sec (dev, (u8*) buffer1, buffer1_len);
	return 0;
}

static int dvb_net_tx(struct sk_buff *skb, struct net_device *dev)
{
	dev_kfree_skb(skb);
	return 0;
}

static u8 mask_normal[6]={0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
static u8 mask_allmulti[6]={0xff, 0xff, 0xff, 0x00, 0x00, 0x00};
static u8 mac_allmulti[6]={0x01, 0x00, 0x5e, 0x00, 0x00, 0x00};
static u8 mask_promisc[6]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

static int dvb_net_filter_sec_set(struct net_device *dev,
		   struct dmx_section_filter **secfilter,
		   u8 *mac, u8 *mac_mask)
{
	struct dvb_net_priv *priv = dev->priv;
	int ret;

	*secfilter=NULL;
	ret = priv->secfeed->allocate_filter(priv->secfeed, secfilter);
	if (ret<0) {
		printk("%s: could not get filter\n", dev->name);
		return ret;
	}

	(*secfilter)->priv=(void *) dev;

	memset((*secfilter)->filter_value, 0x00, DMX_MAX_FILTER_SIZE);
	memset((*secfilter)->filter_mask,  0x00, DMX_MAX_FILTER_SIZE);
	memset((*secfilter)->filter_mode,  0xff, DMX_MAX_FILTER_SIZE);

	(*secfilter)->filter_value[0]=0x3e;
	(*secfilter)->filter_value[3]=mac[5];
	(*secfilter)->filter_value[4]=mac[4];
	(*secfilter)->filter_value[8]=mac[3];
	(*secfilter)->filter_value[9]=mac[2];
	(*secfilter)->filter_value[10]=mac[1];
	(*secfilter)->filter_value[11]=mac[0];

	(*secfilter)->filter_mask[0] = 0xff;
	(*secfilter)->filter_mask[3] = mac_mask[5];
	(*secfilter)->filter_mask[4] = mac_mask[4];
	(*secfilter)->filter_mask[8] = mac_mask[3];
	(*secfilter)->filter_mask[9] = mac_mask[2];
	(*secfilter)->filter_mask[10] = mac_mask[1];
	(*secfilter)->filter_mask[11]=mac_mask[0];

	dprintk("%s: filter mac=%02x %02x %02x %02x %02x %02x\n",
	       dev->name, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
	dprintk("%s: filter mask=%02x %02x %02x %02x %02x %02x\n",
	       dev->name, mac_mask[0], mac_mask[1], mac_mask[2],
	       mac_mask[3], mac_mask[4], mac_mask[5]);

	return 0;
}

static int dvb_net_feed_start(struct net_device *dev)
{
	int ret = 0, i;
	struct dvb_net_priv *priv = dev->priv;
	struct dmx_demux *demux = priv->demux;
	unsigned char *mac = (unsigned char *) dev->dev_addr;

	dprintk("%s: rx_mode %i\n", __FUNCTION__, priv->rx_mode);
	down(&priv->mutex);
	if (priv->tsfeed || priv->secfeed || priv->secfilter || priv->multi_secfilter[0])
		printk("%s: BUG %d\n", __FUNCTION__, __LINE__);

	priv->secfeed=NULL;
	priv->secfilter=NULL;
	priv->tsfeed = NULL;

	if (priv->feedtype == DVB_NET_FEEDTYPE_MPE) {
		dprintk("%s: alloc secfeed\n", __FUNCTION__);
		ret=demux->allocate_section_feed(demux, &priv->secfeed,
					 dvb_net_sec_callback);
		if (ret<0) {
			printk("%s: could not allocate section feed\n", dev->name);
			goto error;
		}

		ret = priv->secfeed->set(priv->secfeed, priv->pid, 32768, 1);

		if (ret<0) {
			printk("%s: could not set section feed\n", dev->name);
			priv->demux->release_section_feed(priv->demux, priv->secfeed);
			priv->secfeed=NULL;
			goto error;
		}

		if (priv->rx_mode != RX_MODE_PROMISC) {
			dprintk("%s: set secfilter\n", __FUNCTION__);
			dvb_net_filter_sec_set(dev, &priv->secfilter, mac, mask_normal);
		}

		switch (priv->rx_mode) {
		case RX_MODE_MULTI:
			for (i = 0; i < priv->multi_num; i++) {
				dprintk("%s: set multi_secfilter[%d]\n", __FUNCTION__, i);
				dvb_net_filter_sec_set(dev, &priv->multi_secfilter[i],
						       priv->multi_macs[i], mask_normal);
			}
			break;
		case RX_MODE_ALL_MULTI:
			priv->multi_num=1;
			dprintk("%s: set multi_secfilter[0]\n", __FUNCTION__);
			dvb_net_filter_sec_set(dev, &priv->multi_secfilter[0],