232 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (c) 2017, Linaro Limited4 * Author: Georgi Djakov <georgi.djakov@linaro.org>5 */6 7#include <linux/bitops.h>8#include <linux/delay.h>9#include <linux/kernel.h>10#include <linux/regmap.h>11 12#include "clk-regmap-mux-div.h"13 14#define CMD_RCGR 0x015#define CMD_RCGR_UPDATE BIT(0)16#define CMD_RCGR_DIRTY_CFG BIT(4)17#define CMD_RCGR_ROOT_OFF BIT(31)18#define CFG_RCGR 0x419 20#define to_clk_regmap_mux_div(_hw) \21 container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr)22 23int mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div)24{25 int ret, count;26 u32 val, mask;27 const char *name = clk_hw_get_name(&md->clkr.hw);28 29 val = (div << md->hid_shift) | (src << md->src_shift);30 mask = ((BIT(md->hid_width) - 1) << md->hid_shift) |31 ((BIT(md->src_width) - 1) << md->src_shift);32 33 ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset,34 mask, val);35 if (ret)36 return ret;37 38 ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset,39 CMD_RCGR_UPDATE, CMD_RCGR_UPDATE);40 if (ret)41 return ret;42 43 /* Wait for update to take effect */44 for (count = 500; count > 0; count--) {45 ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset,46 &val);47 if (ret)48 return ret;49 if (!(val & CMD_RCGR_UPDATE))50 return 0;51 udelay(1);52 }53 54 pr_err("%s: RCG did not update its configuration", name);55 return -EBUSY;56}57EXPORT_SYMBOL_GPL(mux_div_set_src_div);58 59static void mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src,60 u32 *div)61{62 u32 val, d, s;63 const char *name = clk_hw_get_name(&md->clkr.hw);64 65 regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val);66 67 if (val & CMD_RCGR_DIRTY_CFG) {68 pr_err("%s: RCG configuration is pending\n", name);69 return;70 }71 72 regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val);73 s = (val >> md->src_shift);74 s &= BIT(md->src_width) - 1;75 *src = s;76 77 d = (val >> md->hid_shift);78 d &= BIT(md->hid_width) - 1;79 *div = d;80}81 82static inline bool is_better_rate(unsigned long req, unsigned long best,83 unsigned long new)84{85 return (req <= new && new < best) || (best < req && best < new);86}87 88static int mux_div_determine_rate(struct clk_hw *hw,89 struct clk_rate_request *req)90{91 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);92 unsigned int i, div, max_div;93 unsigned long actual_rate, best_rate = 0;94 unsigned long req_rate = req->rate;95 96 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {97 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);98 unsigned long parent_rate = clk_hw_get_rate(parent);99 100 max_div = BIT(md->hid_width) - 1;101 for (div = 1; div < max_div; div++) {102 parent_rate = mult_frac(req_rate, div, 2);103 parent_rate = clk_hw_round_rate(parent, parent_rate);104 actual_rate = mult_frac(parent_rate, 2, div);105 106 if (is_better_rate(req_rate, best_rate, actual_rate)) {107 best_rate = actual_rate;108 req->rate = best_rate;109 req->best_parent_rate = parent_rate;110 req->best_parent_hw = parent;111 }112 113 if (actual_rate < req_rate || best_rate <= req_rate)114 break;115 }116 }117 118 if (!best_rate)119 return -EINVAL;120 121 return 0;122}123 124static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,125 unsigned long prate, u32 src)126{127 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);128 int ret;129 u32 div, max_div, best_src = 0, best_div = 0;130 unsigned int i;131 unsigned long actual_rate, best_rate = 0;132 133 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {134 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);135 unsigned long parent_rate = clk_hw_get_rate(parent);136 137 max_div = BIT(md->hid_width) - 1;138 for (div = 1; div < max_div; div++) {139 parent_rate = mult_frac(rate, div, 2);140 parent_rate = clk_hw_round_rate(parent, parent_rate);141 actual_rate = mult_frac(parent_rate, 2, div);142 143 if (is_better_rate(rate, best_rate, actual_rate)) {144 best_rate = actual_rate;145 best_src = md->parent_map[i];146 best_div = div - 1;147 }148 149 if (actual_rate < rate || best_rate <= rate)150 break;151 }152 }153 154 ret = mux_div_set_src_div(md, best_src, best_div);155 if (!ret) {156 md->div = best_div;157 md->src = best_src;158 }159 160 return ret;161}162 163static u8 mux_div_get_parent(struct clk_hw *hw)164{165 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);166 const char *name = clk_hw_get_name(hw);167 u32 i, div, src = 0;168 169 mux_div_get_src_div(md, &src, &div);170 171 for (i = 0; i < clk_hw_get_num_parents(hw); i++)172 if (src == md->parent_map[i])173 return i;174 175 pr_err("%s: Can't find parent with src %d\n", name, src);176 return 0;177}178 179static int mux_div_set_parent(struct clk_hw *hw, u8 index)180{181 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);182 183 return mux_div_set_src_div(md, md->parent_map[index], md->div);184}185 186static int mux_div_set_rate(struct clk_hw *hw,187 unsigned long rate, unsigned long prate)188{189 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);190 191 return __mux_div_set_rate_and_parent(hw, rate, prate, md->src);192}193 194static int mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,195 unsigned long prate, u8 index)196{197 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);198 199 return __mux_div_set_rate_and_parent(hw, rate, prate,200 md->parent_map[index]);201}202 203static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate)204{205 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);206 u32 div, src;207 int i, num_parents = clk_hw_get_num_parents(hw);208 const char *name = clk_hw_get_name(hw);209 210 mux_div_get_src_div(md, &src, &div);211 for (i = 0; i < num_parents; i++)212 if (src == md->parent_map[i]) {213 struct clk_hw *p = clk_hw_get_parent_by_index(hw, i);214 unsigned long parent_rate = clk_hw_get_rate(p);215 216 return mult_frac(parent_rate, 2, div + 1);217 }218 219 pr_err("%s: Can't find parent %d\n", name, src);220 return 0;221}222 223const struct clk_ops clk_regmap_mux_div_ops = {224 .get_parent = mux_div_get_parent,225 .set_parent = mux_div_set_parent,226 .set_rate = mux_div_set_rate,227 .set_rate_and_parent = mux_div_set_rate_and_parent,228 .determine_rate = mux_div_determine_rate,229 .recalc_rate = mux_div_recalc_rate,230};231EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops);232