hid-u2f: New driver

Signed-off-by: Andy Lutomirski <luto@amacapital.net>
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index f42df4d..cfc87e1 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -774,6 +774,17 @@
 	  a THRUSTMASTER Dual Trigger 3-in-1 or a THRUSTMASTER Ferrari GT
 	  Rumble Force or Force Feedback Wheel.
 
+config HID_U2F
+       tristate "FIDO U2F HID device support"
+       depends on HID
+       ---help---
+         This driver supports "Universal 2nd Factor" devices compliant
+	 with the FIDO Alliance specification.
+
+	 In contrast to accessing these devices using hidraw, the U2F
+	 driver provides real enumeration, a character device, and proper
+	 isolation between client applications.
+
 config HID_WACOM
 	tristate "Wacom Intuos/Graphire tablet support (USB)"
 	depends on HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index e2850d8..72a869d 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -113,6 +113,7 @@
 obj-$(CONFIG_HID_TIVO)		+= hid-tivo.o
 obj-$(CONFIG_HID_TOPSEED)	+= hid-topseed.o
 obj-$(CONFIG_HID_TWINHAN)	+= hid-twinhan.o
+obj-$(CONFIG_HID_U2F)		+= hid-u2f.o
 obj-$(CONFIG_HID_UCLOGIC)	+= hid-uclogic.o
 obj-$(CONFIG_HID_XINMO)		+= hid-xinmo.o
 obj-$(CONFIG_HID_ZEROPLUS)	+= hid-zpff.o
