misc: nv-sensorhub: tty line disc for sensorhub

Line discipline driver for sensorhub stream.

Bug 1591540

Change-Id: Ic867019b4d06091d27b55ee2560fc337e2e59523
Signed-off-by: Arun Kannan <akannan@nvidia.com>
Reviewed-on: http://git-master/r/664091
GVS: Gerrit_Virtual_Submit
Reviewed-by: Kamal Balagopalan <kbalagopalan@nvidia.com>
Tested-by: Kamal Balagopalan <kbalagopalan@nvidia.com>
Reviewed-by: Venkat Moganty <vmoganty@nvidia.com>

7 files changed, 912 insertions, 0 deletions

diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index b2d969ffbf32..cbf45c249388 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -667,4 +667,5 @@ source "drivers/misc/vmw_vmci/Kconfig"
 source "drivers/misc/issp/Kconfig"
 source "drivers/misc/tegra-profiler/Kconfig"
 source "drivers/misc/gps/Kconfig"
+source "drivers/misc/nv-sensorhub-ldisc/Kconfig"
 endmenu
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index e16746257063..b36cc1342f82 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -86,3 +86,4 @@ obj-$(CONFIG_DENVER_CPU)	+= force_idle_t132.o
 obj-$(CONFIG_ARCH_TEGRA)        +=tegra_timerinfo.o
 obj-$(CONFIG_MODS) 		+= mods/
 obj-$(CONFIG_DENVER_CPU)	+= idle_test_t132.o
