[Bluez-devel] [PATCH] - BT HID message parsing framework and fixes for Keyboards in boot mode.

[Matthew Grant <grantma@anathoth.gen.nz>]

[-- Attachment #1.1: Type: text/plain, Size: 1657 bytes --]

Dear Marcel,

I have reworked the patches as you have requested.  They are attached to
this message.

This is the BT HID message parsing framework that I wrote, based on the
Bluetooth HID specs. The kernel versions the patches are against are
mentioned in their names.  The one against 2.6.10-rc3-bk6 is the one for
the standard BT HID code in the stock kernel, then other contains fixes
for report mode processing where the BT device is stuck in Boot mode due
to being first of all configured by the computer BIOS.

The devices I tested this with are Apple BT keyboard, and Logitech Mx900
mouse.  the changes are all conservative, and should work with all
devices.  

I have left the debug work I did as is in the patches as I need it to
debug the work I am doing.  It fixes various compile problems with the
debug code, as well as adding output of data that I found useful in
diagnosing my keyboard problems.  I am planning to continue my work on
the BT HID as I am able, and hope to do phase 2 (SET_ and GET_) HID
state machines over the Christmas holidays.  The Debug stuff has to stay
there.  Basically if you want me to significant work in the BT HID area,
I need a free hand to do things.  I already have to spend quite a bit of
time making a patch for the standard kernel, as well as against the
2.6.x-mhx patches which I do my development against.  We need to discuss
how we are going to work this so that it does not take too much of your
time.

I am hoping to get a Web site up shortly with these patches and further
work, as well as a version control system for a BT HID project!

Best Regards,

Matthew Grant

[-- Attachment #1.2: linux-2.6.9-mh5-bthid-2004121501.patch --]
[-- Type: text/x-patch, Size: 14150 bytes --]

Signed-off-by: Matthew Grant <grantma@anathoth.gen.nz>

Implements changes in core BT HID for incoming packet processing, plus start of
processing for incoming messages in report mode.  Also sets a device stuck in boot
mode to report mode when device first connects.

--- linux-2.6.9-mh4/net/bluetooth/hidp/core.c	2004-12-14 08:28:20.300042400 +1300
+++ kernel-source-2.6.9-mag-bt/net/bluetooth/hidp/core.c	2004-12-13 22:40:51.000000000 +1300
@@ -46,7 +46,7 @@
 
 #include "hidp.h"
 
-#ifndef CONFIG_BT_HIDP_DEBUG
+#ifndef CONFIG_BT_HIDP_DEBUG_BT
 #undef  BT_DBG
 #define BT_DBG(D...)
 #endif
@@ -138,8 +138,8 @@
 	struct sk_buff *skb;
 	unsigned char newleds;
 
-	BT_DBG("session %p hid %p data %p size %d", session, device, data, size);
-
+	BT_DBG("");
+	 
 	if (type != EV_LED)
 		return -1;
 
@@ -159,7 +159,7 @@
 		return -ENOMEM;
 	}
 
-	*skb_put(skb, 1) = 0xa2;
+	*skb_put(skb, 1) = HIDP_TRANS_DATA | HIDP_DATA_RTYPE_OUPUT;
 	*skb_put(skb, 1) = 0x01;
 	*skb_put(skb, 1) = newleds;
 
@@ -263,7 +263,35 @@
 		del_timer(&session->timer);
 }
 
-static inline void hidp_send_message(struct hidp_session *session, unsigned char hdr)
+static inline int hidp_send_ctrl_message(struct hidp_session *session,
+			unsigned char hdr,
+			unsigned char *data, int size)
+{
+	struct sk_buff *skb;
+
+	BT_DBG("session %p data %p size %d", session, data, size);
+
+	if (!(skb = alloc_skb(size + 1, GFP_ATOMIC))) {
+		BT_ERR("Can't allocate memory for new frame");
+		return -ENOMEM;
+	}
+
+	*skb_put(skb, 1) = hdr;
+	if (size > 0)
+		memcpy(skb_put(skb, size), data, size);
+
+	skb_queue_tail(&session->ctrl_transmit, skb);
+
+	hidp_schedule(session);
+
+	return 0;
+}
+
+/* Send a 1 byte control message.
+ * For calling outside HID kthread process 
+ */
+static inline void hidp_send_ctrl_byte(struct hidp_session *session,
+		 			unsigned char hdr)
 {
 	struct sk_buff *skb;
 
@@ -281,29 +309,205 @@
 	hidp_schedule(session);
 }
 
