From patchwork Wed Sep 16 17:11:16 2020 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Sumit Gupta X-Patchwork-Id: 11780165 Return-Path: Received: from mail.kernel.org (pdx-korg-mail-1.web.codeaurora.org [172.30.200.123]) by pdx-korg-patchwork-2.web.codeaurora.org (Postfix) with ESMTP id 91C756CA for ; Wed, 16 Sep 2020 17:11:48 +0000 (UTC) Received: from merlin.infradead.org (merlin.infradead.org [205.233.59.134]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-GCM-SHA384 (256/256 bits)) (No client certificate requested) by mail.kernel.org (Postfix) with ESMTPS id 6010420770 for ; Wed, 16 Sep 2020 17:11:48 +0000 (UTC) Authentication-Results: mail.kernel.org; dkim=pass (2048-bit key) header.d=lists.infradead.org header.i=@lists.infradead.org header.b="3UNkioj9"; dkim=fail reason="signature verification failed" (2048-bit key) header.d=nvidia.com header.i=@nvidia.com header.b="e5NpJqEt" DMARC-Filter: OpenDMARC Filter v1.3.2 mail.kernel.org 6010420770 Authentication-Results: mail.kernel.org; dmarc=fail (p=none dis=none) header.from=nvidia.com Authentication-Results: mail.kernel.org; spf=none smtp.mailfrom=linux-arm-kernel-bounces+patchwork-linux-arm=patchwork.kernel.org@lists.infradead.org DKIM-Signature: v=1; a=rsa-sha256; q=dns/txt; c=relaxed/relaxed; d=lists.infradead.org; s=merlin.20170209; h=Sender:Content-Transfer-Encoding: Content-Type:Cc:List-Subscribe:List-Help:List-Post:List-Archive: List-Unsubscribe:List-Id:MIME-Version:References:In-Reply-To:Message-ID:Date: Subject:To:From:Reply-To:Content-ID:Content-Description:Resent-Date: Resent-From:Resent-Sender:Resent-To:Resent-Cc:Resent-Message-ID:List-Owner; bh=cIairRLt55NM0o73cz2fHSDdN7nJtVcG3MSMWKJNNnA=; b=3UNkioj9Ik0h+4CZaEFCtG9Hv YRTL0gBPZoUIsAtgn7zmQo3ptN3MZ4iLc5TDT7PquyxidrKz95KZ6JDUXO2T7psB1Vj2F02LaUyQm 4yB+uQ3DdjHPPB538SBkH4f9G4SIKe+lSuZty+huV/DTZ9KTIaYDypDx8gaya4IumDdpEeoxIV8GN 3QVjEyUstQjaOPT1FJ+gDen96yGfLOpCPuDJ0sC2xrUbFliHd2s5v4EXzSfIm/0KHI0EcyFftejVV FeIR5gdhlGgvpd4Qzkn5x48v1pcf87SOp6ndtqCv7pmP+ih0xH+ZKh8vwbyt0qlNzlzRvuOMxfeUL JnhJA4BLA==; Received: from localhost ([::1] helo=merlin.infradead.org) by merlin.infradead.org with esmtp (Exim 4.92.3 #3 (Red Hat Linux)) id 1kIay0-0005WP-R1; Wed, 16 Sep 2020 17:11:36 +0000 Received: from hqnvemgate26.nvidia.com ([216.228.121.65]) by merlin.infradead.org with esmtps (Exim 4.92.3 #3 (Red Hat Linux)) id 1kIaxt-0005UA-FE for linux-arm-kernel@lists.infradead.org; Wed, 16 Sep 2020 17:11:31 +0000 Received: from hqpgpgate101.nvidia.com (Not Verified[216.228.121.13]) by hqnvemgate26.nvidia.com (using TLS: TLSv1.2, DES-CBC3-SHA) id ; Wed, 16 Sep 2020 10:11:14 -0700 Received: from hqmail.nvidia.com ([172.20.161.6]) by hqpgpgate101.nvidia.com (PGP Universal service); Wed, 16 Sep 2020 10:11:27 -0700 X-PGP-Universal: processed; by hqpgpgate101.nvidia.com on Wed, 16 Sep 2020 10:11:27 -0700 Received: from HQMAIL101.nvidia.com (172.20.187.10) by HQMAIL105.nvidia.com (172.20.187.12) with Microsoft SMTP Server (TLS) id 15.0.1473.3; Wed, 16 Sep 2020 17:11:27 +0000 Received: from hqnvemgw03.nvidia.com (10.124.88.68) by HQMAIL101.nvidia.com (172.20.187.10) with Microsoft SMTP Server (TLS) id 15.0.1473.3 via Frontend Transport; Wed, 16 Sep 2020 17:11:27 +0000 Received: from sumitg-l4t.nvidia.com (Not Verified[10.24.37.103]) by hqnvemgw03.nvidia.com with Trustwave SEG (v7, 5, 8, 10121) id ; Wed, 16 Sep 2020 10:11:27 -0700 From: Sumit Gupta To: , , , , , , , Subject: [Patch 1/2] cpufreq: tegra194: get consistent cpuinfo_cur_freq Date: Wed, 16 Sep 2020 22:41:16 +0530 Message-ID: <1600276277-7290-2-git-send-email-sumitg@nvidia.com> X-Mailer: git-send-email 2.7.4 In-Reply-To: <1600276277-7290-1-git-send-email-sumitg@nvidia.com> References: <1600276277-7290-1-git-send-email-sumitg@nvidia.com> X-NVConfidentiality: public MIME-Version: 1.0 DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=nvidia.com; s=n1; t=1600276274; bh=oXH+axlIrWrmMMW7JliHScXS/IYj9I3hPRTXw99Y67M=; h=X-PGP-Universal:From:To:CC:Subject:Date:Message-ID:X-Mailer: In-Reply-To:References:X-NVConfidentiality:MIME-Version: Content-Type; b=e5NpJqEtVyF0iSjm3HSYcIZEjbmI05YLKBUhC83u3VL1qlZA20cd13Okj2u9fKqoe /L5Fcp1uRMIAYT5Q1yNkUTqMaqhmE2Dy9LzjKVKTxUXvt2R07j6soJ96kyU5nzllqM OIebOGaUZpbpz46qVQbxJsPY72KhxAY3I1oAr02zpPWQOS1qUmP6UOKjmnGEnsGBsg ljFa0ODuT/WNxQ7onZ7AygSQJ2ER+a7f/hUhb4oXqX5ZwSQ6ExHQDwv4nOHXLsk+zX HIL37shFft9GCvSLNj235VXgsgXdmitp+dFq8h9yzAR2lQd0d0q5mU9npk+9KJIg5u vstmOw40P0TKg== X-CRM114-Version: 20100106-BlameMichelson ( TRE 0.8.0 (BSD) ) MR-646709E3 X-CRM114-CacheID: sfid-20200916_131129_619255_0BF2B6D0 X-CRM114-Status: GOOD ( 18.38 ) X-Spam-Score: -8.2 (--------) X-Spam-Report: SpamAssassin version 3.4.4 on merlin.infradead.org summary: Content analysis details: (-8.2 points) pts rule name description ---- ---------------------- -------------------------------------------------- -5.0 RCVD_IN_DNSWL_HI RBL: Sender listed at https://www.dnswl.org/, high trust [216.228.121.65 listed in list.dnswl.org] 0.0 SPF_HELO_NONE SPF: HELO does not publish an SPF Record -0.0 SPF_PASS SPF: sender matches SPF record -0.1 DKIM_VALID_AU Message has a valid DKIM or DK signature from author's domain -0.1 DKIM_VALID_EF Message has a valid DKIM or DK signature from envelope-from domain -0.1 DKIM_VALID Message has at least one valid DKIM or DK signature 0.1 DKIM_SIGNED Message has a DKIM or DK signature, not necessarily valid -3.0 DKIMWL_WL_HIGH DKIMwl.org - Whitelisted High sender X-BeenThere: linux-arm-kernel@lists.infradead.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Cc: ksitaraman@nvidia.com, sumitg@nvidia.com, bbasu@nvidia.com Sender: "linux-arm-kernel" Errors-To: linux-arm-kernel-bounces+patchwork-linux-arm=patchwork.kernel.org@lists.infradead.org Frequency returned by 'cpuinfo_cur_freq' using counters is not fixed and keeps changing slightly. This change returns a consistent value from freq_table. If the reconstructed frequency has acceptable delta from the last written value, then return the frequency corresponding to the last written ndiv value from freq_table. Otherwise, print a warning and return the reconstructed freq. Signed-off-by: Sumit Gupta Reviewed-by: Jon Hunter Tested-by: Jon Hunter Acked-by: Thierry Reding --- drivers/cpufreq/tegra194-cpufreq.c | 66 ++++++++++++++++++++++++++++++++------ 1 file changed, 57 insertions(+), 9 deletions(-) diff --git a/drivers/cpufreq/tegra194-cpufreq.c b/drivers/cpufreq/tegra194-cpufreq.c index e1d931c..d5b608d 100644 --- a/drivers/cpufreq/tegra194-cpufreq.c +++ b/drivers/cpufreq/tegra194-cpufreq.c @@ -180,9 +180,65 @@ static unsigned int tegra194_get_speed_common(u32 cpu, u32 delay) return (rate_mhz * KHZ); /* in KHz */ } +static void get_cpu_ndiv(void *ndiv) +{ + u64 ndiv_val; + + asm volatile("mrs %0, s3_0_c15_c0_4" : "=r" (ndiv_val) : ); + + *(u64 *)ndiv = ndiv_val; +} + +static void set_cpu_ndiv(void *data) +{ + struct cpufreq_frequency_table *tbl = data; + u64 ndiv_val = (u64)tbl->driver_data; + + asm volatile("msr s3_0_c15_c0_4, %0" : : "r" (ndiv_val)); +} + static unsigned int tegra194_get_speed(u32 cpu) { - return tegra194_get_speed_common(cpu, US_DELAY); + struct cpufreq_frequency_table *table, *pos; + struct cpufreq_policy policy; + unsigned int rate; + u64 ndiv; + int err; + + cpufreq_get_policy(&policy, cpu); + table = policy.freq_table; + + /* reconstruct actual cpu freq using counters*/ + rate = tegra194_get_speed_common(cpu, US_DELAY); + + /* get last written ndiv value*/ + err = smp_call_function_single(cpu, get_cpu_ndiv, &ndiv, true); + if (err) { + pr_err("cpufreq: Failed to get ndiv for CPU%d, ret:%d\n", + cpu, err); + return rate; + } + + /* if the reconstructed frequency has acceptable delta from + * the last written value, then return freq corresponding + * to the last written ndiv value from freq_table. This is + * done to return consistent value. + */ + cpufreq_for_each_valid_entry(pos, table) { + if (pos->driver_data != ndiv) + continue; + + if (abs(pos->frequency - rate) > 115200) { + pr_warn("cpufreq: high delta (%d) on CPU%d\n", + abs(pos->frequency - rate), cpu); + pr_warn("cpufreq: cur:%u, set:%u, set ndiv:%llu\n", + rate, pos->frequency, ndiv); + } else { + rate = pos->frequency; + } + break; + } + return rate; } static int tegra194_cpufreq_init(struct cpufreq_policy *policy) @@ -209,14 +265,6 @@ static int tegra194_cpufreq_init(struct cpufreq_policy *policy) return 0; } -static void set_cpu_ndiv(void *data) -{ - struct cpufreq_frequency_table *tbl = data; - u64 ndiv_val = (u64)tbl->driver_data; - - asm volatile("msr s3_0_c15_c0_4, %0" : : "r" (ndiv_val)); -} - static int tegra194_cpufreq_set_target(struct cpufreq_policy *policy, unsigned int index) { From patchwork Wed Sep 16 17:11:17 2020 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Sumit Gupta X-Patchwork-Id: 11780167 Return-Path: Received: from mail.kernel.org (pdx-korg-mail-1.web.codeaurora.org [172.30.200.123]) by pdx-korg-patchwork-2.web.codeaurora.org (Postfix) with ESMTP id 6F4CD746 for ; Wed, 16 Sep 2020 17:11:55 +0000 (UTC) Received: from merlin.infradead.org (merlin.infradead.org [205.233.59.134]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-GCM-SHA384 (256/256 bits)) (No client certificate requested) by mail.kernel.org (Postfix) with ESMTPS id 3DB9A20809 for ; Wed, 16 Sep 2020 17:11:55 +0000 (UTC) Authentication-Results: mail.kernel.org; dkim=pass (2048-bit key) header.d=lists.infradead.org header.i=@lists.infradead.org header.b="XKGN2H6k"; dkim=fail reason="signature verification failed" (2048-bit key) header.d=nvidia.com header.i=@nvidia.com header.b="Q4hACVQO" DMARC-Filter: OpenDMARC Filter v1.3.2 mail.kernel.org 3DB9A20809 Authentication-Results: mail.kernel.org; dmarc=fail (p=none dis=none) header.from=nvidia.com Authentication-Results: mail.kernel.org; spf=none smtp.mailfrom=linux-arm-kernel-bounces+patchwork-linux-arm=patchwork.kernel.org@lists.infradead.org DKIM-Signature: v=1; a=rsa-sha256; q=dns/txt; c=relaxed/relaxed; d=lists.infradead.org; s=merlin.20170209; h=Sender:Content-Transfer-Encoding: Content-Type:Cc:List-Subscribe:List-Help:List-Post:List-Archive: List-Unsubscribe:List-Id:MIME-Version:References:In-Reply-To:Message-ID:Date: Subject:To:From:Reply-To:Content-ID:Content-Description:Resent-Date: Resent-From:Resent-Sender:Resent-To:Resent-Cc:Resent-Message-ID:List-Owner; bh=cg91534F1qXsfDnvYFYdqdXVmPv5VtBPdpHHqPWaQVg=; b=XKGN2H6kz4WZdOYiOspUBjLZW jfeH5UVJkEYK9HnMnE6fZbSxx1DGncIE0w9s4EXtYcSL+C1Q36yR4gj5JXm/UxoXTADUMwGYawzmI FaHTyx/cE2C/NWCQe4MHV7RQKS8xd619+2E4WduUWKNyv69CQhrJXFMHtluEDz14PDdZOX9BZleiD khFLXiL+5Ov47F//Fvaa9p2ta8QpHRE5uUEbaJQiOzaL6vJL3DGZ4k4Z1QMIq2KVnZbFt1MrU1YKp g+tyML1+2yNYEjzQAKa/XFflfjheunpeH0xZZVXVV5Sx49HGtfypV5OHeivrBSonGY/IsxKBBbcFJ HTxQhJ8IA==; Received: from localhost ([::1] helo=merlin.infradead.org) by merlin.infradead.org with esmtp (Exim 4.92.3 #3 (Red Hat Linux)) id 1kIay8-0005Xk-9w; Wed, 16 Sep 2020 17:11:44 +0000 Received: from hqnvemgate26.nvidia.com ([216.228.121.65]) by merlin.infradead.org with esmtps (Exim 4.92.3 #3 (Red Hat Linux)) id 1kIaxx-0005Vd-Mq for linux-arm-kernel@lists.infradead.org; Wed, 16 Sep 2020 17:11:36 +0000 Received: from hqpgpgate101.nvidia.com (Not Verified[216.228.121.13]) by hqnvemgate26.nvidia.com (using TLS: TLSv1.2, DES-CBC3-SHA) id ; Wed, 16 Sep 2020 10:11:20 -0700 Received: from hqmail.nvidia.com ([172.20.161.6]) by hqpgpgate101.nvidia.com (PGP Universal service); Wed, 16 Sep 2020 10:11:32 -0700 X-PGP-Universal: processed; by hqpgpgate101.nvidia.com on Wed, 16 Sep 2020 10:11:32 -0700 Received: from HQMAIL101.nvidia.com (172.20.187.10) by HQMAIL107.nvidia.com (172.20.187.13) with Microsoft SMTP Server (TLS) id 15.0.1473.3; Wed, 16 Sep 2020 17:11:32 +0000 Received: from hqnvemgw03.nvidia.com (10.124.88.68) by HQMAIL101.nvidia.com (172.20.187.10) with Microsoft SMTP Server (TLS) id 15.0.1473.3 via Frontend Transport; Wed, 16 Sep 2020 17:11:32 +0000 Received: from sumitg-l4t.nvidia.com (Not Verified[10.24.37.103]) by hqnvemgw03.nvidia.com with Trustwave SEG (v7, 5, 8, 10121) id ; Wed, 16 Sep 2020 10:11:31 -0700 From: Sumit Gupta To: , , , , , , , Subject: [Patch 2/2] cpufreq: tegra194: Fix unlisted boot freq warning Date: Wed, 16 Sep 2020 22:41:17 +0530 Message-ID: <1600276277-7290-3-git-send-email-sumitg@nvidia.com> X-Mailer: git-send-email 2.7.4 In-Reply-To: <1600276277-7290-1-git-send-email-sumitg@nvidia.com> References: <1600276277-7290-1-git-send-email-sumitg@nvidia.com> X-NVConfidentiality: public MIME-Version: 1.0 DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=nvidia.com; s=n1; t=1600276280; bh=ox9oENiyu3gx4p8E6vGXmfBT1FSXs+lmzFKORun6J7Q=; h=X-PGP-Universal:From:To:CC:Subject:Date:Message-ID:X-Mailer: In-Reply-To:References:X-NVConfidentiality:MIME-Version: Content-Type; b=Q4hACVQO1Pz1oFwn+w5viQHnibREBYv6f6WAPoqftNGYN7g0zq+ZrCVKc3HuoAdab YKyrgp7mnQ6xwJAj/j0eT3jg51Mrogr0E8TCmt+lCtWr+rPJoLqmB3mnDcyz2GP/7t IL1urfnIZ/FiJJSGMcm1KUMpJbY7ceXpWCpSQFN2BJtmiqV68NDvfd4LW09wI6DWqv nn1ZDIor6PRotVsNh2XFP9h/eiGrzxSnqs9mtbeb+22LN6uk3I6jBHkcDNmV3BEWPr qjm05qmeNCYapeXOVN2BhmckCmyyNVt2g37qlMxIMtLql7O7DMFcbKsAfip0EpGmsb b1ukze47CaP5Q== X-CRM114-Version: 20100106-BlameMichelson ( TRE 0.8.0 (BSD) ) MR-646709E3 X-CRM114-CacheID: sfid-20200916_131133_861649_9DA4A53A X-CRM114-Status: GOOD ( 23.32 ) X-Spam-Score: -8.2 (--------) X-Spam-Report: SpamAssassin version 3.4.4 on merlin.infradead.org summary: Content analysis details: (-8.2 points) pts rule name description ---- ---------------------- -------------------------------------------------- -5.0 RCVD_IN_DNSWL_HI RBL: Sender listed at https://www.dnswl.org/, high trust [216.228.121.65 listed in list.dnswl.org] 0.0 SPF_HELO_NONE SPF: HELO does not publish an SPF Record -0.0 SPF_PASS SPF: sender matches SPF record -0.1 DKIM_VALID_AU Message has a valid DKIM or DK signature from author's domain -0.1 DKIM_VALID_EF Message has a valid DKIM or DK signature from envelope-from domain -0.1 DKIM_VALID Message has at least one valid DKIM or DK signature 0.1 DKIM_SIGNED Message has a DKIM or DK signature, not necessarily valid -3.0 DKIMWL_WL_HIGH DKIMwl.org - Whitelisted High sender X-BeenThere: linux-arm-kernel@lists.infradead.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Cc: ksitaraman@nvidia.com, sumitg@nvidia.com, bbasu@nvidia.com Sender: "linux-arm-kernel" Errors-To: linux-arm-kernel-bounces+patchwork-linux-arm=patchwork.kernel.org@lists.infradead.org Warning coming during boot because the boot freq set by bootloader gets filtered out due to big freq steps while creating freq_table. Fixing this by setting closest ndiv value from freq_table. Warning: cpufreq: cpufreq_online: CPU0: Running at unlisted freq cpufreq: cpufreq_online: CPU0: Unlisted initial frequency changed Also, added change in init to wait till current frequency becomes equal or near to the previously requested frequency. This is done because it takes some time to restore the previous frequency while turning-on non-boot cores during exit from SC7(Suspend-to-RAM). Signed-off-by: Sumit Gupta Reviewed-by: Jon Hunter Tested-by: Jon Hunter Acked-by: Thierry Reding --- drivers/cpufreq/tegra194-cpufreq.c | 118 ++++++++++++++++++++++++++++++++++--- 1 file changed, 111 insertions(+), 7 deletions(-) diff --git a/drivers/cpufreq/tegra194-cpufreq.c b/drivers/cpufreq/tegra194-cpufreq.c index d5b608d..c3c058a3 100644 --- a/drivers/cpufreq/tegra194-cpufreq.c +++ b/drivers/cpufreq/tegra194-cpufreq.c @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -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 @@ -241,26 +241,130 @@ static unsigned int tegra194_get_speed(u32 cpu) return rate; } +static int +freqtable_find_index_closest_ndiv(struct cpufreq_frequency_table *table, + unsigned int target_ndiv) +{ + struct cpufreq_frequency_table *pos; + unsigned int ndiv; + int idx, best = -1; + + cpufreq_for_each_valid_entry_idx(pos, table, idx) { + ndiv = pos->driver_data; + + if (ndiv == target_ndiv) + return idx; + + if (ndiv < target_ndiv) { + best = idx; + continue; + } + + /* No ndiv found below target_ndiv */ + if (best == -1) + return idx; + + /* Choose the closest ndiv */ + if (target_ndiv - table[best].driver_data > ndiv - target_ndiv) + return idx; + + return best; + } + + return best; +} + +static int +freqtable_set_closest_ndiv(struct cpufreq_frequency_table *freq_table, + int cpu) +{ + u64 ndiv; + int idx, ret; + + if (!cpu_online(cpu)) + return -EINVAL; + + /* get ndiv for the last frequency request from software */ + ret = smp_call_function_single(cpu, get_cpu_ndiv, &ndiv, true); + if (ret) { + pr_err("cpufreq: Failed to get ndiv for CPU%d\n", cpu); + return ret; + } + + /* while creating freq_table during boot, if the ndiv value got + * filtered out due to large freq steps, then find closest ndiv + * from freq_table and set that. + */ + idx = freqtable_find_index_closest_ndiv(freq_table, ndiv); + + if (ndiv != freq_table[idx].driver_data) { + pr_debug("cpufreq: new freq:%d ndiv:%d, old ndiv:%llu\n", + freq_table[idx].frequency, + freq_table[idx].driver_data, ndiv); + + ret = smp_call_function_single(cpu, set_cpu_ndiv, + freq_table + idx, true); + if (ret) { + pr_err("cpufreq: setting ndiv for CPU%d failed\n", + cpu); + return ret; + } + } + + return freq_table[idx].frequency; +} + 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 new_freq, 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); policy->freq_table = data->tables[cl]; - policy->cpuinfo.transition_latency = TEGRA_CPUFREQ_TRANSITION_LATENCY; + policy->cpuinfo.transition_latency = + TEGRA_CPUFREQ_TRANSITION_LATENCY; + + /* Find and set the closest ndiv from freq_table if the boot freq + * already set is filtered out from freq_table or not present. + */ + new_freq = freqtable_set_closest_ndiv(policy->freq_table, policy->cpu); + if (new_freq < 0) { + pr_err("cpufreq: set closest ndiv for CPU%d failed(%d)\n", + policy->cpu, new_freq); + return new_freq; + } + + /* 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 - new_freq) <= 115200, 0, + 100 * USEC_PER_MSEC, false, policy->cpu, + US_DELAY); + if (ret) + pr_warn("cpufreq: time taken to restore freq >100ms\n"); + + pr_debug("cpufreq: cpu%d, curfreq:%d, setfreq:%d\n", policy->cpu, + policy->cur, new_freq); return 0; }