Merge bk://linux-bt.bkbits.net/bt-2.5

into viper.qualcomm.com:/usr/src/bt-2.5

include/net/bluetooth/bluetooth.h

@@ -50,6 +50,7 @@
 #define BTPROTO_HCI     1
 #define BTPROTO_SCO   	2
 #define BTPROTO_RFCOMM	3
+#define BTPROTO_BNEP    4
 
 #define SOL_HCI     0
 #define SOL_L2CAP   6
@@ -199,14 +200,4 @@ int hci_sock_cleanup(void);
 
 int bterr(__u16 code);
 
-#ifndef MODULE_LICENSE
-#define MODULE_LICENSE(x)
-#endif
-
-#ifndef list_for_each_safe
-#define list_for_each_safe(pos, n, head) \
-	for (pos = (head)->next, n = pos->next; pos != (head); \
-		pos = n, n = pos->next)
-#endif
-
 #endif /* __BLUETOOTH_H */

include/net/bluetooth/hci.h

@@ -113,10 +113,10 @@ enum {
 #define ACL_PTYPE_MASK	(~SCO_PTYPE_MASK)
 
 /* ACL flags */
-#define ACL_CONT		0x0001
-#define ACL_START		0x0002
-#define ACL_ACTIVE_BCAST	0x0010
-#define ACL_PICO_BCAST		0x0020
+#define ACL_CONT		0x01
+#define ACL_START		0x02
+#define ACL_ACTIVE_BCAST	0x04
+#define ACL_PICO_BCAST		0x08
 
 /* Baseband links */
 #define SCO_LINK	0x00
@@ -542,7 +542,7 @@ typedef struct {
 	bdaddr_t bdaddr;
 	__u8     role;
 } __attribute__ ((packed)) evt_role_change;
-#define EVT_ROLE_CHANGE_SIZE 1
+#define EVT_ROLE_CHANGE_SIZE 8
 
 #define EVT_PIN_CODE_REQ        0x16
 typedef struct {
@@ -658,6 +658,12 @@ struct sockaddr_hci {
 #define HCI_DEV_NONE	0xffff
 
 struct hci_filter {
+	unsigned long type_mask;
+	unsigned long event_mask[2];
+	__u16   opcode;
+};
+
+struct hci_ufilter {
 	__u32   type_mask;
 	__u32   event_mask[2];
 	__u16   opcode;
@@ -668,20 +674,6 @@ struct hci_filter {
 #define HCI_FLT_OGF_BITS	63
 #define HCI_FLT_OCF_BITS	127
 
-#if BITS_PER_LONG == 64
-static inline void hci_set_bit(int nr, void *addr)
-{
-	*((__u32 *) addr + (nr >> 5)) |= ((__u32) 1 << (nr & 31));
-}
-static inline int hci_test_bit(int nr, void *addr)
-{
-	return *((__u32 *) addr + (nr >> 5)) & ((__u32) 1 << (nr & 31));
-}
-#else
-#define hci_set_bit	set_bit
-#define hci_test_bit	test_bit
-#endif
-
 /* Ioctl requests structures */
 struct hci_dev_stats {
 	__u32 err_rx;

include/net/bluetooth/hci_core.h

@@ -149,7 +149,7 @@ struct hci_conn {
 
 extern struct hci_proto *hci_proto[];
 extern struct list_head hdev_list;
-extern spinlock_t hdev_list_lock;
+extern rwlock_t hdev_list_lock;
 
 /* ----- Inquiry cache ----- */
 #define INQUIRY_CACHE_AGE_MAX   (HZ*30)   // 30 seconds
@@ -339,8 +339,8 @@ static inline void hci_sched_tx(struct hci_dev *hdev)
 /* ----- HCI protocols ----- */
 struct hci_proto {
 	char 		*name;
-	__u32		id;
-	__u32		flags;
+	unsigned int	id;
+	unsigned long	flags;
 
 	void		*priv;
 
@@ -450,12 +450,11 @@ struct hci_pinfo {
 #define HCI_SFLT_MAX_OGF 4
 
 struct hci_sec_filter {
-	__u32 type_mask;
-	__u32 event_mask[2];
-	__u32 ocf_mask[HCI_SFLT_MAX_OGF + 1][4];
+	unsigned long type_mask;
+	unsigned long event_mask[2];
+	unsigned long ocf_mask[HCI_SFLT_MAX_OGF + 1][4];
 };
 
-
 /* ----- HCI requests ----- */
 #define HCI_REQ_DONE	  0
 #define HCI_REQ_PEND	  1

net/bluetooth/af_bluetooth.c

@@ -27,7 +27,7 @@
  *
  * $Id: af_bluetooth.c,v 1.3 2002/04/17 17:37:15 maxk Exp $
  */
-#define VERSION "2.0"
+#define VERSION "2.2"
 
 #include <linux/config.h>
 #include <linux/module.h>
@@ -57,7 +57,7 @@
 #endif
 
 /* Bluetooth sockets */
-#define BLUEZ_MAX_PROTO	4
+#define BLUEZ_MAX_PROTO	5
 static struct net_proto_family *bluez_proto[BLUEZ_MAX_PROTO];
 
 static kmem_cache_t *bluez_sock_cache;
@@ -136,18 +136,18 @@ struct sock *bluez_sock_alloc(struct socket *sock, int proto, int pi_size, int p
 
 void bluez_sock_link(struct bluez_sock_list *l, struct sock *sk)
 {
-	write_lock(&l->lock);
+	write_lock_bh(&l->lock);
 	sk->next = l->head;
 	l->head = sk;
 	sock_hold(sk);
-	write_unlock(&l->lock);
+	write_unlock_bh(&l->lock);
 }
 
 void bluez_sock_unlink(struct bluez_sock_list *l, struct sock *sk)
 {
 	struct sock **skp;
 
-	write_lock(&l->lock);
+	write_lock_bh(&l->lock);
 	for (skp = &l->head; *skp; skp = &((*skp)->next)) {
 		if (*skp == sk) {
 			*skp = sk->next;
@@ -155,7 +155,7 @@ void bluez_sock_unlink(struct bluez_sock_list *l, struct sock *sk)
 			break;
 		}
 	}
-	write_unlock(&l->lock);
+	write_unlock_bh(&l->lock);
 }
 
 void bluez_accept_enqueue(struct sock *parent, struct sock *sk)
@@ -265,6 +265,9 @@ unsigned int bluez_sock_poll(struct file * file, struct socket *sock, poll_table
 	if (sk->state == BT_CLOSED)
 		mask |= POLLHUP;
 
+	if (sk->state == BT_CONNECT || sk->state == BT_CONNECT2)
+		return mask;
+	
 	if (sock_writeable(sk))
 		mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
 	else

net/bluetooth/hci_conn.c

@@ -73,7 +73,7 @@ void hci_acl_connect(struct hci_conn *conn)
 	bacpy(&cp.bdaddr, &conn->dst);
 
 	if ((ie = inquiry_cache_lookup(hdev, &conn->dst)) &&
-			inquiry_entry_age(ie) > INQUIRY_ENTRY_AGE_MAX) {
+			inquiry_entry_age(ie) <= INQUIRY_ENTRY_AGE_MAX) {
 		cp.pscan_rep_mode = ie->info.pscan_rep_mode;
 		cp.pscan_mode     = ie->info.pscan_mode;
 		cp.clock_offset   = ie->info.clock_offset | __cpu_to_le16(0x8000);
@@ -188,9 +188,6 @@ int hci_conn_del(struct hci_conn *conn)
 			acl->link = NULL;
 			hci_conn_put(acl);
 		}
-
-		/* Unacked frames */
-		hdev->sco_cnt += conn->sent;
 	} else {
 		struct hci_conn *sco = conn->link;
 		if (sco)
@@ -220,7 +217,7 @@ struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src)
 
 	BT_DBG("%s -> %s", batostr(src), batostr(dst));
 
-	spin_lock_bh(&hdev_list_lock);
+	read_lock_bh(&hdev_list_lock);
 
 	list_for_each(p, &hdev_list) {
 		struct hci_dev *d;
@@ -248,7 +245,7 @@ struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src)
 	if (hdev)
 		hci_dev_hold(hdev);
 
-	spin_unlock_bh(&hdev_list_lock);
+	read_unlock_bh(&hdev_list_lock);
 	return hdev;
 }
 

net/bluetooth/hci_core.c

@@ -30,6 +30,7 @@
 
 #include <linux/config.h>
 #include <linux/module.h>
+#include <linux/kmod.h>
 
 #include <linux/types.h>
 #include <linux/errno.h>
@@ -66,7 +67,7 @@ rwlock_t hci_task_lock = RW_LOCK_UNLOCKED;
 
 /* HCI device list */
 LIST_HEAD(hdev_list);
-spinlock_t hdev_list_lock;
+rwlock_t hdev_list_lock = RW_LOCK_UNLOCKED;
 
 /* HCI protocols */
 #define HCI_MAX_PROTO	2
@@ -75,7 +76,6 @@ struct hci_proto *hci_proto[HCI_MAX_PROTO];
 /* HCI notifiers list */
 static struct notifier_block *hci_notifier;
 
-
 /* ---- HCI notifications ---- */
 
 int hci_register_notifier(struct notifier_block *nb)
@@ -93,6 +93,32 @@ void hci_notify(struct hci_dev *hdev, int event)
 	notifier_call_chain(&hci_notifier, event, hdev);
 }
 
+/* ---- HCI hotplug support ---- */
+
+#ifdef CONFIG_HOTPLUG
+
+static int hci_run_hotplug(char *dev, char *action)
+{
+	char *argv[3], *envp[5], dstr[20], astr[32];
+
+	sprintf(dstr, "DEVICE=%s", dev);
+	sprintf(astr, "ACTION=%s", action);
+
+        argv[0] = hotplug_path;
+        argv[1] = "bluetooth";
+        argv[2] = NULL;
+
+	envp[0] = "HOME=/";
+	envp[1] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
+	envp[2] = dstr;
+	envp[3] = astr;
+	envp[4] = NULL;
+	
+	return call_usermodehelper(argv[0], argv, envp);
+}
+#else
+#define hci_run_hotplug(A...)
+#endif
 
 /* ---- HCI requests ---- */
 
@@ -270,7 +296,7 @@ struct hci_dev *hci_dev_get(int index)
 	if (index < 0)
 		return NULL;
 
-	spin_lock(&hdev_list_lock);
+	read_lock(&hdev_list_lock);
 	list_for_each(p, &hdev_list) {
 		hdev = list_entry(p, struct hci_dev, list);
 		if (hdev->id == index) {
@@ -280,7 +306,7 @@ struct hci_dev *hci_dev_get(int index)
 	}
 	hdev = NULL;
 done:
-	spin_unlock(&hdev_list_lock);
+	read_unlock(&hdev_list_lock);
 	return hdev;
 }
 
@@ -699,7 +725,7 @@ int hci_get_dev_list(unsigned long arg)
 		return -ENOMEM;
 	dr = dl->dev_req;
 
-	spin_lock_bh(&hdev_list_lock);
+	read_lock_bh(&hdev_list_lock);
 	list_for_each(p, &hdev_list) {
 		struct hci_dev *hdev;
 		hdev = list_entry(p, struct hci_dev, list);
@@ -708,7 +734,7 @@ int hci_get_dev_list(unsigned long arg)
 		if (++n >= dev_num)
 			break;
 	}
-	spin_unlock_bh(&hdev_list_lock);
+	read_unlock_bh(&hdev_list_lock);
 
 	dl->dev_num = n;
 	size = n * sizeof(struct hci_dev_req) + sizeof(__u16);
@@ -768,7 +794,7 @@ int hci_register_dev(struct hci_dev *hdev)
 	if (!hdev->open || !hdev->close || !hdev->destruct)
 		return -EINVAL;
 
-	spin_lock_bh(&hdev_list_lock);
+	write_lock_bh(&hdev_list_lock);
 
 	/* Find first available device id */
 	list_for_each(p, &hdev_list) {
@@ -807,11 +833,12 @@ int hci_register_dev(struct hci_dev *hdev)
 
 	atomic_set(&hdev->promisc, 0);
 		
-	hci_notify(hdev, HCI_DEV_REG);
-
 	MOD_INC_USE_COUNT;
 
-	spin_unlock_bh(&hdev_list_lock);
+	write_unlock_bh(&hdev_list_lock);
+
+	hci_notify(hdev, HCI_DEV_REG);
+	hci_run_hotplug(hdev->name, "register");
 
 	return id;
 }
@@ -821,13 +848,15 @@ int hci_unregister_dev(struct hci_dev *hdev)
 {
 	BT_DBG("%p name %s type %d", hdev, hdev->name, hdev->type);
 
-	spin_lock_bh(&hdev_list_lock);
+	write_lock_bh(&hdev_list_lock);
 	list_del(&hdev->list);
-	spin_unlock_bh(&hdev_list_lock);
+	write_unlock_bh(&hdev_list_lock);
 
 	hci_dev_do_close(hdev);
 
 	hci_notify(hdev, HCI_DEV_UNREG);
+	hci_run_hotplug(hdev->name, "unregister");
+	
 	hci_dev_put(hdev);
 
 	MOD_DEC_USE_COUNT;
@@ -1103,14 +1132,13 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
 {
 	struct conn_hash *h = &hdev->conn_hash;
 	struct hci_conn  *conn = NULL;
-	int num = 0, min = 0xffff;
+	int num = 0, min = ~0;
         struct list_head *p;
 
 	/* We don't have to lock device here. Connections are always 
 	 * added and removed with TX task disabled. */
 	list_for_each(p, &h->list) {
 		struct hci_conn *c;
-
 		c = list_entry(p, struct hci_conn, list);
 
 		if (c->type != type || c->state != BT_CONNECTED
@@ -1118,14 +1146,15 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
 			continue;
 		num++;
 
-		if (c->sent < min || type == SCO_LINK) {
+		if (c->sent < min) {
 			min  = c->sent;
 			conn = c;
 		}
 	}
 
 	if (conn) {
-		int q = hdev->acl_cnt / num;
+		int cnt = (type == ACL_LINK ? hdev->acl_cnt : hdev->sco_cnt);
+		int q = cnt / num;
 		*quote = q ? q : 1;
 	} else
 		*quote = 0;
@@ -1134,6 +1163,25 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
 	return conn;
 }
 
+static inline void hci_acl_tx_to(struct hci_dev *hdev)
+{
+	struct conn_hash *h = &hdev->conn_hash;
+        struct list_head *p;
+	struct hci_conn  *c;
+
+	BT_ERR("%s ACL tx timeout", hdev->name);
+
+	/* Kill stalled connections */
+	list_for_each(p, &h->list) {
+		c = list_entry(p, struct hci_conn, list);
+		if (c->type == ACL_LINK && c->sent) {
+			BT_ERR("%s killing stalled ACL connection %s",
+				hdev->name, batostr(&c->dst));
+			hci_acl_disconn(c, 0x13);
+		}
+	}
+}
+
 static inline void hci_sched_acl(struct hci_dev *hdev)
 {
 	struct hci_conn *conn;
@@ -1142,21 +1190,19 @@ static inline void hci_sched_acl(struct hci_dev *hdev)
 
 	BT_DBG("%s", hdev->name);
 
-	if (!hdev->acl_cnt && (jiffies - hdev->acl_last_tx) > HZ*5) {
-		BT_ERR("%s ACL tx timeout", hdev->name);
-		hdev->acl_cnt++;
-	}
+	/* ACL tx timeout must be longer than maximum
+	 * link supervision timeout (40.9 seconds) */
+	if (!hdev->acl_cnt && (jiffies - hdev->acl_last_tx) > (HZ * 45))
+		hci_acl_tx_to(hdev);
 
 	while (hdev->acl_cnt && (conn = hci_low_sent(hdev, ACL_LINK, &quote))) {
-		while (quote && (skb = skb_dequeue(&conn->data_q))) {
+		while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
 			BT_DBG("skb %p len %d", skb, skb->len);
-
 			hci_send_frame(skb);
 			hdev->acl_last_tx = jiffies;
 
-			conn->sent++;
 			hdev->acl_cnt--;
-			quote--;
+			conn->sent++;
 		}
 	}
 }
@@ -1170,15 +1216,14 @@ static inline void hci_sched_sco(struct hci_dev *hdev)
 
 	BT_DBG("%s", hdev->name);
 
-	while ((conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
-		while (quote && (skb = skb_dequeue(&conn->data_q))) {
+	while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
+		while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
 			BT_DBG("skb %p len %d", skb, skb->len);
-
 			hci_send_frame(skb);
 
-			//conn->sent++;
-			//hdev->sco_cnt--;
-			quote--;
+			conn->sent++;
+			if (conn->sent == ~0)
+				conn->sent = 0;
 		}
 	}
 }
@@ -1205,7 +1250,6 @@ static void hci_tx_task(unsigned long arg)
 	read_unlock(&hci_task_lock);
 }
 
-
 /* ----- HCI RX task (incomming data proccessing) ----- */
 
 /* ACL data packet */
@@ -1367,9 +1411,6 @@ static void hci_cmd_task(unsigned long arg)
 
 int hci_core_init(void)
 {
-	/* Init locks */
-	spin_lock_init(&hdev_list_lock);
-
 	return 0;
 }
 

net/bluetooth/hci_event.c

@@ -352,15 +352,12 @@ static void hci_cs_link_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
 			hci_dev_lock(hdev);
 	
 			acl = conn_hash_lookup_handle(hdev, handle);
-			if (!acl || !(sco = acl->link)) {
-				hci_dev_unlock(hdev);
-				break;
-			}
-
+			if (acl && (sco = acl->link)) {
 				sco->state = BT_CLOSED;
 
 				hci_proto_connect_cfm(sco, status);
 				hci_conn_del(sco);
+			}
 
 			hci_dev_unlock(hdev);
 		}

net/bluetooth/hci_sock.c

@@ -86,7 +86,7 @@ static struct bluez_sock_list hci_sk_list = {
 /* Send frame to RAW socket */
 void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb)
 {
-	struct sock * sk;
+	struct sock *sk;
 
 	BT_DBG("hdev %p len %d", hdev, skb->len);
 
@@ -105,13 +105,13 @@ void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb)
 		/* Apply filter */
 		flt = &hci_pi(sk)->filter;
 
-		if (!hci_test_bit((skb->pkt_type & HCI_FLT_TYPE_BITS), &flt->type_mask))
+		if (!test_bit((skb->pkt_type & HCI_FLT_TYPE_BITS), &flt->type_mask))
 			continue;
 
 		if (skb->pkt_type == HCI_EVENT_PKT) {
 			register int evt = (*(__u8 *)skb->data & HCI_FLT_EVENT_BITS);
 			
-			if (!hci_test_bit(evt, &flt->event_mask))
+			if (!test_bit(evt, flt->event_mask))
 				continue;
 
 			if (flt->opcode && ((evt == EVT_CMD_COMPLETE && 
@@ -249,7 +249,7 @@ static int hci_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long a
 		err = hci_sock_bound_ioctl(sk, cmd, arg);
 		release_sock(sk);
 		return err;
-	};
+	}
 }
 
 static int hci_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
@@ -393,12 +393,12 @@ static int hci_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len,
 		err = -EPERM;
 
 		if (skb->pkt_type == HCI_COMMAND_PKT) {
-			__u16 opcode = __le16_to_cpu(*(__u16 *)skb->data);
-			__u16 ogf = cmd_opcode_ogf(opcode) - 1;
-			__u16 ocf = cmd_opcode_ocf(opcode) & HCI_FLT_OCF_BITS;
+			u16 opcode = __le16_to_cpu(*(__u16 *)skb->data);
+			u16 ogf = cmd_opcode_ogf(opcode) - 1;
+			u16 ocf = cmd_opcode_ocf(opcode) & HCI_FLT_OCF_BITS;
 
 			if (ogf > HCI_SFLT_MAX_OGF ||
-					!hci_test_bit(ocf, &hci_sec_filter.ocf_mask[ogf]))
+					!test_bit(ocf, hci_sec_filter.ocf_mask[ogf]))
 				goto drop;
 		} else
 			goto drop;
@@ -420,8 +420,8 @@ static int hci_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len,
 
 int hci_sock_setsockopt(struct socket *sock, int level, int optname, char *optval, int len)
 {
+	struct hci_ufilter uf = { .opcode = 0 };
 	struct sock *sk = sock->sk;
-	struct hci_filter flt = { opcode: 0 };
 	int err = 0, opt = 0;
 
 	BT_DBG("sk %p, opt %d", sk, optname);
@@ -454,25 +454,32 @@ int hci_sock_setsockopt(struct socket *sock, int level, int optname, char *optva
 		break;
 
 	case HCI_FILTER:
-		len = MIN(len, sizeof(struct hci_filter));
-		if (copy_from_user(&flt, optval, len)) {
+		len = MIN(len, sizeof(uf));
+		if (copy_from_user(&uf, optval, len)) {
 			err = -EFAULT;
 			break;
 		}
 
 		if (!capable(CAP_NET_RAW)) {
-			flt.type_mask     &= hci_sec_filter.type_mask;
-			flt.event_mask[0] &= hci_sec_filter.event_mask[0];
-			flt.event_mask[1] &= hci_sec_filter.event_mask[1];
+			uf.type_mask &= hci_sec_filter.type_mask;
+			uf.event_mask[0] &= *((u32 *) hci_sec_filter.event_mask + 0);
+			uf.event_mask[1] &= *((u32 *) hci_sec_filter.event_mask + 1);
 		}
 
-		memcpy(&hci_pi(sk)->filter, &flt, len);
+		{	
+			struct hci_filter *f = &hci_pi(sk)->filter;
+		
+			f->type_mask = uf.type_mask;
+			f->opcode    = uf.opcode;
+			*((u32 *) f->event_mask + 0) = uf.event_mask[0];
+			*((u32 *) f->event_mask + 1) = uf.event_mask[0];
+		}
 		break; 
 
 	default:
 		err = -ENOPROTOOPT;
 		break;
-	};
+	}
 	
 	release_sock(sk);
 	return err;
@@ -480,6 +487,7 @@ int hci_sock_setsockopt(struct socket *sock, int level, int optname, char *optva
 
 int hci_sock_getsockopt(struct socket *sock, int level, int optname, char *optval, int *optlen)
 {
+	struct hci_ufilter uf;
 	struct sock *sk = sock->sk;
 	int len, opt; 
 
@@ -508,15 +516,24 @@ int hci_sock_getsockopt(struct socket *sock, int level, int optname, char *optva
 		break;
 
 	case HCI_FILTER:
-		len = MIN(len, sizeof(struct hci_filter));
-		if (copy_to_user(optval, &hci_pi(sk)->filter, len))
+		{
+			struct hci_filter *f = &hci_pi(sk)->filter;
+		
+			uf.type_mask = f->type_mask;
+			uf.opcode    = f->opcode;
+			uf.event_mask[0] = *((u32 *) f->event_mask + 0);
+			uf.event_mask[0] = *((u32 *) f->event_mask + 1);
+		}
+
+		len = MIN(len, sizeof(uf));
+		if (copy_to_user(optval, &uf, len))
 			return -EFAULT;
 		break;
 
 	default:
 		return -ENOPROTOOPT;
 		break;
-	};
+	}
 
 	return 0;
 }

net/bluetooth/l2cap.c

@@ -25,9 +25,9 @@
 /*
  * BlueZ L2CAP core and sockets.
  *
- * $Id: l2cap.c,v 1.8 2002/04/19 00:01:39 maxk Exp $
+ * $Id: l2cap.c,v 1.15 2002/09/09 01:14:52 maxk Exp $
  */
-#define VERSION "2.0"
+#define VERSION "2.1"
 
 #include <linux/config.h>
 #include <linux/module.h>
@@ -51,6 +51,7 @@
 
 #include <asm/system.h>
 #include <asm/uaccess.h>
+#include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -69,7 +70,7 @@ struct bluez_sock_list l2cap_sk_list = {
 
 static int l2cap_conn_del(struct hci_conn *conn, int err);
 
-static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent);
+static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent);
 static void l2cap_chan_del(struct sock *sk, int err);
 static int  l2cap_chan_send(struct sock *sk, struct msghdr *msg, int len);
 
@@ -177,6 +178,14 @@ static int l2cap_conn_del(struct hci_conn *hcon, int err)
 	return 0;
 }
 
+static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
+{
+	struct l2cap_chan_list *l = &conn->chan_list;
+	write_lock(&l->lock);
+	__l2cap_chan_add(conn, sk, parent);
+	write_unlock(&l->lock);
+}
+
 int l2cap_connect(struct sock *sk)
 {
 	bdaddr_t *src = &bluez_sk(sk)->src;
@@ -234,32 +243,26 @@ int l2cap_connect(struct sock *sk)
 }
 
 /* -------- Socket interface ---------- */
-static struct sock *__l2cap_get_sock_by_addr(struct sockaddr_l2 *addr)
+static struct sock *__l2cap_get_sock_by_addr(__u16 psm, bdaddr_t *src)
 {
-	bdaddr_t *src = &addr->l2_bdaddr;
-	__u16 psm = addr->l2_psm;
 	struct sock *sk;
-
 	for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
 		if (l2cap_pi(sk)->psm == psm &&
 				!bacmp(&bluez_sk(sk)->src, src))
 			break;
 	}
-
 	return sk;
 }
 
-/* Find socket listening on psm and source bdaddr.
+/* Find socket with psm and source bdaddr.
  * Returns closest match.
  */
-static struct sock *l2cap_get_sock_listen(bdaddr_t *src, __u16 psm)
+static struct sock *__l2cap_get_sock_by_psm(int state, __u16 psm, bdaddr_t *src)
 {
 	struct sock *sk, *sk1 = NULL;
 
-	read_lock(&l2cap_sk_list.lock);
-
 	for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
-		if (sk->state != BT_LISTEN)
+		if (state && sk->state != state)
 			continue;
 
 		if (l2cap_pi(sk)->psm == psm) {
@@ -272,9 +275,19 @@ static struct sock *l2cap_get_sock_listen(bdaddr_t *src, __u16 psm)
 				sk1 = sk;
 		}
 	}
+	return sk ? sk : sk1;
+}
 
+/* Find socket with given address (psm, src).
+ * Returns locked socket */
+static inline struct sock *l2cap_get_sock_by_psm(int state, __u16 psm, bdaddr_t *src)
+{
+	struct sock *s;
+	read_lock(&l2cap_sk_list.lock);
+	s = __l2cap_get_sock_by_psm(state, psm, src);
+	if (s) bh_lock_sock(s);
 	read_unlock(&l2cap_sk_list.lock);
-	return sk ? sk : sk1;
+	return s;
 }
 
 static void l2cap_sock_destruct(struct sock *sk)
@@ -320,8 +333,6 @@ static void l2cap_sock_kill(struct sock *sk)
 	sock_put(sk);
 }
 
-/* Close socket.
- */
 static void __l2cap_sock_close(struct sock *sk, int reason)
 {
 	BT_DBG("sk %p state %d socket %p", sk, sk->state, sk->socket);
@@ -333,6 +344,7 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
 
 	case BT_CONNECTED:
 	case BT_CONFIG:
+	case BT_CONNECT2:
 		if (sk->type == SOCK_SEQPACKET) {
 			struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 			l2cap_disconn_req req;
@@ -349,7 +361,6 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
 		break;
 
 	case BT_CONNECT:
-	case BT_CONNECT2:
 	case BT_DISCONN:
 		l2cap_chan_del(sk, reason);
 		break;
@@ -380,7 +391,6 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 
 	if (parent) {
 		sk->type = parent->type;
-
 		pi->imtu = l2cap_pi(parent)->imtu;
 		pi->omtu = l2cap_pi(parent)->omtu;
 		pi->link_mode = l2cap_pi(parent)->link_mode;
@@ -405,6 +415,8 @@ static struct sock *l2cap_sock_alloc(struct socket *sock, int proto, int prio)
 
 	sk->destruct = l2cap_sock_destruct;
 	sk->sndtimeo = L2CAP_CONN_TIMEOUT;
+
+	sk->protocol = proto;
 	sk->state    = BT_OPEN;
 
 	l2cap_sock_init_timer(sk);
@@ -421,10 +433,11 @@ static int l2cap_sock_create(struct socket *sock, int protocol)
 
 	BT_DBG("sock %p", sock);
 
-	if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_RAW)
+	sock->state = SS_UNCONNECTED;
+
+	if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
 		return -ESOCKTNOSUPPORT;
 
-	sock->state = SS_UNCONNECTED;
 	sock->ops = &l2cap_sock_ops;
 
 	sk = l2cap_sock_alloc(sock, protocol, GFP_KERNEL);
@@ -453,20 +466,16 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_
 		goto done;
 	}
 
-	write_lock(&l2cap_sk_list.lock);
-
-	if (la->l2_psm && __l2cap_get_sock_by_addr(la)) {
+	write_lock_bh(&l2cap_sk_list.lock);
+	if (la->l2_psm && __l2cap_get_sock_by_addr(la->l2_psm, &la->l2_bdaddr)) {
 		err = -EADDRINUSE;
-		goto unlock;
-	}
-
+	} else {
 		/* Save source address */
 		bacpy(&bluez_sk(sk)->src, &la->l2_bdaddr);
 		l2cap_pi(sk)->psm = la->l2_psm;
 		sk->state = BT_BOUND;
-
-unlock:
-	write_unlock(&l2cap_sk_list.lock);
+	}
+	write_unlock_bh(&l2cap_sk_list.lock);
 
 done:
 	release_sock(sk);
@@ -629,7 +638,6 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
 		bacpy(&la->l2_bdaddr, &bluez_sk(sk)->src);
 
 	la->l2_psm = l2cap_pi(sk)->psm;
-
 	return 0;
 }
 
@@ -646,6 +654,10 @@ static int l2cap_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len,
 	if (msg->msg_flags & MSG_OOB)
 		return -EOPNOTSUPP;
 
+	/* Check outgoing MTU */
+	if (len > l2cap_pi(sk)->omtu)
+		return -EINVAL;
+
 	lock_sock(sk);
 
 	if (sk->state == BT_CONNECTED)
@@ -691,7 +703,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 	default:
 		err = -ENOPROTOOPT;
 		break;
-	};
+	}
 
 	release_sock(sk);
 	return err;
@@ -743,20 +755,37 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
 	default:
 		err = -ENOPROTOOPT;
 		break;
-	};
+	}
 
 	release_sock(sk);
 	return err;
 }
 
