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