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 } 271