From 5bcdc61f0f3f1765382a1840cdd52a8237bd4246 Mon Sep 17 00:00:00 2001 From: Andrzej Kaczmarek Date: Thu, 10 Mar 2011 11:32:02 +0100 Subject: 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 Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/18117 Reviewed-by: Par-Gunnar HJALMDAHL Reviewed-by: Lukasz RYMANOWSKI Reviewed-by: Henrik POSSUNG --- net/bluetooth/bnep/bnep.h | 2 ++ net/bluetooth/bnep/core.c | 53 +++++++++++++++++++++++++++++++++++++++-------- 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; } -- cgit v1.2.3