Bluetooth: Add server socket support for LE connection (b62f328b) · Commits · e / devices / android_kernel_fairphone_FP4

include/net/bluetooth/l2cap.h

+1 −0

Original line number	Original line	Diff line number	Diff line
	@@ -38,6 +38,7 @@
	#define L2CAP_DEFAULT_MAX_PDU_SIZE 1009 /* Sized for 3-DH5 packet */		#define L2CAP_DEFAULT_MAX_PDU_SIZE 1009 /* Sized for 3-DH5 packet */
	#define L2CAP_DEFAULT_ACK_TO 200		#define L2CAP_DEFAULT_ACK_TO 200
	#define L2CAP_LOCAL_BUSY_TRIES 12		#define L2CAP_LOCAL_BUSY_TRIES 12
			#define L2CAP_LE_DEFAULT_MTU 23

	#define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */		#define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
	#define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */		#define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */

net/bluetooth/hci_event.c

+8 −2

Original line number	Original line	Diff line number	Diff line
	@@ -2405,8 +2405,14 @@ static inline void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff
	hci_dev_lock(hdev);		hci_dev_lock(hdev);

	conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &ev->bdaddr);		conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &ev->bdaddr);
	if (!conn)		if (!conn) {
	goto unlock;		conn = hci_conn_add(hdev, LE_LINK, &ev->bdaddr);
			if (!conn) {
			BT_ERR("No memory for new connection");
			hci_dev_unlock(hdev);
			return;
			}
			}

	if (ev->status) {		if (ev->status) {
	hci_proto_connect_cfm(conn, ev->status);		hci_proto_connect_cfm(conn, ev->status);

net/bluetooth/l2cap_core.c

+91 −3

Original line number	Original line	Diff line number	Diff line
	@@ -181,8 +181,16 @@ static void __l2cap_chan_add(struct l2cap_conn conn, struct sock sk, struct so
	l2cap_pi(sk)->conn = conn;		l2cap_pi(sk)->conn = conn;

	if (sk->sk_type == SOCK_SEQPACKET \|\| sk->sk_type == SOCK_STREAM) {		if (sk->sk_type == SOCK_SEQPACKET \|\| sk->sk_type == SOCK_STREAM) {
			if (conn->hcon->type == LE_LINK) {
			/* LE connection */
			l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU;
			l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA;
			l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA;
			} else {
	/* Alloc CID for connection-oriented socket */		/* Alloc CID for connection-oriented socket */
	l2cap_pi(sk)->scid = l2cap_alloc_cid(l);		l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
			l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
			}
	} else if (sk->sk_type == SOCK_DGRAM) {		} else if (sk->sk_type == SOCK_DGRAM) {
	/* Connectionless socket */		/* Connectionless socket */
	l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;		l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
	@@ -581,6 +589,82 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
	}		}
	}		}

			/* Find socket with cid and source bdaddr.
			* Returns closest match, locked.
			*/
			static struct sock l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t src)
			{
			struct sock s, sk = NULL, *sk1 = NULL;
			struct hlist_node *node;

			read_lock(&l2cap_sk_list.lock);

			sk_for_each(sk, node, &l2cap_sk_list.head) {
			if (state && sk->sk_state != state)
			continue;

			if (l2cap_pi(sk)->scid == cid) {
			/* Exact match. */
			if (!bacmp(&bt_sk(sk)->src, src))
			break;

			/* Closest match */
			if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
			sk1 = sk;
			}
			}
			s = node ? sk : sk1;
			if (s)
			bh_lock_sock(s);
			read_unlock(&l2cap_sk_list.lock);

			return s;
			}

			static void l2cap_le_conn_ready(struct l2cap_conn *conn)
			{
			struct l2cap_chan_list *list = &conn->chan_list;
			struct sock parent, uninitialized_var(sk);

			BT_DBG("");

			/* Check if we have socket listening on cid */
			parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
			conn->src);
			if (!parent)
			return;

			/* Check for backlog size */
			if (sk_acceptq_is_full(parent)) {
			BT_DBG("backlog full %d", parent->sk_ack_backlog);
			goto clean;
			}

			sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
			if (!sk)
			goto clean;

			write_lock_bh(&list->lock);

			hci_conn_hold(conn->hcon);

			l2cap_sock_init(sk, parent);
			bacpy(&bt_sk(sk)->src, conn->src);
			bacpy(&bt_sk(sk)->dst, conn->dst);

			__l2cap_chan_add(conn, sk, parent);

			l2cap_sock_set_timer(sk, sk->sk_sndtimeo);

			sk->sk_state = BT_CONNECTED;
			parent->sk_data_ready(parent, 0);

			write_unlock_bh(&list->lock);

			clean:
			bh_unlock_sock(parent);
			}

	static void l2cap_conn_ready(struct l2cap_conn *conn)		static void l2cap_conn_ready(struct l2cap_conn *conn)
	{		{
	struct l2cap_chan_list *l = &conn->chan_list;		struct l2cap_chan_list *l = &conn->chan_list;
	@@ -588,6 +672,9 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)

	BT_DBG("conn %p", conn);		BT_DBG("conn %p", conn);

			if (!conn->hcon->out && conn->hcon->type == LE_LINK)
			l2cap_le_conn_ready(conn);

	read_lock(&l->lock);		read_lock(&l->lock);

	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {		for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
	@@ -670,6 +757,7 @@ static struct l2cap_conn l2cap_conn_add(struct hci_conn hcon, u8 status)
	spin_lock_init(&conn->lock);		spin_lock_init(&conn->lock);
	rwlock_init(&conn->chan_list.lock);		rwlock_init(&conn->chan_list.lock);

			if (hcon->type != LE_LINK)
	setup_timer(&conn->info_timer, l2cap_info_timeout,		setup_timer(&conn->info_timer, l2cap_info_timeout,
	(unsigned long) conn);		(unsigned long) conn);

net/bluetooth/l2cap_sock.c

+5 −2

Original line number	Original line	Diff line number	Diff line
	@@ -103,7 +103,7 @@ static int l2cap_sock_bind(struct socket sock, struct sockaddr addr, int alen)
	len = min_t(unsigned int, sizeof(la), alen);		len = min_t(unsigned int, sizeof(la), alen);
	memcpy(&la, addr, len);		memcpy(&la, addr, len);

	if (la.l2_cid)		if (la.l2_cid && la.l2_psm)
	return -EINVAL;		return -EINVAL;

	lock_sock(sk);		lock_sock(sk);
	@@ -145,6 +145,9 @@ static int l2cap_sock_bind(struct socket sock, struct sockaddr addr, int alen)
	l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;		l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
	}		}

			if (la.l2_cid)
			l2cap_pi(sk)->scid = la.l2_cid;

	write_unlock_bh(&l2cap_sk_list.lock);		write_unlock_bh(&l2cap_sk_list.lock);

	done:		done:
	@@ -266,7 +269,7 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
	goto done;		goto done;
	}		}

	if (!l2cap_pi(sk)->psm) {		if (!l2cap_pi(sk)->psm && !l2cap_pi(sk)->dcid) {
	bdaddr_t *src = &bt_sk(sk)->src;		bdaddr_t *src = &bt_sk(sk)->src;
	u16 psm;		u16 psm;