Commit 33e882a5 authored by Suraj Sumangala's avatar Suraj Sumangala Committed by Marcel Holtmann
Browse files

Bluetooth: Implement hci_reassembly helper to reassemble RX packets 



Implements feature to reassemble received HCI frames from any input stream

Signed-off-by: default avatarSuraj Sumangala <suraj@atheros.com>
Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
parent cd4c5391

include/net/bluetooth/bluetooth.h

+1 −0
Original line number Diff line number Diff line
@@ -138,6 +138,7 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock); 
struct bt_skb_cb {

	__u8 pkt_type;

	__u8 incoming;

	__u16 expect;

	__u8 tx_seq;

	__u8 retries;

	__u8 sar;

net/bluetooth/hci_core.c

+109 −0
Original line number Diff line number Diff line
@@ -1033,6 +1033,115 @@ int hci_recv_frame(struct sk_buff *skb) 
}

EXPORT_SYMBOL(hci_recv_frame);



static int hci_reassembly(struct hci_dev *hdev, int type, void *data,

			  int count, __u8 index, gfp_t gfp_mask)

{

	int len = 0;

	int hlen = 0;

	int remain = count;

	struct sk_buff *skb;

	struct bt_skb_cb *scb;



	if ((type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) ||

				index >= NUM_REASSEMBLY)

		return -EILSEQ;



	skb = hdev->reassembly[index];



	if (!skb) {

		switch (type) {

		case HCI_ACLDATA_PKT:

			len = HCI_MAX_FRAME_SIZE;

			hlen = HCI_ACL_HDR_SIZE;

			break;

		case HCI_EVENT_PKT:

			len = HCI_MAX_EVENT_SIZE;

			hlen = HCI_EVENT_HDR_SIZE;

			break;

		case HCI_SCODATA_PKT:

			len = HCI_MAX_SCO_SIZE;

			hlen = HCI_SCO_HDR_SIZE;

			break;

		}



		skb = bt_skb_alloc(len, gfp_mask);

		if (!skb)

			return -ENOMEM;



		scb = (void *) skb->cb;

		scb->expect = hlen;

		scb->pkt_type = type;



		skb->dev = (void *) hdev;

		hdev->reassembly[index] = skb;

	}



	while (count) {

		scb = (void *) skb->cb;

		len = min(scb->expect, (__u16)count);



		memcpy(skb_put(skb, len), data, len);



		count -= len;

		data += len;

		scb->expect -= len;

		remain = count;



		switch (type) {

		case HCI_EVENT_PKT:

			if (skb->len == HCI_EVENT_HDR_SIZE) {

				struct hci_event_hdr *h = hci_event_hdr(skb);

				scb->expect = h->plen;



				if (skb_tailroom(skb) < scb->expect) {

					kfree_skb(skb);

					hdev->reassembly[index] = NULL;

					return -ENOMEM;

				}

			}

			break;



		case HCI_ACLDATA_PKT:

			if (skb->len  == HCI_ACL_HDR_SIZE) {

				struct hci_acl_hdr *h = hci_acl_hdr(skb);

				scb->expect = __le16_to_cpu(h->dlen);



				if (skb_tailroom(skb) < scb->expect) {

					kfree_skb(skb);

					hdev->reassembly[index] = NULL;

					return -ENOMEM;

				}

			}

			break;



		case HCI_SCODATA_PKT:

			if (skb->len == HCI_SCO_HDR_SIZE) {

				struct hci_sco_hdr *h = hci_sco_hdr(skb);

				scb->expect = h->dlen;



				if (skb_tailroom(skb) < scb->expect) {

					kfree_skb(skb);

					hdev->reassembly[index] = NULL;

					return -ENOMEM;

				}

			}

			break;

		}



		if (scb->expect == 0) {

			/* Complete frame */



			bt_cb(skb)->pkt_type = type;

			hci_recv_frame(skb);



			hdev->reassembly[index] = NULL;

			return remain;

		}

	}



	return remain;

}



/* Receive packet type fragment */

#define __reassembly(hdev, type)  ((hdev)->reassembly[(type) - 1])