-static int l2cap_sock_release(struct socket *sock)
+static int l2cap_sock_shutdown(struct socket *sock, int how)
 {
 	struct sock *sk = sock->sk;
 
 	BT_DBG("sock %p, sk %p", sock, sk);
 
-	if (!sk)
+	if (!sk) return 0;
+
+	l2cap_sock_clear_timer(sk);
+
+	lock_sock(sk);
+	sk->shutdown = SHUTDOWN_MASK;
+	__l2cap_sock_close(sk, ECONNRESET);
+	release_sock(sk);
+
 	return 0;
+}
+
+static int l2cap_sock_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	BT_DBG("sock %p, sk %p", sock, sk);
+
+	if (!sk) return 0;
 
 	sock_orphan(sk);
 	l2cap_sock_close(sk);
@@ -767,24 +796,20 @@ static int l2cap_sock_release(struct socket *sock)
 static struct sock * __l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, __u16 cid)
 {
 	struct sock *s;
-
 	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 		if (l2cap_pi(s)->dcid == cid)
 			break;
 	}
-
 	return s;
 }
 
 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
 {
 	struct sock *s;
-
 	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 		if (l2cap_pi(s)->scid == cid)
 			break;
 	}
-
 	return s;
 }
 
@@ -850,10 +875,15 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct so
 	l2cap_pi(sk)->conn = conn;
 
 	if (sk->type == SOCK_SEQPACKET) {
-		/* Alloc CID for normal socket */
+		/* Alloc CID for connection-oriented socket */
 		l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
+	} else if (sk->type == SOCK_DGRAM) {
+		/* Connectionless socket */
+		l2cap_pi(sk)->scid = 0x0002;
+		l2cap_pi(sk)->dcid = 0x0002;
+		l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 	} else {
-		/* Raw socket can send only signalling messages */
+		/* Raw socket can send/recv signalling messages only */
 		l2cap_pi(sk)->scid = 0x0001;
 		l2cap_pi(sk)->dcid = 0x0001;
 		l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
@@ -865,14 +895,6 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct so
 		bluez_accept_enqueue(parent, sk);
 }
 
