From c92524b8a923ac56cc955238f86186bb75319813 Mon Sep 17 00:00:00 2001 From: Mao Jinlong Date: Tue, 28 Apr 2020 23:18:27 +0800 Subject: [PATCH] coresight: add sw usb mode support for tmc Add SW USB mode support for TMC to use software interface to send data from TMC to USB. Change-Id: I8e4dee6cfd037d8bb81ea27ad2ed996e9980720c Signed-off-by: Shaoqing Liu Signed-off-by: Tingwei Zhang Signed-off-by: Mao Jinlong --- .../hwtracing/coresight/coresight-byte-cntr.c | 311 +++++++++++++++++- .../hwtracing/coresight/coresight-byte-cntr.h | 11 + .../hwtracing/coresight/coresight-tmc-etr.c | 86 ++++- drivers/hwtracing/coresight/coresight-tmc.h | 2 +- 4 files changed, 387 insertions(+), 23 deletions(-) diff --git a/drivers/hwtracing/coresight/coresight-byte-cntr.c b/drivers/hwtracing/coresight/coresight-byte-cntr.c index 1d7c217cd3d5..6fcff596935d 100644 --- a/drivers/hwtracing/coresight/coresight-byte-cntr.c +++ b/drivers/hwtracing/coresight/coresight-byte-cntr.c @@ -1,5 +1,5 @@ // 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 #include #include +#include +#include +#include #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; - atomic_inc(&byte_cntr_data->irq_cnt); - - wake_up(&byte_cntr_data->wq); - + 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 @@ exit_unreg_chrdev_region: 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); diff --git a/drivers/hwtracing/coresight/coresight-byte-cntr.h b/drivers/hwtracing/coresight/coresight-byte-cntr.h index 6dd931fd01f6..ff79167bb095 100644 --- a/drivers/hwtracing/coresight/coresight-byte-cntr.h +++ b/drivers/hwtracing/coresight/coresight-byte-cntr.h @@ -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 diff --git a/drivers/hwtracing/coresight/coresight-tmc-etr.c b/drivers/hwtracing/coresight/coresight-tmc-etr.c index 03f079a2c395..bb133414e743 100644 --- a/drivers/hwtracing/coresight/coresight-tmc-etr.c +++ b/drivers/hwtracing/coresight/coresight-tmc-etr.c @@ -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) @@ -1431,11 +1445,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); @@ -1471,7 +1502,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) { @@ -1956,13 +1989,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) { - __tmc_etr_disable_to_bam(drvdata); - 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; + if (!drvdata->byte_cntr->sw_usb) { + __tmc_etr_disable_to_bam(drvdata); + 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); } @@ -1976,13 +2018,23 @@ static int _tmc_disable_etr_sink(struct coresight_device *csdev, spin_unlock_irqrestore(&drvdata->spinlock, flags); - tmc_etr_byte_cntr_stop(drvdata->byte_cntr); - 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 */ - if (drvdata->etr_buf) { - tmc_etr_free_sysfs_buf(drvdata->etr_buf); - drvdata->etr_buf = NULL; + 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 */ + if (drvdata->etr_buf) { + tmc_etr_free_sysfs_buf(drvdata->etr_buf); + drvdata->etr_buf = NULL; + } } out: dev_info(&csdev->dev, "TMC-ETR disabled\n"); diff --git a/drivers/hwtracing/coresight/coresight-tmc.h b/drivers/hwtracing/coresight/coresight-tmc.h index b9543048aafc..74e36a6d0059 100644 --- a/drivers/hwtracing/coresight/coresight-tmc.h +++ b/drivers/hwtracing/coresight/coresight-tmc.h @@ -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 \