bluetooth: Improve BNEP extension headers handling

Adds handling of extension headers to BNEP control packets.
Adds support for BNEP setup connection request handled in pand.

ST-Ericsson Linux next: -
ST-Ericsson ID: 328144
ST-Ericsson FOSS-OUT-ID: Trivial
Depends-On: Id2cbd106af9b380f63d87ea0b4652ec6cd996fa4

Change-Id: I005e00e2fe934dc19f40533953d5b233676f0d0e
Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@tieto.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/18117
Reviewed-by: Par-Gunnar HJALMDAHL <par-gunnar.p.hjalmdahl@stericsson.com>
Reviewed-by: Lukasz RYMANOWSKI <lukasz.rymanowski@stericsson.com>
Reviewed-by: Henrik POSSUNG <henrik.possung@stericsson.com>

2 files changed, 46 insertions, 9 deletions

diff --git a/net/bluetooth/bnep/bnep.h b/net/bluetooth/bnep/bnep.h
index e7ee5314f39..0545fe0493f 100644
--- a/net/bluetooth/bnep/bnep.h
+++ b/net/bluetooth/bnep/bnep.h
@@ -166,6 +166,8 @@ struct bnep_session {
 
 	struct socket    *sock;
 	struct net_device *dev;
+
+	unsigned int setup_done;
 };
 
 void bnep_net_setup(struct net_device *dev);
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index a779ec70332..fe3b9faeb68 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -117,7 +117,8 @@ static inline void bnep_set_default_proto_filter(struct bnep_session *s)
 }
 #endif
 
-static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
+static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data,
+							int len, int *pkt_size)
 {
 	int n;
 
@@ -133,6 +134,8 @@ static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len
 
 	BT_DBG("filter len %d", n);
 
+	*pkt_size = 2 + n;
+
 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
 	n /= 4;
 	if (n <= BNEP_MAX_PROTO_FILTERS) {
@@ -163,7 +166,8 @@ static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len
 	return 0;
 }
 
-static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
+static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len,
+								int *pkt_size)
 {
 	int n;
 
@@ -179,6 +183,8 @@ static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
 
 	BT_DBG("filter len %d", n);
 
+	*pkt_size = 2 + n;
+
 #ifdef CONFIG_BT_BNEP_MC_FILTER
 	n /= (ETH_ALEN * 2);
 
@@ -224,7 +230,8 @@ static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
 	return 0;
 }
 
-static int bnep_rx_control(struct bnep_session *s, void *data, int len)
+static int bnep_rx_control(struct bnep_session *s, void *data, int len,
+								int *pkt_size)
 {
 	u8  cmd = *(u8 *)data;
 	int err = 0;
@@ -232,6 +239,8 @@ static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 	data++;
 	len--;
 
+	*pkt_size = 0;
+
 	switch (cmd) {
 	case BNEP_CMD_NOT_UNDERSTOOD:
 	case BNEP_SETUP_CONN_RSP:
@@ -241,15 +250,27 @@ static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 		break;
 
 	case BNEP_FILTER_NET_TYPE_SET:
-		err = bnep_ctrl_set_netfilter(s, data, len);
+		err = bnep_ctrl_set_netfilter(s, data, len, pkt_size);
 		break;
 
 	case BNEP_FILTER_MULTI_ADDR_SET:
-		err = bnep_ctrl_set_mcfilter(s, data, len);
+		err = bnep_ctrl_set_mcfilter(s, data, len, pkt_size);
 		break;
 
-	case BNEP_SETUP_CONN_REQ:
-		err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
+	case BNEP_SETUP_CONN_REQ: {
+			u8 uuid_size = *(u8 *)data;
+
+			/* First setup connection should be silently discarded,
+			 * it was already handled when accepting connection.
+			 */
+			if (s->setup_done)
+				err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
+							BNEP_CONN_NOT_ALLOWED);
+			else
+				s->setup_done = 1;
+
+			*pkt_size = 1 + 2 * uuid_size;
+		}
 		break;
 
 	default: {
@@ -262,6 +283,10 @@ static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 		break;
 	}
 
+	if (*pkt_size > 0)
+		/* Add 1 byte for type field */
+		(*pkt_size)++;
+
 	return err;
 }
 
@@ -269,6 +294,7 @@ static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
 {
 	struct bnep_ext_hdr *h;
 	int err = 0;
+	int pkt_size;
 
 	do {
 		h = (void *) skb->data;
@@ -281,7 +307,7 @@ static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
 
 		switch (h->type & BNEP_TYPE_MASK) {
 		case BNEP_EXT_CONTROL:
-			bnep_rx_control(s, skb->data, skb->len);
+			bnep_rx_control(s, skb->data, skb->len, &pkt_size);
 			break;
 
 		default:
@@ -321,7 +347,16 @@ static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
 		goto badframe;
 
 	if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
-		bnep_rx_control(s, skb->data, skb->len);
+		int pkt_size = 0;
+
+		bnep_rx_control(s, skb->data, skb->len, &pkt_size);
+
+		if (pkt_size > 0 && (type & BNEP_EXT_HEADER)) {
+			skb_pull(skb, pkt_size);
+			if (bnep_rx_extension(s, skb) < 0)
+				goto badframe;
+		}
+
 		kfree_skb(skb);
 		return 0;
 	}