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

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2017, Linaro Limited
   4  * Author: Georgi Djakov <[email protected]>
   5  */
   6
   7 #include <linux/bitops.h>
   8 #include <linux/delay.h>
   9 #include <linux/kernel.h>
  10 #include <linux/regmap.h>
  11
  12 #include "clk-regmap-mux-div.h"
  13
  14 #define CMD_RCGR                        0x0
  15 #define CMD_RCGR_UPDATE                 BIT(0)
  16 #define CMD_RCGR_DIRTY_CFG              BIT(4)
  17 #define CMD_RCGR_ROOT_OFF               BIT(31)
  18 #define CFG_RCGR                        0x4
  19
  20 #define to_clk_regmap_mux_div(_hw) \
  21         container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr)
  22
  23 int mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div)
  24 {
  25         int ret, count;
  26         u32 val, mask;
  27         const char *name = clk_hw_get_name(&md->clkr.hw);
  28
  29         val = (div << md->hid_shift) | (src << md->src_shift);
  30         mask = ((BIT(md->hid_width) - 1) << md->hid_shift) |
  31                ((BIT(md->src_width) - 1) << md->src_shift);
  32
  33         ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset,
  34                                  mask, val);
  35         if (ret)
  36                 return ret;
  37
  38         ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset,
  39                                  CMD_RCGR_UPDATE, CMD_RCGR_UPDATE);
  40         if (ret)
  41                 return ret;
  42
  43         /* Wait for update to take effect */
  44         for (count = 500; count > 0; count--) {
  45                 ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset,
  46                                   &val);
  47                 if (ret)
  48                         return ret;
  49                 if (!(val & CMD_RCGR_UPDATE))
  50                         return 0;
  51                 udelay(1);
  52         }
  53
  54         pr_err("%s: RCG did not update its configuration", name);
  55         return -EBUSY;
  56 }
  57 EXPORT_SYMBOL_GPL(mux_div_set_src_div);
  58
  59 static void mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src,
  60                                 u32 *div)
  61 {
  62         u32 val, d, s;
  63         const char *name = clk_hw_get_name(&md->clkr.hw);
  64
  65         regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val);
  66
  67         if (val & CMD_RCGR_DIRTY_CFG) {
  68                 pr_err("%s: RCG configuration is pending\n", name);
  69                 return;
  70         }
  71
  72         regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val);
  73         s = (val >> md->src_shift);
  74         s &= BIT(md->src_width) - 1;
  75         *src = s;
  76
  77         d = (val >> md->hid_shift);
  78         d &= BIT(md->hid_width) - 1;
  79         *div = d;
  80 }
  81
  82 static inline bool is_better_rate(unsigned long req, unsigned long best,
  83                                   unsigned long new)
  84 {
  85         return (req <= new && new < best) || (best < req && best < new);
  86 }
  87
  88 static int mux_div_determine_rate(struct clk_hw *hw,
  89                                   struct clk_rate_request *req)
  90 {
  91         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
  92         unsigned int i, div, max_div;
  93         unsigned long actual_rate, best_rate = 0;
  94         unsigned long req_rate = req->rate;
  95
  96         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
  97                 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
  98                 unsigned long parent_rate = clk_hw_get_rate(parent);
  99
 100                 max_div = BIT(md->hid_width) - 1;
 101                 for (div = 1; div < max_div; div++) {
 102                         parent_rate = mult_frac(req_rate, div, 2);
 103                         parent_rate = clk_hw_round_rate(parent, parent_rate);
 104                         actual_rate = mult_frac(parent_rate, 2, div);
 105
 106                         if (is_better_rate(req_rate, best_rate, actual_rate)) {
 107                                 best_rate = actual_rate;
 108                                 req->rate = best_rate;
 109                                 req->best_parent_rate = parent_rate;
 110                                 req->best_parent_hw = parent;
 111                         }
 112
 113                         if (actual_rate < req_rate || best_rate <= req_rate)
 114                                 break;
 115                 }
 116         }
 117
 118         if (!best_rate)
 119                 return -EINVAL;
 120
 121         return 0;
 122 }
 123
 124 static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
 125                                          unsigned long prate, u32 src)
 126 {
 127         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 128         int ret;
 129         u32 div, max_div, best_src = 0, best_div = 0;
 130         unsigned int i;
 131         unsigned long actual_rate, best_rate = 0;
 132
 133         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 134                 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
 135                 unsigned long parent_rate = clk_hw_get_rate(parent);
 136
 137                 max_div = BIT(md->hid_width) - 1;
 138                 for (div = 1; div < max_div; div++) {
 139                         parent_rate = mult_frac(rate, div, 2);
 140                         parent_rate = clk_hw_round_rate(parent, parent_rate);
 141                         actual_rate = mult_frac(parent_rate, 2, div);
 142
 143                         if (is_better_rate(rate, best_rate, actual_rate)) {
 144                                 best_rate = actual_rate;
 145                                 best_src = md->parent_map[i];
 146                                 best_div = div - 1;
 147                         }
 148
 149                         if (actual_rate < rate || best_rate <= rate)
 150                                 break;
 151                 }
 152         }
 153
 154         ret = mux_div_set_src_div(md, best_src, best_div);
 155         if (!ret) {
 156                 md->div = best_div;
 157                 md->src = best_src;
 158         }
 159
 160         return ret;
 161 }
 162
 163 static u8 mux_div_get_parent(struct clk_hw *hw)
 164 {
 165         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 166         const char *name = clk_hw_get_name(hw);
 167         u32 i, div, src = 0;
 168
 169         mux_div_get_src_div(md, &src, &div);
 170
 171         for (i = 0; i < clk_hw_get_num_parents(hw); i++)
 172                 if (src == md->parent_map[i])
 173                         return i;
 174
 175         pr_err("%s: Can't find parent with src %d\n", name, src);
 176         return 0;
 177 }
 178
 179 static int mux_div_set_parent(struct clk_hw *hw, u8 index)
 180 {
 181         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 182
 183         return mux_div_set_src_div(md, md->parent_map[index], md->div);
 184 }
 185
 186 static int mux_div_set_rate(struct clk_hw *hw,
 187                             unsigned long rate, unsigned long prate)
 188 {
 189         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 190
 191         return __mux_div_set_rate_and_parent(hw, rate, prate, md->src);
 192 }
 193
 194 static int mux_div_set_rate_and_parent(struct clk_hw *hw,  unsigned long rate,
 195                                        unsigned long prate, u8 index)
 196 {
 197         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 198
 199         return __mux_div_set_rate_and_parent(hw, rate, prate,
 200                                              md->parent_map[index]);
 201 }
 202
 203 static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate)
 204 {
 205         struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 206         u32 div, src;
 207         int i, num_parents = clk_hw_get_num_parents(hw);
 208         const char *name = clk_hw_get_name(hw);
 209
 210         mux_div_get_src_div(md, &src, &div);
 211         for (i = 0; i < num_parents; i++)
 212                 if (src == md->parent_map[i]) {
 213                         struct clk_hw *p = clk_hw_get_parent_by_index(hw, i);
 214                         unsigned long parent_rate = clk_hw_get_rate(p);
 215
 216                         return mult_frac(parent_rate, 2, div + 1);
 217                 }
 218
 219         pr_err("%s: Can't find parent %d\n", name, src);
 220         return 0;
 221 }
 222
 223 const struct clk_ops clk_regmap_mux_div_ops = {
 224         .get_parent = mux_div_get_parent,
 225         .set_parent = mux_div_set_parent,
 226         .set_rate = mux_div_set_rate,
 227         .set_rate_and_parent = mux_div_set_rate_and_parent,
 228         .determine_rate = mux_div_determine_rate,
 229         .recalc_rate = mux_div_recalc_rate,
 230 };
 231 EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops);