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