1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2018 Fuzhou Rockchip Electronics Co., Ltd 4 */ 5 6 #include <linux/clk-provider.h> 7 #include <linux/io.h> 8 #include <linux/slab.h> 9 #include "clk.h" 10 11 #define div_mask(width) ((1 << (width)) - 1) 12 13 static bool _is_best_half_div(unsigned long rate, unsigned long now, 14 unsigned long best, unsigned long flags) 15 { 16 if (flags & CLK_DIVIDER_ROUND_CLOSEST) 17 return abs(rate - now) < abs(rate - best); 18 19 return now <= rate && now > best; 20 } 21 22 static unsigned long clk_half_divider_recalc_rate(struct clk_hw *hw, 23 unsigned long parent_rate) 24 { 25 struct clk_divider *divider = to_clk_divider(hw); 26 unsigned int val; 27 28 val = readl(divider->reg) >> divider->shift; 29 val &= div_mask(divider->width); 30 val = val * 2 + 3; 31 32 return DIV_ROUND_UP_ULL(((u64)parent_rate * 2), val); 33 } 34 35 static int clk_half_divider_bestdiv(struct clk_hw *hw, unsigned long rate, 36 unsigned long *best_parent_rate, u8 width, 37 unsigned long flags) 38 { 39 unsigned int i, bestdiv = 0; 40 unsigned long parent_rate, best = 0, now, maxdiv; 41 unsigned long parent_rate_saved = *best_parent_rate; 42 43 if (!rate) 44 rate = 1; 45 46 maxdiv = div_mask(width); 47 48 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { 49 parent_rate = *best_parent_rate; 50 bestdiv = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate); 51 if (bestdiv < 3) 52 bestdiv = 0; 53 else 54 bestdiv = (bestdiv - 3) / 2; 55 bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv; 56 return bestdiv; 57 } 58 59 /* 60 * The maximum divider we can use without overflowing 61 * unsigned long in rate * i below 62 */ 63 maxdiv = min(ULONG_MAX / rate, maxdiv); 64 65 for (i = 0; i <= maxdiv; i++) { 66 if (((u64)rate * (i * 2 + 3)) == ((u64)parent_rate_saved * 2)) { 67 /* 68 * It's the most ideal case if the requested rate can be 69 * divided from parent clock without needing to change 70 * parent rate, so return the divider immediately. 71 */ 72 *best_parent_rate = parent_rate_saved; 73 return i; 74 } 75 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 76 ((u64)rate * (i * 2 + 3)) / 2); 77 now = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), 78 (i * 2 + 3)); 79 80 if (_is_best_half_div(rate, now, best, flags)) { 81 bestdiv = i; 82 best = now; 83 *best_parent_rate = parent_rate; 84 } 85 } 86 87 if (!bestdiv) { 88 bestdiv = div_mask(width); 89 *best_parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 1); 90 } 91 92 return bestdiv; 93 } 94 95 static long clk_half_divider_round_rate(struct clk_hw *hw, unsigned long rate, 96 unsigned long *prate) 97 { 98 struct clk_divider *divider = to_clk_divider(hw); 99 int div; 100 101 div = clk_half_divider_bestdiv(hw, rate, prate, 102 divider->width, 103 divider->flags); 104 105 return DIV_ROUND_UP_ULL(((u64)*prate * 2), div * 2 + 3); 106 } 107 108 static int clk_half_divider_set_rate(struct clk_hw *hw, unsigned long rate, 109 unsigned long parent_rate) 110 { 111 struct clk_divider *divider = to_clk_divider(hw); 112 unsigned int value; 113 unsigned long flags = 0; 114 u32 val; 115 116 value = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate); 117 value = (value - 3) / 2; 118 value = min_t(unsigned int, value, div_mask(divider->width)); 119 120 if (divider->lock) 121 spin_lock_irqsave(divider->lock, flags); 122 else 123 __acquire(divider->lock); 124 125 if (divider->flags & CLK_DIVIDER_HIWORD_MASK) { 126 val = div_mask(divider->width) << (divider->shift + 16); 127 } else { 128 val = readl(divider->reg); 129 val &= ~(div_mask(divider->width) << divider->shift); 130 } 131 val |= value << divider->shift; 132 writel(val, divider->reg); 133 134 if (divider->lock) 135 spin_unlock_irqrestore(divider->lock, flags); 136 else 137 __release(divider->lock); 138 139 return 0; 140 } 141 142 const struct clk_ops clk_half_divider_ops = { 143 .recalc_rate = clk_half_divider_recalc_rate, 144 .round_rate = clk_half_divider_round_rate, 145 .set_rate = clk_half_divider_set_rate, 146 }; 147 EXPORT_SYMBOL_GPL(clk_half_divider_ops); 148 149 /** 150 * Register a clock branch. 151 * Most clock branches have a form like 152 * 153 * src1 --|--\ 154 * |M |--[GATE]-[DIV]- 155 * src2 --|--/ 156 * 157 * sometimes without one of those components. 158 */ 159 struct clk *rockchip_clk_register_halfdiv(const char *name, 160 const char *const *parent_names, 161 u8 num_parents, void __iomem *base, 162 int muxdiv_offset, u8 mux_shift, 163 u8 mux_width, u8 mux_flags, 164 u8 div_shift, u8 div_width, 165 u8 div_flags, int gate_offset, 166 u8 gate_shift, u8 gate_flags, 167 unsigned long flags, 168 spinlock_t *lock) 169 { 170 struct clk *clk; 171 struct clk_mux *mux = NULL; 172 struct clk_gate *gate = NULL; 173 struct clk_divider *div = NULL; 174 const struct clk_ops *mux_ops = NULL, *div_ops = NULL, 175 *gate_ops = NULL; 176 177 if (num_parents > 1) { 178 mux = kzalloc(sizeof(*mux), GFP_KERNEL); 179 if (!mux) 180 return ERR_PTR(-ENOMEM); 181 182 mux->reg = base + muxdiv_offset; 183 mux->shift = mux_shift; 184 mux->mask = BIT(mux_width) - 1; 185 mux->flags = mux_flags; 186 mux->lock = lock; 187 mux_ops = (mux_flags & CLK_MUX_READ_ONLY) ? &clk_mux_ro_ops 188 : &clk_mux_ops; 189 } 190 191 if (gate_offset >= 0) { 192 gate = kzalloc(sizeof(*gate), GFP_KERNEL); 193 if (!gate) 194 goto err_gate; 195 196 gate->flags = gate_flags; 197 gate->reg = base + gate_offset; 198 gate->bit_idx = gate_shift; 199 gate->lock = lock; 200 gate_ops = &clk_gate_ops; 201 } 202 203 if (div_width > 0) { 204 div = kzalloc(sizeof(*div), GFP_KERNEL); 205 if (!div) 206 goto err_div; 207 208 div->flags = div_flags; 209 div->reg = base + muxdiv_offset; 210 div->shift = div_shift; 211 div->width = div_width; 212 div->lock = lock; 213 div_ops = &clk_half_divider_ops; 214 } 215 216 clk = clk_register_composite(NULL, name, parent_names, num_parents, 217 mux ? &mux->hw : NULL, mux_ops, 218 div ? &div->hw : NULL, div_ops, 219 gate ? &gate->hw : NULL, gate_ops, 220 flags); 221 222 return clk; 223 err_div: 224 kfree(gate); 225 err_gate: 226 kfree(mux); 227 return ERR_PTR(-ENOMEM); 228 } 229