1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 4 */ 5 6 #include <linux/clk-provider.h> 7 #include <linux/clkdev.h> 8 #include <linux/clk/at91_pmc.h> 9 #include <linux/of.h> 10 #include <linux/mfd/syscon.h> 11 #include <linux/regmap.h> 12 13 #include "pmc.h" 14 15 #define PROG_ID_MAX 7 16 17 #define PROG_STATUS_MASK(id) (1 << ((id) + 8)) 18 #define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask) 19 #define PROG_MAX_RM9200_CSS 3 20 21 struct clk_programmable { 22 struct clk_hw hw; 23 struct regmap *regmap; 24 u32 *mux_table; 25 u8 id; 26 const struct clk_programmable_layout *layout; 27 struct at91_clk_pms pms; 28 }; 29 30 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) 31 32 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, 33 unsigned long parent_rate) 34 { 35 struct clk_programmable *prog = to_clk_programmable(hw); 36 const struct clk_programmable_layout *layout = prog->layout; 37 unsigned int pckr; 38 unsigned long rate; 39 40 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 41 42 if (layout->is_pres_direct) 43 rate = parent_rate / (PROG_PRES(layout, pckr) + 1); 44 else 45 rate = parent_rate >> PROG_PRES(layout, pckr); 46 47 return rate; 48 } 49 50 static int clk_programmable_determine_rate(struct clk_hw *hw, 51 struct clk_rate_request *req) 52 { 53 struct clk_programmable *prog = to_clk_programmable(hw); 54 const struct clk_programmable_layout *layout = prog->layout; 55 struct clk_hw *parent; 56 long best_rate = -EINVAL; 57 unsigned long parent_rate; 58 unsigned long tmp_rate = 0; 59 int shift; 60 int i; 61 62 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 63 parent = clk_hw_get_parent_by_index(hw, i); 64 if (!parent) 65 continue; 66 67 parent_rate = clk_hw_get_rate(parent); 68 if (layout->is_pres_direct) { 69 for (shift = 0; shift <= layout->pres_mask; shift++) { 70 tmp_rate = parent_rate / (shift + 1); 71 if (tmp_rate <= req->rate) 72 break; 73 } 74 } else { 75 for (shift = 0; shift < layout->pres_mask; shift++) { 76 tmp_rate = parent_rate >> shift; 77 if (tmp_rate <= req->rate) 78 break; 79 } 80 } 81 82 if (tmp_rate > req->rate) 83 continue; 84 85 if (best_rate < 0 || 86 (req->rate - tmp_rate) < (req->rate - best_rate)) { 87 best_rate = tmp_rate; 88 req->best_parent_rate = parent_rate; 89 req->best_parent_hw = parent; 90 } 91 92 if (!best_rate) 93 break; 94 } 95 96 if (best_rate < 0) 97 return best_rate; 98 99 req->rate = best_rate; 100 return 0; 101 } 102 103 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) 104 { 105 struct clk_programmable *prog = to_clk_programmable(hw); 106 const struct clk_programmable_layout *layout = prog->layout; 107 unsigned int mask = layout->css_mask; 108 unsigned int pckr = index; 109 110 if (layout->have_slck_mck) 111 mask |= AT91_PMC_CSSMCK_MCK; 112 113 if (prog->mux_table) 114 pckr = clk_mux_index_to_val(prog->mux_table, 0, index); 115 116 if (index > layout->css_mask) { 117 if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck) 118 return -EINVAL; 119 120 pckr |= AT91_PMC_CSSMCK_MCK; 121 } 122 123 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr); 124 125 return 0; 126 } 127 128 static u8 clk_programmable_get_parent(struct clk_hw *hw) 129 { 130 struct clk_programmable *prog = to_clk_programmable(hw); 131 const struct clk_programmable_layout *layout = prog->layout; 132 unsigned int pckr; 133 u8 ret; 134 135 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 136 137 ret = pckr & layout->css_mask; 138 139 if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret) 140 ret = PROG_MAX_RM9200_CSS + 1; 141 142 if (prog->mux_table) 143 ret = clk_mux_val_to_index(&prog->hw, prog->mux_table, 0, ret); 144 145 return ret; 146 } 147 148 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, 149 unsigned long parent_rate) 150 { 151 struct clk_programmable *prog = to_clk_programmable(hw); 152 const struct clk_programmable_layout *layout = prog->layout; 153 unsigned long div = parent_rate / rate; 154 int shift = 0; 155 156 if (!div) 157 return -EINVAL; 158 159 if (layout->is_pres_direct) { 160 shift = div - 1; 161 162 if (shift > layout->pres_mask) 163 return -EINVAL; 164 } else { 165 shift = fls(div) - 1; 166 167 if (div != (1 << shift)) 168 return -EINVAL; 169 170 if (shift >= layout->pres_mask) 171 return -EINVAL; 172 } 173 174 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), 175 layout->pres_mask << layout->pres_shift, 176 shift << layout->pres_shift); 177 178 return 0; 179 } 180 181 static int clk_programmable_save_context(struct clk_hw *hw) 182 { 183 struct clk_programmable *prog = to_clk_programmable(hw); 184 struct clk_hw *parent_hw = clk_hw_get_parent(hw); 185 186 prog->pms.parent = clk_programmable_get_parent(hw); 187 prog->pms.parent_rate = clk_hw_get_rate(parent_hw); 188 prog->pms.rate = clk_programmable_recalc_rate(hw, prog->pms.parent_rate); 189 190 return 0; 191 } 192 193 static void clk_programmable_restore_context(struct clk_hw *hw) 194 { 195 struct clk_programmable *prog = to_clk_programmable(hw); 196 int ret; 197 198 ret = clk_programmable_set_parent(hw, prog->pms.parent); 199 if (ret) 200 return; 201 202 clk_programmable_set_rate(hw, prog->pms.rate, prog->pms.parent_rate); 203 } 204 205 static const struct clk_ops programmable_ops = { 206 .recalc_rate = clk_programmable_recalc_rate, 207 .determine_rate = clk_programmable_determine_rate, 208 .get_parent = clk_programmable_get_parent, 209 .set_parent = clk_programmable_set_parent, 210 .set_rate = clk_programmable_set_rate, 211 .save_context = clk_programmable_save_context, 212 .restore_context = clk_programmable_restore_context, 213 }; 214 215 struct clk_hw * __init 216 at91_clk_register_programmable(struct regmap *regmap, 217 const char *name, const char **parent_names, 218 struct clk_hw **parent_hws, u8 num_parents, u8 id, 219 const struct clk_programmable_layout *layout, 220 u32 *mux_table) 221 { 222 struct clk_programmable *prog; 223 struct clk_hw *hw; 224 struct clk_init_data init = {}; 225 int ret; 226 227 if (id > PROG_ID_MAX || !(parent_names || parent_hws)) 228 return ERR_PTR(-EINVAL); 229 230 prog = kzalloc(sizeof(*prog), GFP_KERNEL); 231 if (!prog) 232 return ERR_PTR(-ENOMEM); 233 234 init.name = name; 235 init.ops = &programmable_ops; 236 if (parent_hws) 237 init.parent_hws = (const struct clk_hw **)parent_hws; 238 else 239 init.parent_names = parent_names; 240 init.num_parents = num_parents; 241 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 242 243 prog->id = id; 244 prog->layout = layout; 245 prog->hw.init = &init; 246 prog->regmap = regmap; 247 prog->mux_table = mux_table; 248 249 hw = &prog->hw; 250 ret = clk_hw_register(NULL, &prog->hw); 251 if (ret) { 252 kfree(prog); 253 hw = ERR_PTR(ret); 254 } 255 256 return hw; 257 } 258 259 const struct clk_programmable_layout at91rm9200_programmable_layout = { 260 .pres_mask = 0x7, 261 .pres_shift = 2, 262 .css_mask = 0x3, 263 .have_slck_mck = 0, 264 .is_pres_direct = 0, 265 }; 266 267 const struct clk_programmable_layout at91sam9g45_programmable_layout = { 268 .pres_mask = 0x7, 269 .pres_shift = 2, 270 .css_mask = 0x3, 271 .have_slck_mck = 1, 272 .is_pres_direct = 0, 273 }; 274 275 const struct clk_programmable_layout at91sam9x5_programmable_layout = { 276 .pres_mask = 0x7, 277 .pres_shift = 4, 278 .css_mask = 0x7, 279 .have_slck_mck = 0, 280 .is_pres_direct = 0, 281 }; 282