clk/zynqmp/divider.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Zynq UltraScale+ MPSoC Divider support
0004  *
0005  *  Copyright (C) 2016-2019 Xilinx
0006  *
0007  * Adjustable divider clock implementation
0008  */
0009
0010 #include <linux/clk.h>
0011 #include <linux/clk-provider.h>
0012 #include <linux/slab.h>
0013 #include "clk-zynqmp.h"
0014
0015 /*
0016  * DOC: basic adjustable divider clock that cannot gate
0017  *
0018  * Traits of this clock:
0019  * prepare - clk_prepare only ensures that parents are prepared
0020  * enable - clk_enable only ensures that parents are enabled
0021  * rate - rate is adjustable.  clk->rate = ceiling(parent->rate / divisor)
0022  * parent - fixed parent.  No clk_set_parent support
0023  */
0024
0025 #define to_zynqmp_clk_divider(_hw)      \
0026     container_of(_hw, struct zynqmp_clk_divider, hw)
0027
0028 #define CLK_FRAC        BIT(13) /* has a fractional parent */
0029 #define CUSTOM_FLAG_CLK_FRAC    BIT(0) /* has a fractional parent in custom type flag */
0030
0031 /**
0032  * struct zynqmp_clk_divider - adjustable divider clock
0033  * @hw:     handle between common and hardware-specific interfaces
0034  * @flags:  Hardware specific flags
0035  * @is_frac:    The divider is a fractional divider
0036  * @clk_id: Id of clock
0037  * @div_type:   divisor type (TYPE_DIV1 or TYPE_DIV2)
0038  * @max_div:    maximum supported divisor (fetched from firmware)
0039  */
0040 struct zynqmp_clk_divider {
0041     struct clk_hw hw;
0042     u8 flags;
0043     bool is_frac;
0044     u32 clk_id;
0045     u32 div_type;
0046     u16 max_div;
0047 };
0048
0049 static inline int zynqmp_divider_get_val(unsigned long parent_rate,
0050                      unsigned long rate, u16 flags)
0051 {
0052     int up, down;
0053     unsigned long up_rate, down_rate;
0054
0055     if (flags & CLK_DIVIDER_POWER_OF_TWO) {
0056         up = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
0057         down = DIV_ROUND_DOWN_ULL((u64)parent_rate, rate);
0058
0059         up = __roundup_pow_of_two(up);
0060         down = __rounddown_pow_of_two(down);
0061
0062         up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up);
0063         down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down);
0064
0065         return (rate - up_rate) <= (down_rate - rate) ? up : down;
0066
0067     } else {
0068         return DIV_ROUND_CLOSEST(parent_rate, rate);
0069     }
0070 }
0071
0072 /**
0073  * zynqmp_clk_divider_recalc_rate() - Recalc rate of divider clock
0074  * @hw:         handle between common and hardware-specific interfaces
0075  * @parent_rate:    rate of parent clock
0076  *
0077  * Return: 0 on success else error+reason
0078  */
0079 static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw,
0080                             unsigned long parent_rate)
0081 {
0082     struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw);
0083     const char *clk_name = clk_hw_get_name(hw);
0084     u32 clk_id = divider->clk_id;
0085     u32 div_type = divider->div_type;
0086     u32 div, value;
0087     int ret;
0088
0089     ret = zynqmp_pm_clock_getdivider(clk_id, &div);
0090
0091     if (ret)
0092         pr_debug("%s() get divider failed for %s, ret = %d\n",
0093              __func__, clk_name, ret);
0094
0095     if (div_type == TYPE_DIV1)
0096         value = div & 0xFFFF;
0097     else
0098         value = div >> 16;
0099
0100     if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
0101         value = 1 << value;
0102
0103     if (!value) {
0104         WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO),
0105              "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
0106              clk_name);
0107         return parent_rate;
0108     }
0109
0110     return DIV_ROUND_UP_ULL(parent_rate, value);
0111 }
0112
0113 static void zynqmp_get_divider2_val(struct clk_hw *hw,
0114                     unsigned long rate,
0115                     struct zynqmp_clk_divider *divider,
0116                     int *bestdiv)
0117 {
0118     int div1;
0119     int div2;
0120     long error = LONG_MAX;
0121     unsigned long div1_prate;
0122     struct clk_hw *div1_parent_hw;
0123     struct clk_hw *div2_parent_hw = clk_hw_get_parent(hw);
0124     struct zynqmp_clk_divider *pdivider =
0125                 to_zynqmp_clk_divider(div2_parent_hw);
0126
0127     if (!pdivider)
0128         return;
0129
0130     div1_parent_hw = clk_hw_get_parent(div2_parent_hw);
0131     if (!div1_parent_hw)
0132         return;
0133
0134     div1_prate = clk_hw_get_rate(div1_parent_hw);
0135     *bestdiv = 1;
0136     for (div1 = 1; div1 <= pdivider->max_div;) {
0137         for (div2 = 1; div2 <= divider->max_div;) {
0138             long new_error = ((div1_prate / div1) / div2) - rate;
0139
0140             if (abs(new_error) < abs(error)) {
0141                 *bestdiv = div2;
0142                 error = new_error;
0143             }
0144             if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
0145                 div2 = div2 << 1;
0146             else
0147                 div2++;
0148         }
0149         if (pdivider->flags & CLK_DIVIDER_POWER_OF_TWO)
0150             div1 = div1 << 1;
0151         else
0152             div1++;
0153     }
0154 }
0155
0156 /**
0157  * zynqmp_clk_divider_round_rate() - Round rate of divider clock
0158  * @hw:         handle between common and hardware-specific interfaces
0159  * @rate:       rate of clock to be set
0160  * @prate:      rate of parent clock
0161  *
0162  * Return: 0 on success else error+reason
0163  */
0164 static long zynqmp_clk_divider_round_rate(struct clk_hw *hw,
0165                       unsigned long rate,
0166                       unsigned long *prate)
0167 {
0168     struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw);
0169     const char *clk_name = clk_hw_get_name(hw);
0170     u32 clk_id = divider->clk_id;
0171     u32 div_type = divider->div_type;
0172     u32 bestdiv;
0173     int ret;
0174
0175     /* if read only, just return current value */
0176     if (divider->flags & CLK_DIVIDER_READ_ONLY) {
0177         ret = zynqmp_pm_clock_getdivider(clk_id, &bestdiv);
0178
0179         if (ret)
0180             pr_debug("%s() get divider failed for %s, ret = %d\n",
0181                  __func__, clk_name, ret);
0182         if (div_type == TYPE_DIV1)
0183             bestdiv = bestdiv & 0xFFFF;
0184         else
0185             bestdiv  = bestdiv >> 16;
0186
0187         if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
0188             bestdiv = 1 << bestdiv;
0189
0190         return DIV_ROUND_UP_ULL((u64)*prate, bestdiv);
0191     }
0192
0193     bestdiv = zynqmp_divider_get_val(*prate, rate, divider->flags);
0194
0195     /*
0196      * In case of two divisors, compute best divider values and return
0197      * divider2 value based on compute value. div1 will  be automatically
0198      * set to optimum based on required total divider value.
0199      */
0200     if (div_type == TYPE_DIV2 &&
0201         (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
0202         zynqmp_get_divider2_val(hw, rate, divider, &bestdiv);
0203     }
0204
0205     if ((clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && divider->is_frac)
0206         bestdiv = rate % *prate ? 1 : bestdiv;
0207
0208     bestdiv = min_t(u32, bestdiv, divider->max_div);
0209     *prate = rate * bestdiv;
0210
0211     return rate;
0212 }
0213
0214 /**
0215  * zynqmp_clk_divider_set_rate() - Set rate of divider clock
0216  * @hw:         handle between common and hardware-specific interfaces
0217  * @rate:       rate of clock to be set
0218  * @parent_rate:    rate of parent clock
0219  *
0220  * Return: 0 on success else error+reason
0221  */
0222 static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
0223                        unsigned long parent_rate)
0224 {
0225     struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw);
0226     const char *clk_name = clk_hw_get_name(hw);
0227     u32 clk_id = divider->clk_id;
0228     u32 div_type = divider->div_type;
0229     u32 value, div;
0230     int ret;
0231
0232     value = zynqmp_divider_get_val(parent_rate, rate, divider->flags);
0233     if (div_type == TYPE_DIV1) {
0234         div = value & 0xFFFF;
0235         div |= 0xffff << 16;
0236     } else {
0237         div = 0xffff;
0238         div |= value << 16;
0239     }
0240
0241     if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
0242         div = __ffs(div);
0243
0244     ret = zynqmp_pm_clock_setdivider(clk_id, div);
0245
0246     if (ret)
0247         pr_debug("%s() set divider failed for %s, ret = %d\n",
0248              __func__, clk_name, ret);
0249
0250     return ret;
0251 }
0252
0253 static const struct clk_ops zynqmp_clk_divider_ops = {
0254     .recalc_rate = zynqmp_clk_divider_recalc_rate,
0255     .round_rate = zynqmp_clk_divider_round_rate,
0256     .set_rate = zynqmp_clk_divider_set_rate,
0257 };
0258
0259 static const struct clk_ops zynqmp_clk_divider_ro_ops = {
0260     .recalc_rate = zynqmp_clk_divider_recalc_rate,
0261     .round_rate = zynqmp_clk_divider_round_rate,
0262 };
0263
0264 /**
0265  * zynqmp_clk_get_max_divisor() - Get maximum supported divisor from firmware.
0266  * @clk_id:     Id of clock
0267  * @type:       Divider type
0268  *
0269  * Return: Maximum divisor of a clock if query data is successful
0270  *     U16_MAX in case of query data is not success
0271  */
0272 static u32 zynqmp_clk_get_max_divisor(u32 clk_id, u32 type)
0273 {
0274     struct zynqmp_pm_query_data qdata = {0};
0275     u32 ret_payload[PAYLOAD_ARG_CNT];
0276     int ret;
0277
0278     qdata.qid = PM_QID_CLOCK_GET_MAX_DIVISOR;
0279     qdata.arg1 = clk_id;
0280     qdata.arg2 = type;
0281     ret = zynqmp_pm_query_data(qdata, ret_payload);
0282     /*
0283      * To maintain backward compatibility return maximum possible value
0284      * (0xFFFF) if query for max divisor is not successful.
0285      */
0286     if (ret)
0287         return U16_MAX;
0288
0289     return ret_payload[1];
0290 }
0291
0292 static inline unsigned long zynqmp_clk_map_divider_ccf_flags(
0293                            const u32 zynqmp_type_flag)
0294 {
0295     unsigned long ccf_flag = 0;
0296
0297     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ONE_BASED)
0298         ccf_flag |= CLK_DIVIDER_ONE_BASED;
0299     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO)
0300         ccf_flag |= CLK_DIVIDER_POWER_OF_TWO;
0301     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ALLOW_ZERO)
0302         ccf_flag |= CLK_DIVIDER_ALLOW_ZERO;
0303     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO)
0304         ccf_flag |= CLK_DIVIDER_HIWORD_MASK;
0305     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ROUND_CLOSEST)
0306         ccf_flag |= CLK_DIVIDER_ROUND_CLOSEST;
0307     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_READ_ONLY)
0308         ccf_flag |= CLK_DIVIDER_READ_ONLY;
0309     if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_MAX_AT_ZERO)
0310         ccf_flag |= CLK_DIVIDER_MAX_AT_ZERO;
0311
0312     return ccf_flag;
0313 }
0314
0315 /**
0316  * zynqmp_clk_register_divider() - Register a divider clock
0317  * @name:       Name of this clock
0318  * @clk_id:     Id of clock
0319  * @parents:        Name of this clock's parents
0320  * @num_parents:    Number of parents
0321  * @nodes:      Clock topology node
0322  *
0323  * Return: clock hardware to registered clock divider
0324  */
0325 struct clk_hw *zynqmp_clk_register_divider(const char *name,
0326                        u32 clk_id,
0327                        const char * const *parents,
0328                        u8 num_parents,
0329                        const struct clock_topology *nodes)
0330 {
0331     struct zynqmp_clk_divider *div;
0332     struct clk_hw *hw;
0333     struct clk_init_data init;
0334     int ret;
0335
0336     /* allocate the divider */
0337     div = kzalloc(sizeof(*div), GFP_KERNEL);
0338     if (!div)
0339         return ERR_PTR(-ENOMEM);
0340
0341     init.name = name;
0342     if (nodes->type_flag & CLK_DIVIDER_READ_ONLY)
0343         init.ops = &zynqmp_clk_divider_ro_ops;
0344     else
0345         init.ops = &zynqmp_clk_divider_ops;
0346
0347     init.flags = zynqmp_clk_map_common_ccf_flags(nodes->flag);
0348
0349     init.parent_names = parents;
0350     init.num_parents = 1;
0351
0352     /* struct clk_divider assignments */
0353     div->is_frac = !!((nodes->flag & CLK_FRAC) |
0354               (nodes->custom_type_flag & CUSTOM_FLAG_CLK_FRAC));
0355     div->flags = zynqmp_clk_map_divider_ccf_flags(nodes->type_flag);
0356     div->hw.init = &init;
0357     div->clk_id = clk_id;
0358     div->div_type = nodes->type;
0359
0360     /*
0361      * To achieve best possible rate, maximum limit of divider is required
0362      * while computation.
0363      */
0364     div->max_div = zynqmp_clk_get_max_divisor(clk_id, nodes->type);
0365
0366     hw = &div->hw;
0367     ret = clk_hw_register(NULL, hw);
0368     if (ret) {
0369         kfree(div);
0370         hw = ERR_PTR(ret);
0371     }
0372
0373     return hw;
0374 }