16174a1e2SMaxime Ripard /* 26174a1e2SMaxime Ripard * Copyright (C) 2016 Maxime Ripard 36174a1e2SMaxime Ripard * Maxime Ripard <maxime.ripard@free-electrons.com> 46174a1e2SMaxime Ripard * 56174a1e2SMaxime Ripard * This program is free software; you can redistribute it and/or 66174a1e2SMaxime Ripard * modify it under the terms of the GNU General Public License as 76174a1e2SMaxime Ripard * published by the Free Software Foundation; either version 2 of 86174a1e2SMaxime Ripard * the License, or (at your option) any later version. 96174a1e2SMaxime Ripard */ 106174a1e2SMaxime Ripard 116174a1e2SMaxime Ripard #include <linux/clk-provider.h> 1262e59c4eSStephen Boyd #include <linux/io.h> 136174a1e2SMaxime Ripard 146174a1e2SMaxime Ripard #include "ccu_frac.h" 156174a1e2SMaxime Ripard #include "ccu_gate.h" 166174a1e2SMaxime Ripard #include "ccu_nm.h" 176174a1e2SMaxime Ripard 18ee28648cSMaxime Ripard struct _ccu_nm { 196e0d50daSMaxime Ripard unsigned long n, min_n, max_n; 206e0d50daSMaxime Ripard unsigned long m, min_m, max_m; 21ee28648cSMaxime Ripard }; 22ee28648cSMaxime Ripard 2365b66576SJernej Skrabec static unsigned long ccu_nm_calc_rate(unsigned long parent, 2465b66576SJernej Skrabec unsigned long n, unsigned long m) 2565b66576SJernej Skrabec { 2665b66576SJernej Skrabec u64 rate = parent; 2765b66576SJernej Skrabec 2865b66576SJernej Skrabec rate *= n; 2965b66576SJernej Skrabec do_div(rate, m); 3065b66576SJernej Skrabec 3165b66576SJernej Skrabec return rate; 3265b66576SJernej Skrabec } 3365b66576SJernej Skrabec 34ee28648cSMaxime Ripard static void ccu_nm_find_best(unsigned long parent, unsigned long rate, 35ee28648cSMaxime Ripard struct _ccu_nm *nm) 36ee28648cSMaxime Ripard { 37ee28648cSMaxime Ripard unsigned long best_rate = 0; 38ee28648cSMaxime Ripard unsigned long best_n = 0, best_m = 0; 39ee28648cSMaxime Ripard unsigned long _n, _m; 40ee28648cSMaxime Ripard 416e0d50daSMaxime Ripard for (_n = nm->min_n; _n <= nm->max_n; _n++) { 426e0d50daSMaxime Ripard for (_m = nm->min_m; _m <= nm->max_m; _m++) { 4365b66576SJernej Skrabec unsigned long tmp_rate = ccu_nm_calc_rate(parent, 4465b66576SJernej Skrabec _n, _m); 45ee28648cSMaxime Ripard 46ee28648cSMaxime Ripard if (tmp_rate > rate) 47ee28648cSMaxime Ripard continue; 48ee28648cSMaxime Ripard 49ee28648cSMaxime Ripard if ((rate - tmp_rate) < (rate - best_rate)) { 50ee28648cSMaxime Ripard best_rate = tmp_rate; 51ee28648cSMaxime Ripard best_n = _n; 52ee28648cSMaxime Ripard best_m = _m; 53ee28648cSMaxime Ripard } 54ee28648cSMaxime Ripard } 55ee28648cSMaxime Ripard } 56ee28648cSMaxime Ripard 57ee28648cSMaxime Ripard nm->n = best_n; 58ee28648cSMaxime Ripard nm->m = best_m; 59ee28648cSMaxime Ripard } 60ee28648cSMaxime Ripard 616174a1e2SMaxime Ripard static void ccu_nm_disable(struct clk_hw *hw) 626174a1e2SMaxime Ripard { 636174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 646174a1e2SMaxime Ripard 656174a1e2SMaxime Ripard return ccu_gate_helper_disable(&nm->common, nm->enable); 666174a1e2SMaxime Ripard } 676174a1e2SMaxime Ripard 686174a1e2SMaxime Ripard static int ccu_nm_enable(struct clk_hw *hw) 696174a1e2SMaxime Ripard { 706174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 716174a1e2SMaxime Ripard 726174a1e2SMaxime Ripard return ccu_gate_helper_enable(&nm->common, nm->enable); 736174a1e2SMaxime Ripard } 746174a1e2SMaxime Ripard 756174a1e2SMaxime Ripard static int ccu_nm_is_enabled(struct clk_hw *hw) 766174a1e2SMaxime Ripard { 776174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 786174a1e2SMaxime Ripard 796174a1e2SMaxime Ripard return ccu_gate_helper_is_enabled(&nm->common, nm->enable); 806174a1e2SMaxime Ripard } 816174a1e2SMaxime Ripard 826174a1e2SMaxime Ripard static unsigned long ccu_nm_recalc_rate(struct clk_hw *hw, 836174a1e2SMaxime Ripard unsigned long parent_rate) 846174a1e2SMaxime Ripard { 856174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 867d333ef1SChen-Yu Tsai unsigned long rate; 876174a1e2SMaxime Ripard unsigned long n, m; 886174a1e2SMaxime Ripard u32 reg; 896174a1e2SMaxime Ripard 907d333ef1SChen-Yu Tsai if (ccu_frac_helper_is_enabled(&nm->common, &nm->frac)) { 917d333ef1SChen-Yu Tsai rate = ccu_frac_helper_read_rate(&nm->common, &nm->frac); 927d333ef1SChen-Yu Tsai 937d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 947d333ef1SChen-Yu Tsai rate /= nm->fixed_post_div; 957d333ef1SChen-Yu Tsai 967d333ef1SChen-Yu Tsai return rate; 977d333ef1SChen-Yu Tsai } 986174a1e2SMaxime Ripard 996174a1e2SMaxime Ripard reg = readl(nm->common.base + nm->common.reg); 1006174a1e2SMaxime Ripard 1016174a1e2SMaxime Ripard n = reg >> nm->n.shift; 1026174a1e2SMaxime Ripard n &= (1 << nm->n.width) - 1; 103e66f81bbSMaxime Ripard n += nm->n.offset; 104e66f81bbSMaxime Ripard if (!n) 105e66f81bbSMaxime Ripard n++; 1066174a1e2SMaxime Ripard 1076174a1e2SMaxime Ripard m = reg >> nm->m.shift; 1086174a1e2SMaxime Ripard m &= (1 << nm->m.width) - 1; 109e66f81bbSMaxime Ripard m += nm->m.offset; 110e66f81bbSMaxime Ripard if (!m) 111e66f81bbSMaxime Ripard m++; 1126174a1e2SMaxime Ripard 1137d333ef1SChen-Yu Tsai if (ccu_sdm_helper_is_enabled(&nm->common, &nm->sdm)) 1147d333ef1SChen-Yu Tsai rate = ccu_sdm_helper_read_rate(&nm->common, &nm->sdm, m, n); 1157d333ef1SChen-Yu Tsai else 11665b66576SJernej Skrabec rate = ccu_nm_calc_rate(parent_rate, n, m); 117392ba5faSChen-Yu Tsai 1187d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1197d333ef1SChen-Yu Tsai rate /= nm->fixed_post_div; 1207d333ef1SChen-Yu Tsai 1217d333ef1SChen-Yu Tsai return rate; 1226174a1e2SMaxime Ripard } 1236174a1e2SMaxime Ripard 1246174a1e2SMaxime Ripard static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate, 1256174a1e2SMaxime Ripard unsigned long *parent_rate) 1266174a1e2SMaxime Ripard { 1276174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 128ee28648cSMaxime Ripard struct _ccu_nm _nm; 1296174a1e2SMaxime Ripard 1307d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1317d333ef1SChen-Yu Tsai rate *= nm->fixed_post_div; 1324cdbc40dSChen-Yu Tsai 1332d2b61c1SJernej Skrabec if (rate < nm->min_rate) { 1342d2b61c1SJernej Skrabec rate = nm->min_rate; 1352d2b61c1SJernej Skrabec if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1362d2b61c1SJernej Skrabec rate /= nm->fixed_post_div; 1372d2b61c1SJernej Skrabec return rate; 1382d2b61c1SJernej Skrabec } 1392d2b61c1SJernej Skrabec 140cb54fbd2SJernej Skrabec if (nm->max_rate && rate > nm->max_rate) { 141cb54fbd2SJernej Skrabec rate = nm->max_rate; 142cb54fbd2SJernej Skrabec if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 143cb54fbd2SJernej Skrabec rate /= nm->fixed_post_div; 144cb54fbd2SJernej Skrabec return rate; 145cb54fbd2SJernej Skrabec } 146cb54fbd2SJernej Skrabec 1477d333ef1SChen-Yu Tsai if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) { 1487d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1497d333ef1SChen-Yu Tsai rate /= nm->fixed_post_div; 150392ba5faSChen-Yu Tsai return rate; 1517d333ef1SChen-Yu Tsai } 1527d333ef1SChen-Yu Tsai 1537d333ef1SChen-Yu Tsai if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) { 1547d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1557d333ef1SChen-Yu Tsai rate /= nm->fixed_post_div; 1567d333ef1SChen-Yu Tsai return rate; 1577d333ef1SChen-Yu Tsai } 158392ba5faSChen-Yu Tsai 1594162c5ceSChen-Yu Tsai _nm.min_n = nm->n.min ?: 1; 1600c3c8e13SMaxime Ripard _nm.max_n = nm->n.max ?: 1 << nm->n.width; 1616e0d50daSMaxime Ripard _nm.min_m = 1; 162ee28648cSMaxime Ripard _nm.max_m = nm->m.max ?: 1 << nm->m.width; 16387ba9e59SMaxime Ripard 164ee28648cSMaxime Ripard ccu_nm_find_best(*parent_rate, rate, &_nm); 16565b66576SJernej Skrabec rate = ccu_nm_calc_rate(*parent_rate, _nm.n, _nm.m); 1666174a1e2SMaxime Ripard 1677d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1687d333ef1SChen-Yu Tsai rate /= nm->fixed_post_div; 1697d333ef1SChen-Yu Tsai 1707d333ef1SChen-Yu Tsai return rate; 1716174a1e2SMaxime Ripard } 1726174a1e2SMaxime Ripard 1736174a1e2SMaxime Ripard static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate, 1746174a1e2SMaxime Ripard unsigned long parent_rate) 1756174a1e2SMaxime Ripard { 1766174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 177ee28648cSMaxime Ripard struct _ccu_nm _nm; 1786174a1e2SMaxime Ripard unsigned long flags; 1796174a1e2SMaxime Ripard u32 reg; 1806174a1e2SMaxime Ripard 1817d333ef1SChen-Yu Tsai /* Adjust target rate according to post-dividers */ 1827d333ef1SChen-Yu Tsai if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 1837d333ef1SChen-Yu Tsai rate = rate * nm->fixed_post_div; 1847d333ef1SChen-Yu Tsai 185b64dfec0SJernej Škrabec if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) { 186b64dfec0SJernej Škrabec spin_lock_irqsave(nm->common.lock, flags); 187b64dfec0SJernej Škrabec 188b64dfec0SJernej Škrabec /* most SoCs require M to be 0 if fractional mode is used */ 189b64dfec0SJernej Škrabec reg = readl(nm->common.base + nm->common.reg); 190b64dfec0SJernej Škrabec reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); 191b64dfec0SJernej Škrabec writel(reg, nm->common.base + nm->common.reg); 192b64dfec0SJernej Škrabec 193b64dfec0SJernej Škrabec spin_unlock_irqrestore(nm->common.lock, flags); 194b64dfec0SJernej Škrabec 195b64dfec0SJernej Škrabec ccu_frac_helper_enable(&nm->common, &nm->frac); 196b64dfec0SJernej Škrabec 1971d42460aSJernej Škrabec return ccu_frac_helper_set_rate(&nm->common, &nm->frac, 1981d42460aSJernej Škrabec rate, nm->lock); 199b64dfec0SJernej Škrabec } else { 2006174a1e2SMaxime Ripard ccu_frac_helper_disable(&nm->common, &nm->frac); 201b64dfec0SJernej Škrabec } 2026174a1e2SMaxime Ripard 20395ad8ed9SChen-Yu Tsai _nm.min_n = nm->n.min ?: 1; 2040c3c8e13SMaxime Ripard _nm.max_n = nm->n.max ?: 1 << nm->n.width; 2056e0d50daSMaxime Ripard _nm.min_m = 1; 206ee28648cSMaxime Ripard _nm.max_m = nm->m.max ?: 1 << nm->m.width; 20787ba9e59SMaxime Ripard 208392ba5faSChen-Yu Tsai if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) { 209392ba5faSChen-Yu Tsai ccu_sdm_helper_enable(&nm->common, &nm->sdm, rate); 210392ba5faSChen-Yu Tsai 211392ba5faSChen-Yu Tsai /* Sigma delta modulation requires specific N and M factors */ 212392ba5faSChen-Yu Tsai ccu_sdm_helper_get_factors(&nm->common, &nm->sdm, rate, 213392ba5faSChen-Yu Tsai &_nm.m, &_nm.n); 214392ba5faSChen-Yu Tsai } else { 215392ba5faSChen-Yu Tsai ccu_sdm_helper_disable(&nm->common, &nm->sdm); 216ee28648cSMaxime Ripard ccu_nm_find_best(parent_rate, rate, &_nm); 217392ba5faSChen-Yu Tsai } 2186174a1e2SMaxime Ripard 2196174a1e2SMaxime Ripard spin_lock_irqsave(nm->common.lock, flags); 2206174a1e2SMaxime Ripard 2216174a1e2SMaxime Ripard reg = readl(nm->common.base + nm->common.reg); 2226174a1e2SMaxime Ripard reg &= ~GENMASK(nm->n.width + nm->n.shift - 1, nm->n.shift); 2236174a1e2SMaxime Ripard reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); 2246174a1e2SMaxime Ripard 225e66f81bbSMaxime Ripard reg |= (_nm.n - nm->n.offset) << nm->n.shift; 226e66f81bbSMaxime Ripard reg |= (_nm.m - nm->m.offset) << nm->m.shift; 227e66f81bbSMaxime Ripard writel(reg, nm->common.base + nm->common.reg); 2286174a1e2SMaxime Ripard 2296174a1e2SMaxime Ripard spin_unlock_irqrestore(nm->common.lock, flags); 2306174a1e2SMaxime Ripard 2316174a1e2SMaxime Ripard ccu_helper_wait_for_lock(&nm->common, nm->lock); 2326174a1e2SMaxime Ripard 2336174a1e2SMaxime Ripard return 0; 2346174a1e2SMaxime Ripard } 2356174a1e2SMaxime Ripard 2366174a1e2SMaxime Ripard const struct clk_ops ccu_nm_ops = { 2376174a1e2SMaxime Ripard .disable = ccu_nm_disable, 2386174a1e2SMaxime Ripard .enable = ccu_nm_enable, 2396174a1e2SMaxime Ripard .is_enabled = ccu_nm_is_enabled, 2406174a1e2SMaxime Ripard 2416174a1e2SMaxime Ripard .recalc_rate = ccu_nm_recalc_rate, 2426174a1e2SMaxime Ripard .round_rate = ccu_nm_round_rate, 2436174a1e2SMaxime Ripard .set_rate = ccu_nm_set_rate, 2446174a1e2SMaxime Ripard }; 245