--- zzzz-none-000/linux-2.4.17/net/bluetooth/hci_core.c	2001-11-09 22:21:21.000000000 +0000
+++ sangam-fb-401/linux-2.4.17/net/bluetooth/hci_core.c	2005-04-05 07:07:46.000000000 +0000
@@ -25,11 +25,12 @@
 /*
  * BlueZ HCI Core.
  *
- * $Id: hci_core.c,v 1.22 2001/08/03 04:19:50 maxk Exp $
+ * $Id: hci_core.c,v 1.14 2002/08/26 16:57:57 maxk Exp $
  */
 
 #include <linux/config.h>
 #include <linux/module.h>
+#include <linux/kmod.h>
 
 #include <linux/types.h>
 #include <linux/errno.h>
@@ -50,12 +51,11 @@
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
-#include <net/bluetooth/bluez.h>
 #include <net/bluetooth/hci_core.h>
 
 #ifndef HCI_CORE_DEBUG
-#undef  DBG
-#define DBG( A... )
+#undef  BT_DBG
+#define BT_DBG( A... )
 #endif
 
 static void hci_cmd_task(unsigned long arg);
@@ -63,279 +63,69 @@
 static void hci_tx_task(unsigned long arg);
 static void hci_notify(struct hci_dev *hdev, int event);
 
-static rwlock_t hci_task_lock = RW_LOCK_UNLOCKED;
+rwlock_t hci_task_lock = RW_LOCK_UNLOCKED;
 
 /* HCI device list */
-struct hci_dev *hdev_list[HCI_MAX_DEV];
-spinlock_t hdev_list_lock;
-#define GET_HDEV(a) (hdev_list[a])
-
-/* HCI protocol list */
-struct hci_proto *hproto_list[HCI_MAX_PROTO];
-#define GET_HPROTO(a) (hproto_list[a])
+LIST_HEAD(hdev_list);
+rwlock_t hdev_list_lock = RW_LOCK_UNLOCKED;
 
-/* HCI notifiers list */
-struct notifier_block *hci_dev_notifier;
-
-/* HCI device notifications */
-int hci_register_notifier(struct notifier_block *nb)
-{
-	int err, i;
-	struct hci_dev *hdev;
-
-	if ((err = notifier_chain_register(&hci_dev_notifier, nb)))
-		return err;
-
-	/* Notify about already registered devices */
-	spin_lock(&hdev_list_lock);
-	for (i = 0; i < HCI_MAX_DEV; i++) {
-		if (!(hdev = GET_HDEV(i)))
-			continue;
-		if (hdev->flags & HCI_UP)
-			(*nb->notifier_call)(nb, HCI_DEV_UP, hdev);
-	}
-	spin_unlock(&hdev_list_lock);
-
-	return 0;
-}
-
-int hci_unregister_notifier(struct notifier_block *nb)
-{
-	return notifier_chain_unregister(&hci_dev_notifier, nb);
-}
-
-static inline void hci_notify(struct hci_dev *hdev, int event)
-{
-	notifier_call_chain(&hci_dev_notifier, event, hdev);
-}
-
-/* Get HCI device by index (device is locked on return)*/
-struct hci_dev *hci_dev_get(int index)
-{
-	struct hci_dev *hdev;
-	DBG("%d", index);
-
-	if (index < 0 || index >= HCI_MAX_DEV)
-		return NULL;
-
-	spin_lock(&hdev_list_lock);
-	if ((hdev = GET_HDEV(index)))
-		hci_dev_hold(hdev);
-	spin_unlock(&hdev_list_lock);
-
-	return hdev;
-}
+/* HCI protocols */
+#define HCI_MAX_PROTO	2
+struct hci_proto *hci_proto[HCI_MAX_PROTO];
 
-/* Flush inquiry cache */
-void inquiry_cache_flush(struct inquiry_cache *cache)
-{
-	struct inquiry_entry *next = cache->list, *e;
-
-	DBG("cache %p", cache);
-
-	cache->list = NULL;
-	while ((e = next)) {
-		next = e->next;
-		kfree(e);
-	}
-}
-
-/* Lookup by bdaddr.
- * Cache must be locked. */
-static struct inquiry_entry * __inquiry_cache_lookup(struct inquiry_cache *cache, bdaddr_t *bdaddr)
-{
-	struct inquiry_entry *e;
-
-	DBG("cache %p, %s", cache, batostr(bdaddr));
-
-	for (e = cache->list; e; e = e->next)
-		if (!bacmp(&e->info.bdaddr, bdaddr))
-			break;
-
-	return e;
-}
-
-static void inquiry_cache_update(struct inquiry_cache *cache, inquiry_info *info)
-{
-	struct inquiry_entry *e;
+/* HCI notifiers list */
+static struct notifier_block *hci_notifier;
 
-	DBG("cache %p, %s", cache, batostr(&info->bdaddr));
 
-	inquiry_cache_lock(cache);
+/* ---- HCI notifications ---- */
 
-	if (!(e = __inquiry_cache_lookup(cache, &info->bdaddr))) {
-		/* Entry not in the cache. Add new one. */
-		if (!(e = kmalloc(sizeof(struct inquiry_entry), GFP_ATOMIC)))
-			goto unlock;
-		memset(e, 0, sizeof(struct inquiry_entry));
-		e->next     = cache->list;
-		cache->list = e;
-	}
-
-	memcpy(&e->info, info, sizeof(inquiry_info));
-	e->timestamp = jiffies;
-	cache->timestamp = jiffies;
-unlock:
-	inquiry_cache_unlock(cache);
-}
-
-static int inquiry_cache_dump(struct inquiry_cache *cache, int num, __u8 *buf)
+int hci_register_notifier(struct notifier_block *nb)
 {
-	inquiry_info *info = (inquiry_info *) buf;
-	struct inquiry_entry *e;
-	int copied = 0;
-
-	inquiry_cache_lock(cache);
-
-	for (e = cache->list; e && copied < num; e = e->next, copied++)
-		memcpy(info++, &e->info, sizeof(inquiry_info));
-
-	inquiry_cache_unlock(cache);
-
-	DBG("cache %p, copied %d", cache, copied);
-	return copied;
+	return notifier_chain_register(&hci_notifier, nb);
 }
 
-/* --------- BaseBand connections --------- */
-static struct hci_conn *hci_conn_add(struct hci_dev *hdev, __u16 handle, __u8 type, bdaddr_t *dst)
+int hci_unregister_notifier(struct notifier_block *nb)
 {
-	struct hci_conn *conn;
-
-	DBG("%s handle %d dst %s", hdev->name, handle, batostr(dst));
-
-	if ( conn_hash_lookup(&hdev->conn_hash, handle)) {
-		ERR("%s handle 0x%x already exists", hdev->name, handle);
-		return NULL;
-	}
-
-	if (!(conn = kmalloc(sizeof(struct hci_conn), GFP_ATOMIC)))
-		return NULL;
-	memset(conn, 0, sizeof(struct hci_conn));
-
-	bacpy(&conn->dst, dst);
-	conn->handle = handle;
-	conn->type   = type;
-	conn->hdev   = hdev;
-
-	skb_queue_head_init(&conn->data_q);
-
-	hci_dev_hold(hdev);
-	conn_hash_add(&hdev->conn_hash, handle, conn);
-
-	return conn;
+	return notifier_chain_unregister(&hci_notifier, nb);
 }
 
-static int hci_conn_del(struct hci_dev *hdev, struct hci_conn *conn)
+void hci_notify(struct hci_dev *hdev, int event)
 {
-	DBG("%s conn %p handle %d", hdev->name, conn, conn->handle);
-
-	conn_hash_del(&hdev->conn_hash, conn);
-	hci_dev_put(hdev);
-
-	/* Unacked frames */
-	hdev->acl_cnt += conn->sent;
-
-	skb_queue_purge(&conn->data_q);
-
-	kfree(conn);
-	return 0;
+	notifier_call_chain(&hci_notifier, event, hdev);
 }
 
-/* Drop all connection on the device */
-static void hci_conn_hash_flush(struct hci_dev *hdev)
-{
-	struct conn_hash *h = &hdev->conn_hash;
-	struct hci_proto *hp;
-        struct list_head *p;
-
-	DBG("hdev %s", hdev->name);
+/* ---- HCI hotplug support ---- */
 
-	p = h->list.next;
-	while (p != &h->list) {
-		struct hci_conn *c;
-
-		c = list_entry(p, struct hci_conn, list);
-		p = p->next;
-
-		if (c->type == ACL_LINK) {
-			/* ACL link notify L2CAP layer */
-			if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->disconn_ind)
-				hp->disconn_ind(c, 0x16);
-		} else {
-			/* SCO link (no notification) */
-		}
-
-		hci_conn_del(hdev, c);
-	}
-}
+#ifdef CONFIG_HOTPLUG
 
