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