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 int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw, 60 struct clk_rate_request *req) 61 { 62 struct clk_hw *parent; 63 long best_rate = -EINVAL; 64 unsigned long tmp_rate; 65 int best_diff = -1; 66 int tmp_diff; 67 int i; 68 69 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 70 int div; 71 72 parent = clk_hw_get_parent_by_index(hw, i); 73 if (!parent) 74 continue; 75 76 for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) { 77 unsigned long tmp_parent_rate; 78 79 tmp_parent_rate = req->rate * div; 80 tmp_parent_rate = clk_hw_round_rate(parent, 81 tmp_parent_rate); 82 tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div); 83 if (tmp_rate < req->rate) 84 tmp_diff = req->rate - tmp_rate; 85 else 86 tmp_diff = tmp_rate - req->rate; 87 88 if (best_diff < 0 || best_diff > tmp_diff) { 89 best_rate = tmp_rate; 90 best_diff = tmp_diff; 91 req->best_parent_rate = tmp_parent_rate; 92 req->best_parent_hw = parent; 93 } 94 95 if (!best_diff || tmp_rate < req->rate) 96 break; 97 } 98 99 if (!best_diff) 100 break; 101 } 102 103 if (best_rate < 0) 104 return best_rate; 105 106 req->rate = best_rate; 107 return 0; 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_hw *parent = clk_hw_get_parent(hw); 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_hw_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 num_parents; 377 const char *parent_names[USB_SOURCE_MAX]; 378 const char *name = np->name; 379 380 num_parents = of_clk_get_parent_count(np); 381 if (num_parents <= 0 || num_parents > USB_SOURCE_MAX) 382 return; 383 384 of_clk_parent_fill(np, parent_names, num_parents); 385 386 of_property_read_string(np, "clock-output-names", &name); 387 388 clk = at91sam9x5_clk_register_usb(pmc, name, parent_names, num_parents); 389 if (IS_ERR(clk)) 390 return; 391 392 of_clk_add_provider(np, of_clk_src_simple_get, clk); 393 } 394 395 void __init of_at91sam9n12_clk_usb_setup(struct device_node *np, 396 struct at91_pmc *pmc) 397 { 398 struct clk *clk; 399 const char *parent_name; 400 const char *name = np->name; 401 402 parent_name = of_clk_get_parent_name(np, 0); 403 if (!parent_name) 404 return; 405 406 of_property_read_string(np, "clock-output-names", &name); 407 408 clk = at91sam9n12_clk_register_usb(pmc, name, parent_name); 409 if (IS_ERR(clk)) 410 return; 411 412 of_clk_add_provider(np, of_clk_src_simple_get, clk); 413 } 414 415 void __init of_at91rm9200_clk_usb_setup(struct device_node *np, 416 struct at91_pmc *pmc) 417 { 418 struct clk *clk; 419 const char *parent_name; 420 const char *name = np->name; 421 u32 divisors[4] = {0, 0, 0, 0}; 422 423 parent_name = of_clk_get_parent_name(np, 0); 424 if (!parent_name) 425 return; 426 427 of_property_read_u32_array(np, "atmel,clk-divisors", divisors, 4); 428 if (!divisors[0]) 429 return; 430 431 of_property_read_string(np, "clock-output-names", &name); 432 433 clk = at91rm9200_clk_register_usb(pmc, name, parent_name, divisors); 434 if (IS_ERR(clk)) 435 return; 436 437 of_clk_add_provider(np, of_clk_src_simple_get, clk); 438 } 439