-static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
-{
-	struct l2cap_chan_list *l = &conn->chan_list;
-	write_lock(&l->lock);
-	__l2cap_chan_add(conn, sk, parent);
-	write_unlock(&l->lock);
-}
-
 /* Delete channel. 
  * Must be called on the locked socket. */
 static void l2cap_chan_del(struct sock *sk, int err)
@@ -984,25 +1006,31 @@ static int l2cap_chan_send(struct sock *sk, struct msghdr *msg, int len)
 {
 	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 	struct sk_buff *skb, **frag;
-	int err, size, count, sent=0;
+	int err, hlen, count, sent=0;
 	l2cap_hdr *lh;
 
-	/* Check outgoing MTU */
-	if (len > l2cap_pi(sk)->omtu)
-		return -EINVAL;
-
 	BT_DBG("sk %p len %d", sk, len);
 
 	/* First fragment (with L2CAP header) */
-	count = MIN(conn->mtu - L2CAP_HDR_SIZE, len);
-	size  = L2CAP_HDR_SIZE + count;
-	if (!(skb = bluez_skb_send_alloc(sk, size, msg->msg_flags & MSG_DONTWAIT, &err)))
+	if (sk->type == SOCK_DGRAM)
+		hlen = L2CAP_HDR_SIZE + 2;
+	else
+		hlen = L2CAP_HDR_SIZE;
+
+	count = MIN(conn->mtu - hlen, len);
+
+	skb = bluez_skb_send_alloc(sk, hlen + count,
+			msg->msg_flags & MSG_DONTWAIT, &err);
+	if (!skb)
 		return err;
 
 	/* Create L2CAP header */
 	lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
-	lh->len = __cpu_to_le16(len);
 	lh->cid = __cpu_to_le16(l2cap_pi(sk)->dcid);
+	lh->len = __cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+
+	if (sk->type == SOCK_DGRAM)
+		put_unaligned(l2cap_pi(sk)->psm, (__u16 *) skb_put(skb, 2));
 
 	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
 		err = -EFAULT;
@@ -1315,38 +1343,42 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd,
 	BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
 
 	/* Check if we have socket listening on psm */
-	if (!(parent = l2cap_get_sock_listen(conn->src, psm))) {
+	parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
+	if (!parent) {
 		result = L2CAP_CR_BAD_PSM;
-		goto resp;
+		goto sendresp;
 	}
 
-	write_lock(&list->lock);
-	bh_lock_sock(parent);
-
 	result = L2CAP_CR_NO_MEM;
 
-	/* Check if we already have channel with that dcid */
-	if (__l2cap_get_chan_by_dcid(list, scid))
-		goto unlock;
-
 	/* Check for backlog size */
 	if (parent->ack_backlog > parent->max_ack_backlog) {
 		BT_DBG("backlog full %d", parent->ack_backlog); 
-		goto unlock;
+		goto response;
 	}
 
-	if (!(sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC)))
-		goto unlock;
+	sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
+	if (!sk)
+		goto response;
 
-	l2cap_sock_init(sk, parent);
+	write_lock(&list->lock);
+
+	/* Check if we already have channel with that dcid */
+	if (__l2cap_get_chan_by_dcid(list, scid)) {
+		write_unlock(&list->lock);
+		sk->zapped = 1;
+		l2cap_sock_kill(sk);
+		goto response;
+	}
 
+	hci_conn_hold(conn->hcon);
+
+	l2cap_sock_init(sk, parent);
 	bacpy(&bluez_sk(sk)->src, conn->src);
 	bacpy(&bluez_sk(sk)->dst, conn->dst);
 	l2cap_pi(sk)->psm  = psm;
 	l2cap_pi(sk)->dcid = scid;
 
-	hci_conn_hold(conn->hcon);
-
 	__l2cap_chan_add(conn, sk, parent);
 	dcid = l2cap_pi(sk)->scid;
 
@@ -1360,20 +1392,22 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd,
 	
 	if (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) {
 		if (!hci_conn_encrypt(conn->hcon))
-			goto unlock;
+			goto done;
 	} else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
 		if (!hci_conn_auth(conn->hcon))
-			goto unlock;
+			goto done;
 	}
 
 	sk->state = BT_CONFIG;
 	result = status = 0;
 
