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