[RFC 2/5] Bluetooth: Enable 6LoWPAN support for BT LE devices

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



---
 net/bluetooth/ble_6lowpan.c | 1114 ++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 1112 insertions(+), 2 deletions(-)

diff --git a/net/bluetooth/ble_6lowpan.c b/net/bluetooth/ble_6lowpan.c
index 0fd3302..5b3ebe1 100644
--- a/net/bluetooth/ble_6lowpan.c
+++ b/net/bluetooth/ble_6lowpan.c
@@ -69,6 +69,9 @@ struct lowpan_fragment {
 	struct list_head	list;		/* fragments list */
 };
 
+static LIST_HEAD(lowpan_fragments);
+static DEFINE_SPINLOCK(flist_lock);
+
 #define DELETE_TIMEOUT msecs_to_jiffies(1)
 
 /* TTL uncompression values */
@@ -110,8 +113,723 @@ static inline void ble_6lowpan_raw_dump_table(const char *caller, char *msg,
 #endif /* DEBUG */
 }
 
+static inline bool ble_6lowpan_fetch_skb(struct sk_buff *skb,
+					void *data, const unsigned int len)
+{
+	if (unlikely(!pskb_may_pull(skb, len)))
+		return true;
+
+	skb_copy_from_linear_data(skb, data, len);
+	skb_pull(skb, len);
+
+	return false;
+}
+
+/*
+ * Uncompress address function for source and
+ * destination address(non-multicast).
+ *
+ * address_mode is sam value or dam value.
+ */
+static int ble_6lowpan_uncompress_addr(struct sk_buff *skb,
+				struct in6_addr *ipaddr,
+				const u8 address_mode,
+				const u8 *lladdr)
+{
+	bool fail;
+
+	switch (address_mode) {
+	case LOWPAN_IPHC_ADDR_00:
+		/* for global link addresses */
+		fail = ble_6lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
+		break;
+	case LOWPAN_IPHC_ADDR_01:
+		/* fe:80::XXXX:XXXX:XXXX:XXXX */
+		ipaddr->s6_addr[0] = 0xFE;
+		ipaddr->s6_addr[1] = 0x80;
+		fail = ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
+		break;
+	case LOWPAN_IPHC_ADDR_02:
+		/* fe:80::ff:fe00:XXXX */
+		ipaddr->s6_addr[0] = 0xFE;
+		ipaddr->s6_addr[1] = 0x80;
+		ipaddr->s6_addr[11] = 0xFF;
+		ipaddr->s6_addr[12] = 0xFE;
+		fail = ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
+		break;
+	case LOWPAN_IPHC_ADDR_03:
+		fail = false;
+		/* XXX: support only normal addr (IEEE802154_ADDR_LONG) atm */
+
+		/* fe:80::XXXX:XXXX:XXXX:XXXX
+		 *        \_________________/
+		 *              hwaddr
+		 */
+		ipaddr->s6_addr[0] = 0xFE;
+		ipaddr->s6_addr[1] = 0x80;
+		memcpy(&ipaddr->s6_addr[8], lladdr, IEEE802154_ADDR_LEN);
+		break;
+	default:
+		pr_debug("Invalid address mode value: 0x%x\n", address_mode);
+		return -EINVAL;
+	}
+
+	if (fail) {
+		pr_debug("Failed to fetch skb data\n");
+		return -EIO;
+	}
+
+	ble_6lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 addr is",
+			ipaddr->s6_addr, 16);
+
+	return 0;
+}
+
+/*
+ * Uncompress address function for source context
+ * based address(non-multicast).
+ */
+static int ble_6lowpan_uncompress_context_based_src_addr(struct sk_buff *skb,
+						struct in6_addr *ipaddr,
+						const u8 sam)
+{
+	switch (sam) {
+	case LOWPAN_IPHC_ADDR_00:
+		/* unspec address ::
+		 * Do nothing, address is already ::
+		 */
+		break;
+	case LOWPAN_IPHC_ADDR_01:
+		/* TODO */
+	case LOWPAN_IPHC_ADDR_02:
+		/* TODO */
+	case LOWPAN_IPHC_ADDR_03:
+		/* TODO */
+		netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam);
+		return -EINVAL;
+	default:
+		pr_debug("Invalid sam value: 0x%x\n", sam);
+		return -EINVAL;
+	}
+
+	ble_6lowpan_raw_dump_inline(NULL,
+			"Reconstructed context based ipv6 src addr is",
+			ipaddr->s6_addr, 16);
+
+	return 0;
+}
+
+/*
+ * This func is called when the packet from BT LE device
+ * needs to be sent upper layers.
+ */
+static int ble_6lowpan_give_skb_to_upper(struct sk_buff *skb,
+					struct net_device *dev)
+{
+	struct sk_buff *skb_cp;
+	int ret = NET_RX_SUCCESS;
+
+	skb_cp = skb_copy(skb, GFP_ATOMIC);
+	if (!skb_cp) {
+		ret = -ENOMEM;
+	} else {
+		ret = netif_rx(skb_cp);
+
+		BT_DBG("receive skb %d", ret);
+		if (ret < 0)
+			ret = NET_RX_DROP;
+	}
+
+	return ret;
+}
+
+static inline int ble_6lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
+{
+	if (unlikely(!pskb_may_pull(skb, 1)))
+		return -EINVAL;
+
+	*val = skb->data[0];
+	skb_pull(skb, 1);
+
+	return 0;
+}
+
+static inline int ble_6lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
+{
+	if (unlikely(!pskb_may_pull(skb, 2)))
+		return -EINVAL;
+
+	*val = (skb->data[0] << 8) | skb->data[1];
+	skb_pull(skb, 2);
+
+	return 0;
+}
+
+static int ble_6lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr,
+				struct net_device *dev)
+{
+	struct sk_buff *new;
+	int stat;
+
+	new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
+								GFP_ATOMIC);
+	kfree_skb(skb);
+
+	if (!new)
+		return -ENOMEM;
+
+	skb_push(new, sizeof(struct ipv6hdr));
+	skb_reset_network_header(new);
+	skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr));
+
+	new->protocol = htons(ETH_P_IPV6);
+	new->pkt_type = PACKET_HOST;
+	new->dev = dev;
+
+	ble_6lowpan_raw_dump_table(__func__,
+				"raw skb data dump before receiving",
+				new->data, new->len);
+
+	stat = ble_6lowpan_give_skb_to_upper(new, dev);
+
+	kfree_skb(new);
+
+	return stat;
+}
+
+static void lowpan_fragment_timer_expired(unsigned long entry_addr)
+{
+	struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
+
+	pr_debug("timer expired for frame with tag %d\n", entry->tag);
+
+	list_del(&entry->list);
+	dev_kfree_skb(entry->skb);
+	kfree(entry);
+}
+
+static struct lowpan_fragment *ble_6lowpan_alloc_new_frame(struct sk_buff *skb,
+							u16 len, u16 tag)
+{
+	struct lowpan_fragment *frame;
+
+	frame = kzalloc(sizeof(struct lowpan_fragment),
+			GFP_ATOMIC);
+	if (!frame)
+		goto frame_err;
+
+	INIT_LIST_HEAD(&frame->list);
+
+	frame->length = len;
+	frame->tag = tag;
+
+	/* allocate buffer for frame assembling */
+	frame->skb = netdev_alloc_skb_ip_align(skb->dev, frame->length +
+					       sizeof(struct ipv6hdr));
+
+	if (!frame->skb)
+		goto skb_err;
+
+	frame->skb->priority = skb->priority;
+	frame->skb->dev = skb->dev;
+
+	/* reserve headroom for uncompressed ipv6 header */
+	skb_reserve(frame->skb, sizeof(struct ipv6hdr));
+	skb_put(frame->skb, frame->length);
+
+	/* copy the first control block to keep a
+	 * trace of the link-layer addresses in case
+	 * of a link-local compressed address
+	 */
+	memcpy(frame->skb->cb, skb->cb, sizeof(skb->cb));
+
+	init_timer(&frame->timer);
+	/* time out is the same as for ipv6 - 60 sec */
+	frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT;
+	frame->timer.data = (unsigned long)frame;
+	frame->timer.function = lowpan_fragment_timer_expired;
+
+	add_timer(&frame->timer);
+
+	list_add_tail(&frame->list, &lowpan_fragments);
+
+	return frame;
+
+skb_err:
+	kfree(frame);
+frame_err:
+	return NULL;
+}
+
+/* Uncompress function for multicast destination address,
+ * when M bit is set.
+ */
+static int
+lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
+		struct in6_addr *ipaddr,
+		const u8 dam)
+{
+	bool fail;
+
+	switch (dam) {
+	case LOWPAN_IPHC_DAM_00:
+		/* 00:  128 bits.  The full address
+		 * is carried in-line.
+		 */
+		fail = ble_6lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
+		break;
+	case LOWPAN_IPHC_DAM_01:
+		/* 01:  48 bits.  The address takes
+		 * the form ffXX::00XX:XXXX:XXXX.
+		 */
+		ipaddr->s6_addr[0] = 0xFF;
+		fail = ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
+		fail |= ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
+		break;
+	case LOWPAN_IPHC_DAM_10:
+		/* 10:  32 bits.  The address takes
+		 * the form ffXX::00XX:XXXX.
+		 */
+		ipaddr->s6_addr[0] = 0xFF;
+		fail = ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
+		fail |= ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
+		break;
+	case LOWPAN_IPHC_DAM_11:
+		/* 11:  8 bits.  The address takes
+		 * the form ff02::00XX.
+		 */
+		ipaddr->s6_addr[0] = 0xFF;
+		ipaddr->s6_addr[1] = 0x02;
+		fail = ble_6lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
+		break;
+	default:
+		pr_debug("DAM value has a wrong value: 0x%x\n", dam);
+		return -EINVAL;
+	}
+
+	if (fail) {
+		pr_debug("Failed to fetch skb data\n");
+		return -EIO;
+	}
+
+	ble_6lowpan_raw_dump_inline(NULL,
+				"Reconstructed ipv6 multicast addr is",
+				ipaddr->s6_addr, 16);
+
+	return 0;
+}
+
+static int
+ble_6lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh)
+{
+	u8 tmp;
+
+	if (!uh)
+		goto err;
+
+	if (ble_6lowpan_fetch_skb_u8(skb, &tmp))
+		goto err;
+
+	if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) {
+		pr_debug("UDP header uncompression\n");
+		switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
+		case LOWPAN_NHC_UDP_CS_P_00:
+			memcpy(&uh->source, &skb->data[0], 2);
+			memcpy(&uh->dest, &skb->data[2], 2);
+			skb_pull(skb, 4);
+			break;
+		case LOWPAN_NHC_UDP_CS_P_01:
+			memcpy(&uh->source, &skb->data[0], 2);
+			uh->dest =
+			   skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT;
+			skb_pull(skb, 3);
+			break;
+		case LOWPAN_NHC_UDP_CS_P_10:
+			uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT;
+			memcpy(&uh->dest, &skb->data[1], 2);
+			skb_pull(skb, 3);
+			break;
+		case LOWPAN_NHC_UDP_CS_P_11:
+			uh->source =
+			   LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4);
+			uh->dest =
+			   LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f);
+			skb_pull(skb, 1);
+			break;
+		default:
+			pr_debug("ERROR: unknown UDP format\n");
+			goto err;
+			break;
+		}
+
+		pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
+			 uh->source, uh->dest);
+
+		/* copy checksum */
+		memcpy(&uh->check, &skb->data[0], 2);
+		skb_pull(skb, 2);
+
+		/*
+		 * UDP lenght needs to be infered from the lower layers
+		 * here, we obtain the hint from the remaining size of the
+		 * frame
+		 */
+		uh->len = htons(skb->len + sizeof(struct udphdr));
+		pr_debug("uncompressed UDP length: src = %d", uh->len);
+	} else {
+		pr_debug("ERROR: unsupported NH format\n");
+		goto err;
+	}
+
+	return 0;
+err:
+	return -EINVAL;
+}
+
+static int ble_6lowpan_process_data(struct sk_buff *skb, struct net_device *dev)
+{
+	struct ipv6hdr hdr = {};
+	u8 tmp, iphc0, iphc1, num_context = 0;
+	const u8 *_saddr = NULL, *_daddr = NULL;
+	struct ble_6lowpan_dev_info *info;
+	int err;
+
+	ble_6lowpan_raw_dump_table(__func__, "raw skb data dump uncompressed",
+				skb->data, skb->len);
+
+	/* at least two bytes will be used for the encoding */
+	if (skb->len < 2)
+		goto drop;
+
+	if (ble_6lowpan_fetch_skb_u8(skb, &iphc0))
+		goto drop;
+
+	info = ble_6lowpan_dev_info(dev);
+
+	/* fragments assembling */
+	switch (iphc0 & LOWPAN_DISPATCH_MASK) {
+	case LOWPAN_DISPATCH_FRAG1:
+	case LOWPAN_DISPATCH_FRAGN:
+	{
+		struct lowpan_fragment *frame;
+		/* slen stores the rightmost 8 bits of the 11 bits length */
+		u8 slen, offset = 0;
+		u16 len, tag;
+		bool found = false;
+
+		if (ble_6lowpan_fetch_skb_u8(skb, &slen) || /* frame length */
+		    ble_6lowpan_fetch_skb_u16(skb, &tag))  /* fragment tag */
+			goto drop;
+
+		/* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */
+		len = ((iphc0 & 7) << 8) | slen;
+
+		if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) {
+			pr_debug("%s received a FRAG1 packet (tag: %d, "
+				 "size of the entire IP packet: %d)\n",
+				 __func__, tag, len);
+		} else { /* FRAGN */
+			if (ble_6lowpan_fetch_skb_u8(skb, &offset))
+				goto unlock_and_drop;
+			pr_debug("%s received a FRAGN packet (tag: %d, "
+				 "size of the entire IP packet: %d, "
+				 "offset: %d)\n", __func__, tag, len,
+				offset * 8);
+		}
+
+		/*
+		 * check if frame assembling with the same tag is
+		 * already in progress
+		 */
+		spin_lock_bh(&flist_lock);
+
+		list_for_each_entry(frame, &lowpan_fragments, list)
+			if (frame->tag == tag) {
+				found = true;
+				break;
+			}
+
+		/* alloc new frame structure */
+		if (!found) {
+			pr_debug("%s first fragment received for tag %d, "
+				 "begin packet reassembly\n", __func__, tag);
+			frame = ble_6lowpan_alloc_new_frame(skb, len, tag);
+			if (!frame)
+				goto unlock_and_drop;
+		}
+
+		/* if payload fits buffer, copy it */
+		if (likely((offset * 8 + skb->len) <= frame->length))
+			skb_copy_to_linear_data_offset(frame->skb, offset * 8,
+							skb->data, skb->len);
+		else
+			goto unlock_and_drop;
+
+		frame->bytes_rcv += skb->len;
+
+		/* frame assembling complete */
+		if ((frame->bytes_rcv == frame->length) &&
+		     frame->timer.expires > jiffies) {
+			/* if timer haven't expired - first of all delete it */
+			del_timer_sync(&frame->timer);
+			list_del(&frame->list);
+			spin_unlock_bh(&flist_lock);
+
+			pr_debug("%s successfully reassembled fragment "
+				 "(tag %d)\n", __func__, tag);
+
+			dev_kfree_skb(skb);
+			skb = frame->skb;
+			kfree(frame);
+
+			if (ble_6lowpan_fetch_skb_u8(skb, &iphc0))
+				goto drop;
+
+			break;
+		}
+		spin_unlock_bh(&flist_lock);
+
+		return kfree_skb(skb), 0;
+	}
+	default:
+		break;
+	}
+
+	if (ble_6lowpan_fetch_skb_u8(skb, &iphc1))
+		goto drop;
+
+	_saddr = info->ieee802154_addr;
+	_daddr = info->net->dev_addr;
+
+	/* another if the CID flag is set */
+	if (iphc1 & LOWPAN_IPHC_CID) {
+		pr_debug("CID flag is set, increase header with one\n");
+		if (ble_6lowpan_fetch_skb_u8(skb, &num_context))
+			goto drop;
+	}
+
+	hdr.version = 6;
+
+	/* Traffic Class and Flow Label */
+	switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
+	/*
+	 * Traffic Class and FLow Label carried in-line
+	 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
+	 */
+	case 0: /* 00b */
+		if (ble_6lowpan_fetch_skb_u8(skb, &tmp))
+			goto drop;
+
+		memcpy(&hdr.flow_lbl, &skb->data[0], 3);
+		skb_pull(skb, 3);
+		hdr.priority = ((tmp >> 2) & 0x0f);
+		hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
+					(hdr.flow_lbl[0] & 0x0f);
+		break;
+	/*
+	 * Traffic class carried in-line
+	 * ECN + DSCP (1 byte), Flow Label is elided
+	 */
+	case 1: /* 10b */
+		if (ble_6lowpan_fetch_skb_u8(skb, &tmp))
+			goto drop;
+
+		hdr.priority = ((tmp >> 2) & 0x0f);
+		hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
+		break;
+	/*
+	 * Flow Label carried in-line
+	 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
+	 */
+	case 2: /* 01b */
+		if (ble_6lowpan_fetch_skb_u8(skb, &tmp))
+			goto drop;
+
+		hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
+		memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
+		skb_pull(skb, 2);
+		break;
+	/* Traffic Class and Flow Label are elided */
+	case 3: /* 11b */
+		break;
+	default:
+		break;
+	}
+
+	/* Next Header */
+	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
+		/* Next header is carried inline */
+		if (ble_6lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
+			goto drop;
+
+		pr_debug("NH flag is set, next header carried inline: %02x\n",
+			 hdr.nexthdr);
+	}
+
+	/* Hop Limit */
+	if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
+		hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
+	else {
+		if (ble_6lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
+			goto drop;
+	}
+
+	/* Extract SAM to the tmp variable */
+	tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
+
+	if (iphc1 & LOWPAN_IPHC_SAC) {
+		/* Source address context based uncompression */
+		pr_debug("SAC bit is set. Handle context based source address.\n");
+		err = ble_6lowpan_uncompress_context_based_src_addr(
+				skb, &hdr.saddr, tmp);
+	} else {
+		/* Source address uncompression */
+		pr_debug("source address stateless compression\n");
+		err = ble_6lowpan_uncompress_addr(skb, &hdr.saddr, tmp, _saddr);
+	}
+
+	/* Check on error of previous branch */
+	if (err)
+		goto drop;
+
+	/* Extract DAM to the tmp variable */
+	tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
+
+	/* check for Multicast Compression */
+	if (iphc1 & LOWPAN_IPHC_M) {
+		if (iphc1 & LOWPAN_IPHC_DAC) {
+			pr_debug("dest: context-based mcast compression\n");
+			/* TODO: implement this */
+		} else {
+			err = lowpan_uncompress_multicast_daddr(
+						skb, &hdr.daddr, tmp);
+			if (err)
+				goto drop;
+		}
+	} else {
+		pr_debug("dest: stateless compression\n");
+		err = ble_6lowpan_uncompress_addr(skb, &hdr.daddr, tmp, _daddr);
+		if (err)
+			goto drop;
+	}
+
+	/* UDP data uncompression */
+	if (iphc0 & LOWPAN_IPHC_NH_C) {
+		struct udphdr uh;
+		struct sk_buff *new;
+		if (ble_6lowpan_uncompress_udp_header(skb, &uh))
+			goto drop;
+
+		/*
+		 * replace the compressed UDP head by the uncompressed UDP
+		 * header
+		 */
+		new = skb_copy_expand(skb, sizeof(struct udphdr),
+				      skb_tailroom(skb), GFP_ATOMIC);
+		kfree_skb(skb);
+
+		if (!new)
+			return -ENOMEM;
+
+		skb = new;
+
+		skb_push(skb, sizeof(struct udphdr));
+		skb_reset_transport_header(skb);
+		skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
+
+		ble_6lowpan_raw_dump_table(__func__, "raw UDP header dump",
+				      (u8 *)&uh, sizeof(uh));
+
+		hdr.nexthdr = UIP_PROTO_UDP;
+	}
+
+	/* Not fragmented package */
+	hdr.payload_len = htons(skb->len);
+
+	pr_debug("skb headroom size = %d, data length = %d\n",
+		 skb_headroom(skb), skb->len);
+
+	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
+		 "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version,
+		 ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
+
+	ble_6lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
+							sizeof(hdr));
+
+	return ble_6lowpan_skb_deliver(skb, &hdr, dev);
+
+unlock_and_drop:
+	spin_unlock_bh(&flist_lock);
+drop:
+	kfree_skb(skb);
+	return -EINVAL;
+}
+
 static int ble_6lowpan_recv_pkt(struct sk_buff *skb, struct net_device *dev)
 {
+	struct sk_buff *local_skb;
+
+	if (!netif_running(dev))
+		goto drop;
+
+	if (dev->type != ARPHRD_IEEE802154)
+		goto drop;
+
+	ble_6lowpan_raw_dump_table(__func__, "raw recv dump", skb->head,
+							skb->len);
+
+	/* check that it's our buffer */
+	if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
+		/* Copy the packet so that the IPv6 header is
+		 * properly aligned.
+		 */
+		local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
+					    skb_tailroom(skb), GFP_ATOMIC);
+		if (!local_skb)
+			goto drop;
+
+		local_skb->protocol = htons(ETH_P_IPV6);
+		local_skb->pkt_type = PACKET_HOST;
+
+		skb_reset_network_header(local_skb);
+		skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
+
+		if (ble_6lowpan_give_skb_to_upper(local_skb,
+						dev) != NET_RX_SUCCESS) {
+			kfree_skb(local_skb);
+			goto drop;
+		}
+
+		dev->stats.rx_bytes += skb->len;
+		dev->stats.rx_packets++;
+
+		kfree_skb(local_skb);
+		kfree_skb(skb);
+	} else {
+		switch (skb->data[0] & 0xe0) {
+		case LOWPAN_DISPATCH_IPHC:	/* ipv6 datagram */
+		case LOWPAN_DISPATCH_FRAG1:	/* first fragment header */
+		case LOWPAN_DISPATCH_FRAGN:	/* next fragments headers */
+			local_skb = skb_clone(skb, GFP_ATOMIC);
+			if (!local_skb)
+				goto drop;
+			if (ble_6lowpan_process_data(local_skb,
+						dev) != NET_RX_SUCCESS)
+				goto drop;
+
+			dev->stats.rx_bytes += skb->len;
+			dev->stats.rx_packets++;
+
+			kfree_skb(skb);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return NET_RX_SUCCESS;
+
+drop:
 	kfree_skb(skb);
 	return NET_RX_DROP;
 }
@@ -147,19 +865,406 @@ static void ble_6lowpan_do_send(struct l2cap_conn *conn, struct sk_buff *skb)
 	BT_DBG("conn %p, skb %p len %d priority %u", conn, skb, skb->len,
 	       skb->priority);
 
-	return;
+	hci_send_acl(conn->hchan, skb, ACL_START);
+}
+
+static inline int ble_6lowpan_skbuff_copy(void *msg, int len,
+					int count, int mtu,
+					struct sk_buff *skb,
+					struct net_device *dev)
+{
+	struct sk_buff **frag;
+	int sent = 0;
+
+	memcpy(skb_put(skb, count), msg, count);
+
+	sent += count;
+	msg  += count;
+	len  -= count;
+
+	dev->stats.tx_bytes += count;
+	dev->stats.tx_packets++;
+
+	ble_6lowpan_raw_dump_table(__func__, "Sending", skb->data, skb->len);
+
+	/* Continuation fragments (no L2CAP header) */
+	frag = &skb_shinfo(skb)->frag_list;
+	while (len > 0) {
+		struct sk_buff *tmp;
+
+		count = min_t(unsigned int, mtu, len);
+
+		tmp = bt_skb_alloc(count, GFP_KERNEL);
+		if (IS_ERR(tmp))
+			return PTR_ERR(tmp);
+
+		*frag = tmp;
+
+		memcpy(skb_put(*frag, count), msg, count);
+
+		ble_6lowpan_raw_dump_table(__func__, "Sending fragment",
+					(*frag)->data, count);
+
+		(*frag)->priority = skb->priority;
+
+		sent += count;
+		msg  += count;
+		len  -= count;
+
+		skb->len += (*frag)->len;
+		skb->data_len += (*frag)->len;
+
+		frag = &(*frag)->next;
+
+		dev->stats.tx_bytes += count;
+		dev->stats.tx_packets++;
+	}
+
+	return sent;
+}
+
+static struct sk_buff *ble_6lowpan_create_pdu(struct l2cap_conn *conn,
+					void *msg, size_t len,
+					u32 priority,
+					struct net_device *dev)
+{
+	struct sk_buff *skb;
+	int err, count;
+	struct l2cap_hdr *lh;
+
+	if (conn->mtu > (L2CAP_LE_MIN_MTU + L2CAP_HDR_SIZE))
+		/* XXX: This should be not needed and atm is only used for
+		 * testing purposes */
+		conn->mtu = L2CAP_LE_MIN_MTU + L2CAP_HDR_SIZE;
+
+	count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
+
+	BT_DBG("conn %p len %zu mtu %d count %d", conn, len, conn->mtu, count);
+
+	skb = bt_skb_alloc(count + L2CAP_HDR_SIZE, GFP_KERNEL);
+	if (IS_ERR(skb))
+		return skb;
+
+	skb->priority = priority;
+
+	lh = (struct l2cap_hdr *)skb_put(skb, L2CAP_HDR_SIZE);
+	lh->cid = cpu_to_le16(L2CAP_FC_6LOWPAN);
+	lh->len = cpu_to_le16(len);
+
+	err = ble_6lowpan_skbuff_copy(msg, len, count, conn->mtu, skb, dev);
+	if (unlikely(err < 0)) {
+		kfree_skb(skb);
+		BT_DBG("skbuff copy %d failed", err);
+		return ERR_PTR(err);
+	}
+
+	return skb;
 }
 
 static int lowpan_conn_send(struct l2cap_conn *conn,
 			void *msg, size_t len, u32 priority,
 			struct net_device *dev)
 {
-	struct sk_buff *skb = {0};
+	struct sk_buff *skb;
+
+	skb = ble_6lowpan_create_pdu(conn, msg, len, priority, dev);
+	if (IS_ERR(skb))
+		return -EINVAL;
 
 	ble_6lowpan_do_send(conn, skb);
 	return 0;
 }
 
