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