[Bluetooth] Add HIDP message parsing

This patch introduces changes to incoming packet processing and
parsing structure for recieved messages.
Signed-off-by: Matthew Grant <grantma@anathoth.gen.nz>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

net/bluetooth/hidp/core.c

@@ -50,7 +50,7 @@
 #define BT_DBG(D...)
 #endif
 
-#define VERSION "1.0"
+#define VERSION "1.1"
 
 static DECLARE_RWSEM(hidp_session_sem);
 static LIST_HEAD(hidp_session_list);
@@ -130,7 +130,7 @@ static int hidp_input_event(struct input_dev *dev, unsigned int type, unsigned i
 	struct sk_buff *skb;
 	unsigned char newleds;
 
-	BT_DBG("session %p hid %p data %p size %d", session, device, data, size);
+	BT_DBG("input %p type %d code %d value %d", dev, type, code, value);
 
 	if (type != EV_LED)
 		return -1;
@@ -151,7 +151,7 @@ static int hidp_input_event(struct input_dev *dev, unsigned int type, unsigned i
 		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,36 +232,171 @@ static inline void hidp_del_timer(struct hidp_session *session)
 		del_timer(&session->timer);
 }
 
-static inline void hidp_send_message(struct hidp_session *session, unsigned char hdr)
+static 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", session);
+	BT_DBG("session %p data %p size %d", session, data, size);
 
-	if (!(skb = alloc_skb(1, GFP_ATOMIC))) {
-		BT_ERR("Can't allocate memory for message");
-		return;
+	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 (data && size > 0)
+		memcpy(skb_put(skb, size), data, size);
 
 	skb_queue_tail(&session->ctrl_transmit, skb);
 
+	return 0;
+}
+
+static int inline hidp_send_ctrl_message(struct hidp_session *session,
+			unsigned char hdr, unsigned char *data, int size)
+{
+	int err;
+
+	err = __hidp_send_ctrl_message(session, hdr, data, size);
+
 	hidp_schedule(session);
+
+	return err;
+}
+
+static inline void hidp_process_handshake(struct hidp_session *session, unsigned char param)
+{
+	BT_DBG("session %p param 0x%02x", session, param);
+
+	switch (param) {
+	case HIDP_HSHK_SUCCESSFUL:
+		/* FIXME: 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:
+		/* FIXME: Call into SET_ GET_ handlers here */
+		break;
+
+	case HIDP_HSHK_ERR_UNKNOWN:
+		break;
+
+	case HIDP_HSHK_ERR_FATAL:
+		/* Device requests a reboot, as this is the only way this error
+ 		 * can be recovered. */
+		__hidp_send_ctrl_message(session,
+			HIDP_TRANS_HID_CONTROL | HIDP_CTRL_SOFT_RESET, NULL, 0);
+		break;
+
+	default:
+		__hidp_send_ctrl_message(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0);
+		break;
+	}
+}
+
+static inline void hidp_process_hid_control(struct hidp_session *session, unsigned char param)
+{
+	BT_DBG("session %p param 0x%02x", session, 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);
+		break;
+
+	case HIDP_CTRL_HARD_RESET:
+	case HIDP_CTRL_SOFT_RESET:
+	case HIDP_CTRL_SUSPEND:
+	case HIDP_CTRL_EXIT_SUSPEND:
+		/* FIXME: We have to parse these and return no error */
+		break;
+
+	default:
+		__hidp_send_ctrl_message(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0);
+		break;
+	}
 }
 
-static inline int hidp_recv_frame(struct hidp_session *session, struct sk_buff *skb)
+static inline void hidp_process_data(struct hidp_session *session, struct sk_buff *skb, unsigned char param)
 {
-	__u8 hdr;
+	BT_DBG("session %p skb %p len %d param 0x%02x", session, skb, skb->len, param);
+
+	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:
+		break;
+
+	default:
+		__hidp_send_ctrl_message(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0);
+	}
+}
+
+static inline void hidp_recv_ctrl_frame(struct hidp_session *session, struct sk_buff *skb)
+{
+	unsigned char hdr, type, param;
 
 	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_HEADER_TRANS_MASK;
+	param = hdr & HIDP_HEADER_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:
+		hidp_process_data(session, skb, param);
+		break;
+
+	default:
+		__hidp_send_ctrl_message(session,
+			HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_UNSUPPORTED_REQUEST, NULL, 0);
+		break;
+	}
+
+	kfree_skb(skb);
+}
+
+static inline void hidp_recv_intr_frame(struct hidp_session *session, struct sk_buff *skb)
+{
+	unsigned char hdr;
+
+	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 {
@@ -269,7 +404,6 @@ static inline int hidp_recv_frame(struct hidp_session *session, struct sk_buff *
 	}
 
 	kfree_skb(skb);
-	return 0;
 }
 
 static int hidp_send_frame(struct socket *sock, unsigned char *data, int len)
@@ -350,12 +484,12 @@ static int hidp_session(void *arg)
 
 		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 +648,8 @@ int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock,
 		goto unlink;
 
 	if (session->input) {
-		hidp_send_message(session, 0x70);
+		hidp_send_ctrl_message(session,
+			HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_BOOT, NULL, 0);
 		session->flags |= (1 << HIDP_BOOT_PROTOCOL_MODE);
 
 		session->leds = 0xff;
@@ -554,7 +689,8 @@ int hidp_del_connection(struct hidp_conndel_req *req)
 	session = __hidp_get_session(&req->bdaddr);
 	if (session) {
 		if (req->flags & (1 << HIDP_VIRTUAL_CABLE_UNPLUG)) {
-			hidp_send_message(session, 0x15);
+			hidp_send_ctrl_message(session,
+				HIDP_TRANS_HID_CONTROL | HIDP_CTRL_VIRTUAL_CABLE_UNPLUG, NULL, 0);
 		} else {
 			/* Flush the transmit queues */
 			skb_queue_purge(&session->ctrl_transmit);

net/bluetooth/hidp/hidp.h

@@ -26,6 +26,51 @@
 #include <linux/types.h>
 #include <net/bluetooth/bluetooth.h>
 
+/* HIDP header masks */
+#define HIDP_HEADER_TRANS_MASK			0xf0
+#define HIDP_HEADER_PARAM_MASK			0x0f
+
+/* HIDP transaction types */
+#define HIDP_TRANS_HANDSHAKE			0x00
+#define HIDP_TRANS_HID_CONTROL			0x10
+#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
+
+/* HIDP handshake results */
+#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
+
+/* HIDP control operation parameters */
+#define HIDP_CTRL_NOP				0x00
+#define HIDP_CTRL_HARD_RESET			0x01
+#define HIDP_CTRL_SOFT_RESET			0x02
+#define HIDP_CTRL_SUSPEND			0x03
+#define HIDP_CTRL_EXIT_SUSPEND			0x04
+#define HIDP_CTRL_VIRTUAL_CABLE_UNPLUG		0x05
+
+/* HIDP data transaction headers */
+#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
+
+/* HIDP protocol header parameters */
+#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)