drivers/clk/sunxi-ng/ccu_mux.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.h>
  12 #include <linux/clk-provider.h>
  13 #include <linux/delay.h>
  14
  15 #include "ccu_gate.h"
  16 #include "ccu_mux.h"
  17
  18 void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
  19                                              struct ccu_mux_internal *cm,
  20                                              int parent_index,
  21                                              unsigned long *parent_rate)
  22 {
  23         u16 prediv = 1;
  24         u32 reg;
  25         int i;
  26
  27         if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
  28               (common->features & CCU_FEATURE_VARIABLE_PREDIV) ||
  29               (common->features & CCU_FEATURE_ALL_PREDIV)))
  30                 return;
  31
  32         if (common->features & CCU_FEATURE_ALL_PREDIV) {
  33                 *parent_rate = *parent_rate / common->prediv;
  34                 return;
  35         }
  36
  37         reg = readl(common->base + common->reg);
  38         if (parent_index < 0) {
  39                 parent_index = reg >> cm->shift;
  40                 parent_index &= (1 << cm->width) - 1;
  41         }
  42
  43         if (common->features & CCU_FEATURE_FIXED_PREDIV)
  44                 for (i = 0; i < cm->n_predivs; i++)
  45                         if (parent_index == cm->fixed_predivs[i].index)
  46                                 prediv = cm->fixed_predivs[i].div;
  47
  48         if (common->features & CCU_FEATURE_VARIABLE_PREDIV)
  49                 if (parent_index == cm->variable_prediv.index) {
  50                         u8 div;
  51
  52                         div = reg >> cm->variable_prediv.shift;
  53                         div &= (1 << cm->variable_prediv.width) - 1;
  54                         prediv = div + 1;
  55                 }
  56
  57         *parent_rate = *parent_rate / prediv;
  58 }
  59
  60 int ccu_mux_helper_determine_rate(struct ccu_common *common,
  61                                   struct ccu_mux_internal *cm,
  62                                   struct clk_rate_request *req,
  63                                   unsigned long (*round)(struct ccu_mux_internal *,
  64                                                          struct clk_hw *,
  65                                                          unsigned long *,
  66                                                          unsigned long,
  67                                                          void *),
  68                                   void *data)
  69 {
  70         unsigned long best_parent_rate = 0, best_rate = 0;
  71         struct clk_hw *best_parent, *hw = &common->hw;
  72         unsigned int i;
  73
  74         if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
  75                 unsigned long adj_parent_rate;
  76
  77                 best_parent = clk_hw_get_parent(hw);
  78                 best_parent_rate = clk_hw_get_rate(best_parent);
  79
  80                 adj_parent_rate = best_parent_rate;
  81                 ccu_mux_helper_adjust_parent_for_prediv(common, cm, -1,
  82                                                         &adj_parent_rate);
  83
  84                 best_rate = round(cm, best_parent, &adj_parent_rate,
  85                                   req->rate, data);
  86
  87                 goto out;
  88         }
  89
  90         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
  91                 unsigned long tmp_rate, parent_rate, adj_parent_rate;
  92                 struct clk_hw *parent;
  93
  94                 parent = clk_hw_get_parent_by_index(hw, i);
  95                 if (!parent)
  96                         continue;
  97
  98                 if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) {
  99                         struct clk_rate_request parent_req = *req;
 100                         int ret = __clk_determine_rate(parent, &parent_req);
 101
 102                         if (ret)
 103                                 continue;
 104
 105                         parent_rate = parent_req.rate;
 106                 } else {
 107                         parent_rate = clk_hw_get_rate(parent);
 108                 }
 109
 110                 adj_parent_rate = parent_rate;
 111                 ccu_mux_helper_adjust_parent_for_prediv(common, cm, i,
 112                                                         &adj_parent_rate);
 113
 114                 tmp_rate = round(cm, parent, &adj_parent_rate, req->rate, data);
 115                 if (tmp_rate == req->rate) {
 116                         best_parent = parent;
 117                         best_parent_rate = parent_rate;
 118                         best_rate = tmp_rate;
 119                         goto out;
 120                 }
 121
 122                 if ((req->rate - tmp_rate) < (req->rate - best_rate)) {
 123                         best_rate = tmp_rate;
 124                         best_parent_rate = parent_rate;
 125                         best_parent = parent;
 126                 }
 127         }
 128
 129         if (best_rate == 0)
 130                 return -EINVAL;
 131
 132 out:
 133         req->best_parent_hw = best_parent;
 134         req->best_parent_rate = best_parent_rate;
 135         req->rate = best_rate;
 136         return 0;
 137 }
 138
 139 u8 ccu_mux_helper_get_parent(struct ccu_common *common,
 140                              struct ccu_mux_internal *cm)
 141 {
 142         u32 reg;
 143         u8 parent;
 144
 145         reg = readl(common->base + common->reg);
 146         parent = reg >> cm->shift;
 147         parent &= (1 << cm->width) - 1;
 148
 149         if (cm->table) {
 150                 int num_parents = clk_hw_get_num_parents(&common->hw);
 151                 int i;
 152
 153                 for (i = 0; i < num_parents; i++)
 154                         if (cm->table[i] == parent)
 155                                 return i;
 156         }
 157
 158         return parent;
 159 }
 160
 161 int ccu_mux_helper_set_parent(struct ccu_common *common,
 162                               struct ccu_mux_internal *cm,
 163                               u8 index)
 164 {
 165         unsigned long flags;
 166         u32 reg;
 167
 168         if (cm->table)
 169                 index = cm->table[index];
 170
 171         spin_lock_irqsave(common->lock, flags);
 172
 173         reg = readl(common->base + common->reg);
 174         reg &= ~GENMASK(cm->width + cm->shift - 1, cm->shift);
 175         writel(reg | (index << cm->shift), common->base + common->reg);
 176
 177         spin_unlock_irqrestore(common->lock, flags);
 178
 179         return 0;
 180 }
 181
 182 static void ccu_mux_disable(struct clk_hw *hw)
 183 {
 184         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 185
 186         return ccu_gate_helper_disable(&cm->common, cm->enable);
 187 }
 188
 189 static int ccu_mux_enable(struct clk_hw *hw)
 190 {
 191         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 192
 193         return ccu_gate_helper_enable(&cm->common, cm->enable);
 194 }
 195
 196 static int ccu_mux_is_enabled(struct clk_hw *hw)
 197 {
 198         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 199
 200         return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
 201 }
 202
 203 static u8 ccu_mux_get_parent(struct clk_hw *hw)
 204 {
 205         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 206
 207         return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
 208 }
 209
 210 static int ccu_mux_set_parent(struct clk_hw *hw, u8 index)
 211 {
 212         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 213
 214         return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
 215 }
 216
 217 static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw,
 218                                          unsigned long parent_rate)
 219 {
 220         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 221
 222         ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
 223                                                 &parent_rate);
 224
 225         return parent_rate;
 226 }
 227
 228 const struct clk_ops ccu_mux_ops = {
 229         .disable        = ccu_mux_disable,
 230         .enable         = ccu_mux_enable,
 231         .is_enabled     = ccu_mux_is_enabled,
 232
 233         .get_parent     = ccu_mux_get_parent,
 234         .set_parent     = ccu_mux_set_parent,
 235
 236         .determine_rate = __clk_mux_determine_rate,
 237         .recalc_rate    = ccu_mux_recalc_rate,
 238 };
 239
 240 /*
 241  * This clock notifier is called when the frequency of the of the parent
 242  * PLL clock is to be changed. The idea is to switch the parent to a
 243  * stable clock, such as the main oscillator, while the PLL frequency
 244  * stabilizes.
 245  */
 246 static int ccu_mux_notifier_cb(struct notifier_block *nb,
 247                                unsigned long event, void *data)
 248 {
 249         struct ccu_mux_nb *mux = to_ccu_mux_nb(nb);
 250         int ret = 0;
 251
 252         if (event == PRE_RATE_CHANGE) {
 253                 mux->original_index = ccu_mux_helper_get_parent(mux->common,
 254                                                                 mux->cm);
 255                 ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
 256                                                 mux->bypass_index);
 257         } else if (event == POST_RATE_CHANGE) {
 258                 ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
 259                                                 mux->original_index);
 260         }
 261
 262         udelay(mux->delay_us);
 263
 264         return notifier_from_errno(ret);
 265 }
 266
 267 int ccu_mux_notifier_register(struct clk *clk, struct ccu_mux_nb *mux_nb)
 268 {
 269         mux_nb->clk_nb.notifier_call = ccu_mux_notifier_cb;
 270
 271         return clk_notifier_register(clk, &mux_nb->clk_nb);
 272 }