clk/qcom/clk-regmap-mux-div.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (c) 2017, Linaro Limited
0004  * Author: Georgi Djakov <georgi.djakov@linaro.org>
0005  */
0006
0007 #include <linux/bitops.h>
0008 #include <linux/delay.h>
0009 #include <linux/kernel.h>
0010 #include <linux/regmap.h>
0011
0012 #include "clk-regmap-mux-div.h"
0013
0014 #define CMD_RCGR            0x0
0015 #define CMD_RCGR_UPDATE         BIT(0)
0016 #define CMD_RCGR_DIRTY_CFG      BIT(4)
0017 #define CMD_RCGR_ROOT_OFF       BIT(31)
0018 #define CFG_RCGR            0x4
0019
0020 #define to_clk_regmap_mux_div(_hw) \
0021     container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr)
0022
0023 int mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div)
0024 {
0025     int ret, count;
0026     u32 val, mask;
0027     const char *name = clk_hw_get_name(&md->clkr.hw);
0028
0029     val = (div << md->hid_shift) | (src << md->src_shift);
0030     mask = ((BIT(md->hid_width) - 1) << md->hid_shift) |
0031            ((BIT(md->src_width) - 1) << md->src_shift);
0032
0033     ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset,
0034                  mask, val);
0035     if (ret)
0036         return ret;
0037
0038     ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset,
0039                  CMD_RCGR_UPDATE, CMD_RCGR_UPDATE);
0040     if (ret)
0041         return ret;
0042
0043     /* Wait for update to take effect */
0044     for (count = 500; count > 0; count--) {
0045         ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset,
0046                   &val);
0047         if (ret)
0048             return ret;
0049         if (!(val & CMD_RCGR_UPDATE))
0050             return 0;
0051         udelay(1);
0052     }
0053
0054     pr_err("%s: RCG did not update its configuration", name);
0055     return -EBUSY;
0056 }
0057 EXPORT_SYMBOL_GPL(mux_div_set_src_div);
0058
0059 static void mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src,
0060                 u32 *div)
0061 {
0062     u32 val, d, s;
0063     const char *name = clk_hw_get_name(&md->clkr.hw);
0064
0065     regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val);
0066
0067     if (val & CMD_RCGR_DIRTY_CFG) {
0068         pr_err("%s: RCG configuration is pending\n", name);
0069         return;
0070     }
0071
0072     regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val);
0073     s = (val >> md->src_shift);
0074     s &= BIT(md->src_width) - 1;
0075     *src = s;
0076
0077     d = (val >> md->hid_shift);
0078     d &= BIT(md->hid_width) - 1;
0079     *div = d;
0080 }
0081
0082 static inline bool is_better_rate(unsigned long req, unsigned long best,
0083                   unsigned long new)
0084 {
0085     return (req <= new && new < best) || (best < req && best < new);
0086 }
0087
0088 static int mux_div_determine_rate(struct clk_hw *hw,
0089                   struct clk_rate_request *req)
0090 {
0091     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0092     unsigned int i, div, max_div;
0093     unsigned long actual_rate, best_rate = 0;
0094     unsigned long req_rate = req->rate;
0095
0096     for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
0097         struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
0098         unsigned long parent_rate = clk_hw_get_rate(parent);
0099
0100         max_div = BIT(md->hid_width) - 1;
0101         for (div = 1; div < max_div; div++) {
0102             parent_rate = mult_frac(req_rate, div, 2);
0103             parent_rate = clk_hw_round_rate(parent, parent_rate);
0104             actual_rate = mult_frac(parent_rate, 2, div);
0105
0106             if (is_better_rate(req_rate, best_rate, actual_rate)) {
0107                 best_rate = actual_rate;
0108                 req->rate = best_rate;
0109                 req->best_parent_rate = parent_rate;
0110                 req->best_parent_hw = parent;
0111             }
0112
0113             if (actual_rate < req_rate || best_rate <= req_rate)
0114                 break;
0115         }
0116     }
0117
0118     if (!best_rate)
0119         return -EINVAL;
0120
0121     return 0;
0122 }
0123
0124 static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
0125                      unsigned long prate, u32 src)
0126 {
0127     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0128     int ret;
0129     u32 div, max_div, best_src = 0, best_div = 0;
0130     unsigned int i;
0131     unsigned long actual_rate, best_rate = 0;
0132
0133     for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
0134         struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
0135         unsigned long parent_rate = clk_hw_get_rate(parent);
0136
0137         max_div = BIT(md->hid_width) - 1;
0138         for (div = 1; div < max_div; div++) {
0139             parent_rate = mult_frac(rate, div, 2);
0140             parent_rate = clk_hw_round_rate(parent, parent_rate);
0141             actual_rate = mult_frac(parent_rate, 2, div);
0142
0143             if (is_better_rate(rate, best_rate, actual_rate)) {
0144                 best_rate = actual_rate;
0145                 best_src = md->parent_map[i];
0146                 best_div = div - 1;
0147             }
0148
0149             if (actual_rate < rate || best_rate <= rate)
0150                 break;
0151         }
0152     }
0153
0154     ret = mux_div_set_src_div(md, best_src, best_div);
0155     if (!ret) {
0156         md->div = best_div;
0157         md->src = best_src;
0158     }
0159
0160     return ret;
0161 }
0162
0163 static u8 mux_div_get_parent(struct clk_hw *hw)
0164 {
0165     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0166     const char *name = clk_hw_get_name(hw);
0167     u32 i, div, src = 0;
0168
0169     mux_div_get_src_div(md, &src, &div);
0170
0171     for (i = 0; i < clk_hw_get_num_parents(hw); i++)
0172         if (src == md->parent_map[i])
0173             return i;
0174
0175     pr_err("%s: Can't find parent with src %d\n", name, src);
0176     return 0;
0177 }
0178
0179 static int mux_div_set_parent(struct clk_hw *hw, u8 index)
0180 {
0181     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0182
0183     return mux_div_set_src_div(md, md->parent_map[index], md->div);
0184 }
0185
0186 static int mux_div_set_rate(struct clk_hw *hw,
0187                 unsigned long rate, unsigned long prate)
0188 {
0189     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0190
0191     return __mux_div_set_rate_and_parent(hw, rate, prate, md->src);
0192 }
0193
0194 static int mux_div_set_rate_and_parent(struct clk_hw *hw,  unsigned long rate,
0195                        unsigned long prate, u8 index)
0196 {
0197     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0198
0199     return __mux_div_set_rate_and_parent(hw, rate, prate,
0200                          md->parent_map[index]);
0201 }
0202
0203 static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate)
0204 {
0205     struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
0206     u32 div, src;
0207     int i, num_parents = clk_hw_get_num_parents(hw);
0208     const char *name = clk_hw_get_name(hw);
0209
0210     mux_div_get_src_div(md, &src, &div);
0211     for (i = 0; i < num_parents; i++)
0212         if (src == md->parent_map[i]) {
0213             struct clk_hw *p = clk_hw_get_parent_by_index(hw, i);
0214             unsigned long parent_rate = clk_hw_get_rate(p);
0215
0216             return mult_frac(parent_rate, 2, div + 1);
0217         }
0218
0219     pr_err("%s: Can't find parent %d\n", name, src);
0220     return 0;
0221 }
0222
0223 const struct clk_ops clk_regmap_mux_div_ops = {
0224     .get_parent = mux_div_get_parent,
0225     .set_parent = mux_div_set_parent,
0226     .set_rate = mux_div_set_rate,
0227     .set_rate_and_parent = mux_div_set_rate_and_parent,
0228     .determine_rate = mux_div_determine_rate,
0229     .recalc_rate = mux_div_recalc_rate,
0230 };
0231 EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops);