Merge "coresight: add sw usb mode support for tmc" (3ba80746) · Commits · e / devices / android_kernel_fairphone_FP5

drivers/hwtracing/coresight/coresight-byte-cntr.c

+306 −5

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-2020, The Linux Foundation. All rights reserved.
		*
		* Description: CoreSight Trace Memory Controller driver
		*/
		@@ -10,11 +10,19 @@
		#include <linux/moduleparam.h>
		#include <linux/delay.h>
		#include <linux/uaccess.h>
		#include <linux/usb/usb_qdss.h>
		#include <linux/time.h>
		#include <linux/slab.h>

		#include "coresight-byte-cntr.h"
		#include "coresight-priv.h"
		#include "coresight-tmc.h"

		#define USB_BLK_SIZE 65536
		#define USB_SG_NUM (USB_BLK_SIZE / PAGE_SIZE)
		#define USB_BUF_NUM 255
		#define USB_TIME_OUT (5 * HZ)

		static struct tmc_drvdata *tmcdrvdata;

		static void tmc_etr_read_bytes(struct byte_cntr byte_cntr_data, loff_t ppos,
		@@ -39,11 +47,16 @@ static irqreturn_t etr_handler(int irq, void *data)
		{
		struct byte_cntr *byte_cntr_data = data;

		if (tmcdrvdata->out_mode == TMC_ETR_OUT_MODE_USB
		&& byte_cntr_data->sw_usb) {
		atomic_inc(&byte_cntr_data->irq_cnt);
		wake_up(&byte_cntr_data->usb_wait_wq);
		} else if (tmcdrvdata->out_mode == TMC_ETR_OUT_MODE_MEM) {
		atomic_inc(&byte_cntr_data->irq_cnt);

		wake_up(&byte_cntr_data->wq);

		}
		return IRQ_HANDLED;

		}

		static void tmc_etr_flush_bytes(loff_t ppos, size_t bytes, size_t len)
		@@ -177,6 +190,56 @@ static int tmc_etr_byte_cntr_release(struct inode in, struct file fp)
		return 0;
		}

		static int usb_bypass_start(struct byte_cntr *byte_cntr_data)
		{
		if (!byte_cntr_data)
		return -ENOMEM;

		mutex_lock(&byte_cntr_data->usb_bypass_lock);


		dev_info(&tmcdrvdata->csdev->dev,
		"%s: Start usb bypass\n", __func__);
		if (!tmcdrvdata->enable) {
		mutex_unlock(&byte_cntr_data->usb_bypass_lock);
		return -EINVAL;
		}

		atomic_set(&byte_cntr_data->usb_free_buf, USB_BUF_NUM);
		byte_cntr_data->offset = tmcdrvdata->etr_buf->offset;
		byte_cntr_data->read_active = true;
		/*
		* IRQ is a '8- byte' counter and to observe interrupt at
		* 'block_size' bytes of data
		*/
		coresight_csr_set_byte_cntr(byte_cntr_data->csr, USB_BLK_SIZE / 8);

		atomic_set(&byte_cntr_data->irq_cnt, 0);
		mutex_unlock(&byte_cntr_data->usb_bypass_lock);

		return 0;
		}

		void usb_bypass_stop(struct byte_cntr *byte_cntr_data)
		{
		if (!byte_cntr_data)
		return;

		mutex_lock(&byte_cntr_data->usb_bypass_lock);
		if (byte_cntr_data->read_active)
		byte_cntr_data->read_active = false;
		else {
		mutex_unlock(&byte_cntr_data->usb_bypass_lock);
		return;
		}
		wake_up(&byte_cntr_data->usb_wait_wq);
		pr_info("coresight: stop usb bypass\n");
		coresight_csr_set_byte_cntr(byte_cntr_data->csr, 0);
		mutex_unlock(&byte_cntr_data->usb_bypass_lock);

		}
		EXPORT_SYMBOL(usb_bypass_stop);

		static int tmc_etr_byte_cntr_open(struct inode in, struct file fp)
		{
		struct byte_cntr *byte_cntr_data =
		@@ -260,6 +323,237 @@ static int byte_cntr_register_chardev(struct byte_cntr *byte_cntr_data)
		return ret;
		}

		static int usb_transfer_small_packet(struct qdss_request *usb_req,
		struct byte_cntr drvdata, size_t small_size)
		{
		int ret = 0;
		struct etr_buf *etr_buf = tmcdrvdata->etr_buf;
		size_t req_size, actual;
		long w_offset;

		w_offset = tmc_sg_get_rwp_offset(tmcdrvdata);
		req_size = ((w_offset < drvdata->offset) ? etr_buf->size : 0) +
		w_offset - drvdata->offset;
		req_size = ((req_size + *small_size) < USB_BLK_SIZE) ? req_size :
		(USB_BLK_SIZE - *small_size);

		while (req_size > 0) {

		usb_req = kzalloc(sizeof(*usb_req), GFP_KERNEL);
		if (!usb_req) {
		ret = -EFAULT;
		goto out;
		}

		actual = tmc_etr_buf_get_data(etr_buf, drvdata->offset,
		req_size, &usb_req->buf);
		usb_req->length = actual;
		drvdata->usb_req = usb_req;
		req_size -= actual;

		if ((drvdata->offset + actual) >= tmcdrvdata->size)
		drvdata->offset = 0;
		else
		drvdata->offset += actual;

		*small_size += actual;

		if (atomic_read(&drvdata->usb_free_buf) > 0) {
		ret = usb_qdss_write(tmcdrvdata->usbch, usb_req);

		if (ret) {
		kfree(usb_req);
		usb_req = NULL;
		drvdata->usb_req = NULL;
		dev_err(&tmcdrvdata->csdev->dev,
		"Write data failed:%d\n", ret);
		goto out;
		}

		atomic_dec(&drvdata->usb_free_buf);
		} else {
		dev_dbg(&tmcdrvdata->csdev->dev,
		"Drop data, offset = %d, len = %d\n",
		drvdata->offset, req_size);
		kfree(usb_req);
		drvdata->usb_req = NULL;
		}
		}

		out:
		return ret;
		}

		static void usb_read_work_fn(struct work_struct *work)
		{
		int ret, i, seq = 0;
		struct qdss_request *usb_req = NULL;
		struct etr_buf *etr_buf = tmcdrvdata->etr_buf;
		size_t actual, req_size, req_sg_num, small_size = 0;
		char *buf;
		struct byte_cntr *drvdata =
		container_of(work, struct byte_cntr, read_work);


		while (tmcdrvdata->enable
		&& tmcdrvdata->out_mode == TMC_ETR_OUT_MODE_USB) {
		if (!atomic_read(&drvdata->irq_cnt)) {
		ret = wait_event_interruptible_timeout(
		drvdata->usb_wait_wq,
		atomic_read(&drvdata->irq_cnt) > 0
		\|\| !tmcdrvdata->enable \|\| tmcdrvdata->out_mode
		!= TMC_ETR_OUT_MODE_USB
		\|\| !drvdata->read_active, USB_TIME_OUT);
		if (ret == -ERESTARTSYS \|\| !tmcdrvdata->enable
		\|\| tmcdrvdata->out_mode != TMC_ETR_OUT_MODE_USB
		\|\| !drvdata->read_active)
		break;

		if (ret == 0) {
		ret = usb_transfer_small_packet(usb_req,
		drvdata, &small_size);
		if (ret && ret != -EAGAIN)
		return;
		continue;
		}
		}

		req_size = USB_BLK_SIZE - small_size;
		small_size = 0;

		if (req_size > 0) {
		seq++;
		req_sg_num = (req_size - 1) / PAGE_SIZE + 1;
		usb_req = kzalloc(sizeof(*usb_req), GFP_KERNEL);
		if (!usb_req)
		return;
		usb_req->sg = kcalloc(req_sg_num,
		sizeof(*(usb_req->sg)), GFP_KERNEL);
		if (!usb_req->sg) {
		kfree(usb_req);
		usb_req = NULL;
		return;
		}

		for (i = 0; i < req_sg_num; i++) {
		actual = tmc_etr_buf_get_data(etr_buf,
		drvdata->offset,
		PAGE_SIZE, &buf);

		if (actual <= 0) {
		kfree(usb_req->sg);
		kfree(usb_req);
		usb_req = NULL;
		dev_err(&tmcdrvdata->csdev->dev, "No data in ETR\n");
		return;
		}

		sg_set_buf(&usb_req->sg[i], buf, actual);

		if (i == 0)
		usb_req->buf = buf;
		if (i == req_sg_num - 1)
		sg_mark_end(&usb_req->sg[i]);

		if ((drvdata->offset + actual) >=
		tmcdrvdata->size)
		drvdata->offset = 0;
		else
		drvdata->offset += actual;
		}

		usb_req->length = req_size;
		drvdata->usb_req = usb_req;
		usb_req->num_sgs = i;

		if (atomic_read(&drvdata->usb_free_buf) > 0) {
		ret = usb_qdss_write(tmcdrvdata->usbch,
		drvdata->usb_req);
		if (ret) {
		kfree(usb_req->sg);
		kfree(usb_req);
		usb_req = NULL;
		drvdata->usb_req = NULL;
		dev_err(&tmcdrvdata->csdev->dev,
		"Write data failed:%d\n", ret);
		if (ret == -EAGAIN)
		continue;
		return;
		}
		atomic_dec(&drvdata->usb_free_buf);

		} else {
		dev_dbg(&tmcdrvdata->csdev->dev,
		"Drop data, offset = %d, seq = %d, irq = %d\n",
		drvdata->offset, seq,
		atomic_read(&drvdata->irq_cnt));
		kfree(usb_req->sg);
		kfree(usb_req);
		drvdata->usb_req = NULL;
		}
		}

		if (atomic_read(&drvdata->irq_cnt) > 0)
		atomic_dec(&drvdata->irq_cnt);
		}
		dev_err(&tmcdrvdata->csdev->dev, "TMC has been stopped.\n");
		}

		static void usb_write_done(struct byte_cntr *drvdata,
		struct qdss_request *d_req)
		{
		atomic_inc(&drvdata->usb_free_buf);
		if (d_req->status)
		pr_err_ratelimited("USB write failed err:%d\n", d_req->status);
		kfree(d_req->sg);
		kfree(d_req);
		}

		void usb_bypass_notifier(void *priv, unsigned int event,
		struct qdss_request d_req, struct usb_qdss_ch ch)
		{
		struct byte_cntr *drvdata = priv;

		if (!drvdata)
		return;

		switch (event) {
		case USB_QDSS_CONNECT:
		usb_qdss_alloc_req(ch, USB_BUF_NUM, 0);
		usb_bypass_start(drvdata);
		queue_work(drvdata->usb_wq, &(drvdata->read_work));
		break;

		case USB_QDSS_DISCONNECT:
		usb_bypass_stop(drvdata);
		break;

		case USB_QDSS_DATA_WRITE_DONE:
		usb_write_done(drvdata, d_req);
		break;

		default:
		break;
		}
		}
		EXPORT_SYMBOL(usb_bypass_notifier);


		static int usb_bypass_init(struct byte_cntr *byte_cntr_data)
		{
		byte_cntr_data->usb_wq = create_singlethread_workqueue("byte-cntr");
		if (!byte_cntr_data->usb_wq)
		return -ENOMEM;

		byte_cntr_data->offset = 0;
		mutex_init(&byte_cntr_data->usb_bypass_lock);
		init_waitqueue_head(&byte_cntr_data->usb_wait_wq);
		atomic_set(&byte_cntr_data->usb_free_buf, USB_BUF_NUM);
		INIT_WORK(&(byte_cntr_data->read_work), usb_read_work_fn);

		return 0;
		}

		struct byte_cntr byte_cntr_init(struct amba_device adev,
		struct tmc_drvdata *drvdata)
		{
		@@ -277,6 +571,13 @@ struct byte_cntr byte_cntr_init(struct amba_device adev,
		if (!byte_cntr_data)
		return NULL;

		byte_cntr_data->sw_usb = of_property_read_bool(np, "qcom,sw-usb");
		if (byte_cntr_data->sw_usb) {
		ret = usb_bypass_init(byte_cntr_data);
		if (ret)
		return NULL;
		}

		ret = devm_request_irq(dev, byte_cntr_irq, etr_handler,
		IRQF_TRIGGER_RISING \| IRQF_SHARED,
		"tmc-etr", byte_cntr_data);

drivers/hwtracing/coresight/coresight-byte-cntr.h

+11 −0

Original line number	Diff line number	Diff line
		@@ -14,16 +14,27 @@ struct byte_cntr {
		struct class *driver_class;
		bool enable;
		bool read_active;
		bool sw_usb;
		uint32_t byte_cntr_value;
		uint32_t block_size;
		int byte_cntr_irq;
		atomic_t irq_cnt;
		atomic_t usb_free_buf;
		wait_queue_head_t wq;
		wait_queue_head_t usb_wait_wq;
		struct workqueue_struct *usb_wq;
		struct qdss_request *usb_req;
		struct work_struct read_work;
		struct mutex usb_bypass_lock;
		struct mutex byte_cntr_lock;
		struct coresight_csr *csr;
		unsigned long offset;
		};

		extern void usb_bypass_notifier(void *priv, unsigned int event,
		struct qdss_request d_req, struct usb_qdss_ch ch);
		extern void tmc_etr_byte_cntr_start(struct byte_cntr *byte_cntr_data);
		extern void tmc_etr_byte_cntr_stop(struct byte_cntr *byte_cntr_data);
		extern void usb_bypass_stop(struct byte_cntr *byte_cntr_data);

		#endif

drivers/hwtracing/coresight/coresight-tmc-etr.c

+69 −17

Original line number	Diff line number	Diff line
		@@ -249,6 +249,20 @@ void tmc_free_sg_table(struct tmc_sg_table *sg_table)
		tmc_free_data_pages(sg_table);
		}

		long tmc_sg_get_rwp_offset(struct tmc_drvdata *drvdata)
		{
		struct etr_buf *etr_buf = drvdata->etr_buf;
		struct etr_sg_table *etr_table = etr_buf->private;
		struct tmc_sg_table *table = etr_table->sg_table;
		u64 rwp;
		long w_offset;

		rwp = tmc_read_rwp(drvdata);
		w_offset = tmc_sg_get_data_page_offset(table, rwp);

		return w_offset;
		}

		/*
		* Alloc pages for the table. Since this will be used by the device,
		* allocate the pages closer to the device (i.e, dev_to_node(dev)
		@@ -1444,11 +1458,28 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev)
		*/
		/* Allocate memory with the locks released */
		free_buf = new_buf = tmc_etr_setup_sysfs_buf(drvdata);
		if (IS_ERR(new_buf)) {
		if (IS_ERR(new_buf))
		return -ENOMEM;
		}
		coresight_cti_map_trigout(drvdata->cti_flush, 3, 0);
		coresight_cti_map_trigin(drvdata->cti_reset, 2, 0);
		} else if (drvdata->byte_cntr->sw_usb) {
		if (!drvdata->etr_buf) {
		free_buf = new_buf =
		tmc_etr_setup_sysfs_buf(drvdata);
		if (IS_ERR(new_buf))
		return -ENOMEM;
		}
		coresight_cti_map_trigout(drvdata->cti_flush, 3, 0);
		coresight_cti_map_trigin(drvdata->cti_reset, 0, 0);

		drvdata->usbch = usb_qdss_open("qdss_mdm",
		drvdata->byte_cntr,
		usb_bypass_notifier);
		if (IS_ERR_OR_NULL(drvdata->usbch)) {
		dev_err(&csdev->dev, "usb_qdss_open failed\n");
		return -ENODEV;
		}

		} else {
		drvdata->usbch = usb_qdss_open("qdss", drvdata,
		usb_notifier);
		@@ -1484,7 +1515,9 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev)
		free_buf = sysfs_buf;
		drvdata->sysfs_buf = new_buf;
		}
		if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM) {
		if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM \|\|
		(drvdata->out_mode == TMC_ETR_OUT_MODE_USB
		&& drvdata->byte_cntr->sw_usb)) {
		ret = tmc_etr_enable_hw(drvdata, drvdata->sysfs_buf);
		}
		if (!ret) {
		@@ -1969,13 +2002,22 @@ static int _tmc_disable_etr_sink(struct coresight_device *csdev,
		WARN_ON_ONCE(drvdata->mode == CS_MODE_DISABLED);
		if (drvdata->mode != CS_MODE_DISABLED) {
		if (drvdata->out_mode == TMC_ETR_OUT_MODE_USB) {
		if (!drvdata->byte_cntr->sw_usb) {
		__tmc_etr_disable_to_bam(drvdata);
		spin_unlock_irqrestore(&drvdata->spinlock, flags);
		spin_unlock_irqrestore(&drvdata->spinlock,
		flags);
		tmc_etr_bam_disable(drvdata);
		usb_qdss_close(drvdata->usbch);
		drvdata->usbch = NULL;
		drvdata->mode = CS_MODE_DISABLED;
		goto out;
		} else {
		spin_unlock_irqrestore(&drvdata->spinlock,
		flags);
		usb_qdss_close(drvdata->usbch);
		spin_lock_irqsave(&drvdata->spinlock, flags);
		tmc_etr_disable_hw(drvdata);
		}
		} else {
		tmc_etr_disable_hw(drvdata);
		}
		@@ -1989,7 +2031,16 @@ static int _tmc_disable_etr_sink(struct coresight_device *csdev,

		spin_unlock_irqrestore(&drvdata->spinlock, flags);

		if ((drvdata->out_mode == TMC_ETR_OUT_MODE_USB
		&& drvdata->byte_cntr->sw_usb)
		\|\| drvdata->out_mode == TMC_ETR_OUT_MODE_MEM) {
		if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM)
		tmc_etr_byte_cntr_stop(drvdata->byte_cntr);
		else {
		usb_bypass_stop(drvdata->byte_cntr);
		flush_workqueue(drvdata->byte_cntr->usb_wq);
		drvdata->usbch = NULL;
		}
		coresight_cti_unmap_trigin(drvdata->cti_reset, 2, 0);
		coresight_cti_unmap_trigout(drvdata->cti_flush, 3, 0);
		/* Free memory outside the spinlock if need be */
		@@ -1997,6 +2048,7 @@ static int _tmc_disable_etr_sink(struct coresight_device *csdev,
		tmc_etr_free_sysfs_buf(drvdata->etr_buf);
		drvdata->etr_buf = NULL;
		}
		}
		out:
		dev_info(&csdev->dev, "TMC-ETR disabled\n");
		return 0;

drivers/hwtracing/coresight/coresight-tmc.h

+1 −1

Original line number	Diff line number	Diff line
		@@ -339,7 +339,7 @@ ssize_t tmc_etr_get_sysfs_trace(struct tmc_drvdata *drvdata,
		ssize_t tmc_etr_buf_get_data(struct etr_buf *etr_buf,
		u64 offset, size_t len, char **bufpp);
		int tmc_etr_switch_mode(struct tmc_drvdata drvdata, const char out_mode);

		long tmc_sg_get_rwp_offset(struct tmc_drvdata *drvdata);

		#define TMC_REG_PAIR(name, lo_off, hi_off) \
		static inline u64 \