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