diff --git a/drivers/clk/tegra/Makefile b/drivers/clk/tegra/Makefile index b71692391bd6..c77d072eb333 100644 --- a/drivers/clk/tegra/Makefile +++ b/drivers/clk/tegra/Makefile @@ -24,3 +24,4 @@ obj-$(CONFIG_ARCH_TEGRA_132_SOC) += clk-tegra124.o obj-y += cvb.o obj-$(CONFIG_ARCH_TEGRA_210_SOC) += clk-tegra210.o obj-$(CONFIG_CLK_TEGRA_BPMP) += clk-bpmp.o +obj-y += clk-utils.o diff --git a/drivers/clk/tegra/clk-divider.c b/drivers/clk/tegra/clk-divider.c index 16e0aee14773..8f175ff62f73 100644 --- a/drivers/clk/tegra/clk-divider.c +++ b/drivers/clk/tegra/clk-divider.c @@ -32,35 +32,15 @@ static int get_div(struct tegra_clk_frac_div *divider, unsigned long rate, unsigned long parent_rate) { - u64 divider_ux1 = parent_rate; - u8 flags = divider->flags; - int mul; + int div; - if (!rate) + div = div_frac_get(rate, parent_rate, divider->width, + divider->frac_width, divider->flags); + + if (div < 0) return 0; - mul = get_mul(divider); - - if (!(flags & TEGRA_DIVIDER_INT)) - divider_ux1 *= mul; - - if (flags & TEGRA_DIVIDER_ROUND_UP) - divider_ux1 += rate - 1; - - do_div(divider_ux1, rate); - - if (flags & TEGRA_DIVIDER_INT) - divider_ux1 *= mul; - - divider_ux1 -= mul; - - if ((s64)divider_ux1 < 0) - return 0; - - if (divider_ux1 > get_max_div(divider)) - return get_max_div(divider); - - return divider_ux1; + return div; } static unsigned long clk_frac_div_recalc_rate(struct clk_hw *hw, diff --git a/drivers/clk/tegra/clk-utils.c b/drivers/clk/tegra/clk-utils.c new file mode 100644 index 000000000000..1a5daae4e501 --- /dev/null +++ b/drivers/clk/tegra/clk-utils.c @@ -0,0 +1,43 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2018, NVIDIA CORPORATION. All rights reserved. + */ + +#include + +#include "clk.h" + +#define div_mask(w) ((1 << (w)) - 1) + +int div_frac_get(unsigned long rate, unsigned parent_rate, u8 width, + u8 frac_width, u8 flags) +{ + u64 divider_ux1 = parent_rate; + int mul; + + if (!rate) + return 0; + + mul = 1 << frac_width; + + if (!(flags & TEGRA_DIVIDER_INT)) + divider_ux1 *= mul; + + if (flags & TEGRA_DIVIDER_ROUND_UP) + divider_ux1 += rate - 1; + + do_div(divider_ux1, rate); + + if (flags & TEGRA_DIVIDER_INT) + divider_ux1 *= mul; + + if (divider_ux1 < mul) + return 0; + + divider_ux1 -= mul; + + if (divider_ux1 > div_mask(width)) + return div_mask(width); + + return divider_ux1; +} diff --git a/drivers/clk/tegra/clk.h b/drivers/clk/tegra/clk.h index e3b9c22ad8b2..c733841aa6db 100644 --- a/drivers/clk/tegra/clk.h +++ b/drivers/clk/tegra/clk.h @@ -812,6 +812,9 @@ extern tegra_clk_apply_init_table_func tegra_clk_apply_init_table; int tegra_pll_wait_for_lock(struct tegra_clk_pll *pll); u16 tegra_pll_get_fixed_mdiv(struct clk_hw *hw, unsigned long input_rate); int tegra_pll_p_div_to_hw(struct tegra_clk_pll *pll, u8 p_div); +int div_frac_get(unsigned long rate, unsigned parent_rate, u8 width, + u8 frac_width, u8 flags); + /* Combined read fence with delay */ #define fence_udelay(delay, reg) \