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