[Bluetooth] Support responses with zero SCID

According to the Bluetooth specification there is no need to fill in
the SCID on a failing response. Without a SCID the L2CAP core can't
find the corresponding request for it. In the case of the zero SCID
the signal identifier should be used to match the response to its
request.
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

net/bluetooth/l2cap.c

@@ -57,7 +57,7 @@
 #define BT_DBG(D...)
 #endif
 
-#define VERSION "2.4"
+#define VERSION "2.5"
 
 static struct proto_ops l2cap_sock_ops;
 
@@ -74,8 +74,8 @@ static void __l2cap_sock_close(struct sock *sk, int reason);
 static void l2cap_sock_close(struct sock *sk);
 static void l2cap_sock_kill(struct sock *sk);
 
-static int l2cap_send_req(struct l2cap_conn *conn, u8 code, u16 len, void *data);
-static int l2cap_send_rsp(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data);
+static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
+				u8 code, u8 ident, u16 dlen, void *data);
 
 /* ---- L2CAP timers ---- */
 static void l2cap_sock_timeout(unsigned long arg)
@@ -174,6 +174,40 @@ static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, stru
 	write_unlock(&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(&conn->lock);
+
+	if (++conn->tx_ident > 128)
+		conn->tx_ident = 1;
+
+	id = conn->tx_ident;
+
+	spin_unlock(&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(u16 psm, bdaddr_t *src)
 {
@@ -286,7 +320,8 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
 
 			req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
 			req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
-			l2cap_send_req(conn, L2CAP_DISCONN_REQ, sizeof(req), &req);
+			l2cap_send_cmd(conn, l2cap_get_ident(conn),
+					L2CAP_DISCONN_REQ, sizeof(req), &req);
 		} else {
 			l2cap_chan_del(sk, reason);
 		}
@@ -365,7 +400,8 @@ static int l2cap_sock_create(struct socket *sock, int protocol)
 
 	sock->state = SS_UNCONNECTED;
 
-	if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
+	if (sock->type != SOCK_SEQPACKET &&
+			sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
 		return -ESOCKTNOSUPPORT;
 
 	if (sock->type == SOCK_RAW && !capable(CAP_NET_RAW))
@@ -459,9 +495,11 @@ static int l2cap_do_connect(struct sock *sk)
 	if (hcon->state == BT_CONNECTED) {
 		if (sk->sk_type == SOCK_SEQPACKET) {
 			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_req(conn, L2CAP_CONN_REQ, sizeof(req), &req);
+			l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
+					L2CAP_CONN_REQ, sizeof(req), &req);
 		} else {
 			l2cap_sock_clear_timer(sk);
 			sk->sk_state = BT_CONNECTED;
@@ -911,6 +949,25 @@ static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16
 	return s;
 }
 
+static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
+{
+	struct sock *s;
+	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
+		if (l2cap_pi(s)->ident == ident)
+			break;
+	}
+	return s;
+}
+
+static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
+{
+	struct sock *s;
+	read_lock(&l->lock);
+	s = __l2cap_get_chan_by_ident(l, ident);
+	read_unlock(&l->lock);
+	return s;
+}
+
 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 {
 	u16 cid = 0x0040;
@@ -1030,9 +1087,10 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
 			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_req(conn, L2CAP_CONN_REQ, sizeof(req), &req);
+			l2cap_send_cmd(conn, l2cap_pi(sk)->ident, L2CAP_CONN_REQ, sizeof(req), &req);
 		}
 
 		bh_unlock_sock(sk);
@@ -1092,27 +1150,6 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
 }
 
 /* ---- L2CAP signalling commands ---- */
-static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
-{
-	u8 id;
-
-	/* Get next available identificator.
-	 *    1 - 199 are used by kernel.
-	 *  200 - 254 are used by utilities like l2ping, etc 
-	 */
-
-	spin_lock(&conn->lock);
-
-	if (++conn->tx_ident > 199)
-		conn->tx_ident = 1;
-
-	id = conn->tx_ident;
-
-	spin_unlock(&conn->lock);
-
-	return id;
-}
-
 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
 				u8 code, u8 ident, u16 dlen, void *data)
 {
@@ -1171,29 +1208,6 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
 	return NULL;
 }
 
