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 <jinlmao@codeaurora.org>
Signed-off-by: Yuanfang Zhang <zhangyuanfang@codeaurora.org>

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

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

@@ -8,12 +8,13 @@
 #include <linux/amba/bus.h>
 #include <linux/wait.h>
 #include <linux/mutex.h>
-
+#include <linux/msm_mhi_dev.h>
 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

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;

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 {