[Patch 3/4] memory: tegra: add check if MRQ_EMC_DVFS_LATENCY is supported

[Sumit Gupta]

Add check to ensure that "MRQ_EMC_DVFS_LATENCY" is supported by the
BPMP-FW before making the MRQ request. Currently, if the BPMP-FW
doesn't support this MRQ, then the "tegra186_emc_probe" fails.
Due to this the Memory Interconnect initialization also doesn't
happen. Memory Interconnect is not dependent on this MRQ and can
initialize even when this MRQ is not supported in any platform.
The check ensures that the MRQ is called only when it is supported
by the BPMP-FW and Interconnect initializes independent of this MRQ.
Also, moved the code to new function for better readability.

Signed-off-by: Sumit Gupta <sumitg@xxxxxxxxxx>
---
 drivers/memory/tegra/tegra186-emc.c | 136 +++++++++++++++-------------
 1 file changed, 71 insertions(+), 65 deletions(-)

diff --git a/drivers/memory/tegra/tegra186-emc.c b/drivers/memory/tegra/tegra186-emc.c
index 6ad8a4023dd7..83981ae3ea86 100644
--- a/drivers/memory/tegra/tegra186-emc.c
+++ b/drivers/memory/tegra/tegra186-emc.c
@@ -155,6 +155,73 @@ DEFINE_DEBUGFS_ATTRIBUTE(tegra186_emc_debug_max_rate_fops,
 			  tegra186_emc_debug_max_rate_get,
 			  tegra186_emc_debug_max_rate_set, "%llu\n");
 
+static int tegra186_emc_get_emc_dvfs_latency(struct tegra186_emc *emc)
+{
+	struct mrq_emc_dvfs_latency_response response;
+	struct tegra_bpmp_message msg;
+	unsigned int i;
+	int err;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_EMC_DVFS_LATENCY;
+	msg.tx.data = NULL;
+	msg.tx.size = 0;
+	msg.rx.data = &response;
+	msg.rx.size = sizeof(response);
+
+	err = tegra_bpmp_transfer(emc->bpmp, &msg);
+	if (err < 0) {
+		dev_err(emc->dev, "failed to EMC DVFS pairs: %d\n", err);
+		return err;
+	}
+	if (msg.rx.ret < 0) {
+		dev_err(emc->dev, "EMC DVFS MRQ failed: %d (BPMP error code)\n", msg.rx.ret);
+		return -EINVAL;
+	}
+
+	emc->debugfs.min_rate = ULONG_MAX;
+	emc->debugfs.max_rate = 0;
+
+	emc->num_dvfs = response.num_pairs;
+
+	emc->dvfs = devm_kmalloc_array(emc->dev, emc->num_dvfs, sizeof(*emc->dvfs), GFP_KERNEL);
+	if (!emc->dvfs)
+		return -ENOMEM;
+
+	dev_dbg(emc->dev, "%u DVFS pairs:\n", emc->num_dvfs);
+
+	for (i = 0; i < emc->num_dvfs; i++) {
+		emc->dvfs[i].rate = response.pairs[i].freq * 1000;
+		emc->dvfs[i].latency = response.pairs[i].latency;
+
+		if (emc->dvfs[i].rate < emc->debugfs.min_rate)
+			emc->debugfs.min_rate = emc->dvfs[i].rate;
+
+		if (emc->dvfs[i].rate > emc->debugfs.max_rate)
+			emc->debugfs.max_rate = emc->dvfs[i].rate;
+
+		dev_dbg(emc->dev, "  %2u: %lu Hz -> %lu us\n", i,
+			emc->dvfs[i].rate, emc->dvfs[i].latency);
+	}
+
+	err = clk_set_rate_range(emc->clk, emc->debugfs.min_rate, emc->debugfs.max_rate);
+	if (err < 0) {
+		dev_err(emc->dev, "failed to set rate range [%lu-%lu] for %pC\n",
+			emc->debugfs.min_rate, emc->debugfs.max_rate, emc->clk);
+		return err;
+	}
+
+	emc->debugfs.root = debugfs_create_dir("emc", NULL);
+	debugfs_create_file("available_rates", S_IRUGO, emc->debugfs.root,
+			    emc, &tegra186_emc_debug_available_rates_fops);
+	debugfs_create_file("min_rate", S_IRUGO | S_IWUSR, emc->debugfs.root,
+			    emc, &tegra186_emc_debug_min_rate_fops);
+	debugfs_create_file("max_rate", S_IRUGO | S_IWUSR, emc->debugfs.root,
+			    emc, &tegra186_emc_debug_max_rate_fops);
+
+	return 0;
+}
+
 /*
  * tegra_emc_icc_set_bw() - Set BW api for EMC provider
  * @src: ICC node for External Memory Controller (EMC)
@@ -251,10 +318,7 @@ static int tegra_emc_interconnect_init(struct tegra186_emc *emc)
 static int tegra186_emc_probe(struct platform_device *pdev)
 {
 	struct tegra_mc *mc = dev_get_drvdata(pdev->dev.parent);
-	struct mrq_emc_dvfs_latency_response response;
-	struct tegra_bpmp_message msg;
 	struct tegra186_emc *emc;
-	unsigned int i;
 	int err;
 
 	emc = devm_kzalloc(&pdev->dev, sizeof(*emc), GFP_KERNEL);
@@ -275,70 +339,12 @@ static int tegra186_emc_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, emc);
 	emc->dev = &pdev->dev;
 
-	memset(&msg, 0, sizeof(msg));
-	msg.mrq = MRQ_EMC_DVFS_LATENCY;
-	msg.tx.data = NULL;
-	msg.tx.size = 0;
-	msg.rx.data = &response;
-	msg.rx.size = sizeof(response);
-
-	err = tegra_bpmp_transfer(emc->bpmp, &msg);
-	if (err < 0) {
-		dev_err(&pdev->dev, "failed to EMC DVFS pairs: %d\n", err);
-		goto put_bpmp;
-	}
-	if (msg.rx.ret < 0) {
-		err = -EINVAL;
-		dev_err(&pdev->dev, "EMC DVFS MRQ failed: %d (BPMP error code)\n", msg.rx.ret);
-		goto put_bpmp;
-	}
-
-	emc->debugfs.min_rate = ULONG_MAX;
-	emc->debugfs.max_rate = 0;
-
-	emc->num_dvfs = response.num_pairs;
-
-	emc->dvfs = devm_kmalloc_array(&pdev->dev, emc->num_dvfs,
-				       sizeof(*emc->dvfs), GFP_KERNEL);
-	if (!emc->dvfs) {
-		err = -ENOMEM;
-		goto put_bpmp;
-	}
-
-	dev_dbg(&pdev->dev, "%u DVFS pairs:\n", emc->num_dvfs);
-
-	for (i = 0; i < emc->num_dvfs; i++) {
-		emc->dvfs[i].rate = response.pairs[i].freq * 1000;
-		emc->dvfs[i].latency = response.pairs[i].latency;
-
-		if (emc->dvfs[i].rate < emc->debugfs.min_rate)
-			emc->debugfs.min_rate = emc->dvfs[i].rate;
-
-		if (emc->dvfs[i].rate > emc->debugfs.max_rate)
-			emc->debugfs.max_rate = emc->dvfs[i].rate;
-
-		dev_dbg(&pdev->dev, "  %2u: %lu Hz -> %lu us\n", i,
-			emc->dvfs[i].rate, emc->dvfs[i].latency);
-	}
-
-	err = clk_set_rate_range(emc->clk, emc->debugfs.min_rate,
-				 emc->debugfs.max_rate);
-	if (err < 0) {
-		dev_err(&pdev->dev,
-			"failed to set rate range [%lu-%lu] for %pC\n",
-			emc->debugfs.min_rate, emc->debugfs.max_rate,
-			emc->clk);
-		goto put_bpmp;
+	if (tegra_bpmp_mrq_is_supported(emc->bpmp, MRQ_EMC_DVFS_LATENCY)) {
+		err = tegra186_emc_get_emc_dvfs_latency(emc);
+		if (err)
+			goto put_bpmp;
 	}
 
-	emc->debugfs.root = debugfs_create_dir("emc", NULL);
-	debugfs_create_file("available_rates", S_IRUGO, emc->debugfs.root,
-			    emc, &tegra186_emc_debug_available_rates_fops);
-	debugfs_create_file("min_rate", S_IRUGO | S_IWUSR, emc->debugfs.root,
-			    emc, &tegra186_emc_debug_min_rate_fops);
-	debugfs_create_file("max_rate", S_IRUGO | S_IWUSR, emc->debugfs.root,
-			    emc, &tegra186_emc_debug_max_rate_fops);
-
 	if (mc && mc->soc->icc_ops) {
 		if (tegra_bpmp_mrq_is_supported(emc->bpmp, MRQ_BWMGR_INT)) {
 			mc->bwmgr_mrq_supported = true;
-- 
2.17.1