423 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>4 */5 6#include <linux/clk-provider.h>7#include <linux/clkdev.h>8#include <linux/clk/at91_pmc.h>9#include <linux/of.h>10#include <linux/mfd/syscon.h>11#include <linux/regmap.h>12 13#include "pmc.h"14 15#define SAM9X5_USB_DIV_SHIFT 816#define SAM9X5_USB_MAX_DIV 0xf17 18#define RM9200_USB_DIV_SHIFT 2819#define RM9200_USB_DIV_TAB_SIZE 420 21#define SAM9X5_USBS_MASK GENMASK(0, 0)22#define SAM9X60_USBS_MASK GENMASK(1, 0)23 24struct at91sam9x5_clk_usb {25 struct clk_hw hw;26 struct regmap *regmap;27 struct at91_clk_pms pms;28 u32 usbs_mask;29 u8 num_parents;30};31 32#define to_at91sam9x5_clk_usb(hw) \33 container_of(hw, struct at91sam9x5_clk_usb, hw)34 35struct at91rm9200_clk_usb {36 struct clk_hw hw;37 struct regmap *regmap;38 u32 divisors[4];39};40 41#define to_at91rm9200_clk_usb(hw) \42 container_of(hw, struct at91rm9200_clk_usb, hw)43 44static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw,45 unsigned long parent_rate)46{47 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);48 unsigned int usbr;49 u8 usbdiv;50 51 regmap_read(usb->regmap, AT91_PMC_USB, &usbr);52 usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT;53 54 return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1));55}56 57static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw,58 struct clk_rate_request *req)59{60 struct clk_hw *parent;61 long best_rate = -EINVAL;62 unsigned long tmp_rate;63 int best_diff = -1;64 int tmp_diff;65 int i;66 67 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {68 int div;69 70 parent = clk_hw_get_parent_by_index(hw, i);71 if (!parent)72 continue;73 74 for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) {75 unsigned long tmp_parent_rate;76 77 tmp_parent_rate = req->rate * div;78 tmp_parent_rate = clk_hw_round_rate(parent,79 tmp_parent_rate);80 if (!tmp_parent_rate)81 continue;82 83 tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div);84 if (tmp_rate < req->rate)85 tmp_diff = req->rate - tmp_rate;86 else87 tmp_diff = tmp_rate - req->rate;88 89 if (best_diff < 0 || best_diff > tmp_diff) {90 best_rate = tmp_rate;91 best_diff = tmp_diff;92 req->best_parent_rate = tmp_parent_rate;93 req->best_parent_hw = parent;94 }95 96 if (!best_diff || tmp_rate < req->rate)97 break;98 }99 100 if (!best_diff)101 break;102 }103 104 if (best_rate < 0)105 return best_rate;106 107 req->rate = best_rate;108 return 0;109}110 111static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index)112{113 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);114 115 if (index >= usb->num_parents)116 return -EINVAL;117 118 regmap_update_bits(usb->regmap, AT91_PMC_USB, usb->usbs_mask, index);119 120 return 0;121}122 123static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw)124{125 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);126 unsigned int usbr;127 128 regmap_read(usb->regmap, AT91_PMC_USB, &usbr);129 130 return usbr & usb->usbs_mask;131}132 133static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,134 unsigned long parent_rate)135{136 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);137 unsigned long div;138 139 if (!rate)140 return -EINVAL;141 142 div = DIV_ROUND_CLOSEST(parent_rate, rate);143 if (div > SAM9X5_USB_MAX_DIV + 1 || !div)144 return -EINVAL;145 146 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV,147 (div - 1) << SAM9X5_USB_DIV_SHIFT);148 149 return 0;150}151 152static int at91sam9x5_usb_save_context(struct clk_hw *hw)153{154 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);155 struct clk_hw *parent_hw = clk_hw_get_parent(hw);156 157 usb->pms.parent = at91sam9x5_clk_usb_get_parent(hw);158 usb->pms.parent_rate = clk_hw_get_rate(parent_hw);159 usb->pms.rate = at91sam9x5_clk_usb_recalc_rate(hw, usb->pms.parent_rate);160 161 return 0;162}163 164static void at91sam9x5_usb_restore_context(struct clk_hw *hw)165{166 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);167 int ret;168 169 ret = at91sam9x5_clk_usb_set_parent(hw, usb->pms.parent);170 if (ret)171 return;172 173 at91sam9x5_clk_usb_set_rate(hw, usb->pms.rate, usb->pms.parent_rate);174}175 176static const struct clk_ops at91sam9x5_usb_ops = {177 .recalc_rate = at91sam9x5_clk_usb_recalc_rate,178 .determine_rate = at91sam9x5_clk_usb_determine_rate,179 .get_parent = at91sam9x5_clk_usb_get_parent,180 .set_parent = at91sam9x5_clk_usb_set_parent,181 .set_rate = at91sam9x5_clk_usb_set_rate,182 .save_context = at91sam9x5_usb_save_context,183 .restore_context = at91sam9x5_usb_restore_context,184};185 186static int at91sam9n12_clk_usb_enable(struct clk_hw *hw)187{188 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);189 190 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS,191 AT91_PMC_USBS);192 193 return 0;194}195 196static void at91sam9n12_clk_usb_disable(struct clk_hw *hw)197{198 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);199 200 regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 0);201}202 203static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw)204{205 struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);206 unsigned int usbr;207 208 regmap_read(usb->regmap, AT91_PMC_USB, &usbr);209 210 return usbr & AT91_PMC_USBS;211}212 213static const struct clk_ops at91sam9n12_usb_ops = {214 .enable = at91sam9n12_clk_usb_enable,215 .disable = at91sam9n12_clk_usb_disable,216 .is_enabled = at91sam9n12_clk_usb_is_enabled,217 .recalc_rate = at91sam9x5_clk_usb_recalc_rate,218 .determine_rate = at91sam9x5_clk_usb_determine_rate,219 .set_rate = at91sam9x5_clk_usb_set_rate,220};221 222static struct clk_hw * __init223_at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,224 const char **parent_names, u8 num_parents,225 u32 usbs_mask)226{227 struct at91sam9x5_clk_usb *usb;228 struct clk_hw *hw;229 struct clk_init_data init;230 int ret;231 232 usb = kzalloc(sizeof(*usb), GFP_KERNEL);233 if (!usb)234 return ERR_PTR(-ENOMEM);235 236 init.name = name;237 init.ops = &at91sam9x5_usb_ops;238 init.parent_names = parent_names;239 init.num_parents = num_parents;240 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |241 CLK_SET_RATE_PARENT;242 243 usb->hw.init = &init;244 usb->regmap = regmap;245 usb->usbs_mask = usbs_mask;246 usb->num_parents = num_parents;247 248 hw = &usb->hw;249 ret = clk_hw_register(NULL, &usb->hw);250 if (ret) {251 kfree(usb);252 hw = ERR_PTR(ret);253 }254 255 return hw;256}257 258struct clk_hw * __init259at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,260 const char **parent_names, u8 num_parents)261{262 return _at91sam9x5_clk_register_usb(regmap, name, parent_names,263 num_parents, SAM9X5_USBS_MASK);264}265 266struct clk_hw * __init267sam9x60_clk_register_usb(struct regmap *regmap, const char *name,268 const char **parent_names, u8 num_parents)269{270 return _at91sam9x5_clk_register_usb(regmap, name, parent_names,271 num_parents, SAM9X60_USBS_MASK);272}273 274struct clk_hw * __init275at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name,276 const char *parent_name)277{278 struct at91sam9x5_clk_usb *usb;279 struct clk_hw *hw;280 struct clk_init_data init;281 int ret;282 283 usb = kzalloc(sizeof(*usb), GFP_KERNEL);284 if (!usb)285 return ERR_PTR(-ENOMEM);286 287 init.name = name;288 init.ops = &at91sam9n12_usb_ops;289 init.parent_names = &parent_name;290 init.num_parents = 1;291 init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT;292 293 usb->hw.init = &init;294 usb->regmap = regmap;295 296 hw = &usb->hw;297 ret = clk_hw_register(NULL, &usb->hw);298 if (ret) {299 kfree(usb);300 hw = ERR_PTR(ret);301 }302 303 return hw;304}305 306static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw,307 unsigned long parent_rate)308{309 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);310 unsigned int pllbr;311 u8 usbdiv;312 313 regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr);314 315 usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT;316 if (usb->divisors[usbdiv])317 return parent_rate / usb->divisors[usbdiv];318 319 return 0;320}321 322static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,323 unsigned long *parent_rate)324{325 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);326 struct clk_hw *parent = clk_hw_get_parent(hw);327 unsigned long bestrate = 0;328 int bestdiff = -1;329 unsigned long tmprate;330 int tmpdiff;331 int i = 0;332 333 for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {334 unsigned long tmp_parent_rate;335 336 if (!usb->divisors[i])337 continue;338 339 tmp_parent_rate = rate * usb->divisors[i];340 tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate);341 tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]);342 if (tmprate < rate)343 tmpdiff = rate - tmprate;344 else345 tmpdiff = tmprate - rate;346 347 if (bestdiff < 0 || bestdiff > tmpdiff) {348 bestrate = tmprate;349 bestdiff = tmpdiff;350 *parent_rate = tmp_parent_rate;351 }352 353 if (!bestdiff)354 break;355 }356 357 return bestrate;358}359 360static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,361 unsigned long parent_rate)362{363 int i;364 struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);365 unsigned long div;366 367 if (!rate)368 return -EINVAL;369 370 div = DIV_ROUND_CLOSEST(parent_rate, rate);371 372 for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {373 if (usb->divisors[i] == div) {374 regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR,375 AT91_PMC_USBDIV,376 i << RM9200_USB_DIV_SHIFT);377 378 return 0;379 }380 }381 382 return -EINVAL;383}384 385static const struct clk_ops at91rm9200_usb_ops = {386 .recalc_rate = at91rm9200_clk_usb_recalc_rate,387 .round_rate = at91rm9200_clk_usb_round_rate,388 .set_rate = at91rm9200_clk_usb_set_rate,389};390 391struct clk_hw * __init392at91rm9200_clk_register_usb(struct regmap *regmap, const char *name,393 const char *parent_name, const u32 *divisors)394{395 struct at91rm9200_clk_usb *usb;396 struct clk_hw *hw;397 struct clk_init_data init;398 int ret;399 400 usb = kzalloc(sizeof(*usb), GFP_KERNEL);401 if (!usb)402 return ERR_PTR(-ENOMEM);403 404 init.name = name;405 init.ops = &at91rm9200_usb_ops;406 init.parent_names = &parent_name;407 init.num_parents = 1;408 init.flags = CLK_SET_RATE_PARENT;409 410 usb->hw.init = &init;411 usb->regmap = regmap;412 memcpy(usb->divisors, divisors, sizeof(usb->divisors));413 414 hw = &usb->hw;415 ret = clk_hw_register(NULL, &usb->hw);416 if (ret) {417 kfree(usb);418 hw = ERR_PTR(ret);419 }420 421 return hw;422}423