-int hci_connect(struct hci_dev *hdev, bdaddr_t *bdaddr)
+static int hci_run_hotplug(char *dev, char *action)
 {
-	struct inquiry_cache *cache = &hdev->inq_cache;
-	struct inquiry_entry *e;
-	create_conn_cp cc;
-	__u16 clock_offset;
+	char *argv[3], *envp[5], dstr[20], astr[32];
 
-	DBG("%s bdaddr %s", hdev->name, batostr(bdaddr));
+	sprintf(dstr, "DEVICE=%s", dev);
+	sprintf(astr, "ACTION=%s", action);
 
-	if (!(hdev->flags & HCI_UP))
-		return -ENODEV;
-
-	inquiry_cache_lock_bh(cache);
+        argv[0] = hotplug_path;
+        argv[1] = "bluetooth";
+        argv[2] = NULL;
 
-	if (!(e = __inquiry_cache_lookup(cache, bdaddr)) || inquiry_entry_age(e) > INQUIRY_ENTRY_AGE_MAX) {
-		cc.pscan_rep_mode = 0;
-		cc.pscan_mode     = 0;
-		clock_offset      = 0;
-	} else {
-		cc.pscan_rep_mode = e->info.pscan_rep_mode;
-		cc.pscan_mode     = e->info.pscan_mode;
-		clock_offset      = __le16_to_cpu(e->info.clock_offset) & 0x8000;
-	}
-
-	inquiry_cache_unlock_bh(cache);
-
-	bacpy(&cc.bdaddr, bdaddr);
-	cc.pkt_type	= __cpu_to_le16(hdev->pkt_type);
-	cc.clock_offset	= __cpu_to_le16(clock_offset);
-
-	if (lmp_rswitch_capable(hdev))
-		cc.role_switch	= 0x01;
-	else
-		cc.role_switch	= 0x00;
-		
-	hci_send_cmd(hdev, OGF_LINK_CTL, OCF_CREATE_CONN, CREATE_CONN_CP_SIZE, &cc);
-
-	return 0;
-}
-
-int hci_disconnect(struct hci_conn *conn, __u8 reason)
-{
-	disconnect_cp dc;
-
-	DBG("conn %p handle %d", conn, conn->handle);
-
-	dc.handle = __cpu_to_le16(conn->handle);
-	dc.reason = reason;
-	hci_send_cmd(conn->hdev, OGF_LINK_CTL, OCF_DISCONNECT, DISCONNECT_CP_SIZE, &dc);
-
-	return 0;
-}
-
-/* --------- HCI request handling ------------ */
-static inline void hci_req_lock(struct hci_dev *hdev)
-{
-	down(&hdev->req_lock);
+	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
 
-static inline void hci_req_unlock(struct hci_dev *hdev)
-{
-	up(&hdev->req_lock);
-}
+/* ---- HCI requests ---- */
 
-static inline void hci_req_complete(struct hci_dev *hdev, int result)
+void hci_req_complete(struct hci_dev *hdev, int result)
 {
-	DBG("%s result 0x%2.2x", hdev->name, result);
+	BT_DBG("%s result 0x%2.2x", hdev->name, result);
 
 	if (hdev->req_status == HCI_REQ_PEND) {
 		hdev->req_result = result;
@@ -344,9 +134,9 @@
 	}
 }
 
-static inline void hci_req_cancel(struct hci_dev *hdev, int err)
+void hci_req_cancel(struct hci_dev *hdev, int err)
 {
-	DBG("%s err 0x%2.2x", hdev->name, err);
+	BT_DBG("%s err 0x%2.2x", hdev->name, err);
 
 	if (hdev->req_status == HCI_REQ_PEND) {
 		hdev->req_result = err;
@@ -356,23 +146,22 @@
 }
 
 /* Execute request and wait for completion. */
-static int __hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev, unsigned long opt),
-                         unsigned long opt, __u32 timeout)
+static int __hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev, unsigned long opt), unsigned long opt, __u32 timeout)
 {
 	DECLARE_WAITQUEUE(wait, current);
 	int err = 0;
 
-	DBG("%s start", hdev->name);
+	BT_DBG("%s start", hdev->name);
 
 	hdev->req_status = HCI_REQ_PEND;
 
 	add_wait_queue(&hdev->req_wait_q, &wait);
-	current->state = TASK_INTERRUPTIBLE;
+	set_current_state(TASK_INTERRUPTIBLE);
 
 	req(hdev, opt);
 	schedule_timeout(timeout);
 
-	current->state = TASK_RUNNING;
+	set_current_state(TASK_RUNNING);
 	remove_wait_queue(&hdev->req_wait_q, &wait);
 
 	if (signal_pending(current))
@@ -394,7 +183,7 @@
 
 	hdev->req_status = hdev->req_result = 0;
 
-	DBG("%s end: err %d", hdev->name, err);
+	BT_DBG("%s end: err %d", hdev->name, err);
 
 	return err;
 }
@@ -412,10 +201,9 @@
 	return ret;
 }
 
