[PATCH v1 04/10] devfreq: Factor out devfreq_set_governor[_locked]()

[Jie Zhan]

Factor out common governor setting logic into devfreq_set_governor() to
reduce code duplication in governor_store() and devfreq_add_device().

Additionally, devfreq_set_governor_locked() is used when
'devfreq_list_lock' is already held by the caller, e.g. in
devfreq_set_governor(), to avoid an immediate relock after unlock.

Note that the new functions support setting a governor even if
devfreq->governor is NULL.

Signed-off-by: Jie Zhan <zhanjie9 at hisilicon.com>
---
 drivers/devfreq/devfreq.c | 129 +++++++++++++++++++-------------------
 1 file changed, 63 insertions(+), 66 deletions(-)

diff --git a/drivers/devfreq/devfreq.c b/drivers/devfreq/devfreq.c
index e54e3092e0e0..378a01f71165 100644
--- a/drivers/devfreq/devfreq.c
+++ b/drivers/devfreq/devfreq.c
@@ -313,6 +313,64 @@ static struct devfreq_governor *try_then_request_governor(const char *name)
 	return governor;
 }
 
+static int devfreq_set_governor_locked(struct devfreq *df,
+				       const struct devfreq_governor *new_gov)
+{
+	const struct devfreq_governor *old_gov;
+	struct device *dev;
+	int ret;
+
+	old_gov = df->governor;
+	dev = &df->dev;
+
+	if (old_gov) {
+		if (old_gov == new_gov)
+			return 0;
+
+		if (IS_SUPPORTED_FLAG(old_gov->flags, IMMUTABLE) ||
+		    IS_SUPPORTED_FLAG(new_gov->flags, IMMUTABLE))
+			return -EINVAL;
+
+		/* Stop the current governor */
+		ret = df->governor->event_handler(df, DEVFREQ_GOV_STOP, NULL);
+		if (ret) {
+			dev_warn(dev, "%s: Governor %s not stopped(%d)\n",
+				 __func__, df->governor->name, ret);
+			return ret;
+		}
+	}
+
+	/* Start the new governor */
+	df->governor = new_gov;
+	ret = df->governor->event_handler(df, DEVFREQ_GOV_START, NULL);
+	if (ret) {
+		dev_warn(dev, "%s: Governor %s not started(%d)\n",
+			 __func__, df->governor->name, ret);
+
+		if (!old_gov)
+			return ret;
+
+		/* Restore previous governor */
+		df->governor = old_gov;
+		ret = df->governor->event_handler(df, DEVFREQ_GOV_START, NULL);
+		if (ret) {
+			dev_err(dev, "%s: restore Governor %s failed (%d)\n",
+				__func__, old_gov->name, ret);
+			df->governor = NULL;
+			return ret;
+		}
+	}
+
+	return sysfs_update_group(&df->dev.kobj, &gov_attr_group);
+}
+
+static int devfreq_set_governor(struct devfreq *df,
+				const struct devfreq_governor *new_gov)
+{
+	guard(mutex)(&devfreq_list_lock);
+	return devfreq_set_governor_locked(df, new_gov);
+}
+
 static int devfreq_notify_transition(struct devfreq *devfreq,
 		struct devfreq_freqs *freqs, unsigned int state)
 {
@@ -936,22 +994,11 @@ struct devfreq *devfreq_add_device(struct device *dev,
 	}
 
 	mutex_lock(&devfreq_list_lock);
-	devfreq->governor = governor;
-	err = devfreq->governor->event_handler(devfreq, DEVFREQ_GOV_START,
-						NULL);
-	if (err) {
-		dev_err_probe(dev, err,
-			"%s: Unable to start governor for the device\n",
-			 __func__);
-		goto err_init;
-	}
-
-	err = sysfs_update_group(&devfreq->dev.kobj, &gov_attr_group);
+	err = devfreq_set_governor_locked(devfreq, governor);
 	if (err)
-		goto err_init;
+		goto err_devfreq;
 
 	list_add(&devfreq->node, &devfreq_list);
-
 	mutex_unlock(&devfreq_list_lock);
 
 	if (devfreq->profile->is_cooling_device) {
@@ -962,8 +1009,6 @@ struct devfreq *devfreq_add_device(struct device *dev,
 
 	return devfreq;
 
-err_init:
-	mutex_unlock(&devfreq_list_lock);
 err_devfreq:
 	devfreq_remove_device(devfreq);
 	devfreq = NULL;
@@ -1409,7 +1454,7 @@ static ssize_t governor_store(struct device *dev, struct device_attribute *attr,
 	struct devfreq *df = to_devfreq(dev);
 	int ret;
 	char str_governor[DEVFREQ_NAME_LEN + 1];
-	const struct devfreq_governor *governor, *prev_governor;
+	const struct devfreq_governor *governor;
 
 	if (!df->governor)
 		return -EINVAL;
@@ -1422,57 +1467,9 @@ static ssize_t governor_store(struct device *dev, struct device_attribute *attr,
 	if (IS_ERR(governor))
 		return PTR_ERR(governor);
 
-	guard(mutex)(&devfreq_list_lock);
-	if (df->governor == governor)
-		return count;
-
-	if (IS_SUPPORTED_FLAG(df->governor->flags, IMMUTABLE) ||
-	    IS_SUPPORTED_FLAG(governor->flags, IMMUTABLE))
-		return -EINVAL;
-
-	/*
-	 * Stop the current governor and remove the specific sysfs files
-	 * which depend on current governor.
-	 */
-	ret = df->governor->event_handler(df, DEVFREQ_GOV_STOP, NULL);
-	if (ret) {
-		dev_warn(dev, "%s: Governor %s not stopped(%d)\n",
-			 __func__, df->governor->name, ret);
-		return ret;
-	}
-
-	/*
-	 * Start the new governor and create the specific sysfs files
-	 * which depend on the new governor.
-	 */
-	prev_governor = df->governor;
-	df->governor = governor;
-	ret = df->governor->event_handler(df, DEVFREQ_GOV_START, NULL);
-	if (ret) {
-		dev_warn(dev, "%s: Governor %s not started(%d)\n",
-			 __func__, df->governor->name, ret);
-
-		/* Restore previous governor */
-		df->governor = prev_governor;
-		ret = df->governor->event_handler(df, DEVFREQ_GOV_START, NULL);
-		if (ret) {
-			dev_err(dev,
-				"%s: reverting to Governor %s failed (%d)\n",
-				__func__, prev_governor->name, ret);
-			df->governor = NULL;
-			return ret;
-		}
-	}
-
-	/*
-	 * Create the sysfs files for the new governor. But if failed to start
-	 * the new governor, restore the sysfs files of previous governor.
-	 */
-	ret = sysfs_update_group(&df->dev.kobj, &gov_attr_group);
-	if (ret)
-		return ret;
+	ret = devfreq_set_governor(df, governor);
 
-	return count;
+	return ret ? ret : count;
 }
 static DEVICE_ATTR_RW(governor);
 
-- 
2.43.0