drivers/clk/sunxi-ng/ccu_nm.c

   1 /*
   2  * Copyright (C) 2016 Maxime Ripard
   3  * Maxime Ripard <maxime.ripard@free-electrons.com>
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 2 of
   8  * the License, or (at your option) any later version.
   9  */
  10
  11 #include <linux/clk-provider.h>
  12
  13 #include "ccu_frac.h"
  14 #include "ccu_gate.h"
  15 #include "ccu_nm.h"
  16
  17 struct _ccu_nm {
  18         unsigned long   n, min_n, max_n;
  19         unsigned long   m, min_m, max_m;
  20 };
  21
  22 static unsigned long ccu_nm_calc_rate(unsigned long parent,
  23                                       unsigned long n, unsigned long m)
  24 {
  25         u64 rate = parent;
  26
  27         rate *= n;
  28         do_div(rate, m);
  29
  30         return rate;
  31 }
  32
  33 static void ccu_nm_find_best(unsigned long parent, unsigned long rate,
  34                              struct _ccu_nm *nm)
  35 {
  36         unsigned long best_rate = 0;
  37         unsigned long best_n = 0, best_m = 0;
  38         unsigned long _n, _m;
  39
  40         for (_n = nm->min_n; _n <= nm->max_n; _n++) {
  41                 for (_m = nm->min_m; _m <= nm->max_m; _m++) {
  42                         unsigned long tmp_rate = ccu_nm_calc_rate(parent,
  43                                                                   _n, _m);
  44
  45                         if (tmp_rate > rate)
  46                                 continue;
  47
  48                         if ((rate - tmp_rate) < (rate - best_rate)) {
  49                                 best_rate = tmp_rate;
  50                                 best_n = _n;
  51                                 best_m = _m;
  52                         }
  53                 }
  54         }
  55
  56         nm->n = best_n;
  57         nm->m = best_m;
  58 }
  59
  60 static void ccu_nm_disable(struct clk_hw *hw)
  61 {
  62         struct ccu_nm *nm = hw_to_ccu_nm(hw);
  63
  64         return ccu_gate_helper_disable(&nm->common, nm->enable);
  65 }
  66
  67 static int ccu_nm_enable(struct clk_hw *hw)
  68 {
  69         struct ccu_nm *nm = hw_to_ccu_nm(hw);
  70
  71         return ccu_gate_helper_enable(&nm->common, nm->enable);
  72 }
  73
  74 static int ccu_nm_is_enabled(struct clk_hw *hw)
  75 {
  76         struct ccu_nm *nm = hw_to_ccu_nm(hw);
  77
  78         return ccu_gate_helper_is_enabled(&nm->common, nm->enable);
  79 }
  80
  81 static unsigned long ccu_nm_recalc_rate(struct clk_hw *hw,
  82                                         unsigned long parent_rate)
  83 {
  84         struct ccu_nm *nm = hw_to_ccu_nm(hw);
  85         unsigned long rate;
  86         unsigned long n, m;
  87         u32 reg;
  88
  89         if (ccu_frac_helper_is_enabled(&nm->common, &nm->frac)) {
  90                 rate = ccu_frac_helper_read_rate(&nm->common, &nm->frac);
  91
  92                 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
  93                         rate /= nm->fixed_post_div;
  94
  95                 return rate;
  96         }
  97
  98         reg = readl(nm->common.base + nm->common.reg);
  99
 100         n = reg >> nm->n.shift;
 101         n &= (1 << nm->n.width) - 1;
 102         n += nm->n.offset;
 103         if (!n)
 104                 n++;
 105
 106         m = reg >> nm->m.shift;
 107         m &= (1 << nm->m.width) - 1;
 108         m += nm->m.offset;
 109         if (!m)
 110                 m++;
 111
 112         if (ccu_sdm_helper_is_enabled(&nm->common, &nm->sdm))
 113                 rate = ccu_sdm_helper_read_rate(&nm->common, &nm->sdm, m, n);
 114         else
 115                 rate = ccu_nm_calc_rate(parent_rate, n, m);
 116
 117         if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 118                 rate /= nm->fixed_post_div;
 119
 120         return rate;
 121 }
 122
 123 static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate,
 124                               unsigned long *parent_rate)
 125 {
 126         struct ccu_nm *nm = hw_to_ccu_nm(hw);
 127         struct _ccu_nm _nm;
 128
 129         if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 130                 rate *= nm->fixed_post_div;
 131
 132         if (rate < nm->min_rate) {
 133                 rate = nm->min_rate;
 134                 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 135                         rate /= nm->fixed_post_div;
 136                 return rate;
 137         }
 138
 139         if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) {
 140                 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 141                         rate /= nm->fixed_post_div;
 142                 return rate;
 143         }
 144
 145         if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) {
 146                 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 147                         rate /= nm->fixed_post_div;
 148                 return rate;
 149         }
 150
 151         _nm.min_n = nm->n.min ?: 1;
 152         _nm.max_n = nm->n.max ?: 1 << nm->n.width;
 153         _nm.min_m = 1;
 154         _nm.max_m = nm->m.max ?: 1 << nm->m.width;
 155
 156         ccu_nm_find_best(*parent_rate, rate, &_nm);
 157         rate = ccu_nm_calc_rate(*parent_rate, _nm.n, _nm.m);
 158
 159         if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 160                 rate /= nm->fixed_post_div;
 161
 162         return rate;
 163 }
 164
 165 static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate,
 166                            unsigned long parent_rate)
 167 {
 168         struct ccu_nm *nm = hw_to_ccu_nm(hw);
 169         struct _ccu_nm _nm;
 170         unsigned long flags;
 171         u32 reg;
 172
 173         /* Adjust target rate according to post-dividers */
 174         if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 175                 rate = rate * nm->fixed_post_div;
 176
 177         if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) {
 178                 spin_lock_irqsave(nm->common.lock, flags);
 179
 180                 /* most SoCs require M to be 0 if fractional mode is used */
 181                 reg = readl(nm->common.base + nm->common.reg);
 182                 reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift);
 183                 writel(reg, nm->common.base + nm->common.reg);
 184
 185                 spin_unlock_irqrestore(nm->common.lock, flags);
 186
 187                 ccu_frac_helper_enable(&nm->common, &nm->frac);
 188
 189                 return ccu_frac_helper_set_rate(&nm->common, &nm->frac,
 190                                                 rate, nm->lock);
 191         } else {
 192                 ccu_frac_helper_disable(&nm->common, &nm->frac);
 193         }
 194
 195         _nm.min_n = nm->n.min ?: 1;
 196         _nm.max_n = nm->n.max ?: 1 << nm->n.width;
 197         _nm.min_m = 1;
 198         _nm.max_m = nm->m.max ?: 1 << nm->m.width;
 199
 200         if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) {
 201                 ccu_sdm_helper_enable(&nm->common, &nm->sdm, rate);
 202
 203                 /* Sigma delta modulation requires specific N and M factors */
 204                 ccu_sdm_helper_get_factors(&nm->common, &nm->sdm, rate,
 205                                            &_nm.m, &_nm.n);
 206         } else {
 207                 ccu_sdm_helper_disable(&nm->common, &nm->sdm);
 208                 ccu_nm_find_best(parent_rate, rate, &_nm);
 209         }
 210
 211         spin_lock_irqsave(nm->common.lock, flags);
 212
 213         reg = readl(nm->common.base + nm->common.reg);
 214         reg &= ~GENMASK(nm->n.width + nm->n.shift - 1, nm->n.shift);
 215         reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift);
 216
 217         reg |= (_nm.n - nm->n.offset) << nm->n.shift;
 218         reg |= (_nm.m - nm->m.offset) << nm->m.shift;
 219         writel(reg, nm->common.base + nm->common.reg);
 220
 221         spin_unlock_irqrestore(nm->common.lock, flags);
 222
 223         ccu_helper_wait_for_lock(&nm->common, nm->lock);
 224
 225         return 0;
 226 }
 227
 228 const struct clk_ops ccu_nm_ops = {
 229         .disable        = ccu_nm_disable,
 230         .enable         = ccu_nm_enable,
 231         .is_enabled     = ccu_nm_is_enabled,
 232
 233         .recalc_rate    = ccu_nm_recalc_rate,
 234         .round_rate     = ccu_nm_round_rate,
 235         .set_rate       = ccu_nm_set_rate,
 236 };