clk-composite.c 7.97 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
/*
 * Copyright (c) 2013 NVIDIA CORPORATION.  All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <linux/clk.h>
#include <linux/clk-provider.h>
#include <linux/err.h>
#include <linux/slab.h>

#define to_clk_composite(_hw) container_of(_hw, struct clk_composite, hw)

static u8 clk_composite_get_parent(struct clk_hw *hw)
{
	struct clk_composite *composite = to_clk_composite(hw);
	const struct clk_ops *mux_ops = composite->mux_ops;
	struct clk_hw *mux_hw = composite->mux_hw;

	mux_hw->clk = hw->clk;

	return mux_ops->get_parent(mux_hw);
}

static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
{
	struct clk_composite *composite = to_clk_composite(hw);
	const struct clk_ops *mux_ops = composite->mux_ops;
	struct clk_hw *mux_hw = composite->mux_hw;

	mux_hw->clk = hw->clk;

	return mux_ops->set_parent(mux_hw, index);
}

static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
					    unsigned long parent_rate)
{
	struct clk_composite *composite = to_clk_composite(hw);
50 51
	const struct clk_ops *rate_ops = composite->rate_ops;
	struct clk_hw *rate_hw = composite->rate_hw;
52

53
	rate_hw->clk = hw->clk;
54

55
	return rate_ops->recalc_rate(rate_hw, parent_rate);
56 57
}

58
static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate,
59 60
					unsigned long min_rate,
					unsigned long max_rate,
61
					unsigned long *best_parent_rate,
62
					struct clk_hw **best_parent_p)
63 64 65 66 67 68
{
	struct clk_composite *composite = to_clk_composite(hw);
	const struct clk_ops *rate_ops = composite->rate_ops;
	const struct clk_ops *mux_ops = composite->mux_ops;
	struct clk_hw *rate_hw = composite->rate_hw;
	struct clk_hw *mux_hw = composite->mux_hw;
69 70 71 72 73 74
	struct clk *parent;
	unsigned long parent_rate;
	long tmp_rate, best_rate = 0;
	unsigned long rate_diff;
	unsigned long best_rate_diff = ULONG_MAX;
	int i;
75 76 77

	if (rate_hw && rate_ops && rate_ops->determine_rate) {
		rate_hw->clk = hw->clk;
78 79 80
		return rate_ops->determine_rate(rate_hw, rate, min_rate,
						max_rate,
						best_parent_rate,
81
						best_parent_p);
82 83 84 85 86
	} else if (rate_hw && rate_ops && rate_ops->round_rate &&
		   mux_hw && mux_ops && mux_ops->set_parent) {
		*best_parent_p = NULL;

		if (__clk_get_flags(hw->clk) & CLK_SET_RATE_NO_REPARENT) {
87 88 89
			parent = clk_get_parent(mux_hw->clk);
			*best_parent_p = __clk_get_hw(parent);
			*best_parent_rate = __clk_get_rate(parent);
90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110

			return rate_ops->round_rate(rate_hw, rate,
						    best_parent_rate);
		}

		for (i = 0; i < __clk_get_num_parents(mux_hw->clk); i++) {
			parent = clk_get_parent_by_index(mux_hw->clk, i);
			if (!parent)
				continue;

			parent_rate = __clk_get_rate(parent);

			tmp_rate = rate_ops->round_rate(rate_hw, rate,
							&parent_rate);
			if (tmp_rate < 0)
				continue;

			rate_diff = abs(rate - tmp_rate);

			if (!rate_diff || !*best_parent_p
				       || best_rate_diff > rate_diff) {
111
				*best_parent_p = __clk_get_hw(parent);
112 113 114 115 116 117 118 119 120 121
				*best_parent_rate = parent_rate;
				best_rate_diff = rate_diff;
				best_rate = tmp_rate;
			}

			if (!rate_diff)
				return rate;
		}

		return best_rate;
122 123
	} else if (mux_hw && mux_ops && mux_ops->determine_rate) {
		mux_hw->clk = hw->clk;
124 125
		return mux_ops->determine_rate(mux_hw, rate, min_rate,
					       max_rate, best_parent_rate,
126 127 128 129 130 131 132
					       best_parent_p);
	} else {
		pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
		return 0;
	}
}

133 134 135 136
static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
				  unsigned long *prate)
{
	struct clk_composite *composite = to_clk_composite(hw);
137 138
	const struct clk_ops *rate_ops = composite->rate_ops;
	struct clk_hw *rate_hw = composite->rate_hw;
139

140
	rate_hw->clk = hw->clk;
141

142
	return rate_ops->round_rate(rate_hw, rate, prate);
143 144 145 146 147 148
}

static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
			       unsigned long parent_rate)
{
	struct clk_composite *composite = to_clk_composite(hw);
149 150
	const struct clk_ops *rate_ops = composite->rate_ops;
	struct clk_hw *rate_hw = composite->rate_hw;
151

152
	rate_hw->clk = hw->clk;
153

154
	return rate_ops->set_rate(rate_hw, rate, parent_rate);
155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192
}

static int clk_composite_is_enabled(struct clk_hw *hw)
{
	struct clk_composite *composite = to_clk_composite(hw);
	const struct clk_ops *gate_ops = composite->gate_ops;
	struct clk_hw *gate_hw = composite->gate_hw;

	gate_hw->clk = hw->clk;

	return gate_ops->is_enabled(gate_hw);
}

static int clk_composite_enable(struct clk_hw *hw)
{
	struct clk_composite *composite = to_clk_composite(hw);
	const struct clk_ops *gate_ops = composite->gate_ops;
	struct clk_hw *gate_hw = composite->gate_hw;

	gate_hw->clk = hw->clk;

	return gate_ops->enable(gate_hw);
}

static void clk_composite_disable(struct clk_hw *hw)
{
	struct clk_composite *composite = to_clk_composite(hw);
	const struct clk_ops *gate_ops = composite->gate_ops;
	struct clk_hw *gate_hw = composite->gate_hw;

	gate_hw->clk = hw->clk;

	gate_ops->disable(gate_hw);
}

struct clk *clk_register_composite(struct device *dev, const char *name,
			const char **parent_names, int num_parents,
			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
193
			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215
			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
			unsigned long flags)
{
	struct clk *clk;
	struct clk_init_data init;
	struct clk_composite *composite;
	struct clk_ops *clk_composite_ops;

	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
	if (!composite) {
		pr_err("%s: could not allocate composite clk\n", __func__);
		return ERR_PTR(-ENOMEM);
	}

	init.name = name;
	init.flags = flags | CLK_IS_BASIC;
	init.parent_names = parent_names;
	init.num_parents = num_parents;

	clk_composite_ops = &composite->ops;

	if (mux_hw && mux_ops) {
216
		if (!mux_ops->get_parent) {
217 218 219 220 221 222 223
			clk = ERR_PTR(-EINVAL);
			goto err;
		}

		composite->mux_hw = mux_hw;
		composite->mux_ops = mux_ops;
		clk_composite_ops->get_parent = clk_composite_get_parent;
224 225
		if (mux_ops->set_parent)
			clk_composite_ops->set_parent = clk_composite_set_parent;
226 227
		if (mux_ops->determine_rate)
			clk_composite_ops->determine_rate = clk_composite_determine_rate;
228 229
	}

230
	if (rate_hw && rate_ops) {
231
		if (!rate_ops->recalc_rate) {
232 233 234
			clk = ERR_PTR(-EINVAL);
			goto err;
		}
235
		clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
236

237 238 239 240 241 242 243 244 245 246 247 248 249 250 251
		if (rate_ops->determine_rate)
			clk_composite_ops->determine_rate =
				clk_composite_determine_rate;
		else if (rate_ops->round_rate)
			clk_composite_ops->round_rate =
				clk_composite_round_rate;

		/* .set_rate requires either .round_rate or .determine_rate */
		if (rate_ops->set_rate) {
			if (rate_ops->determine_rate || rate_ops->round_rate)
				clk_composite_ops->set_rate =
						clk_composite_set_rate;
			else
				WARN(1, "%s: missing round_rate op is required\n",
						__func__);
252 253
		}

254 255
		composite->rate_hw = rate_hw;
		composite->rate_ops = rate_ops;
256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281
	}

	if (gate_hw && gate_ops) {
		if (!gate_ops->is_enabled || !gate_ops->enable ||
		    !gate_ops->disable) {
			clk = ERR_PTR(-EINVAL);
			goto err;
		}

		composite->gate_hw = gate_hw;
		composite->gate_ops = gate_ops;
		clk_composite_ops->is_enabled = clk_composite_is_enabled;
		clk_composite_ops->enable = clk_composite_enable;
		clk_composite_ops->disable = clk_composite_disable;
	}

	init.ops = clk_composite_ops;
	composite->hw.init = &init;

	clk = clk_register(dev, &composite->hw);
	if (IS_ERR(clk))
		goto err;

	if (composite->mux_hw)
		composite->mux_hw->clk = clk;

282 283
	if (composite->rate_hw)
		composite->rate_hw->clk = clk;
284 285 286 287 288 289 290 291 292 293

	if (composite->gate_hw)
		composite->gate_hw->clk = clk;

	return clk;

err:
	kfree(composite);
	return clk;
}