-/* --------- HCI requests ---------- */
 static void hci_reset_req(struct hci_dev *hdev, unsigned long opt)
 {
-	DBG("%s %ld", hdev->name, opt);
+	BT_DBG("%s %ld", hdev->name, opt);
 
 	/* Reset device */
 	hci_send_cmd(hdev, OGF_HOST_CTL, OCF_RESET, 0, NULL);
@@ -423,27 +211,44 @@
 
 static void hci_init_req(struct hci_dev *hdev, unsigned long opt)
 {
-	set_event_flt_cp ec;
+	set_event_flt_cp ef;
 	__u16 param;
 
-	DBG("%s %ld", hdev->name, opt);
+	BT_DBG("%s %ld", hdev->name, opt);
 
 	/* Mandatory initialization */
 
+	/* Reset */
+	if (test_bit(HCI_QUIRK_RESET_ON_INIT, &hdev->quirks))
+		hci_send_cmd(hdev, OGF_HOST_CTL, OCF_RESET, 0, NULL);
+
 	/* Read Local Supported Features */
 	hci_send_cmd(hdev, OGF_INFO_PARAM, OCF_READ_LOCAL_FEATURES, 0, NULL);
 
 	/* Read Buffer Size (ACL mtu, max pkt, etc.) */
 	hci_send_cmd(hdev, OGF_INFO_PARAM, OCF_READ_BUFFER_SIZE, 0, NULL);
 
+#if 0
+	/* Host buffer size */
+	{
+		host_buffer_size_cp bs;
+		bs.acl_mtu = __cpu_to_le16(HCI_MAX_ACL_SIZE);
+		bs.sco_mtu = HCI_MAX_SCO_SIZE;
+		bs.acl_max_pkt = __cpu_to_le16(0xffff);
+		bs.sco_max_pkt = __cpu_to_le16(0xffff);
+		hci_send_cmd(hdev, OGF_HOST_CTL, OCF_HOST_BUFFER_SIZE,
+				HOST_BUFFER_SIZE_CP_SIZE, &bs);
+	}
+#endif
+
 	/* Read BD Address */
 	hci_send_cmd(hdev, OGF_INFO_PARAM, OCF_READ_BD_ADDR, 0, NULL);
 
 	/* Optional initialization */
 
 	/* Clear Event Filters */
-	ec.flt_type  = FLT_CLEAR_ALL;
-	hci_send_cmd(hdev, OGF_HOST_CTL, OCF_SET_EVENT_FLT, 1, &ec);
+	ef.flt_type  = FLT_CLEAR_ALL;
+	hci_send_cmd(hdev, OGF_HOST_CTL, OCF_SET_EVENT_FLT, 1, &ef);
 
 	/* Page timeout ~20 secs */
 	param = __cpu_to_le16(0x8000);
@@ -458,7 +263,7 @@
 {
 	__u8 scan = opt;
 
-	DBG("%s %x", hdev->name, scan);
+	BT_DBG("%s %x", hdev->name, scan);
 
 	/* Inquiry and Page scans */
 	hci_send_cmd(hdev, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE, 1, &scan);
@@ -468,27 +273,190 @@
 {
 	__u8 auth = opt;
 
-	DBG("%s %x", hdev->name, auth);
+	BT_DBG("%s %x", hdev->name, auth);
 
 	/* Authentication */
 	hci_send_cmd(hdev, OGF_HOST_CTL, OCF_WRITE_AUTH_ENABLE, 1, &auth);
 }
 
+static void hci_encrypt_req(struct hci_dev *hdev, unsigned long opt)
+{
+	__u8 encrypt = opt;
+
+	BT_DBG("%s %x", hdev->name, encrypt);
+
+	/* Authentication */
+	hci_send_cmd(hdev, OGF_HOST_CTL, OCF_WRITE_ENCRYPT_MODE, 1, &encrypt);
+}
+
+/* Get HCI device by index. 
+ * Device is locked on return. */
+struct hci_dev *hci_dev_get(int index)
+{
+	struct hci_dev *hdev;
+	struct list_head *p;
+
+	BT_DBG("%d", index);
+
+	if (index < 0)
+		return NULL;
+
+	read_lock(&hdev_list_lock);
+	list_for_each(p, &hdev_list) {
+		hdev = list_entry(p, struct hci_dev, list);
+		if (hdev->id == index) {
+			hci_dev_hold(hdev);
+			goto done;
+		}
+	}
+	hdev = NULL;
+done:
+	read_unlock(&hdev_list_lock);
+	return hdev;
+}
+
+/* ---- Inquiry support ---- */
+void inquiry_cache_flush(struct hci_dev *hdev)
+{
+	struct inquiry_cache *cache = &hdev->inq_cache;
+	struct inquiry_entry *next  = cache->list, *e;
+
+	BT_DBG("cache %p", cache);
+
+	cache->list = NULL;
+	while ((e = next)) {
+		next = e->next;
+		kfree(e);
+	}
+}
+
+struct inquiry_entry *inquiry_cache_lookup(struct hci_dev *hdev, bdaddr_t *bdaddr)
+{
+	struct inquiry_cache *cache = &hdev->inq_cache;
+	struct inquiry_entry *e;
+
+	BT_DBG("cache %p, %s", cache, batostr(bdaddr));
+
+	for (e = cache->list; e; e = e->next)
+		if (!bacmp(&e->info.bdaddr, bdaddr))
+			break;
+	return e;
+}
+
+void inquiry_cache_update(struct hci_dev *hdev, inquiry_info *info)
+{
+	struct inquiry_cache *cache = &hdev->inq_cache;
+	struct inquiry_entry *e;
+
+	BT_DBG("cache %p, %s", cache, batostr(&info->bdaddr));
+
+	if (!(e = inquiry_cache_lookup(hdev, &info->bdaddr))) {
+		/* Entry not in the cache. Add new one. */
+		if (!(e = kmalloc(sizeof(struct inquiry_entry), GFP_ATOMIC)))
+			return;
+		memset(e, 0, sizeof(struct inquiry_entry));
+		e->next     = cache->list;
+		cache->list = e;
+	}
+
+	memcpy(&e->info, info, sizeof(inquiry_info));
+	e->timestamp = jiffies;
+	cache->timestamp = jiffies;
+}
+
+int inquiry_cache_dump(struct hci_dev *hdev, int num, __u8 *buf)
+{
+	struct inquiry_cache *cache = &hdev->inq_cache;
+	inquiry_info *info = (inquiry_info *) buf;
+	struct inquiry_entry *e;
+	int copied = 0;
+
+	for (e = cache->list; e && copied < num; e = e->next, copied++)
+		memcpy(info++, &e->info, sizeof(inquiry_info));
+
+	BT_DBG("cache %p, copied %d", cache, copied);
+	return copied;
+}
+
 static void hci_inq_req(struct hci_dev *hdev, unsigned long opt)
 {
 	struct hci_inquiry_req *ir = (struct hci_inquiry_req *) opt;
 	inquiry_cp ic;
 
-	DBG("%s", hdev->name);
+	BT_DBG("%s", hdev->name);
+
+	if (test_bit(HCI_INQUIRY, &hdev->flags))
+		return;
 
 	/* Start Inquiry */
 	memcpy(&ic.lap, &ir->lap, 3);
-	ic.lenght  = ir->length;
+	ic.length  = ir->length;
 	ic.num_rsp = ir->num_rsp;
 	hci_send_cmd(hdev, OGF_LINK_CTL, OCF_INQUIRY, INQUIRY_CP_SIZE, &ic);
 }
 
-/* HCI ioctl helpers */
+int hci_inquiry(unsigned long arg)
+{
+	struct hci_inquiry_req ir;
+	struct hci_dev *hdev;
+	int err = 0, do_inquiry = 0, max_rsp;
+	long timeo;
+	__u8 *buf, *ptr;
+
+	ptr = (void *) arg;
+	if (copy_from_user(&ir, ptr, sizeof(ir)))
+		return -EFAULT;
+
+	if (!(hdev = hci_dev_get(ir.dev_id)))
+		return -ENODEV;
+
+	hci_dev_lock_bh(hdev);
+	if (inquiry_cache_age(hdev) > INQUIRY_CACHE_AGE_MAX || 
+					inquiry_cache_empty(hdev) ||
+					ir.flags & IREQ_CACHE_FLUSH) {
+		inquiry_cache_flush(hdev);
+		do_inquiry = 1;
+	}
+	hci_dev_unlock_bh(hdev);
+
+	timeo = ir.length * 2 * HZ;
+	if (do_inquiry && (err = hci_request(hdev, hci_inq_req, (unsigned long)&ir, timeo)) < 0)
+		goto done;
+
+	/* for unlimited number of responses we will use buffer with 255 entries */
+	max_rsp = (ir.num_rsp == 0) ? 255 : ir.num_rsp;
+
+	/* cache_dump can't sleep. Therefore we allocate temp buffer and then
+	 * copy it to the user space.
+	 */
+	if (!(buf = kmalloc(sizeof(inquiry_info) * max_rsp, GFP_KERNEL))) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	hci_dev_lock_bh(hdev);
+	ir.num_rsp = inquiry_cache_dump(hdev, max_rsp, buf);
+	hci_dev_unlock_bh(hdev);
+
+	BT_DBG("num_rsp %d", ir.num_rsp);
+
+	if (!verify_area(VERIFY_WRITE, ptr, sizeof(ir) + 
+				(sizeof(inquiry_info) * ir.num_rsp))) {
+		copy_to_user(ptr, &ir, sizeof(ir));
+		ptr += sizeof(ir);
+	        copy_to_user(ptr, buf, sizeof(inquiry_info) * ir.num_rsp);
+	} else 
+		err = -EFAULT;
+
+	kfree(buf);
+
+done:
+	hci_dev_put(hdev);
+	return err;
+}
+
+/* ---- HCI ioctl helpers ---- */
+
 int hci_dev_open(__u16 dev)
 {
 	struct hci_dev *hdev;
@@ -497,11 +465,11 @@
 	if (!(hdev = hci_dev_get(dev)))
 		return -ENODEV;
 
-	DBG("%s %p", hdev->name, hdev);
+	BT_DBG("%s %p", hdev->name, hdev);
 
 	hci_req_lock(hdev);
 
-	if (hdev->flags & HCI_UP) {
+	if (test_bit(HCI_UP, &hdev->flags)) {
 		ret = -EALREADY;
 		goto done;
 	}
@@ -511,18 +479,18 @@
 		goto done;
 	}
 
-	if (hdev->flags & HCI_NORMAL) {
+	if (!test_bit(HCI_RAW, &hdev->flags)) {
 		atomic_set(&hdev->cmd_cnt, 1);
-		hdev->flags |= HCI_INIT;
+		set_bit(HCI_INIT, &hdev->flags);
 
 		//__hci_request(hdev, hci_reset_req, 0, HZ);
 		ret = __hci_request(hdev, hci_init_req, 0, HCI_INIT_TIMEOUT);
        
-		hdev->flags &= ~HCI_INIT;
+		clear_bit(HCI_INIT, &hdev->flags);
 	}
 
 	if (!ret) {
-		hdev->flags |= HCI_UP;
+		set_bit(HCI_UP, &hdev->flags);
 		hci_notify(hdev, HCI_DEV_UP);
 	} else {	
 		/* Init failed, cleanup */
@@ -542,42 +510,36 @@
 		}
 
 		hdev->close(hdev);
+		hdev->flags = 0;
 	}
 
 done:
 	hci_req_unlock(hdev);
 	hci_dev_put(hdev);
-
 	return ret;
 }
 
-int hci_dev_close(__u16 dev)
+static int hci_dev_do_close(struct hci_dev *hdev)
 {
-	struct hci_dev *hdev;
-
-	if (!(hdev = hci_dev_get(dev)))
-		return -ENODEV;
-
-	DBG("%s %p", hdev->name, hdev);
+	BT_DBG("%s %p", hdev->name, hdev);
 
 	hci_req_cancel(hdev, ENODEV);
 	hci_req_lock(hdev);
 
-	if (!(hdev->flags & HCI_UP))
-		goto done;
+	if (!test_and_clear_bit(HCI_UP, &hdev->flags)) {
+		hci_req_unlock(hdev);
+		return 0;
+	}
 
 	/* Kill RX and TX tasks */
 	tasklet_kill(&hdev->rx_task);
 	tasklet_kill(&hdev->tx_task);
 
-	inquiry_cache_flush(&hdev->inq_cache);
-
+	hci_dev_lock_bh(hdev);
+	inquiry_cache_flush(hdev);
 	hci_conn_hash_flush(hdev);
-
-	/* Clear flags */
-	hdev->flags &= HCI_SOCK;
-	hdev->flags |= HCI_NORMAL;
-
+	hci_dev_unlock_bh(hdev);
+	
 	hci_notify(hdev, HCI_DEV_DOWN);
 
 	if (hdev->flush)
@@ -586,9 +548,9 @@
 	/* Reset device */
 	skb_queue_purge(&hdev->cmd_q);
 	atomic_set(&hdev->cmd_cnt, 1);
-	hdev->flags |= HCI_INIT;
-	__hci_request(hdev, hci_reset_req, 0, HZ);
-	hdev->flags &= ~HCI_INIT;
+	set_bit(HCI_INIT, &hdev->flags);
+	__hci_request(hdev, hci_reset_req, 0, HZ/4);
+	clear_bit(HCI_INIT, &hdev->flags);
 
 	/* Kill cmd task */
 	tasklet_kill(&hdev->cmd_task);
@@ -605,17 +567,28 @@
 	}
 
 	/* After this point our queues are empty
-	 * and no tasks are scheduled.
-	 */
+	 * and no tasks are scheduled. */
 	hdev->close(hdev);
 
-done:
-	hci_req_unlock(hdev);
-	hci_dev_put(hdev);
+	/* Clear flags */
+	hdev->flags = 0;
 
+	hci_req_unlock(hdev);
 	return 0;
 }
 
+int hci_dev_close(__u16 dev)
+{
+	struct hci_dev *hdev;
+	int err;
+	
+	if (!(hdev = hci_dev_get(dev)))
+		return -ENODEV;
+	err = hci_dev_do_close(hdev);
+	hci_dev_put(hdev);
+	return err;
+}
+
 int hci_dev_reset(__u16 dev)
 {
 	struct hci_dev *hdev;
@@ -627,16 +600,17 @@
 	hci_req_lock(hdev);
 	tasklet_disable(&hdev->tx_task);
 
-	if (!(hdev->flags & HCI_UP))
+	if (!test_bit(HCI_UP, &hdev->flags))
 		goto done;
 
 	/* Drop queues */
 	skb_queue_purge(&hdev->rx_q);
 	skb_queue_purge(&hdev->cmd_q);
 
-	inquiry_cache_flush(&hdev->inq_cache);
-
+	hci_dev_lock_bh(hdev);
+	inquiry_cache_flush(hdev);
 	hci_conn_hash_flush(hdev);
+	hci_dev_unlock_bh(hdev);
 
 	if (hdev->flush)
 		hdev->flush(hdev);
@@ -650,7 +624,6 @@
 	tasklet_enable(&hdev->tx_task);
 	hci_req_unlock(hdev);
 	hci_dev_put(hdev);
-
 	return ret;
 }
 
@@ -669,30 +642,11 @@
 	return ret;
 }
 
-int hci_dev_setauth(unsigned long arg)
-{
-	struct hci_dev *hdev;
-	struct hci_dev_req dr;
-	int ret = 0;
-
-	if (copy_from_user(&dr, (void *) arg, sizeof(dr)))
-		return -EFAULT;
-
-	if (!(hdev = hci_dev_get(dr.dev_id)))
-		return -ENODEV;
-
-	ret = hci_request(hdev, hci_auth_req, dr.dev_opt, HCI_INIT_TIMEOUT);
-
-	hci_dev_put(hdev);
-
-	return ret;
-}
-
-int hci_dev_setscan(unsigned long arg)
+int hci_dev_cmd(unsigned int cmd, unsigned long arg)
 {
 	struct hci_dev *hdev;
 	struct hci_dev_req dr;
-	int ret = 0;
+	int err = 0;
 
 	if (copy_from_user(&dr, (void *) arg, sizeof(dr)))
 		return -EFAULT;
@@ -700,75 +654,105 @@
 	if (!(hdev = hci_dev_get(dr.dev_id)))
 		return -ENODEV;
 
-	ret = hci_request(hdev, hci_scan_req, dr.dev_opt, HCI_INIT_TIMEOUT);
-
-	hci_dev_put(hdev);
+	switch (cmd) {
+	case HCISETAUTH:
+		err = hci_request(hdev, hci_auth_req, dr.dev_opt, HCI_INIT_TIMEOUT);
+		break;
 
-	return ret;
-}
+	case HCISETENCRYPT:
+		if (!lmp_encrypt_capable(hdev)) {
+			err = -EOPNOTSUPP;
+			break;
+		}
 
-int hci_dev_setptype(unsigned long arg)
-{
-	struct hci_dev *hdev;
-	struct hci_dev_req dr;
-	int ret = 0;
+		if (!test_bit(HCI_AUTH, &hdev->flags)) {
+			/* Auth must be enabled first */
+			err = hci_request(hdev, hci_auth_req,
+					dr.dev_opt, HCI_INIT_TIMEOUT);
+			if (err)
+				break;
+		}
+			
+		err = hci_request(hdev, hci_encrypt_req,
+					dr.dev_opt, HCI_INIT_TIMEOUT);
+		break;
+	
+	case HCISETSCAN:
+		err = hci_request(hdev, hci_scan_req, dr.dev_opt, HCI_INIT_TIMEOUT);
+		break;
+	
+	case HCISETPTYPE:
+		hdev->pkt_type = (__u16) dr.dev_opt;
+		break;
+		
+	case HCISETLINKPOL:
+		hdev->link_policy = (__u16) dr.dev_opt;
+		break;
 
-	if (copy_from_user(&dr, (void *) arg, sizeof(dr)))
-		return -EFAULT;
+	case HCISETLINKMODE:
+		hdev->link_mode = ((__u16) dr.dev_opt) & (HCI_LM_MASTER | HCI_LM_ACCEPT);
+		break;
 
-	if (!(hdev = hci_dev_get(dr.dev_id)))
-		return -ENODEV;
+	case HCISETACLMTU:
+		hdev->acl_mtu  = *((__u16 *)&dr.dev_opt + 1);
+		hdev->acl_pkts = *((__u16 *)&dr.dev_opt + 0);
+		break;
 
-	hdev->pkt_type = (__u16) dr.dev_opt;
+	case HCISETSCOMTU:
+		hdev->sco_mtu  = *((__u16 *)&dr.dev_opt + 1);
+		hdev->sco_pkts = *((__u16 *)&dr.dev_opt + 0);
+		break;
 
+	default:
+		err = -EINVAL;
+		break;
+	}	
 	hci_dev_put(hdev);
-
-	return ret;
+	return err;
 }
 
-int hci_dev_list(unsigned long arg)
+int hci_get_dev_list(unsigned long arg)
 {
 	struct hci_dev_list_req *dl;
 	struct hci_dev_req *dr;
-	struct hci_dev *hdev;
-	int i, n, size;
+	struct list_head *p;
+	int n = 0, size, err;
 	__u16 dev_num;
 
 	if (get_user(dev_num, (__u16 *) arg))
 		return -EFAULT;
 
-	/* Avoid long loop, overflow */
-	if (dev_num > 2048)
+	if (!dev_num || dev_num > (PAGE_SIZE * 2) / sizeof(*dr))
 		return -EINVAL;
-	
-	size = dev_num * sizeof(struct hci_dev_req) + sizeof(__u16);
 
-	if (verify_area(VERIFY_WRITE, (void *) arg, size))
-		return -EFAULT;
+	size = sizeof(*dl) + dev_num * sizeof(*dr);
 
 	if (!(dl = kmalloc(size, GFP_KERNEL)))
 		return -ENOMEM;
+
 	dr = dl->dev_req;
 
-	spin_lock_bh(&hdev_list_lock);
-	for (i = 0, n = 0; i < HCI_MAX_DEV && n < dev_num; i++) {
-		if ((hdev = hdev_list[i])) {
-			(dr + n)->dev_id  = hdev->id;
-			(dr + n)->dev_opt = hdev->flags;
-			n++;
-		}
+	read_lock_bh(&hdev_list_lock);
+	list_for_each(p, &hdev_list) {
+		struct hci_dev *hdev;
+		hdev = list_entry(p, struct hci_dev, list);
+		(dr + n)->dev_id  = hdev->id;
+		(dr + n)->dev_opt = hdev->flags;
+		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);
+	size = sizeof(*dl) + n * sizeof(*dr);
 
-	copy_to_user((void *) arg, dl, size);
+	err = copy_to_user((void *) arg, dl, size);
+	kfree(dl);
 
-	return 0;
+	return err ? -EFAULT : 0;
 }
 
-int hci_dev_info(unsigned long arg)
+int hci_get_dev_info(unsigned long arg)
 {
 	struct hci_dev *hdev;
 	struct hci_dev_info di;
@@ -786,9 +770,11 @@
 	di.flags    = hdev->flags;
 	di.pkt_type = hdev->pkt_type;
 	di.acl_mtu  = hdev->acl_mtu;
-	di.acl_max  = hdev->acl_max;
+	di.acl_pkts = hdev->acl_pkts;
 	di.sco_mtu  = hdev->sco_mtu;
-	di.sco_max  = hdev->sco_max;
+	di.sco_pkts = hdev->sco_pkts;
+	di.link_policy = hdev->link_policy;
+	di.link_mode   = hdev->link_mode;
 
 	memcpy(&di.stat, &hdev->stat, sizeof(di.stat));
 	memcpy(&di.features, &hdev->features, sizeof(di.features));
@@ -801,258 +787,168 @@
 	return err;
 }
 
-__u32 hci_dev_setmode(struct hci_dev *hdev, __u32 mode)
-{
-	__u32 omode = hdev->flags & HCI_MODE_MASK;
-
-	hdev->flags &= ~HCI_MODE_MASK;
-	hdev->flags |= (mode & HCI_MODE_MASK);
 
-	return omode;
-}
+/* ---- Interface to HCI drivers ---- */
 
-__u32 hci_dev_getmode(struct hci_dev *hdev)
+/* Register HCI device */
+int hci_register_dev(struct hci_dev *hdev)
 {
-	return hdev->flags & HCI_MODE_MASK;
-}
+	struct list_head *head = &hdev_list, *p;
+	int id = 0;
 
-int hci_conn_list(unsigned long arg)
-{
-	struct hci_conn_list_req req, *cl;
-	struct hci_conn_info *ci;
-	struct hci_dev *hdev;
-	struct list_head *p;
-	int n = 0, size;
+	BT_DBG("%p name %s type %d", hdev, hdev->name, hdev->type);
 
-	if (copy_from_user(&req, (void *) arg, sizeof(req)))
-		return -EFAULT;
+	if (!hdev->open || !hdev->close || !hdev->destruct)
+		return -EINVAL;
 
-	if (!(hdev = hci_dev_get(req.dev_id)))
-		return -ENODEV;
+	write_lock_bh(&hdev_list_lock);
 
-	/* Set a limit to avoid overlong loops, and also numeric overflow - AC */
-	if(req.conn_num < 2048)
-		return -EINVAL;
+	/* Find first available device id */
+	list_for_each(p, &hdev_list) {
+	       	if (list_entry(p, struct hci_dev, list)->id != id)
+			break;
+		head = p; id++;
+	}
 	
-	size = req.conn_num * sizeof(struct hci_conn_info) + sizeof(req);
+	sprintf(hdev->name, "hci%d", id);
+	hdev->id = id;
+	list_add(&hdev->list, head);
 
-	if (!(cl = kmalloc(size, GFP_KERNEL)))
-		return -ENOMEM;
-	ci = cl->conn_info;
+	atomic_set(&hdev->refcnt, 1);
+	spin_lock_init(&hdev->lock);
+			
+	hdev->flags = 0;
+	hdev->pkt_type  = (HCI_DM1 | HCI_DH1 | HCI_HV1);
+	hdev->link_mode = (HCI_LM_ACCEPT);
 
-	local_bh_disable();
-	conn_hash_lock(&hdev->conn_hash);
-	list_for_each(p, &hdev->conn_hash.list) {
-		register struct hci_conn *c;
-		c = list_entry(p, struct hci_conn, list);
+	tasklet_init(&hdev->cmd_task, hci_cmd_task,(unsigned long) hdev);
+	tasklet_init(&hdev->rx_task, hci_rx_task, (unsigned long) hdev);
+	tasklet_init(&hdev->tx_task, hci_tx_task, (unsigned long) hdev);
 
-		(ci + n)->handle = c->handle;
-		bacpy(&(ci + n)->bdaddr, &c->dst);
-		n++;
-	}
-	conn_hash_unlock(&hdev->conn_hash);
-	local_bh_enable();
-
-	cl->dev_id = hdev->id;
-	cl->conn_num = n;
-	size = n * sizeof(struct hci_conn_info) + sizeof(req);
+	skb_queue_head_init(&hdev->rx_q);
+	skb_queue_head_init(&hdev->cmd_q);
+	skb_queue_head_init(&hdev->raw_q);
 
-	hci_dev_put(hdev);
-
-	if(copy_to_user((void *) arg, cl, size))
-		return -EFAULT;
-	return 0;
-}
+	init_waitqueue_head(&hdev->req_wait_q);
+	init_MUTEX(&hdev->req_lock);
 
-int hci_inquiry(unsigned long arg)
-{
-	struct inquiry_cache *cache;
-	struct hci_inquiry_req ir;
-	struct hci_dev *hdev;
-	int err = 0, do_inquiry = 0;
-	long timeo;
-	__u8 *buf, *ptr;
+	inquiry_cache_init(hdev);
 
-	ptr = (void *) arg;
-	if (copy_from_user(&ir, ptr, sizeof(ir)))
-		return -EFAULT;
+	conn_hash_init(hdev);
 
-	if (!(hdev = hci_dev_get(ir.dev_id)))
-		return -ENODEV;
+	memset(&hdev->stat, 0, sizeof(struct hci_dev_stats));
 
-	cache = &hdev->inq_cache;
+	atomic_set(&hdev->promisc, 0);
 
-	inquiry_cache_lock(cache);
-	if (inquiry_cache_age(cache) > INQUIRY_CACHE_AGE_MAX || ir.flags & IREQ_CACHE_FLUSH) {
-		inquiry_cache_flush(cache);
-		do_inquiry = 1;
-	}
-	inquiry_cache_unlock(cache);
+	MOD_INC_USE_COUNT;
 
-	/* Limit inquiry time, also avoid overflows */
+	write_unlock_bh(&hdev_list_lock);
 
-	if(ir.length > 2048 || ir.num_rsp > 2048)
-	{
-		err = -EINVAL;
-		goto done;
-	}
+	hci_notify(hdev, HCI_DEV_REG);
+	hci_run_hotplug(hdev->name, "register");
 
-	timeo = ir.length * 2 * HZ;
-	if (do_inquiry && (err = hci_request(hdev, hci_inq_req, (unsigned long)&ir, timeo)) < 0)
-		goto done;
+	return id;
+}
 
-	/* cache_dump can't sleep. Therefore we allocate temp buffer and then
-	 * copy it to the user space.
-	 */
-	if (!(buf = kmalloc(sizeof(inquiry_info) * ir.num_rsp, GFP_KERNEL))) {
-		err = -ENOMEM;
-		goto done;
-	}
-	ir.num_rsp = inquiry_cache_dump(cache, ir.num_rsp, buf);
+/* Unregister HCI device */
+int hci_unregister_dev(struct hci_dev *hdev)
+{
+	BT_DBG("%p name %s type %d", hdev, hdev->name, hdev->type);
 
-	DBG("num_rsp %d", ir.num_rsp);
+	write_lock_bh(&hdev_list_lock);
+	list_del(&hdev->list);
+	write_unlock_bh(&hdev_list_lock);
 
-	if (!verify_area(VERIFY_WRITE, ptr, sizeof(ir) + (sizeof(inquiry_info) * ir.num_rsp))) {
-		copy_to_user(ptr, &ir, sizeof(ir));
-		ptr += sizeof(ir);
-	        copy_to_user(ptr, buf, sizeof(inquiry_info) * ir.num_rsp);
-	} else 
-		err = -EFAULT;
+	hci_dev_do_close(hdev);
 
-	kfree(buf);
+	hci_notify(hdev, HCI_DEV_UNREG);
+	hci_run_hotplug(hdev->name, "unregister");
 
-done:
 	hci_dev_put(hdev);
 
-	return err;
+	MOD_DEC_USE_COUNT;
+	return 0;
 }
 
-/* Interface to HCI drivers */
-
-/* Register HCI device */
-int hci_register_dev(struct hci_dev *hdev)
+/* Suspend HCI device */
+int hci_suspend_dev(struct hci_dev *hdev)
 {
-	int i;
-
-	DBG("%p name %s type %d", hdev, hdev->name, hdev->type);
-
-	/* Find free slot */
-	spin_lock_bh(&hdev_list_lock);
-	for (i = 0; i < HCI_MAX_DEV; i++) {
-		if (!hdev_list[i]) {
-			hdev_list[i] = hdev;
-
-			sprintf(hdev->name, "hci%d", i);
-			atomic_set(&hdev->refcnt, 0);
-			hdev->id    = i;
-			hdev->flags = HCI_NORMAL;
-
-			hdev->pkt_type = (HCI_DM1 | HCI_DH1);
-
-			tasklet_init(&hdev->cmd_task, hci_cmd_task, (unsigned long) hdev);
-			tasklet_init(&hdev->rx_task, hci_rx_task, (unsigned long) hdev);
-			tasklet_init(&hdev->tx_task, hci_tx_task, (unsigned long) hdev);
-
-			skb_queue_head_init(&hdev->rx_q);
-			skb_queue_head_init(&hdev->cmd_q);
-			skb_queue_head_init(&hdev->raw_q);
-
-			init_waitqueue_head(&hdev->req_wait_q);
-			init_MUTEX(&hdev->req_lock);
-
-			inquiry_cache_init(&hdev->inq_cache);
-
-			conn_hash_init(&hdev->conn_hash);
-
-			memset(&hdev->stat, 0, sizeof(struct hci_dev_stats));
-
-			hci_notify(hdev, HCI_DEV_REG);
-
-			MOD_INC_USE_COUNT;
-			break;
-		}
-	}
-	spin_unlock_bh(&hdev_list_lock);
-
-	return (i == HCI_MAX_DEV) ? -1 : i;
+	hci_notify(hdev, HCI_DEV_SUSPEND);
+	hci_run_hotplug(hdev->name, "suspend");
+	return 0;
 }
 
-/* Unregister HCI device */
-int hci_unregister_dev(struct hci_dev *hdev)
+/* Resume HCI device */
+int hci_resume_dev(struct hci_dev *hdev)
 {
-	int i;
-
-	DBG("%p name %s type %d", hdev, hdev->name, hdev->type);
+	hci_notify(hdev, HCI_DEV_RESUME);
+	hci_run_hotplug(hdev->name, "resume");
+	return 0;
+}       
 
-	if (hdev->flags & HCI_UP)
-		hci_dev_close(hdev->id);
+/* Receive frame from HCI drivers */
+int hci_recv_frame(struct sk_buff *skb)
+{
+	struct hci_dev *hdev = (struct hci_dev *) skb->dev;
 
-	/* Find device slot */
-	spin_lock(&hdev_list_lock);
-	for (i = 0; i < HCI_MAX_DEV; i++) {
-		if (hdev_list[i] == hdev) {
-			hdev_list[i] = NULL;
-			MOD_DEC_USE_COUNT;
-			break;
-		}
+	if (!hdev || (!test_bit(HCI_UP, &hdev->flags) && 
+				!test_bit(HCI_INIT, &hdev->flags)) ) {
+		kfree_skb(skb);
+		return -1;
 	}
-	spin_unlock(&hdev_list_lock);
 
-	hci_notify(hdev, HCI_DEV_UNREG);
-
-	/* Sleep while device is in use */
-	while (atomic_read(&hdev->refcnt)) {
-		int sleep_cnt = 100;
+	BT_DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);
 
-		DBG("%s sleeping on lock %d", hdev->name, atomic_read(&hdev->refcnt));
+	/* Incomming skb */
+	bluez_cb(skb)->incomming = 1;
 
-		sleep_on_timeout(&hdev->req_wait_q, HZ*10);
-		if (!(--sleep_cnt))
-			break;
-	}
+	/* Time stamp */
+	do_gettimeofday(&skb->stamp);
 
+	/* Queue frame for rx task */
+	skb_queue_tail(&hdev->rx_q, skb);
+	hci_sched_rx(hdev);
 	return 0;
 }
 
-/* Interface to upper protocols */
+/* ---- Interface to upper protocols ---- */
 
 /* Register/Unregister protocols.
- * hci_task_lock is used to ensure that no tasks are running.
- */
-int hci_register_proto(struct hci_proto *hproto)
+ * hci_task_lock is used to ensure that no tasks are running. */
+int hci_register_proto(struct hci_proto *hp)
 {
 	int err = 0;
 
-	DBG("%p name %s", hproto, hproto->name);
+	BT_DBG("%p name %s id %d", hp, hp->name, hp->id);
 
-	if (hproto->id >= HCI_MAX_PROTO)
+	if (hp->id >= HCI_MAX_PROTO)
 		return -EINVAL;
 
 	write_lock_bh(&hci_task_lock);
 
-	if (!hproto_list[hproto->id])
-		hproto_list[hproto->id] = hproto;
+	if (!hci_proto[hp->id])
+		hci_proto[hp->id] = hp;
 	else
-		err = -1;
+		err = -EEXIST;
 
 	write_unlock_bh(&hci_task_lock);
 
 	return err;
 }
 
-int hci_unregister_proto(struct hci_proto *hproto)
+int hci_unregister_proto(struct hci_proto *hp)
 {
 	int err = 0;
 
-	DBG("%p name %s", hproto, hproto->name);
+	BT_DBG("%p name %s id %d", hp, hp->name, hp->id);
 
-	if (hproto->id > HCI_MAX_PROTO)
+	if (hp->id >= HCI_MAX_PROTO)
 		return -EINVAL;
 
 	write_lock_bh(&hci_task_lock);
 
-	if (hproto_list[hproto->id])
-		hproto_list[hproto->id] = NULL;
+	if (hci_proto[hp->id])
+		hci_proto[hp->id] = NULL;
 	else
 		err = -ENOENT;
 
@@ -1070,10 +966,14 @@
 		return -ENODEV;
 	}
 
-	DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);
+	BT_DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);
+
+	if (atomic_read(&hdev->promisc)) {
+		/* Time stamp */
+	        do_gettimeofday(&skb->stamp);
 
-	if (hdev->flags & HCI_SOCK)
 		hci_send_to_sock(hdev, skb);
+	}
 
 	/* Get rid of skb owner, prior to sending to the driver. */
 	skb_orphan(skb);