-unlock:
-	bh_unlock_sock(parent);
+done:
 	write_unlock(&list->lock);
 
-resp:
+response:
+	bh_unlock_sock(parent);
+
+sendresp:
 	rsp.scid   = __cpu_to_le16(scid);
 	rsp.dcid   = __cpu_to_le16(dcid);
 	rsp.result = __cpu_to_le16(result);
@@ -1678,10 +1712,37 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, __u16 cid, struct
 	return 0;
 }
 
+static inline int l2cap_conless_channel(struct l2cap_conn *conn, __u16 psm, struct sk_buff *skb)
+{
+	struct sock *sk;
+
+	sk = l2cap_get_sock_by_psm(0, psm, conn->src);
+	if (!sk)
+		goto drop;
+
+	BT_DBG("sk %p, len %d", sk, skb->len);
+
+	if (sk->state != BT_BOUND && sk->state != BT_CONNECTED)
+		goto drop;
+
+	if (l2cap_pi(sk)->imtu < skb->len)
+		goto drop;
+
+	if (!sock_queue_rcv_skb(sk, skb))
+		goto done;
+
+drop:
+	kfree_skb(skb);
+
+done:
+	if (sk) bh_unlock_sock(sk);
+	return 0;
+}
+
 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
 {
 	l2cap_hdr *lh = (l2cap_hdr *) skb->data;
-	__u16 cid, len;
+	__u16 cid, psm, len;
 
 	skb_pull(skb, L2CAP_HDR_SIZE);
 	cid = __le16_to_cpu(lh->cid);
@@ -1689,10 +1750,21 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
 
 	BT_DBG("len %d, cid 0x%4.4x", len, cid);
 
-	if (cid == 0x0001)
+	switch (cid) {
+	case 0x0001:
 		l2cap_sig_channel(conn, skb);
-	else	
+		break;
+
+	case 0x0002:
+		psm = get_unaligned((__u16 *) skb->data);
+		skb_pull(skb, 2);
+		l2cap_conless_channel(conn, psm, skb);
+		break;
+		
+	default:
 		l2cap_data_channel(conn, cid, skb);
+		break;
+	}
 }
 
 /* ------------ L2CAP interface with lower layer (HCI) ------------- */