+obj-$(CONFIG_NV_SENSORHUB)      += nv-sensorhub-ldisc/
diff --git a/drivers/misc/nv-sensorhub-ldisc/Kconfig b/drivers/misc/nv-sensorhub-ldisc/Kconfig
new file mode 100644
index 000000000000..591a3b99b3ab
--- /dev/null
+++ b/drivers/misc/nv-sensorhub-ldisc/Kconfig
@@ -0,0 +1,5 @@
+config NV_SENSORHUB
+	tristate "Line discipline for Sensorhub"
+	depends on TTY
+	help
+		TTY line discipline for sensorhub stream
diff --git a/drivers/misc/nv-sensorhub-ldisc/Makefile b/drivers/misc/nv-sensorhub-ldisc/Makefile
new file mode 100644
index 000000000000..5af4c6a4c57e
--- /dev/null
+++ b/drivers/misc/nv-sensorhub-ldisc/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for Sensorhub line discipline
+#
+
+obj-$(CONFIG_NV_SENSORHUB) += sh_ldisc.o
diff --git a/drivers/misc/nv-sensorhub-ldisc/sh_interface.h b/drivers/misc/nv-sensorhub-ldisc/sh_interface.h
new file mode 100644
index 000000000000..e79ca15bd8e3
--- /dev/null
+++ b/drivers/misc/nv-sensorhub-ldisc/sh_interface.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+ * 02111-1307, USA
+ */
+
+#ifndef SH_INTERFACE_H_
+#define SH_INTERFACE_H_
+
+/* Packet payload structures */
+
+/* Read /dev/shub_cam */
+struct __attribute__ ((__packed__)) camera_payload_t {
+	uint64_t	timestamp;
+	uint32_t	pwm_pulse_num;
+};
+
+/* Read /dev/shub_accel */
+struct  __attribute__ ((__packed__)) accel_payload_t {
+	uint64_t	timestamp;
+	uint32_t	pwm_pulse_num;
+	uint16_t	accel[3];
+};
+
+/* Read /dev/shub_gyro */
+struct  __attribute__ ((__packed__)) gyro_payload_t {
+	uint64_t	timestamp;
+	uint32_t	pwm_pulse_num;
+	uint16_t	gyro[3];
+};
+
+/* Read /dev/shub_mag */
+struct  __attribute__ ((__packed__)) mag_payload_t {
+	uint64_t	timestamp;
+	uint16_t	mag[3];
+};
+
+/* Read /dev/shub_baro */
+struct  __attribute__ ((__packed__)) baro_payload_t {
+	uint64_t	timestamp;
+	uint32_t	baro;
+};
+
+/* Write or Read /dev/shub_mcu */
+struct  __attribute__ ((__packed__)) mcu_payload_t {
+	union {
+		uint32_t	cmd;
+		uint32_t	rsp;
+	};
+};
+
+/* Commands from AP to sensor hub - directed to sensor hub controller */
+/* Write /dev/shub_mcu */
+#define CMD_PING			0x21
+#define CMD_START_TS			0x22
+#define CMD_STOP_TS			0x23
+#define CMD_START			CMD_PING
+#define CMD_END				CMD_STOP_TS
+
+/* Responses from sensor hub to AP */
+/* Read /dev/shub_mcu */
+#define RSP_PING			0x21
+#define RSP_START_TS			0x22
+#define RSP_STOP_TS			0x23
+
+#endif  /* SH_INTERFACE_H */
diff --git a/drivers/misc/nv-sensorhub-ldisc/sh_ldisc.c b/drivers/misc/nv-sensorhub-ldisc/sh_ldisc.c
new file mode 100644
index 000000000000..d4553708098d
--- /dev/null
+++ b/drivers/misc/nv-sensorhub-ldisc/sh_ldisc.c
@@ -0,0 +1,747 @@
+/*
+ * Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+ * 02111-1307, USA
+ */
+
+#include <linux/module.h>
+#include <linux/tty.h>
+#include <linux/slab.h>
+#include <linux/circ_buf.h>
+#include <linux/sched.h>
+#include <linux/miscdevice.h>
+#include <linux/crc32.h>
+#include <linux/poll.h>
+
+#include "sh_private.h"
+
+
+/* NOTE: The order here matches the Message Type
+ * macros listed in sh_interface.h interface header file */
+enum client_devs_num {
+	DEV_MCU = 0, /* NOTE: This is the Sensorhub MCU itself */
+
+	/* The following are the connected sensor devices */
+	DEV_CAM,
+	DEV_ACCEL,
+	DEV_GYRO,
+	DEV_MAG,
+	DEV_BARO,
+	NUM_DEVS
+};
+
+static char *client_devs_name[] = {
+	"shub_mcu", /* NOTE: This is the sensorhub itself */
+
+	/* The following are the connected sensor devices */
+	"shub_cam",
+	"shub_accel",
+	"shub_gyro",
+	"shub_mag",
+	"shub_baro"
+};
+
+struct client_dev {
+	/* Refer to parent data structure */
+	struct ldisc_priv *ld_data;
+
+	/* Misc dev. node */
+	struct miscdevice mdev;
+
+	/* Circular buffer where payloads are buffered */
+	char *read_buf; /* Alloc 32k */
+	int read_head;
+	int read_tail;
+	struct mutex read_buf_lock;
+
+	/* Allows blocking read. */
+	wait_queue_head_t readq;
+};
+
+struct ldisc_priv {
+	/* Refer to parent data structure */
+	struct tty_struct *tty;
+
+	/* Write lock to single underlying tty device */
+	struct mutex tty_write_lock;
+
+	/* Read buffer - to hold one pkt before de-muxing */
+	union {
+		struct sensor_hub_pkt_t pkt;
+		unsigned char    pkt_buf[sizeof(struct sensor_hub_pkt_t)];
+	};
+	int pkt_byte_idx;
+	int pyld_len;
+
+	/* Device nodes to de-multiplex data from sensorhub */
+	struct client_dev client_devs[NUM_DEVS];
+};
+
+/* We are using crc32 to validate packets */
+#define CRC_SIZE		4
+
+#define PYLD(pkt)		(*(uint32_t *)(pkt + \
+				sizeof(struct sensor_hub_pkt_header_t)))
+
+#define CRC(pkt, pyld_sz)	(*(uint32_t *)(pkt + \
+				sizeof(struct sensor_hub_pkt_header_t) + \
+				pyld_sz))
+
+#define CRC_DATA_SZ(pyld_sz)	(sizeof(struct sensor_hub_pkt_header_t) \
+				 + pyld_sz)
+
+#define PKT_SZ(pyld_sz)		(sizeof(struct sensor_hub_pkt_header_t) \
+				 + pyld_sz + CRC_SIZE)
+
+#if 0
+uint32_t add_pkt_cnt;
+uint32_t read_pkt_cnt;
+#endif
+
+/*****************************************************************************/
+
+static int
+pkt_type_valid(unsigned char byte)
+{
+	if ((byte >= MSG_SENSOR_START && byte <= MSG_SENSOR_END))
+		return 1;
+	return 0;
+}
+
+static int
+pkt_payload_len(unsigned char type)
+{
+	switch (type) {
+	case MSG_CAMERA:
+		return sizeof(struct camera_payload_t);
+	case MSG_ACCEL:
+		return sizeof(struct accel_payload_t);
+	case MSG_GYRO:
+		return sizeof(struct gyro_payload_t);
+	case MSG_MAG:
+		return sizeof(struct mag_payload_t);
+	case MSG_BARO:
+		return sizeof(struct baro_payload_t);
+	case MSG_MCU:
+		return sizeof(struct mcu_payload_t);
+	default:
+		return -1;
+	}
+}
+
+static uint32_t
+pkt_crc(char *pkt, int len)
+{
+#if 0
+	uint32_t crc;
+
+	crc = crc32_le(~0, pkt, len);
+
+	return crc;
+#endif
+
+	/* FIX IT: Temp code until CRC module is coded up in fw */
+	uint32_t crc;
+
+	crc = 0xCAFEBABA;
+
+	return crc;
+}
+
+/*****************************************************************************/
+
+static ssize_t
+client_dev_write(struct file *file, const char __user *buffer,
+		 size_t count, loff_t *ppos)
+{
+	struct client_dev *dev = file->private_data;
+	struct ldisc_priv *ld_data = dev->ld_data;
+	struct tty_struct *tty = ld_data->tty;
+	const char *b;
+	int c;
+	int retval = 0;
+	char pkt[sizeof(struct sensor_hub_pkt_t)];
+
+	if (count == 0)
+		return 0;
+
+	/* Only writes to shub_mcu device is supported for now */
+	if (count != sizeof(struct mcu_payload_t))
+		return 0;
+
+	if (mutex_lock_interruptible(&ld_data->tty_write_lock))
+		return -EINTR;
+
+	pr_debug("sh_ldisc: write line to dev struct 0x%p\n", dev);
+
+	/* Make a packet and then send to Sensorhub */
+	/* Header */
+	((struct sensor_hub_pkt_t *)pkt)->header.start = SENSOR_HUB_START;
+	((struct sensor_hub_pkt_t *)pkt)->header.type = MSG_MCU;
+	/* Payload */
+	if (get_user(PYLD(pkt), buffer)) {
+		pr_err("sh_ldisc: Copy from user-space failed.\n");
+		return -EFAULT;
+	}
+
+	/* CRC */
+	CRC(pkt, sizeof(struct mcu_payload_t)) =
+	    pkt_crc(pkt, CRC_DATA_SZ(sizeof(struct mcu_payload_t)));
+
+	b = pkt;
+	count = PKT_SZ(sizeof(struct mcu_payload_t));
+
+#if 0
+	int idx = 0;
+	int dbg_cnt = count;
+	while (dbg_cnt--)
+		pr_debug("sh_ldisc: write char: 0x%x\n", pkt[idx++]);
+#endif
+
+	while (1) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (signal_pending(current)) {
+			retval = -ERESTARTSYS;
+			break;
+		}
+		while (count > 0) {
+			c = tty->ops->write(tty, b, count);
+			if (c < 0) {
+				retval = c;
+				goto break_out;
+			}
+			if (!c)
+				break;
+			b += c;
+			count -= c;
+		}
+		if (!count)
+			break;
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			break;
+		}
+		schedule();
+	}
+break_out:
+
+	if (tty->ops->flush_chars)
+		tty->ops->flush_chars(tty);
+
+	__set_current_state(TASK_RUNNING);
+	if (b - buffer != count && tty->fasync)
+		set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+
+	mutex_unlock(&ld_data->tty_write_lock);
+
+	return (b - buffer) ? b - buffer : retval;
+}
+
+static ssize_t
+client_dev_read(struct file *file, char __user *buffer,
+				size_t count, loff_t *ppos)
+{
+	struct client_dev *dev = file->private_data;
+	ssize_t size;
+	ssize_t msg_size;
+	ssize_t retval = 0;
+	char __user *b = buffer;
+	int type_idx;
+
+	if (mutex_lock_interruptible(&dev->read_buf_lock))
+		return -ERESTARTSYS;
+
+	pr_debug("sh_ldisc: read line from dev struct 0x%p\n", dev);
+#if 0
+	pr_debug("sh_ldisc: read pkt # %d at tail %d\n",
+		read_pkt_cnt++, dev->read_tail);
+#endif
+
+	while (CIRC_CNT(dev->read_head, dev->read_tail, N_TTY_BUF_SIZE) == 0) {
+		/* nothing to read */
+		mutex_unlock(&dev->read_buf_lock); /* release the lock */
+		if (file->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		if (wait_event_interruptible(dev->readq,
+			(CIRC_CNT(dev->read_head, dev->read_tail,
+				  N_TTY_BUF_SIZE) != 0)))
+			/* signal: tell fs layer to handle it */
+			return -ERESTARTSYS;
+
+		/* otherwise loop, but first reacquire the lock */
+		if (mutex_lock_interruptible(&dev->read_buf_lock))
+			return -ERESTARTSYS;
+	}
+
+	/* get the payload size */
+	type_idx = ((dev->read_tail + sizeof(char)) & (N_TTY_BUF_SIZE-1));
+	msg_size = pkt_payload_len(dev->read_buf[type_idx]);
+
+	if (msg_size > count)
+		retval = -EINVAL;
+	else {
+		dev->read_tail = dev->read_tail +
+				 sizeof(struct sensor_hub_pkt_header_t);
+
+		while (msg_size) {
+			int c;
+
+			c = dev->read_buf[dev->read_tail];
+			dev->read_tail = ((dev->read_tail+1) &
+					  (N_TTY_BUF_SIZE-1));
+			msg_size--;
+
+			if (put_user(c, b++)) {
+				b--;
+				/* Move read_tail to end of packet for sync */
+				dev->read_tail = ((dev->read_tail+msg_size) &
+						  (N_TTY_BUF_SIZE-1));
+				pr_err("sh_ldisc: Copy to user-space failed.\n");
+				retval = -EFAULT;
+				break;
+			}
+		}
+
+		/* Move read_tail to past crc */
+		dev->read_tail = ((dev->read_tail+CRC_SIZE) &
+				  (N_TTY_BUF_SIZE-1));
+	}
+
+	mutex_unlock(&dev->read_buf_lock); /* release the lock */
+
+	size = b - buffer;
+	if (size)
+		retval = size;
+
+	return retval;
+}
+
+static unsigned int client_dev_poll(struct file *file, poll_table *wait)
+{
+	struct client_dev *dev = file->private_data;
+	unsigned int retval = 0;
+
+	poll_wait(file, &dev->readq, wait);
+
+	mutex_lock(&dev->read_buf_lock);
+	/* Check if there is data in dev buffer */
+	if (CIRC_CNT(dev->read_head, dev->read_tail, N_TTY_BUF_SIZE))
+		retval |= POLLIN | POLLRDNORM;
+	mutex_unlock(&dev->read_buf_lock);
+
+	/* You can always write */
+	retval |= POLLOUT | POLLWRNORM;
+
+	return retval;
+}
+
+static int
+client_dev_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *md = file->private_data;
+	struct client_dev *dev;
+
+	pr_info("%s : %s\n", __func__, md->name);
+
+	dev = container_of(md, struct client_dev, mdev);
+
+	mutex_lock(&dev->read_buf_lock);
+	dev->read_head = dev->read_tail = 0;
+	mutex_unlock(&dev->read_buf_lock);
+
+	file->private_data = dev;
+	nonseekable_open(inode, file);
+
+	return 0;
+}
+
+static int
+client_dev_release(struct inode *inode, struct file *file)
+{
+	pr_info("%s\n", __func__);
+	return 0;
+}
+
+static const struct file_operations client_dev_fops = {
+	.owner      = THIS_MODULE,
+	.open       = client_dev_open,
+	.release    = client_dev_release,
+	.read       = client_dev_read,
+	.write      = client_dev_write,
+	.poll       = client_dev_poll,
+	.llseek     = no_llseek,
+};
+
+/*****************************************************************************/
+
+static int
+create_client_devs(struct ldisc_priv *ld_data)
+{
+	int i;
+
+	for (i = 0; i < NUM_DEVS; i++) {
+		struct client_dev *dev = &ld_data->client_devs[i];
+		struct miscdevice *md = &dev->mdev;
+
+		memset(dev, 0, sizeof(struct client_dev));
+
+		dev->read_buf = kzalloc(N_TTY_BUF_SIZE, GFP_KERNEL);
+		if (!dev->read_buf) {
+			pr_err("%s: out of memory\n", __func__);
+			goto err_exit;
+		} else {
+			dev->ld_data = ld_data;
+			mutex_init(&dev->read_buf_lock);
+			init_waitqueue_head(&dev->readq);
+
+			md->fops = &client_dev_fops;
+			md->minor = MISC_DYNAMIC_MINOR;
+			md->name = client_devs_name[i];
+
+			if (misc_register(md) != 0) {
+				pr_err("%s: misc_register fail\n", __func__);
+				kfree(dev->read_buf);
+				goto err_exit;
+			}
+			pr_debug("sh_ldisc: init dev %s 0x%p\n",
+				 client_devs_name[i], dev);
+		}
+	}
+
+	return 1;
+
+err_exit:
+	while (--i >= 0) {
+		struct client_dev *dev = &ld_data->client_devs[i];
+		struct miscdevice *md = &dev->mdev;
+
+		if (misc_deregister(md) != 0)
+			pr_err("%s: misc_deregister fail\n", __func__);
+
+			kfree(dev->read_buf);
+	}
+
+	return 0;
+}
+
+static void
+delete_client_devs(struct ldisc_priv *ld_data)
+{
+	int i;
+
+	for (i = 0; i < NUM_DEVS; i++) {
+		struct client_dev *dev = &ld_data->client_devs[i];
+		struct miscdevice *md = &dev->mdev;
+
+		wake_up_interruptible(&dev->readq);
+
+		if (misc_deregister(md) != 0)
+			pr_err("%s: misc_register fail\n", __func__);
+
+		kfree(dev->read_buf);
+	}
+}
+
+/*****************************************************************************/
+
+static void
+client_dev_add_pkt(struct client_dev *dev, unsigned char *pkt, int count)
+{
+	int buf_space_available;
+
+	mutex_lock(&dev->read_buf_lock);
+
+	pr_debug("sh_ldisc: add line to dev struct 0x%p\n", dev);
+#if 0
+	pr_debug("sh_ldisc: add pkt # %d at head %d\n",
+		 add_pkt_cnt++, dev->read_head);
+#endif
+	buf_space_available = CIRC_SPACE(dev->read_head, dev->read_tail,
+					 N_TTY_BUF_SIZE);
+
+	if (buf_space_available > count) {
+		int space = CIRC_SPACE_TO_END(dev->read_head, dev->read_tail,
+					      N_TTY_BUF_SIZE);
+		if (space > count) {
+			memcpy(&dev->read_buf[dev->read_head], pkt, count);
+		} else {
+			memcpy(&dev->read_buf[dev->read_head], pkt, space);
+			memcpy(&dev->read_buf[0], pkt + space, count - space);
+		}
+		dev->read_head += count;
+		dev->read_head &= (N_TTY_BUF_SIZE - 1);
+		wake_up_interruptible(&dev->readq);
+	} else {
+		pr_err("sh_ldisc: Discard pkt due to lack of buffer space.\n");
+	}
+
+	mutex_unlock(&dev->read_buf_lock);
+}
+
+static inline void
+sh_ldisc_parse_pkt(struct tty_struct *tty, unsigned char c)
+{
+	struct ldisc_priv *ld_data = tty->disc_data;
+	uint32_t crc;
+
+	/* sanity check index */
+	if (ld_data->pkt_byte_idx >= sizeof(ld_data->pkt_buf)) {
+		/* Reset if byte index longer than longest packet */
+		ld_data->pkt_byte_idx = 0;
+	}
+
+	ld_data->pkt_buf[ld_data->pkt_byte_idx] = c;
+
+	switch (ld_data->pkt_byte_idx++) {
+	case 0:
+		/* expecting Magic value */
+		if (c != SENSOR_HUB_START) {
+			ld_data->pkt_byte_idx = 0;
+			pr_debug("sh_ldisc: msg start not recvd 0x%x\n", c);
+		}
+		break;
+
+	case 1:
+		/* Expecting message type */
+		if (!pkt_type_valid(c)) {
+			pr_debug("sh_ldisc: msg type 0x%x not valid\n", c);
+			ld_data->pkt_byte_idx = 0;
+			break;
+		}
+		/* Calc payload len from msg. type */
+		ld_data->pyld_len = pkt_payload_len(c);
+		if (ld_data->pyld_len < 0) {
+			pr_debug("sh_ldisc: msg len 0x%x not valid\n",
+				 ld_data->pyld_len);
+			ld_data->pkt_byte_idx = 0;
+		}
+		break;
+
+	default:
+		/* Nothing to do until last byte has been received */
+		if (ld_data->pkt_byte_idx ==
+			sizeof(struct sensor_hub_pkt_header_t) +
+				ld_data->pyld_len + CRC_SIZE) {
+#if 0
+			/* For debugging */
+			/* Print the accumulated packet content */
+			int dbg_cnt = sizeof(struct sensor_hub_pkt_header_t) +
+			ld_data->pyld_len + CRC_SIZE;
+			int dbg_idx = 0;
+			pr_debug("sh_ldisc: hdr len 0x%x\n",
+			       sizeof(struct sensor_hub_pkt_header_t));
+			pr_debug("sh_ldisc: pyl len 0x%x\n",
+			       ld_data->pyld_len);
+			pr_debug("sh_ldisc: crc len 0x%x\n",
+			       CRC_SIZE);
+			pr_debug("sh_ldisc: pkt addr 0x%p\n",
+			       ld_data->pkt_buf);
+			pr_debug("sh_ldisc: crc addr 0x%p\n",
+			       ld_data->pkt_buf +
+			       sizeof(struct sensor_hub_pkt_header_t) +
+			       ld_data->pyld_len);
+			pr_debug("sh_ldisc: crc 0x%x\n",
+			       *(uint32_t *)(ld_data->pkt_buf +
+				sizeof(struct sensor_hub_pkt_header_t) +
+				ld_data->pyld_len));
+			while (dbg_cnt--) {
+				pr_debug("sh_ldisc: pkt byte 0x%x\n",
+					ld_data->pkt_buf[dbg_idx++]);
+			}
+#endif
+			/* validate packet crc */
+			crc = pkt_crc(ld_data->pkt_buf,
+				      CRC_DATA_SZ(ld_data->pyld_len));
+
+			if (crc ==
+			    CRC(ld_data->pkt_buf, ld_data->pyld_len)) {
+
+				unsigned char type =
+				    ld_data->pkt.header.type;
+				int dev = DEV_MCU;
+
+				/* If sensor */
+				if (MSG_SENSOR_START <= type &&
+				    type <= MSG_SENSOR_END) {
+					dev = type;
+				}
+
+				/* Add the payload to the
+				 * corresponding dev buffer */
+				client_dev_add_pkt(
+					&ld_data->client_devs[dev],
+					ld_data->pkt_buf,
+					ld_data->pkt_byte_idx);
+			} else {
+				pr_err("sh_ldisc: crc not valid\n");
+			}
+
+			/* Packet de-muxed successfully or dropped.
+			 * Clear to start over. */
+			ld_data->pkt_byte_idx = 0;
+			ld_data->pyld_len = 0;
+		}
+		break;
+	}
+}
+
+static void
+sh_ldisc_recv_from_tty(struct tty_struct *tty, const unsigned char *cp,
+			char *fp, int count)
+{
+	const unsigned char *p;
+	char *f, flags = TTY_NORMAL;
+	int i;
+	char    buf[64];
+
+	for (i = count, p = cp, f = fp; i; i--, p++) {
+		if (f)
+			flags = *f++;
+		switch (flags) {
+		case TTY_NORMAL:
+			pr_debug("sh_ldisc: tty recv 0x%X\n", *p);
+			sh_ldisc_parse_pkt(tty, *p);
+			break;
+		case TTY_BREAK:
+		case TTY_PARITY:
+		case TTY_FRAME:
+		case TTY_OVERRUN:
+			pr_debug("sh_ldisc: tty ctrl\n");
+			/* Skip errors */
+			break;
+		default:
+			pr_err("sh_ldisc: %s: unknown flag %d\n",
+			       tty_name(tty, buf), flags);
+			break;
+		}
+	}
+}
+
+static int
+sh_ldisc_ioctl(struct tty_struct *tty, struct file *file,
+			unsigned int cmd, unsigned long arg)
+{
+	int err;
+
+	err = -EFAULT;
+
+	switch (cmd) {
+	case TCFLSH:
+		pr_debug("%s flush ioctl\n", __func__);
+
+		/* flush our buffers and the serial port's buffer */
+		if (arg == TCIOFLUSH || arg == TCOFLUSH)
+			;
+		err = n_tty_ioctl_helper(tty, file, cmd, arg);
+		break;
+
+	default:
+		pr_debug("%s default ioctl\n", __func__);
+
+		err = tty_mode_ioctl(tty, file, cmd, arg);
+		break;
+	}
+
+	return err;
+}
+
+static int
+sh_ldisc_open(struct tty_struct *tty)
+{
+	struct ldisc_priv *ld_data;
+
+	pr_info("%s\n", __func__);
+
+	ld_data = kzalloc(sizeof(*ld_data), GFP_KERNEL);
+	if (!ld_data)
+		goto err;
+
+	mutex_init(&ld_data->tty_write_lock);
+
+	/* Init priv data */
+	ld_data->tty = tty;
+	ld_data->pkt_byte_idx = 0;
+	ld_data->pyld_len = 0;
+
+#if 0
+	add_pkt_cnt = 0;
+	read_pkt_cnt = 0;
+#endif
+
+	if (!create_client_devs(ld_data))
+		goto err_free_bufs;
+
+	/* Init tty struct */
+	tty->disc_data = ld_data;
+	tty->receive_room = N_TTY_BUF_SIZE;
+
+	return 0;
+
+err_free_bufs:
+	kfree(ld_data);
+err:
+	return -ENOMEM;
+}
+
+static void
+sh_ldisc_close(struct tty_struct *tty)
+{
+	struct ldisc_priv *ld_data = tty->disc_data;
+
+	pr_info("%s\n", __func__);
+
+	delete_client_devs(ld_data);
+
+	kfree(ld_data);
+
+	tty->disc_data = NULL;
+}
+
+
+static struct tty_ldisc_ops sh_ldisc = {
+	.owner  = THIS_MODULE,
+	.magic  = TTY_LDISC_MAGIC,
+	.name   = "nv_senshub",
+	.open   = sh_ldisc_open,
+	.close  = sh_ldisc_close,
+	.ioctl  = sh_ldisc_ioctl,
+	.receive_buf = sh_ldisc_recv_from_tty,
+};
+
+static int __init
+sh_ldisc_init(void)
+{
+	int err;
+
+	err = tty_register_ldisc(N_NV_SENSHUB, &sh_ldisc);
+	if (err != 0)
+		pr_err("nv-sensorhub: error %d registering line disc.\n", err);
+	return err;
+}
+
+static void __exit
+sh_ldisc_cleanup(void)
+{
+	if (tty_unregister_ldisc(N_NV_SENSHUB) != 0)
+		pr_err("nv-sensorhub: failed to unregister line disc.\n");
+}
+
+module_init(sh_ldisc_init);
+module_exit(sh_ldisc_cleanup);
+
+MODULE_DESCRIPTION("Nvidia sensorhub driver");
+MODULE_AUTHOR("Arun Kannan <akannan@nvidia.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/nv-sensorhub-ldisc/sh_private.h b/drivers/misc/nv-sensorhub-ldisc/sh_private.h
new file mode 100644
index 000000000000..aa39611e77e5
--- /dev/null
+++ b/drivers/misc/nv-sensorhub-ldisc/sh_private.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+ * 02111-1307, USA
+ */
+
+#ifndef SH_PRIVATE_H_
+#define SH_PRIVATE_H_
+
+#include "sh_interface.h"
+
+/*
+ * SENSOR HUB FIRMWARE INTERFACE
+ *
+------------------------------------------------------
+|            |            |            |             |
+|  Start (S) |    Type    |  Payload   |   CRC32     |
+|  (1-byte)  |  (1-byte)  |(0-18 bytes)|  (4-bytes)  |
+|            |            |            |             |
+------------------------------------------------------
+*
+*
+*/
+
+/* Packet start */
+#define SENSOR_HUB_START		'S'
+
+/* Packet type */
+/* Messages from sensor hub to AP */
+/* NOTE: This matches the enum list in client_devs_num in source file */
+#define MSG_MCU				0x00 /* Read /dev/shub_mcu */
+#define MSG_CAMERA			0x01 /* Read /dev/shub_cam */
+#define MSG_ACCEL			0x02 /* Read /dev/shub_accel */
+#define MSG_GYRO			0x03 /* Read /dev/shub_gyro */
+#define MSG_MAG				0x04 /* Read /dev/shub_mag */
+#define MSG_BARO			0x05 /* Read /dev/shub_baro */
+#define MSG_SENSOR_START		MSG_MCU
+#define MSG_SENSOR_END			MSG_BARO
+
+/* Packet header */
+struct __attribute__ ((__packed__)) sensor_hub_pkt_header_t {
+	unsigned char	start;
+	unsigned char	type;
+};
+
+/* Packet payload */
+/* Included from sh_interface.h */
+
+/* Biggest possible packet */
+struct __attribute__ ((__packed__)) sensor_hub_pkt_t {
+	struct sensor_hub_pkt_header_t header;
+	union {
+		struct camera_payload_t	cam_payload;
+		struct accel_payload_t	accel_payload;
+		struct gyro_payload_t	gyro_payload;
+		struct mag_payload_t	mag_payload;
+		struct baro_payload_t	baro_payload;
+		struct mcu_payload_t	mcu_payload;
+	} payload;
+	uint32_t crc32;
+};
+
+#endif  /* SH_PRIVATE_H */