@@ -1081,128 +981,6 @@
 	return hdev->send(skb);
 }
 
-/* Connection scheduler */
-static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int *quote)
-{
-	struct conn_hash *h = &hdev->conn_hash;
-	struct hci_conn *conn = NULL;
-	int num = 0, min = 0xffff;
-        struct list_head *p;
-
-	conn_hash_lock(h);
-	list_for_each(p, &h->list) {
-		register struct hci_conn *c;
-
-		c = list_entry(p, struct hci_conn, list);
-
-		if (c->type != type || skb_queue_empty(&c->data_q))
-			continue;
-		num++;
-
-		if (c->sent < min) {
-			min  = c->sent;
-			conn = c;
-		}
-	}
-	conn_hash_unlock(h);
-
-	if (conn) {
-		int q = hdev->acl_cnt / num;
-		*quote = q ? q : 1;
-	} else
-		*quote = 0;
-
-	DBG("conn %p quote %d", conn, *quote);
-
-	return conn;
-}
-
-static inline void hci_sched_acl(struct hci_dev *hdev)
-{
-	struct hci_conn *conn;
-	struct sk_buff *skb;
-	int quote;
-
-	DBG("%s", hdev->name);
-
-	while (hdev->acl_cnt && (conn = hci_low_sent(hdev, ACL_LINK, &quote))) {
-		while (quote && (skb = skb_dequeue(&conn->data_q))) {
-			DBG("skb %p len %d", skb, skb->len);
-
-			hci_send_frame(skb);
-
-			conn->sent++;
-			hdev->acl_cnt--;
-			quote--;
-		}
-	}
-}
-
-/* Schedule SCO */
-static inline void hci_sched_sco(struct hci_dev *hdev)
-{
-	/* FIXME: For now we queue SCO packets to the raw queue 
-
-		while (hdev->sco_cnt && (skb = skb_dequeue(&conn->data_q))) {
-			hci_send_frame(skb);
-			conn->sco_sent++;
-			hdev->sco_cnt--;
-		}
-	*/
-}
-
-/* Get data from the previously sent command */
-static void * hci_sent_cmd_data(struct hci_dev *hdev, __u16 ogf, __u16 ocf)
-{
-	hci_command_hdr *hc;
-
-	if (!hdev->sent_cmd)
-		return NULL;
-
-	hc = (void *) hdev->sent_cmd->data;
-
-	if (hc->opcode != __cpu_to_le16(cmd_opcode_pack(ogf, ocf)))
-		return NULL;
-
-	DBG("%s ogf 0x%x ocf 0x%x", hdev->name, ogf, ocf);
-
-	return hdev->sent_cmd->data + HCI_COMMAND_HDR_SIZE;
-}
-
-/* Send raw HCI frame */
-int hci_send_raw(struct sk_buff *skb)
-{
-	struct hci_dev *hdev = (struct hci_dev *) skb->dev;
-
-	if (!hdev) {
-		kfree_skb(skb);
-		return -ENODEV;
-	}
-
-	DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);
-
-	if (hdev->flags & HCI_NORMAL) {
-		/* Queue frame according it's type */
-		switch (skb->pkt_type) {
-		case HCI_COMMAND_PKT:
-			skb_queue_tail(&hdev->cmd_q, skb);
-			hci_sched_cmd(hdev);
-			return 0;
-
-		case HCI_ACLDATA_PKT:
-		case HCI_SCODATA_PKT:
-			/* FIXME:
-		 	 * Check header here and queue to apropriate connection.
-		 	 */
-			break;
-		}
-	}
-
-	skb_queue_tail(&hdev->raw_q, skb);
-	hci_sched_tx(hdev);
-	return 0;
-}
-
 /* Send HCI command */
 int hci_send_cmd(struct hci_dev *hdev, __u16 ogf, __u16 ocf, __u32 plen, void *param)
 {
@@ -1210,10 +988,10 @@
 	hci_command_hdr *hc;
 	struct sk_buff *skb;
 
-	DBG("%s ogf 0x%x ocf 0x%x plen %d", hdev->name, ogf, ocf, plen);
+	BT_DBG("%s ogf 0x%x ocf 0x%x plen %d", hdev->name, ogf, ocf, plen);
 
 	if (!(skb = bluez_skb_alloc(len, GFP_ATOMIC))) {
-		ERR("%s Can't allocate memory for HCI command", hdev->name);
+		BT_ERR("%s Can't allocate memory for HCI command", hdev->name);
 		return -ENOMEM;
 	}
 	
@@ -1224,7 +1002,7 @@
 	if (plen)
 		memcpy(skb_put(skb, plen), param, plen);
 
-	DBG("skb len %d", skb->len);
+	BT_DBG("skb len %d", skb->len);
 
 	skb->pkt_type = HCI_COMMAND_PKT;
 	skb->dev = (void *) hdev;
@@ -1234,10 +1012,28 @@
 	return 0;
 }
 
