Merge "diag: Handle drops for diag over rpmsg" (f221eabf) · Commits · e / devices / android_kernel_oneplus_sm7250

drivers/char/diag/diagfwd_peripheral.c

+6 −2

Original line number	Diff line number	Diff line
		// SPDX-License-Identifier: GPL-2.0-only
		/* Copyright (c) 2015-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2015-2019, 2021, The Linux Foundation. All rights reserved.
		*/
		#include <linux/slab.h>
		#include <linux/err.h>
		@@ -1440,6 +1440,7 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int buf_num)
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"Buffer 1 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
		fwd_info->peripheral, fwd_info->type, buf_num);
		rpmsg_mark_buffers_free(peripheral, type, buf_num);
		}
		} else if (buf_num == 2 && fwd_info->buf_2) {
		/*
		@@ -1466,6 +1467,7 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int buf_num)
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"Buffer 2 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
		fwd_info->peripheral, fwd_info->type, buf_num);
		rpmsg_mark_buffers_free(peripheral, type, buf_num);
		}
		} else if (buf_num >= 3 && (buf_num % 2)) {
		/*
		@@ -1501,6 +1503,7 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int buf_num)
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"Buffer 1 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
		fwd_info->peripheral, fwd_info->type, buf_num);
		rpmsg_mark_buffers_free(peripheral, type, 1);
		}
		} else if (buf_num >= 4 && !(buf_num % 2)) {
		/*
		@@ -1536,6 +1539,7 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int buf_num)
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"Buffer 2 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
		fwd_info->peripheral, fwd_info->type, buf_num);
		rpmsg_mark_buffers_free(peripheral, type, 2);
		}
		} else
		pr_err("diag: In %s, invalid buf_num %d\n", __func__, buf_num);

drivers/char/diag/diagfwd_rpmsg.c

+229 −97

Original line number	Diff line number	Diff line
		// SPDX-License-Identifier: GPL-2.0-only
		/* Copyright (c) 2017-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2017-2019, 2021, The Linux Foundation. All rights reserved.
		*/

		#include <linux/slab.h>
		@@ -24,11 +24,22 @@
		#define PERI_RPMSG rpmsg_info->peripheral

		struct diag_rpmsg_read_work {
		struct diag_rpmsg_info *rpmsg_info;
		const void *ptr_read_done;
		const void *ptr_rx_done;
		size_t ptr_read_size;
		struct work_struct work;
		struct list_head rx_list_head;
		spinlock_t rx_lock;
		};

		static struct diag_rpmsg_read_work *read_work_struct;

		/**
		** struct rx_buff_list - holds rx rpmsg data, before it will be consumed
		** by diagfwd_channel_read_done worker, item per rx packet
		**/
		struct rx_buff_list {
		struct list_head list;
		void *rpmsg_rx_buf;
		int rx_buf_size;
		struct diag_rpmsg_info *rpmsg_info;
		};

		struct diag_rpmsg_info rpmsg_data[NUM_PERIPHERALS] = {
		@@ -36,7 +47,7 @@ struct diag_rpmsg_info rpmsg_data[NUM_PERIPHERALS] = {
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_DATA,
		.edge = "mpss",
		.name = "DIAG_DATA",
		.name = "DIAG",
		.buf1 = NULL,
		.buf2 = NULL,
		.hdl = NULL
		@@ -54,7 +65,7 @@ struct diag_rpmsg_info rpmsg_data[NUM_PERIPHERALS] = {
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_DATA,
		.edge = "wcnss",
		.name = "DIAG_DATA",
		.name = "APPS_RIVA_DATA",
		.buf1 = NULL,
		.buf2 = NULL,
		.hdl = NULL
		@@ -102,7 +113,7 @@ struct diag_rpmsg_info rpmsg_cntl[NUM_PERIPHERALS] = {
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_CNTL,
		.edge = "mpss",
		.name = "DIAG_CTRL",
		.name = "DIAG_CNTL",
		.buf1 = NULL,
		.buf2 = NULL,
		.hdl = NULL
		@@ -120,7 +131,7 @@ struct diag_rpmsg_info rpmsg_cntl[NUM_PERIPHERALS] = {
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_CNTL,
		.edge = "wcnss",
		.name = "DIAG_CTRL",
		.name = "APPS_RIVA_CTRL",
		.buf1 = NULL,
		.buf2 = NULL,
		.hdl = NULL
		@@ -168,7 +179,7 @@ struct diag_rpmsg_info rpmsg_dci[NUM_PERIPHERALS] = {
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_DCI,
		.edge = "mpss",
		.name = "DIAG_DCI_DATA",
		.name = "DIAG_2",
		.buf1 = NULL,
		.buf2 = NULL,
		.hdl = NULL
		@@ -300,7 +311,7 @@ struct diag_rpmsg_info rpmsg_dci_cmd[NUM_PERIPHERALS] = {
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_DCI_CMD,
		.edge = "mpss",
		.name = "DIAG_DCI_CMD",
		.name = "DIAG_2_CMD",
		.buf1 = NULL,
		.buf2 = NULL,
		.hdl = NULL
		@@ -463,7 +474,7 @@ static int diag_rpmsg_read(void ctxt, unsigned char buf, int buf_len)
		rpmsg_info->buf2 = buf;
		}
		mutex_unlock(&driver->diagfwd_channel_mutex[rpmsg_info->peripheral]);

		queue_work(rpmsg_info->wq, &read_work_struct->work);
		return ret_val;
		}

		@@ -488,14 +499,13 @@ static void diag_rpmsg_read_work_fn(struct work_struct *work)
		return;
		}
		mutex_unlock(&driver->rpmsginfo_mutex[PERI_RPMSG]);

		diagfwd_channel_read(rpmsg_info->fwd_ctxt);
		}

		static int diag_rpmsg_write(void ctxt, unsigned char buf, int len)
		{
		struct diag_rpmsg_info *rpmsg_info = NULL;
		int err = 0;
		struct diag_rpmsg_info *rpmsg_info = NULL;
		struct rpmsg_device *rpdev = NULL;

		if (!ctxt \|\| !buf)
		@@ -518,6 +528,7 @@ static int diag_rpmsg_write(void ctxt, unsigned char buf, int len)
		}

		rpdev = (struct rpmsg_device *)rpmsg_info->hdl;

		err = rpmsg_send(rpdev->ept, buf, len);
		if (!err) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to rpmsg, len: %d\n",
		@@ -599,85 +610,151 @@ static int diag_rpmsg_notify_cb(struct rpmsg_device rpdev, void data, int len,
		void *priv, u32 src)
		{
		struct diag_rpmsg_info *rpmsg_info = NULL;
		struct diagfwd_info *fwd_info = NULL;
		struct diag_rpmsg_read_work *read_work = NULL;
		void *buf = NULL;
		struct rx_buff_list *rx_item;
		unsigned long flags;

		if (!rpdev \|\| !data)
		return -EINVAL;

		rpmsg_info = dev_get_drvdata(&rpdev->dev);

		if (!rpmsg_info \|\| !rpmsg_info->fwd_ctxt) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "diag: Invalid rpmsg info\n");
		return -EINVAL;
		}

		rx_item = kzalloc(sizeof(*rx_item), GFP_ATOMIC);
		if (!rx_item)
		return -ENOMEM;

		rx_item->rpmsg_rx_buf = kmemdup(data, len, GFP_ATOMIC);
		if (!rx_item->rpmsg_rx_buf) {
		kfree(rx_item);
		return -ENOMEM;
		}

		rx_item->rx_buf_size = len;
		rx_item->rpmsg_info = rpmsg_info;

		spin_lock_irqsave(&read_work_struct->rx_lock, flags);
		list_add(&rx_item->list, &read_work_struct->rx_list_head);
		spin_unlock_irqrestore(&read_work_struct->rx_lock, flags);

		queue_work(rpmsg_info->wq, &read_work_struct->work);
		return 0;
		}

		static void diag_rpmsg_notify_rx_work_fn(struct work_struct *work)
		{
		struct diag_rpmsg_info *rpmsg_info;
		struct rx_buff_list *rx_item;
		struct diagfwd_info *fwd_info;
		void *buf = NULL;
		unsigned long flags;

		spin_lock_irqsave(&read_work_struct->rx_lock, flags);
		if (!list_empty(&read_work_struct->rx_list_head)) {
		/* detach last entry */
		rx_item = list_last_entry(&read_work_struct->rx_list_head,
		struct rx_buff_list, list);

		spin_unlock_irqrestore(&read_work_struct->rx_lock, flags);

		if (!rx_item)
		return;

		rpmsg_info = rx_item->rpmsg_info;
		if (!rpmsg_info)
		return;

		fwd_info = rpmsg_info->fwd_ctxt;
		if (!fwd_info)
		return;

		if (!rpmsg_info->buf1 && !rpmsg_info->buf2) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"dropping data for %s len %d\n",
		rpmsg_info->name, len);
		return 0;
		rpmsg_info->name, rx_item->rx_buf_size);
		return;
		}

		fwd_info = rpmsg_info->fwd_ctxt;

		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"diag: received data of length: %d for p:%d, t:%d\n",
		len, rpmsg_info->peripheral, rpmsg_info->type);
		"diag: received data of length: %d, p: %d, t: %d\n",
		rx_item->rx_buf_size, rpmsg_info->peripheral,
		rpmsg_info->type);

		if (rpmsg_info->buf1 && !fwd_info->buffer_status[BUF_1_INDEX] &&
		atomic_read(&fwd_info->buf_1->in_busy)) {
		atomic_read(&(fwd_info->buf_1->in_busy))) {
		buf = rpmsg_info->buf1;
		fwd_info->buffer_status[BUF_1_INDEX] = 1;
		} else if (rpmsg_info->buf2 && !fwd_info->buffer_status[BUF_2_INDEX] &&
		} else if (rpmsg_info->buf2 &&
		!fwd_info->buffer_status[BUF_2_INDEX] &&
		atomic_read(&fwd_info->buf_2->in_busy) &&
		(fwd_info->type == TYPE_DATA)) {
		buf = rpmsg_info->buf2;
		fwd_info->buffer_status[BUF_2_INDEX] = 1;
		} else {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"Both the rpmsg buffers are busy\n");
		buf = NULL;
		}

		if (!buf)
		return 0;
		return;

		memcpy(buf, data, len);
		memcpy(buf, rx_item->rpmsg_rx_buf, rx_item->rx_buf_size);
		mutex_lock(&driver->diagfwd_channel_mutex[PERI_RPMSG]);

		read_work = kmalloc(sizeof(*read_work), GFP_ATOMIC);
		if (!read_work) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"diag: Could not allocate read_work\n");
		return 0;
		diagfwd_channel_read_done(rpmsg_info->fwd_ctxt,
		(unsigned char *)(buf), rx_item->rx_buf_size);

		mutex_unlock(&driver->diagfwd_channel_mutex[PERI_RPMSG]);

		list_del(&rx_item->list);
		kfree(rx_item->rpmsg_rx_buf);
		kfree(rx_item);
		} else {
		spin_unlock_irqrestore(&read_work_struct->rx_lock, flags);
		}
		read_work->rpmsg_info = rpmsg_info;
		read_work->ptr_read_done = buf;
		read_work->ptr_read_size = len;
		INIT_WORK(&read_work->work, diag_rpmsg_notify_rx_work_fn);
		queue_work(rpmsg_info->wq, &read_work->work);
		return 0;

		return;
		}

		static void diag_rpmsg_notify_rx_work_fn(struct work_struct *work)
		static struct diag_rpmsg_info *get_info_ptr(int type, int peripheral)
		{
		if (type == TYPE_CMD)
		return &rpmsg_cmd[peripheral];
		else if (type == TYPE_CNTL)
		return &rpmsg_cntl[peripheral];
		else if (type == TYPE_DATA)
		return &rpmsg_data[peripheral];
		else if (type == TYPE_DCI_CMD)
		return &rpmsg_dci_cmd[peripheral];
		else if (type == TYPE_DCI)
		return &rpmsg_dci[peripheral];
		else
		return NULL;
		}

		void rpmsg_mark_buffers_free(uint8_t peripheral, uint8_t type, int buf_num)
		{
		struct diag_rpmsg_read_work *read_work = container_of(work,
		struct diag_rpmsg_read_work, work);
		struct diag_rpmsg_info *rpmsg_info = read_work->rpmsg_info;
		struct diag_rpmsg_info *rpmsg_info;

		if (!rpmsg_info \|\| !rpmsg_info->hdl) {
		kfree(read_work);
		read_work = NULL;
		if ((peripheral != PERIPHERAL_WDSP) &&
		(peripheral != PERIPHERAL_WCNSS) &&
		(peripheral != PERIPHERAL_MODEM))
		return;
		}

		mutex_lock(&driver->diagfwd_channel_mutex[rpmsg_info->peripheral]);
		diagfwd_channel_read_done(rpmsg_info->fwd_ctxt,
		(unsigned char *)(read_work->ptr_read_done),
		read_work->ptr_read_size);
		rpmsg_info = get_info_ptr(type, peripheral);
		if (!rpmsg_info)
		return;

		if (read_work->ptr_read_done == rpmsg_info->buf1)
		if (buf_num == 1) {
		rpmsg_info->buf1 = NULL;
		else if (read_work->ptr_read_done == rpmsg_info->buf2)
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "marked buf1 NULL");
		} else if (buf_num == 2) {
		rpmsg_info->buf2 = NULL;
		kfree(read_work);
		read_work = NULL;
		mutex_unlock(&driver->diagfwd_channel_mutex[rpmsg_info->peripheral]);
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "marked buf2 NULL");
		}
		}

		static void rpmsg_late_init(struct diag_rpmsg_info *rpmsg_info)
		@@ -771,7 +848,9 @@ int diag_rpmsg_init(void)
		struct diag_rpmsg_info *rpmsg_info = NULL;

		for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
		if (peripheral != PERIPHERAL_WDSP)
		if ((peripheral != PERIPHERAL_WDSP) &&
		(peripheral != PERIPHERAL_WCNSS) &&
		(peripheral != PERIPHERAL_MODEM))
		continue;
		rpmsg_info = &rpmsg_cntl[peripheral];
		__diag_rpmsg_init(rpmsg_info);
		@@ -788,6 +867,18 @@ int diag_rpmsg_init(void)
		__diag_rpmsg_init(&rpmsg_dci[peripheral]);
		__diag_rpmsg_init(&rpmsg_dci_cmd[peripheral]);
		}
		read_work_struct = kmalloc(sizeof(*read_work_struct), GFP_ATOMIC);
		if (!read_work_struct) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		"diag: Could not allocate read_work\n");
		return 0;
		}
		kmemleak_not_leak(read_work_struct);

		INIT_WORK(&read_work_struct->work, diag_rpmsg_notify_rx_work_fn);
		INIT_LIST_HEAD(&read_work_struct->rx_list_head);
		spin_lock_init(&read_work_struct->rx_lock);

		return 0;
		}

		@@ -815,7 +906,9 @@ void diag_rpmsg_early_exit(void)
		int peripheral = 0;

		for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
		if (peripheral != PERIPHERAL_WDSP)
		if ((peripheral != PERIPHERAL_WDSP) &&
		(peripheral != PERIPHERAL_WCNSS) &&
		(peripheral != PERIPHERAL_MODEM))
		continue;
		mutex_lock(&driver->rpmsginfo_mutex[peripheral]);
		__diag_rpmsg_exit(&rpmsg_cntl[peripheral]);
		@@ -837,11 +930,11 @@ void diag_rpmsg_exit(void)
		}
		}

		static struct diag_rpmsg_info diag_get_rpmsg_ptr(char name)
		static struct diag_rpmsg_info diag_get_rpmsg_ptr(char name, int pid)
		{

		if (!name)
		return NULL;
		if (pid == PERIPHERAL_WDSP) {
		if (!strcmp(name, "DIAG_CMD"))
		return &rpmsg_cmd[PERIPHERAL_WDSP];
		else if (!strcmp(name, "DIAG_CTRL"))
		@@ -854,41 +947,74 @@ static struct diag_rpmsg_info diag_get_rpmsg_ptr(char name)
		return &rpmsg_dci[PERIPHERAL_WDSP];
		else
		return NULL;
		} else if (pid == PERIPHERAL_WCNSS) {
		if (!strcmp(name, "APPS_RIVA_DATA"))
		return &rpmsg_data[PERIPHERAL_WCNSS];
		else if (!strcmp(name, "APPS_RIVA_CTRL"))
		return &rpmsg_cntl[PERIPHERAL_WCNSS];
		else
		return NULL;
		} else if (pid == PERIPHERAL_MODEM) {
		if (!strcmp(name, "DIAG_CMD"))
		return &rpmsg_cmd[PERIPHERAL_MODEM];
		else if (!strcmp(name, "DIAG_CNTL"))
		return &rpmsg_cntl[PERIPHERAL_MODEM];
		else if (!strcmp(name, "DIAG"))
		return &rpmsg_data[PERIPHERAL_MODEM];
		else if (!strcmp(name, "DIAG_2_CMD"))
		return &rpmsg_dci_cmd[PERIPHERAL_MODEM];
		else if (!strcmp(name, "DIAG_2"))
		return &rpmsg_dci[PERIPHERAL_MODEM];
		else
		return NULL;
		}
		return NULL;
		}

		static int diag_rpmsg_probe(struct rpmsg_device *rpdev)
		{
		struct diag_rpmsg_info *rpmsg_info = NULL;
		int peripheral = -1;

		if (!rpdev)
		return 0;
		if (strcmp(rpdev->dev.parent->of_node->name, "wdsp"))
		return 0;

		rpmsg_info = diag_get_rpmsg_ptr(rpdev->id.name);
		if (rpmsg_info) {
		if (!strcmp(rpdev->dev.parent->of_node->name, "wdsp"))
		peripheral = PERIPHERAL_WDSP;
		else if (!strcmp(rpdev->dev.parent->of_node->name, "wcnss"))
		peripheral = PERIPHERAL_WCNSS;
		else if (!strcmp(rpdev->dev.parent->of_node->name, "modem"))
		peripheral = PERIPHERAL_MODEM;

		rpmsg_info = diag_get_rpmsg_ptr(rpdev->id.name, peripheral);
		if (rpmsg_info) {
		mutex_lock(&driver->rpmsginfo_mutex[PERI_RPMSG]);
		rpmsg_info->hdl = rpdev;
		atomic_set(&rpmsg_info->opened, 1);
		mutex_unlock(&driver->rpmsginfo_mutex[PERI_RPMSG]);

		dev_set_drvdata(&rpdev->dev, rpmsg_info);
		diagfwd_channel_read(rpmsg_info->fwd_ctxt);
		queue_work(rpmsg_info->wq, &rpmsg_info->open_work);
		}

		return 0;
		}

		static void diag_rpmsg_remove(struct rpmsg_device *rpdev)
		{
		struct diag_rpmsg_info *rpmsg_info = NULL;
		int peripheral = -1;

		if (!rpdev)
		return;

		rpmsg_info = diag_get_rpmsg_ptr(rpdev->id.name);
		if (!strcmp(rpdev->dev.parent->of_node->name, "wdsp"))
		peripheral = PERIPHERAL_WDSP;
		else if (!strcmp(rpdev->dev.parent->of_node->name, "wcnss"))
		peripheral = PERIPHERAL_WCNSS;
		else if (!strcmp(rpdev->dev.parent->of_node->name, "modem"))
		peripheral = PERIPHERAL_MODEM;

		rpmsg_info = diag_get_rpmsg_ptr(rpdev->id.name, peripheral);
		if (rpmsg_info) {
		mutex_lock(&driver->rpmsginfo_mutex[PERI_RPMSG]);
		atomic_set(&rpmsg_info->opened, 0);
		@@ -898,6 +1024,12 @@ static void diag_rpmsg_remove(struct rpmsg_device *rpdev)
		}

		static struct rpmsg_device_id rpmsg_diag_table[] = {
		{ .name = "APPS_RIVA_DATA" },
		{ .name = "APPS_RIVA_CTRL" },
		{ .name = "DIAG" },
		{ .name = "DIAG_CNTL" },
		{ .name = "DIAG_2" },
		{ .name = "DIAG_2_CMD" },
		{ .name = "DIAG_CMD" },
		{ .name = "DIAG_CTRL" },
		{ .name = "DIAG_DATA" },

drivers/char/diag/diagfwd_rpmsg.h

+2 −1

Original line number	Diff line number	Diff line
		/* SPDX-License-Identifier: GPL-2.0 */
		/* Copyright (c) 2017-2018, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2017-2018, 2021, The Linux Foundation. All rights reserved.
		*/

		#ifndef DIAGFWD_RPMSG_H
		@@ -43,5 +43,6 @@ int diag_rpmsg_init(void);
		void diag_rpmsg_early_exit(void);
		void diag_rpmsg_invalidate(void ctxt, struct diagfwd_info fwd_ctxt);
		int diag_rpmsg_check_state(void *ctxt);
		void rpmsg_mark_buffers_free(uint8_t peripheral, uint8_t type, int buf_num);

		#endif