misc: tegra-profiler: add cpufreq notifier

Add cpufreq notifier into the profiler.

Bug 1618622

Change-Id: I06352c515c64f524fb88898dadefe1d07cedfe0f
Signed-off-by: Igor Nabirushkin <inabirushkin@nvidia.com>
Reviewed-on: http://git-master/r/714765
(cherry picked from commit b93eebf3afb92cfa97eb66067e5a5ef252a20ae8)
Reviewed-on: http://git-master/r/747751
GVS: Gerrit_Virtual_Submit
Reviewed-by: Andrey Trachenko <atrachenko@nvidia.com>
Reviewed-by: Winnie Hsu <whsu@nvidia.com>

5 files changed, 216 insertions, 149 deletions

diff --git a/drivers/misc/tegra-profiler/dwarf_unwind.c b/drivers/misc/tegra-profiler/dwarf_unwind.c
index 35409b13a644..ef5c4ad420eb 100644
--- a/drivers/misc/tegra-profiler/dwarf_unwind.c
+++ b/drivers/misc/tegra-profiler/dwarf_unwind.c
@@ -1789,8 +1789,6 @@ unwind_frame(struct ex_region_info *ri,
 	rs->cfa_register = -1;
 	rs_initial->cfa_register = -1;
 
-	rs->cfa_register = 0;
-
 	set_rule(rs, regnum_lr(mode), DW_WHERE_UNDEF, 0);
 
 	if (cie.initial_insn) {
diff --git a/drivers/misc/tegra-profiler/eh_unwind.c b/drivers/misc/tegra-profiler/eh_unwind.c
index d8edc82f7757..bfbcd144822f 100644
--- a/drivers/misc/tegra-profiler/eh_unwind.c
+++ b/drivers/misc/tegra-profiler/eh_unwind.c
@@ -910,7 +910,6 @@ unwind_exec_insn(struct quadd_mmap_area *mmap,
 		if (insn == 0xb3)
 			vsp++;
 
-		ctrl->vrs[SP] = (unsigned long)vsp;
 		ctrl->vrs[SP] = (u32)(unsigned long)vsp;
 
 		pr_debug("CMD_VFP_POP (%#lx %#lx): pop {D%lu-D%lu}\n",
diff --git a/drivers/misc/tegra-profiler/main.c b/drivers/misc/tegra-profiler/main.c
index b737f9910cfd..f9d7d4bcdd9b 100644
--- a/drivers/misc/tegra-profiler/main.c
+++ b/drivers/misc/tegra-profiler/main.c
@@ -85,14 +85,14 @@ static int start(void)
 		return -EBUSY;
 	}
 
-	preempt_disable();
-
 	if (!atomic_cmpxchg(&ctx.started, 0, 1)) {
+		preempt_disable();
+
 		if (ctx.pmu) {
 			err = ctx.pmu->enable();
 			if (err) {
 				pr_err("error: pmu enable\n");
-				goto errout;
+				goto errout_preempt;
 			}
 		}
 
@@ -100,46 +100,48 @@ static int start(void)
 			err = ctx.pl310->enable();
 			if (err) {
 				pr_err("error: pl310 enable\n");
-				goto errout;
+				goto errout_preempt;
 			}
 		}
 
 		ctx.comm->reset();
 
-		err = quadd_power_clk_start();
-		if (err < 0) {
-			pr_err("error: power_clk start\n");
-			goto errout;
-		}
-
 		err = quadd_hrt_start();
 		if (err) {
 			pr_err("error: hrt start\n");
+			goto errout_preempt;
+		}
+
+		preempt_enable();
+
+		err = quadd_power_clk_start();
+		if (err < 0) {
+			pr_err("error: power_clk start\n");
 			goto errout;
 		}
 	}
 
-	preempt_enable();
 	return 0;
 
+errout_preempt:
+	preempt_enable();
+
 errout:
 	atomic_set(&ctx.started, 0);
 	tegra_profiler_unlock();
-	preempt_enable();
 
 	return err;
 }
 
 static void stop(void)
 {
-	preempt_disable();
-
 	if (atomic_cmpxchg(&ctx.started, 1, 0)) {
+		preempt_disable();
+
 		quadd_hrt_stop();
 
 		ctx.comm->reset();
 
-		quadd_power_clk_stop();
 		quadd_unwind_stop();
 
 		if (ctx.pmu)
@@ -149,9 +151,11 @@ static void stop(void)
 			ctx.pl310->disable();
 
 		tegra_profiler_unlock();
-	}
 
-	preempt_enable();
+		preempt_enable();
+
+		quadd_power_clk_stop();
+	}
 }
 
 static inline int is_event_supported(struct source_info *si, int event)
