target/linux/qualcommax/patches-6.1/0100-clk-qcom-clk-rcg2-introduce-support-for-multiple-con.patch

   1 From 032be4f49dda786fea9e1501212f6cd09a7ded96 Mon Sep 17 00:00:00 2001
   2 From: Christian Marangi <ansuelsmth@gmail.com>
   3 Date: Thu, 3 Nov 2022 14:49:43 +0100
   4 Subject: [PATCH] clk: qcom: clk-rcg2: introduce support for multiple conf for
   5  same freq
   6
   7 Some RCG frequency can be reached by multiple configuration.
   8
   9 We currently declare multiple configuration for the same frequency but
  10 that is not supported and always the first configuration will be taken.
  11
  12 These multiple configuration are needed as based on the current parent
  13 configuration, it may be needed to use a different configuration to
  14 reach the same frequency.
  15
  16 To handle this introduce 2 new macro, FM and C.
  17
  18 - FM is used to declare an empty freq_tbl with just the frequency and an
  19   array of confs to insert all the config for the provided frequency.
  20
  21 - C is used to declare a fre_conf where src, pre_div, m and n are
  22   provided.
  23
  24 The driver is changed to handle this special freq_tbl and select the
  25 correct config by calculating the final rate and deciding based on the
  26 one that is less different than the requested one.
  27
  28 Tested-by: Robert Marko <robimarko@gmail.com>
  29 Signed-off-by: Christian Marangi <ansuelsmth@gmail.com>
  30 ---
  31  drivers/clk/qcom/clk-rcg.h  | 14 ++++++-
  32  drivers/clk/qcom/clk-rcg2.c | 84 +++++++++++++++++++++++++++++++++----
  33  2 files changed, 88 insertions(+), 10 deletions(-)
  34
  35 --- a/drivers/clk/qcom/clk-rcg.h
  36 +++ b/drivers/clk/qcom/clk-rcg.h
  37 @@ -7,7 +7,17 @@
  38  #include <linux/clk-provider.h>
  39  #include "clk-regmap.h"
  40
  41 -#define F(f, s, h, m, n) { (f), (s), (2 * (h) - 1), (m), (n) }
  42 +#define F(f, s, h, m, n) { (f), (s), (2 * (h) - 1), (m), (n), 0, NULL }
  43 +
  44 +#define FM(_f, _confs) { .freq = (_f), .confs_num = ARRAY_SIZE(_confs), .confs = (_confs) }
  45 +#define C(s, h, m, n) { (s), (2 * (h) - 1), (m), (n) }
  46 +
  47 +struct freq_conf {
  48 +       u8 src;
  49 +       u8 pre_div;
  50 +       u16 m;
  51 +       u16 n;
  52 +};
  53
  54  struct freq_tbl {
  55         unsigned long freq;
  56 @@ -15,6 +25,8 @@ struct freq_tbl {
  57         u8 pre_div;
  58         u16 m;
  59         u16 n;
  60 +       int confs_num;
  61 +       const struct freq_conf *confs;
  62  };
  63
  64  /**
  65 --- a/drivers/clk/qcom/clk-rcg2.c
  66 +++ b/drivers/clk/qcom/clk-rcg2.c
  67 @@ -203,11 +203,60 @@ clk_rcg2_recalc_rate(struct clk_hw *hw,
  68         return __clk_rcg2_recalc_rate(hw, parent_rate, cfg);
  69  }
  70
  71 +static void
  72 +clk_rcg2_select_conf(struct clk_hw *hw, struct freq_tbl *f_tbl,
  73 +                    const struct freq_tbl *f, unsigned long req_rate)
  74 +{
  75 +       unsigned long best_rate = 0, parent_rate, rate;
  76 +       const struct freq_conf *conf, *best_conf;
  77 +       struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  78 +       struct clk_hw *p;
  79 +       int index, i;
  80 +
  81 +       /* Search in each provided config the one that is near the wanted rate */
  82 +       for (i = 0, conf = f->confs; i < f->confs_num; i++, conf++) {
  83 +               index = qcom_find_src_index(hw, rcg->parent_map, conf->src);
  84 +               if (index < 0)
  85 +                       continue;
  86 +
  87 +               p = clk_hw_get_parent_by_index(hw, index);
  88 +               if (!p)
  89 +                       continue;
  90 +
  91 +               parent_rate =  clk_hw_get_rate(p);
  92 +               rate = calc_rate(parent_rate, conf->n, conf->m, conf->n, conf->pre_div);
  93 +
  94 +               if (rate == req_rate) {
  95 +                       best_conf = conf;
  96 +                       break;
  97 +               }
  98 +
  99 +               if (abs(req_rate - rate) < abs(best_rate - rate)) {
 100 +                       best_rate = rate;
 101 +                       best_conf = conf;
 102 +               }
 103 +       }
 104 +
 105 +       /*
 106 +        * Very unlikely.
 107 +        * Force the first conf if we can't find a correct config.
 108 +        */
 109 +       if (unlikely(i == f->confs_num))
 110 +               best_conf = f->confs;
 111 +
 112 +       /* Apply the config */
 113 +       f_tbl->src = best_conf->src;
 114 +       f_tbl->pre_div = best_conf->pre_div;
 115 +       f_tbl->m = best_conf->m;
 116 +       f_tbl->n = best_conf->n;
 117 +}
 118 +
 119  static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f,
 120                                     struct clk_rate_request *req,
 121                                     enum freq_policy policy)
 122  {
 123         unsigned long clk_flags, rate = req->rate;
 124 +       struct freq_tbl f_tbl;
 125         struct clk_hw *p;
 126         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 127         int index;
 128 @@ -226,7 +275,15 @@ static int _freq_tbl_determine_rate(stru
 129         if (!f)
 130                 return -EINVAL;
 131
 132 -       index = qcom_find_src_index(hw, rcg->parent_map, f->src);
 133 +       f_tbl = *f;
 134 +       /*
 135 +        * A single freq may be reached by multiple configuration.
 136 +        * Try to find the bast one if we have this kind of freq_table.
 137 +        */
 138 +       if (f->confs)
 139 +               clk_rcg2_select_conf(hw, &f_tbl, f, rate);
 140 +
 141 +       index = qcom_find_src_index(hw, rcg->parent_map, f_tbl.src);
 142         if (index < 0)
 143                 return index;
 144
 145 @@ -236,18 +293,18 @@ static int _freq_tbl_determine_rate(stru
 146                 return -EINVAL;
 147
 148         if (clk_flags & CLK_SET_RATE_PARENT) {
 149 -               rate = f->freq;
 150 -               if (f->pre_div) {
 151 +               rate = f_tbl.freq;
 152 +               if (f_tbl.pre_div) {
 153                         if (!rate)
 154                                 rate = req->rate;
 155                         rate /= 2;
 156 -                       rate *= f->pre_div + 1;
 157 +                       rate *= f_tbl.pre_div + 1;
 158                 }
 159
 160 -               if (f->n) {
 161 +               if (f_tbl.n) {
 162                         u64 tmp = rate;
 163 -                       tmp = tmp * f->n;
 164 -                       do_div(tmp, f->m);
 165 +                       tmp = tmp * f_tbl.n;
 166 +                       do_div(tmp, f_tbl.m);
 167                         rate = tmp;
 168                 }
 169         } else {
 170 @@ -255,7 +312,7 @@ static int _freq_tbl_determine_rate(stru
 171         }
 172         req->best_parent_hw = p;
 173         req->best_parent_rate = rate;
 174 -       req->rate = f->freq;
 175 +       req->rate = f_tbl.freq;
 176
 177         return 0;
 178  }
 179 @@ -351,6 +408,7 @@ static int __clk_rcg2_set_rate(struct cl
 180  {
 181         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 182         const struct freq_tbl *f;
 183 +       struct freq_tbl f_tbl;
 184
 185         switch (policy) {
 186         case FLOOR:
 187 @@ -366,7 +424,15 @@ static int __clk_rcg2_set_rate(struct cl
 188         if (!f)
 189                 return -EINVAL;
 190
 191 -       return clk_rcg2_configure(rcg, f);
 192 +       f_tbl = *f;
 193 +       /*
 194 +        * A single freq may be reached by multiple configuration.
 195 +        * Try to find the best one if we have this kind of freq_table.
 196 +        */
 197 +       if (f->confs)
 198 +               clk_rcg2_select_conf(hw, &f_tbl, f, rate);
 199 +
 200 +       return clk_rcg2_configure(rcg, &f_tbl);
 201  }
 202
 203  static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,