Merge "diag: Enable diag over rpmsg communication for wcnss" (1fac48b8) · Commits · e / devices / android_kernel_xiaomi_nabu

arch/arm64/boot/dts/qcom/sdm429.dtsi

+7 −0

Original line number	Diff line number	Diff line
		@@ -1043,6 +1043,13 @@
		qcom,net-id = <1>;
		qcom,low-latency;
		};

		qcom,diag {
		qcom,smd-channels = "APPS_RIVA_DATA";
		};
		qcom,diag_cntl {
		qcom,smd-channels = "APPS_RIVA_CTRL";
		};
		};

		adsp {

drivers/char/diag/diagfwd_rpmsg.c

+173 −98

Original line number	Diff line number	Diff line
		/* Copyright (c) 2017-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2017-2020, The Linux Foundation. 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
		@@ -32,11 +32,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] = {
		@@ -62,7 +73,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
		@@ -128,7 +139,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
		@@ -471,7 +482,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;
		}

		@@ -496,14 +507,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)
		@@ -526,6 +536,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",
		@@ -607,85 +618,117 @@ 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);
		list_del(&rx_item->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;

		memcpy(buf, data, len);

		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;
		}
		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;
		}

		static void diag_rpmsg_notify_rx_work_fn(struct work_struct *work)
		{
		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;

		if (!rpmsg_info \|\| !rpmsg_info->hdl) {
		kfree(read_work);
		read_work = NULL;
		return;
		}

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

		diagfwd_channel_read_done(rpmsg_info->fwd_ctxt,
		(unsigned char *)(read_work->ptr_read_done),
		read_work->ptr_read_size);
		(unsigned char *)(buf), rx_item->rx_buf_size);

		if (read_work->ptr_read_done == rpmsg_info->buf1)
		if (buf == rpmsg_info->buf1)
		rpmsg_info->buf1 = NULL;
		else if (read_work->ptr_read_done == rpmsg_info->buf2)
		else if (buf == rpmsg_info->buf2)
		rpmsg_info->buf2 = NULL;
		kfree(read_work);
		read_work = NULL;
		mutex_unlock(&driver->diagfwd_channel_mutex[rpmsg_info->peripheral]);

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

		kfree(rx_item->rpmsg_rx_buf);
		kfree(rx_item);
		} else {
		spin_unlock_irqrestore(&read_work_struct->rx_lock, flags);
		}

		return;
		}

		static void rpmsg_late_init(struct diag_rpmsg_info *rpmsg_info)
		@@ -779,7 +822,8 @@ 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))
		continue;
		rpmsg_info = &rpmsg_cntl[peripheral];
		__diag_rpmsg_init(rpmsg_info);
		@@ -796,6 +840,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;
		}

		@@ -823,7 +879,8 @@ 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))
		continue;
		mutex_lock(&driver->rpmsginfo_mutex[peripheral]);
		__diag_rpmsg_exit(&rpmsg_cntl[peripheral]);
		@@ -845,11 +902,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"))
		@@ -862,41 +919,57 @@ 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;
		}
		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;

		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;

		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);
		@@ -906,6 +979,8 @@ 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_CMD" },
		{ .name = "DIAG_CTRL" },
		{ .name = "DIAG_DATA" },

drivers/rpmsg/qcom_smd.c

+19 −2

Original line number	Diff line number	Diff line
		@@ -169,9 +169,9 @@ struct qcom_smd_endpoint {
		struct qcom_smd_channel *qsch;
		};

		#define to_smd_device(_rpdev) container_of(_rpdev, struct qcom_smd_device, rpdev)
		#define to_smd_device(r) container_of(r, struct qcom_smd_device, rpdev)
		#define to_smd_edge(d) container_of(d, struct qcom_smd_edge, dev)
		#define to_smd_endpoint(ept) container_of(ept, struct qcom_smd_endpoint, ept)
		#define to_smd_endpoint(e) container_of(e, struct qcom_smd_endpoint, ept)

		/**
		* struct qcom_smd_channel - smd channel struct
		@@ -1021,9 +1021,26 @@ static struct device_node qcom_smd_match_channel(struct device_node edge_node,

		return NULL;
		}
		static int qcom_smd_announce_create(struct rpmsg_device *rpdev)
		{
		struct qcom_smd_endpoint *qept = to_smd_endpoint(rpdev->ept);
		struct qcom_smd_channel *channel = qept->qsch;
		unsigned long flags;
		bool kick_state;

		spin_lock_irqsave(&channel->recv_lock, flags);
		kick_state = qcom_smd_channel_intr(channel);
		spin_unlock_irqrestore(&channel->recv_lock, flags);

		if (kick_state)
		schedule_work(&channel->edge->state_work);

		return 0;
		}

		static const struct rpmsg_device_ops qcom_smd_device_ops = {
		.create_ept = qcom_smd_create_ept,
		.announce_create = qcom_smd_announce_create,
		};

		static const struct rpmsg_endpoint_ops qcom_smd_endpoint_ops = {