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 }; 148