diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h index aaed0887f51..baef0541cce 100644 --- a/include/net/bluetooth/l2cap.h +++ b/include/net/bluetooth/l2cap.h @@ -489,6 +489,7 @@ struct l2cap_pinfo { __u8 fixed_channel; __u8 num_conf_req; __u8 num_conf_rsp; + __u8 incoming; __u8 fcs; __u8 sec_level; @@ -691,6 +692,8 @@ void l2cap_sock_kill(struct sock *sk); void l2cap_sock_init(struct sock *sk, struct sock *parent); struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio); +struct sock *l2cap_find_sock_by_fixed_cid_and_dir(__le16 cid, bdaddr_t *src, + bdaddr_t *dst, int server); void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err); void l2cap_chan_del(struct sock *sk, int err); int l2cap_do_connect(struct sock *sk); diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index bf7d3487b69..42f7eccb1a9 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -930,18 +930,24 @@ static void l2cap_conn_start(struct l2cap_conn *conn) } /* Find socket with fixed cid with given source and destination bdaddrs. - * Returns closest match, locked. + * Direction of the req/rsp must match. */ -static struct sock *l2cap_get_sock_by_fixed_scid(int state, - __le16 cid, bdaddr_t *src, bdaddr_t *dst) +struct sock *l2cap_find_sock_by_fixed_cid_and_dir(__le16 cid, bdaddr_t *src, + bdaddr_t *dst, int incoming) { struct sock *sk = NULL, *sk1 = NULL; struct hlist_node *node; + BT_DBG(" %d", incoming); + read_lock(&l2cap_sk_list.lock); sk_for_each(sk, node, &l2cap_sk_list.head) { - if (state && sk->sk_state != state) + + if (incoming && !l2cap_pi(sk)->incoming) + continue; + + if (!incoming && l2cap_pi(sk)->incoming) continue; if (l2cap_pi(sk)->scid == cid && !bacmp(&bt_sk(sk)->dst, dst)) { @@ -1022,13 +1028,12 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn) l2cap_sock_init(sk, parent); bacpy(&bt_sk(sk)->src, conn->src); bacpy(&bt_sk(sk)->dst, conn->dst); + l2cap_pi(sk)->incoming = 1; bt_accept_enqueue(parent, sk); __l2cap_chan_add(conn, sk); - l2cap_sock_set_timer(sk, sk->sk_sndtimeo); - sk->sk_state = BT_CONNECTED; parent->sk_data_ready(parent, 0); @@ -1081,6 +1086,9 @@ static void l2cap_conn_ready(struct l2cap_conn *conn) } read_unlock(&l->lock); + + if (conn->hcon->out && conn->hcon->type == LE_LINK) + l2cap_le_conn_ready(conn); } /* Notify sockets that we cannot guaranty reliability anymore */ @@ -7210,16 +7218,24 @@ done: return 0; } -static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb) +static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, + struct sk_buff *skb) { struct sock *sk; struct sk_buff *skb_rsp; struct l2cap_hdr *lh; + int dir; u8 mtu_rsp[] = {L2CAP_ATT_MTU_RSP, 23, 0}; u8 err_rsp[] = {L2CAP_ATT_ERROR, 0x00, 0x00, 0x00, L2CAP_ATT_NOT_SUPPORTED}; - sk = l2cap_get_sock_by_fixed_scid(0, cid, conn->src, conn->dst); + dir = (skb->data[0] & L2CAP_ATT_RESPONSE_BIT) ? 0 : 1; + + sk = l2cap_find_sock_by_fixed_cid_and_dir(cid, conn->src, + conn->dst, dir); + + BT_DBG("sk %p, dir:%d", sk, dir); + if (!sk) goto drop; @@ -7442,6 +7458,7 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) struct l2cap_chan_list *l; struct l2cap_conn *conn = hcon->l2cap_data; struct sock *sk; + int smp = 0; if (!conn) return 0; @@ -7463,9 +7480,7 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) l2cap_chan_ready(sk); } - del_timer(&hcon->smp_timer); - smp_link_encrypt_cmplt(conn, status, encrypt); - + smp = 1; bh_unlock_sock(sk); continue; } @@ -7529,6 +7544,11 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) read_unlock(&l->lock); + if (smp) { + del_timer(&hcon->smp_timer); + smp_link_encrypt_cmplt(conn, status, encrypt); + } + return 0; } diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c index 0ad70afbc83..1e90a143028 100644 --- a/net/bluetooth/l2cap_sock.c +++ b/net/bluetooth/l2cap_sock.c @@ -1105,6 +1105,7 @@ static int l2cap_sock_shutdown(struct socket *sock, int how) static int l2cap_sock_release(struct socket *sock) { struct sock *sk = sock->sk; + struct sock *srv_sk = NULL; int err; BT_DBG("sock %p, sk %p", sock, sk); @@ -1112,6 +1113,16 @@ static int l2cap_sock_release(struct socket *sock) if (!sk) return 0; + /* If this is an ATT Client socket, find the matching Server */ + if (l2cap_pi(sk)->scid == L2CAP_CID_LE_DATA && !l2cap_pi(sk)->incoming) + srv_sk = l2cap_find_sock_by_fixed_cid_and_dir(L2CAP_CID_LE_DATA, + &bt_sk(sk)->src, &bt_sk(sk)->dst, 1); + + /* If server socket found, request tear down */ + BT_DBG("client:%p server:%p", sk, srv_sk); + if (srv_sk) + l2cap_sock_set_timer(srv_sk, 1); + err = l2cap_sock_shutdown(sock, 2); sock_orphan(sk);