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