+static u8 lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift,
+				const struct in6_addr *ipaddr,
+				const unsigned char *lladdr)
+{
+	u8 val = 0;
+
+	if (is_addr_mac_addr_based(ipaddr, lladdr))
+		val = 3; /* 0-bits */
+	else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
+		/* compress IID to 16 bits xxxx::XXXX */
+		memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2);
+		*hc06_ptr += 2;
+		val = 2; /* 16-bits */
+	} else {
+		/* do not compress IID => xxxx::IID */
+		memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8);
+		*hc06_ptr += 8;
+		val = 1; /* 64-bits */
+	}
+
+	return rol8(val, shift);
+}
+
+static void ble_6lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb)
+{
+	struct udphdr *uh = udp_hdr(skb);
+
+	if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) ==
+				LOWPAN_NHC_UDP_4BIT_PORT) &&
+	    ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) ==
+				LOWPAN_NHC_UDP_4BIT_PORT)) {
+		pr_debug("UDP header: both ports compression to 4 bits\n");
+		**hc06_ptr = LOWPAN_NHC_UDP_CS_P_11;
+		**(hc06_ptr + 1) = /* subtraction is faster */
+		   (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) +
+		       ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4));
+		*hc06_ptr += 2;
+	} else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) ==
+			LOWPAN_NHC_UDP_8BIT_PORT) {
+		pr_debug("UDP header: remove 8 bits of dest\n");
+		**hc06_ptr = LOWPAN_NHC_UDP_CS_P_01;
+		memcpy(*hc06_ptr + 1, &uh->source, 2);
+		**(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT);
+		*hc06_ptr += 4;
+	} else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) ==
+			LOWPAN_NHC_UDP_8BIT_PORT) {
+		pr_debug("UDP header: remove 8 bits of source\n");
+		**hc06_ptr = LOWPAN_NHC_UDP_CS_P_10;
+		memcpy(*hc06_ptr + 1, &uh->dest, 2);
+		**(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT);
+		*hc06_ptr += 4;
+	} else {
+		pr_debug("UDP header: can't compress\n");
+		**hc06_ptr = LOWPAN_NHC_UDP_CS_P_00;
+		memcpy(*hc06_ptr + 1, &uh->source, 2);
+		memcpy(*hc06_ptr + 3, &uh->dest, 2);
+		*hc06_ptr += 5;
+	}
+
+	/* checksum is always inline */
+	memcpy(*hc06_ptr, &uh->check, 2);
+	*hc06_ptr += 2;
+
+	/* skip the UDP header */
+	skb_pull(skb, sizeof(struct udphdr));
+}
+
+static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
+{
+	if (unlikely(!pskb_may_pull(skb, 1)))
+		return -EINVAL;
+
+	*val = skb->data[0];
+	skb_pull(skb, 1);
+
+	return 0;
+}
+
+static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
+{
+	if (unlikely(!pskb_may_pull(skb, 2)))
+		return -EINVAL;
+
+	*val = (skb->data[0] << 8) | skb->data[1];
+	skb_pull(skb, 2);
+
+	return 0;
+}
+
+static int ble_6lowpan_header_create(struct sk_buff *skb,
+				struct net_device *dev,
+				unsigned short type, const void *_daddr,
+				const void *_saddr, unsigned int len)
+{
+	u8 tmp, iphc0, iphc1, *hc06_ptr;
+	struct ipv6hdr *hdr;
+	u8 *saddr, *daddr;
+	u8 head[100];
+	struct ble_6lowpan_dev_info *info;
+
+	if (type != ETH_P_IPV6)
+		return -EINVAL;
+
+	info = ble_6lowpan_dev_info(dev);
+	daddr = info->ieee802154_addr;
+	saddr = info->net->dev_addr;
+
+	hdr = ipv6_hdr(skb);
+	hc06_ptr = head + 2;
+
+	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
+		 "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version,
+		 ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit);
+
+	ble_6lowpan_raw_dump_table(__func__, "raw skb network header dump",
+		skb_network_header(skb), sizeof(struct ipv6hdr));
+
+	ble_6lowpan_raw_dump_inline(__func__, "saddr",
+			(unsigned char *)saddr, 8);
+
+	/*
+	 * As we copy some bit-length fields, in the IPHC encoding bytes,
+	 * we sometimes use |=
+	 * If the field is 0, and the current bit value in memory is 1,
+	 * this does not work. We therefore reset the IPHC encoding here
+	 */
+	iphc0 = LOWPAN_DISPATCH_IPHC;
+	iphc1 = 0;
+
+	/* TODO: context lookup */
+
+	ble_6lowpan_raw_dump_inline(__func__, "daddr",
+			(unsigned char *)daddr, 8);
+
+	ble_6lowpan_raw_dump_table(__func__,
+				"sending raw skb network uncompressed packet",
+				skb->data, skb->len);
+
+	/*
+	 * Traffic class, flow label
+	 * If flow label is 0, compress it. If traffic class is 0, compress it
+	 * We have to process both in the same time as the offset of traffic
+	 * class depends on the presence of version and flow label
+	 */
+
+	/* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
+	tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
+	tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
+
+	if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
+	     (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
+		/* flow label can be compressed */
+		iphc0 |= LOWPAN_IPHC_FL_C;
+		if ((hdr->priority == 0) &&
+		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
+			/* compress (elide) all */
+			iphc0 |= LOWPAN_IPHC_TC_C;
+		} else {
+			/* compress only the flow label */
+			*hc06_ptr = tmp;
+			hc06_ptr += 1;
+		}
+	} else {
+		/* Flow label cannot be compressed */
+		if ((hdr->priority == 0) &&
+		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
+			/* compress only traffic class */
+			iphc0 |= LOWPAN_IPHC_TC_C;
+			*hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
+			memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
+			hc06_ptr += 3;
+		} else {
+			/* compress nothing */
+			memcpy(hc06_ptr, &hdr, 4);
+			/* replace the top byte with new ECN | DSCP format */
+			*hc06_ptr = tmp;
+			hc06_ptr += 4;
+		}
+	}
+
+	/* NOTE: payload length is always compressed */
+
+	/* Next Header is compress if UDP */
+	if (hdr->nexthdr == UIP_PROTO_UDP)
+		iphc0 |= LOWPAN_IPHC_NH_C;
+
+	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
+		*hc06_ptr = hdr->nexthdr;
+		hc06_ptr += 1;
+	}
+
+	/*
+	 * Hop limit
+	 * if 1:   compress, encoding is 01
+	 * if 64:  compress, encoding is 10
+	 * if 255: compress, encoding is 11
+	 * else do not compress
+	 */
+	switch (hdr->hop_limit) {
+	case 1:
+		iphc0 |= LOWPAN_IPHC_TTL_1;
+		break;
+	case 64:
+		iphc0 |= LOWPAN_IPHC_TTL_64;
+		break;
+	case 255:
+		iphc0 |= LOWPAN_IPHC_TTL_255;
+		break;
+	default:
+		*hc06_ptr = hdr->hop_limit;
+		hc06_ptr += 1;
+		break;
+	}
+
+	/* source address compression */
+	if (is_addr_unspecified(&hdr->saddr)) {
+		pr_debug("source address is unspecified, setting SAC\n");
+		iphc1 |= LOWPAN_IPHC_SAC;
+	/* TODO: context lookup */
+	} else if (is_addr_link_local(&hdr->saddr)) {
+		pr_debug("source address is link-local\n");
+		iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
+				LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
+	} else {
+		pr_debug("send the full source address\n");
+		memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
+		hc06_ptr += 16;
+	}
+
+	/* destination address compression */
+	if (is_addr_mcast(&hdr->daddr)) {
+		pr_debug("destination address is multicast: ");
+		iphc1 |= LOWPAN_IPHC_M;
+		if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
+			pr_debug("compressed to 1 octet\n");
+			iphc1 |= LOWPAN_IPHC_DAM_11;
+			/* use last byte */
+			*hc06_ptr = hdr->daddr.s6_addr[15];
+			hc06_ptr += 1;
+		} else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
+			pr_debug("compressed to 4 octets\n");
+			iphc1 |= LOWPAN_IPHC_DAM_10;
+			/* second byte + the last three */
+			*hc06_ptr = hdr->daddr.s6_addr[1];
+			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
+			hc06_ptr += 4;
+		} else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
+			pr_debug("compressed to 6 octets\n");
+			iphc1 |= LOWPAN_IPHC_DAM_01;
+			/* second byte + the last five */
+			*hc06_ptr = hdr->daddr.s6_addr[1];
+			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
+			hc06_ptr += 6;
+		} else {
+			pr_debug("using full address\n");
+			iphc1 |= LOWPAN_IPHC_DAM_00;
+			memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
+			hc06_ptr += 16;
+		}
+	} else {
+		/* TODO: context lookup */
+		if (is_addr_link_local(&hdr->daddr)) {
+			pr_debug("dest address is unicast and link-local\n");
+			iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
+				LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
+		} else {
+			pr_debug("dest address is unicast: using full one\n");
+			memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
+			hc06_ptr += 16;
+		}
+	}
+
+	/* UDP header compression */
+	if (hdr->nexthdr == UIP_PROTO_UDP)
+		ble_6lowpan_compress_udp_header(&hc06_ptr, skb);
+
+	head[0] = iphc0;
+	head[1] = iphc1;
+
+	skb_pull(skb, sizeof(struct ipv6hdr));
+	memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
+
+	BT_DBG("header len %d skb %u", (int)(hc06_ptr - head), skb->len);
+
+	ble_6lowpan_raw_dump_table(__func__, "raw skb data dump compressed",
+				skb->data, skb->len);
+
+	return 0;
+}
+
 /* Packet to BT LE device */
 static int ble_6lowpan_send(struct l2cap_conn *conn, const void *saddr,
 			const void *daddr, struct sk_buff *skb,
@@ -199,6 +1304,10 @@ static const struct net_device_ops ble_6lowpan_netdev_ops = {
 	.ndo_start_xmit		= ble_6lowpan_xmit,
 };
 
+static struct header_ops ble_6lowpan_header_ops = {
+	.create	= ble_6lowpan_header_create,
+};
+
 static void ble_6lowpan_setup(struct net_device *dev)
 {
 	dev->addr_len		= IEEE802154_ADDR_LEN;
@@ -212,6 +1321,7 @@ static void ble_6lowpan_setup(struct net_device *dev)
 	dev->watchdog_timeo	= 0;
 
 	dev->netdev_ops		= &ble_6lowpan_netdev_ops;
+	dev->header_ops		= &ble_6lowpan_header_ops;
 	dev->destructor		= free_netdev;
 }
 
-- 
1.7.11.7

--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html




[Index of Archives]     [Bluez Devel]     [Linux Wireless Networking]     [Linux Wireless Personal Area Networking]     [Linux ATH6KL]     [Linux USB Devel]     [Linux Media Drivers]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [Big List of Linux Books]

  Powered by Linux