PM / OPP: Support adjusting OPP voltages at runtime

[firefly-linux-kernel-4.4.55.git] / drivers / base / power / opp / core.c

diff --git a/drivers/base/power/opp/core.c b/drivers/base/power/opp/core.c
index 433b60092972d56abba55897158d6c22156cf631..9d232ea6c718d8b77da957b7efbd49c25119d235 100644 (file)
--- a/drivers/base/power/opp/core.c
+++ b/drivers/base/power/opp/core.c
@@ -1769,6 +1769,94 @@ unlock:
        return r;
 }
 
+/*
+ * dev_pm_opp_adjust_voltage() - helper to change the voltage of an OPP
+ * @dev:               device for which we do this operation
+ * @freq:              OPP frequency to adjust voltage of
+ * @u_volt:            new OPP voltage
+ *
+ * Change the voltage of an OPP with an RCU operation.
+ *
+ * Return: -EINVAL for bad pointers, -ENOMEM if no memory available for the
+ * copy operation, returns 0 if no modifcation was done OR modification was
+ * successful.
+ *
+ * Locking: The internal device_opp and opp structures are RCU protected.
+ * Hence this function internally uses RCU updater strategy with mutex locks to
+ * keep the integrity of the internal data structures. Callers should ensure
+ * that this function is *NOT* called under RCU protection or in contexts where
+ * mutex locking or synchronize_rcu() blocking calls cannot be used.
+ */
+int dev_pm_opp_adjust_voltage(struct device *dev, unsigned long freq,
+                             unsigned long u_volt)
+{
+       struct opp_table *opp_table;
+       struct dev_pm_opp *new_opp, *tmp_opp, *opp = ERR_PTR(-ENODEV);
+       int r = 0;
+
+       /* keep the node allocated */
+       new_opp = kmalloc(sizeof(*new_opp), GFP_KERNEL);
+       if (!new_opp)
+               return -ENOMEM;
+
+       mutex_lock(&opp_table_lock);
+
+       /* Find the opp_table */
+       opp_table = _find_opp_table(dev);
+       if (IS_ERR(opp_table)) {
+               r = PTR_ERR(opp_table);
+               dev_warn(dev, "%s: Device OPP not found (%d)\n", __func__, r);
+               goto unlock;
+       }
+
+       /* Do we have the frequency? */
+       list_for_each_entry(tmp_opp, &opp_table->opp_list, node) {
+               if (tmp_opp->rate == freq) {
+                       opp = tmp_opp;
+                       break;
+               }
+       }
+       if (IS_ERR(opp)) {
+               r = PTR_ERR(opp);
+               goto unlock;
+       }
+
+       /* Is update really needed? */
+       if (opp->u_volt == u_volt)
+               goto unlock;
+       /* copy the old data over */
+       *new_opp = *opp;
+
+       /* plug in new node */
+       new_opp->u_volt = u_volt;
+
+       if (new_opp->u_volt_min > u_volt)
+               new_opp->u_volt_min = u_volt;
+       if (new_opp->u_volt_max < u_volt)
+               new_opp->u_volt_max = u_volt;
+
+       _opp_remove(opp_table, opp, false);
+       r = _opp_add(dev, new_opp, opp_table);
+       if (r) {
+               dev_err(dev, "Failed to add new_opp (u_volt=%lu)\n", u_volt);
+               _opp_add(dev, opp, opp_table);
+               goto unlock;
+       }
+
+       mutex_unlock(&opp_table_lock);
+
+       /* Notify the change of the OPP */
+       srcu_notifier_call_chain(&opp_table->srcu_head,
+                                OPP_EVENT_ADJUST_VOLTAGE, new_opp);
+
+       return 0;
+
+unlock:
+       mutex_unlock(&opp_table_lock);
+       kfree(new_opp);
+       return r;
+}
+
 /**
  * dev_pm_opp_enable() - Enable a specific OPP
  * @dev:       device for which we do this operation