diff --git a/drivers/devfreq/bimc-bwmon.c b/drivers/devfreq/bimc-bwmon.c index a5b3ef2640bf..726a3f70d936 100644 --- a/drivers/devfreq/bimc-bwmon.c +++ b/drivers/devfreq/bimc-bwmon.c @@ -215,7 +215,8 @@ static int start_bw_hwmon(struct bw_hwmon *hw, unsigned long mbps) IRQF_ONESHOT | IRQF_SHARED, dev_name(m->dev), m); if (ret) { - dev_err(m->dev, "Unable to register interrupt handler!\n"); + dev_err(m->dev, "Unable to register interrupt handler! (%d)\n", + ret); return ret; } @@ -244,6 +245,30 @@ static void stop_bw_hwmon(struct bw_hwmon *hw) mon_irq_clear(m); } +static int suspend_bw_hwmon(struct bw_hwmon *hw) +{ + struct bwmon *m = to_bwmon(hw); + + disable_irq(m->irq); + mon_disable(m); + mon_irq_disable(m); + mon_irq_clear(m); + + return 0; +} + +static int resume_bw_hwmon(struct bw_hwmon *hw) +{ + struct bwmon *m = to_bwmon(hw); + + mon_clear(m); + mon_irq_enable(m); + mon_enable(m); + enable_irq(m->irq); + + return 0; +} + /*************************************************************************/ static int bimc_bwmon_driver_probe(struct platform_device *pdev) @@ -299,6 +324,8 @@ static int bimc_bwmon_driver_probe(struct platform_device *pdev) return -EINVAL; m->hw.start_hwmon = &start_bw_hwmon; m->hw.stop_hwmon = &stop_bw_hwmon; + m->hw.suspend_hwmon = &suspend_bw_hwmon; + m->hw.resume_hwmon = &resume_bw_hwmon; m->hw.meas_bw_and_set_irq = &meas_bw_and_set_irq; ret = register_bw_hwmon(dev, &m->hw); diff --git a/drivers/devfreq/governor_bw_hwmon.c b/drivers/devfreq/governor_bw_hwmon.c index a65c7d49870a..299b1af3a7a5 100644 --- a/drivers/devfreq/governor_bw_hwmon.c +++ b/drivers/devfreq/governor_bw_hwmon.c @@ -40,6 +40,8 @@ struct hwmon_node { unsigned int bw_step; unsigned long prev_ab; unsigned long *dev_ab; + unsigned long resume_freq; + unsigned long resume_ab; ktime_t prev_ts; bool mon_started; struct list_head list; @@ -219,10 +221,61 @@ int update_bw_hwmon(struct bw_hwmon *hwmon) return 0; } -static int start_monitoring(struct devfreq *df) +static int start_monitor(struct devfreq *df, bool init) +{ + struct hwmon_node *node = df->data; + struct bw_hwmon *hw = node->hw; + struct device *dev = df->dev.parent; + unsigned long mbps; + int ret; + + node->prev_ts = ktime_get(); + + if (init) { + node->prev_ab = 0; + node->resume_freq = 0; + node->resume_ab = 0; + mbps = (df->previous_freq * node->io_percent) / 100; + ret = hw->start_hwmon(hw, mbps); + } else { + ret = hw->resume_hwmon(hw); + } + + if (ret) { + dev_err(dev, "Unable to start HW monitor! (%d)\n", ret); + return ret; + } + + if (init) + devfreq_monitor_start(df); + else + devfreq_monitor_resume(df); + + node->mon_started = true; + + return 0; +} + +static void stop_monitor(struct devfreq *df, bool init) +{ + struct hwmon_node *node = df->data; + struct bw_hwmon *hw = node->hw; + + node->mon_started = false; + + if (init) { + devfreq_monitor_stop(df); + hw->stop_hwmon(hw); + } else { + devfreq_monitor_suspend(df); + hw->suspend_hwmon(hw); + } + +} + +static int gov_start(struct devfreq *df) { int ret = 0; - unsigned long mbps; struct device *dev = df->dev.parent; struct hwmon_node *node; struct bw_hwmon *hw; @@ -247,17 +300,8 @@ static int start_monitoring(struct devfreq *df) node->orig_data = df->data; df->data = node; - node->prev_ts = ktime_get(); - node->prev_ab = 0; - mbps = (df->previous_freq * node->io_percent) / 100; - ret = hw->start_hwmon(hw, mbps); - if (ret) { - dev_err(dev, "Unable to start HW monitor!\n"); + if (start_monitor(df, true)) goto err_start; - } - - devfreq_monitor_start(df); - node->mon_started = true; ret = sysfs_create_group(&df->dev.kobj, node->attr_grp); if (ret) @@ -266,9 +310,7 @@ static int start_monitoring(struct devfreq *df) return 0; err_sysfs: - node->mon_started = false; - devfreq_monitor_stop(df); - hw->stop_hwmon(hw); + stop_monitor(df, true); err_start: df->data = node->orig_data; node->orig_data = NULL; @@ -277,15 +319,13 @@ err_start: return ret; } -static void stop_monitoring(struct devfreq *df) +static void gov_stop(struct devfreq *df) { struct hwmon_node *node = df->data; struct bw_hwmon *hw = node->hw; sysfs_remove_group(&df->dev.kobj, node->attr_grp); - node->mon_started = false; - devfreq_monitor_stop(df); - hw->stop_hwmon(hw); + stop_monitor(df, true); df->data = node->orig_data; node->orig_data = NULL; hw->df = NULL; @@ -300,12 +340,67 @@ static void stop_monitoring(struct devfreq *df) node->dev_ab = NULL; } +static int gov_suspend(struct devfreq *df) +{ + struct hwmon_node *node = df->data; + unsigned long resume_freq = df->previous_freq; + unsigned long resume_ab = *node->dev_ab; + + if (!node->hw->suspend_hwmon) + return -EPERM; + + if (node->resume_freq) { + dev_warn(df->dev.parent, "Governor already suspended!\n"); + return -EBUSY; + } + + stop_monitor(df, false); + + mutex_lock(&df->lock); + update_devfreq(df); + mutex_unlock(&df->lock); + + node->resume_freq = resume_freq; + node->resume_ab = resume_ab; + + return 0; +} + +static int gov_resume(struct devfreq *df) +{ + struct hwmon_node *node = df->data; + + if (!node->hw->resume_hwmon) + return -EPERM; + + if (!node->resume_freq) { + dev_warn(df->dev.parent, "Governor already resumed!\n"); + return -EBUSY; + } + + mutex_lock(&df->lock); + update_devfreq(df); + mutex_unlock(&df->lock); + + node->resume_freq = 0; + node->resume_ab = 0; + + return start_monitor(df, false); +} + static int devfreq_bw_hwmon_get_freq(struct devfreq *df, unsigned long *freq) { unsigned long mbps; struct hwmon_node *node = df->data; + /* Suspend/resume sequence */ + if (!node->mon_started) { + *freq = node->resume_freq; + *node->dev_ab = node->resume_ab; + return 0; + } + mbps = measure_bw_and_set_irq(node); compute_bw(node, mbps, freq, node->dev_ab); @@ -345,7 +440,7 @@ static int devfreq_bw_hwmon_ev_handler(struct devfreq *df, sample_ms = min(MAX_MS, sample_ms); df->profile->polling_ms = sample_ms; - ret = start_monitoring(df); + ret = gov_start(df); if (ret) return ret; @@ -354,7 +449,7 @@ static int devfreq_bw_hwmon_ev_handler(struct devfreq *df, break; case DEVFREQ_GOV_STOP: - stop_monitoring(df); + gov_stop(df); dev_dbg(df->dev.parent, "Disabled dev BW HW monitor governor\n"); break; @@ -365,6 +460,30 @@ static int devfreq_bw_hwmon_ev_handler(struct devfreq *df, sample_ms = min(MAX_MS, sample_ms); devfreq_interval_update(df, &sample_ms); break; + + case DEVFREQ_GOV_SUSPEND: + ret = gov_suspend(df); + if (ret) { + dev_err(df->dev.parent, + "Unable to suspend BW HW mon governor (%d)\n", + ret); + return ret; + } + + dev_dbg(df->dev.parent, "Suspended BW HW mon governor\n"); + break; + + case DEVFREQ_GOV_RESUME: + ret = gov_resume(df); + if (ret) { + dev_err(df->dev.parent, + "Unable to resume BW HW mon governor (%d)\n", + ret); + return ret; + } + + dev_dbg(df->dev.parent, "Resumed BW HW mon governor\n"); + break; } return 0; diff --git a/drivers/devfreq/governor_bw_hwmon.h b/drivers/devfreq/governor_bw_hwmon.h index c5779b59db81..8c368e591274 100644 --- a/drivers/devfreq/governor_bw_hwmon.h +++ b/drivers/devfreq/governor_bw_hwmon.h @@ -47,6 +47,8 @@ struct bw_hwmon { int (*start_hwmon)(struct bw_hwmon *hw, unsigned long mbps); void (*stop_hwmon)(struct bw_hwmon *hw); + int (*suspend_hwmon)(struct bw_hwmon *hw); + int (*resume_hwmon)(struct bw_hwmon *hw); unsigned long (*meas_bw_and_set_irq)(struct bw_hwmon *hw, unsigned int tol, unsigned int us); struct device *dev;