summaryrefslogtreecommitdiff
path: root/drivers/misc
diff options
context:
space:
mode:
authorArun Kannan <akannan@nvidia.com>2014-12-15 14:58:31 -0800
committerWinnie Hsu <whsu@nvidia.com>2015-01-09 12:02:23 -0800
commit23c93603667d9314a4ba19c32b6c69c24b60cf26 (patch)
tree4789e2700831b5d26fdbfa714f5dcfaf145b875e /drivers/misc
parent3a4d2022369f4bfc1701d6543226e01d7f6f8e0d (diff)
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>
Diffstat (limited to 'drivers/misc')
-rw-r--r--drivers/misc/Kconfig1
-rw-r--r--drivers/misc/Makefile1
-rw-r--r--drivers/misc/nv-sensorhub-ldisc/Kconfig5
-rw-r--r--drivers/misc/nv-sensorhub-ldisc/Makefile5
-rw-r--r--drivers/misc/nv-sensorhub-ldisc/sh_interface.h78
-rw-r--r--drivers/misc/nv-sensorhub-ldisc/sh_ldisc.c747
-rw-r--r--drivers/misc/nv-sensorhub-ldisc/sh_private.h75
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 */