intel_th: Update scratchpad bits according to enabled output activity

[cascardo/linux.git] / drivers / hwtracing / intel_th / gth.c

diff --git a/drivers/hwtracing/intel_th/gth.c b/drivers/hwtracing/intel_th/gth.c
index e4c9811..9beea0b 100644 (file)
--- a/drivers/hwtracing/intel_th/gth.c
+++ b/drivers/hwtracing/intel_th/gth.c
@@ -286,6 +286,10 @@ static int intel_th_gth_reset(struct gth_device *gth)
        if (scratchpad & 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);
+
        /* output ports */
        for (port = 0; port < 8; port++) {
                if (gth_output_parm_get(gth, port, TH_OUTPUT_PARM(port)) ==
@@ -488,6 +492,10 @@ static void intel_th_gth_disable(struct intel_th_device *thdev,
        if (!count)
                dev_dbg(&thdev->dev, "timeout waiting for GTH[%d] PLE\n",
                        output->port);
+
+       reg = ioread32(gth->base + REG_GTH_SCRPD0);
+       reg &= ~output->scratchpad;
+       iowrite32(reg, gth->base + REG_GTH_SCRPD0);
 }
 
 /**
@@ -502,7 +510,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);
-       u32 scr = 0xfc0000;
+       u32 scr = 0xfc0000, scrpd;
        int master;
 
        spin_lock(&gth->gth_lock);
@@ -517,6 +525,10 @@ static void intel_th_gth_enable(struct intel_th_device *thdev,
        output->active = true;
        spin_unlock(&gth->gth_lock);
 
+       scrpd = ioread32(gth->base + REG_GTH_SCRPD0);
+       scrpd |= output->scratchpad;
+       iowrite32(scrpd, gth->base + REG_GTH_SCRPD0);
+
        iowrite32(scr, gth->base + REG_GTH_SCR);
        iowrite32(0, gth->base + REG_GTH_SCR2);
 }