diff --git a/drivers/misc/tegra-profiler/power_clk.c b/drivers/misc/tegra-profiler/power_clk.c
index c3b5aa3596c7..b29c24dde327 100644
--- a/drivers/misc/tegra-profiler/power_clk.c
+++ b/drivers/misc/tegra-profiler/power_clk.c
@@ -16,10 +16,10 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/module.h>
 #include <linux/cpufreq.h>
 #include <linux/clk.h>
 #include <linux/notifier.h>
+#include <linux/workqueue.h>
 #include <linux/cpu.h>
 #include <linux/timer.h>
 #include <linux/err.h>
@@ -34,7 +34,8 @@
 
 #define POWER_CLK_MAX_VALUES	32
 
-typedef int (*notifier_call_ft)(struct notifier_block *, unsigned long, void *);
+typedef int (*notifier_call_ft)(struct notifier_block *,
+				unsigned long, void *);
 
 struct power_clk_data {
 	unsigned long value;
@@ -75,83 +76,6 @@ enum {
 
 static struct power_clk_context_s power_ctx;
 
-static void check_clks(void);
-
-static void read_source(struct power_clk_source *s)
-{
-	int i;
-
-	mutex_lock(&s->lock);
-
-	switch (s->type) {
-	case QUADD_POWER_CLK_CPU:
-		/* update cpu frequency */
-		for (i = 0; i < nr_cpu_ids; i++)
-			s->data[i].value = cpufreq_get(i);
-		break;
-
-	case QUADD_POWER_CLK_GPU:
-		/* update gpu frequency */
-		s->clkp = clk_get_sys("3d", NULL);
-		if (!IS_ERR_OR_NULL(s->clkp)) {
-			s->data[0].value =
-				clk_get_rate(s->clkp) / 1000;
-			clk_put(s->clkp);
-		}
-		break;
-
-	case QUADD_POWER_CLK_EMC:
-		/* update emc frequency */
-		s->clkp = clk_get_sys("cpu", "emc");
-		if (!IS_ERR_OR_NULL(s->clkp)) {
-			s->data[0].value =
-				clk_get_rate(s->clkp) / 1000;
-			clk_put(s->clkp);
-		}
-		break;
-
-	default:
-		pr_err_once("%s: error: invalid power_clk type\n", __func__);
-		return;
-	}
-
-	mutex_unlock(&s->lock);
-	s->counter++;
-}
-
-static int
-gpu_notifier_call(struct notifier_block *nb, unsigned long val, void *ptr)
-{
-	read_source(&power_ctx.gpu);
-	check_clks();
-
-	return 0;
-}
-
-static int
-emc_notifier_call(struct notifier_block *nb, unsigned long val, void *ptr)
-{
-	read_source(&power_ctx.emc);
-	check_clks();
-
-	return 0;
-}
-
-static int
-cpu_notifier_call(struct notifier_block *nb, unsigned long val, void *ptr)
-{
-	read_source(&power_ctx.cpu);
-
-#ifndef CONFIG_COMMON_CLK
-	read_source(&power_ctx.gpu);
-	read_source(&power_ctx.emc);
-#endif
-
-	check_clks();
-
-	return 0;
-}
-
 static void make_sample(void)
 {
 	int i;
@@ -194,72 +118,209 @@ static void make_sample(void)
 		power_rate->emc = 0;
 
 	mutex_unlock(&s->lock);
-/*
+
 	pr_debug("make_sample: cpu: %u/%u/%u/%u, gpu: %u, emc: %u\n",
 		 extra_cpus[0], extra_cpus[1], extra_cpus[2], extra_cpus[3],
 		 power_rate->gpu, power_rate->emc);
-*/
+
 	vec.base = extra_cpus;
 	vec.len = power_rate->nr_cpus * sizeof(extra_cpus[0]);
 
 	quadd_put_sample(&record, &vec, 1);
 }
 
-static inline int is_data_changed(struct power_clk_source *s)
+static inline int
+is_data_changed(struct power_clk_source *s)
 {
 	int i;
 
-	mutex_lock(&s->lock);
 	for (i = 0; i < s->nr; i++) {
-		if (s->data[i].value != s->data[i].prev) {
-			mutex_unlock(&s->lock);
+		if (s->data[i].value != s->data[i].prev)
 			return 1;
-		}
 	}
-	mutex_unlock(&s->lock);
 
 	return 0;
 }
 
-static inline void update_data(struct power_clk_source *s)
+static inline void
+update_data(struct power_clk_source *s)
 {
 	int i;
 
-	mutex_lock(&s->lock);
-
 	for (i = 0; i < s->nr; i++)
 		s->data[i].prev = s->data[i].value;
-
-	mutex_unlock(&s->lock);
 }
 
 static void check_clks(void)
 {
+	struct power_clk_source *s;
 	int changed = 0;
 
-	if (is_data_changed(&power_ctx.cpu)) {
-		update_data(&power_ctx.cpu);
+	s = &power_ctx.gpu;
+	mutex_lock(&s->lock);
+	if (is_data_changed(s)) {
+		update_data(s);
 		changed = 1;
 	}
+	mutex_unlock(&s->lock);
 
-	if (is_data_changed(&power_ctx.gpu)) {
-		update_data(&power_ctx.gpu);
+	s = &power_ctx.emc;
+	mutex_lock(&s->lock);
+	if (is_data_changed(s)) {
+		update_data(s);
 		changed = 1;
 	}
+	mutex_unlock(&s->lock);
+
+	if (changed) {
+		pr_debug("gpu: %lu, emc: %lu\n",
+			 power_ctx.gpu.data[0].value,
+			 power_ctx.emc.data[0].value);
 
-	if (is_data_changed(&power_ctx.emc)) {
-		update_data(&power_ctx.emc);
-		changed = 1;
-	}
-/*
-	pr_debug("cpu: %lu/%lu/%lu/%lu, gpu: %lu, emc: %lu, changed: %s\n",
-		 power_ctx.cpu.data[0].value, power_ctx.cpu.data[1].value,
-		 power_ctx.cpu.data[2].value, power_ctx.cpu.data[3].value,
-		 power_ctx.gpu.data[0].value, power_ctx.emc.data[0].value,
-		 changed ? "yes" : "no");
-*/
-	if (changed)
 		make_sample();
+	}
+}
+
+static void check_source(struct power_clk_source *s)
+{
+	mutex_lock(&s->lock);
+
+	if (!is_data_changed(s)) {
+		mutex_unlock(&s->lock);
+		return;
+	}
+
+	pr_debug("cpu: %lu/%lu/%lu/%lu\n",
+		 power_ctx.cpu.data[0].value,
+		 power_ctx.cpu.data[1].value,
+		 power_ctx.cpu.data[2].value,
+		 power_ctx.cpu.data[3].value);
+
+	update_data(s);
+	mutex_unlock(&s->lock);
+
+	make_sample();
+}
+
+static void
+read_source(struct power_clk_source *s, int cpu)
+{
+	mutex_lock(&s->lock);
+
+	switch (s->type) {
+	case QUADD_POWER_CLK_CPU:
+		/* update cpu frequency */
+		if (cpu < 0 || cpu >= POWER_CLK_MAX_VALUES) {
+			pr_err_once("error: cpu id: %d\n", cpu);
+			break;
+		}
+
+		s->data[cpu].value = cpufreq_get(cpu);
+		pr_debug("QUADD_POWER_CLK_CPU, cpu: %d, value: %lu\n",
+			 cpu, s->data[cpu].value);
+		break;
+
+	case QUADD_POWER_CLK_GPU:
+		/* update gpu frequency */
+		s->clkp = clk_get_sys("3d", NULL);
+		if (!IS_ERR_OR_NULL(s->clkp)) {
+			s->data[0].value =
+				clk_get_rate(s->clkp) / 1000;
+			clk_put(s->clkp);
+		}
+		pr_debug("QUADD_POWER_CLK_GPU, value: %lu\n",
+			 s->data[0].value);
+		break;
+
+	case QUADD_POWER_CLK_EMC:
+		/* update emc frequency */
+		s->clkp = clk_get_sys("cpu", "emc");
+		if (!IS_ERR_OR_NULL(s->clkp)) {
+			s->data[0].value =
+				clk_get_rate(s->clkp) / 1000;
+			clk_put(s->clkp);
+		}
+		pr_debug("QUADD_POWER_CLK_EMC, value: %lu\n",
+			 s->data[0].value);
+		break;
+
+	default:
+		pr_err_once("error: invalid power_clk type\n");
+		return;
+	}
+
+	s->counter++;
+	mutex_unlock(&s->lock);
+}
+
+static int
+gpu_notifier_call(struct notifier_block *nb,
+		  unsigned long action, void *data)
+{
+	read_source(&power_ctx.gpu, -1);
+	check_clks();
+
+	return NOTIFY_DONE;
+}
+
+static int
+emc_notifier_call(struct notifier_block *nb,
+		  unsigned long action, void *data)
+{
+	read_source(&power_ctx.emc, -1);
+	check_clks();
+
+	return NOTIFY_DONE;
+}
+
+static void
+read_cpufreq(struct power_clk_source *s, struct cpufreq_freqs *freq)
+{
+	int cpu, cpufreq;
+
+	mutex_lock(&s->lock);
+
+	if (!atomic_read(&s->active)) {
+		mutex_unlock(&s->lock);
+		return;
+	}
+
+	cpu = freq->cpu;
+	cpufreq = freq->new;
+
+	if (cpu >= POWER_CLK_MAX_VALUES) {
+		pr_err_once("error: cpu id: %d\n", cpu);
+		mutex_unlock(&s->lock);
+		return;
+	}
+
+	s->data[cpu].value = cpufreq;
+
+	pr_debug("[%d] cpufreq: %u --> %u\n",
+		 cpu, freq->old, cpufreq);
+
+	mutex_unlock(&s->lock);
+
+	check_source(s);
+}
+
+static int
+cpufreq_notifier_call(struct notifier_block *nb,
+		      unsigned long action, void *hcpu)
+{
+	struct cpufreq_freqs *freq;
+	struct power_clk_source *s = &power_ctx.cpu;
+
+	if (!atomic_read(&s->active))
+		return 0;
+
+	if (action == CPUFREQ_POSTCHANGE ||
+	    action == CPUFREQ_RESUMECHANGE) {
+		freq = hcpu;
+		read_cpufreq(s, freq);
+	}
+
+	return 0;
 }
 
 static void reset_data(struct power_clk_source *s)
@@ -282,21 +343,19 @@ static void init_source(struct power_clk_source *s,
 {
 	s->type = type;
 	s->nb.notifier_call = notifier;
-	s->nr = nr_values;
+	s->nr = min_t(int, nr_values, POWER_CLK_MAX_VALUES);
 
 	mutex_init(&s->lock);
 	reset_data(s);
 }
 
 static void
-power_clk_work_func(struct work_struct *dummy)
+power_clk_work_func(struct work_struct *work)
 {
-#ifndef CONFIG_COMMON_CLK
-	read_source(&power_ctx.gpu);
-	read_source(&power_ctx.emc);
+	read_source(&power_ctx.gpu, -1);
+	read_source(&power_ctx.emc, -1);
 
 	check_clks();
-#endif
 }
 
 static DECLARE_WORK(power_clk_work, power_clk_work_func);
@@ -313,7 +372,6 @@ static void power_clk_timer(unsigned long data)
 int quadd_power_clk_start(void)
 {
 	struct power_clk_source *s;
-	int status;
 	struct timer_list *timer = &power_ctx.timer;
 	struct quadd_parameters *param = &power_ctx.quadd_ctx->param;
 
@@ -326,7 +384,9 @@ int quadd_power_clk_start(void)
 	power_ctx.period = 0;
 #else
 	power_ctx.period = MSEC_PER_SEC / param->power_rate_freq;
+	pr_info("clk: use timer\n");
 #endif
+
 	pr_info("power_clk: start, freq: %d\n",
 		param->power_rate_freq);
 
@@ -372,12 +432,8 @@ int quadd_power_clk_start(void)
 
 	/* setup cpu frequency notifier */
 	s = &power_ctx.cpu;
-	status = register_cpu_notifier(&s->nb);
-	if (status < 0) {
-		pr_err("error: could not setup cpu freq\n");
-		return status;
-	}
 	reset_data(s);
+	atomic_set(&s->active, 1);
 
 	if (power_ctx.period > 0) {
 		init_timer(timer);
@@ -387,51 +443,59 @@ int quadd_power_clk_start(void)
 		add_timer(timer);
 	}
 
-	atomic_set(&s->active, 1);
-
 	return 0;
 }
 
 void quadd_power_clk_stop(void)
 {
 	struct power_clk_source *s;
+	struct quadd_parameters *param = &power_ctx.quadd_ctx->param;
 
-	if (power_ctx.quadd_ctx->param.power_rate_freq == 0)
+	if (param->power_rate_freq == 0)
 		return;
 
 	if (power_ctx.period > 0)
 		del_timer_sync(&power_ctx.timer);
 
 	s = &power_ctx.gpu;
+	mutex_lock(&s->lock);
 	if (atomic_cmpxchg(&s->active, 1, 0)) {
 #ifdef CONFIG_COMMON_CLK
 		if (s->clkp)
 			clk_notifier_unregister(s->clkp, &s->nb);
 #endif
 	}
+	mutex_unlock(&s->lock);
 
 	s = &power_ctx.emc;
+	mutex_lock(&s->lock);
 	if (atomic_cmpxchg(&s->active, 1, 0)) {
 #ifdef CONFIG_COMMON_CLK
 		if (s->clkp)
 			clk_notifier_unregister(s->clkp, &s->nb);
 #endif
 	}
+	mutex_unlock(&s->lock);
 
 	s = &power_ctx.cpu;
-	if (atomic_cmpxchg(&s->active, 1, 0)) {
-		pr_info("power_clk: stop\n");
-		unregister_cpu_notifier(&s->nb);
-	}
+	mutex_unlock(&s->lock);
+	atomic_set(&s->active, 0);
+	mutex_lock(&s->lock);
+
+	pr_info("power_clk: stop\n");
 }
 
 int quadd_power_clk_init(struct quadd_ctx *quadd_ctx)
 {
-	init_source(&power_ctx.cpu, cpu_notifier_call, nr_cpu_ids,
+	init_source(&power_ctx.cpu, cpufreq_notifier_call, nr_cpu_ids,
 		    QUADD_POWER_CLK_CPU);
+
 	init_source(&power_ctx.gpu, gpu_notifier_call, 1, QUADD_POWER_CLK_GPU);
 	init_source(&power_ctx.emc, emc_notifier_call, 1, QUADD_POWER_CLK_EMC);
 
+	cpufreq_register_notifier(&power_ctx.cpu.nb,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+
 	power_ctx.quadd_ctx = quadd_ctx;
 
 	return 0;
@@ -440,4 +504,6 @@ int quadd_power_clk_init(struct quadd_ctx *quadd_ctx)
 void quadd_power_clk_deinit(void)
 {
 	quadd_power_clk_stop();
+	cpufreq_unregister_notifier(&power_ctx.cpu.nb,
+				    CPUFREQ_TRANSITION_NOTIFIER);
 }
diff --git a/drivers/misc/tegra-profiler/version.h b/drivers/misc/tegra-profiler/version.h
index 15e5c14f5b07..e92e395940fd 100644
--- a/drivers/misc/tegra-profiler/version.h
+++ b/drivers/misc/tegra-profiler/version.h
@@ -18,7 +18,7 @@
 #ifndef __QUADD_VERSION_H
 #define __QUADD_VERSION_H
 
-#define QUADD_MODULE_VERSION		"1.93"
+#define QUADD_MODULE_VERSION		"1.94"
 #define QUADD_MODULE_BRANCH		"Dev"
 
 #endif	/* __QUADD_VERSION_H */