+/* Get data from the previously sent command */
+void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 ogf, __u16 ocf)
+{
+	hci_command_hdr *hc;
+
+	if (!hdev->sent_cmd)
+		return NULL;
+
+	hc = (void *) hdev->sent_cmd->data;
+
+	if (hc->opcode != __cpu_to_le16(cmd_opcode_pack(ogf, ocf)))
+		return NULL;
+
+	BT_DBG("%s ogf 0x%x ocf 0x%x", hdev->name, ogf, ocf);
+
+	return hdev->sent_cmd->data + HCI_COMMAND_HDR_SIZE;
+}
+
 /* Send ACL data */
 static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags)
 {
-	int len = skb->len;	
+	int len = skb->len;
 	hci_acl_hdr *ah;
 
 	ah = (hci_acl_hdr *) skb_push(skb, HCI_ACL_HDR_SIZE);
@@ -1252,7 +1048,7 @@
 	struct hci_dev *hdev = conn->hdev;
 	struct sk_buff *list;
 
-	DBG("%s conn %p flags 0x%x", hdev->name, conn, flags);
+	BT_DBG("%s conn %p flags 0x%x", hdev->name, conn, flags);
 
 	skb->dev = (void *) hdev;
 	skb->pkt_type = HCI_ACLDATA_PKT;
@@ -1260,12 +1056,12 @@
 
 	if (!(list = skb_shinfo(skb)->frag_list)) {
 		/* Non fragmented */
-		DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len);
+		BT_DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len);
 		
 		skb_queue_tail(&conn->data_q, skb);
 	} else {
 		/* Fragmented */
-		DBG("%s frag %p len %d", hdev->name, skb, skb->len);
+		BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len);
 
 		skb_shinfo(skb)->frag_list = NULL;
 
