[Bluetooth] Retrieve L2CAP features mask on connection setup

The Bluetooth 1.2 specification introduced a specific features mask
value to interoperate with newer versions of the specification. So far
this piece of information was never needed, but future extensions will
rely on it. This patch adds a generic way to retrieve this information
only once per connection setup.
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

include/net/bluetooth/l2cap.h

@@ -29,7 +29,8 @@
 #define L2CAP_DEFAULT_MTU	672
 #define L2CAP_DEFAULT_FLUSH_TO	0xFFFF
 
-#define L2CAP_CONN_TIMEOUT	(HZ * 40)
+#define L2CAP_CONN_TIMEOUT	(40000) /* 40 seconds */
+#define L2CAP_INFO_TIMEOUT	(4000)  /*  4 seconds */
 
 /* L2CAP socket address */
 struct sockaddr_l2 {
@@ -160,7 +161,6 @@ struct l2cap_disconn_rsp {
 
 struct l2cap_info_req {
 	__le16      type;
-	__u8        data[0];
 } __attribute__ ((packed));
 
 struct l2cap_info_rsp {
@@ -192,6 +192,13 @@ struct l2cap_conn {
 
 	unsigned int	mtu;
 
+	__u32		feat_mask;
+
+	__u8		info_state;
+	__u8		info_ident;
+
+	struct timer_list info_timer;
+
 	spinlock_t	lock;
 
 	struct sk_buff *rx_skb;
@@ -202,6 +209,9 @@ struct l2cap_conn {
 	struct l2cap_chan_list chan_list;
 };
 
+#define L2CAP_INFO_CL_MTU_REQ_SENT	0x01
+#define L2CAP_INFO_FEAT_MASK_REQ_SENT	0x02
+
 /* ----- L2CAP channel and socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
 

net/bluetooth/l2cap.c

@@ -258,7 +258,119 @@ static void l2cap_chan_del(struct sock *sk, int err)
 		sk->sk_state_change(sk);
 }
 
+static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
+{
+	u8 id;
+
+	/* Get next available identificator.
+	 *    1 - 128 are used by kernel.
+	 *  129 - 199 are reserved.
+	 *  200 - 254 are used by utilities like l2ping, etc.
+	 */
+
+	spin_lock_bh(&conn->lock);
+
+	if (++conn->tx_ident > 128)
+		conn->tx_ident = 1;
+
+	id = conn->tx_ident;
+
+	spin_unlock_bh(&conn->lock);
+
+	return id;
+}
+
+static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
+{
+	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
+
+	BT_DBG("code 0x%2.2x", code);
+
+	if (!skb)
+		return -ENOMEM;
+
+	return hci_send_acl(conn->hcon, skb, 0);
+}
+
 /* ---- L2CAP connections ---- */
+static void l2cap_conn_start(struct l2cap_conn *conn)
+{
+	struct l2cap_chan_list *l = &conn->chan_list;
+	struct sock *sk;
+
+	BT_DBG("conn %p", conn);
+
+	read_lock(&l->lock);
+
+	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
+		bh_lock_sock(sk);
+
+		if (sk->sk_type != SOCK_SEQPACKET) {
+			l2cap_sock_clear_timer(sk);
+			sk->sk_state = BT_CONNECTED;
+			sk->sk_state_change(sk);
+		} else if (sk->sk_state == BT_CONNECT) {
+			struct l2cap_conn_req req;
+			l2cap_pi(sk)->ident = l2cap_get_ident(conn);
+			req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+			req.psm  = l2cap_pi(sk)->psm;
+			l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
+					L2CAP_CONN_REQ, sizeof(req), &req);
+		}
+
+		bh_unlock_sock(sk);
+	}
+
+	read_unlock(&l->lock);
+}
+
+static void l2cap_conn_ready(struct l2cap_conn *conn)
+{
+	BT_DBG("conn %p", conn);
+
+	if (conn->chan_list.head || !hlist_empty(&l2cap_sk_list.head)) {
+		struct l2cap_info_req req;
+
+		req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
+
+		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
+		conn->info_ident = l2cap_get_ident(conn);
+
+		mod_timer(&conn->info_timer,
+			jiffies + msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
+
+		l2cap_send_cmd(conn, conn->info_ident,
+					L2CAP_INFO_REQ, sizeof(req), &req);
+	}
+}
+
+/* Notify sockets that we cannot guaranty reliability anymore */
+static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
+{
+	struct l2cap_chan_list *l = &conn->chan_list;
+	struct sock *sk;
+
+	BT_DBG("conn %p", conn);
+
+	read_lock(&l->lock);
+
+	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
+		if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
+			sk->sk_err = err;
+	}
+
+	read_unlock(&l->lock);
+}
+
+static void l2cap_info_timeout(unsigned long arg)
+{
+	struct l2cap_conn *conn = (void *) arg;
+
+	conn->info_ident = 0;
+
+	l2cap_conn_start(conn);
+}
+
 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 {
 	struct l2cap_conn *conn = hcon->l2cap_data;
@@ -279,6 +391,12 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 	conn->src = &hcon->hdev->bdaddr;
 	conn->dst = &hcon->dst;
 
+	conn->feat_mask = 0;
+
+	init_timer(&conn->info_timer);
+	conn->info_timer.function = l2cap_info_timeout;
+	conn->info_timer.data = (unsigned long) conn;
+
 	spin_lock_init(&conn->lock);
 	rwlock_init(&conn->chan_list.lock);
 
@@ -318,40 +436,6 @@ static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, stru
 	write_unlock_bh(&l->lock);
 }
 
-static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
-{
-	u8 id;
-
-	/* Get next available identificator.
-	 *    1 - 128 are used by kernel.
-	 *  129 - 199 are reserved.
-	 *  200 - 254 are used by utilities like l2ping, etc.
-	 */
-
-	spin_lock_bh(&conn->lock);
-
-	if (++conn->tx_ident > 128)
-		conn->tx_ident = 1;
-
-	id = conn->tx_ident;
-
-	spin_unlock_bh(&conn->lock);
-
-	return id;
-}
-
-static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
-{
-	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
-
-	BT_DBG("code 0x%2.2x", code);
-
-	if (!skb)
-		return -ENOMEM;
-
-	return hci_send_acl(conn->hcon, skb, 0);
-}
-
 /* ---- Socket interface ---- */
 static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
 {
@@ -529,7 +613,7 @@ static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int p
 	INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
 
 	sk->sk_destruct = l2cap_sock_destruct;
-	sk->sk_sndtimeo = L2CAP_CONN_TIMEOUT;
+	sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
 
 	sock_reset_flag(sk, SOCK_ZAPPED);
 
@@ -649,6 +733,11 @@ static int l2cap_do_connect(struct sock *sk)
 	l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 
 	if (hcon->state == BT_CONNECTED) {
+		if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) {
+			l2cap_conn_ready(conn);
+			goto done;
+		}
+
 		if (sk->sk_type == SOCK_SEQPACKET) {
 			struct l2cap_conn_req req;
 			l2cap_pi(sk)->ident = l2cap_get_ident(conn);
@@ -1083,52 +1172,6 @@ static int l2cap_sock_release(struct socket *sock)
 	return err;
 }
 
-static void l2cap_conn_ready(struct l2cap_conn *conn)
-{
-	struct l2cap_chan_list *l = &conn->chan_list;
-	struct sock *sk;
-
-	BT_DBG("conn %p", conn);
-
-	read_lock(&l->lock);
-
-	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
-		bh_lock_sock(sk);
-
-		if (sk->sk_type != SOCK_SEQPACKET) {
-			l2cap_sock_clear_timer(sk);
-			sk->sk_state = BT_CONNECTED;
-			sk->sk_state_change(sk);
-		} else if (sk->sk_state == BT_CONNECT) {
-			struct l2cap_conn_req req;
-			l2cap_pi(sk)->ident = l2cap_get_ident(conn);
-			req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-			req.psm  = l2cap_pi(sk)->psm;
-			l2cap_send_cmd(conn, l2cap_pi(sk)->ident, L2CAP_CONN_REQ, sizeof(req), &req);
-		}
-
-		bh_unlock_sock(sk);
-	}
-
-	read_unlock(&l->lock);
-}
-
-/* Notify sockets that we cannot guaranty reliability anymore */
-static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
-{
-	struct l2cap_chan_list *l = &conn->chan_list;
-	struct sock *sk;
-
-	BT_DBG("conn %p", conn);
-
-	read_lock(&l->lock);
-	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
-		if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
-			sk->sk_err = err;
-	}
-	read_unlock(&l->lock);
-}
-
 static void l2cap_chan_ready(struct sock *sk)
 {
 	struct sock *parent = bt_sk(sk)->parent;
@@ -1399,6 +1442,23 @@ static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 fla
 	return ptr - data;
 }
 
+static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+{
+	struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
+
+	if (rej->reason != 0x0000)
+		return 0;
+
+	if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
+					cmd->ident == conn->info_ident) {
+		conn->info_ident = 0;
+		del_timer(&conn->info_timer);
+		l2cap_conn_start(conn);
+	}
+
+	return 0;
+}
+
 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
 {
 	struct l2cap_chan_list *list = &conn->chan_list;
@@ -1739,6 +1799,15 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 
 	BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
 
+	conn->info_ident = 0;
+
+	del_timer(&conn->info_timer);
+
+	if (type == L2CAP_IT_FEAT_MASK)
+		conn->feat_mask = __le32_to_cpu(get_unaligned((__le32 *) rsp->data));
+
+	l2cap_conn_start(conn);
+
 	return 0;
 }
 
@@ -1768,7 +1837,7 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
 
 		switch (cmd.code) {
 		case L2CAP_COMMAND_REJ:
-			/* FIXME: We should process this */
+			l2cap_command_rej(conn, &cmd, data);
 			break;
 
 		case L2CAP_CONN_REQ: