cpufreq: tegra186: set valid freq from freq_table

If the frequency set by bootloader is not present in freq_table.
Then set a valid freq from freq_table which is closest to the set
frequency.
This patch fixes below error which comes because the boot freq
is not present in present. Due to this the freq get function retuns
zero.
  cpufreq: cpufreq_online: ->get() failed
  cpufreq: cpufreq_online: ->get() failed

Signed-off-by: Sumit Gupta <sumitg@nvidia.com>

drivers/cpufreq/tegra186-cpufreq.c

@@ -51,10 +51,42 @@ struct tegra186_cpufreq_data {
 	struct tegra186_cpufreq_cluster *clusters;
 };
 
+static unsigned int tegra186_cpufreq_get(unsigned int cpu)
+{
+	struct cpufreq_frequency_table *tbl;
+	struct cpufreq_policy *policy;
+	void __iomem *edvd_reg;
+	unsigned int i, freq = 0;
+	u32 ndiv;
+
+	policy = cpufreq_cpu_get(cpu);
+	if (!policy)
+		return -EINVAL;
+
+	tbl = policy->freq_table;
+	edvd_reg = policy->driver_data;
+	ndiv = readl(edvd_reg) & EDVD_CORE_VOLT_FREQ_F_MASK;
+
+	for (i = 0; tbl[i].frequency != CPUFREQ_TABLE_END; i++) {
+		if ((tbl[i].driver_data & EDVD_CORE_VOLT_FREQ_F_MASK) == ndiv) {
+			freq = tbl[i].frequency;
+			break;
+		}
+	}
+
+	cpufreq_cpu_put(policy);
+
+	return freq;
+}
+
 static int tegra186_cpufreq_init(struct cpufreq_policy *policy)
 {
 	struct tegra186_cpufreq_data *data = cpufreq_get_driver_data();
-	unsigned int i;
+	struct cpufreq_frequency_table *pos;
+	void __iomem *edvd_reg;
+	unsigned int boot_freq;
+	u32 ndiv, prev_ndiv;
+	int i, ret;
 
 	for (i = 0; i < data->num_clusters; i++) {
 		struct tegra186_cpufreq_cluster *cluster = &data->clusters[i];
@@ -77,6 +109,39 @@ static int tegra186_cpufreq_init(struct cpufreq_policy *policy)
 
 	policy->cpuinfo.transition_latency = 300 * 1000;
 
+	ret = cpufreq_table_validate_and_sort(policy);
+	if (ret)
+		return ret;
+
+	policy->cur = tegra186_cpufreq_get(policy->cpu);
+
+	/* Are we running at unknown frequency ? */
+	ret = cpufreq_frequency_table_get_index(policy, policy->cur);
+	if (ret == -EINVAL) {
+		/*
+		 * If freq is not present in table then find frequency
+		 * closest to the last div and set that.
+		 */
+		edvd_reg = policy->driver_data;
+		ndiv = readl(edvd_reg) & EDVD_CORE_VOLT_FREQ_F_MASK;
+
+		cpufreq_for_each_valid_entry(pos, policy->freq_table) {
+			if ((prev_ndiv < ndiv) && (pos->driver_data > ndiv)) {
+				boot_freq = pos->frequency;
+				break;
+			}
+			prev_ndiv = pos->driver_data;
+		}
+		if (pos->frequency == CPUFREQ_TABLE_END)
+			boot_freq = policy->max;
+
+		ret = __cpufreq_driver_target(policy, boot_freq - 1,
+					      CPUFREQ_RELATION_L);
+		if (ret)
+			return ret;
+	}
+	policy->cur = tegra186_cpufreq_get(policy->cpu);
+
 	return 0;
 }
 
@@ -92,34 +157,6 @@ static int tegra186_cpufreq_set_target(struct cpufreq_policy *policy,
 	return 0;
 }
 
-static unsigned int tegra186_cpufreq_get(unsigned int cpu)
-{
-	struct cpufreq_frequency_table *tbl;
-	struct cpufreq_policy *policy;
-	void __iomem *edvd_reg;
-	unsigned int i, freq = 0;
-	u32 ndiv;
-
-	policy = cpufreq_cpu_get(cpu);
-	if (!policy)
-		return 0;
-
-	tbl = policy->freq_table;
-	edvd_reg = policy->driver_data;
-	ndiv = readl(edvd_reg) & EDVD_CORE_VOLT_FREQ_F_MASK;
-
-	for (i = 0; tbl[i].frequency != CPUFREQ_TABLE_END; i++) {
-		if ((tbl[i].driver_data & EDVD_CORE_VOLT_FREQ_F_MASK) == ndiv) {
-			freq = tbl[i].frequency;
-			break;
-		}
-	}
-
-	cpufreq_cpu_put(policy);
-
-	return freq;
-}
-
 static struct cpufreq_driver tegra186_cpufreq_driver = {
 	.name = "tegra186",
 	.flags = CPUFREQ_STICKY | CPUFREQ_HAVE_GOVERNOR_PER_POLICY |