@@ -1280,7 +1076,7 @@
 			skb->pkt_type = HCI_ACLDATA_PKT;
 			hci_add_acl_hdr(skb, conn->handle, flags | ACL_CONT);
 		
-			DBG("%s frag %p len %d", hdev->name, skb, skb->len);
+			BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len);
 
 			__skb_queue_tail(&conn->data_q, skb);
 		} while (list);
@@ -1298,7 +1094,7 @@
 	struct hci_dev *hdev = conn->hdev;
 	hci_sco_hdr hs;
 
-	DBG("%s len %d", hdev->name, skb->len);
+	BT_DBG("%s len %d", hdev->name, skb->len);
 
 	if (skb->len > hdev->sco_mtu) {
 		kfree_skb(skb);
@@ -1315,544 +1111,136 @@
 	skb->pkt_type = HCI_SCODATA_PKT;
 	skb_queue_tail(&conn->data_q, skb);
 	hci_sched_tx(hdev);
-
 	return 0;
 }
 
-/* Handle HCI Event packets */
-
-/* Command Complete OGF LINK_CTL  */
-static void hci_cc_link_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
-{
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	default:
-		DBG("%s Command complete: ogf LINK_CTL ocf %x", hdev->name, ocf);
-		break;
-	};
-}
-
-/* Command Complete OGF LINK_POLICY  */
-static void hci_cc_link_policy(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
-{
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	default:
-		DBG("%s: Command complete: ogf LINK_POLICY ocf %x", hdev->name, ocf);
-		break;
-	};
-}
-
-/* Command Complete OGF HOST_CTL  */
-static void hci_cc_host_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
-{
-	__u8 status, param;
-	void *sent;
-
-
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	case OCF_RESET:
-		status = *((__u8 *) skb->data);
-
-		hci_req_complete(hdev, status);
-		break;
-
-	case OCF_SET_EVENT_FLT:
-		status = *((__u8 *) skb->data);
-
-		if (status) {
-			DBG("%s SET_EVENT_FLT failed %d", hdev->name, status);
-		} else {
-			DBG("%s SET_EVENT_FLT succeseful", hdev->name);
-		}
-		break;
-
-	case OCF_WRITE_AUTH_ENABLE:
-		if (!(sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_AUTH_ENABLE)))
-			break;
-
-		status = *((__u8 *) skb->data);
-		param  = *((__u8 *) sent);
-
-		if (!status) {
-			if (param == AUTH_ENABLED)
-				hdev->flags |= HCI_AUTH;
-			else
-				hdev->flags &= ~HCI_AUTH;
-		}
-		hci_req_complete(hdev, status);
-		break;
-
-	case OCF_WRITE_CA_TIMEOUT:
-		status = *((__u8 *) skb->data);
-
-		if (status) {
-			DBG("%s OCF_WRITE_CA_TIMEOUT failed %d", hdev->name, status);
-		} else {
-			DBG("%s OCF_WRITE_CA_TIMEOUT succeseful", hdev->name);
-		}
-		break;
-
-	case OCF_WRITE_PG_TIMEOUT:
-		status = *((__u8 *) skb->data);
-
-		if (status) {
-			DBG("%s OCF_WRITE_PG_TIMEOUT failed %d", hdev->name, status);
-		} else {
-			DBG("%s: OCF_WRITE_PG_TIMEOUT succeseful", hdev->name);
-		}
-		break;
-
-	case OCF_WRITE_SCAN_ENABLE:
-		if (!(sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE)))
-			break;
-		status = *((__u8 *) skb->data);
-		param  = *((__u8 *) sent);
-
-		DBG("param 0x%x", param);
-
-		if (!status) {
-			switch (param) {
-			case IS_ENA_PS_ENA:
-				hdev->flags |=  HCI_PSCAN | HCI_ISCAN;
-				break;
-
-			case IS_ENA_PS_DIS:
-				hdev->flags &= ~HCI_PSCAN;
-				hdev->flags |=  HCI_ISCAN;
-				break;
-
-			case IS_DIS_PS_ENA:
-				hdev->flags &= ~HCI_ISCAN;
-				hdev->flags |=  HCI_PSCAN;
-				break;
-
-			default:
-				hdev->flags &= ~(HCI_ISCAN | HCI_PSCAN);
-				break;
-			};
-		}
-		hci_req_complete(hdev, status);
-		break;
-
-	default:
-		DBG("%s Command complete: ogf HOST_CTL ocf %x", hdev->name, ocf);
-		break;
-	};
-}
+/* ---- HCI TX task (outgoing data) ---- */
 