@@ -1859,8 +1931,8 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, __u16
 	BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
 
 	if (flags & ACL_START) {
-		int flen, tlen, size;
-		l2cap_hdr *lh;
+		l2cap_hdr *hdr;
+		int len;
 
 		if (conn->rx_len) {
 			BT_ERR("Unexpected start frame (len %d)", skb->len);
@@ -1869,30 +1941,28 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, __u16
 			conn->rx_len = 0;
 		}
 
-		if (skb->len < L2CAP_HDR_SIZE) {
+		if (skb->len < 2) {
 			BT_ERR("Frame is too small (len %d)", skb->len);
 			goto drop;
 		}
 
-		lh = (l2cap_hdr *)skb->data;
-		tlen = __le16_to_cpu(lh->len);
-		flen = skb->len - L2CAP_HDR_SIZE;
+		hdr = (l2cap_hdr *) skb->data;
+		len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
 
-		BT_DBG("Start: total len %d, frag len %d", tlen, flen);
+		BT_DBG("Start: total len %d, frag len %d", len, skb->len);
 
-		if (flen == tlen) {
+		if (len == skb->len) {
 			/* Complete frame received */
 			l2cap_recv_frame(conn, skb);
 			return 0;
 		}
 
 		/* Allocate skb for the complete frame (with header) */
-		size = L2CAP_HDR_SIZE + tlen;
-		if (!(conn->rx_skb = bluez_skb_alloc(size, GFP_ATOMIC)))
+		if (!(conn->rx_skb = bluez_skb_alloc(len, GFP_ATOMIC)))
 			goto drop;
 
 		memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
-		conn->rx_len = tlen - flen;
+		conn->rx_len = len - skb->len;
 	} else {
 		BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
 
@@ -1932,7 +2002,7 @@ static int l2cap_sock_dump(char *buf, struct bluez_sock_list *list)
 	struct sock *sk;
 	char *ptr = buf;
 
-	write_lock(&list->lock);
+	read_lock_bh(&list->lock);
 
 	for (sk = list->head; sk; sk = sk->next) {
 		pi = l2cap_pi(sk);
@@ -1942,7 +2012,7 @@ static int l2cap_sock_dump(char *buf, struct bluez_sock_list *list)
 				pi->link_mode);
 	}
 
-	write_unlock(&list->lock);
+	read_unlock_bh(&list->lock);
 
 	ptr += sprintf(ptr, "\n");
 	return ptr - buf;
@@ -1983,12 +2053,12 @@ static struct proto_ops l2cap_sock_ops = {
 	.sendmsg =      l2cap_sock_sendmsg,
 	.recvmsg =      bluez_sock_recvmsg,
 	.poll    =      bluez_sock_poll,
+	.mmap    =      sock_no_mmap,
 	.socketpair =   sock_no_socketpair,
 	.ioctl      =   sock_no_ioctl,
-	.shutdown =	sock_no_shutdown,
+	.shutdown   =   l2cap_sock_shutdown,
 	.setsockopt =   l2cap_sock_setsockopt,
-	.getsockopt =	l2cap_sock_getsockopt,
-	.mmap =		sock_no_mmap
+	.getsockopt =   l2cap_sock_getsockopt
 };
 
 static struct net_proto_family l2cap_sock_family_ops = {
@@ -2002,9 +2072,9 @@ static struct hci_proto l2cap_hci_proto = {
 	.connect_ind =  l2cap_connect_ind,
 	.connect_cfm =  l2cap_connect_cfm,
 	.disconn_ind =  l2cap_disconn_ind,
-	.recv_acldata =	l2cap_recv_acldata,
 	.auth_cfm    =  l2cap_auth_cfm,
-	.encrypt_cfm =	l2cap_encrypt_cfm
+	.encrypt_cfm =  l2cap_encrypt_cfm,
+	.recv_acldata =	l2cap_recv_acldata
 };
 
 int __init l2cap_init(void)

net/bluetooth/lib.c

@@ -105,7 +105,7 @@ int bterr(__u16 code)
 		return EACCES;
 
 	case 0x06:
-		return EINVAL;
+		return EBADE;
 
 	case 0x07:
 		return ENOMEM;

net/bluetooth/sco.c

@@ -27,7 +27,7 @@
  *
  * $Id: sco.c,v 1.3 2002/04/17 17:37:16 maxk Exp $
  */
-#define VERSION "0.2"
+#define VERSION "0.3"
 
 #include <linux/config.h>
 #include <linux/module.h>
@@ -67,16 +67,15 @@ static struct bluez_sock_list sco_sk_list = {
 	.lock = RW_LOCK_UNLOCKED
 };
 
-static inline int sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent);
+static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent);
 static void sco_chan_del(struct sock *sk, int err);
-static inline struct sock * sco_chan_get(struct sco_conn *conn);
 
 static int  sco_conn_del(struct hci_conn *conn, int err);
 
 static void sco_sock_close(struct sock *sk);
 static void sco_sock_kill(struct sock *sk);
 
-/* ----- SCO timers ------ */
+/* ---- SCO timers ---- */
 static void sco_sock_timeout(unsigned long arg)
 {
 	struct sock *sk = (struct sock *) arg;
@@ -115,7 +114,7 @@ static void sco_sock_init_timer(struct sock *sk)
 	sk->timer.data = (unsigned long)sk;
 }
 
-/* -------- SCO connections --------- */
+/* ---- SCO connections ---- */
 static struct sco_conn *sco_conn_add(struct hci_conn *hcon, __u8 status)
 {
 	struct hci_dev *hdev = hcon->hdev;
@@ -150,6 +149,15 @@ static struct sco_conn *sco_conn_add(struct hci_conn *hcon, __u8 status)
 	return conn;
 }
 
+static inline struct sock *sco_chan_get(struct sco_conn *conn)
+{
+	struct sock *sk = NULL;
+	sco_conn_lock(conn);
+	sk = conn->sk;
+	sco_conn_unlock(conn);
+	return sk;
+}
+
 static int sco_conn_del(struct hci_conn *hcon, int err)
 {
 	struct sco_conn *conn;
@@ -176,6 +184,20 @@ static int sco_conn_del(struct hci_conn *hcon, int err)
 	return 0;
 }
 
+static inline int sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent)
+{
+	int err = 0;
+
+	sco_conn_lock(conn);
+	if (conn->sk) {
+		err = -EBUSY;
+	} else {
+		__sco_chan_add(conn, sk, parent);
+	}
+	sco_conn_unlock(conn);
+	return err;
+}
+
 int sco_connect(struct sock *sk)
 {
 	bdaddr_t *src = &bluez_sk(sk)->src;
@@ -462,23 +484,20 @@ static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
 		goto done;
 	}
 
