modules: moto_f_diag: add tty diag module

Add module to override the f_diag gadget module
with tty calls for factory mode.

Change-Id: I208dcebdf00e629847b4ed3b7e597f968cb324ae
Signed-off-by: Ryan Lattrel <ryanl@motorola.com>
Reviewed-on: https://gerrit.mot.com/1128711
SME-Granted: SME Approvals Granted
SLTApproved: Slta Waiver
Tested-by: Jira Key
Reviewed-by: Kenneth Kessler <kennykessler@motorola.com>
Submit-Approved: Jira Key

drivers/moto_f_diag/Android.mk

@@ -0,0 +1,8 @@
+DLKM_DIR := motorola/kernel/modules
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := moto_f_diag.ko
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_PATH := $(KERNEL_MODULES_OUT)
+include $(DLKM_DIR)/AndroidKernelModule.mk

drivers/moto_f_diag/Kbuild

@@ -0,0 +1,7 @@
+# add -Wall to try to catch everything we can.
+EXTRA_CFLAGS += -Wall
+EXTRA_CFLAGS += -I$(TOP)/motorola/kernel/modules/include
+
+moto_f_diag-objs += tty_diag.o
+moto_f_diag-objs += f_diag.o
+obj-m += moto_f_diag.o

drivers/moto_f_diag/Makefile

@@ -0,0 +1,10 @@
+KERNEL_SRC ?= /lib/modules/$(shell uname -r)/build
+
+all:
+	$(MAKE) -C $(KERNEL_SRC) M=$(shell pwd) modules $(KBUILD_OPTIONS)
+
+modules_install:
+	$(MAKE) INSTALL_MOD_STRIP=1 -C $(KERNEL_SRC) M=$(shell pwd) modules_install
+
+clean:
+	$(MAKE) -C $(KERNEL_SRC) M=$(PWD) clean

drivers/moto_f_diag/f_diag.c

