summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--drivers/hwtracing/coresight/coresight-etm4x.c82
1 files changed, 53 insertions, 29 deletions
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 747afc875f91..0c35cd5e0d1d 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -1388,18 +1388,57 @@ static struct notifier_block etm4_cpu_pm_nb = {
.notifier_call = etm4_cpu_pm_notify,
};
-static int etm4_cpu_pm_register(void)
+/* Setup PM. Called with cpus locked. Deals with error conditions and counts */
+static int etm4_pm_setup_cpuslocked(void)
{
- if (IS_ENABLED(CONFIG_CPU_PM))
- return cpu_pm_register_notifier(&etm4_cpu_pm_nb);
+ int ret;
- return 0;
+ if (etm4_count++)
+ return 0;
+
+ ret = cpu_pm_register_notifier(&etm4_cpu_pm_nb);
+ if (ret)
+ goto reduce_count;
+
+ ret = cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ARM_CORESIGHT_STARTING,
+ "arm/coresight4:starting",
+ etm4_starting_cpu, etm4_dying_cpu);
+
+ if (ret)
+ goto unregister_notifier;
+
+ ret = cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ONLINE_DYN,
+ "arm/coresight4:online",
+ etm4_online_cpu, NULL);
+
+ /* HP dyn state ID returned in ret on success */
+ if (ret > 0) {
+ hp_online = ret;
+ return 0;
+ }
+
+ /* failed dyn state - remove others */
+ cpuhp_remove_state_nocalls_cpuslocked(CPUHP_AP_ARM_CORESIGHT_STARTING);
+
+unregister_notifier:
+ cpu_pm_unregister_notifier(&etm4_cpu_pm_nb);
+
+reduce_count:
+ --etm4_count;
+ return ret;
}
-static void etm4_cpu_pm_unregister(void)
+static void etm4_pm_clear(void)
{
- if (IS_ENABLED(CONFIG_CPU_PM))
- cpu_pm_unregister_notifier(&etm4_cpu_pm_nb);
+ if (--etm4_count != 0)
+ return;
+
+ cpu_pm_unregister_notifier(&etm4_cpu_pm_nb);
+ cpuhp_remove_state_nocalls(CPUHP_AP_ARM_CORESIGHT_STARTING);
+ if (hp_online) {
+ cpuhp_remove_state_nocalls(hp_online);
+ hp_online = 0;
+ }
}
static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
@@ -1453,24 +1492,15 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
etm4_init_arch_data, drvdata, 1))
dev_err(dev, "ETM arch init failed\n");
- if (!etm4_count++) {
- cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ARM_CORESIGHT_STARTING,
- "arm/coresight4:starting",
- etm4_starting_cpu, etm4_dying_cpu);
- ret = cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ONLINE_DYN,
- "arm/coresight4:online",
- etm4_online_cpu, NULL);
- if (ret < 0)
- goto err_arch_supported;
- hp_online = ret;
+ ret = etm4_pm_setup_cpuslocked();
+ cpus_read_unlock();
- ret = etm4_cpu_pm_register();
- if (ret)
- goto err_arch_supported;
+ /* etm4_pm_setup_cpuslocked() does its own cleanup - exit on error */
+ if (ret) {
+ etmdrvdata[drvdata->cpu] = NULL;
+ return ret;
}
- cpus_read_unlock();
-
if (etm4_arch_supported(drvdata->arch) == false) {
ret = -EINVAL;
goto err_arch_supported;
@@ -1517,13 +1547,7 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
err_arch_supported:
etmdrvdata[drvdata->cpu] = NULL;
- if (--etm4_count == 0) {
- etm4_cpu_pm_unregister();
-
- cpuhp_remove_state_nocalls(CPUHP_AP_ARM_CORESIGHT_STARTING);
- if (hp_online)
- cpuhp_remove_state_nocalls(hp_online);
- }
+ etm4_pm_clear();
return ret;
}