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 USB_SOURCE_MAX 2 21 22 #define SAM9X5_USB_DIV_SHIFT 8 23 #define SAM9X5_USB_MAX_DIV 0xf 24 25 #define RM9200_USB_DIV_SHIFT 28 26 #define RM9200_USB_DIV_TAB_SIZE 4 27 28 struct at91sam9x5_clk_usb { 29 struct clk_hw hw; 30 struct regmap *regmap; 31 }; 32 33 #define to_at91sam9x5_clk_usb(hw) \ 34 container_of(hw, struct at91sam9x5_clk_usb, hw) 35 36 struct at91rm9200_clk_usb { 37 struct clk_hw hw; 38 struct regmap *regmap; 39 u32 divisors[4]; 40 }; 41 42 #define to_at91rm9200_clk_usb(hw) \ 43 container_of(hw, struct at91rm9200_clk_usb, hw) 44 45 static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw, 46 unsigned long parent_rate) 47 { 48 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 49 unsigned int usbr; 50 u8 usbdiv; 51 52 regmap_read(usb->regmap, AT91_PMC_USB, &usbr); 53 usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT; 54 55 return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1)); 56 } 57 58 static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw, 59 struct clk_rate_request *req) 60 { 61 struct clk_hw *parent; 62 long best_rate = -EINVAL; 63 unsigned long tmp_rate; 64 int best_diff = -1; 65 int tmp_diff; 66 int i; 67 68 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 69 int div; 70 71 parent = clk_hw_get_parent_by_index(hw, i); 72 if (!parent) 73 continue; 74 75 for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) { 76 unsigned long tmp_parent_rate; 77 78 tmp_parent_rate = req->rate * div; 79 tmp_parent_rate = clk_hw_round_rate(parent, 80 tmp_parent_rate); 81 tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div); 82 if (tmp_rate < req->rate) 83 tmp_diff = req->rate - tmp_rate; 84 else 85 tmp_diff = tmp_rate - req->rate; 86 87 if (best_diff < 0 || best_diff > tmp_diff) { 88 best_rate = tmp_rate; 89 best_diff = tmp_diff; 90 req->best_parent_rate = tmp_parent_rate; 91 req->best_parent_hw = parent; 92 } 93 94 if (!best_diff || tmp_rate < req->rate) 95 break; 96 } 97 98 if (!best_diff) 99 break; 100 } 101 102 if (best_rate < 0) 103 return best_rate; 104 105 req->rate = best_rate; 106 return 0; 107 } 108 109 static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index) 110 { 111 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 112 113 if (index > 1) 114 return -EINVAL; 115 116 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 117 index ? AT91_PMC_USBS : 0); 118 119 return 0; 120 } 121 122 static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw) 123 { 124 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 125 unsigned int usbr; 126 127 regmap_read(usb->regmap, AT91_PMC_USB, &usbr); 128 129 return usbr & AT91_PMC_USBS; 130 } 131 132 static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate, 133 unsigned long parent_rate) 134 { 135 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 136 unsigned long div; 137 138 if (!rate) 139 return -EINVAL; 140 141 div = DIV_ROUND_CLOSEST(parent_rate, rate); 142 if (div > SAM9X5_USB_MAX_DIV + 1 || !div) 143 return -EINVAL; 144 145 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV, 146 (div - 1) << SAM9X5_USB_DIV_SHIFT); 147 148 return 0; 149 } 150 151 static const struct clk_ops at91sam9x5_usb_ops = { 152 .recalc_rate = at91sam9x5_clk_usb_recalc_rate, 153 .determine_rate = at91sam9x5_clk_usb_determine_rate, 154 .get_parent = at91sam9x5_clk_usb_get_parent, 155 .set_parent = at91sam9x5_clk_usb_set_parent, 156 .set_rate = at91sam9x5_clk_usb_set_rate, 157 }; 158 159 static int at91sam9n12_clk_usb_enable(struct clk_hw *hw) 160 { 161 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 162 163 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 164 AT91_PMC_USBS); 165 166 return 0; 167 } 168 169 static void at91sam9n12_clk_usb_disable(struct clk_hw *hw) 170 { 171 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 172 173 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 0); 174 } 175 176 static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw) 177 { 178 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); 179 unsigned int usbr; 180 181 regmap_read(usb->regmap, AT91_PMC_USB, &usbr); 182 183 return usbr & AT91_PMC_USBS; 184 } 185 186 static const struct clk_ops at91sam9n12_usb_ops = { 187 .enable = at91sam9n12_clk_usb_enable, 188 .disable = at91sam9n12_clk_usb_disable, 189 .is_enabled = at91sam9n12_clk_usb_is_enabled, 190 .recalc_rate = at91sam9x5_clk_usb_recalc_rate, 191 .determine_rate = at91sam9x5_clk_usb_determine_rate, 192 .set_rate = at91sam9x5_clk_usb_set_rate, 193 }; 194 195 static struct clk * __init 196 at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name, 197 const char **parent_names, u8 num_parents) 198 { 199 struct at91sam9x5_clk_usb *usb; 200 struct clk *clk = NULL; 201 struct clk_init_data init; 202 203 usb = kzalloc(sizeof(*usb), GFP_KERNEL); 204 if (!usb) 205 return ERR_PTR(-ENOMEM); 206 207 init.name = name; 208 init.ops = &at91sam9x5_usb_ops; 209 init.parent_names = parent_names; 210 init.num_parents = num_parents; 211 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | 212 CLK_SET_RATE_PARENT; 213 214 usb->hw.init = &init; 215 usb->regmap = regmap; 216 217 clk = clk_register(NULL, &usb->hw); 218 if (IS_ERR(clk)) 219 kfree(usb); 220 221 return clk; 222 } 223 224 static struct clk * __init 225 at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name, 226 const char *parent_name) 227 { 228 struct at91sam9x5_clk_usb *usb; 229 struct clk *clk = NULL; 230 struct clk_init_data init; 231 232 usb = kzalloc(sizeof(*usb), GFP_KERNEL); 233 if (!usb) 234 return ERR_PTR(-ENOMEM); 235 236 init.name = name; 237 init.ops = &at91sam9n12_usb_ops; 238 init.parent_names = &parent_name; 239 init.num_parents = 1; 240 init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT; 241 242 usb->hw.init = &init; 243 usb->regmap = regmap; 244 245 clk = clk_register(NULL, &usb->hw); 246 if (IS_ERR(clk)) 247 kfree(usb); 248 249 return clk; 250 } 251 252 static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw, 253 unsigned long parent_rate) 254 { 255 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); 256 unsigned int pllbr; 257 u8 usbdiv; 258 259 regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr); 260 261 usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT; 262 if (usb->divisors[usbdiv]) 263 return parent_rate / usb->divisors[usbdiv]; 264 265 return 0; 266 } 267 268 static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate, 269 unsigned long *parent_rate) 270 { 271 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); 272 struct clk_hw *parent = clk_hw_get_parent(hw); 273 unsigned long bestrate = 0; 274 int bestdiff = -1; 275 unsigned long tmprate; 276 int tmpdiff; 277 int i = 0; 278 279 for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) { 280 unsigned long tmp_parent_rate; 281 282 if (!usb->divisors[i]) 283 continue; 284 285 tmp_parent_rate = rate * usb->divisors[i]; 286 tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate); 287 tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]); 288 if (tmprate < rate) 289 tmpdiff = rate - tmprate; 290 else 291 tmpdiff = tmprate - rate; 292 293 if (bestdiff < 0 || bestdiff > tmpdiff) { 294 bestrate = tmprate; 295 bestdiff = tmpdiff; 296 *parent_rate = tmp_parent_rate; 297 } 298 299 if (!bestdiff) 300 break; 301 } 302 303 return bestrate; 304 } 305 306 static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate, 307 unsigned long parent_rate) 308 { 309 int i; 310 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); 311 unsigned long div; 312 313 if (!rate) 314 return -EINVAL; 315 316 div = DIV_ROUND_CLOSEST(parent_rate, rate); 317 318 for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) { 319 if (usb->divisors[i] == div) { 320 regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR, 321 AT91_PMC_USBDIV, 322 i << RM9200_USB_DIV_SHIFT); 323 324 return 0; 325 } 326 } 327 328 return -EINVAL; 329 } 330 331 static const struct clk_ops at91rm9200_usb_ops = { 332 .recalc_rate = at91rm9200_clk_usb_recalc_rate, 333 .round_rate = at91rm9200_clk_usb_round_rate, 334 .set_rate = at91rm9200_clk_usb_set_rate, 335 }; 336 337 static struct clk * __init 338 at91rm9200_clk_register_usb(struct regmap *regmap, const char *name, 339 const char *parent_name, const u32 *divisors) 340 { 341 struct at91rm9200_clk_usb *usb; 342 struct clk *clk = NULL; 343 struct clk_init_data init; 344 345 usb = kzalloc(sizeof(*usb), GFP_KERNEL); 346 if (!usb) 347 return ERR_PTR(-ENOMEM); 348 349 init.name = name; 350 init.ops = &at91rm9200_usb_ops; 351 init.parent_names = &parent_name; 352 init.num_parents = 1; 353 init.flags = CLK_SET_RATE_PARENT; 354 355 usb->hw.init = &init; 356 usb->regmap = regmap; 357 memcpy(usb->divisors, divisors, sizeof(usb->divisors)); 358 359 clk = clk_register(NULL, &usb->hw); 360 if (IS_ERR(clk)) 361 kfree(usb); 362 363 return clk; 364 } 365 366 static void __init of_at91sam9x5_clk_usb_setup(struct device_node *np) 367 { 368 struct clk *clk; 369 int num_parents; 370 const char *parent_names[USB_SOURCE_MAX]; 371 const char *name = np->name; 372 struct regmap *regmap; 373 374 num_parents = of_clk_get_parent_count(np); 375 if (num_parents <= 0 || num_parents > USB_SOURCE_MAX) 376 return; 377 378 of_clk_parent_fill(np, parent_names, num_parents); 379 380 of_property_read_string(np, "clock-output-names", &name); 381 382 regmap = syscon_node_to_regmap(of_get_parent(np)); 383 if (IS_ERR(regmap)) 384 return; 385 386 clk = at91sam9x5_clk_register_usb(regmap, name, parent_names, 387 num_parents); 388 if (IS_ERR(clk)) 389 return; 390 391 of_clk_add_provider(np, of_clk_src_simple_get, clk); 392 } 393 CLK_OF_DECLARE(at91sam9x5_clk_usb, "atmel,at91sam9x5-clk-usb", 394 of_at91sam9x5_clk_usb_setup); 395 396 static void __init of_at91sam9n12_clk_usb_setup(struct device_node *np) 397 { 398 struct clk *clk; 399 const char *parent_name; 400 const char *name = np->name; 401 struct regmap *regmap; 402 403 parent_name = of_clk_get_parent_name(np, 0); 404 if (!parent_name) 405 return; 406 407 of_property_read_string(np, "clock-output-names", &name); 408 409 regmap = syscon_node_to_regmap(of_get_parent(np)); 410 if (IS_ERR(regmap)) 411 return; 412 413 clk = at91sam9n12_clk_register_usb(regmap, name, parent_name); 414 if (IS_ERR(clk)) 415 return; 416 417 of_clk_add_provider(np, of_clk_src_simple_get, clk); 418 } 419 CLK_OF_DECLARE(at91sam9n12_clk_usb, "atmel,at91sam9n12-clk-usb", 420 of_at91sam9n12_clk_usb_setup); 421 422 static void __init of_at91rm9200_clk_usb_setup(struct device_node *np) 423 { 424 struct clk *clk; 425 const char *parent_name; 426 const char *name = np->name; 427 u32 divisors[4] = {0, 0, 0, 0}; 428 struct regmap *regmap; 429 430 parent_name = of_clk_get_parent_name(np, 0); 431 if (!parent_name) 432 return; 433 434 of_property_read_u32_array(np, "atmel,clk-divisors", divisors, 4); 435 if (!divisors[0]) 436 return; 437 438 of_property_read_string(np, "clock-output-names", &name); 439 440 regmap = syscon_node_to_regmap(of_get_parent(np)); 441 if (IS_ERR(regmap)) 442 return; 443 444 clk = at91rm9200_clk_register_usb(regmap, name, parent_name, divisors); 445 if (IS_ERR(clk)) 446 return; 447 448 of_clk_add_provider(np, of_clk_src_simple_get, clk); 449 } 450 CLK_OF_DECLARE(at91rm9200_clk_usb, "atmel,at91rm9200-clk-usb", 451 of_at91rm9200_clk_usb_setup); 452