-static int l2cap_send_req(struct l2cap_conn *conn, u8 code, u16 len, void *data)
-{
-	u8 ident = l2cap_get_ident(conn);
-	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);
-}
-
-static int l2cap_send_rsp(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);
-}
-
 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
 {
 	struct l2cap_conf_opt *opt = *ptr;
@@ -1412,7 +1426,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 	status = L2CAP_CS_AUTHEN_PEND;
 	sk->sk_state = BT_CONNECT2;
 	l2cap_pi(sk)->ident = cmd->ident;
-	
+
 	if (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) {
 		if (!hci_conn_encrypt(conn->hcon))
 			goto done;
@@ -1435,7 +1449,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 	rsp.dcid   = __cpu_to_le16(dcid);
 	rsp.result = __cpu_to_le16(result);
 	rsp.status = __cpu_to_le16(status);
-	l2cap_send_rsp(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 	return 0;
 }
 
@@ -1453,16 +1467,23 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
 	BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
 
-	if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
-		return 0;
+	if (scid) {
+		if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
+			return 0;
+	} else {
+		if (!(sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident)))
+			return 0;
+	}
 
 	switch (result) {
 	case L2CAP_CR_SUCCESS:
 		sk->sk_state = BT_CONFIG;
+		l2cap_pi(sk)->ident = 0;
 		l2cap_pi(sk)->dcid = dcid;
 		l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
 
-		l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);
+		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
+					l2cap_build_conf_req(sk, req), req);
 		break;
 
 	case L2CAP_CR_PEND:
@@ -1497,12 +1518,14 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
 	if (flags & 0x0001) {
 		/* Incomplete config. Send empty response. */
-		l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
+		l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
+				l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
 		goto unlock;
 	}
 
 	/* Complete config. */
-	l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, &result), rsp);
+	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
+			l2cap_build_conf_rsp(sk, rsp, &result), rsp);
 
 	if (result)
 		goto unlock;
@@ -1515,7 +1538,8 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 		l2cap_chan_ready(sk);
 	} else if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
 		u8 req[64];
-		l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);
+		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
+					l2cap_build_conf_req(sk, req), req);
 	}
 
 unlock:
@@ -1550,8 +1574,8 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 			 * resend config request that we sent earlier. It is
 			 * stupid, but it helps qualification testing which
 			 * expects at least some response from us. */
-			l2cap_send_req(conn, L2CAP_CONF_REQ,
-				l2cap_build_conf_req(sk, req), req);
+			l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
+						l2cap_build_conf_req(sk, req), req);
 			goto done;
 		}
 
@@ -1563,7 +1587,8 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 			struct l2cap_disconn_req req;
 			req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
 			req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
-			l2cap_send_req(conn, L2CAP_DISCONN_REQ, sizeof(req), &req);
+			l2cap_send_cmd(conn, l2cap_get_ident(conn),
+					L2CAP_DISCONN_REQ, sizeof(req), &req);
 		}
 		goto done;
 	}
@@ -1601,7 +1626,7 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 
 	rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
 	rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
-	l2cap_send_rsp(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
+	l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
 
 	sk->sk_shutdown = SHUTDOWN_MASK;
 
@@ -1645,7 +1670,7 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
 
 	rsp.type   = __cpu_to_le16(type);
 	rsp.result = __cpu_to_le16(L2CAP_IR_NOTSUPP);
-	l2cap_send_rsp(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
+	l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
 
 	return 0;
 }
@@ -1716,7 +1741,7 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
 			break;
 
 		case L2CAP_ECHO_REQ:
-			l2cap_send_rsp(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
+			l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
 			break;
 
 		case L2CAP_ECHO_RSP:
@@ -1742,7 +1767,7 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
 
 			/* FIXME: Map err to a valid reason */
 			rej.reason = __cpu_to_le16(0);
-			l2cap_send_rsp(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
+			l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
 		}
 
 		data += cmd.len;
@@ -1939,7 +1964,8 @@ static int l2cap_auth_cfm(struct hci_conn *hcon, u8 status)
 		rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
 		rsp.result = __cpu_to_le16(result);
 		rsp.status = __cpu_to_le16(0);
-		l2cap_send_rsp(conn, l2cap_pi(sk)->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+		l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
+				L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
 		bh_unlock_sock(sk);
 	}
@@ -1985,7 +2011,8 @@ static int l2cap_encrypt_cfm(struct hci_conn *hcon, u8 status)
 		rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
 		rsp.result = __cpu_to_le16(result);
 		rsp.status = __cpu_to_le16(0);
-		l2cap_send_rsp(conn, l2cap_pi(sk)->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+		l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
+				L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
 		bh_unlock_sock(sk);
 	}
@@ -2237,7 +2264,7 @@ static void __exit l2cap_exit(void)
 void l2cap_load(void)
 {
 	/* Dummy function to trigger automatic L2CAP module loading by
-	 * other modules that use L2CAP sockets but don not use any othe
+	 * other modules that use L2CAP sockets but don't use any other
 	 * symbols from it. */
 	return;
 }