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> 126174a1e2SMaxime Ripard 136174a1e2SMaxime Ripard #include "ccu_frac.h" 146174a1e2SMaxime Ripard #include "ccu_gate.h" 156174a1e2SMaxime Ripard #include "ccu_nm.h" 166174a1e2SMaxime Ripard 17ee28648cSMaxime Ripard struct _ccu_nm { 18ee28648cSMaxime Ripard unsigned long n, max_n; 19ee28648cSMaxime Ripard unsigned long m, max_m; 20ee28648cSMaxime Ripard }; 21ee28648cSMaxime Ripard 22ee28648cSMaxime Ripard static void ccu_nm_find_best(unsigned long parent, unsigned long rate, 23ee28648cSMaxime Ripard struct _ccu_nm *nm) 24ee28648cSMaxime Ripard { 25ee28648cSMaxime Ripard unsigned long best_rate = 0; 26ee28648cSMaxime Ripard unsigned long best_n = 0, best_m = 0; 27ee28648cSMaxime Ripard unsigned long _n, _m; 28ee28648cSMaxime Ripard 29ee28648cSMaxime Ripard for (_n = 1; _n <= nm->max_n; _n++) { 30ee28648cSMaxime Ripard for (_m = 1; _n <= nm->max_m; _m++) { 31ee28648cSMaxime Ripard unsigned long tmp_rate = parent * _n / _m; 32ee28648cSMaxime Ripard 33ee28648cSMaxime Ripard if (tmp_rate > rate) 34ee28648cSMaxime Ripard continue; 35ee28648cSMaxime Ripard 36ee28648cSMaxime Ripard if ((rate - tmp_rate) < (rate - best_rate)) { 37ee28648cSMaxime Ripard best_rate = tmp_rate; 38ee28648cSMaxime Ripard best_n = _n; 39ee28648cSMaxime Ripard best_m = _m; 40ee28648cSMaxime Ripard } 41ee28648cSMaxime Ripard } 42ee28648cSMaxime Ripard } 43ee28648cSMaxime Ripard 44ee28648cSMaxime Ripard nm->n = best_n; 45ee28648cSMaxime Ripard nm->m = best_m; 46ee28648cSMaxime Ripard } 47ee28648cSMaxime Ripard 486174a1e2SMaxime Ripard static void ccu_nm_disable(struct clk_hw *hw) 496174a1e2SMaxime Ripard { 506174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 516174a1e2SMaxime Ripard 526174a1e2SMaxime Ripard return ccu_gate_helper_disable(&nm->common, nm->enable); 536174a1e2SMaxime Ripard } 546174a1e2SMaxime Ripard 556174a1e2SMaxime Ripard static int ccu_nm_enable(struct clk_hw *hw) 566174a1e2SMaxime Ripard { 576174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 586174a1e2SMaxime Ripard 596174a1e2SMaxime Ripard return ccu_gate_helper_enable(&nm->common, nm->enable); 606174a1e2SMaxime Ripard } 616174a1e2SMaxime Ripard 626174a1e2SMaxime Ripard static int ccu_nm_is_enabled(struct clk_hw *hw) 636174a1e2SMaxime Ripard { 646174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 656174a1e2SMaxime Ripard 666174a1e2SMaxime Ripard return ccu_gate_helper_is_enabled(&nm->common, nm->enable); 676174a1e2SMaxime Ripard } 686174a1e2SMaxime Ripard 696174a1e2SMaxime Ripard static unsigned long ccu_nm_recalc_rate(struct clk_hw *hw, 706174a1e2SMaxime Ripard unsigned long parent_rate) 716174a1e2SMaxime Ripard { 726174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 736174a1e2SMaxime Ripard unsigned long n, m; 746174a1e2SMaxime Ripard u32 reg; 756174a1e2SMaxime Ripard 766174a1e2SMaxime Ripard if (ccu_frac_helper_is_enabled(&nm->common, &nm->frac)) 776174a1e2SMaxime Ripard return ccu_frac_helper_read_rate(&nm->common, &nm->frac); 786174a1e2SMaxime Ripard 796174a1e2SMaxime Ripard reg = readl(nm->common.base + nm->common.reg); 806174a1e2SMaxime Ripard 816174a1e2SMaxime Ripard n = reg >> nm->n.shift; 826174a1e2SMaxime Ripard n &= (1 << nm->n.width) - 1; 836174a1e2SMaxime Ripard 846174a1e2SMaxime Ripard m = reg >> nm->m.shift; 856174a1e2SMaxime Ripard m &= (1 << nm->m.width) - 1; 866174a1e2SMaxime Ripard 876174a1e2SMaxime Ripard return parent_rate * (n + 1) / (m + 1); 886174a1e2SMaxime Ripard } 896174a1e2SMaxime Ripard 906174a1e2SMaxime Ripard static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate, 916174a1e2SMaxime Ripard unsigned long *parent_rate) 926174a1e2SMaxime Ripard { 936174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 94ee28648cSMaxime Ripard struct _ccu_nm _nm; 956174a1e2SMaxime Ripard 96ee28648cSMaxime Ripard _nm.max_n = 1 << nm->n.width; 97ee28648cSMaxime Ripard _nm.max_m = nm->m.max ?: 1 << nm->m.width; 9887ba9e59SMaxime Ripard 99ee28648cSMaxime Ripard ccu_nm_find_best(*parent_rate, rate, &_nm); 1006174a1e2SMaxime Ripard 101ee28648cSMaxime Ripard return *parent_rate * _nm.n / _nm.m; 1026174a1e2SMaxime Ripard } 1036174a1e2SMaxime Ripard 1046174a1e2SMaxime Ripard static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate, 1056174a1e2SMaxime Ripard unsigned long parent_rate) 1066174a1e2SMaxime Ripard { 1076174a1e2SMaxime Ripard struct ccu_nm *nm = hw_to_ccu_nm(hw); 108ee28648cSMaxime Ripard struct _ccu_nm _nm; 1096174a1e2SMaxime Ripard unsigned long flags; 1106174a1e2SMaxime Ripard u32 reg; 1116174a1e2SMaxime Ripard 1126174a1e2SMaxime Ripard if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) 1136174a1e2SMaxime Ripard return ccu_frac_helper_set_rate(&nm->common, &nm->frac, rate); 1146174a1e2SMaxime Ripard else 1156174a1e2SMaxime Ripard ccu_frac_helper_disable(&nm->common, &nm->frac); 1166174a1e2SMaxime Ripard 117ee28648cSMaxime Ripard _nm.max_n = 1 << nm->n.width; 118ee28648cSMaxime Ripard _nm.max_m = nm->m.max ?: 1 << nm->m.width; 11987ba9e59SMaxime Ripard 120ee28648cSMaxime Ripard ccu_nm_find_best(parent_rate, rate, &_nm); 1216174a1e2SMaxime Ripard 1226174a1e2SMaxime Ripard spin_lock_irqsave(nm->common.lock, flags); 1236174a1e2SMaxime Ripard 1246174a1e2SMaxime Ripard reg = readl(nm->common.base + nm->common.reg); 1256174a1e2SMaxime Ripard reg &= ~GENMASK(nm->n.width + nm->n.shift - 1, nm->n.shift); 1266174a1e2SMaxime Ripard reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); 1276174a1e2SMaxime Ripard 128ee28648cSMaxime Ripard writel(reg | ((_nm.m - 1) << nm->m.shift) | ((_nm.n - 1) << nm->n.shift), 1296174a1e2SMaxime Ripard nm->common.base + nm->common.reg); 1306174a1e2SMaxime Ripard 1316174a1e2SMaxime Ripard spin_unlock_irqrestore(nm->common.lock, flags); 1326174a1e2SMaxime Ripard 1336174a1e2SMaxime Ripard ccu_helper_wait_for_lock(&nm->common, nm->lock); 1346174a1e2SMaxime Ripard 1356174a1e2SMaxime Ripard return 0; 1366174a1e2SMaxime Ripard } 1376174a1e2SMaxime Ripard 1386174a1e2SMaxime Ripard const struct clk_ops ccu_nm_ops = { 1396174a1e2SMaxime Ripard .disable = ccu_nm_disable, 1406174a1e2SMaxime Ripard .enable = ccu_nm_enable, 1416174a1e2SMaxime Ripard .is_enabled = ccu_nm_is_enabled, 1426174a1e2SMaxime Ripard 1436174a1e2SMaxime Ripard .recalc_rate = ccu_nm_recalc_rate, 1446174a1e2SMaxime Ripard .round_rate = ccu_nm_round_rate, 1456174a1e2SMaxime Ripard .set_rate = ccu_nm_set_rate, 1466174a1e2SMaxime Ripard }; 147