Merge "diag: usb: Add support for partial writes over USB" (fe71690a) · Commits · e / devices / android_kernel_xiaomi_markw

drivers/char/diag/diag_debugfs.c

+4 −2

Original line number	Diff line number	Diff line
		@@ -429,7 +429,8 @@ static ssize_t diag_dbgfs_read_usbinfo(struct file file, char __user ubuf,
		"read work pending: %d\n"
		"read done work pending: %d\n"
		"connect work pending: %d\n"
		"disconnect work pending: %d\n\n",
		"disconnect work pending: %d\n"
		"max size supported: %d\n\n",
		usb_info->id,
		usb_info->name,
		usb_info->hdl,
		@@ -443,7 +444,8 @@ static ssize_t diag_dbgfs_read_usbinfo(struct file file, char __user ubuf,
		work_pending(&usb_info->read_work),
		work_pending(&usb_info->read_done_work),
		work_pending(&usb_info->connect_work),
		work_pending(&usb_info->disconnect_work));
		work_pending(&usb_info->disconnect_work),
		usb_info->max_size);
		bytes_in_buffer += bytes_written;

		/* Check if there is room to add another table entry */

drivers/char/diag/diag_usb.c

+206 −5

Original line number	Diff line number	Diff line
		@@ -21,14 +21,17 @@
		#include <linux/diagchar.h>
		#include <linux/delay.h>
		#include <linux/kmemleak.h>
		#include <linux/list.h>
		#ifdef CONFIG_DIAG_OVER_USB
		#include <linux/usb/usbdiag.h>
		#endif
		#include "diag_usb.h"
		#include "diag_mux.h"
		#include "diagmem.h"
		#include "diag_ipc_logging.h"

		#define DIAG_USB_STRING_SZ 10
		#define DIAG_USB_MAX_SIZE 16384

		struct diag_usb_info diag_usb[NUM_DIAG_USB_DEV] = {
		{
		@@ -43,6 +46,7 @@ struct diag_usb_info diag_usb[NUM_DIAG_USB_DEV] = {
		.usb_wq = NULL,
		.read_cnt = 0,
		.write_cnt = 0,
		.max_size = DIAG_USB_MAX_SIZE,
		},
		#ifdef CONFIG_DIAGFWD_BRIDGE_CODE
		{
		@@ -57,6 +61,7 @@ struct diag_usb_info diag_usb[NUM_DIAG_USB_DEV] = {
		.usb_wq = NULL,
		.read_cnt = 0,
		.write_cnt = 0,
		.max_size = DIAG_USB_MAX_SIZE,
		},
		{
		.id = DIAG_USB_MDM2,
		@@ -70,6 +75,7 @@ struct diag_usb_info diag_usb[NUM_DIAG_USB_DEV] = {
		.usb_wq = NULL,
		.read_cnt = 0,
		.write_cnt = 0,
		.max_size = DIAG_USB_MAX_SIZE,
		},
		{
		.id = DIAG_USB_QSC,
		@@ -83,10 +89,81 @@ struct diag_usb_info diag_usb[NUM_DIAG_USB_DEV] = {
		.usb_wq = NULL,
		.read_cnt = 0,
		.write_cnt = 0,
		.max_size = DIAG_USB_MAX_SIZE,
		}
		#endif
		};

		static int diag_usb_buf_tbl_add(struct diag_usb_info *usb_info,
		unsigned char *buf, uint32_t len, int ctxt)
		{
		struct list_head start, temp;
		struct diag_usb_buf_tbl_t *entry = NULL;

		list_for_each_safe(start, temp, &usb_info->buf_tbl) {
		entry = list_entry(start, struct diag_usb_buf_tbl_t, track);
		if (entry->buf == buf) {
		atomic_inc(&entry->ref_count);
		return 0;
		}
		}

		/* New buffer, not found in the list */
		entry = kzalloc(sizeof(struct diag_usb_buf_tbl_t), GFP_ATOMIC);
		if (!entry)
		return -ENOMEM;

		entry->buf = buf;
		entry->ctxt = ctxt;
		entry->len = len;
		atomic_set(&entry->ref_count, 1);
		INIT_LIST_HEAD(&entry->track);
		list_add_tail(&entry->track, &usb_info->buf_tbl);

		return 0;
		}

		static void diag_usb_buf_tbl_remove(struct diag_usb_info *usb_info,
		unsigned char *buf)
		{
		struct list_head start, temp;
		struct diag_usb_buf_tbl_t *entry = NULL;

		list_for_each_safe(start, temp, &usb_info->buf_tbl) {
		entry = list_entry(start, struct diag_usb_buf_tbl_t, track);
		if (entry->buf == buf) {
		DIAG_LOG(DIAG_DEBUG_MUX, "ref_count-- for %p\n", buf);
		atomic_dec(&entry->ref_count);
		/*
		* Remove reference from the table if it is the
		* only instance of the buffer
		*/
		if (atomic_read(&entry->ref_count) == 0)
		list_del(&entry->track);
		break;
		}
		}
		}

		static struct diag_usb_buf_tbl_t *diag_usb_buf_tbl_get(
		struct diag_usb_info *usb_info,
		unsigned char *buf)
		{
		struct list_head start, temp;
		struct diag_usb_buf_tbl_t *entry = NULL;

		list_for_each_safe(start, temp, &usb_info->buf_tbl) {
		entry = list_entry(start, struct diag_usb_buf_tbl_t, track);
		if (entry->buf == buf) {
		DIAG_LOG(DIAG_DEBUG_MUX, "ref_count-- for %p\n", buf);
		atomic_dec(&entry->ref_count);
		return entry;
		}
		}

		return NULL;
		}

		/*
		* This function is called asynchronously when USB is connected and
		* synchronously when Diag wants to connect to USB explicitly.
		@@ -215,18 +292,41 @@ static void diag_usb_write_done(struct diag_usb_info *ch,
		struct diag_request *req)
		{
		int ctxt = 0;
		int len = 0;
		struct diag_usb_buf_tbl_t *entry = NULL;
		unsigned char *buf = NULL;
		unsigned long flags;

		if (!ch \|\| !req)
		return;

		ch->write_cnt++;
		ctxt = (int)(uintptr_t)req->context;
		if (ch->ops && ch->ops->write_done)
		ch->ops->write_done(req->buf, req->actual, ctxt, DIAG_USB_MODE);
		entry = diag_usb_buf_tbl_get(ch, req->context);
		if (!entry) {
		pr_err_ratelimited("diag: In %s, unable to find entry %p in the table\n",
		__func__, req->context);
		return;
		}
		if (atomic_read(&entry->ref_count) != 0) {
		DIAG_LOG(DIAG_DEBUG_MUX, "partial write_done ref %d\n",
		atomic_read(&entry->ref_count));
		diag_ws_on_copy_complete(DIAG_WS_MUX);
		diagmem_free(driver, req, ch->mempool);
		return;
		}
		DIAG_LOG(DIAG_DEBUG_MUX, "full write_done, ctxt: %d\n",
		ctxt);
		spin_lock_irqsave(&ch->write_lock, flags);
		list_del(&entry->track);
		ctxt = entry->ctxt;
		buf = entry->buf;
		len = entry->len;
		kfree(entry);
		diag_ws_on_copy_complete(DIAG_WS_MUX);
		spin_unlock_irqrestore(&ch->write_lock, flags);

		if (ch->ops && ch->ops->write_done)
		ch->ops->write_done(buf, len, ctxt, DIAG_USB_MODE);
		diagmem_free(driver, req, ch->mempool);
		}

		@@ -244,6 +344,7 @@ static void diag_usb_notifier(void *priv, unsigned event,

		switch (event) {
		case USB_DIAG_CONNECT:
		usb_info->max_size = usb_diag_request_size(usb_info->hdl);
		atomic_set(&usb_info->connected, 1);
		pr_info("diag: USB channel %s connected\n", usb_info->name);
		queue_work(usb_info->usb_wq,
		@@ -283,6 +384,88 @@ int diag_usb_queue_read(int id)
		return 0;
		}

		static int diag_usb_write_ext(struct diag_usb_info *usb_info,
		unsigned char *buf, int len, int ctxt)
		{
		int err = 0;
		int write_len = 0;
		int bytes_remaining = len;
		int offset = 0;
		unsigned long flags;
		struct diag_request *req = NULL;

		if (!usb_info \|\| !buf \|\| len <= 0) {
		pr_err_ratelimited("diag: In %s, usb_info: %p buf: %p, len: %d\n",
		__func__, usb_info, buf, len);
		return -EINVAL;
		}

		spin_lock_irqsave(&usb_info->write_lock, flags);
		while (bytes_remaining > 0) {
		req = diagmem_alloc(driver, sizeof(struct diag_request),
		usb_info->mempool);
		if (!req) {
		/*
		* This should never happen. It either means that we are
		* trying to write more buffers than the max supported
		* by this particualar diag USB channel at any given
		* instance, or the previous write ptrs are stuck in
		* the USB layer.
		*/
		pr_err_ratelimited("diag: In %s, cannot retrieve USB write ptrs for USB channel %s\n",
		__func__, usb_info->name);
		spin_unlock_irqrestore(&usb_info->write_lock, flags);
		return -ENOMEM;
		}

		write_len = (bytes_remaining > usb_info->max_size) ?
		usb_info->max_size : (bytes_remaining);

		req->buf = buf + offset;
		req->length = write_len;
		req->context = (void *)buf;

		if (!usb_info->hdl \|\| !atomic_read(&usb_info->connected) \|\|
		!atomic_read(&usb_info->diag_state)) {
		pr_debug_ratelimited("diag: USB ch %s is not connected\n",
		usb_info->name);
		diagmem_free(driver, req, usb_info->mempool);
		spin_unlock_irqrestore(&usb_info->write_lock, flags);
		return -ENODEV;
		}

		if (diag_usb_buf_tbl_add(usb_info, buf, len, ctxt)) {
		diagmem_free(driver, req, usb_info->mempool);
		spin_unlock_irqrestore(&usb_info->write_lock, flags);
		return -ENOMEM;
		}

		diag_ws_on_read(DIAG_WS_MUX, len);
		err = usb_diag_write(usb_info->hdl, req);
		diag_ws_on_copy(DIAG_WS_MUX);
		if (err) {
		pr_err_ratelimited("diag: In %s, error writing to usb channel %s, err: %d\n",
		__func__, usb_info->name, err);
		DIAG_LOG(DIAG_DEBUG_MUX,
		"ERR! unable to write t usb, err: %d\n", err);
		diag_ws_on_copy_fail(DIAG_WS_MUX);
		diag_usb_buf_tbl_remove(usb_info, buf);
		diagmem_free(driver, req, usb_info->mempool);
		spin_unlock_irqrestore(&usb_info->write_lock, flags);
		return err;
		}
		offset += write_len;
		bytes_remaining -= write_len;
		DIAG_LOG(DIAG_DEBUG_MUX,
		"bytes_remaining: %d write_len: %d, len: %d\n",
		bytes_remaining, write_len, len);
		}
		DIAG_LOG(DIAG_DEBUG_MUX, "done writing!");
		spin_unlock_irqrestore(&usb_info->write_lock, flags);

		return 0;
		}

		int diag_usb_write(int id, unsigned char *buf, int len, int ctxt)
		{
		int err = 0;
		@@ -298,6 +481,12 @@ int diag_usb_write(int id, unsigned char *buf, int len, int ctxt)

		usb_info = &diag_usb[id];

		if (len > usb_info->max_size) {
		DIAG_LOG(DIAG_DEBUG_MUX, "len: %d, max_size: %d\n",
		len, usb_info->max_size);
		return diag_usb_write_ext(usb_info, buf, len, ctxt);
		}

		req = diagmem_alloc(driver, sizeof(struct diag_request),
		usb_info->mempool);
		if (!req) {
		@@ -314,7 +503,7 @@ int diag_usb_write(int id, unsigned char *buf, int len, int ctxt)

		req->buf = buf;
		req->length = len;
		req->context = (void *)(uintptr_t)ctxt;
		req->context = (void *)buf;

		if (!usb_info->hdl \|\| !atomic_read(&usb_info->connected) \|\|
		!atomic_read(&usb_info->diag_state)) {
		@@ -325,16 +514,27 @@ int diag_usb_write(int id, unsigned char *buf, int len, int ctxt)
		}

		spin_lock_irqsave(&usb_info->write_lock, flags);
		if (diag_usb_buf_tbl_add(usb_info, buf, len, ctxt)) {
		DIAG_LOG(DIAG_DEBUG_MUX, "ERR! unable to add buf %p to table\n",
		buf);
		diagmem_free(driver, req, usb_info->mempool);
		spin_unlock_irqrestore(&usb_info->write_lock, flags);
		return -ENOMEM;
		}

		diag_ws_on_read(DIAG_WS_MUX, len);
		err = usb_diag_write(usb_info->hdl, req);
		diag_ws_on_copy(DIAG_WS_MUX);
		spin_unlock_irqrestore(&usb_info->write_lock, flags);
		if (err) {
		pr_err_ratelimited("diag: In %s, error writing to usb channel %s, err: %d\n",
		__func__, usb_info->name, err);
		diag_ws_on_copy_fail(DIAG_WS_MUX);
		DIAG_LOG(DIAG_DEBUG_MUX,
		"ERR! unable to write t usb, err: %d\n", err);
		diag_usb_buf_tbl_remove(usb_info, buf);
		diagmem_free(driver, req, usb_info->mempool);
		}
		spin_unlock_irqrestore(&usb_info->write_lock, flags);

		return err;
		}
		@@ -411,6 +611,7 @@ int diag_usb_register(int id, int ctxt, struct diag_mux_ops *ops)
		* in USB mode. Set the state to 1.
		*/
		atomic_set(&ch->diag_state, 1);
		INIT_LIST_HEAD(&ch->buf_tbl);
		diagmem_init(driver, ch->mempool);
		INIT_WORK(&(ch->read_work), usb_read_work_fn);
		INIT_WORK(&(ch->read_done_work), usb_read_done_work_fn);

drivers/char/diag/diag_usb.h

+10 −0

Original line number	Diff line number	Diff line
		@@ -38,6 +38,14 @@

		#define DIAG_USB_MODE 0

		struct diag_usb_buf_tbl_t {
		struct list_head track;
		unsigned char *buf;
		uint32_t len;
		atomic_t ref_count;
		int ctxt;
		};

		struct diag_usb_info {
		int id;
		int ctxt;
		@@ -47,6 +55,8 @@ struct diag_usb_info {
		atomic_t read_pending;
		int enabled;
		int mempool;
		int max_size;
		struct list_head buf_tbl;
		unsigned long read_cnt;
		unsigned long write_cnt;
		spinlock_t lock;

drivers/char/diag/diagchar_core.c

+3 −4

Original line number	Diff line number	Diff line
		@@ -2915,12 +2915,11 @@ static int __init diagchar_init(void)
		* POOL_TYPE_MUX_APPS is for the buffers in the Diag MUX layer.
		* The number of buffers encompasses Diag data generated on
		* the Apss processor + 1 for the responses generated exclusively on
		* the Apps processor + data from data channels (2 channels per
		* peripheral) + data from command channels
		* the Apps processor + data from data channels (4 channels per
		* peripheral) + data from command channels (2)
		*/
		diagmem_setsize(POOL_TYPE_MUX_APPS, itemsize_usb_apps,
		poolsize_usb_apps + 1 + (NUM_PERIPHERALS * 2) +
		NUM_PERIPHERALS);
		poolsize_usb_apps + 1 + (NUM_PERIPHERALS * 6));
		driver->num_clients = max_clients;
		driver->logging_mode = USB_MODE;
		for (i = 0; i < DIAG_NUM_PROC; i++) {