drivers/clk/actions/owl-factor.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 //
   3 // OWL factor clock driver
   4 //
   5 // Copyright (c) 2014 Actions Semi Inc.
   6 // Author: David Liu <liuwei@actions-semi.com>
   7 //
   8 // Copyright (c) 2018 Linaro Ltd.
   9 // Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
  10
  11 #include <linux/clk-provider.h>
  12 #include <linux/regmap.h>
  13 #include <linux/slab.h>
  14
  15 #include "owl-factor.h"
  16
  17 static unsigned int _get_table_maxval(const struct clk_factor_table *table)
  18 {
  19         unsigned int maxval = 0;
  20         const struct clk_factor_table *clkt;
  21
  22         for (clkt = table; clkt->div; clkt++)
  23                 if (clkt->val > maxval)
  24                         maxval = clkt->val;
  25         return maxval;
  26 }
  27
  28 static int _get_table_div_mul(const struct clk_factor_table *table,
  29                         unsigned int val, unsigned int *mul, unsigned int *div)
  30 {
  31         const struct clk_factor_table *clkt;
  32
  33         for (clkt = table; clkt->div; clkt++) {
  34                 if (clkt->val == val) {
  35                         *mul = clkt->mul;
  36                         *div = clkt->div;
  37                         return 1;
  38                 }
  39         }
  40
  41         return 0;
  42 }
  43
  44 static unsigned int _get_table_val(const struct clk_factor_table *table,
  45                         unsigned long rate, unsigned long parent_rate)
  46 {
  47         const struct clk_factor_table *clkt;
  48         int val = -1;
  49         u64 calc_rate;
  50
  51         for (clkt = table; clkt->div; clkt++) {
  52                 calc_rate = parent_rate * clkt->mul;
  53                 do_div(calc_rate, clkt->div);
  54
  55                 if ((unsigned long)calc_rate <= rate) {
  56                         val = clkt->val;
  57                         break;
  58                 }
  59         }
  60
  61         if (val == -1)
  62                 val = _get_table_maxval(table);
  63
  64         return val;
  65 }
  66
  67 static int clk_val_best(struct clk_hw *hw, unsigned long rate,
  68                         unsigned long *best_parent_rate)
  69 {
  70         struct owl_factor *factor = hw_to_owl_factor(hw);
  71         struct owl_factor_hw *factor_hw = &factor->factor_hw;
  72         const struct clk_factor_table *clkt = factor_hw->table;
  73         unsigned long parent_rate, try_parent_rate, best = 0, cur_rate;
  74         unsigned long parent_rate_saved = *best_parent_rate;
  75         int bestval = 0;
  76
  77         if (!rate)
  78                 rate = 1;
  79
  80         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
  81                 parent_rate = *best_parent_rate;
  82                 bestval = _get_table_val(clkt, rate, parent_rate);
  83                 return bestval;
  84         }
  85
  86         for (clkt = factor_hw->table; clkt->div; clkt++) {
  87                 try_parent_rate = rate * clkt->div / clkt->mul;
  88
  89                 if (try_parent_rate == parent_rate_saved) {
  90                         pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n",
  91                                 __func__, clkt->val, clkt->mul, clkt->div,
  92                                 try_parent_rate);
  93                         /*
  94                          * It's the most ideal case if the requested rate can be
  95                          * divided from parent clock without any need to change
  96                          * parent rate, so return the divider immediately.
  97                          */
  98                         *best_parent_rate = parent_rate_saved;
  99                         return clkt->val;
 100                 }
 101
 102                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
 103                                 try_parent_rate);
 104                 cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul;
 105                 if (cur_rate <= rate && cur_rate > best) {
 106                         bestval = clkt->val;
 107                         best = cur_rate;
 108                         *best_parent_rate = parent_rate;
 109                 }
 110         }
 111
 112         if (!bestval) {
 113                 bestval = _get_table_maxval(clkt);
 114                 *best_parent_rate = clk_hw_round_rate(
 115                                 clk_hw_get_parent(hw), 1);
 116         }
 117
 118         return bestval;
 119 }
 120
 121 long owl_factor_helper_round_rate(struct owl_clk_common *common,
 122                                 const struct owl_factor_hw *factor_hw,
 123                                 unsigned long rate,
 124                                 unsigned long *parent_rate)
 125 {
 126         const struct clk_factor_table *clkt = factor_hw->table;
 127         unsigned int val, mul = 0, div = 1;
 128
 129         val = clk_val_best(&common->hw, rate, parent_rate);
 130         _get_table_div_mul(clkt, val, &mul, &div);
 131
 132         return *parent_rate * mul / div;
 133 }
 134
 135 static long owl_factor_round_rate(struct clk_hw *hw, unsigned long rate,
 136                         unsigned long *parent_rate)
 137 {
 138         struct owl_factor *factor = hw_to_owl_factor(hw);
 139         struct owl_factor_hw *factor_hw = &factor->factor_hw;
 140
 141         return owl_factor_helper_round_rate(&factor->common, factor_hw,
 142                                         rate, parent_rate);
 143 }
 144
 145 unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common,
 146                                          const struct owl_factor_hw *factor_hw,
 147                                          unsigned long parent_rate)
 148 {
 149         const struct clk_factor_table *clkt = factor_hw->table;
 150         unsigned long long int rate;
 151         u32 reg, val, mul, div;
 152
 153         div = 0;
 154         mul = 0;
 155
 156         regmap_read(common->regmap, factor_hw->reg, &reg);
 157
 158         val = reg >> factor_hw->shift;
 159         val &= div_mask(factor_hw);
 160
 161         _get_table_div_mul(clkt, val, &mul, &div);
 162         if (!div) {
 163                 WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO),
 164                         "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
 165                         __clk_get_name(common->hw.clk));
 166                 return parent_rate;
 167         }
 168
 169         rate = (unsigned long long int)parent_rate * mul;
 170         do_div(rate, div);
 171
 172         return rate;
 173 }
 174
 175 static unsigned long owl_factor_recalc_rate(struct clk_hw *hw,
 176                         unsigned long parent_rate)
 177 {
 178         struct owl_factor *factor = hw_to_owl_factor(hw);
 179         struct owl_factor_hw *factor_hw = &factor->factor_hw;
 180         struct owl_clk_common *common = &factor->common;
 181
 182         return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate);
 183 }
 184
 185 int owl_factor_helper_set_rate(const struct owl_clk_common *common,
 186                                 const struct owl_factor_hw *factor_hw,
 187                                 unsigned long rate,
 188                                 unsigned long parent_rate)
 189 {
 190         u32 val, reg;
 191
 192         val = _get_table_val(factor_hw->table, rate, parent_rate);
 193
 194         if (val > div_mask(factor_hw))
 195                 val = div_mask(factor_hw);
 196
 197         regmap_read(common->regmap, factor_hw->reg, &reg);
 198
 199         reg &= ~(div_mask(factor_hw) << factor_hw->shift);
 200         reg |= val << factor_hw->shift;
 201
 202         regmap_write(common->regmap, factor_hw->reg, reg);
 203
 204         return 0;
 205 }
 206
 207 static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate,
 208                                unsigned long parent_rate)
 209 {
 210         struct owl_factor *factor = hw_to_owl_factor(hw);
 211         struct owl_factor_hw *factor_hw = &factor->factor_hw;
 212         struct owl_clk_common *common = &factor->common;
 213
 214         return owl_factor_helper_set_rate(common, factor_hw,
 215                                         rate, parent_rate);
 216 }
 217
 218 const struct clk_ops owl_factor_ops = {
 219         .round_rate     = owl_factor_round_rate,
 220         .recalc_rate    = owl_factor_recalc_rate,
 221         .set_rate       = owl_factor_set_rate,
 222 };