123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422 |
- // SPDX-License-Identifier: GPL-2.0-or-later
- /*
- * Copyright (C) 2013 Boris BREZILLON <[email protected]>
- */
- #include <linux/clk-provider.h>
- #include <linux/clkdev.h>
- #include <linux/clk/at91_pmc.h>
- #include <linux/of.h>
- #include <linux/mfd/syscon.h>
- #include <linux/regmap.h>
- #include "pmc.h"
- #define SAM9X5_USB_DIV_SHIFT 8
- #define SAM9X5_USB_MAX_DIV 0xf
- #define RM9200_USB_DIV_SHIFT 28
- #define RM9200_USB_DIV_TAB_SIZE 4
- #define SAM9X5_USBS_MASK GENMASK(0, 0)
- #define SAM9X60_USBS_MASK GENMASK(1, 0)
- struct at91sam9x5_clk_usb {
- struct clk_hw hw;
- struct regmap *regmap;
- struct at91_clk_pms pms;
- u32 usbs_mask;
- u8 num_parents;
- };
- #define to_at91sam9x5_clk_usb(hw) \
- container_of(hw, struct at91sam9x5_clk_usb, hw)
- struct at91rm9200_clk_usb {
- struct clk_hw hw;
- struct regmap *regmap;
- u32 divisors[4];
- };
- #define to_at91rm9200_clk_usb(hw) \
- container_of(hw, struct at91rm9200_clk_usb, hw)
- static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw,
- unsigned long parent_rate)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- unsigned int usbr;
- u8 usbdiv;
- regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
- usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT;
- return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1));
- }
- static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw,
- struct clk_rate_request *req)
- {
- struct clk_hw *parent;
- long best_rate = -EINVAL;
- unsigned long tmp_rate;
- int best_diff = -1;
- int tmp_diff;
- int i;
- for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
- int div;
- parent = clk_hw_get_parent_by_index(hw, i);
- if (!parent)
- continue;
- for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) {
- unsigned long tmp_parent_rate;
- tmp_parent_rate = req->rate * div;
- tmp_parent_rate = clk_hw_round_rate(parent,
- tmp_parent_rate);
- if (!tmp_parent_rate)
- continue;
- tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div);
- if (tmp_rate < req->rate)
- tmp_diff = req->rate - tmp_rate;
- else
- tmp_diff = tmp_rate - req->rate;
- if (best_diff < 0 || best_diff > tmp_diff) {
- best_rate = tmp_rate;
- best_diff = tmp_diff;
- req->best_parent_rate = tmp_parent_rate;
- req->best_parent_hw = parent;
- }
- if (!best_diff || tmp_rate < req->rate)
- break;
- }
- if (!best_diff)
- break;
- }
- if (best_rate < 0)
- return best_rate;
- req->rate = best_rate;
- return 0;
- }
- static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- if (index >= usb->num_parents)
- return -EINVAL;
- regmap_update_bits(usb->regmap, AT91_PMC_USB, usb->usbs_mask, index);
- return 0;
- }
- static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- unsigned int usbr;
- regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
- return usbr & usb->usbs_mask;
- }
- static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
- unsigned long parent_rate)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- unsigned long div;
- if (!rate)
- return -EINVAL;
- div = DIV_ROUND_CLOSEST(parent_rate, rate);
- if (div > SAM9X5_USB_MAX_DIV + 1 || !div)
- return -EINVAL;
- regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV,
- (div - 1) << SAM9X5_USB_DIV_SHIFT);
- return 0;
- }
- static int at91sam9x5_usb_save_context(struct clk_hw *hw)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- struct clk_hw *parent_hw = clk_hw_get_parent(hw);
- usb->pms.parent = at91sam9x5_clk_usb_get_parent(hw);
- usb->pms.parent_rate = clk_hw_get_rate(parent_hw);
- usb->pms.rate = at91sam9x5_clk_usb_recalc_rate(hw, usb->pms.parent_rate);
- return 0;
- }
- static void at91sam9x5_usb_restore_context(struct clk_hw *hw)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- int ret;
- ret = at91sam9x5_clk_usb_set_parent(hw, usb->pms.parent);
- if (ret)
- return;
- at91sam9x5_clk_usb_set_rate(hw, usb->pms.rate, usb->pms.parent_rate);
- }
- static const struct clk_ops at91sam9x5_usb_ops = {
- .recalc_rate = at91sam9x5_clk_usb_recalc_rate,
- .determine_rate = at91sam9x5_clk_usb_determine_rate,
- .get_parent = at91sam9x5_clk_usb_get_parent,
- .set_parent = at91sam9x5_clk_usb_set_parent,
- .set_rate = at91sam9x5_clk_usb_set_rate,
- .save_context = at91sam9x5_usb_save_context,
- .restore_context = at91sam9x5_usb_restore_context,
- };
- static int at91sam9n12_clk_usb_enable(struct clk_hw *hw)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS,
- AT91_PMC_USBS);
- return 0;
- }
- static void at91sam9n12_clk_usb_disable(struct clk_hw *hw)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 0);
- }
- static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw)
- {
- struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
- unsigned int usbr;
- regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
- return usbr & AT91_PMC_USBS;
- }
- static const struct clk_ops at91sam9n12_usb_ops = {
- .enable = at91sam9n12_clk_usb_enable,
- .disable = at91sam9n12_clk_usb_disable,
- .is_enabled = at91sam9n12_clk_usb_is_enabled,
- .recalc_rate = at91sam9x5_clk_usb_recalc_rate,
- .determine_rate = at91sam9x5_clk_usb_determine_rate,
- .set_rate = at91sam9x5_clk_usb_set_rate,
- };
- static struct clk_hw * __init
- _at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
- const char **parent_names, u8 num_parents,
- u32 usbs_mask)
- {
- struct at91sam9x5_clk_usb *usb;
- struct clk_hw *hw;
- struct clk_init_data init;
- int ret;
- usb = kzalloc(sizeof(*usb), GFP_KERNEL);
- if (!usb)
- return ERR_PTR(-ENOMEM);
- init.name = name;
- init.ops = &at91sam9x5_usb_ops;
- init.parent_names = parent_names;
- init.num_parents = num_parents;
- init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
- CLK_SET_RATE_PARENT;
- usb->hw.init = &init;
- usb->regmap = regmap;
- usb->usbs_mask = usbs_mask;
- usb->num_parents = num_parents;
- hw = &usb->hw;
- ret = clk_hw_register(NULL, &usb->hw);
- if (ret) {
- kfree(usb);
- hw = ERR_PTR(ret);
- }
- return hw;
- }
- struct clk_hw * __init
- at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
- const char **parent_names, u8 num_parents)
- {
- return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
- num_parents, SAM9X5_USBS_MASK);
- }
- struct clk_hw * __init
- sam9x60_clk_register_usb(struct regmap *regmap, const char *name,
- const char **parent_names, u8 num_parents)
- {
- return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
- num_parents, SAM9X60_USBS_MASK);
- }
- struct clk_hw * __init
- at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name,
- const char *parent_name)
- {
- struct at91sam9x5_clk_usb *usb;
- struct clk_hw *hw;
- struct clk_init_data init;
- int ret;
- usb = kzalloc(sizeof(*usb), GFP_KERNEL);
- if (!usb)
- return ERR_PTR(-ENOMEM);
- init.name = name;
- init.ops = &at91sam9n12_usb_ops;
- init.parent_names = &parent_name;
- init.num_parents = 1;
- init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT;
- usb->hw.init = &init;
- usb->regmap = regmap;
- hw = &usb->hw;
- ret = clk_hw_register(NULL, &usb->hw);
- if (ret) {
- kfree(usb);
- hw = ERR_PTR(ret);
- }
- return hw;
- }
- static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw,
- unsigned long parent_rate)
- {
- struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
- unsigned int pllbr;
- u8 usbdiv;
- regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr);
- usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT;
- if (usb->divisors[usbdiv])
- return parent_rate / usb->divisors[usbdiv];
- return 0;
- }
- static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,
- unsigned long *parent_rate)
- {
- struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
- struct clk_hw *parent = clk_hw_get_parent(hw);
- unsigned long bestrate = 0;
- int bestdiff = -1;
- unsigned long tmprate;
- int tmpdiff;
- int i = 0;
- for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
- unsigned long tmp_parent_rate;
- if (!usb->divisors[i])
- continue;
- tmp_parent_rate = rate * usb->divisors[i];
- tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate);
- tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]);
- if (tmprate < rate)
- tmpdiff = rate - tmprate;
- else
- tmpdiff = tmprate - rate;
- if (bestdiff < 0 || bestdiff > tmpdiff) {
- bestrate = tmprate;
- bestdiff = tmpdiff;
- *parent_rate = tmp_parent_rate;
- }
- if (!bestdiff)
- break;
- }
- return bestrate;
- }
- static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
- unsigned long parent_rate)
- {
- int i;
- struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
- unsigned long div;
- if (!rate)
- return -EINVAL;
- div = DIV_ROUND_CLOSEST(parent_rate, rate);
- for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
- if (usb->divisors[i] == div) {
- regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR,
- AT91_PMC_USBDIV,
- i << RM9200_USB_DIV_SHIFT);
- return 0;
- }
- }
- return -EINVAL;
- }
- static const struct clk_ops at91rm9200_usb_ops = {
- .recalc_rate = at91rm9200_clk_usb_recalc_rate,
- .round_rate = at91rm9200_clk_usb_round_rate,
- .set_rate = at91rm9200_clk_usb_set_rate,
- };
- struct clk_hw * __init
- at91rm9200_clk_register_usb(struct regmap *regmap, const char *name,
- const char *parent_name, const u32 *divisors)
- {
- struct at91rm9200_clk_usb *usb;
- struct clk_hw *hw;
- struct clk_init_data init;
- int ret;
- usb = kzalloc(sizeof(*usb), GFP_KERNEL);
- if (!usb)
- return ERR_PTR(-ENOMEM);
- init.name = name;
- init.ops = &at91rm9200_usb_ops;
- init.parent_names = &parent_name;
- init.num_parents = 1;
- init.flags = CLK_SET_RATE_PARENT;
- usb->hw.init = &init;
- usb->regmap = regmap;
- memcpy(usb->divisors, divisors, sizeof(usb->divisors));
- hw = &usb->hw;
- ret = clk_hw_register(NULL, &usb->hw);
- if (ret) {
- kfree(usb);
- hw = ERR_PTR(ret);
- }
- return hw;
- }
|