diff options
Diffstat (limited to 'drivers/clk/zynqmp/divider.c')
| -rw-r--r-- | drivers/clk/zynqmp/divider.c | 118 |
1 files changed, 113 insertions, 5 deletions
diff --git a/drivers/clk/zynqmp/divider.c b/drivers/clk/zynqmp/divider.c index d8f5b70d2709..4be2cc76aa2e 100644 --- a/drivers/clk/zynqmp/divider.c +++ b/drivers/clk/zynqmp/divider.c @@ -2,7 +2,7 @@ /* * Zynq UltraScale+ MPSoC Divider support * - * Copyright (C) 2016-2018 Xilinx + * Copyright (C) 2016-2019 Xilinx * * Adjustable divider clock implementation */ @@ -41,12 +41,30 @@ struct zynqmp_clk_divider { bool is_frac; u32 clk_id; u32 div_type; + u16 max_div; }; static inline int zynqmp_divider_get_val(unsigned long parent_rate, - unsigned long rate) + unsigned long rate, u16 flags) { - return DIV_ROUND_CLOSEST(parent_rate, rate); + int up, down; + unsigned long up_rate, down_rate; + + if (flags & CLK_DIVIDER_POWER_OF_TWO) { + up = DIV_ROUND_UP_ULL((u64)parent_rate, rate); + down = DIV_ROUND_DOWN_ULL((u64)parent_rate, rate); + + up = __roundup_pow_of_two(up); + down = __rounddown_pow_of_two(down); + + up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up); + down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down); + + return (rate - up_rate) <= (down_rate - rate) ? up : down; + + } else { + return DIV_ROUND_CLOSEST(parent_rate, rate); + } } /** @@ -78,6 +96,9 @@ static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw, else value = div >> 16; + if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) + value = 1 << value; + if (!value) { WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO), "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n", @@ -88,6 +109,42 @@ static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw, return DIV_ROUND_UP_ULL(parent_rate, value); } +static void zynqmp_get_divider2_val(struct clk_hw *hw, + unsigned long rate, + unsigned long parent_rate, + struct zynqmp_clk_divider *divider, + int *bestdiv) +{ + int div1; + int div2; + long error = LONG_MAX; + struct clk_hw *parent_hw = clk_hw_get_parent(hw); + struct zynqmp_clk_divider *pdivider = to_zynqmp_clk_divider(parent_hw); + + if (!pdivider) + return; + + *bestdiv = 1; + for (div1 = 1; div1 <= pdivider->max_div;) { + for (div2 = 1; div2 <= divider->max_div;) { + long new_error = ((parent_rate / div1) / div2) - rate; + + if (abs(new_error) < abs(error)) { + *bestdiv = div2; + error = new_error; + } + if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) + div2 = div2 << 1; + else + div2++; + } + if (pdivider->flags & CLK_DIVIDER_POWER_OF_TWO) + div1 = div1 << 1; + else + div1++; + } +} + /** * zynqmp_clk_divider_round_rate() - Round rate of divider clock * @hw: handle between common and hardware-specific interfaces @@ -120,10 +177,23 @@ static long zynqmp_clk_divider_round_rate(struct clk_hw *hw, else bestdiv = bestdiv >> 16; + if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) + bestdiv = 1 << bestdiv; + return DIV_ROUND_UP_ULL((u64)*prate, bestdiv); } - bestdiv = zynqmp_divider_get_val(*prate, rate); + bestdiv = zynqmp_divider_get_val(*prate, rate, divider->flags); + + /* + * In case of two divisors, compute best divider values and return + * divider2 value based on compute value. div1 will be automatically + * set to optimum based on required total divider value. + */ + if (div_type == TYPE_DIV2 && + (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { + zynqmp_get_divider2_val(hw, rate, *prate, divider, &bestdiv); + } if ((clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && divider->is_frac) bestdiv = rate % *prate ? 1 : bestdiv; @@ -151,7 +221,7 @@ static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate, int ret; const struct zynqmp_eemi_ops *eemi_ops = zynqmp_pm_get_eemi_ops(); - value = zynqmp_divider_get_val(parent_rate, rate); + value = zynqmp_divider_get_val(parent_rate, rate, divider->flags); if (div_type == TYPE_DIV1) { div = value & 0xFFFF; div |= 0xffff << 16; @@ -160,6 +230,9 @@ static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate, div |= value << 16; } + if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) + div = __ffs(div); + ret = eemi_ops->clock_setdivider(clk_id, div); if (ret) @@ -176,6 +249,35 @@ static const struct clk_ops zynqmp_clk_divider_ops = { }; /** + * zynqmp_clk_get_max_divisor() - Get maximum supported divisor from firmware. + * @clk_id: Id of clock + * @type: Divider type + * + * Return: Maximum divisor of a clock if query data is successful + * U16_MAX in case of query data is not success + */ +u32 zynqmp_clk_get_max_divisor(u32 clk_id, u32 type) +{ + const struct zynqmp_eemi_ops *eemi_ops = zynqmp_pm_get_eemi_ops(); + struct zynqmp_pm_query_data qdata = {0}; + u32 ret_payload[PAYLOAD_ARG_CNT]; + int ret; + + qdata.qid = PM_QID_CLOCK_GET_MAX_DIVISOR; + qdata.arg1 = clk_id; + qdata.arg2 = type; + ret = eemi_ops->query_data(qdata, ret_payload); + /* + * To maintain backward compatibility return maximum possible value + * (0xFFFF) if query for max divisor is not successful. + */ + if (ret) + return U16_MAX; + + return ret_payload[1]; +} + +/** * zynqmp_clk_register_divider() - Register a divider clock * @name: Name of this clock * @clk_id: Id of clock @@ -215,6 +317,12 @@ struct clk_hw *zynqmp_clk_register_divider(const char *name, div->clk_id = clk_id; div->div_type = nodes->type; + /* + * To achieve best possible rate, maximum limit of divider is required + * while computation. + */ + div->max_div = zynqmp_clk_get_max_divisor(clk_id, nodes->type); + hw = &div->hw; ret = clk_hw_register(NULL, hw); if (ret) { |