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_nkmp.h" 16 17 struct _ccu_nkmp { 18 unsigned long n, max_n; 19 unsigned long k, max_k; 20 unsigned long m, max_m; 21 unsigned long p, max_p; 22 }; 23 24 static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate, 25 struct _ccu_nkmp *nkmp) 26 { 27 unsigned long best_rate = 0; 28 unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0; 29 unsigned long _n, _k, _m, _p; 30 31 for (_k = 1; _k <= nkmp->max_k; _k++) { 32 for (_p = 0; _p <= nkmp->max_p; _p++) { 33 unsigned long tmp_rate; 34 35 rational_best_approximation(rate / _k, parent >> _p, 36 nkmp->max_n, nkmp->max_m, 37 &_n, &_m); 38 39 tmp_rate = (parent * _n * _k >> _p) / _m; 40 41 if (tmp_rate > rate) 42 continue; 43 44 if ((rate - tmp_rate) < (rate - best_rate)) { 45 best_rate = tmp_rate; 46 best_n = _n; 47 best_k = _k; 48 best_m = _m; 49 best_p = _p; 50 } 51 } 52 } 53 54 nkmp->n = best_n; 55 nkmp->k = best_k; 56 nkmp->m = best_m; 57 nkmp->p = best_p; 58 } 59 60 static void ccu_nkmp_disable(struct clk_hw *hw) 61 { 62 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 63 64 return ccu_gate_helper_disable(&nkmp->common, nkmp->enable); 65 } 66 67 static int ccu_nkmp_enable(struct clk_hw *hw) 68 { 69 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 70 71 return ccu_gate_helper_enable(&nkmp->common, nkmp->enable); 72 } 73 74 static int ccu_nkmp_is_enabled(struct clk_hw *hw) 75 { 76 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 77 78 return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable); 79 } 80 81 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw, 82 unsigned long parent_rate) 83 { 84 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 85 unsigned long n, m, k, p; 86 u32 reg; 87 88 reg = readl(nkmp->common.base + nkmp->common.reg); 89 90 n = reg >> nkmp->n.shift; 91 n &= (1 << nkmp->n.width) - 1; 92 93 k = reg >> nkmp->k.shift; 94 k &= (1 << nkmp->k.width) - 1; 95 96 m = reg >> nkmp->m.shift; 97 m &= (1 << nkmp->m.width) - 1; 98 99 p = reg >> nkmp->p.shift; 100 p &= (1 << nkmp->p.width) - 1; 101 102 return (parent_rate * (n + 1) * (k + 1) >> p) / (m + 1); 103 } 104 105 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate, 106 unsigned long *parent_rate) 107 { 108 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 109 struct _ccu_nkmp _nkmp; 110 111 _nkmp.max_n = 1 << nkmp->n.width; 112 _nkmp.max_k = 1 << nkmp->k.width; 113 _nkmp.max_m = 1 << nkmp->m.width; 114 _nkmp.max_p = (1 << nkmp->p.width) - 1; 115 116 ccu_nkmp_find_best(*parent_rate, rate, 117 &_nkmp); 118 119 return (*parent_rate * _nkmp.n * _nkmp.k >> _nkmp.p) / _nkmp.m; 120 } 121 122 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate, 123 unsigned long parent_rate) 124 { 125 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 126 struct _ccu_nkmp _nkmp; 127 unsigned long flags; 128 u32 reg; 129 130 _nkmp.max_n = 1 << nkmp->n.width; 131 _nkmp.max_k = 1 << nkmp->k.width; 132 _nkmp.max_m = 1 << nkmp->m.width; 133 _nkmp.max_p = (1 << nkmp->p.width) - 1; 134 135 ccu_nkmp_find_best(parent_rate, rate, &_nkmp); 136 137 spin_lock_irqsave(nkmp->common.lock, flags); 138 139 reg = readl(nkmp->common.base + nkmp->common.reg); 140 reg &= ~GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift); 141 reg &= ~GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift); 142 reg &= ~GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift); 143 reg &= ~GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift); 144 145 reg |= (_nkmp.n - 1) << nkmp->n.shift; 146 reg |= (_nkmp.k - 1) << nkmp->k.shift; 147 reg |= (_nkmp.m - 1) << nkmp->m.shift; 148 reg |= _nkmp.p << nkmp->p.shift; 149 150 writel(reg, nkmp->common.base + nkmp->common.reg); 151 152 spin_unlock_irqrestore(nkmp->common.lock, flags); 153 154 ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock); 155 156 return 0; 157 } 158 159 const struct clk_ops ccu_nkmp_ops = { 160 .disable = ccu_nkmp_disable, 161 .enable = ccu_nkmp_enable, 162 .is_enabled = ccu_nkmp_is_enabled, 163 164 .recalc_rate = ccu_nkmp_recalc_rate, 165 .round_rate = ccu_nkmp_round_rate, 166 .set_rate = ccu_nkmp_set_rate, 167 }; 168