4 files changed, 42 insertions, 19 deletions

diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 98b49462f4e9..6fe6050a3889 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -107,7 +107,6 @@ static void cs_check_cpu(int cpu, unsigned int load)
 
 static void cs_dbs_timer(struct work_struct *work)
 {
-        struct delayed_work *dw = to_delayed_work(work);
         struct cs_cpu_dbs_info_s *dbs_info = container_of(work,
                         struct cs_cpu_dbs_info_s, cdbs.work.work);
         unsigned int cpu = dbs_info->cdbs.cur_policy->cpu;
@@ -116,12 +115,15 @@ static void cs_dbs_timer(struct work_struct *work)
         struct dbs_data *dbs_data = dbs_info->cdbs.cur_policy->governor_data;
         struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
         int delay = delay_for_sampling_rate(cs_tuners->sampling_rate);
+        bool modify_all = true;
 
         mutex_lock(&core_dbs_info->cdbs.timer_mutex);
-        if (need_load_eval(&core_dbs_info->cdbs, cs_tuners->sampling_rate))
+        if (!need_load_eval(&core_dbs_info->cdbs, cs_tuners->sampling_rate))
+                modify_all = false;
+        else
                 dbs_check_cpu(dbs_data, cpu);
 
-        schedule_delayed_work_on(smp_processor_id(), dw, delay);
+        gov_queue_work(dbs_data, dbs_info->cdbs.cur_policy, delay, modify_all);
         mutex_unlock(&core_dbs_info->cdbs.timer_mutex);
 }
 
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index 26fbb729bc1c..326f0c2e2bd5 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -178,20 +178,38 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 }
 EXPORT_SYMBOL_GPL(dbs_check_cpu);
 
-static inline void dbs_timer_init(struct dbs_data *dbs_data, int cpu,
-                                  unsigned int sampling_rate)
+static inline void __gov_queue_work(int cpu, struct dbs_data *dbs_data,
+                unsigned int delay)
 {
-        int delay = delay_for_sampling_rate(sampling_rate);
         struct cpu_dbs_common_info *cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
 
-        schedule_delayed_work_on(cpu, &cdbs->work, delay);
+        mod_delayed_work_on(cpu, system_wq, &cdbs->work, delay);
 }
 
-static inline void dbs_timer_exit(struct dbs_data *dbs_data, int cpu)
+void gov_queue_work(struct dbs_data *dbs_data, struct cpufreq_policy *policy,
+                unsigned int delay, bool all_cpus)
 {
-        struct cpu_dbs_common_info *cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
+        int i;
+
+        if (!all_cpus) {
+                __gov_queue_work(smp_processor_id(), dbs_data, delay);
+        } else {
+                for_each_cpu(i, policy->cpus)
+                        __gov_queue_work(i, dbs_data, delay);
+        }
+}
+EXPORT_SYMBOL_GPL(gov_queue_work);
+
+static inline void gov_cancel_work(struct dbs_data *dbs_data,
+                struct cpufreq_policy *policy)
+{
+        struct cpu_dbs_common_info *cdbs;
+        int i;
 
-        cancel_delayed_work_sync(&cdbs->work);
+        for_each_cpu(i, policy->cpus) {
+                cdbs = dbs_data->cdata->get_cpu_cdbs(i);
+                cancel_delayed_work_sync(&cdbs->work);
+        }
 }
 
 /* Will return if we need to evaluate cpu load again or not */
@@ -380,16 +398,15 @@ int cpufreq_governor_dbs(struct cpufreq_policy *policy,
                 /* Initiate timer time stamp */
                 cpu_cdbs->time_stamp = ktime_get();
 
-                for_each_cpu(j, policy->cpus)
-                        dbs_timer_init(dbs_data, j, sampling_rate);
+                gov_queue_work(dbs_data, policy,
+                                delay_for_sampling_rate(sampling_rate), true);
                 break;
 
         case CPUFREQ_GOV_STOP:
                 if (dbs_data->cdata->governor == GOV_CONSERVATIVE)
                         cs_dbs_info->enable = 0;
 
-                for_each_cpu(j, policy->cpus)
-                        dbs_timer_exit(dbs_data, j);
+                gov_cancel_work(dbs_data, policy);
 
                 mutex_lock(&dbs_data->mutex);
                 mutex_destroy(&cpu_cdbs->timer_mutex);
diff --git a/drivers/cpufreq/cpufreq_governor.h b/drivers/cpufreq/cpufreq_governor.h
index 27b588aeacc1..c9c269f2b973 100644
--- a/drivers/cpufreq/cpufreq_governor.h
+++ b/drivers/cpufreq/cpufreq_governor.h
@@ -262,4 +262,6 @@ bool need_load_eval(struct cpu_dbs_common_info *cdbs,
                 unsigned int sampling_rate);
 int cpufreq_governor_dbs(struct cpufreq_policy *policy,
                 struct common_dbs_data *cdata, unsigned int event);
+void gov_queue_work(struct dbs_data *dbs_data, struct cpufreq_policy *policy,
+                unsigned int delay, bool all_cpus);
 #endif /* _CPUFREQ_GOVERNER_H */
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index c90d345c636a..459f9ee39c74 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -216,7 +216,6 @@ static void od_check_cpu(int cpu, unsigned int load_freq)
 
 static void od_dbs_timer(struct work_struct *work)
 {
-        struct delayed_work *dw = to_delayed_work(work);
         struct od_cpu_dbs_info_s *dbs_info =
                 container_of(work, struct od_cpu_dbs_info_s, cdbs.work.work);
         unsigned int cpu = dbs_info->cdbs.cur_policy->cpu;
@@ -225,10 +224,13 @@ static void od_dbs_timer(struct work_struct *work)
         struct dbs_data *dbs_data = dbs_info->cdbs.cur_policy->governor_data;
         struct od_dbs_tuners *od_tuners = dbs_data->tuners;
         int delay = 0, sample_type = core_dbs_info->sample_type;
+        bool modify_all = true;
 
         mutex_lock(&core_dbs_info->cdbs.timer_mutex);
-        if (!need_load_eval(&core_dbs_info->cdbs, od_tuners->sampling_rate))
+        if (!need_load_eval(&core_dbs_info->cdbs, od_tuners->sampling_rate)) {
+                modify_all = false;
                 goto max_delay;
+        }
 
         /* Common NORMAL_SAMPLE setup */
         core_dbs_info->sample_type = OD_NORMAL_SAMPLE;
@@ -250,7 +252,7 @@ max_delay:
                 delay = delay_for_sampling_rate(od_tuners->sampling_rate
                                 * core_dbs_info->rate_mult);
 
-        schedule_delayed_work_on(smp_processor_id(), dw, delay);
+        gov_queue_work(dbs_data, dbs_info->cdbs.cur_policy, delay, modify_all);
         mutex_unlock(&core_dbs_info->cdbs.timer_mutex);
 }
 
@@ -310,8 +312,8 @@ static void update_sampling_rate(struct dbs_data *dbs_data,
                         cancel_delayed_work_sync(&dbs_info->cdbs.work);
                         mutex_lock(&dbs_info->cdbs.timer_mutex);
 
-                        schedule_delayed_work_on(cpu, &dbs_info->cdbs.work,
-                                        usecs_to_jiffies(new_rate));
+                        gov_queue_work(dbs_data, dbs_info->cdbs.cur_policy,
+                                        usecs_to_jiffies(new_rate), true);
 
                 }
                 mutex_unlock(&dbs_info->cdbs.timer_mutex);