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/io.h> 13 14 #include "ccu_gate.h" 15 #include "ccu_nk.h" 16 17 struct _ccu_nk { 18 unsigned long n, min_n, max_n; 19 unsigned long k, min_k, max_k; 20 }; 21 22 static void ccu_nk_find_best(unsigned long parent, unsigned long rate, 23 struct _ccu_nk *nk) 24 { 25 unsigned long best_rate = 0; 26 unsigned int best_k = 0, best_n = 0; 27 unsigned int _k, _n; 28 29 for (_k = nk->min_k; _k <= nk->max_k; _k++) { 30 for (_n = nk->min_n; _n <= nk->max_n; _n++) { 31 unsigned long tmp_rate = parent * _n * _k; 32 33 if (tmp_rate > rate) 34 continue; 35 36 if ((rate - tmp_rate) < (rate - best_rate)) { 37 best_rate = tmp_rate; 38 best_k = _k; 39 best_n = _n; 40 } 41 } 42 } 43 44 nk->k = best_k; 45 nk->n = best_n; 46 } 47 48 static void ccu_nk_disable(struct clk_hw *hw) 49 { 50 struct ccu_nk *nk = hw_to_ccu_nk(hw); 51 52 return ccu_gate_helper_disable(&nk->common, nk->enable); 53 } 54 55 static int ccu_nk_enable(struct clk_hw *hw) 56 { 57 struct ccu_nk *nk = hw_to_ccu_nk(hw); 58 59 return ccu_gate_helper_enable(&nk->common, nk->enable); 60 } 61 62 static int ccu_nk_is_enabled(struct clk_hw *hw) 63 { 64 struct ccu_nk *nk = hw_to_ccu_nk(hw); 65 66 return ccu_gate_helper_is_enabled(&nk->common, nk->enable); 67 } 68 69 static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw, 70 unsigned long parent_rate) 71 { 72 struct ccu_nk *nk = hw_to_ccu_nk(hw); 73 unsigned long rate, n, k; 74 u32 reg; 75 76 reg = readl(nk->common.base + nk->common.reg); 77 78 n = reg >> nk->n.shift; 79 n &= (1 << nk->n.width) - 1; 80 n += nk->n.offset; 81 if (!n) 82 n++; 83 84 k = reg >> nk->k.shift; 85 k &= (1 << nk->k.width) - 1; 86 k += nk->k.offset; 87 if (!k) 88 k++; 89 90 rate = parent_rate * n * k; 91 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) 92 rate /= nk->fixed_post_div; 93 94 return rate; 95 } 96 97 static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate, 98 unsigned long *parent_rate) 99 { 100 struct ccu_nk *nk = hw_to_ccu_nk(hw); 101 struct _ccu_nk _nk; 102 103 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) 104 rate *= nk->fixed_post_div; 105 106 _nk.min_n = nk->n.min ?: 1; 107 _nk.max_n = nk->n.max ?: 1 << nk->n.width; 108 _nk.min_k = nk->k.min ?: 1; 109 _nk.max_k = nk->k.max ?: 1 << nk->k.width; 110 111 ccu_nk_find_best(*parent_rate, rate, &_nk); 112 rate = *parent_rate * _nk.n * _nk.k; 113 114 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) 115 rate = rate / nk->fixed_post_div; 116 117 return rate; 118 } 119 120 static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate, 121 unsigned long parent_rate) 122 { 123 struct ccu_nk *nk = hw_to_ccu_nk(hw); 124 unsigned long flags; 125 struct _ccu_nk _nk; 126 u32 reg; 127 128 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) 129 rate = rate * nk->fixed_post_div; 130 131 _nk.min_n = nk->n.min ?: 1; 132 _nk.max_n = nk->n.max ?: 1 << nk->n.width; 133 _nk.min_k = nk->k.min ?: 1; 134 _nk.max_k = nk->k.max ?: 1 << nk->k.width; 135 136 ccu_nk_find_best(parent_rate, rate, &_nk); 137 138 spin_lock_irqsave(nk->common.lock, flags); 139 140 reg = readl(nk->common.base + nk->common.reg); 141 reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift); 142 reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift); 143 144 reg |= (_nk.k - nk->k.offset) << nk->k.shift; 145 reg |= (_nk.n - nk->n.offset) << nk->n.shift; 146 writel(reg, nk->common.base + nk->common.reg); 147 148 spin_unlock_irqrestore(nk->common.lock, flags); 149 150 ccu_helper_wait_for_lock(&nk->common, nk->lock); 151 152 return 0; 153 } 154 155 const struct clk_ops ccu_nk_ops = { 156 .disable = ccu_nk_disable, 157 .enable = ccu_nk_enable, 158 .is_enabled = ccu_nk_is_enabled, 159 160 .recalc_rate = ccu_nk_recalc_rate, 161 .round_rate = ccu_nk_round_rate, 162 .set_rate = ccu_nk_set_rate, 163 }; 164