-static inline int hidp_recv_frame(struct hidp_session *session, struct sk_buff *skb)
+/* Send a 1 byte control message.
+ * For calling inside HID kthread process 
+ */
+static inline void hidp_send_ctrl_reply(struct hidp_session *session,
+		 			unsigned char hdr)
+{
+	struct sk_buff *skb;
+
+	BT_DBG("session %p", session);
+
+	if (!(skb = alloc_skb(1, GFP_ATOMIC))) {
+		BT_ERR("Can't allocate memory for message");
+		return;
+	}
+
+	*skb_put(skb, 1) = hdr;
+
+	skb_queue_tail(&session->ctrl_transmit, skb);
+}
+
+static inline void hidp_process_handshake(struct hidp_session *session, __u8 param)
+{
+	switch (param) {
+	case HIDP_HSHK_SUCCESSFUL:
+		/* Call into SET_ GET_ handlers here */
+		break;
+	case HIDP_HSHK_NOT_READY:
+	case HIDP_HSHK_ERR_INVALID_REPORT_ID:
+	case HIDP_HSHK_ERR_UNSUPPORTED_REQUEST:
+	case HIDP_HSHK_ERR_INVALID_PARAMETER:
+		/* Call into SET_ GET_ handlers here */
+		break;
+	case HIDP_HSHK_ERR_UNKNOWN:
+		BT_INFO("HANDSHAKE parameter ERR_UNKNOWN seen.");
+		break;
+	case HIDP_HSHK_ERR_FATAL:
+		/* Device requests a reboot, as this is the only way this error
+ 		 * can be recovered.
+		 */
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HID_CONTROL | HIDP_CTRL_SOFT_RESET);
+		break;
+	default:
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER);
+		break;
+	}
+
+}
+
+static inline void hidp_process_hid_control (struct hidp_session *session, __u8 param)
+{
+	switch (param) {
+	case HIDP_CTRL_NOP:
+		break;
+	case HIDP_CTRL_VIRTUAL_CABLE_UNPLUG:
+		/* Flush the transmit queues */
+		skb_queue_purge(&session->ctrl_transmit);
+		skb_queue_purge(&session->intr_transmit);
+
+		/* Kill session thread */
+		atomic_inc(&session->terminate);
+
+		/* Do some funky HCI stuff here to delete pairing on dongle? */
+		break;
+	case HIDP_CTRL_HARD_RESET:
+	case HIDP_CTRL_SOFT_RESET:
+	case HIDP_CTRL_SUSPEND:
+	case HIDP_CTRL_EXIT_SUSPEND:
+		/* We have to parse these and return no error */
+		break;
+	default:
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER);
+		break;
+	}
+}
+
+static inline int hidp_process_data(struct hidp_session *session, struct sk_buff *skb, __u8 param)
+{
+	int result = 0;
+
+	BT_INFO("DATA packet on control channel, GET_, SET_ not implemented");
+
+	switch (param) {
+	case HIDP_DATA_RTYPE_INPUT:
+		hidp_set_timer(session);
+
+		if (session->input)
+			hidp_input_report(session, skb);
+
+		if (session->hid) {
+			result = hid_recv_report(session->hid, 
+					HID_INPUT_REPORT, 
+					skb->data, 
+					skb->len);
+			switch (result) {
+			case -EPROTOTYPE:
+				hidp_send_ctrl_reply(session,
+                        		HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_REPORT);
+				break;
+			case -EBADF:
+				hidp_send_ctrl_reply(session,
+                        		HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_REPORT_ID);
+				break;
+			default:
+				break;
+			}
+		}
+		break;
+	case HIDP_DATA_RTYPE_OTHER:
+	case HIDP_DATA_RTYPE_OUPUT:
+	case HIDP_DATA_RTYPE_FEATURE:
+		BT_DBG("Unimplemented DATA parameter 0x%01x", param);
+		break;
+	default:
+		BT_DBG("Invalid DATA parameter 0x%01x", param);
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER);
+	}
+	
+	return 0;
+}
+
+
+static inline int hidp_recv_ctrl_frame(struct hidp_session *session, struct sk_buff *skb)
 {
 	__u8 hdr;
+	__u8 type;
+	__u8 param;
+	int err = 0;
 
 	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 
 	hdr = skb->data[0];
 	skb_pull(skb, 1);
 
-	if (hdr == 0xa1) {
-		hidp_set_timer(session);
+	type = hdr & HIDP_THDR_TRANS_MASK;
+	param = hdr & HIDP_THDR_PARAM_MASK;
 
+	switch (type) {
+	case HIDP_TRANS_HANDSHAKE:
+		hidp_process_handshake(session, param);
+		break;
+	case HIDP_TRANS_HID_CONTROL:
+		hidp_process_hid_control(session, param);
+		break;
+	case HIDP_TRANS_DATA:
+		err = hidp_process_data(session, skb, param);
+		break;
+	default:
+		BT_DBG("Unsupported protocol header 0x%02x", hdr);
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_UNSUPPORTED_REQUEST);
+		break;
+	}
+		
+	kfree_skb(skb);
+	return err;
+}
+
+/* This needs to be separate as there are only DATA and DATC packets on the 
+ * interrupt channel. iData recieved, and no result given, so we swallow errors
+ * and throw out invalid stuff. MAG
+ */ 
+static inline void hidp_recv_intr_frame(struct hidp_session *session, struct sk_buff *skb)
+{
+	__u8 hdr;
+	int err = 0;
+
+	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
+
+	hdr = skb->data[0];
+	skb_pull(skb, 1);
+
+	if (hdr == (HIDP_TRANS_DATA | HIDP_DATA_RTYPE_INPUT)) {
+		hidp_set_timer(session);
 		if (session->input)
 			hidp_input_report(session, skb);
 
 		if (session->hid)
-			hid_recv_report(session->hid, HID_INPUT_REPORT, skb->data, skb->len);
-	} else {
-		BT_DBG("Unsupported protocol header 0x%02x", hdr);
+			err = hid_recv_report(session->hid, 
+					HID_INPUT_REPORT, 
+					skb->data, 
+					skb->len);
+			switch (err) {
+			case -EPROTOTYPE:
+				hidp_send_ctrl_reply(session,
+                        		HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_REPORT);
+				break;
+			default:
+				break;
+			}
 	}
-
+	else {
+		BT_INFO("Unsupported protocol header 0x%02x", hdr);
+	}
+		
 	kfree_skb(skb);
-	return 0;
 }
 
 static int hidp_send_frame(struct socket *sock, unsigned char *data, int len)
