@@ -8,8 +8,10 @@
* Vinayak Holikatti <h.vinayak@samsung.com>
*/
+#include <linux/clk.h>
#include <linux/module.h>
#include <linux/platform_device.h>
+#include <linux/pm_opp.h>
#include <linux/pm_runtime.h>
#include <linux/of.h>
@@ -17,6 +19,8 @@
#include "ufshcd-pltfrm.h"
#include <ufs/unipro.h>
+#include <trace/events/ufs.h>
+
#define UFSHCD_DEFAULT_LANES_PER_DIRECTION 2
static int ufshcd_parse_clock_info(struct ufs_hba *hba)
@@ -205,6 +209,112 @@ static void ufshcd_init_lanes_per_dir(struct ufs_hba *hba)
}
}
+static int ufshcd_opp_config_clks(struct device *dev, struct opp_table *opp_table,
+ struct dev_pm_opp *opp, void *data,
+ bool scaling_down)
+{
+ struct ufs_hba *hba = dev_get_drvdata(dev);
+ struct list_head *head = &hba->clk_list_head;
+ struct ufs_clk_info *clki;
+ unsigned long freq;
+ u8 idx = 0;
+ int ret;
+
+ list_for_each_entry(clki, head, list) {
+ if (!IS_ERR_OR_NULL(clki->clk)) {
+ freq = dev_pm_opp_get_freq_indexed(opp, idx++);
+
+ /* Do not set rate for clocks having frequency as 0 */
+ if (!freq)
+ continue;
+
+ ret = clk_set_rate(clki->clk, freq);
+ if (ret) {
+ dev_err(dev, "%s: %s clk set rate(%ldHz) failed, %d\n",
+ __func__, clki->name, freq, ret);
+ return ret;
+ }
+
+ trace_ufshcd_clk_scaling(dev_name(dev),
+ (scaling_down ? "scaled down" : "scaled up"),
+ clki->name, hba->clk_scaling.target_freq, freq);
+ }
+ }
+
+ return 0;
+}
+
+static int ufshcd_parse_operating_points(struct ufs_hba *hba)
+{
+ struct device *dev = hba->dev;
+ struct device_node *np = dev->of_node;
+ struct dev_pm_opp_config config = {};
+ struct ufs_clk_info *clki;
+ const char **clk_names;
+ int cnt, i, ret;
+
+ if (!of_find_property(np, "operating-points-v2", NULL))
+ return 0;
+
+ if (of_find_property(np, "freq-table-hz", NULL)) {
+ dev_err(dev, "%s: operating-points and freq-table-hz are incompatible\n",
+ __func__);
+ return -EINVAL;
+ }
+
+ cnt = of_property_count_strings(np, "clock-names");
+ if (cnt <= 0) {
+ dev_err(dev, "%s: Missing clock-names\n", __func__);
+ return -ENODEV;
+ }
+
+ /* OPP expects clk_names to be NULL terminated */
+ clk_names = devm_kcalloc(dev, cnt + 1, sizeof(*clk_names), GFP_KERNEL);
+ if (!clk_names)
+ return -ENOMEM;
+
+ /*
+ * We still need to get reference to all clocks as the UFS core uses
+ * them separately.
+ */
+ for (i = 0; i < cnt; i++) {
+ ret = of_property_read_string_index(np, "clock-names", i,
+ &clk_names[i]);
+ if (ret)
+ return ret;
+
+ clki = devm_kzalloc(dev, sizeof(*clki), GFP_KERNEL);
+ if (!clki)
+ return -ENOMEM;
+
+ clki->name = devm_kstrdup(dev, clk_names[i], GFP_KERNEL);
+ if (!clki->name)
+ return -ENOMEM;
+
+ if (!strcmp(clk_names[i], "ref_clk"))
+ clki->keep_link_active = true;
+
+ list_add_tail(&clki->list, &hba->clk_list_head);
+ }
+
+ config.clk_names = clk_names,
+ config.config_clks = ufshcd_opp_config_clks;
+
+ ret = devm_pm_opp_set_config(dev, &config);
+ if (ret)
+ return ret;
+
+ ret = devm_pm_opp_of_add_table(dev);
+ if (ret) {
+ dev_err(dev, "Failed to add OPP table: %d\n", ret);
+ return ret;
+ }
+
+ hba->use_pm_opp = true;
+
+ return 0;
+}
+
/**
* ufshcd_get_pwr_dev_param - get finally agreed attributes for
* power mode change
@@ -371,6 +481,12 @@ int ufshcd_pltfrm_init(struct platform_device *pdev,
ufshcd_init_lanes_per_dir(hba);
+ err = ufshcd_parse_operating_points(hba);
+ if (err) {
+ dev_err(dev, "%s: OPP parse failed %d\n", __func__, err);
+ goto dealloc_host;
+ }
+
err = ufshcd_init(hba, mmio_base, irq);
if (err) {
dev_err(dev, "Initialization failed\n");