1 /* 2 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License as published by 6 * the Free Software Foundation; either version 2 of the License, or 7 * (at your option) any later version. 8 * 9 */ 10 11 #include <linux/clk-provider.h> 12 #include <linux/clkdev.h> 13 #include <linux/clk/at91_pmc.h> 14 #include <linux/of.h> 15 #include <linux/mfd/syscon.h> 16 #include <linux/regmap.h> 17 18 #include "pmc.h" 19 20 #define PROG_ID_MAX 7 21 22 #define PROG_STATUS_MASK(id) (1 << ((id) + 8)) 23 #define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask) 24 #define PROG_MAX_RM9200_CSS 3 25 26 struct clk_programmable { 27 struct clk_hw hw; 28 struct regmap *regmap; 29 u8 id; 30 const struct clk_programmable_layout *layout; 31 }; 32 33 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) 34 35 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, 36 unsigned long parent_rate) 37 { 38 struct clk_programmable *prog = to_clk_programmable(hw); 39 const struct clk_programmable_layout *layout = prog->layout; 40 unsigned int pckr; 41 unsigned long rate; 42 43 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 44 45 if (layout->is_pres_direct) 46 rate = parent_rate / (PROG_PRES(layout, pckr) + 1); 47 else 48 rate = parent_rate >> PROG_PRES(layout, pckr); 49 50 return rate; 51 } 52 53 static int clk_programmable_determine_rate(struct clk_hw *hw, 54 struct clk_rate_request *req) 55 { 56 struct clk_programmable *prog = to_clk_programmable(hw); 57 const struct clk_programmable_layout *layout = prog->layout; 58 struct clk_hw *parent; 59 long best_rate = -EINVAL; 60 unsigned long parent_rate; 61 unsigned long tmp_rate = 0; 62 int shift; 63 int i; 64 65 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 66 parent = clk_hw_get_parent_by_index(hw, i); 67 if (!parent) 68 continue; 69 70 parent_rate = clk_hw_get_rate(parent); 71 if (layout->is_pres_direct) { 72 for (shift = 0; shift <= layout->pres_mask; shift++) { 73 tmp_rate = parent_rate / (shift + 1); 74 if (tmp_rate <= req->rate) 75 break; 76 } 77 } else { 78 for (shift = 0; shift < layout->pres_mask; shift++) { 79 tmp_rate = parent_rate >> shift; 80 if (tmp_rate <= req->rate) 81 break; 82 } 83 } 84 85 if (tmp_rate > req->rate) 86 continue; 87 88 if (best_rate < 0 || 89 (req->rate - tmp_rate) < (req->rate - best_rate)) { 90 best_rate = tmp_rate; 91 req->best_parent_rate = parent_rate; 92 req->best_parent_hw = parent; 93 } 94 95 if (!best_rate) 96 break; 97 } 98 99 if (best_rate < 0) 100 return best_rate; 101 102 req->rate = best_rate; 103 return 0; 104 } 105 106 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) 107 { 108 struct clk_programmable *prog = to_clk_programmable(hw); 109 const struct clk_programmable_layout *layout = prog->layout; 110 unsigned int mask = layout->css_mask; 111 unsigned int pckr = index; 112 113 if (layout->have_slck_mck) 114 mask |= AT91_PMC_CSSMCK_MCK; 115 116 if (index > layout->css_mask) { 117 if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck) 118 return -EINVAL; 119 120 pckr |= AT91_PMC_CSSMCK_MCK; 121 } 122 123 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr); 124 125 return 0; 126 } 127 128 static u8 clk_programmable_get_parent(struct clk_hw *hw) 129 { 130 struct clk_programmable *prog = to_clk_programmable(hw); 131 const struct clk_programmable_layout *layout = prog->layout; 132 unsigned int pckr; 133 u8 ret; 134 135 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 136 137 ret = pckr & layout->css_mask; 138 139 if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret) 140 ret = PROG_MAX_RM9200_CSS + 1; 141 142 return ret; 143 } 144 145 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, 146 unsigned long parent_rate) 147 { 148 struct clk_programmable *prog = to_clk_programmable(hw); 149 const struct clk_programmable_layout *layout = prog->layout; 150 unsigned long div = parent_rate / rate; 151 int shift = 0; 152 153 if (!div) 154 return -EINVAL; 155 156 if (layout->is_pres_direct) { 157 shift = div - 1; 158 159 if (shift > layout->pres_mask) 160 return -EINVAL; 161 } else { 162 shift = fls(div) - 1; 163 164 if (div != (1 << shift)) 165 return -EINVAL; 166 167 if (shift >= layout->pres_mask) 168 return -EINVAL; 169 } 170 171 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), 172 layout->pres_mask << layout->pres_shift, 173 shift << layout->pres_shift); 174 175 return 0; 176 } 177 178 static const struct clk_ops programmable_ops = { 179 .recalc_rate = clk_programmable_recalc_rate, 180 .determine_rate = clk_programmable_determine_rate, 181 .get_parent = clk_programmable_get_parent, 182 .set_parent = clk_programmable_set_parent, 183 .set_rate = clk_programmable_set_rate, 184 }; 185 186 struct clk_hw * __init 187 at91_clk_register_programmable(struct regmap *regmap, 188 const char *name, const char **parent_names, 189 u8 num_parents, u8 id, 190 const struct clk_programmable_layout *layout) 191 { 192 struct clk_programmable *prog; 193 struct clk_hw *hw; 194 struct clk_init_data init; 195 int ret; 196 197 if (id > PROG_ID_MAX) 198 return ERR_PTR(-EINVAL); 199 200 prog = kzalloc(sizeof(*prog), GFP_KERNEL); 201 if (!prog) 202 return ERR_PTR(-ENOMEM); 203 204 init.name = name; 205 init.ops = &programmable_ops; 206 init.parent_names = parent_names; 207 init.num_parents = num_parents; 208 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 209 210 prog->id = id; 211 prog->layout = layout; 212 prog->hw.init = &init; 213 prog->regmap = regmap; 214 215 hw = &prog->hw; 216 ret = clk_hw_register(NULL, &prog->hw); 217 if (ret) { 218 kfree(prog); 219 hw = ERR_PTR(ret); 220 } else { 221 pmc_register_pck(id); 222 } 223 224 return hw; 225 } 226 227 const struct clk_programmable_layout at91rm9200_programmable_layout = { 228 .pres_mask = 0x7, 229 .pres_shift = 2, 230 .css_mask = 0x3, 231 .have_slck_mck = 0, 232 .is_pres_direct = 0, 233 }; 234 235 const struct clk_programmable_layout at91sam9g45_programmable_layout = { 236 .pres_mask = 0x7, 237 .pres_shift = 2, 238 .css_mask = 0x3, 239 .have_slck_mck = 1, 240 .is_pres_direct = 0, 241 }; 242 243 const struct clk_programmable_layout at91sam9x5_programmable_layout = { 244 .pres_mask = 0x7, 245 .pres_shift = 4, 246 .css_mask = 0x7, 247 .have_slck_mck = 0, 248 .is_pres_direct = 0, 249 }; 250