@@ -389,12 +593,12 @@
 
 		while ((skb = skb_dequeue(&ctrl_sk->sk_receive_queue))) {
 			skb_orphan(skb);
-			hidp_recv_frame(session, skb);
+			hidp_recv_ctrl_frame(session, skb);
 		}
 
 		while ((skb = skb_dequeue(&intr_sk->sk_receive_queue))) {
 			skb_orphan(skb);
-			hidp_recv_frame(session, skb);
+			hidp_recv_intr_frame(session, skb);
 		}
 
 		hidp_process_transmit(session);
@@ -608,13 +812,23 @@
 		goto unlink;
 
 	if (session->input) {
-		hidp_send_message(session, 0x70);
+		hidp_send_ctrl_byte(session, 
+			HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_BOOT);
 		session->flags |= (1 << HIDP_BOOT_PROTOCOL_MODE);
 
 		session->leds = 0xff;
 		hidp_input_event(session->input, EV_LED, 0, 0);
 	}
 
+	/* Make sure device IS in report mode
+         * Some devices start off in boot protocol, and do not respond correctly 
+	 * to a reset command.
+         */
+	if (session->hid) {
+		hidp_send_ctrl_byte(session,
+                        HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_REPORT);
+	}
+
 	up_write(&hidp_session_sem);
 	return 0;
 
