1 files changed, 52 insertions, 0 deletions

diff --git a/drivers/cpufreq/cppc_cpufreq.c b/drivers/cpufreq/cppc_cpufreq.c
index a9d3eec32795..30f302149730 100644
--- a/drivers/cpufreq/cppc_cpufreq.c
+++ b/drivers/cpufreq/cppc_cpufreq.c
@@ -296,10 +296,62 @@ static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
         return ret;
 }
 
+static inline u64 get_delta(u64 t1, u64 t0)
+{
+        if (t1 > t0 || t0 > ~(u32)0)
+                return t1 - t0;
+
+        return (u32)t1 - (u32)t0;
+}
+
+static int cppc_get_rate_from_fbctrs(struct cppc_cpudata *cpu,
+                                     struct cppc_perf_fb_ctrs fb_ctrs_t0,
+                                     struct cppc_perf_fb_ctrs fb_ctrs_t1)
+{
+        u64 delta_reference, delta_delivered;
+        u64 reference_perf, delivered_perf;
+
+        reference_perf = fb_ctrs_t0.reference_perf;
+
+        delta_reference = get_delta(fb_ctrs_t1.reference,
+                                    fb_ctrs_t0.reference);
+        delta_delivered = get_delta(fb_ctrs_t1.delivered,
+                                    fb_ctrs_t0.delivered);
+
+        /* Check to avoid divide-by zero */
+        if (delta_reference || delta_delivered)
+                delivered_perf = (reference_perf * delta_delivered) /
+                                        delta_reference;
+        else
+                delivered_perf = cpu->perf_ctrls.desired_perf;
+
+        return cppc_cpufreq_perf_to_khz(cpu, delivered_perf);
+}
+
+static unsigned int cppc_cpufreq_get_rate(unsigned int cpunum)
+{
+        struct cppc_perf_fb_ctrs fb_ctrs_t0 = {0}, fb_ctrs_t1 = {0};
+        struct cppc_cpudata *cpu = all_cpu_data[cpunum];
+        int ret;
+
+        ret = cppc_get_perf_ctrs(cpunum, &fb_ctrs_t0);
+        if (ret)
+                return ret;
+
+        udelay(2); /* 2usec delay between sampling */
+
+        ret = cppc_get_perf_ctrs(cpunum, &fb_ctrs_t1);
+        if (ret)
+                return ret;
+
+        return cppc_get_rate_from_fbctrs(cpu, fb_ctrs_t0, fb_ctrs_t1);
+}
+
 static struct cpufreq_driver cppc_cpufreq_driver = {
         .flags = CPUFREQ_CONST_LOOPS,
         .verify = cppc_verify_policy,
         .target = cppc_cpufreq_set_target,
+        .get = cppc_cpufreq_get_rate,
         .init = cppc_cpufreq_cpu_init,
         .stop_cpu = cppc_cpufreq_stop_cpu,
         .name = "cppc_cpufreq",