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