2 files changed, 55 insertions, 33 deletions

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 338d8c3eeda..98ddd867cfd 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -135,6 +135,50 @@ static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn
 	return c;
 }
 
+struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
+{
+	struct sock *sk;
+	struct hlist_node *node;
+	sk_for_each(sk, node, &l2cap_sk_list.head) {
+		struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+
+		if (chan->sport == psm && !bacmp(&bt_sk(sk)->src, src))
+			goto found;
+	}
+
+	sk = NULL;
+found:
+	return sk;
+}
+
+int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
+{
+	write_lock_bh(&l2cap_sk_list.lock);
+
+	if (__l2cap_get_sock_by_addr(psm, src)) {
+		write_unlock_bh(&l2cap_sk_list.lock);
+		return -EADDRINUSE;
+	}
+
+	chan->psm = psm;
+	chan->sport = psm;
+
+	write_unlock_bh(&l2cap_sk_list.lock);
+
+	return 0;
+}
+
+int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid)
+{
+	write_lock_bh(&l2cap_sk_list.lock);
+
+	chan->scid = scid;
+
+	write_unlock_bh(&l2cap_sk_list.lock);
+
+	return 0;
+}
+
 static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 {
 	u16 cid = L2CAP_CID_DYN_START;
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 09cc7a00534..2156dcecec0 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -78,22 +78,6 @@ void l2cap_sock_clear_timer(struct sock *sk)
 	sk_stop_timer(sk, &sk->sk_timer);
 }
 
-static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
-{
-	struct sock *sk;
-	struct hlist_node *node;
-	sk_for_each(sk, node, &l2cap_sk_list.head) {
-		struct l2cap_chan *chan = l2cap_pi(sk)->chan;
-
-		if (chan->sport == psm && !bacmp(&bt_sk(sk)->src, src))
-			goto found;
-	}
-
-	sk = NULL;
-found:
-	return sk;
-}
-
 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 {
 	struct sock *sk = sock->sk;
@@ -136,26 +120,20 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 		}
 	}
 
-	write_lock_bh(&l2cap_sk_list.lock);
+	if (la.l2_cid)
+		err = l2cap_add_scid(chan, la.l2_cid);
+	else
+		err = l2cap_add_psm(chan, &la.l2_bdaddr, la.l2_psm);
 
-	if (la.l2_psm && __l2cap_get_sock_by_addr(la.l2_psm, &la.l2_bdaddr)) {
-		err = -EADDRINUSE;
-	} else {
-		/* Save source address */
-		bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
-		chan->psm   = la.l2_psm;
-		chan->sport = la.l2_psm;
-		sk->sk_state = BT_BOUND;
-
-		if (__le16_to_cpu(la.l2_psm) == 0x0001 ||
-					__le16_to_cpu(la.l2_psm) == 0x0003)
-			chan->sec_level = BT_SECURITY_SDP;
-	}
+	if (err < 0)
+		goto done;
 
-	if (la.l2_cid)
-		chan->scid = la.l2_cid;
+	if (__le16_to_cpu(la.l2_psm) == 0x0001 ||
+				__le16_to_cpu(la.l2_psm) == 0x0003)
+		chan->sec_level = BT_SECURITY_SDP;
 
-	write_unlock_bh(&l2cap_sk_list.lock);
+	bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
+	sk->sk_state = BT_BOUND;
 
 done:
 	release_sock(sk);