-	write_lock(&sco_sk_list.lock);
+	write_lock_bh(&sco_sk_list.lock);
 	
 	if (bacmp(src, BDADDR_ANY) && __sco_get_sock_by_addr(src)) {
 		err = -EADDRINUSE;
-		goto unlock;
-	}
-
+	} else {
 		/* Save source address */
 		bacpy(&bluez_sk(sk)->src, &sa->sco_bdaddr);
 		sk->state = BT_BOUND;
+	}
 	
-unlock:
-	write_unlock(&sco_sk_list.lock);
+	write_unlock_bh(&sco_sk_list.lock);
 
 done:
 	release_sock(sk);
-
 	return err;
 }
 
@@ -649,7 +668,7 @@ int sco_sock_setsockopt(struct socket *sock, int level, int optname, char *optva
 	default:
 		err = -ENOPROTOOPT;
 		break;
-	};
+	}
 
 	release_sock(sk);
 	return err;
@@ -703,7 +722,7 @@ int sco_sock_getsockopt(struct socket *sock, int level, int optname, char *optva
 	default:
 		err = -ENOPROTOOPT;
 		break;
-	};
+	}
 
 	release_sock(sk);
 	return err;
@@ -735,29 +754,6 @@ static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *
 		bluez_accept_enqueue(parent, sk);
 }
 
