From 4ac10ebfb01a53bccab96b56b19442082bb5c292 Mon Sep 17 00:00:00 2001 From: Yuanfang Zhang Date: Fri, 11 Dec 2020 14:44:30 +0800 Subject: [PATCH] coresight-tmc: Add support of pcie mode Add pcie mode to tmc-etr to support diag over stm over pcie. etr data will be written to qdss mhi out channel, when pcie mode is enabled. Change-Id: Ibca55f9221ff460a7d473f23cc05c22125e85961 Signed-off-by: Mao Jinlong Signed-off-by: Yuanfang Zhang --- .../hwtracing/coresight/coresight-byte-cntr.c | 209 ++++++++++++++++++ .../hwtracing/coresight/coresight-byte-cntr.h | 14 +- .../hwtracing/coresight/coresight-tmc-etr.c | 23 +- drivers/hwtracing/coresight/coresight-tmc.h | 4 + 4 files changed, 244 insertions(+), 6 deletions(-) diff --git a/drivers/hwtracing/coresight/coresight-byte-cntr.c b/drivers/hwtracing/coresight/coresight-byte-cntr.c index 18a44f24384d..6da6771aaf72 100644 --- a/drivers/hwtracing/coresight/coresight-byte-cntr.c +++ b/drivers/hwtracing/coresight/coresight-byte-cntr.c @@ -22,6 +22,7 @@ #define USB_SG_NUM (USB_BLK_SIZE / PAGE_SIZE) #define USB_BUF_NUM 255 #define USB_TIME_OUT (5 * HZ) +#define PCIE_BLK_SIZE 32768 static struct tmc_drvdata *tmcdrvdata; @@ -54,6 +55,9 @@ static irqreturn_t etr_handler(int irq, void *data) } else if (tmcdrvdata->out_mode == TMC_ETR_OUT_MODE_MEM) { atomic_inc(&byte_cntr_data->irq_cnt); wake_up(&byte_cntr_data->wq); + } else if (tmcdrvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) { + atomic_inc(&byte_cntr_data->irq_cnt); + wake_up(&byte_cntr_data->pcie_wait_wq); } return IRQ_HANDLED; @@ -176,6 +180,49 @@ void tmc_etr_byte_cntr_stop(struct byte_cntr *byte_cntr_data) } EXPORT_SYMBOL(tmc_etr_byte_cntr_stop); +static void etr_pcie_close_channel(struct byte_cntr *byte_cntr_data) +{ + if (!byte_cntr_data) + return; + + mutex_lock(&byte_cntr_data->byte_cntr_lock); + mhi_dev_close_channel(byte_cntr_data->out_handle); + byte_cntr_data->pcie_chan_opened = false; + mutex_unlock(&byte_cntr_data->byte_cntr_lock); +} + +int etr_pcie_start(struct byte_cntr *byte_cntr_data) +{ + if (!byte_cntr_data) + return -ENOMEM; + + mutex_lock(&byte_cntr_data->byte_cntr_lock); + coresight_csr_set_byte_cntr(byte_cntr_data->csr, PCIE_BLK_SIZE / 8); + atomic_set(&byte_cntr_data->irq_cnt, 0); + mutex_unlock(&byte_cntr_data->byte_cntr_lock); + + if (!byte_cntr_data->pcie_chan_opened) + queue_work(byte_cntr_data->pcie_wq, + &byte_cntr_data->pcie_open_work); + + queue_work(byte_cntr_data->pcie_wq, &byte_cntr_data->pcie_write_work); + return 0; +} +EXPORT_SYMBOL(etr_pcie_start); + +void etr_pcie_stop(struct byte_cntr *byte_cntr_data) +{ + if (!byte_cntr_data) + return; + + etr_pcie_close_channel(byte_cntr_data); + wake_up(&byte_cntr_data->pcie_wait_wq); + + mutex_lock(&byte_cntr_data->byte_cntr_lock); + coresight_csr_set_byte_cntr(byte_cntr_data->csr, 0); + mutex_unlock(&byte_cntr_data->byte_cntr_lock); +} +EXPORT_SYMBOL(etr_pcie_stop); static int tmc_etr_byte_cntr_release(struct inode *in, struct file *fp) { @@ -608,6 +655,167 @@ void usb_bypass_notifier(void *priv, unsigned int event, } EXPORT_SYMBOL(usb_bypass_notifier); +static void etr_pcie_client_cb(struct mhi_dev_client_cb_data *cb_data) +{ + struct byte_cntr *byte_cntr_data = NULL; + + if (!cb_data) + return; + + byte_cntr_data = cb_data->user_data; + if (!byte_cntr_data) + return; + + switch (cb_data->ctrl_info) { + case MHI_STATE_CONNECTED: + if (cb_data->channel == byte_cntr_data->pcie_out_chan) { + dev_dbg(&tmcdrvdata->csdev->dev, "PCIE out channel connected.\n"); + queue_work(byte_cntr_data->pcie_wq, + &byte_cntr_data->pcie_open_work); + } + + break; + case MHI_STATE_DISCONNECTED: + if (cb_data->channel == byte_cntr_data->pcie_out_chan) { + dev_dbg(&tmcdrvdata->csdev->dev, + "PCIE out channel disconnected.\n"); + etr_pcie_close_channel(byte_cntr_data); + } + break; + default: + break; + } +} + +static void etr_pcie_write_complete_cb(void *req) +{ + struct mhi_req *mreq = req; + + if (!mreq) + return; + kfree(req); +} + +static void etr_pcie_open_work_fn(struct work_struct *work) +{ + int ret = 0; + struct byte_cntr *byte_cntr_data = container_of(work, + struct byte_cntr, + pcie_open_work); + + if (!byte_cntr_data) + return; + + /* Open write channel*/ + ret = mhi_dev_open_channel(byte_cntr_data->pcie_out_chan, + &byte_cntr_data->out_handle, + NULL); + if (ret < 0) { + dev_err(&tmcdrvdata->csdev->dev, "%s: open pcie out channel fail %d\n", + __func__, ret); + } else { + dev_dbg(&tmcdrvdata->csdev->dev, + "Open pcie out channel successfully\n"); + mutex_lock(&byte_cntr_data->byte_cntr_lock); + byte_cntr_data->pcie_chan_opened = true; + mutex_unlock(&byte_cntr_data->byte_cntr_lock); + } + +} + +static void etr_pcie_write_work_fn(struct work_struct *work) +{ + int ret = 0; + struct mhi_req *req; + size_t actual; + int bytes_to_write; + char *buf; + + struct byte_cntr *byte_cntr_data = container_of(work, + struct byte_cntr, + pcie_write_work); + + while (tmcdrvdata->enable + && tmcdrvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) { + if (!atomic_read(&byte_cntr_data->irq_cnt)) { + ret = wait_event_interruptible( + byte_cntr_data->pcie_wait_wq, + atomic_read(&byte_cntr_data->irq_cnt) > 0 + || !tmcdrvdata->enable + || tmcdrvdata->out_mode != TMC_ETR_OUT_MODE_PCIE + || !byte_cntr_data->pcie_chan_opened); + if (ret == -ERESTARTSYS || !tmcdrvdata->enable + || tmcdrvdata->out_mode != TMC_ETR_OUT_MODE_PCIE + || !byte_cntr_data->pcie_chan_opened) + break; + } + + actual = PCIE_BLK_SIZE; + buf = (char *)(tmcdrvdata->buf + byte_cntr_data->offset); + req = kzalloc(sizeof(*req), GFP_KERNEL); + if (!req) + break; + + tmc_etr_read_bytes(byte_cntr_data, (loff_t *)&byte_cntr_data->offset, + PCIE_BLK_SIZE, &actual, &buf); + + if (actual <= 0) { + kfree(req); + req = NULL; + break; + } + + req->buf = buf; + req->client = byte_cntr_data->out_handle; + req->context = byte_cntr_data; + req->len = actual; + req->chan = byte_cntr_data->pcie_out_chan; + req->mode = DMA_ASYNC; + req->client_cb = etr_pcie_write_complete_cb; + req->snd_cmpl = 1; + + bytes_to_write = mhi_dev_write_channel(req); + if (bytes_to_write != PCIE_BLK_SIZE) { + dev_err(&tmcdrvdata->csdev->dev, "Write error %d\n", + bytes_to_write); + + kfree(req); + req = NULL; + break; + } + + mutex_lock(&byte_cntr_data->byte_cntr_lock); + if (byte_cntr_data->offset + actual >= tmcdrvdata->size) + byte_cntr_data->offset = 0; + else + byte_cntr_data->offset += actual; + mutex_unlock(&byte_cntr_data->byte_cntr_lock); + } +} + +int etr_register_pcie_channel(struct byte_cntr *byte_cntr_data) +{ + return mhi_register_state_cb(etr_pcie_client_cb, byte_cntr_data, + byte_cntr_data->pcie_out_chan); +} + +static int etr_pcie_init(struct byte_cntr *byte_cntr_data) +{ + if (!byte_cntr_data) + return -EIO; + + byte_cntr_data->pcie_out_chan = MHI_CLIENT_QDSS_IN; + byte_cntr_data->offset = 0; + byte_cntr_data->pcie_chan_opened = false; + INIT_WORK(&byte_cntr_data->pcie_open_work, etr_pcie_open_work_fn); + INIT_WORK(&byte_cntr_data->pcie_write_work, etr_pcie_write_work_fn); + init_waitqueue_head(&byte_cntr_data->pcie_wait_wq); + byte_cntr_data->pcie_wq = create_singlethread_workqueue("etr_pcie"); + if (!byte_cntr_data->pcie_wq) + return -ENOMEM; + + return etr_register_pcie_channel(byte_cntr_data); +} static int usb_bypass_init(struct byte_cntr *byte_cntr_data) { @@ -672,6 +880,7 @@ struct byte_cntr *byte_cntr_init(struct amba_device *adev, init_waitqueue_head(&byte_cntr_data->wq); mutex_init(&byte_cntr_data->byte_cntr_lock); + etr_pcie_init(byte_cntr_data); return byte_cntr_data; } EXPORT_SYMBOL(byte_cntr_init); diff --git a/drivers/hwtracing/coresight/coresight-byte-cntr.h b/drivers/hwtracing/coresight/coresight-byte-cntr.h index ff79167bb095..96c664f9c18e 100644 --- a/drivers/hwtracing/coresight/coresight-byte-cntr.h +++ b/drivers/hwtracing/coresight/coresight-byte-cntr.h @@ -8,12 +8,13 @@ #include #include #include - +#include struct byte_cntr { struct cdev dev; struct class *driver_class; bool enable; bool read_active; + bool pcie_chan_opened; bool sw_usb; uint32_t byte_cntr_value; uint32_t block_size; @@ -22,6 +23,7 @@ struct byte_cntr { atomic_t usb_free_buf; wait_queue_head_t wq; wait_queue_head_t usb_wait_wq; + wait_queue_head_t pcie_wait_wq; struct workqueue_struct *usb_wq; struct qdss_request *usb_req; struct work_struct read_work; @@ -29,6 +31,12 @@ struct byte_cntr { struct mutex byte_cntr_lock; struct coresight_csr *csr; unsigned long offset; + u32 pcie_out_chan; + struct mhi_dev_client *out_handle; + struct work_struct pcie_open_work; + struct work_struct pcie_write_work; + struct workqueue_struct *pcie_wq; + void (*event_notifier)(struct mhi_dev_client_cb_reason *cb); }; extern void usb_bypass_notifier(void *priv, unsigned int event, @@ -36,5 +44,7 @@ extern void usb_bypass_notifier(void *priv, unsigned int event, 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); - +extern int etr_register_pcie_channel(struct byte_cntr *byte_cntr_data); +extern int etr_pcie_start(struct byte_cntr *byte_cntr_data); +extern void etr_pcie_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 378c7167cfcb..dce2333ac654 100644 --- a/drivers/hwtracing/coresight/coresight-tmc-etr.c +++ b/drivers/hwtracing/coresight/coresight-tmc-etr.c @@ -1091,6 +1091,9 @@ tmc_etr_setup_sysfs_buf(struct tmc_drvdata *drvdata) && drvdata->byte_cntr->sw_usb) return tmc_alloc_etr_buf(drvdata, TMC_ETR_SW_USB_BUF_SIZE, 0, cpu_to_node(0), NULL); + else if (drvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) + return tmc_alloc_etr_buf(drvdata, TMC_ETR_PCIE_MEM_SIZE, + 0, cpu_to_node(0), NULL); else return tmc_alloc_etr_buf(drvdata, drvdata->size, 0, cpu_to_node(0), NULL); @@ -1461,7 +1464,8 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev) if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM || (drvdata->out_mode == TMC_ETR_OUT_MODE_USB - && drvdata->byte_cntr->sw_usb)) { + && drvdata->byte_cntr->sw_usb) + || drvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) { /* * If we are enabling the ETR from disabled state, we need to make * sure we have a buffer with the right size. The etr_buf is not reset @@ -1475,7 +1479,9 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev) && sysfs_buf->size != drvdata->size) || (drvdata->out_mode == TMC_ETR_OUT_MODE_USB && drvdata->byte_cntr->sw_usb - && sysfs_buf->size != TMC_ETR_SW_USB_BUF_SIZE)) { + && sysfs_buf->size != TMC_ETR_SW_USB_BUF_SIZE) + || (drvdata->out_mode == TMC_ETR_OUT_MODE_PCIE + && sysfs_buf->size != TMC_ETR_PCIE_MEM_SIZE)) { spin_unlock_irqrestore(&drvdata->spinlock, flags); /* @@ -1534,6 +1540,8 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev) if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM) tmc_etr_byte_cntr_start(drvdata->byte_cntr); + if (drvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) + etr_pcie_start(drvdata->byte_cntr); if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM || (drvdata->byte_cntr->sw_usb && @@ -2048,10 +2056,15 @@ static int _tmc_disable_etr_sink(struct coresight_device *csdev, if ((drvdata->out_mode == TMC_ETR_OUT_MODE_USB && drvdata->byte_cntr->sw_usb) - || drvdata->out_mode == TMC_ETR_OUT_MODE_MEM) { + || drvdata->out_mode == TMC_ETR_OUT_MODE_MEM + || drvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) { + if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM) tmc_etr_byte_cntr_stop(drvdata->byte_cntr); - else { + else if (drvdata->out_mode == TMC_ETR_OUT_MODE_PCIE) { + etr_pcie_stop(drvdata->byte_cntr); + flush_workqueue(drvdata->byte_cntr->pcie_wq); + } else { usb_bypass_stop(drvdata->byte_cntr); flush_workqueue(drvdata->byte_cntr->usb_wq); drvdata->usbch = NULL; @@ -2086,6 +2099,8 @@ int tmc_etr_switch_mode(struct tmc_drvdata *drvdata, const char *out_mode) new_mode = TMC_ETR_OUT_MODE_MEM; else if (!strcmp(out_mode, str_tmc_etr_out_mode[TMC_ETR_OUT_MODE_USB])) new_mode = TMC_ETR_OUT_MODE_USB; + else if (!strcmp(out_mode, str_tmc_etr_out_mode[TMC_ETR_OUT_MODE_PCIE])) + new_mode = TMC_ETR_OUT_MODE_PCIE; else { mutex_unlock(&drvdata->mem_lock); return -EINVAL; diff --git a/drivers/hwtracing/coresight/coresight-tmc.h b/drivers/hwtracing/coresight/coresight-tmc.h index 90921dcf635b..4bbae28641f6 100644 --- a/drivers/hwtracing/coresight/coresight-tmc.h +++ b/drivers/hwtracing/coresight/coresight-tmc.h @@ -103,6 +103,8 @@ #define TMC_ETR_BAM_PIPE_INDEX 0 #define TMC_ETR_BAM_NR_PIPES 2 +#define TMC_ETR_PCIE_MEM_SIZE 0x400000 + #define TMC_AUTH_NSID_MASK GENMASK(1, 0) enum tmc_config_type { @@ -152,12 +154,14 @@ enum tmc_etr_out_mode { TMC_ETR_OUT_MODE_NONE, TMC_ETR_OUT_MODE_MEM, TMC_ETR_OUT_MODE_USB, + TMC_ETR_OUT_MODE_PCIE, }; static const char * const str_tmc_etr_out_mode[] = { [TMC_ETR_OUT_MODE_NONE] = "none", [TMC_ETR_OUT_MODE_MEM] = "mem", [TMC_ETR_OUT_MODE_USB] = "usb", + [TMC_ETR_OUT_MODE_PCIE] = "pcie", }; struct tmc_etr_bam_data {