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_hw * __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_hw *hw; 201 struct clk_init_data init; 202 int ret; 203 204 usb = kzalloc(sizeof(*usb), GFP_KERNEL); 205 if (!usb) 206 return ERR_PTR(-ENOMEM); 207 208 init.name = name; 209 init.ops = &at91sam9x5_usb_ops; 210 init.parent_names = parent_names; 211 init.num_parents = num_parents; 212 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | 213 CLK_SET_RATE_PARENT; 214 215 usb->hw.init = &init; 216 usb->regmap = regmap; 217 218 hw = &usb->hw; 219 ret = clk_hw_register(NULL, &usb->hw); 220 if (ret) { 221 kfree(usb); 222 hw = ERR_PTR(ret); 223 } 224 225 return hw; 226 } 227 228 static struct clk_hw * __init 229 at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name, 230 const char *parent_name) 231 { 232 struct at91sam9x5_clk_usb *usb; 233 struct clk_hw *hw; 234 struct clk_init_data init; 235 int ret; 236 237 usb = kzalloc(sizeof(*usb), GFP_KERNEL); 238 if (!usb) 239 return ERR_PTR(-ENOMEM); 240 241 init.name = name; 242 init.ops = &at91sam9n12_usb_ops; 243 init.parent_names = &parent_name; 244 init.num_parents = 1; 245 init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT; 246 247 usb->hw.init = &init; 248 usb->regmap = regmap; 249 250 hw = &usb->hw; 251 ret = clk_hw_register(NULL, &usb->hw); 252 if (ret) { 253 kfree(usb); 254 hw = ERR_PTR(ret); 255 } 256 257 return hw; 258 } 259 260 static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw, 261 unsigned long parent_rate) 262 { 263 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); 264 unsigned int pllbr; 265 u8 usbdiv; 266 267 regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr); 268 269 usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT; 270 if (usb->divisors[usbdiv]) 271 return parent_rate / usb->divisors[usbdiv]; 272 273 return 0; 274 } 275 276 static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate, 277 unsigned long *parent_rate) 278 { 279 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); 280 struct clk_hw *parent = clk_hw_get_parent(hw); 281 unsigned long bestrate = 0; 282 int bestdiff = -1; 283 unsigned long tmprate; 284 int tmpdiff; 285 int i = 0; 286 287 for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) { 288 unsigned long tmp_parent_rate; 289 290 if (!usb->divisors[i]) 291 continue; 292 293 tmp_parent_rate = rate * usb->divisors[i]; 294 tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate); 295 tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]); 296 if (tmprate < rate) 297 tmpdiff = rate - tmprate; 298 else 299 tmpdiff = tmprate - rate; 300 301 if (bestdiff < 0 || bestdiff > tmpdiff) { 302 bestrate = tmprate; 303 bestdiff = tmpdiff; 304 *parent_rate = tmp_parent_rate; 305 } 306 307 if (!bestdiff) 308 break; 309 } 310 311 return bestrate; 312 } 313 314 static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate, 315 unsigned long parent_rate) 316 { 317 int i; 318 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); 319 unsigned long div; 320 321 if (!rate) 322 return -EINVAL; 323 324 div = DIV_ROUND_CLOSEST(parent_rate, rate); 325 326 for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) { 327 if (usb->divisors[i] == div) { 328 regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR, 329 AT91_PMC_USBDIV, 330 i << RM9200_USB_DIV_SHIFT); 331 332 return 0; 333 } 334 } 335 336 return -EINVAL; 337 } 338 339 static const struct clk_ops at91rm9200_usb_ops = { 340 .recalc_rate = at91rm9200_clk_usb_recalc_rate, 341 .round_rate = at91rm9200_clk_usb_round_rate, 342 .set_rate = at91rm9200_clk_usb_set_rate, 343 }; 344 345 static struct clk_hw * __init 346 at91rm9200_clk_register_usb(struct regmap *regmap, const char *name, 347 const char *parent_name, const u32 *divisors) 348 { 349 struct at91rm9200_clk_usb *usb; 350 struct clk_hw *hw; 351 struct clk_init_data init; 352 int ret; 353 354 usb = kzalloc(sizeof(*usb), GFP_KERNEL); 355 if (!usb) 356 return ERR_PTR(-ENOMEM); 357 358 init.name = name; 359 init.ops = &at91rm9200_usb_ops; 360 init.parent_names = &parent_name; 361 init.num_parents = 1; 362 init.flags = CLK_SET_RATE_PARENT; 363 364 usb->hw.init = &init; 365 usb->regmap = regmap; 366 memcpy(usb->divisors, divisors, sizeof(usb->divisors)); 367 368 hw = &usb->hw; 369 ret = clk_hw_register(NULL, &usb->hw); 370 if (ret) { 371 kfree(usb); 372 hw = ERR_PTR(ret); 373 } 374 375 return hw; 376 } 377 378 static void __init of_at91sam9x5_clk_usb_setup(struct device_node *np) 379 { 380 struct clk_hw *hw; 381 unsigned int num_parents; 382 const char *parent_names[USB_SOURCE_MAX]; 383 const char *name = np->name; 384 struct regmap *regmap; 385 386 num_parents = of_clk_get_parent_count(np); 387 if (num_parents == 0 || num_parents > USB_SOURCE_MAX) 388 return; 389 390 of_clk_parent_fill(np, parent_names, num_parents); 391 392 of_property_read_string(np, "clock-output-names", &name); 393 394 regmap = syscon_node_to_regmap(of_get_parent(np)); 395 if (IS_ERR(regmap)) 396 return; 397 398 hw = at91sam9x5_clk_register_usb(regmap, name, parent_names, 399 num_parents); 400 if (IS_ERR(hw)) 401 return; 402 403 of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw); 404 } 405 CLK_OF_DECLARE(at91sam9x5_clk_usb, "atmel,at91sam9x5-clk-usb", 406 of_at91sam9x5_clk_usb_setup); 407 408 static void __init of_at91sam9n12_clk_usb_setup(struct device_node *np) 409 { 410 struct clk_hw *hw; 411 const char *parent_name; 412 const char *name = np->name; 413 struct regmap *regmap; 414 415 parent_name = of_clk_get_parent_name(np, 0); 416 if (!parent_name) 417 return; 418 419 of_property_read_string(np, "clock-output-names", &name); 420 421 regmap = syscon_node_to_regmap(of_get_parent(np)); 422 if (IS_ERR(regmap)) 423 return; 424 425 hw = at91sam9n12_clk_register_usb(regmap, name, parent_name); 426 if (IS_ERR(hw)) 427 return; 428 429 of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw); 430 } 431 CLK_OF_DECLARE(at91sam9n12_clk_usb, "atmel,at91sam9n12-clk-usb", 432 of_at91sam9n12_clk_usb_setup); 433 434 static void __init of_at91rm9200_clk_usb_setup(struct device_node *np) 435 { 436 struct clk_hw *hw; 437 const char *parent_name; 438 const char *name = np->name; 439 u32 divisors[4] = {0, 0, 0, 0}; 440 struct regmap *regmap; 441 442 parent_name = of_clk_get_parent_name(np, 0); 443 if (!parent_name) 444 return; 445 446 of_property_read_u32_array(np, "atmel,clk-divisors", divisors, 4); 447 if (!divisors[0]) 448 return; 449 450 of_property_read_string(np, "clock-output-names", &name); 451 452 regmap = syscon_node_to_regmap(of_get_parent(np)); 453 if (IS_ERR(regmap)) 454 return; 455 hw = at91rm9200_clk_register_usb(regmap, name, parent_name, divisors); 456 if (IS_ERR(hw)) 457 return; 458 459 of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw); 460 } 461 CLK_OF_DECLARE(at91rm9200_clk_usb, "atmel,at91rm9200-clk-usb", 462 of_at91rm9200_clk_usb_setup); 463