5 files changed, 82 insertions, 9 deletions

diff --git a/drivers/hwtracing/intel_th/core.c b/drivers/hwtracing/intel_th/core.c
index 998e3e55073a..1a023e30488c 100644
--- a/drivers/hwtracing/intel_th/core.c
+++ b/drivers/hwtracing/intel_th/core.c
@@ -436,8 +436,9 @@ static const struct intel_th_subdevice {
 		.nres	= 1,
 		.res	= {
 			{
+				/* Handle TSCU from GTH driver */
 				.start	= REG_GTH_OFFSET,
-				.end	= REG_GTH_OFFSET + REG_GTH_LENGTH - 1,
+				.end	= REG_TSCU_OFFSET + REG_TSCU_LENGTH - 1,
 				.flags	= IORESOURCE_MEM,
 			},
 		},
diff --git a/drivers/hwtracing/intel_th/gth.c b/drivers/hwtracing/intel_th/gth.c
index 7d9d667fe017..018678ec3c13 100644
--- a/drivers/hwtracing/intel_th/gth.c
+++ b/drivers/hwtracing/intel_th/gth.c
@@ -285,16 +285,16 @@ gth_output_parm_get(struct gth_device *gth, int port, unsigned int parm)
  */
 static int intel_th_gth_reset(struct gth_device *gth)
 {
-	u32 scratchpad;
+	u32 reg;
 	int port, i;
 
-	scratchpad = ioread32(gth->base + REG_GTH_SCRPD0);
-	if (scratchpad & SCRPD_DEBUGGER_IN_USE)
+	reg = ioread32(gth->base + REG_GTH_SCRPD0);
+	if (reg & SCRPD_DEBUGGER_IN_USE)
 		return -EBUSY;
 
 	/* Always save/restore STH and TU registers in S0ix entry/exit */
-	scratchpad |= SCRPD_STH_IS_ENABLED | SCRPD_TRIGGER_IS_ENABLED;
-	iowrite32(scratchpad, gth->base + REG_GTH_SCRPD0);
+	reg |= SCRPD_STH_IS_ENABLED | SCRPD_TRIGGER_IS_ENABLED;
+	iowrite32(reg, gth->base + REG_GTH_SCRPD0);
 
 	/* output ports */
 	for (port = 0; port < 8; port++) {
@@ -512,6 +512,15 @@ static void intel_th_gth_disable(struct intel_th_device *thdev,
 	iowrite32(reg, gth->base + REG_GTH_SCRPD0);
 }
 
+static void gth_tscu_resync(struct gth_device *gth)
+{
+	u32 reg;
+
+	reg = ioread32(gth->base + REG_TSCU_TSUCTRL);
+	reg &= ~TSUCTRL_CTCRESYNC;
+	iowrite32(reg, gth->base + REG_TSCU_TSUCTRL);
+}
+
 /**
  * intel_th_gth_enable() - enable tracing to an output device
  * @thdev:	GTH device
@@ -524,6 +533,7 @@ static void intel_th_gth_enable(struct intel_th_device *thdev,
 				struct intel_th_output *output)
 {
 	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+	struct intel_th *th = to_intel_th(thdev);
 	u32 scr = 0xfc0000, scrpd;
 	int master;
 
@@ -539,6 +549,9 @@ static void intel_th_gth_enable(struct intel_th_device *thdev,
 	output->active = true;
 	spin_unlock(&gth->gth_lock);
 
+	if (INTEL_TH_CAP(th, tscu_enable))
+		gth_tscu_resync(gth);
+
 	scrpd = ioread32(gth->base + REG_GTH_SCRPD0);
 	scrpd |= output->scratchpad;
 	iowrite32(scrpd, gth->base + REG_GTH_SCRPD0);
diff --git a/drivers/hwtracing/intel_th/gth.h b/drivers/hwtracing/intel_th/gth.h
index 56f0d2620577..f3d234251a12 100644
--- a/drivers/hwtracing/intel_th/gth.h
+++ b/drivers/hwtracing/intel_th/gth.h
@@ -55,9 +55,14 @@ enum {
 	REG_GTH_SCRPD1		= 0xe4, /* ScratchPad[1] */
 	REG_GTH_SCRPD2		= 0xe8, /* ScratchPad[2] */
 	REG_GTH_SCRPD3		= 0xec, /* ScratchPad[3] */
+	REG_TSCU_TSUCTRL	= 0x2000, /* TSCU control register */
+	REG_TSCU_TSCUSTAT	= 0x2004, /* TSCU status register */
 };
 
 /* waiting for Pipeline Empty bit(s) to assert for GTH */
 #define GTH_PLE_WAITLOOP_DEPTH	10000
 
+#define TSUCTRL_CTCRESYNC	BIT(0)
+#define TSCUSTAT_CTCSYNCING	BIT(1)
+
 #endif /* __INTEL_TH_GTH_H__ */
diff --git a/drivers/hwtracing/intel_th/intel_th.h b/drivers/hwtracing/intel_th/intel_th.h
index 78a4fb28b135..99ad563fc40d 100644
--- a/drivers/hwtracing/intel_th/intel_th.h
+++ b/drivers/hwtracing/intel_th/intel_th.h
@@ -298,6 +298,10 @@ enum {
 	REG_GTH_OFFSET		= 0x0000,
 	REG_GTH_LENGTH		= 0x2000,
 
+	/* Timestamp counter unit (TSCU) */
+	REG_TSCU_OFFSET		= 0x2000,
+	REG_TSCU_LENGTH		= 0x1000,
+
 	/* Software Trace Hub (STH) [0x4000..0x4fff] */
 	REG_STH_OFFSET		= 0x4000,
 	REG_STH_LENGTH		= 0x2000,
diff --git a/drivers/hwtracing/intel_th/pci.c b/drivers/hwtracing/intel_th/pci.c
index aed6d594991e..bc9cebc30526 100644
--- a/drivers/hwtracing/intel_th/pci.c
+++ b/drivers/hwtracing/intel_th/pci.c
@@ -27,6 +27,49 @@
 
 #define BAR_MASK (BIT(TH_MMIO_CONFIG) | BIT(TH_MMIO_SW))
 
+#define PCI_REG_NPKDSC	0x80
+#define NPKDSC_TSACT	BIT(5)
+
+static int intel_th_pci_activate(struct intel_th *th)
+{
+	struct pci_dev *pdev = to_pci_dev(th->dev);
+	u32 npkdsc;
+	int err;
+
+	if (!INTEL_TH_CAP(th, tscu_enable))
+		return 0;
+
+	err = pci_read_config_dword(pdev, PCI_REG_NPKDSC, &npkdsc);
+	if (!err) {
+		npkdsc |= NPKDSC_TSACT;
+		err = pci_write_config_dword(pdev, PCI_REG_NPKDSC, npkdsc);
+	}
+
+	if (err)
+		dev_err(&pdev->dev, "failed to read NPKDSC register\n");
+
+	return err;
+}
+
+static void intel_th_pci_deactivate(struct intel_th *th)
+{
+	struct pci_dev *pdev = to_pci_dev(th->dev);
+	u32 npkdsc;
+	int err;
+
+	if (!INTEL_TH_CAP(th, tscu_enable))
+		return;
+
+	err = pci_read_config_dword(pdev, PCI_REG_NPKDSC, &npkdsc);
+	if (!err) {
+		npkdsc |= NPKDSC_TSACT;
+		err = pci_write_config_dword(pdev, PCI_REG_NPKDSC, npkdsc);
+	}
+
+	if (err)
+		dev_err(&pdev->dev, "failed to read NPKDSC register\n");
+}
+
 static int intel_th_pci_probe(struct pci_dev *pdev,
 			      const struct pci_device_id *id)
 {
@@ -47,6 +90,9 @@ static int intel_th_pci_probe(struct pci_dev *pdev,
 	if (IS_ERR(th))
 		return PTR_ERR(th);
 
+	th->activate   = intel_th_pci_activate;
+	th->deactivate = intel_th_pci_deactivate;
+
 	pci_set_master(pdev);
 
 	return 0;
@@ -59,6 +105,10 @@ static void intel_th_pci_remove(struct pci_dev *pdev)
 	intel_th_free(th);
 }
 
+static const struct intel_th_drvdata intel_th_2x = {
+	.tscu_enable	= 1,
+};
+
 static const struct pci_device_id intel_th_pci_id_table[] = {
 	{
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x9d26),
@@ -96,17 +146,17 @@ static const struct pci_device_id intel_th_pci_id_table[] = {
 	{
 		/* Gemini Lake */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x318e),
-		.driver_data = (kernel_ulong_t)0,
+		.driver_data = (kernel_ulong_t)&intel_th_2x,
 	},
 	{
 		/* Cannon Lake H */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xa326),
-		.driver_data = (kernel_ulong_t)0,
+		.driver_data = (kernel_ulong_t)&intel_th_2x,
 	},
 	{
 		/* Cannon Lake LP */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x9da6),
-		.driver_data = (kernel_ulong_t)0,
+		.driver_data = (kernel_ulong_t)&intel_th_2x,
 	},
 	{ 0 },
 };