diff --git a/drivers/hid/hid-u2f.c b/drivers/hid/hid-u2f.c
new file mode 100644
index 0000000..6289753
--- /dev/null
+++ b/drivers/hid/hid-u2f.c
@@ -0,0 +1,388 @@
+/*
+ * FIDO Alliance U2F driver
+ *
+ * Copyright (c) 2014 Andy Lutomirski
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include "hid-ids.h"
+
+#define U2FHID_FRAME_TYPE_MASK	0x80
+#define U2FHID_FRAME_TYPE_INIT	0x80
+#define U2FHID_FRAME_TYPE_CONT	0x00
+
+#define U2FHID_CMD_INIT		0x06
+
+#define U2FHID_REPORT_SIZE	64
+
+struct u2fhid_header {
+	u8 channel_id[4];
+	u8 type;
+} __packed;
+
+struct u2fhid_frame {
+	u8 channel_id[4];
+
+	/*
+	 * If the high bit is set, this is an INIT (initial) frame.  If not,
+	 * then this is a CONT (continuation) frame.
+	 */
+	u8 type;
+	union {
+		struct {
+			__be16 len;
+			u8 data[U2FHID_REPORT_SIZE - 7];
+		} __packed init;
+
+		struct {
+			u8 data[U2FHID_REPORT_SIZE - 5];
+		} __packed cont;
+	} __packed;
+} __packed;
+
+struct u2fhid_init_req {
+	u8 nonce[8];
+} __packed;
+
+struct u2fhid_init_resp {
+	u8 nonce[8];
+	u8 channel_id[4];
+	u8 u2fhid_version;
+	u8 major_device_version;
+	u8 minor_device_version;
+	u8 build_device_version;
+	u8 capflags;
+};
+
+struct u2f {
+	u8 channel_id[4];
+
+	spinlock_t lock;
+
+	wait_queue_head_t wq;
+
+	/* Any change due to data from the device will wake wq. */
+	int is_receiving;
+
+	/* Valid iff is_receiving. */
+	struct u2f_recv {
+		void *buffer;	/* NULL if the receive has been abandoned. */
+		size_t buffer_len;
+
+		bool got_first;
+		size_t total_len;
+		size_t len_received;
+		u8 response_type;	/* only if got_first */
+
+		bool check_nonce;
+		u8 nonce[8];
+	} recv;
+};
+
+/*
+static struct class *u2f_class;
+
+static const struct attribute_group *u2f_groups[] = {
+	NULL,
+};
+*/
+
+static int u2f_sendrecv(struct hid_device *hdev, struct u2f *u2f,
+			u8 cmd, const void *out, int outlen,
+			void *in, int inlen)
+{
+	int ret;
+	struct {
+		u8 reportnum;
+		struct u2fhid_frame frame;
+	} __packed frame;
+
+	BUILD_BUG_ON(sizeof(struct u2fhid_frame) != U2FHID_REPORT_SIZE);
+	BUILD_BUG_ON(sizeof(frame) != U2FHID_REPORT_SIZE + 1);
+
+	memset(&frame, 0, sizeof(frame));
+	memcpy(frame.frame.channel_id, u2f->channel_id, 4);
+	frame.frame.type = cmd | U2FHID_FRAME_TYPE_INIT;
+	frame.frame.init.len = cpu_to_be16(outlen);
+	memcpy(frame.frame.init.data, out, outlen);
+
+	// TODO: Handle send fragmentation
+
+	//print_hex_dump(KERN_ERR, "u2f: ", DUMP_PREFIX_OFFSET,
+	//	       16, 1, &frame, sizeof(frame), 0);
+
+	spin_lock_irq(&u2f->lock);
+
+	ACCESS_ONCE(u2f->is_receiving) = 1;
+	memset(&u2f->recv, 0, sizeof(u2f->recv));
+	u2f->recv.buffer = in;
+	u2f->recv.buffer_len = inlen;
+
+	BUG_ON(!ACCESS_ONCE(u2f->is_receiving));
+
+	spin_unlock_irq(&u2f->lock);
+
+	ret = hid_hw_output_report(hdev, (u8 *)&frame, sizeof(frame));
+	if (ret < 0) {
+		dev_err(&hdev->dev, "send failed");
+		return ret;
+	}
+
+	wait_event_interruptible(u2f->wq, !ACCESS_ONCE(u2f->is_receiving));
+
+	spin_lock_irq(&u2f->lock);
+	u2f->recv.buffer = NULL;
+	ret = u2f->recv.total_len;
+	spin_unlock_irq(&u2f->lock);
+
+	return ret;
+}
+
+static int u2f_init_channel(struct hid_device *hdev, struct u2f *u2f)
+{
+	int ret;
+	struct u2fhid_init_req initreq;
+	struct u2fhid_init_resp initresp;
+
+	/* Until we are assigned a channel, use 0xffffffff. */
+	memset(u2f->channel_id, 0xff, sizeof(u2f->channel_id));
+	
+	get_random_bytes(initreq.nonce, sizeof(initreq.nonce));
+
+	ret = u2f_sendrecv(hdev, u2f, U2FHID_CMD_INIT,
+			   &initreq, sizeof(initreq),
+			   &initresp, sizeof(initresp));
+	if (ret < 0)
+		return ret;
+	if (ret < sizeof(struct u2fhid_init_resp)) {
+		dev_err(&hdev->dev, "U2FHID_INIT response was too short\n");
+		return -EIO;
+	}
+	if (memcmp(initresp.nonce, initreq.nonce, sizeof(initreq.nonce))) {
+		/*
+		 * This indicates a race against a hidraw user.  We could
+		 * add code to survive the race, but the race is unlikely,
+		 * so just bail if it happens.
+		 */
+		dev_err(&hdev->dev, "U2FHID_INIT nonce mismatch\n");
+		return -EIO;
+	}
+
+	memcpy(u2f->channel_id, &initresp.channel_id, sizeof(u2f->channel_id));
+
+	dev_info(&hdev->dev, "U2FHID v%d, device v%d.%d.%d, caps 0x%x\n",
+		 (int)initresp.u2fhid_version,
+		 (int)initresp.major_device_version,
+		 (int)initresp.minor_device_version,
+		 (int)initresp.build_device_version,
+		 (int)initresp.capflags);
+
+	return 0;
+}
+
+static int u2f_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int retval;
+	struct u2f *u2f;
+
+	retval = hid_parse(hdev);
+	if (retval) {
+		hid_err(hdev, "parse failed\n");
+		goto exit;
+	}
+
+	/*
+	 * U2F isn't an input device, and it uses raw reports, so trying
+	 * to access it with hiddev makes no sense.
+	 */
+	retval = hid_hw_start(hdev, HID_CONNECT_DRIVER | HID_CONNECT_HIDRAW);
+	if (retval) {
+		hid_err(hdev, "hw start failed\n");
+		goto exit;
+	}
+
+	u2f = devm_kzalloc(&hdev->dev, sizeof(*u2f), GFP_KERNEL);
+	if (!u2f) {
+		retval = -ENOMEM;
+		goto exit_stop;
+	}
+
+	spin_lock_init(&u2f->lock);
+	init_waitqueue_head(&u2f->wq);
+
+	hid_set_drvdata(hdev, u2f);
+
+	hid_device_io_start(hdev);
+	retval = hid_hw_open(hdev);
+	if (retval) {
+		dev_err(&hdev->dev, "failed to open device");
+		goto exit_stop;
+	}
+
+	retval = u2f_init_channel(hdev, u2f);
+	if (retval != 0)
+		goto exit_close;
+
+	return 0;
+
+exit_close:
+	hid_hw_close(hdev);
+	hid_device_io_stop(hdev);
+exit_stop:
+	hid_hw_stop(hdev);
+exit:
+	return retval;
+}
+
+static void u2f_remove(struct hid_device *hdev)
+{
+	hid_hw_close(hdev);
+	hid_hw_stop(hdev);
+}
+
+static int u2f_raw_event(struct hid_device *hdev,
+			 struct hid_report *report, u8 *data, int size)
+{
+	struct u2f *u2f = hid_get_drvdata(hdev);
+	struct u2fhid_header *header;
+	unsigned long flags;
+	void *payload;
+	size_t payload_len;
+	
+	//dev_info(&hdev->dev, "Got %d bytes\n", size);
+
+	//print_hex_dump(KERN_ERR, "resp: ", DUMP_PREFIX_OFFSET,
+	//	       16, 1, data, size, 0);
+
+	if (size < sizeof(struct u2fhid_header))
+		return 0;
+
+	spin_lock_irqsave(&u2f->lock, flags);
+
+	if (!u2f->is_receiving)
+		goto out;
+
+	header = (struct u2fhid_header *)data;
+	if (memcmp(header->channel_id, u2f->channel_id, 4) != 0)
+		goto out;
+
+	/* Handle the frame header. */
+	if (!u2f->recv.got_first) {
+		if ((header->type & U2FHID_FRAME_TYPE_MASK) !=
+		    U2FHID_FRAME_TYPE_INIT) {
+			dev_err(&hdev->dev, "got continuation instead of init\n");
+			/* XXX: error out? */
+			goto out;
+		}
+
+		if (size < sizeof(struct u2fhid_header) + 2) {
+			dev_err(&hdev->dev, "got short frame\n");
+			goto out;
+		}
+
+		u2f->recv.response_type =
+			header->type & ~U2FHID_FRAME_TYPE_MASK;
+		u2f->recv.total_len =
+			be16_to_cpu(*(__be16*)(header + 1));
+		u2f->recv.got_first = true;
+
+		payload = data + (sizeof(struct u2fhid_header) + 2);
+		payload_len = size - (sizeof(struct u2fhid_header) + 2);
+	} else {
+		if (header->type !=
+		    (u2f->recv.response_type | U2FHID_FRAME_TYPE_CONT)) {
+			dev_err(&hdev->dev, "bad continuation\n");
+			goto out;
+		}
+
+		payload = header + 1;
+		payload_len = size - sizeof(struct u2fhid_header);
+	}
+
+	/* Truncate excess data. */
+	if (payload_len > u2f->recv.total_len - u2f->recv.len_received)
+		payload_len = u2f->recv.total_len - u2f->recv.len_received;
+
+	/* Store the payload, if appropriate. */
+	if (u2f->recv.buffer && u2f->recv.len_received < u2f->recv.buffer_len) {
+		memcpy(u2f->recv.buffer + u2f->recv.len_received,
+		       payload,
+		       min(payload_len,
+			   u2f->recv.buffer_len - u2f->recv.len_received));
+	}
+	u2f->recv.len_received += payload_len;
+
+	if (u2f->recv.len_received == u2f->recv.total_len) {
+		ACCESS_ONCE(u2f->is_receiving) = 0;
+		wake_up_all(&u2f->wq);
+	}
+	
+out:
+	spin_unlock_irqrestore(&u2f->lock, flags);
+	
+	return 0;
+}
+
+static const struct hid_device_id u2f_devices[] = {
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_FIDO_U2F, HID_ANY_ID, HID_ANY_ID) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, u2f_devices);
+
+static struct hid_driver u2f_driver = {
+		.name = "u2f",
+		.id_table = u2f_devices,
+		.probe = u2f_probe,
+		.remove = u2f_remove,
+		.raw_event = u2f_raw_event
+};
+
+static int __init u2f_init(void)
+{
+	int retval;
+
+	/*
+	u2f_class = class_create(THIS_MODULE, "u2f");
+	if (IS_ERR(u2f_class))
+		return PTR_ERR(u2f_class);
+	u2f_class->dev_groups = u2f_groups;
+	*/
+
+	retval = hid_register_driver(&u2f_driver);
+	/*
+	if (retval)
+		class_destroy(u2f_class);
+	*/
+	return retval;
+}
+
+static void __exit u2f_exit(void)
+{
+	hid_unregister_driver(&u2f_driver);
+	//class_destroy(u2f_class);
+}
+
+module_init(u2f_init);
+module_exit(u2f_exit);
+
+MODULE_AUTHOR("Andy Lutomirski");
+MODULE_DESCRIPTION("FIDO U2F HID Driver");
+MODULE_LICENSE("GPL v2");