diff --git a/drivers/cpufreq/tegra194-cpufreq.c b/drivers/cpufreq/tegra194-cpufreq.c
index d250e49..cc28b1e3 100644
--- a/drivers/cpufreq/tegra194-cpufreq.c
+++ b/drivers/cpufreq/tegra194-cpufreq.c
@@ -7,6 +7,7 @@
 #include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/iopoll.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
@@ -21,7 +22,6 @@
 #define KHZ                     1000
 #define REF_CLK_MHZ             408 /* 408 MHz */
 #define US_DELAY                500
-#define US_DELAY_MIN            2
 #define CPUFREQ_TBL_STEP_HZ     (50 * KHZ * KHZ)
 #define MAX_CNT                 ~0U
 
@@ -249,17 +249,22 @@ static unsigned int tegra194_get_speed(u32 cpu)
 static int tegra194_cpufreq_init(struct cpufreq_policy *policy)
 {
 	struct tegra194_cpufreq_data *data = cpufreq_get_driver_data();
-	u32 cpu;
+	u32 cpu = policy->cpu;
+	int ret;
 	u32 cl;
 
-	smp_call_function_single(policy->cpu, get_cpu_cluster, &cl, true);
+	if (!cpu_online(cpu))
+		return -EINVAL;
+
+	ret = smp_call_function_single(cpu, get_cpu_cluster, &cl, true);
+	if (ret) {
+		pr_err("cpufreq: Failed to get cluster for CPU%d\n", cpu);
+		return ret;
+	}
 
 	if (cl >= data->num_clusters)
 		return -EINVAL;
 
-	/* boot freq */
-	policy->cur = tegra194_get_speed_common(policy->cpu, US_DELAY_MIN);
-
 	/* set same policy for all cpus in a cluster */
 	for (cpu = (cl * 2); cpu < ((cl + 1) * 2); cpu++)
 		cpumask_set_cpu(cpu, policy->cpus);
@@ -267,7 +272,23 @@ static int tegra194_cpufreq_init(struct cpufreq_policy *policy)
 	policy->freq_table = data->tables[cl];
 	policy->cpuinfo.transition_latency = TEGRA_CPUFREQ_TRANSITION_LATENCY;
 
-	return 0;
+	policy->cur = tegra194_get_speed_common(policy->cpu, US_DELAY);
+
+	ret = cpufreq_table_validate_and_sort(policy);
+	if (ret)
+		return ret;
+
+	/* Are we running at unknown frequency ? */
+	ret = cpufreq_frequency_table_get_index(policy, policy->cur);
+	if (ret == -EINVAL) {
+		ret = __cpufreq_driver_target(policy, policy->cur - 1,
+					      CPUFREQ_RELATION_L);
+		if (ret)
+			return ret;
+		policy->cur = tegra194_get_speed_common(policy->cpu, US_DELAY);
+	}
+
+	return ret;
 }
 
 static int tegra194_cpufreq_set_target(struct cpufreq_policy *policy,
@@ -285,6 +306,55 @@ static int tegra194_cpufreq_set_target(struct cpufreq_policy *policy,
 	return 0;
 }
 
+static int tegra194_cpufreq_online(struct cpufreq_policy *policy)
+{
+	unsigned int interm_freq, last_set_freq;
+	struct cpufreq_frequency_table *pos;
+	u64 ndiv;
+	int ret;
+
+	if (!cpu_online(policy->cpu))
+		return -EINVAL;
+
+	/* get ndiv for the last frequency request from software  */
+	ret = smp_call_function_single(policy->cpu, get_cpu_ndiv, &ndiv, true);
+	if (ret) {
+		pr_err("cpufreq: Failed to get ndiv for CPU%d\n", policy->cpu);
+		return ret;
+	}
+
+	cpufreq_for_each_valid_entry(pos, policy->freq_table) {
+		if (pos->driver_data == ndiv) {
+			last_set_freq = pos->frequency;
+			break;
+		}
+	}
+
+	policy->cur = tegra194_get_speed_common(policy->cpu, US_DELAY);
+	interm_freq =  policy->cur;
+
+	/*
+	 * It takes some time to restore the previous frequency while
+	 * turning-on non-boot cores during exit from SC7(Suspend-to-RAM).
+	 * So, wait till it reaches the previous value and timeout if the
+	 * time taken to reach requested freq is >100ms
+	 */
+	ret = read_poll_timeout(tegra194_get_speed_common, policy->cur,
+				abs(policy->cur - last_set_freq) <= 115200, 0,
+				100 * USEC_PER_MSEC, false, policy->cpu,
+				US_DELAY);
+	if (ret)
+		pr_warn("cpufreq: cpu%d, cur:%u, last set:%u, intermed:%u\n",
+			policy->cpu, policy->cur, last_set_freq, interm_freq);
+
+	return ret;
+}
+
+static int tegra194_cpufreq_offline(struct cpufreq_policy *policy)
+{
+	return 0;
+}
+
 static struct cpufreq_driver tegra194_cpufreq_driver = {
 	.name = "tegra194",
 	.flags = CPUFREQ_STICKY | CPUFREQ_CONST_LOOPS |
@@ -294,6 +364,8 @@ static struct cpufreq_driver tegra194_cpufreq_driver = {
 	.get = tegra194_get_speed,
 	.init = tegra194_cpufreq_init,
 	.attr = cpufreq_generic_attr,
+	.online = tegra194_cpufreq_online,
+	.offline = tegra194_cpufreq_offline,
 };
 
 static void tegra194_cpufreq_free_resources(void)