@@ -654,7 +868,8 @@
 	session = __hidp_get_session(&req->bdaddr);
 	if (session) {
 		if (req->flags & (1 << HIDP_VIRTUAL_CABLE_UNPLUG)) {
-			hidp_send_message(session, 0x15);
+			hidp_send_ctrl_byte(session, 
+				HIDP_TRANS_HID_CONTROL | HIDP_CTRL_VIRTUAL_CABLE_UNPLUG);
 		} else {
 			/* Flush the transmit queues */
 			skb_queue_purge(&session->ctrl_transmit);
--- linux-2.6.9-mh4/net/bluetooth/hidp/hid.c	2004-12-14 08:28:20.312040576 +1300
+++ kernel-source-2.6.9-mag-bt/net/bluetooth/hidp/hid.c	2004-12-13 08:21:23.000000000 +1300
@@ -858,11 +858,11 @@
 
 	if (!size) {
 		dbg("empty report");
-		return -1;
+		return -EINVAL;
 	}
 
-#ifdef DEBUG_DATA
-	printk(KERN_DEBUG __FILE__ ": report (size %u) (%snumbered)\n", len, report_enum->numbered ? "" : "un");
+#ifdef CONFIG_BT_HIDP_DEBUG_DATA
+	printk(KERN_DEBUG __FILE__ ": report (size %u) (%snumbered)\n", size, report_enum->numbered ? "" : "un");
 #endif
 
 	n = 0;				/* Normally report number is 0 */
@@ -871,7 +871,7 @@
 		size--;
 	}
 
-#ifdef DEBUG_DATA
+#ifdef CONFIG_BT_HIDP_DEBUG_DATA
 	{
 		int i;
 		printk(KERN_DEBUG __FILE__ ": report %d (size %u) = ", n, size);
@@ -883,14 +883,33 @@
 
 	if (!(report = report_enum->report_id_hash[n])) {
 		dbg("undefined report_id %d received", n);
-		return -1;
+		return -EBADF;
 	}
 
 	rsize = ((report->size - 1) >> 3) + 1;
 
+#ifdef CONFIG_BT_HIDP_DEBUG_DATA
+	dbg("report %d, expected %d bits, %d maxfield", report->id, report->size, report->maxfield);
+	for (n = 0; n < report->maxfield; n++)
+		dbg("report:field %d:%d, count %d , offset %d, size(bits) %d",
+			report->id, n,
+			report->field[n]->report_count,
+			report->field[n]->report_offset,
+			report->field[n]->report_size);
+#endif
+
 	if (size < rsize) {
-		dbg("report %d is too short, (%d < %d)", report->id, size, rsize);
-		return -1;
+		dbg("report %d is too short, (%d < %d) %d bits expected, %d maxfield", report->id, size, rsize, report->size, report->maxfield);
+		if (size == 8) {
+			/* 
+			 * FIXME - need to check if device is a keyboard!!
+			 */
+			dbg("tell upper layer wrong protocol - switch to report"); 
+			return -EPROTOTYPE;
+		}
+		else {
+			return -EMSGSIZE;
+		}
 	}
 
 	for (n = 0; n < report->maxfield; n++)
--- linux-2.6.9-mh4/net/bluetooth/hidp/hid.h	2004-12-14 08:28:20.315040120 +1300
+++ kernel-source-2.6.9-mag-bt/net/bluetooth/hidp/hid.h	2004-11-28 13:20:48.000000000 +1300
@@ -26,7 +26,7 @@
 #ifndef __HID_H
 #define __HID_H
 
-#ifdef DEBUG
+#ifdef CONFIG_BT_HIDP_DEBUG_REPORT
 #define dbg(format, arg...) printk(KERN_DEBUG "%s: " format "\n" , __FILE__ , ## arg)
 #else
 #define dbg(format, arg...) do {} while (0)
--- linux-2.6.9-mh4/net/bluetooth/hidp/hidp.h	2004-12-14 08:28:20.316039968 +1300
+++ kernel-source-2.6.9-mag-bt/net/bluetooth/hidp/hidp.h	2004-12-06 08:29:42.000000000 +1300
@@ -26,6 +26,71 @@
 #include <linux/types.h>
 #include <net/bluetooth/bluetooth.h>
 
+/*
+ * Bluetooth HID packet defines
+ */
+
+/*
+ * HID Transaction Types, and Transaction header stuff
+ */
+#define HIDP_THDR_TRANS_MASK		0xF0
+#define HIDP_THDR_PARAM_MASK		0x0F
+
+#define HIDP_TRANS_HANDSHAKE		0x00
+#define HIDP_TRANS_HID_CONTROL		0x10
+#define HIDP_TRANS_RSRVD_2		0x20
+#define HIDP_TRANS_RSRVD_3		0x30
+#define HIDP_TRANS_GET_REPORT		0x40
+#define HIDP_TRANS_SET_REPORT		0x50
+#define HIDP_TRANS_GET_PROTOCOL		0x60
+#define HIDP_TRANS_SET_PROTOCOL		0x70
+#define HIDP_TRANS_GET_IDLE		0x80
+#define HIDP_TRANS_SET_IDLE		0x90
+#define HIDP_TRANS_DATA			0xA0
+#define HIDP_TRANS_DATC			0xB0
+#define HIDP_TRANS_RSRVD_C		0xC0
+#define HIDP_TRANS_RSRVD_D		0xD0
+#define HIDP_TRANS_RSVRD_E		0xE0
+#define HIDP_TRANS_RSVRD_F		0xF0
+
+/*
+ * HID Handshake results returned in the result parameter of the handshake 
+ * transaction HID packet 
+ */
+#define HIDP_HSHK_SUCCESSFUL			0x00
+#define HIDP_HSHK_NOT_READY			0x01
+#define HIDP_HSHK_ERR_INVALID_REPORT_ID		0x02
+#define HIDP_HSHK_ERR_UNSUPPORTED_REQUEST	0x03
+#define HIDP_HSHK_ERR_INVALID_PARAMETER		0x04
+#define HIDP_HSHK_ERR_UNKNOWN			0x0E
+#define HIDP_HSHK_ERR_FATAL			0x0F
+
+/*
+ * HID HID_CONTROL operation parameter
+ */
+#define HIDP_CTRL_NOP			0x00	/* No operation */
+#define HIDP_CTRL_HARD_RESET		0x01	/* Request hard reset */
+#define HIDP_CTRL_SOFT_RESET		0x02	/* Request soft reset */
+#define HIDP_CTRL_SUSPEND		0x03	/* Request device to suspend */
+#define HIDP_CTRL_EXIT_SUSPEND		0x04	/* request exit suspend */
+#define HIDP_CTRL_VIRTUAL_CABLE_UNPLUG	0x05	/* only one Mouse/kbd can send */
+
+/*
+ * HID DATA Transaction header parameter nibble
+ */
+#define HIDP_DATA_RTYPE_MASK		0x03
+#define HIDP_DATA_RSRVD_MASK		0x0C
+#define HIDP_DATA_RTYPE_OTHER		0x00
+#define HIDP_DATA_RTYPE_INPUT		0x01
+#define HIDP_DATA_RTYPE_OUPUT		0x02
+#define HIDP_DATA_RTYPE_FEATURE		0x03
+
+/*
+ * HID SET_PROTOCOL header parameter nibble
+ */
+#define HIDP_PROTO_BOOT			0x00
+#define HIDP_PROTO_REPORT		0x01
+
 /* HIDP ioctl defines */
 #define HIDPCONNADD	_IOW('H', 200, int)
 #define HIDPCONNDEL	_IOW('H', 201, int)
--- linux-2.6.9-mh4/net/bluetooth/hidp/sock.c	2004-10-19 10:55:07.000000000 +1300
+++ kernel-source-2.6.9-mag-bt/net/bluetooth/hidp/sock.c	2004-11-19 20:44:16.000000000 +1300
@@ -40,7 +40,7 @@
 
 #include "hidp.h"
 
-#ifndef CONFIG_BT_HIDP_DEBUG
+#ifndef CONFIG_BT_HIDP_DEBUG_BT
 #undef  BT_DBG
 #define BT_DBG(D...)
 #endif

[-- Attachment #1.3: linux-2.6.10-rc3-bk6-bthid-2004121501.patch --]
[-- Type: text/x-patch, Size: 10814 bytes --]

Signed-off-by: Matthew Grant <grantma@anathoth.gen.nz>

Updates to BT HID for phase 1 of implementation of BT HID spec.

Introduces changes to incoming packet processing, and parsing structure
for recieved messages.

diff -uNr linux-2.6.10-rc3-bk6/net/bluetooth/hidp/core.c linux-2.6.10-rc3-bk6-magbthid/net/bluetooth/hidp/core.c
--- linux-2.6.10-rc3-bk6/net/bluetooth/hidp/core.c	2004-10-19 10:54:55.000000000 +1300
+++ linux-2.6.10-rc3-bk6-magbthid/net/bluetooth/hidp/core.c	2004-12-15 09:05:22.954391480 +1300
@@ -45,7 +45,7 @@
 
 #include "hidp.h"
 
-#ifndef CONFIG_BT_HIDP_DEBUG
+#ifndef CONFIG_BT_HIDP_DEBUG_BT
 #undef  BT_DBG
 #define BT_DBG(D...)
 #endif
@@ -130,8 +130,8 @@
 	struct sk_buff *skb;
 	unsigned char newleds;
 
-	BT_DBG("session %p hid %p data %p size %d", session, device, data, size);
-
+	BT_DBG("");
+	 
 	if (type != EV_LED)
 		return -1;
 
@@ -151,7 +151,7 @@
 		return -ENOMEM;
 	}
 
-	*skb_put(skb, 1) = 0xa2;
+	*skb_put(skb, 1) = HIDP_TRANS_DATA | HIDP_DATA_RTYPE_OUPUT;
 	*skb_put(skb, 1) = 0x01;
 	*skb_put(skb, 1) = newleds;
 
@@ -232,7 +232,35 @@
 		del_timer(&session->timer);
 }
 
-static inline void hidp_send_message(struct hidp_session *session, unsigned char hdr)
+static inline int hidp_send_ctrl_message(struct hidp_session *session,
+			unsigned char hdr,
+			unsigned char *data, int size)
+{
+	struct sk_buff *skb;
+
+	BT_DBG("session %p data %p size %d", session, data, size);
+
+	if (!(skb = alloc_skb(size + 1, GFP_ATOMIC))) {
+		BT_ERR("Can't allocate memory for new frame");
+		return -ENOMEM;
+	}
+
+	*skb_put(skb, 1) = hdr;
+	if (size > 0)
+		memcpy(skb_put(skb, size), data, size);
+
+	skb_queue_tail(&session->ctrl_transmit, skb);
+
+	hidp_schedule(session);
+
+	return 0;
+}
+
+/* Send a 1 byte control message.
+ * For calling outside HID kthread process 
+ */
+static inline void hidp_send_ctrl_byte(struct hidp_session *session,
+		 			unsigned char hdr)
 {
 	struct sk_buff *skb;
 
@@ -250,26 +278,172 @@
 	hidp_schedule(session);
 }
 
-static inline int hidp_recv_frame(struct hidp_session *session, struct sk_buff *skb)
+/* Send a 1 byte control message.
+ * For calling inside HID kthread process 
+ */
+static inline void hidp_send_ctrl_reply(struct hidp_session *session,
+		 			unsigned char hdr)
+{
+	struct sk_buff *skb;
+
+	BT_DBG("session %p", session);
+
+	if (!(skb = alloc_skb(1, GFP_ATOMIC))) {
+		BT_ERR("Can't allocate memory for message");
+		return;
+	}
+
+	*skb_put(skb, 1) = hdr;
+
+	skb_queue_tail(&session->ctrl_transmit, skb);
+}
+
+static inline void hidp_process_handshake(struct hidp_session *session, __u8 param)
+{
+	switch (param) {
+	case HIDP_HSHK_SUCCESSFUL:
+		/* Call into SET_ GET_ handlers here */
+		break;
+	case HIDP_HSHK_NOT_READY:
+	case HIDP_HSHK_ERR_INVALID_REPORT_ID:
+	case HIDP_HSHK_ERR_UNSUPPORTED_REQUEST:
+	case HIDP_HSHK_ERR_INVALID_PARAMETER:
+		/* Call into SET_ GET_ handlers here */
+		break;
+	case HIDP_HSHK_ERR_UNKNOWN:
+		BT_INFO("HANDSHAKE parameter ERR_UNKNOWN seen.");
+		break;
+	case HIDP_HSHK_ERR_FATAL:
+		/* Device requests a reboot, as this is the only way this error
+ 		 * can be recovered.
+		 */
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HID_CONTROL | HIDP_CTRL_SOFT_RESET);
+		break;
+	default:
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER);
+		break;
+	}
+
+}
+
+static inline void hidp_process_hid_control (struct hidp_session *session, __u8 param)
+{
+	switch (param) {
+	case HIDP_CTRL_NOP:
+		break;
+	case HIDP_CTRL_VIRTUAL_CABLE_UNPLUG:
+		/* Flush the transmit queues */
+		skb_queue_purge(&session->ctrl_transmit);
+		skb_queue_purge(&session->intr_transmit);
+
+		/* Kill session thread */
+		atomic_inc(&session->terminate);
+
+		/* Do some funky HCI stuff here to delete pairing on dongle? */
+		break;
+	case HIDP_CTRL_HARD_RESET:
+	case HIDP_CTRL_SOFT_RESET:
+	case HIDP_CTRL_SUSPEND:
+	case HIDP_CTRL_EXIT_SUSPEND:
+		/* We have to parse these and return no error */
+		break;
+	default:
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER);
+		break;
+	}
+}
+
+static inline int hidp_process_data(struct hidp_session *session, struct sk_buff *skb, __u8 param)
+{
+	int result = 0;
+
+	BT_INFO("DATA packet on control channel, GET_, SET_ not implemented");
+
+	switch (param) {
+	case HIDP_DATA_RTYPE_INPUT:
+		hidp_set_timer(session);
+
+		if (session->input)
+			hidp_input_report(session, skb);
+		break;
+	case HIDP_DATA_RTYPE_OTHER:
+	case HIDP_DATA_RTYPE_OUPUT:
+	case HIDP_DATA_RTYPE_FEATURE:
+		BT_DBG("Unimplemented DATA parameter 0x%01x", param);
+		break;
+	default:
+		BT_DBG("Invalid DATA parameter 0x%01x", param);
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER);
+	}
+	
+	return 0;
+}
+
+
+static inline int hidp_recv_ctrl_frame(struct hidp_session *session, struct sk_buff *skb)
 {
 	__u8 hdr;
+	__u8 type;
+	__u8 param;
+	int err = 0;
 
 	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 
 	hdr = skb->data[0];
 	skb_pull(skb, 1);
 
-	if (hdr == 0xa1) {
-		hidp_set_timer(session);
+	type = hdr & HIDP_THDR_TRANS_MASK;
+	param = hdr & HIDP_THDR_PARAM_MASK;
 
-		if (session->input)
-			hidp_input_report(session, skb);
-	} else {
+	switch (type) {
+	case HIDP_TRANS_HANDSHAKE:
+		hidp_process_handshake(session, param);
+		break;
+	case HIDP_TRANS_HID_CONTROL:
+		hidp_process_hid_control(session, param);
+		break;
+	case HIDP_TRANS_DATA:
+		err = hidp_process_data(session, skb, param);
+		break;
+	default:
 		BT_DBG("Unsupported protocol header 0x%02x", hdr);
+		hidp_send_ctrl_reply(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_UNSUPPORTED_REQUEST);
+		break;
 	}
+		
+	kfree_skb(skb);
+	return err;
+}
+
+/* This needs to be separate as there are only DATA and DATC packets on the 
+ * interrupt channel. iData recieved, and no result given, so we swallow errors
+ * and throw out invalid stuff. MAG
+ */ 
+static inline void hidp_recv_intr_frame(struct hidp_session *session, struct sk_buff *skb)
+{
+	__u8 hdr;
+	int err = 0;
 
+	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
+
+	hdr = skb->data[0];
+	skb_pull(skb, 1);
+
+	if (hdr == (HIDP_TRANS_DATA | HIDP_DATA_RTYPE_INPUT)) {
+		hidp_set_timer(session);
+		if (session->input)
+			hidp_input_report(session, skb);
+	}
+	else {
+		BT_INFO("Unsupported protocol header 0x%02x", hdr);
+	}
+		
 	kfree_skb(skb);
-	return 0;
 }
 
 static int hidp_send_frame(struct socket *sock, unsigned char *data, int len)
@@ -350,12 +524,12 @@
 
 		while ((skb = skb_dequeue(&ctrl_sk->sk_receive_queue))) {
 			skb_orphan(skb);
-			hidp_recv_frame(session, skb);
+			hidp_recv_ctrl_frame(session, skb);
 		}
 
 		while ((skb = skb_dequeue(&intr_sk->sk_receive_queue))) {
 			skb_orphan(skb);
-			hidp_recv_frame(session, skb);
+			hidp_recv_intr_frame(session, skb);
 		}
 
 		hidp_process_transmit(session);
@@ -514,7 +688,8 @@
 		goto unlink;
 
 	if (session->input) {
-		hidp_send_message(session, 0x70);
+		hidp_send_ctrl_byte(session, 
+			HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_BOOT);
 		session->flags |= (1 << HIDP_BOOT_PROTOCOL_MODE);
 
 		session->leds = 0xff;
@@ -554,7 +729,8 @@
 	session = __hidp_get_session(&req->bdaddr);
 	if (session) {
 		if (req->flags & (1 << HIDP_VIRTUAL_CABLE_UNPLUG)) {
-			hidp_send_message(session, 0x15);
+			hidp_send_ctrl_byte(session, 
+				HIDP_TRANS_HID_CONTROL | HIDP_CTRL_VIRTUAL_CABLE_UNPLUG);
 		} else {
 			/* Flush the transmit queues */
 			skb_queue_purge(&session->ctrl_transmit);
diff -uNr linux-2.6.10-rc3-bk6/net/bluetooth/hidp/hidp.h linux-2.6.10-rc3-bk6-magbthid/net/bluetooth/hidp/hidp.h
--- linux-2.6.10-rc3-bk6/net/bluetooth/hidp/hidp.h	2004-10-19 10:53:50.000000000 +1300
+++ linux-2.6.10-rc3-bk6-magbthid/net/bluetooth/hidp/hidp.h	2004-12-14 08:47:42.000000000 +1300
@@ -26,6 +26,71 @@
 #include <linux/types.h>
 #include <net/bluetooth/bluetooth.h>
 
+/*
+ * Bluetooth HID packet defines
+ */
+
+/*
+ * HID Transaction Types, and Transaction header stuff
+ */
+#define HIDP_THDR_TRANS_MASK		0xF0
+#define HIDP_THDR_PARAM_MASK		0x0F
+
+#define HIDP_TRANS_HANDSHAKE		0x00
+#define HIDP_TRANS_HID_CONTROL		0x10
+#define HIDP_TRANS_RSRVD_2		0x20
+#define HIDP_TRANS_RSRVD_3		0x30
+#define HIDP_TRANS_GET_REPORT		0x40
+#define HIDP_TRANS_SET_REPORT		0x50
+#define HIDP_TRANS_GET_PROTOCOL		0x60
+#define HIDP_TRANS_SET_PROTOCOL		0x70
+#define HIDP_TRANS_GET_IDLE		0x80
+#define HIDP_TRANS_SET_IDLE		0x90
+#define HIDP_TRANS_DATA			0xA0
+#define HIDP_TRANS_DATC			0xB0
+#define HIDP_TRANS_RSRVD_C		0xC0
+#define HIDP_TRANS_RSRVD_D		0xD0
+#define HIDP_TRANS_RSVRD_E		0xE0
+#define HIDP_TRANS_RSVRD_F		0xF0
+
+/*
+ * HID Handshake results returned in the result parameter of the handshake 
+ * transaction HID packet 
+ */
+#define HIDP_HSHK_SUCCESSFUL			0x00
+#define HIDP_HSHK_NOT_READY			0x01
+#define HIDP_HSHK_ERR_INVALID_REPORT_ID		0x02
+#define HIDP_HSHK_ERR_UNSUPPORTED_REQUEST	0x03
+#define HIDP_HSHK_ERR_INVALID_PARAMETER		0x04
+#define HIDP_HSHK_ERR_UNKNOWN			0x0E
+#define HIDP_HSHK_ERR_FATAL			0x0F
+
+/*
+ * HID HID_CONTROL operation parameter
+ */
+#define HIDP_CTRL_NOP			0x00	/* No operation */
+#define HIDP_CTRL_HARD_RESET		0x01	/* Request hard reset */
+#define HIDP_CTRL_SOFT_RESET		0x02	/* Request soft reset */
+#define HIDP_CTRL_SUSPEND		0x03	/* Request device to suspend */
+#define HIDP_CTRL_EXIT_SUSPEND		0x04	/* request exit suspend */
+#define HIDP_CTRL_VIRTUAL_CABLE_UNPLUG	0x05	/* only one Mouse/kbd can send */
+
+/*
+ * HID DATA Transaction header parameter nibble
+ */
+#define HIDP_DATA_RTYPE_MASK		0x03
+#define HIDP_DATA_RSRVD_MASK		0x0C
+#define HIDP_DATA_RTYPE_OTHER		0x00
+#define HIDP_DATA_RTYPE_INPUT		0x01
+#define HIDP_DATA_RTYPE_OUPUT		0x02
+#define HIDP_DATA_RTYPE_FEATURE		0x03
+
+/*
+ * HID SET_PROTOCOL header parameter nibble
+ */
+#define HIDP_PROTO_BOOT			0x00
+#define HIDP_PROTO_REPORT		0x01
+
 /* HIDP ioctl defines */
 #define HIDPCONNADD	_IOW('H', 200, int)
 #define HIDPCONNDEL	_IOW('H', 201, int)
diff -uNr linux-2.6.10-rc3-bk6/net/bluetooth/hidp/sock.c linux-2.6.10-rc3-bk6-magbthid/net/bluetooth/hidp/sock.c
--- linux-2.6.10-rc3-bk6/net/bluetooth/hidp/sock.c	2004-10-19 10:55:07.000000000 +1300
+++ linux-2.6.10-rc3-bk6-magbthid/net/bluetooth/hidp/sock.c	2004-12-14 08:47:42.000000000 +1300
@@ -40,7 +40,7 @@
 
 #include "hidp.h"
 
-#ifndef CONFIG_BT_HIDP_DEBUG
+#ifndef CONFIG_BT_HIDP_DEBUG_BT
 #undef  BT_DBG
 #define BT_DBG(D...)
 #endif

[-- Attachment #2: This is a digitally signed message part --]
[-- Type: application/pgp-signature, Size: 189 bytes --]