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