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/mfd/syscon.h> 16 #include <linux/regmap.h> 17 18 #include "pmc.h" 19 20 DEFINE_SPINLOCK(pmc_pcr_lock); 21 22 #define PERIPHERAL_ID_MIN 2 23 #define PERIPHERAL_ID_MAX 31 24 #define PERIPHERAL_MASK(id) (1 << ((id) & PERIPHERAL_ID_MAX)) 25 26 #define PERIPHERAL_RSHIFT_MASK 0x3 27 #define PERIPHERAL_RSHIFT(val) (((val) >> 16) & PERIPHERAL_RSHIFT_MASK) 28 29 #define PERIPHERAL_MAX_SHIFT 3 30 31 struct clk_peripheral { 32 struct clk_hw hw; 33 struct regmap *regmap; 34 u32 id; 35 }; 36 37 #define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw) 38 39 struct clk_sam9x5_peripheral { 40 struct clk_hw hw; 41 struct regmap *regmap; 42 struct clk_range range; 43 spinlock_t *lock; 44 u32 id; 45 u32 div; 46 bool auto_div; 47 }; 48 49 #define to_clk_sam9x5_peripheral(hw) \ 50 container_of(hw, struct clk_sam9x5_peripheral, hw) 51 52 static int clk_peripheral_enable(struct clk_hw *hw) 53 { 54 struct clk_peripheral *periph = to_clk_peripheral(hw); 55 int offset = AT91_PMC_PCER; 56 u32 id = periph->id; 57 58 if (id < PERIPHERAL_ID_MIN) 59 return 0; 60 if (id > PERIPHERAL_ID_MAX) 61 offset = AT91_PMC_PCER1; 62 regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id)); 63 64 return 0; 65 } 66 67 static void clk_peripheral_disable(struct clk_hw *hw) 68 { 69 struct clk_peripheral *periph = to_clk_peripheral(hw); 70 int offset = AT91_PMC_PCDR; 71 u32 id = periph->id; 72 73 if (id < PERIPHERAL_ID_MIN) 74 return; 75 if (id > PERIPHERAL_ID_MAX) 76 offset = AT91_PMC_PCDR1; 77 regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id)); 78 } 79 80 static int clk_peripheral_is_enabled(struct clk_hw *hw) 81 { 82 struct clk_peripheral *periph = to_clk_peripheral(hw); 83 int offset = AT91_PMC_PCSR; 84 unsigned int status; 85 u32 id = periph->id; 86 87 if (id < PERIPHERAL_ID_MIN) 88 return 1; 89 if (id > PERIPHERAL_ID_MAX) 90 offset = AT91_PMC_PCSR1; 91 regmap_read(periph->regmap, offset, &status); 92 93 return status & PERIPHERAL_MASK(id) ? 1 : 0; 94 } 95 96 static const struct clk_ops peripheral_ops = { 97 .enable = clk_peripheral_enable, 98 .disable = clk_peripheral_disable, 99 .is_enabled = clk_peripheral_is_enabled, 100 }; 101 102 struct clk_hw * __init 103 at91_clk_register_peripheral(struct regmap *regmap, const char *name, 104 const char *parent_name, u32 id) 105 { 106 struct clk_peripheral *periph; 107 struct clk_init_data init; 108 struct clk_hw *hw; 109 int ret; 110 111 if (!name || !parent_name || id > PERIPHERAL_ID_MAX) 112 return ERR_PTR(-EINVAL); 113 114 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 115 if (!periph) 116 return ERR_PTR(-ENOMEM); 117 118 init.name = name; 119 init.ops = &peripheral_ops; 120 init.parent_names = (parent_name ? &parent_name : NULL); 121 init.num_parents = (parent_name ? 1 : 0); 122 init.flags = 0; 123 124 periph->id = id; 125 periph->hw.init = &init; 126 periph->regmap = regmap; 127 128 hw = &periph->hw; 129 ret = clk_hw_register(NULL, &periph->hw); 130 if (ret) { 131 kfree(periph); 132 hw = ERR_PTR(ret); 133 } 134 135 return hw; 136 } 137 138 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph) 139 { 140 struct clk_hw *parent; 141 unsigned long parent_rate; 142 int shift = 0; 143 144 if (!periph->auto_div) 145 return; 146 147 if (periph->range.max) { 148 parent = clk_hw_get_parent_by_index(&periph->hw, 0); 149 parent_rate = clk_hw_get_rate(parent); 150 if (!parent_rate) 151 return; 152 153 for (; shift < PERIPHERAL_MAX_SHIFT; shift++) { 154 if (parent_rate >> shift <= periph->range.max) 155 break; 156 } 157 } 158 159 periph->auto_div = false; 160 periph->div = shift; 161 } 162 163 static int clk_sam9x5_peripheral_enable(struct clk_hw *hw) 164 { 165 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 166 unsigned long flags; 167 168 if (periph->id < PERIPHERAL_ID_MIN) 169 return 0; 170 171 spin_lock_irqsave(periph->lock, flags); 172 regmap_write(periph->regmap, AT91_PMC_PCR, 173 (periph->id & AT91_PMC_PCR_PID_MASK)); 174 regmap_update_bits(periph->regmap, AT91_PMC_PCR, 175 AT91_PMC_PCR_DIV_MASK | AT91_PMC_PCR_CMD | 176 AT91_PMC_PCR_EN, 177 AT91_PMC_PCR_DIV(periph->div) | 178 AT91_PMC_PCR_CMD | 179 AT91_PMC_PCR_EN); 180 spin_unlock_irqrestore(periph->lock, flags); 181 182 return 0; 183 } 184 185 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw) 186 { 187 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 188 unsigned long flags; 189 190 if (periph->id < PERIPHERAL_ID_MIN) 191 return; 192 193 spin_lock_irqsave(periph->lock, flags); 194 regmap_write(periph->regmap, AT91_PMC_PCR, 195 (periph->id & AT91_PMC_PCR_PID_MASK)); 196 regmap_update_bits(periph->regmap, AT91_PMC_PCR, 197 AT91_PMC_PCR_EN | AT91_PMC_PCR_CMD, 198 AT91_PMC_PCR_CMD); 199 spin_unlock_irqrestore(periph->lock, flags); 200 } 201 202 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw) 203 { 204 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 205 unsigned long flags; 206 unsigned int status; 207 208 if (periph->id < PERIPHERAL_ID_MIN) 209 return 1; 210 211 spin_lock_irqsave(periph->lock, flags); 212 regmap_write(periph->regmap, AT91_PMC_PCR, 213 (periph->id & AT91_PMC_PCR_PID_MASK)); 214 regmap_read(periph->regmap, AT91_PMC_PCR, &status); 215 spin_unlock_irqrestore(periph->lock, flags); 216 217 return status & AT91_PMC_PCR_EN ? 1 : 0; 218 } 219 220 static unsigned long 221 clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw, 222 unsigned long parent_rate) 223 { 224 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 225 unsigned long flags; 226 unsigned int status; 227 228 if (periph->id < PERIPHERAL_ID_MIN) 229 return parent_rate; 230 231 spin_lock_irqsave(periph->lock, flags); 232 regmap_write(periph->regmap, AT91_PMC_PCR, 233 (periph->id & AT91_PMC_PCR_PID_MASK)); 234 regmap_read(periph->regmap, AT91_PMC_PCR, &status); 235 spin_unlock_irqrestore(periph->lock, flags); 236 237 if (status & AT91_PMC_PCR_EN) { 238 periph->div = PERIPHERAL_RSHIFT(status); 239 periph->auto_div = false; 240 } else { 241 clk_sam9x5_peripheral_autodiv(periph); 242 } 243 244 return parent_rate >> periph->div; 245 } 246 247 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw, 248 unsigned long rate, 249 unsigned long *parent_rate) 250 { 251 int shift = 0; 252 unsigned long best_rate; 253 unsigned long best_diff; 254 unsigned long cur_rate = *parent_rate; 255 unsigned long cur_diff; 256 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 257 258 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) 259 return *parent_rate; 260 261 if (periph->range.max) { 262 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 263 cur_rate = *parent_rate >> shift; 264 if (cur_rate <= periph->range.max) 265 break; 266 } 267 } 268 269 if (rate >= cur_rate) 270 return cur_rate; 271 272 best_diff = cur_rate - rate; 273 best_rate = cur_rate; 274 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 275 cur_rate = *parent_rate >> shift; 276 if (cur_rate < rate) 277 cur_diff = rate - cur_rate; 278 else 279 cur_diff = cur_rate - rate; 280 281 if (cur_diff < best_diff) { 282 best_diff = cur_diff; 283 best_rate = cur_rate; 284 } 285 286 if (!best_diff || cur_rate < rate) 287 break; 288 } 289 290 return best_rate; 291 } 292 293 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw, 294 unsigned long rate, 295 unsigned long parent_rate) 296 { 297 int shift; 298 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 299 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { 300 if (parent_rate == rate) 301 return 0; 302 else 303 return -EINVAL; 304 } 305 306 if (periph->range.max && rate > periph->range.max) 307 return -EINVAL; 308 309 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 310 if (parent_rate >> shift == rate) { 311 periph->auto_div = false; 312 periph->div = shift; 313 return 0; 314 } 315 } 316 317 return -EINVAL; 318 } 319 320 static const struct clk_ops sam9x5_peripheral_ops = { 321 .enable = clk_sam9x5_peripheral_enable, 322 .disable = clk_sam9x5_peripheral_disable, 323 .is_enabled = clk_sam9x5_peripheral_is_enabled, 324 .recalc_rate = clk_sam9x5_peripheral_recalc_rate, 325 .round_rate = clk_sam9x5_peripheral_round_rate, 326 .set_rate = clk_sam9x5_peripheral_set_rate, 327 }; 328 329 struct clk_hw * __init 330 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock, 331 const char *name, const char *parent_name, 332 u32 id, const struct clk_range *range) 333 { 334 struct clk_sam9x5_peripheral *periph; 335 struct clk_init_data init; 336 struct clk_hw *hw; 337 int ret; 338 339 if (!name || !parent_name) 340 return ERR_PTR(-EINVAL); 341 342 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 343 if (!periph) 344 return ERR_PTR(-ENOMEM); 345 346 init.name = name; 347 init.ops = &sam9x5_peripheral_ops; 348 init.parent_names = (parent_name ? &parent_name : NULL); 349 init.num_parents = (parent_name ? 1 : 0); 350 init.flags = 0; 351 352 periph->id = id; 353 periph->hw.init = &init; 354 periph->div = 0; 355 periph->regmap = regmap; 356 periph->lock = lock; 357 periph->auto_div = true; 358 periph->range = *range; 359 360 hw = &periph->hw; 361 ret = clk_hw_register(NULL, &periph->hw); 362 if (ret) { 363 kfree(periph); 364 hw = ERR_PTR(ret); 365 } else { 366 clk_sam9x5_peripheral_autodiv(periph); 367 pmc_register_id(id); 368 } 369 370 return hw; 371 } 372