Maxime Ripard | adbfb00 | 2016-06-29 21:05:30 +0200 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2016 Maxime Ripard |
| 3 | * Maxime Ripard <maxime.ripard@free-electrons.com> |
| 4 | * |
| 5 | * This program is free software; you can redistribute it and/or |
| 6 | * modify it under the terms of the GNU General Public License as |
| 7 | * published by the Free Software Foundation; either version 2 of |
| 8 | * the License, or (at your option) any later version. |
| 9 | */ |
| 10 | |
| 11 | #include <linux/clk-provider.h> |
| 12 | #include <linux/rational.h> |
| 13 | |
| 14 | #include "ccu_gate.h" |
| 15 | #include "ccu_nk.h" |
| 16 | |
| 17 | void ccu_nk_find_best(unsigned long parent, unsigned long rate, |
| 18 | unsigned int max_n, unsigned int max_k, |
| 19 | unsigned int *n, unsigned int *k) |
| 20 | { |
| 21 | unsigned long best_rate = 0; |
| 22 | unsigned int best_k = 0, best_n = 0; |
| 23 | unsigned int _k, _n; |
| 24 | |
| 25 | for (_k = 1; _k <= max_k; _k++) { |
| 26 | for (_n = 1; _n <= max_n; _n++) { |
| 27 | unsigned long tmp_rate = parent * _n * _k; |
| 28 | |
| 29 | if (tmp_rate > rate) |
| 30 | continue; |
| 31 | |
| 32 | if ((rate - tmp_rate) < (rate - best_rate)) { |
| 33 | best_rate = tmp_rate; |
| 34 | best_k = _k; |
| 35 | best_n = _n; |
| 36 | } |
| 37 | } |
| 38 | } |
| 39 | |
| 40 | *k = best_k; |
| 41 | *n = best_n; |
| 42 | } |
| 43 | |
| 44 | static void ccu_nk_disable(struct clk_hw *hw) |
| 45 | { |
| 46 | struct ccu_nk *nk = hw_to_ccu_nk(hw); |
| 47 | |
| 48 | return ccu_gate_helper_disable(&nk->common, nk->enable); |
| 49 | } |
| 50 | |
| 51 | static int ccu_nk_enable(struct clk_hw *hw) |
| 52 | { |
| 53 | struct ccu_nk *nk = hw_to_ccu_nk(hw); |
| 54 | |
| 55 | return ccu_gate_helper_enable(&nk->common, nk->enable); |
| 56 | } |
| 57 | |
| 58 | static int ccu_nk_is_enabled(struct clk_hw *hw) |
| 59 | { |
| 60 | struct ccu_nk *nk = hw_to_ccu_nk(hw); |
| 61 | |
| 62 | return ccu_gate_helper_is_enabled(&nk->common, nk->enable); |
| 63 | } |
| 64 | |
| 65 | static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw, |
| 66 | unsigned long parent_rate) |
| 67 | { |
| 68 | struct ccu_nk *nk = hw_to_ccu_nk(hw); |
| 69 | unsigned long rate, n, k; |
| 70 | u32 reg; |
| 71 | |
| 72 | reg = readl(nk->common.base + nk->common.reg); |
| 73 | |
| 74 | n = reg >> nk->n.shift; |
| 75 | n &= (1 << nk->n.width) - 1; |
| 76 | |
| 77 | k = reg >> nk->k.shift; |
| 78 | k &= (1 << nk->k.width) - 1; |
| 79 | |
| 80 | rate = parent_rate * (n + 1) * (k + 1); |
| 81 | |
| 82 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) |
| 83 | rate /= nk->fixed_post_div; |
| 84 | |
| 85 | return rate; |
| 86 | } |
| 87 | |
| 88 | static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate, |
| 89 | unsigned long *parent_rate) |
| 90 | { |
| 91 | struct ccu_nk *nk = hw_to_ccu_nk(hw); |
| 92 | unsigned int n, k; |
| 93 | |
| 94 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) |
| 95 | rate *= nk->fixed_post_div; |
| 96 | |
| 97 | ccu_nk_find_best(*parent_rate, rate, |
| 98 | 1 << nk->n.width, 1 << nk->k.width, |
| 99 | &n, &k); |
| 100 | |
| 101 | rate = *parent_rate * n * k; |
| 102 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) |
| 103 | rate = rate / nk->fixed_post_div; |
| 104 | |
| 105 | return rate; |
| 106 | } |
| 107 | |
| 108 | static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate, |
| 109 | unsigned long parent_rate) |
| 110 | { |
| 111 | struct ccu_nk *nk = hw_to_ccu_nk(hw); |
| 112 | unsigned long flags; |
| 113 | unsigned int n, k; |
| 114 | u32 reg; |
| 115 | |
| 116 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) |
| 117 | rate = rate * nk->fixed_post_div; |
| 118 | |
| 119 | ccu_nk_find_best(parent_rate, rate, |
| 120 | 1 << nk->n.width, 1 << nk->k.width, |
| 121 | &n, &k); |
| 122 | |
| 123 | spin_lock_irqsave(nk->common.lock, flags); |
| 124 | |
| 125 | reg = readl(nk->common.base + nk->common.reg); |
| 126 | reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift); |
| 127 | reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift); |
| 128 | |
| 129 | writel(reg | ((k - 1) << nk->k.shift) | ((n - 1) << nk->n.shift), |
| 130 | nk->common.base + nk->common.reg); |
| 131 | |
| 132 | spin_unlock_irqrestore(nk->common.lock, flags); |
| 133 | |
| 134 | ccu_helper_wait_for_lock(&nk->common, nk->lock); |
| 135 | |
| 136 | return 0; |
| 137 | } |
| 138 | |
| 139 | const struct clk_ops ccu_nk_ops = { |
| 140 | .disable = ccu_nk_disable, |
| 141 | .enable = ccu_nk_enable, |
| 142 | .is_enabled = ccu_nk_is_enabled, |
| 143 | |
| 144 | .recalc_rate = ccu_nk_recalc_rate, |
| 145 | .round_rate = ccu_nk_round_rate, |
| 146 | .set_rate = ccu_nk_set_rate, |
| 147 | }; |