diff --git a/drivers/hwtracing/coresight/coresight-trbe.c b/drivers/hwtracing/coresight/coresight-trbe.c index 14e35b9660d7..c7cbca45f2de 100644 --- a/drivers/hwtracing/coresight/coresight-trbe.c +++ b/drivers/hwtracing/coresight/coresight-trbe.c @@ -116,6 +116,20 @@ static int trbe_errata_cpucaps[] = { */ #define TRBE_WORKAROUND_OVERWRITE_FILL_MODE_SKIP_BYTES 256 +/* + * struct trbe_save_state: Register values representing TRBE state + * @trblimitr - Trace Buffer Limit Address Register value + * @trbbaser - Trace Buffer Base Register value + * @trbptr - Trace Buffer Write Pointer Register value + * @trbsr - Trace Buffer Status Register value + */ +struct trbe_save_state { + u64 trblimitr; + u64 trbbaser; + u64 trbptr; + u64 trbsr; +}; + /* * struct trbe_cpudata: TRBE instance specific data * @trbe_flag - TRBE dirty/access flag support @@ -134,6 +148,7 @@ struct trbe_cpudata { enum cs_mode mode; struct trbe_buf *buf; struct trbe_drvdata *drvdata; + struct trbe_save_state save_state; DECLARE_BITMAP(errata, TRBE_ERRATA_MAX); }; @@ -1189,6 +1204,46 @@ static irqreturn_t arm_trbe_irq_handler(int irq, void *dev) return IRQ_HANDLED; } +static int arm_trbe_save(struct coresight_device *csdev) +{ + struct trbe_cpudata *cpudata = dev_get_drvdata(&csdev->dev); + struct trbe_save_state *state = &cpudata->save_state; + + state->trblimitr = read_sysreg_s(SYS_TRBLIMITR_EL1); + + /* Disable the unit, ensure the writes to memory are complete */ + if (state->trblimitr & TRBLIMITR_EL1_E) + trbe_drain_and_disable_local(cpudata); + + state->trbbaser = read_sysreg_s(SYS_TRBBASER_EL1); + state->trbptr = read_sysreg_s(SYS_TRBPTR_EL1); + state->trbsr = read_sysreg_s(SYS_TRBSR_EL1); + return 0; +} + +static void arm_trbe_restore(struct coresight_device *csdev) +{ + struct trbe_cpudata *cpudata = dev_get_drvdata(&csdev->dev); + struct trbe_save_state *state = &cpudata->save_state; + + write_sysreg_s(state->trbbaser, SYS_TRBBASER_EL1); + write_sysreg_s(state->trbptr, SYS_TRBPTR_EL1); + write_sysreg_s(state->trbsr, SYS_TRBSR_EL1); + + if (!(state->trblimitr & TRBLIMITR_EL1_E)) { + write_sysreg_s(state->trblimitr, SYS_TRBLIMITR_EL1); + } else { + /* + * The section K5.5 Context switching, Arm ARM (ARM DDI 0487 + * L.a), S_PKLXF requires a Context synchronization event to + * guarantee the Trace Buffer Unit will observe the new values + * of the system registers. + */ + isb(); + set_trbe_enabled(cpudata, state->trblimitr); + } +} + static const struct coresight_ops_sink arm_trbe_sink_ops = { .enable = arm_trbe_enable, .disable = arm_trbe_disable, @@ -1198,7 +1253,9 @@ static const struct coresight_ops_sink arm_trbe_sink_ops = { }; static const struct coresight_ops arm_trbe_cs_ops = { - .sink_ops = &arm_trbe_sink_ops, + .pm_save_disable = arm_trbe_save, + .pm_restore_enable = arm_trbe_restore, + .sink_ops = &arm_trbe_sink_ops, }; static ssize_t align_show(struct device *dev, struct device_attribute *attr, char *buf)