1 /* 2 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License as published by 6 * the Free Software Foundation; either version 2 of the License, or 7 * (at your option) any later version. 8 * 9 */ 10 11 #include <linux/clk-provider.h> 12 #include <linux/clkdev.h> 13 #include <linux/clk/at91_pmc.h> 14 #include <linux/of.h> 15 #include <linux/of_address.h> 16 #include <linux/of_irq.h> 17 #include <linux/io.h> 18 #include <linux/wait.h> 19 #include <linux/sched.h> 20 #include <linux/interrupt.h> 21 #include <linux/irq.h> 22 23 #include "pmc.h" 24 25 #define PROG_SOURCE_MAX 5 26 #define PROG_ID_MAX 7 27 28 #define PROG_STATUS_MASK(id) (1 << ((id) + 8)) 29 #define PROG_PRES_MASK 0x7 30 #define PROG_MAX_RM9200_CSS 3 31 32 struct clk_programmable_layout { 33 u8 pres_shift; 34 u8 css_mask; 35 u8 have_slck_mck; 36 }; 37 38 struct clk_programmable { 39 struct clk_hw hw; 40 struct at91_pmc *pmc; 41 unsigned int irq; 42 wait_queue_head_t wait; 43 u8 id; 44 u8 css; 45 u8 pres; 46 u8 slckmck; 47 const struct clk_programmable_layout *layout; 48 }; 49 50 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) 51 52 53 static irqreturn_t clk_programmable_irq_handler(int irq, void *dev_id) 54 { 55 struct clk_programmable *prog = (struct clk_programmable *)dev_id; 56 57 wake_up(&prog->wait); 58 59 return IRQ_HANDLED; 60 } 61 62 static int clk_programmable_prepare(struct clk_hw *hw) 63 { 64 u32 tmp; 65 struct clk_programmable *prog = to_clk_programmable(hw); 66 struct at91_pmc *pmc = prog->pmc; 67 const struct clk_programmable_layout *layout = prog->layout; 68 u8 id = prog->id; 69 u32 mask = PROG_STATUS_MASK(id); 70 71 tmp = prog->css | (prog->pres << layout->pres_shift); 72 if (layout->have_slck_mck && prog->slckmck) 73 tmp |= AT91_PMC_CSSMCK_MCK; 74 75 pmc_write(pmc, AT91_PMC_PCKR(id), tmp); 76 77 while (!(pmc_read(pmc, AT91_PMC_SR) & mask)) 78 wait_event(prog->wait, pmc_read(pmc, AT91_PMC_SR) & mask); 79 80 return 0; 81 } 82 83 static int clk_programmable_is_ready(struct clk_hw *hw) 84 { 85 struct clk_programmable *prog = to_clk_programmable(hw); 86 struct at91_pmc *pmc = prog->pmc; 87 88 return !!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_PCKR(prog->id)); 89 } 90 91 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, 92 unsigned long parent_rate) 93 { 94 u32 tmp; 95 struct clk_programmable *prog = to_clk_programmable(hw); 96 struct at91_pmc *pmc = prog->pmc; 97 const struct clk_programmable_layout *layout = prog->layout; 98 99 tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)); 100 prog->pres = (tmp >> layout->pres_shift) & PROG_PRES_MASK; 101 102 return parent_rate >> prog->pres; 103 } 104 105 static long clk_programmable_round_rate(struct clk_hw *hw, unsigned long rate, 106 unsigned long *parent_rate) 107 { 108 unsigned long best_rate = *parent_rate; 109 unsigned long best_diff; 110 unsigned long new_diff; 111 unsigned long cur_rate; 112 int shift = shift; 113 114 if (rate > *parent_rate) 115 return *parent_rate; 116 else 117 best_diff = *parent_rate - rate; 118 119 if (!best_diff) 120 return best_rate; 121 122 for (shift = 1; shift < PROG_PRES_MASK; shift++) { 123 cur_rate = *parent_rate >> shift; 124 125 if (cur_rate > rate) 126 new_diff = cur_rate - rate; 127 else 128 new_diff = rate - cur_rate; 129 130 if (!new_diff) 131 return cur_rate; 132 133 if (new_diff < best_diff) { 134 best_diff = new_diff; 135 best_rate = cur_rate; 136 } 137 138 if (rate > cur_rate) 139 break; 140 } 141 142 return best_rate; 143 } 144 145 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) 146 { 147 struct clk_programmable *prog = to_clk_programmable(hw); 148 const struct clk_programmable_layout *layout = prog->layout; 149 if (index > layout->css_mask) { 150 if (index > PROG_MAX_RM9200_CSS && layout->have_slck_mck) { 151 prog->css = 0; 152 prog->slckmck = 1; 153 return 0; 154 } else { 155 return -EINVAL; 156 } 157 } 158 159 prog->css = index; 160 return 0; 161 } 162 163 static u8 clk_programmable_get_parent(struct clk_hw *hw) 164 { 165 u32 tmp; 166 u8 ret; 167 struct clk_programmable *prog = to_clk_programmable(hw); 168 struct at91_pmc *pmc = prog->pmc; 169 const struct clk_programmable_layout *layout = prog->layout; 170 171 tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)); 172 prog->css = tmp & layout->css_mask; 173 ret = prog->css; 174 if (layout->have_slck_mck) { 175 prog->slckmck = !!(tmp & AT91_PMC_CSSMCK_MCK); 176 if (prog->slckmck && !ret) 177 ret = PROG_MAX_RM9200_CSS + 1; 178 } 179 180 return ret; 181 } 182 183 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, 184 unsigned long parent_rate) 185 { 186 struct clk_programmable *prog = to_clk_programmable(hw); 187 unsigned long best_rate = parent_rate; 188 unsigned long best_diff; 189 unsigned long new_diff; 190 unsigned long cur_rate; 191 int shift = 0; 192 193 if (rate > parent_rate) 194 return parent_rate; 195 else 196 best_diff = parent_rate - rate; 197 198 if (!best_diff) { 199 prog->pres = shift; 200 return 0; 201 } 202 203 for (shift = 1; shift < PROG_PRES_MASK; shift++) { 204 cur_rate = parent_rate >> shift; 205 206 if (cur_rate > rate) 207 new_diff = cur_rate - rate; 208 else 209 new_diff = rate - cur_rate; 210 211 if (!new_diff) 212 break; 213 214 if (new_diff < best_diff) { 215 best_diff = new_diff; 216 best_rate = cur_rate; 217 } 218 219 if (rate > cur_rate) 220 break; 221 } 222 223 prog->pres = shift; 224 return 0; 225 } 226 227 static const struct clk_ops programmable_ops = { 228 .prepare = clk_programmable_prepare, 229 .is_prepared = clk_programmable_is_ready, 230 .recalc_rate = clk_programmable_recalc_rate, 231 .round_rate = clk_programmable_round_rate, 232 .get_parent = clk_programmable_get_parent, 233 .set_parent = clk_programmable_set_parent, 234 .set_rate = clk_programmable_set_rate, 235 }; 236 237 static struct clk * __init 238 at91_clk_register_programmable(struct at91_pmc *pmc, unsigned int irq, 239 const char *name, const char **parent_names, 240 u8 num_parents, u8 id, 241 const struct clk_programmable_layout *layout) 242 { 243 int ret; 244 struct clk_programmable *prog; 245 struct clk *clk = NULL; 246 struct clk_init_data init; 247 char irq_name[11]; 248 249 if (id > PROG_ID_MAX) 250 return ERR_PTR(-EINVAL); 251 252 prog = kzalloc(sizeof(*prog), GFP_KERNEL); 253 if (!prog) 254 return ERR_PTR(-ENOMEM); 255 256 init.name = name; 257 init.ops = &programmable_ops; 258 init.parent_names = parent_names; 259 init.num_parents = num_parents; 260 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 261 262 prog->id = id; 263 prog->layout = layout; 264 prog->hw.init = &init; 265 prog->pmc = pmc; 266 prog->irq = irq; 267 init_waitqueue_head(&prog->wait); 268 irq_set_status_flags(prog->irq, IRQ_NOAUTOEN); 269 snprintf(irq_name, sizeof(irq_name), "clk-prog%d", id); 270 ret = request_irq(prog->irq, clk_programmable_irq_handler, 271 IRQF_TRIGGER_HIGH, irq_name, prog); 272 if (ret) 273 return ERR_PTR(ret); 274 275 clk = clk_register(NULL, &prog->hw); 276 if (IS_ERR(clk)) 277 kfree(prog); 278 279 return clk; 280 } 281 282 static const struct clk_programmable_layout at91rm9200_programmable_layout = { 283 .pres_shift = 2, 284 .css_mask = 0x3, 285 .have_slck_mck = 0, 286 }; 287 288 static const struct clk_programmable_layout at91sam9g45_programmable_layout = { 289 .pres_shift = 2, 290 .css_mask = 0x3, 291 .have_slck_mck = 1, 292 }; 293 294 static const struct clk_programmable_layout at91sam9x5_programmable_layout = { 295 .pres_shift = 4, 296 .css_mask = 0x7, 297 .have_slck_mck = 0, 298 }; 299 300 static void __init 301 of_at91_clk_prog_setup(struct device_node *np, struct at91_pmc *pmc, 302 const struct clk_programmable_layout *layout) 303 { 304 int num; 305 u32 id; 306 int i; 307 unsigned int irq; 308 struct clk *clk; 309 int num_parents; 310 const char *parent_names[PROG_SOURCE_MAX]; 311 const char *name; 312 struct device_node *progclknp; 313 314 num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells"); 315 if (num_parents <= 0 || num_parents > PROG_SOURCE_MAX) 316 return; 317 318 for (i = 0; i < num_parents; ++i) { 319 parent_names[i] = of_clk_get_parent_name(np, i); 320 if (!parent_names[i]) 321 return; 322 } 323 324 num = of_get_child_count(np); 325 if (!num || num > (PROG_ID_MAX + 1)) 326 return; 327 328 for_each_child_of_node(np, progclknp) { 329 if (of_property_read_u32(progclknp, "reg", &id)) 330 continue; 331 332 if (of_property_read_string(np, "clock-output-names", &name)) 333 name = progclknp->name; 334 335 irq = irq_of_parse_and_map(progclknp, 0); 336 if (!irq) 337 continue; 338 339 clk = at91_clk_register_programmable(pmc, irq, name, 340 parent_names, num_parents, 341 id, layout); 342 if (IS_ERR(clk)) 343 continue; 344 345 of_clk_add_provider(progclknp, of_clk_src_simple_get, clk); 346 } 347 } 348 349 350 void __init of_at91rm9200_clk_prog_setup(struct device_node *np, 351 struct at91_pmc *pmc) 352 { 353 of_at91_clk_prog_setup(np, pmc, &at91rm9200_programmable_layout); 354 } 355 356 void __init of_at91sam9g45_clk_prog_setup(struct device_node *np, 357 struct at91_pmc *pmc) 358 { 359 of_at91_clk_prog_setup(np, pmc, &at91sam9g45_programmable_layout); 360 } 361 362 void __init of_at91sam9x5_clk_prog_setup(struct device_node *np, 363 struct at91_pmc *pmc) 364 { 365 of_at91_clk_prog_setup(np, pmc, &at91sam9x5_programmable_layout); 366 } 367