@@ -0,0 +1,73 @@
+/* f_diag.c
+ * Copyright (C) 2007 Google, Inc.
+ * Copyright (c) 2008-2017, The Linux Foundation. All rights reserved.
+ * Author: Brian Swetland <swetland@google.com>
+ * Copyright (c) 2018, Motorola Mobility LLC.
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+#include <linux/module.h>
+#include <linux/usb/usbdiag.h>
+
+#include "tty_diag.h"
+
+struct usb_diag_ch *usb_diag_open(const char *name, void *priv,
+		void (*notify)(void *, unsigned int, struct diag_request *))
+{
+	return tty_diag_channel_open(name, priv, notify);
+}
+EXPORT_SYMBOL(usb_diag_open);
+
+void usb_diag_close(struct usb_diag_ch *ch)
+{
+	return tty_diag_channel_close(ch);
+}
+EXPORT_SYMBOL(usb_diag_close);
+
+int usb_diag_alloc_req(struct usb_diag_ch *ch, int n_write, int n_read)
+{
+	return 0;
+}
+EXPORT_SYMBOL(usb_diag_alloc_req);
+#define DWC3_MAX_REQUEST_SIZE (16 * 1024 * 1024)
+
+int usb_diag_request_size(struct usb_diag_ch *ch)
+{
+	return DWC3_MAX_REQUEST_SIZE;
+}
+EXPORT_SYMBOL(usb_diag_request_size);
+
+int usb_diag_read(struct usb_diag_ch *ch, struct diag_request *d_req)
+{
+	return tty_diag_channel_read(ch, d_req);
+}
+EXPORT_SYMBOL(usb_diag_read);
+
+int usb_diag_write(struct usb_diag_ch *ch, struct diag_request *d_req)
+{
+	return tty_diag_channel_write(ch, d_req);
+}
+EXPORT_SYMBOL(usb_diag_write);
+
+static int __init diag_init(void)
+{
+	return diag_tty_init();
+}
+
+static void __exit diag_exit(void)
+{
+	diag_tty_exit();
+}
+
+module_init(diag_init);
+module_exit(diag_exit);
+
+MODULE_DESCRIPTION("TTY diag function driver");
+MODULE_LICENSE("GPL v2");

drivers/moto_f_diag/tty_diag.c

@@ -0,0 +1,374 @@
+/* Copyright (c) 2018, Motorola Mobility LLC. 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 and
+ * only 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/major.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+
+#include "tty_diag.h"
+
+#define DIAG_MAJOR 185
+#define DIAG_TTY_MINOR_COUNT 3
+
+static DEFINE_SPINLOCK(diag_tty_lock);
+
+static struct tty_driver *diag_tty_driver;
+static struct usb_diag_ch legacy_ch;
+static struct diag_request *d_req_ptr;
+
+struct diag_tty_data {
+	struct tty_port port;
+	struct tty_struct *tty;
+	int open_count;
+};
+
+static struct diag_tty_data diag_tty[DIAG_TTY_MINOR_COUNT];
+static int diag_packet_incomplete;
+
+static int ttydiag_dbg_cmd_code, ttydiag_dbg_subsys_id, dbg_tty_minor;
+
+/* 1 - would add channel dispatch checking logic
+ *
+ *	by default, this is off (0)
+ *
+ *
+ *	try `echo "1">/sys/class/diag/diag/dbg_ftm' to turn it on.
+ */
+static int dbg_ftm_flag;
+
+static int diag_tty_open(struct tty_struct *tty, struct file *f)
+{
+	int n = tty->index;
+	struct diag_tty_data *tty_data;
+	unsigned long flags;
+
+	tty_data = diag_tty + n;
+
+	if (n < 0 || n >= DIAG_TTY_MINOR_COUNT)
+		return -ENODEV;
+
+	if (tty_data->open_count >= 1)
+		return -EBUSY;
+
+	spin_lock_irqsave(&diag_tty_lock, flags);
+
+	tty_data->tty = tty;
+	tty->driver_data = tty_data;
+	tty_data->open_count++;
+
+	spin_unlock_irqrestore(&diag_tty_lock, flags);
+
+	legacy_ch.notify(legacy_ch.priv, USB_DIAG_CONNECT, NULL);
+
+	return 0;
+}
+
+static void diag_tty_close(struct tty_struct *tty, struct file *f)
+{
+	struct diag_tty_data *tty_data = tty->driver_data;
+	int disconnect_channel = 1;
+	int i;
+	unsigned long flags;
+
+	if (tty_data == NULL)
+		return;
+
+	spin_lock_irqsave(&diag_tty_lock, flags);
+	tty_data->open_count--;
+
+	for (i = 0; i < DIAG_TTY_MINOR_COUNT; i++) {
+		if (diag_tty[i].open_count) {
+			disconnect_channel = 0;
+			break;
+		}
+	}
+
+	if (disconnect_channel && legacy_ch.notify && legacy_ch.priv)
+		legacy_ch.priv_usb = NULL;
+
+	if (tty_data->open_count == 0)
+		tty->driver_data = NULL;
+
+	spin_unlock_irqrestore(&diag_tty_lock, flags);
+}
+
+static int diag_tty_write(struct tty_struct *tty,
+				const unsigned char *buf, int len)
+{
+	struct diag_tty_data *tty_data = tty->driver_data;
+	struct diag_request *d_req_temp = d_req_ptr;
+	unsigned long flags;
+
+	spin_lock_irqsave(&diag_tty_lock, flags);
+
+	/* Make sure diag char driver is ready and no outstanding request */
+	if ((d_req_ptr == NULL) || legacy_ch.priv_usb) {
+		spin_unlock_irqrestore(&diag_tty_lock, flags);
+		return -EAGAIN;
+	}
+
+	/* Diag packet must fit in buff and be written all at once */
+	if (len > d_req_ptr->length) {
+		spin_unlock_irqrestore(&diag_tty_lock, flags);
+		return -EMSGSIZE;
+	}
+
+	/* Check whether fresh packet */
+	if (!diag_packet_incomplete) {
+		memcpy(d_req_ptr->buf, buf, len);
+		d_req_ptr->actual = len;
+	} else {
+		if (d_req_ptr->actual + len > d_req_ptr->length) {
+			d_req_ptr->actual = 0;
+			diag_packet_incomplete = 0;
+			spin_unlock_irqrestore(&diag_tty_lock, flags);
+			return -EMSGSIZE;
+		} else {
+			memcpy(d_req_ptr->buf + d_req_ptr->actual, buf, len);
+			d_req_ptr->actual += len;
+		}
+	}
+
+	/* Check if packet is now complete */
+	if (d_req_ptr->buf[d_req_ptr->actual - 1] != 0x7E) {
+		diag_packet_incomplete = 1;
+		spin_unlock_irqrestore(&diag_tty_lock, flags);
+		return len;
+	} else
+		diag_packet_incomplete = 0;
+
+	if (dbg_ftm_flag == 1) {
+
+		if (tty_data != NULL) {
+			if (!strncmp(tty_data->tty->name, "ttydiag0",
+						strlen("ttydiag0"))) {
+				dbg_tty_minor = 0;
+				ttydiag_dbg_cmd_code =
+					(int)(*(char *)d_req_ptr->buf);
+				ttydiag_dbg_subsys_id =
+					(int)(*(char *)(d_req_ptr->buf+1));
+			}
+			if (!strncmp(tty_data->tty->name, "ttydiag1",
+						strlen("ttydiag1"))) {
+				dbg_tty_minor = 1;
+				ttydiag_dbg_cmd_code =
+					(int)(*(char *)d_req_ptr->buf);
+				ttydiag_dbg_subsys_id =
+					(int)(*(char *)(d_req_ptr->buf+1));
+			}
+		}
+	}
+
+	/* Set active tty for responding */
+	legacy_ch.priv_usb = tty_data;
+
+	spin_unlock_irqrestore(&diag_tty_lock, flags);
+	legacy_ch.notify(legacy_ch.priv, USB_DIAG_READ_DONE, d_req_temp);
+
+	return len;
+}
+
+static int diag_tty_write_room(struct tty_struct *tty)
+{
+	if ((d_req_ptr == NULL) || legacy_ch.priv_usb)
+		return 0;
+	else
+		return d_req_ptr->length;
+}
+
+static int diag_tty_chars_in_buffer(struct tty_struct *tty)
+{
+	/* Data is always written when available, this should be changed if
+	   write to userspace is changed to delayed work */
+	return 0;
+}
+
+static const struct tty_operations diag_tty_ops = {
+	.open = diag_tty_open,
+	.close = diag_tty_close,
+	.write = diag_tty_write,
+	.write_room = diag_tty_write_room,
+	.chars_in_buffer = diag_tty_chars_in_buffer,
+};
+
+/* Diag char driver ready */
+struct usb_diag_ch *tty_diag_channel_open(const char *name, void *priv,
+		void (*notify)(void *, unsigned, struct diag_request *))
+{
+	int i;
+	unsigned long flags;
+
+	if (legacy_ch.priv != NULL)
+		return ERR_PTR(-EBUSY);
+
+	spin_lock_init(&diag_tty_lock);
+	spin_lock_irqsave(&diag_tty_lock, flags);
+	legacy_ch.priv = priv;
+	legacy_ch.notify = notify;
+	spin_unlock_irqrestore(&diag_tty_lock, flags);
+
+	for (i = 0; i < DIAG_TTY_MINOR_COUNT; i++) {
+		tty_port_init(&diag_tty[i].port);
+		tty_port_register_device(&diag_tty[i].port, diag_tty_driver,
+				i, NULL);
+	}
+
+	return &legacy_ch;
+}
+
+/* Diag char driver no longer ready */
+void tty_diag_channel_close(struct usb_diag_ch *diag_ch)
+{
+	struct diag_tty_data *priv_usb = diag_ch->priv_usb;
+	unsigned long flags;
+	int i;
+
+	if (diag_ch->priv_usb) {
+		tty_insert_flip_char(&priv_usb->port, 0x00, TTY_BREAK);
+		tty_flip_buffer_push(&priv_usb->port);
+	}
+	spin_lock_irqsave(&diag_tty_lock, flags);
+	diag_ch->priv = NULL;
+	diag_ch->notify = NULL;
+
+	for (i = DIAG_TTY_MINOR_COUNT - 1; i >= 0; i--)
+		tty_unregister_device(diag_tty_driver, i);
+
+	spin_unlock_irqrestore(&diag_tty_lock, flags);
+}
+
+/* Diag char driver prepares tty driver to receive a write from userspace */
+int tty_diag_channel_read(struct usb_diag_ch *diag_ch,
+				struct diag_request *d_req)
+{
+	d_req_ptr = d_req;
+
+	return 0;
+}
+
+/* Diag char driver has diag packet ready for userspace */
+int tty_diag_channel_write(struct usb_diag_ch *diag_ch,
+				struct diag_request *d_req)
+{
+	struct diag_tty_data *tty_data = diag_ch->priv_usb;
+	unsigned char *tty_buf;
+	int tty_allocated;
+	unsigned long flags;
+	int cmd_code, subsys_id;
+
+	/* If diag packet is not 1:1 response (perhaps logging packet?),
+	   try primary channel */
+	if (tty_data == NULL)
+		tty_data = &(diag_tty[0]);
+
+	spin_lock_irqsave(&diag_tty_lock, flags);
+
+	if (dbg_ftm_flag == 1) {
+		cmd_code = (int)(*(char *)d_req->buf);
+		subsys_id = (int)(*(char *)(d_req->buf+1));
+
+		if (cmd_code == ttydiag_dbg_cmd_code &&
+				subsys_id == ttydiag_dbg_subsys_id &&
+				dbg_tty_minor != -1) {
+			/* respond to last tty */
+			ttydiag_dbg_cmd_code = 0;
+			ttydiag_dbg_subsys_id = 0;
+			tty_data = &(diag_tty[dbg_tty_minor]);
+			dbg_tty_minor = -1;
+		} else {
+			tty_data = &(diag_tty[2]);
+		}
+	}
+
+	if (tty_data->tty == NULL) {
+		spin_unlock_irqrestore(&diag_tty_lock, flags);
+		return -EIO;
+	}
+
+	tty_allocated = tty_prepare_flip_string(&tty_data->port,
+						&tty_buf, d_req->length);
+
+	if (tty_allocated < d_req->length) {
+		spin_unlock_irqrestore(&diag_tty_lock, flags);
+		return -ENOMEM;
+	}
+
+	/* Unset active tty for next request diag tool */
+	diag_ch->priv_usb = NULL;
+
+	memcpy(tty_buf, d_req->buf, d_req->length);
+	tty_flip_buffer_push(&tty_data->port);
+
+	d_req->actual = d_req->length;
+	spin_unlock_irqrestore(&diag_tty_lock, flags);
+
+	diag_ch->notify(diag_ch->priv, USB_DIAG_WRITE_DONE_SYNC, d_req);
+
+	return 0;
+}
+
+int diag_tty_init(void)
+{
+	int result;
+
+	legacy_ch.notify = NULL;
+	legacy_ch.priv = NULL;
+	legacy_ch.priv_usb = NULL;
+	diag_packet_incomplete = 0;
+	dbg_tty_minor = -1;
+
+	diag_tty_driver = alloc_tty_driver(DIAG_TTY_MINOR_COUNT);
+	if (diag_tty_driver == NULL)
+		return -ENOMEM;
+
+	diag_tty_driver->owner = THIS_MODULE;
+	diag_tty_driver->driver_name = "tty_diag";
+	diag_tty_driver->name = "ttydiag";
+	diag_tty_driver->major = DIAG_MAJOR;
+	diag_tty_driver->minor_start = 0;
+	diag_tty_driver->type = TTY_DRIVER_TYPE_SYSTEM;
+	diag_tty_driver->subtype = SYSTEM_TYPE_TTY;
+	diag_tty_driver->init_termios = tty_std_termios;
+	diag_tty_driver->init_termios.c_cflag = B115200 | CS8 | CREAD;
+	diag_tty_driver->init_termios.c_iflag = IGNBRK;
+	diag_tty_driver->init_termios.c_oflag = 0;
+	diag_tty_driver->init_termios.c_lflag = 0;
+	diag_tty_driver->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+
+	tty_set_operations(diag_tty_driver, &diag_tty_ops);
+
+	result = tty_register_driver(diag_tty_driver);
+	if (result) {
+		printk(KERN_ERR "Failed to register diag_tty driver.");
+		put_tty_driver(diag_tty_driver);
+		diag_tty_driver = NULL;
+		return result;
+	}
+
+	return 0;
+}
+
+void diag_tty_exit(void)
+{
+	if (diag_tty_driver) {
+		tty_unregister_driver(diag_tty_driver);
+		put_tty_driver(diag_tty_driver);
+	}
+}

drivers/moto_f_diag/tty_diag.h

@@ -0,0 +1,29 @@
+/* Copyright (c) 2018, Motorola Mobility LLC. 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 and
+ * only 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.
+ *
+ */
+
+#ifndef _DRIVERS_TTY_DIAG_H_
+#define _DRIVERS_TTY_DIAG_H_
+
+#include <linux/usb/usbdiag.h>
+
+int diag_tty_init(void);
+void diag_tty_exit(void);
+struct usb_diag_ch *tty_diag_channel_open(const char *name, void *priv,
+		void (*notify)(void *, unsigned, struct diag_request *));
+void tty_diag_channel_close(struct usb_diag_ch *diag_ch);
+int tty_diag_channel_read(struct usb_diag_ch *diag_ch,
+				struct diag_request *d_req);
+int tty_diag_channel_write(struct usb_diag_ch *diag_ch,
+				struct diag_request *d_req);
+
+#endif /* _DRIVERS_TTY_DIAG_H_ */