505 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * mmp mix(div and mux) clock operation source file4 *5 * Copyright (C) 2014 Marvell6 * Chao Xie <chao.xie@marvell.com>7 */8 9#include <linux/clk-provider.h>10#include <linux/slab.h>11#include <linux/io.h>12#include <linux/err.h>13 14#include "clk.h"15 16/*17 * The mix clock is a clock combined mux and div type clock.18 * Because the div field and mux field need to be set at same19 * time, we can not divide it into 2 types of clock20 */21 22#define to_clk_mix(hw) container_of(hw, struct mmp_clk_mix, hw)23 24static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)25{26 unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;27 unsigned int maxdiv = 0;28 struct clk_div_table *clkt;29 30 if (mix->div_flags & CLK_DIVIDER_ONE_BASED)31 return div_mask;32 if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)33 return 1 << div_mask;34 if (mix->div_table) {35 for (clkt = mix->div_table; clkt->div; clkt++)36 if (clkt->div > maxdiv)37 maxdiv = clkt->div;38 return maxdiv;39 }40 return div_mask + 1;41}42 43static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)44{45 struct clk_div_table *clkt;46 47 if (mix->div_flags & CLK_DIVIDER_ONE_BASED)48 return val;49 if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)50 return 1 << val;51 if (mix->div_table) {52 for (clkt = mix->div_table; clkt->div; clkt++)53 if (clkt->val == val)54 return clkt->div;55 if (clkt->div == 0)56 return 0;57 }58 return val + 1;59}60 61static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)62{63 int num_parents = clk_hw_get_num_parents(&mix->hw);64 int i;65 66 if (mix->mux_flags & CLK_MUX_INDEX_BIT)67 return ffs(val) - 1;68 if (mix->mux_flags & CLK_MUX_INDEX_ONE)69 return val - 1;70 if (mix->mux_table) {71 for (i = 0; i < num_parents; i++)72 if (mix->mux_table[i] == val)73 return i;74 if (i == num_parents)75 return 0;76 }77 78 return val;79}80static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)81{82 struct clk_div_table *clkt;83 84 if (mix->div_flags & CLK_DIVIDER_ONE_BASED)85 return div;86 if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)87 return __ffs(div);88 if (mix->div_table) {89 for (clkt = mix->div_table; clkt->div; clkt++)90 if (clkt->div == div)91 return clkt->val;92 if (clkt->div == 0)93 return 0;94 }95 96 return div - 1;97}98 99static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)100{101 if (mix->mux_table)102 return mix->mux_table[mux];103 104 return mux;105}106 107static void _filter_clk_table(struct mmp_clk_mix *mix,108 struct mmp_clk_mix_clk_table *table,109 unsigned int table_size)110{111 int i;112 struct mmp_clk_mix_clk_table *item;113 struct clk_hw *parent, *hw;114 unsigned long parent_rate;115 116 hw = &mix->hw;117 118 for (i = 0; i < table_size; i++) {119 item = &table[i];120 parent = clk_hw_get_parent_by_index(hw, item->parent_index);121 parent_rate = clk_hw_get_rate(parent);122 if (parent_rate % item->rate) {123 item->valid = 0;124 } else {125 item->divisor = parent_rate / item->rate;126 item->valid = 1;127 }128 }129}130 131static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,132 unsigned int change_mux, unsigned int change_div)133{134 struct mmp_clk_mix_reg_info *ri = &mix->reg_info;135 u8 width, shift;136 u32 mux_div, fc_req;137 int ret, timeout = 50;138 unsigned long flags = 0;139 140 if (!change_mux && !change_div)141 return -EINVAL;142 143 if (mix->lock)144 spin_lock_irqsave(mix->lock, flags);145 146 if (mix->type == MMP_CLK_MIX_TYPE_V1147 || mix->type == MMP_CLK_MIX_TYPE_V2)148 mux_div = readl(ri->reg_clk_ctrl);149 else150 mux_div = readl(ri->reg_clk_sel);151 152 if (change_div) {153 width = ri->width_div;154 shift = ri->shift_div;155 mux_div &= ~MMP_CLK_BITS_MASK(width, shift);156 mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift);157 }158 159 if (change_mux) {160 width = ri->width_mux;161 shift = ri->shift_mux;162 mux_div &= ~MMP_CLK_BITS_MASK(width, shift);163 mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);164 }165 166 if (mix->type == MMP_CLK_MIX_TYPE_V1) {167 writel(mux_div, ri->reg_clk_ctrl);168 } else if (mix->type == MMP_CLK_MIX_TYPE_V2) {169 mux_div |= (1 << ri->bit_fc);170 writel(mux_div, ri->reg_clk_ctrl);171 172 do {173 fc_req = readl(ri->reg_clk_ctrl);174 timeout--;175 if (!(fc_req & (1 << ri->bit_fc)))176 break;177 } while (timeout);178 179 if (timeout == 0) {180 pr_err("%s:%s cannot do frequency change\n",181 __func__, clk_hw_get_name(&mix->hw));182 ret = -EBUSY;183 goto error;184 }185 } else {186 fc_req = readl(ri->reg_clk_ctrl);187 fc_req |= 1 << ri->bit_fc;188 writel(fc_req, ri->reg_clk_ctrl);189 writel(mux_div, ri->reg_clk_sel);190 fc_req &= ~(1 << ri->bit_fc);191 }192 193 ret = 0;194error:195 if (mix->lock)196 spin_unlock_irqrestore(mix->lock, flags);197 198 return ret;199}200 201static int mmp_clk_mix_determine_rate(struct clk_hw *hw,202 struct clk_rate_request *req)203{204 struct mmp_clk_mix *mix = to_clk_mix(hw);205 struct mmp_clk_mix_clk_table *item;206 struct clk_hw *parent, *parent_best;207 unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;208 unsigned long gap, gap_best;209 u32 div_val_max;210 unsigned int div;211 int i, j;212 213 214 mix_rate_best = 0;215 parent_rate_best = 0;216 gap_best = ULONG_MAX;217 parent_best = NULL;218 219 if (mix->table) {220 for (i = 0; i < mix->table_size; i++) {221 item = &mix->table[i];222 if (item->valid == 0)223 continue;224 parent = clk_hw_get_parent_by_index(hw,225 item->parent_index);226 parent_rate = clk_hw_get_rate(parent);227 mix_rate = parent_rate / item->divisor;228 gap = abs(mix_rate - req->rate);229 if (!parent_best || gap < gap_best) {230 parent_best = parent;231 parent_rate_best = parent_rate;232 mix_rate_best = mix_rate;233 gap_best = gap;234 if (gap_best == 0)235 goto found;236 }237 }238 } else {239 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {240 parent = clk_hw_get_parent_by_index(hw, i);241 parent_rate = clk_hw_get_rate(parent);242 div_val_max = _get_maxdiv(mix);243 for (j = 0; j < div_val_max; j++) {244 div = _get_div(mix, j);245 mix_rate = parent_rate / div;246 gap = abs(mix_rate - req->rate);247 if (!parent_best || gap < gap_best) {248 parent_best = parent;249 parent_rate_best = parent_rate;250 mix_rate_best = mix_rate;251 gap_best = gap;252 if (gap_best == 0)253 goto found;254 }255 }256 }257 }258 259found:260 if (!parent_best)261 return -EINVAL;262 263 req->best_parent_rate = parent_rate_best;264 req->best_parent_hw = parent_best;265 req->rate = mix_rate_best;266 267 return 0;268}269 270static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,271 unsigned long rate,272 unsigned long parent_rate,273 u8 index)274{275 struct mmp_clk_mix *mix = to_clk_mix(hw);276 unsigned int div;277 u32 div_val, mux_val;278 279 div = parent_rate / rate;280 div_val = _get_div_val(mix, div);281 mux_val = _get_mux_val(mix, index);282 283 return _set_rate(mix, mux_val, div_val, 1, 1);284}285 286static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)287{288 struct mmp_clk_mix *mix = to_clk_mix(hw);289 struct mmp_clk_mix_reg_info *ri = &mix->reg_info;290 unsigned long flags = 0;291 u32 mux_div = 0;292 u8 width, shift;293 u32 mux_val;294 295 if (mix->lock)296 spin_lock_irqsave(mix->lock, flags);297 298 if (mix->type == MMP_CLK_MIX_TYPE_V1299 || mix->type == MMP_CLK_MIX_TYPE_V2)300 mux_div = readl(ri->reg_clk_ctrl);301 else302 mux_div = readl(ri->reg_clk_sel);303 304 if (mix->lock)305 spin_unlock_irqrestore(mix->lock, flags);306 307 width = mix->reg_info.width_mux;308 shift = mix->reg_info.shift_mux;309 310 mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);311 312 return _get_mux(mix, mux_val);313}314 315static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,316 unsigned long parent_rate)317{318 struct mmp_clk_mix *mix = to_clk_mix(hw);319 struct mmp_clk_mix_reg_info *ri = &mix->reg_info;320 unsigned long flags = 0;321 u32 mux_div = 0;322 u8 width, shift;323 unsigned int div;324 325 if (mix->lock)326 spin_lock_irqsave(mix->lock, flags);327 328 if (mix->type == MMP_CLK_MIX_TYPE_V1329 || mix->type == MMP_CLK_MIX_TYPE_V2)330 mux_div = readl(ri->reg_clk_ctrl);331 else332 mux_div = readl(ri->reg_clk_sel);333 334 if (mix->lock)335 spin_unlock_irqrestore(mix->lock, flags);336 337 width = mix->reg_info.width_div;338 shift = mix->reg_info.shift_div;339 340 div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));341 342 return parent_rate / div;343}344 345static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)346{347 struct mmp_clk_mix *mix = to_clk_mix(hw);348 struct mmp_clk_mix_clk_table *item;349 int i;350 u32 div_val, mux_val;351 352 if (mix->table) {353 for (i = 0; i < mix->table_size; i++) {354 item = &mix->table[i];355 if (item->valid == 0)356 continue;357 if (item->parent_index == index)358 break;359 }360 if (i < mix->table_size) {361 div_val = _get_div_val(mix, item->divisor);362 mux_val = _get_mux_val(mix, item->parent_index);363 } else364 return -EINVAL;365 } else {366 mux_val = _get_mux_val(mix, index);367 div_val = 0;368 }369 370 return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);371}372 373static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,374 unsigned long best_parent_rate)375{376 struct mmp_clk_mix *mix = to_clk_mix(hw);377 struct mmp_clk_mix_clk_table *item;378 unsigned long parent_rate;379 unsigned int best_divisor;380 struct clk_hw *parent;381 int i;382 383 best_divisor = best_parent_rate / rate;384 385 if (mix->table) {386 for (i = 0; i < mix->table_size; i++) {387 item = &mix->table[i];388 if (item->valid == 0)389 continue;390 parent = clk_hw_get_parent_by_index(hw,391 item->parent_index);392 parent_rate = clk_hw_get_rate(parent);393 if (parent_rate == best_parent_rate394 && item->divisor == best_divisor)395 break;396 }397 if (i < mix->table_size)398 return _set_rate(mix,399 _get_mux_val(mix, item->parent_index),400 _get_div_val(mix, item->divisor),401 1, 1);402 else403 return -EINVAL;404 } else {405 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {406 parent = clk_hw_get_parent_by_index(hw, i);407 parent_rate = clk_hw_get_rate(parent);408 if (parent_rate == best_parent_rate)409 break;410 }411 if (i < clk_hw_get_num_parents(hw))412 return _set_rate(mix, _get_mux_val(mix, i),413 _get_div_val(mix, best_divisor), 1, 1);414 else415 return -EINVAL;416 }417}418 419static int mmp_clk_mix_init(struct clk_hw *hw)420{421 struct mmp_clk_mix *mix = to_clk_mix(hw);422 423 if (mix->table)424 _filter_clk_table(mix, mix->table, mix->table_size);425 426 return 0;427}428 429const struct clk_ops mmp_clk_mix_ops = {430 .determine_rate = mmp_clk_mix_determine_rate,431 .set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,432 .set_rate = mmp_clk_set_rate,433 .set_parent = mmp_clk_set_parent,434 .get_parent = mmp_clk_mix_get_parent,435 .recalc_rate = mmp_clk_mix_recalc_rate,436 .init = mmp_clk_mix_init,437};438 439struct clk *mmp_clk_register_mix(struct device *dev,440 const char *name,441 const char * const *parent_names,442 u8 num_parents,443 unsigned long flags,444 struct mmp_clk_mix_config *config,445 spinlock_t *lock)446{447 struct mmp_clk_mix *mix;448 struct clk *clk;449 struct clk_init_data init;450 451 mix = kzalloc(sizeof(*mix), GFP_KERNEL);452 if (!mix)453 return ERR_PTR(-ENOMEM);454 455 init.name = name;456 init.flags = flags | CLK_GET_RATE_NOCACHE;457 init.parent_names = parent_names;458 init.num_parents = num_parents;459 init.ops = &mmp_clk_mix_ops;460 461 memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));462 if (config->table) {463 mix->table = kmemdup_array(config->table, config->table_size,464 sizeof(*mix->table), GFP_KERNEL);465 if (!mix->table)466 goto free_mix;467 468 mix->table_size = config->table_size;469 }470 471 if (config->mux_table) {472 mix->mux_table = kmemdup_array(config->mux_table, num_parents,473 sizeof(*mix->mux_table), GFP_KERNEL);474 if (!mix->mux_table) {475 kfree(mix->table);476 goto free_mix;477 }478 }479 480 mix->div_flags = config->div_flags;481 mix->mux_flags = config->mux_flags;482 mix->lock = lock;483 mix->hw.init = &init;484 485 if (config->reg_info.bit_fc >= 32)486 mix->type = MMP_CLK_MIX_TYPE_V1;487 else if (config->reg_info.reg_clk_sel)488 mix->type = MMP_CLK_MIX_TYPE_V3;489 else490 mix->type = MMP_CLK_MIX_TYPE_V2;491 clk = clk_register(dev, &mix->hw);492 493 if (IS_ERR(clk)) {494 kfree(mix->mux_table);495 kfree(mix->table);496 kfree(mix);497 }498 499 return clk;500 501free_mix:502 kfree(mix);503 return ERR_PTR(-ENOMEM);504}505