-/* Command Complete OGF INFO_PARAM  */
-static void hci_cc_info_param(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
+/* HCI Connection scheduler */
+static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int *quote)
 {
-	read_local_features_rp *lf;
-	read_buffer_size_rp *bs;
-	read_bd_addr_rp *ba;
-
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	case OCF_READ_LOCAL_FEATURES:
-		lf = (read_local_features_rp *) skb->data;
-
-		if (lf->status) {
-			DBG("%s READ_LOCAL_FEATURES failed %d", hdev->name, lf->status);
-			break;
-		}
-
-		memcpy(hdev->features, lf->features, sizeof(hdev->features));
-
-		/* Adjust default settings according to features 
-		 * supported by device. */
-		if (hdev->features[0] & LMP_3SLOT)
-			hdev->pkt_type |= (HCI_DM3 | HCI_DH3);
-
-		if (hdev->features[0] & LMP_5SLOT)
-			hdev->pkt_type |= (HCI_DM5 | HCI_DH5);
-
-		DBG("%s: features 0x%x 0x%x 0x%x", hdev->name, lf->features[0], lf->features[1], lf->features[2]);
-
-		break;
-
-	case OCF_READ_BUFFER_SIZE:
-		bs = (read_buffer_size_rp *) skb->data;
-
-		if (bs->status) {
-			DBG("%s READ_BUFFER_SIZE failed %d", hdev->name, bs->status);
-			break;
-		}
-
-		hdev->acl_mtu = __le16_to_cpu(bs->acl_mtu);
-		hdev->sco_mtu = bs->sco_mtu;
-		hdev->acl_max = hdev->acl_cnt = __le16_to_cpu(bs->acl_max_pkt);
-		hdev->sco_max = hdev->sco_cnt = __le16_to_cpu(bs->sco_max_pkt);
-
-		DBG("%s mtu: acl %d, sco %d max_pkt: acl %d, sco %d", hdev->name,
-		    hdev->acl_mtu, hdev->sco_mtu, hdev->acl_max, hdev->sco_max);
+	struct conn_hash *h = &hdev->conn_hash;
+	struct hci_conn  *conn = NULL;
+	int num = 0, min = ~0;
+        struct list_head *p;
 
-		break;
+	/* 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);
 
-	case OCF_READ_BD_ADDR:
-		ba = (read_bd_addr_rp *) skb->data;
+		if (c->type != type || c->state != BT_CONNECTED
+				|| skb_queue_empty(&c->data_q))
+			continue;
+		num++;
 
-		if (!ba->status) {
-			bacpy(&hdev->bdaddr, &ba->bdaddr);
-		} else {
-			DBG("%s: READ_BD_ADDR failed %d", hdev->name, ba->status);
+		if (c->sent < min) {
+			min  = c->sent;
+			conn = c;
 		}
+	}
 
-		hci_req_complete(hdev, ba->status);
-		break;
+	if (conn) {
+		int cnt = (type == ACL_LINK ? hdev->acl_cnt : hdev->sco_cnt);
+		int q = cnt / num;
+		*quote = q ? q : 1;
+	} else
+		*quote = 0;
 
-	default:
-		DBG("%s Command complete: ogf INFO_PARAM ocf %x", hdev->name, ocf);
-		break;
-	};
+	BT_DBG("conn %p quote %d", conn, *quote);
+	return conn;
 }
 
-/* Command Status OGF LINK_CTL  */
-static void hci_cs_link_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
+static inline void hci_acl_tx_to(struct hci_dev *hdev)
 {
-	struct hci_proto * hp;
-
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	case OCF_CREATE_CONN:
-		if (status) {
-			create_conn_cp *cc = hci_sent_cmd_data(hdev, OGF_LINK_CTL, OCF_CREATE_CONN);
-
-			if (!cc)
-				break;
+	struct conn_hash *h = &hdev->conn_hash;
+	struct list_head *p;
+	struct hci_conn  *c;
 
-			DBG("%s Create connection error: status 0x%x %s", hdev->name,
-			    status, batostr(&cc->bdaddr));
+	BT_ERR("%s ACL tx timeout", hdev->name);
 
-			/* Notify upper protocols */
-			if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->connect_cfm) {
-				tasklet_disable(&hdev->tx_task);
-				hp->connect_cfm(hdev, &cc->bdaddr, status, NULL);
-				tasklet_enable(&hdev->tx_task);
-			}
-		}
-		break;
-
-	case OCF_INQUIRY:
-		if (status) {
-			DBG("%s Inquiry error: status 0x%x", hdev->name, status);
-			hci_req_complete(hdev, status);
+	/* 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);
 		}
-		break;
-
-	default:
-		DBG("%s Command status: ogf LINK_CTL ocf %x", hdev->name, ocf);
-		break;
-	};
-}
-
-/* Command Status OGF LINK_POLICY */
-static void hci_cs_link_policy(struct hci_dev *hdev, __u16 ocf, __u8 status)
-{
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	default:
-		DBG("%s Command status: ogf HOST_POLICY ocf %x", hdev->name, ocf);
-		break;
-	};
-}
-
-/* Command Status OGF HOST_CTL */
-static void hci_cs_host_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
-{
-	DBG("%s ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	default:
-		DBG("%s Command status: ogf HOST_CTL ocf %x", hdev->name, ocf);
-		break;
-	};
-}
-
-/* Command Status OGF INFO_PARAM  */
-static void hci_cs_info_param(struct hci_dev *hdev, __u16 ocf, __u8 status)
-{
-	DBG("%s: hci_cs_info_param: ocf 0x%x", hdev->name, ocf);
-
-	switch (ocf) {
-	default:
-		DBG("%s Command status: ogf INFO_PARAM ocf %x", hdev->name, ocf);
-		break;
-	};
-}
-
-/* Inquiry Complete */
-static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
-{
-	__u8 status = *((__u8 *) skb->data);
-
-	DBG("%s status %d", hdev->name, status);
-
-	hci_req_complete(hdev, status);
+	}
 }
 
-/* Inquiry Result */
-static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static inline void hci_sched_acl(struct hci_dev *hdev)
 {
-	inquiry_info *info = (inquiry_info *) (skb->data + 1);
-	int num_rsp = *((__u8 *) skb->data);
-
-	DBG("%s num_rsp %d", hdev->name, num_rsp);
+	struct hci_conn *conn;
+	struct sk_buff *skb;
+	int quote;
 
-	for (; num_rsp; num_rsp--)
-		inquiry_cache_update(&hdev->inq_cache, info++);
-}
+	BT_DBG("%s", hdev->name);
 
-/* Connect Request */
-static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
-{
-	evt_conn_request *cr = (evt_conn_request *) skb->data;
-	struct hci_proto *hp;
-	accept_conn_req_cp ac;
-	int accept = 0;
+	/* 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);
 
-	DBG("%s Connection request: %s type 0x%x", hdev->name, batostr(&cr->bdaddr), cr->link_type);
+	while (hdev->acl_cnt && (conn = hci_low_sent(hdev, ACL_LINK, &quote))) {
+		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;
 
-	/* Notify upper protocols */
-	if (cr->link_type == ACL_LINK) {
-		/* ACL link notify L2CAP */
-		if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->connect_ind) {
-			tasklet_disable(&hdev->tx_task);
-			accept = hp->connect_ind(hdev, &cr->bdaddr);
-			tasklet_enable(&hdev->tx_task);
+			hdev->acl_cnt--;
+			conn->sent++;
 		}
-	} else {
-		/* SCO link (no notification) */
-		/* FIXME: Should be accept it here or let the requester (app) accept it ? */
-		accept = 1;
-	}
-
-	if (accept) {
-		/* Connection accepted by upper layer */
-		bacpy(&ac.bdaddr, &cr->bdaddr);
-		ac.role = 0x01; /* Remain slave */
-		hci_send_cmd(hdev, OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ, ACCEPT_CONN_REQ_CP_SIZE, &ac);
-	} else {
-		/* Connection rejected by upper layer */
-		/* FIXME: 
-		 * Should we use HCI reject here ?
-		 */
-		return;
 	}
 }
 
-/* Connect Complete */
-static void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+/* Schedule SCO */
+static inline void hci_sched_sco(struct hci_dev *hdev)
 {
-	evt_conn_complete *cc = (evt_conn_complete *) skb->data;
-	struct hci_conn *conn = NULL;
-	struct hci_proto *hp;
-
-	DBG("%s", hdev->name);
-
-	tasklet_disable(&hdev->tx_task);
+	struct hci_conn *conn;
+	struct sk_buff *skb;
+	int quote;
 
-	if (!cc->status)
- 		conn = hci_conn_add(hdev, __le16_to_cpu(cc->handle), cc->link_type, &cc->bdaddr);
+	BT_DBG("%s", hdev->name);
 
-	/* Notify upper protocols */
-	if (cc->link_type == ACL_LINK) {
-		/* ACL link notify L2CAP layer */
-		if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->connect_cfm)
-			hp->connect_cfm(hdev, &cc->bdaddr, cc->status, conn);
-	} else {
-		/* SCO link (no notification) */
-	}
-
-	tasklet_enable(&hdev->tx_task);
-}
+	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);
 
-/* Disconnect Complete */
-static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
-{
-	evt_disconn_complete *dc = (evt_disconn_complete *) skb->data;
-	struct hci_conn *conn = NULL;
-	struct hci_proto *hp;
-	__u16 handle = __le16_to_cpu(dc->handle);
-
-	DBG("%s", hdev->name);
-
-	if (!dc->status && (conn = conn_hash_lookup(&hdev->conn_hash, handle))) {
-		tasklet_disable(&hdev->tx_task);
-
-		/* Notify upper protocols */
-		if (conn->type == ACL_LINK) {
-			/* ACL link notify L2CAP layer */
-			if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->disconn_ind)
-				hp->disconn_ind(conn, dc->reason);
-		} else {
-			/* SCO link (no notification) */
+			conn->sent++;
+			if (conn->sent == ~0)
+				conn->sent = 0;
 		}
-
-		hci_conn_del(hdev, conn);
-
-		tasklet_enable(&hdev->tx_task);
 	}
 }
 
-/* Number of completed packets */
-static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_tx_task(unsigned long arg)
 {
-	evt_num_comp_pkts *nc = (evt_num_comp_pkts *) skb->data;
-	__u16 *ptr;
-	int i;
-
-	skb_pull(skb, EVT_NUM_COMP_PKTS_SIZE);
-
-	DBG("%s num_hndl %d", hdev->name, nc->num_hndl);
+	struct hci_dev *hdev = (struct hci_dev *) arg;
+	struct sk_buff *skb;
 
-	if (skb->len < nc->num_hndl * 4) {
-		DBG("%s bad parameters", hdev->name);
-		return;
-	}
+	read_lock(&hci_task_lock);
 
-	tasklet_disable(&hdev->tx_task);
+	BT_DBG("%s acl %d sco %d", hdev->name, hdev->acl_cnt, hdev->sco_cnt);
 
-	for (i = 0, ptr = (__u16 *) skb->data; i < nc->num_hndl; i++) {
-		struct hci_conn *conn;
-		__u16 handle, count;
+	/* Schedule queues and send stuff to HCI driver */
 
-		handle = __le16_to_cpu(get_unaligned(ptr++));
-		count  = __le16_to_cpu(get_unaligned(ptr++));
+	hci_sched_acl(hdev);
 
-		hdev->acl_cnt += count;
+	hci_sched_sco(hdev);
 
-		if ((conn = conn_hash_lookup(&hdev->conn_hash, handle)))
-			conn->sent -= count;
-	}
+	/* Send next queued raw (unknown type) packet */
+	while ((skb = skb_dequeue(&hdev->raw_q)))
+		hci_send_frame(skb);
 
-	tasklet_enable(&hdev->tx_task);
-	
-	hci_sched_tx(hdev);
+	read_unlock(&hci_task_lock);
 }
 
