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_mux.h" 15 16 void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common, 17 struct ccu_mux_internal *cm, 18 int parent_index, 19 unsigned long *parent_rate) 20 { 21 u8 prediv = 1; 22 u32 reg; 23 24 if (!((common->features & CCU_FEATURE_FIXED_PREDIV) || 25 (common->features & CCU_FEATURE_VARIABLE_PREDIV))) 26 return; 27 28 reg = readl(common->base + common->reg); 29 if (parent_index < 0) { 30 parent_index = reg >> cm->shift; 31 parent_index &= (1 << cm->width) - 1; 32 } 33 34 if (common->features & CCU_FEATURE_FIXED_PREDIV) 35 if (parent_index == cm->fixed_prediv.index) 36 prediv = cm->fixed_prediv.div; 37 38 if (common->features & CCU_FEATURE_VARIABLE_PREDIV) 39 if (parent_index == cm->variable_prediv.index) { 40 u8 div; 41 42 div = reg >> cm->variable_prediv.shift; 43 div &= (1 << cm->variable_prediv.width) - 1; 44 prediv = div + 1; 45 } 46 47 *parent_rate = *parent_rate / prediv; 48 } 49 50 int ccu_mux_helper_determine_rate(struct ccu_common *common, 51 struct ccu_mux_internal *cm, 52 struct clk_rate_request *req, 53 unsigned long (*round)(struct ccu_mux_internal *, 54 unsigned long, 55 unsigned long, 56 void *), 57 void *data) 58 { 59 unsigned long best_parent_rate = 0, best_rate = 0; 60 struct clk_hw *best_parent, *hw = &common->hw; 61 unsigned int i; 62 63 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 64 unsigned long tmp_rate, parent_rate; 65 struct clk_hw *parent; 66 67 parent = clk_hw_get_parent_by_index(hw, i); 68 if (!parent) 69 continue; 70 71 parent_rate = clk_hw_get_rate(parent); 72 ccu_mux_helper_adjust_parent_for_prediv(common, cm, i, 73 &parent_rate); 74 75 tmp_rate = round(cm, clk_hw_get_rate(parent), req->rate, data); 76 if (tmp_rate == req->rate) { 77 best_parent = parent; 78 best_parent_rate = parent_rate; 79 best_rate = tmp_rate; 80 goto out; 81 } 82 83 if ((req->rate - tmp_rate) < (req->rate - best_rate)) { 84 best_rate = tmp_rate; 85 best_parent_rate = parent_rate; 86 best_parent = parent; 87 } 88 } 89 90 if (best_rate == 0) 91 return -EINVAL; 92 93 out: 94 req->best_parent_hw = best_parent; 95 req->best_parent_rate = best_parent_rate; 96 req->rate = best_rate; 97 return 0; 98 } 99 100 u8 ccu_mux_helper_get_parent(struct ccu_common *common, 101 struct ccu_mux_internal *cm) 102 { 103 u32 reg; 104 u8 parent; 105 106 reg = readl(common->base + common->reg); 107 parent = reg >> cm->shift; 108 parent &= (1 << cm->width) - 1; 109 110 return parent; 111 } 112 113 int ccu_mux_helper_set_parent(struct ccu_common *common, 114 struct ccu_mux_internal *cm, 115 u8 index) 116 { 117 unsigned long flags; 118 u32 reg; 119 120 spin_lock_irqsave(common->lock, flags); 121 122 reg = readl(common->base + common->reg); 123 reg &= ~GENMASK(cm->width + cm->shift - 1, cm->shift); 124 writel(reg | (index << cm->shift), common->base + common->reg); 125 126 spin_unlock_irqrestore(common->lock, flags); 127 128 return 0; 129 } 130 131 static void ccu_mux_disable(struct clk_hw *hw) 132 { 133 struct ccu_mux *cm = hw_to_ccu_mux(hw); 134 135 return ccu_gate_helper_disable(&cm->common, cm->enable); 136 } 137 138 static int ccu_mux_enable(struct clk_hw *hw) 139 { 140 struct ccu_mux *cm = hw_to_ccu_mux(hw); 141 142 return ccu_gate_helper_enable(&cm->common, cm->enable); 143 } 144 145 static int ccu_mux_is_enabled(struct clk_hw *hw) 146 { 147 struct ccu_mux *cm = hw_to_ccu_mux(hw); 148 149 return ccu_gate_helper_is_enabled(&cm->common, cm->enable); 150 } 151 152 static u8 ccu_mux_get_parent(struct clk_hw *hw) 153 { 154 struct ccu_mux *cm = hw_to_ccu_mux(hw); 155 156 return ccu_mux_helper_get_parent(&cm->common, &cm->mux); 157 } 158 159 static int ccu_mux_set_parent(struct clk_hw *hw, u8 index) 160 { 161 struct ccu_mux *cm = hw_to_ccu_mux(hw); 162 163 return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index); 164 } 165 166 static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw, 167 unsigned long parent_rate) 168 { 169 struct ccu_mux *cm = hw_to_ccu_mux(hw); 170 171 ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1, 172 &parent_rate); 173 174 return parent_rate; 175 } 176 177 const struct clk_ops ccu_mux_ops = { 178 .disable = ccu_mux_disable, 179 .enable = ccu_mux_enable, 180 .is_enabled = ccu_mux_is_enabled, 181 182 .get_parent = ccu_mux_get_parent, 183 .set_parent = ccu_mux_set_parent, 184 185 .determine_rate = __clk_mux_determine_rate, 186 .recalc_rate = ccu_mux_recalc_rate, 187 }; 188