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