-static inline void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
-{
-	hci_event_hdr *he = (hci_event_hdr *) skb->data;
-	evt_cmd_status *cs;
-	evt_cmd_complete *ec;
-	__u16 opcode, ocf, ogf;
-
-	skb_pull(skb, HCI_EVENT_HDR_SIZE);
-
-	DBG("%s evt 0x%x", hdev->name, he->evt);
-
-	switch (he->evt) {
-	case EVT_NUM_COMP_PKTS:
-		hci_num_comp_pkts_evt(hdev, skb);
-		break;
-
-	case EVT_INQUIRY_COMPLETE:
-		hci_inquiry_complete_evt(hdev, skb);
-		break;
-
-	case EVT_INQUIRY_RESULT:
-		hci_inquiry_result_evt(hdev, skb);
-		break;
-
-	case EVT_CONN_REQUEST:
-		hci_conn_request_evt(hdev, skb);
-		break;
-
-	case EVT_CONN_COMPLETE:
-		hci_conn_complete_evt(hdev, skb);
-		break;
-
-	case EVT_DISCONN_COMPLETE:
-		hci_disconn_complete_evt(hdev, skb);
-		break;
-
-	case EVT_CMD_STATUS:
-		cs = (evt_cmd_status *) skb->data;
-		skb_pull(skb, EVT_CMD_STATUS_SIZE);
-				
-		opcode = __le16_to_cpu(cs->opcode);
-		ogf = cmd_opcode_ogf(opcode);
-		ocf = cmd_opcode_ocf(opcode);
-
-		switch (ogf) {
-		case OGF_INFO_PARAM:
-			hci_cs_info_param(hdev, ocf, cs->status);
-			break;
-
-		case OGF_HOST_CTL:
-			hci_cs_host_ctl(hdev, ocf, cs->status);
-			break;
-
-		case OGF_LINK_CTL:
-			hci_cs_link_ctl(hdev, ocf, cs->status);
-			break;
-
-		case OGF_LINK_POLICY:
-			hci_cs_link_policy(hdev, ocf, cs->status);
-			break;
-
-		default:
-			DBG("%s Command Status OGF %x", hdev->name, ogf);
-			break;
-		};
-
-		if (cs->ncmd) {
-			atomic_set(&hdev->cmd_cnt, 1);
-			if (!skb_queue_empty(&hdev->cmd_q))
-				hci_sched_cmd(hdev);
-		}
-		break;
-
-	case EVT_CMD_COMPLETE:
-		ec = (evt_cmd_complete *) skb->data;
-		skb_pull(skb, EVT_CMD_COMPLETE_SIZE);
-
-		opcode = __le16_to_cpu(ec->opcode);
-		ogf = cmd_opcode_ogf(opcode);
-		ocf = cmd_opcode_ocf(opcode);
-
-		switch (ogf) {
-		case OGF_INFO_PARAM:
-			hci_cc_info_param(hdev, ocf, skb);
-			break;
-
-		case OGF_HOST_CTL:
-			hci_cc_host_ctl(hdev, ocf, skb);
-			break;
-
-		case OGF_LINK_CTL:
-			hci_cc_link_ctl(hdev, ocf, skb);
-			break;
-
-		case OGF_LINK_POLICY:
-			hci_cc_link_policy(hdev, ocf, skb);
-			break;
-
-		default:
-			DBG("%s Command Completed OGF %x", hdev->name, ogf);
-			break;
-		};
-
-		if (ec->ncmd) {
-			atomic_set(&hdev->cmd_cnt, 1);
-			if (!skb_queue_empty(&hdev->cmd_q))
-				hci_sched_cmd(hdev);
-		}
-		break;
-	};
 
-	kfree_skb(skb);
-	hdev->stat.evt_rx++;
-}
+/* ----- HCI RX task (incomming data proccessing) ----- */
 
 /* ACL data packet */
 static inline void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb)
@@ -1867,51 +1255,86 @@
 	flags  = acl_flags(handle);
 	handle = acl_handle(handle);
 
-	DBG("%s len %d handle 0x%x flags 0x%x", hdev->name, skb->len, handle, flags);
+	BT_DBG("%s len %d handle 0x%x flags 0x%x", hdev->name, skb->len, handle, flags);
 
-	if ((conn = conn_hash_lookup(&hdev->conn_hash, handle))) {
+	hdev->stat.acl_rx++;
+
+	hci_dev_lock(hdev);
+	conn = conn_hash_lookup_handle(hdev, handle);
+	hci_dev_unlock(hdev);
+	
+	if (conn) {
 		register struct hci_proto *hp;
 
 		/* Send to upper protocol */
-		if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->recv_acldata) {
+		if ((hp = hci_proto[HCI_PROTO_L2CAP]) && hp->recv_acldata) {
 			hp->recv_acldata(conn, skb, flags);
-			goto sent;
+			return;
 		}
 	} else {
-		ERR("%s ACL packet for unknown connection handle %d", hdev->name, handle);
+		BT_ERR("%s ACL packet for unknown connection handle %d", 
+			hdev->name, handle);
 	}
 
 	kfree_skb(skb);
-sent:
-	hdev->stat.acl_rx++;
 }
 
 /* SCO data packet */
 static inline void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
-	DBG("%s len %d", hdev->name, skb->len);
+	hci_sco_hdr *sh = (void *) skb->data;
+	struct hci_conn *conn;
+	__u16 handle;
+
+	skb_pull(skb, HCI_SCO_HDR_SIZE);
+
+	handle = __le16_to_cpu(sh->handle);
+
+	BT_DBG("%s len %d handle 0x%x", hdev->name, skb->len, handle);
 
-	kfree_skb(skb);
 	hdev->stat.sco_rx++;
+
+	hci_dev_lock(hdev);
+	conn = conn_hash_lookup_handle(hdev, handle);
+	hci_dev_unlock(hdev);
+	
+	if (conn) {
+		register struct hci_proto *hp;
+
+		/* Send to upper protocol */
+		if ((hp = hci_proto[HCI_PROTO_SCO]) && hp->recv_scodata) {
+			hp->recv_scodata(conn, skb);
+			return;
+		}
+	} else {
+		BT_ERR("%s SCO packet for unknown connection handle %d", 
+			hdev->name, handle);
+	}
+
+	kfree_skb(skb);
 }
 
-/* ----- HCI tasks ----- */
 void hci_rx_task(unsigned long arg)
 {
 	struct hci_dev *hdev = (struct hci_dev *) arg;
 	struct sk_buff *skb;
 
-	DBG("%s", hdev->name);
+	BT_DBG("%s", hdev->name);
 
 	read_lock(&hci_task_lock);
 
 	while ((skb = skb_dequeue(&hdev->rx_q))) {
-		if (hdev->flags & HCI_SOCK) {
+		if (atomic_read(&hdev->promisc)) {
 			/* Send copy to the sockets */
 			hci_send_to_sock(hdev, skb);
 		}
 
-		if (hdev->flags & HCI_INIT) {
+		if (test_bit(HCI_RAW, &hdev->flags)) {
+			kfree_skb(skb);
+			continue;
+		}
+
+		if (test_bit(HCI_INIT, &hdev->flags)) {
 			/* Don't process data packets in this states. */
 			switch (skb->pkt_type) {
 			case HCI_ACLDATA_PKT:
@@ -1921,64 +1344,43 @@
 			};
 		}
 
-		if (hdev->flags & HCI_NORMAL) {
-			/* Process frame */
-			switch (skb->pkt_type) {
-			case HCI_EVENT_PKT:
-				hci_event_packet(hdev, skb);
-				break;
+		/* Process frame */
+		switch (skb->pkt_type) {
+		case HCI_EVENT_PKT:
+			hci_event_packet(hdev, skb);
+			break;
 
-			case HCI_ACLDATA_PKT:
-				DBG("%s ACL data packet", hdev->name);
-				hci_acldata_packet(hdev, skb);
-				break;
+		case HCI_ACLDATA_PKT:
+			BT_DBG("%s ACL data packet", hdev->name);
+			hci_acldata_packet(hdev, skb);
+			break;
 
-			case HCI_SCODATA_PKT:
-				DBG("%s SCO data packet", hdev->name);
-				hci_scodata_packet(hdev, skb);
-				break;
+		case HCI_SCODATA_PKT:
+			BT_DBG("%s SCO data packet", hdev->name);
+			hci_scodata_packet(hdev, skb);
+			break;
 
-			default:
-				kfree_skb(skb);
-				break;
-			};
-		} else {
+		default:
 			kfree_skb(skb);
+			break;
 		}
 	}
 
 	read_unlock(&hci_task_lock);
 }
 
-static void hci_tx_task(unsigned long arg)
-{
-	struct hci_dev *hdev = (struct hci_dev *) arg;
-	struct sk_buff *skb;
-
-	read_lock(&hci_task_lock);
-
-	DBG("%s acl %d sco %d", hdev->name, hdev->acl_cnt, hdev->sco_cnt);
-
-	/* Schedule queues and send stuff to HCI driver */
-
-	hci_sched_acl(hdev);
-
-	hci_sched_sco(hdev);
-
-	/* Send next queued raw (unknown type) packet */
-	while ((skb = skb_dequeue(&hdev->raw_q)))
-		hci_send_frame(skb);
-
-	read_unlock(&hci_task_lock);
-}
-
 static void hci_cmd_task(unsigned long arg)
 {
 	struct hci_dev *hdev = (struct hci_dev *) arg;
 	struct sk_buff *skb;
 
-	DBG("%s cmd %d", hdev->name, atomic_read(&hdev->cmd_cnt));
+	BT_DBG("%s cmd %d", hdev->name, atomic_read(&hdev->cmd_cnt));
 
+	if (!atomic_read(&hdev->cmd_cnt) && (jiffies - hdev->cmd_last_tx) > HZ) {
+		BT_ERR("%s command tx timeout", hdev->name);
+		atomic_set(&hdev->cmd_cnt, 1);
+	}
+	
 	/* Send queued commands */
 	if (atomic_read(&hdev->cmd_cnt) && (skb = skb_dequeue(&hdev->cmd_q))) {
 		if (hdev->sent_cmd)
@@ -1987,6 +1389,7 @@
 		if ((hdev->sent_cmd = skb_clone(skb, GFP_ATOMIC))) {
 			atomic_dec(&hdev->cmd_cnt);
 			hci_send_frame(skb);
+			hdev->cmd_last_tx = jiffies;
 		} else {
 			skb_queue_head(&hdev->cmd_q, skb);
 			hci_sched_cmd(hdev);
@@ -1994,33 +1397,10 @@
 	}
 }
 
-/* Receive frame from HCI drivers */
-int hci_recv_frame(struct sk_buff *skb)
-{
-	struct hci_dev *hdev = (struct hci_dev *) skb->dev;
-
-	if (!hdev || !(hdev->flags & (HCI_UP | HCI_INIT))) {
-		kfree_skb(skb);
-		return -1;
-	}
-
-	DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);
-
-	/* Incomming skb */
-	bluez_cb(skb)->incomming = 1;
-
-	/* Queue frame for rx task */
-	skb_queue_tail(&hdev->rx_q, skb);
-	hci_sched_rx(hdev);
-
-	return 0;
-}
+/* ---- Initialization ---- */
 
 int hci_core_init(void)
 {
-	/* Init locks */
-	spin_lock_init(&hdev_list_lock);
-
 	return 0;
 }
 
@@ -2028,5 +1408,3 @@
 {
 	return 0;
 }
-
-MODULE_LICENSE("GPL");