-static inline int sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent)
-{
-	int err = 0;
-
-	sco_conn_lock(conn);
-	if (conn->sk) {
-		err = -EBUSY;
-	} else {
-		__sco_chan_add(conn, sk, parent);
-	}
-	sco_conn_unlock(conn);
-	return err;
-}
-
-static inline struct sock * sco_chan_get(struct sco_conn *conn)
-{
-	struct sock *sk = NULL;
-	sco_conn_lock(conn);
-	sk = conn->sk;
-	sco_conn_unlock(conn);
-	return sk;
-}
-
 /* Delete channel. 
  * Must be called on the locked socket. */
 static void sco_chan_del(struct sock *sk, int err)
@@ -895,7 +891,7 @@ static int sco_sock_dump(char *buf, struct bluez_sock_list *list)
 	struct sock *sk;
 	char *ptr = buf;
 
-	write_lock(&list->lock);
+	read_lock_bh(&list->lock);
 
 	for (sk = list->head; sk; sk = sk->next) {
 		pi = sco_pi(sk);
@@ -904,7 +900,7 @@ static int sco_sock_dump(char *buf, struct bluez_sock_list *list)
 				sk->state); 
 	}
 
-	write_unlock(&list->lock);
+	read_unlock_bh(&list->lock);
 
 	ptr += sprintf(ptr, "\n");
 
@@ -946,12 +942,12 @@ static struct proto_ops sco_sock_ops = {
 	.sendmsg =      sco_sock_sendmsg,
 	.recvmsg =      bluez_sock_recvmsg,
 	.poll    =      bluez_sock_poll,
-	.socketpair =	sock_no_socketpair,
 	.ioctl   =      sock_no_ioctl,
+	.mmap    =      sock_no_mmap,
+	.socketpair =   sock_no_socketpair,
 	.shutdown   =   sock_no_shutdown,
 	.setsockopt =   sco_sock_setsockopt,
-	.getsockopt =	sco_sock_getsockopt,
-	.mmap =		sock_no_mmap
+	.getsockopt =   sco_sock_getsockopt
 };
 
 static struct net_proto_family sco_sock_family_ops = {
@@ -965,7 +961,7 @@ static struct hci_proto sco_hci_proto = {
 	.connect_ind =  sco_connect_ind,
 	.connect_cfm =  sco_connect_cfm,
 	.disconn_ind =  sco_disconn_ind,
-	.recv_scodata =	sco_recv_scodata,
+	.recv_scodata =	sco_recv_scodata
 };
 
 int __init sco_init(void)