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_MASK 0x7 24 #define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & PROG_PRES_MASK) 25 #define PROG_MAX_RM9200_CSS 3 26 27 struct clk_programmable { 28 struct clk_hw hw; 29 struct regmap *regmap; 30 u8 id; 31 const struct clk_programmable_layout *layout; 32 }; 33 34 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) 35 36 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, 37 unsigned long parent_rate) 38 { 39 struct clk_programmable *prog = to_clk_programmable(hw); 40 unsigned int pckr; 41 42 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 43 44 return parent_rate >> PROG_PRES(prog->layout, pckr); 45 } 46 47 static int clk_programmable_determine_rate(struct clk_hw *hw, 48 struct clk_rate_request *req) 49 { 50 struct clk_hw *parent; 51 long best_rate = -EINVAL; 52 unsigned long parent_rate; 53 unsigned long tmp_rate; 54 int shift; 55 int i; 56 57 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 58 parent = clk_hw_get_parent_by_index(hw, i); 59 if (!parent) 60 continue; 61 62 parent_rate = clk_hw_get_rate(parent); 63 for (shift = 0; shift < PROG_PRES_MASK; shift++) { 64 tmp_rate = parent_rate >> shift; 65 if (tmp_rate <= req->rate) 66 break; 67 } 68 69 if (tmp_rate > req->rate) 70 continue; 71 72 if (best_rate < 0 || 73 (req->rate - tmp_rate) < (req->rate - best_rate)) { 74 best_rate = tmp_rate; 75 req->best_parent_rate = parent_rate; 76 req->best_parent_hw = parent; 77 } 78 79 if (!best_rate) 80 break; 81 } 82 83 if (best_rate < 0) 84 return best_rate; 85 86 req->rate = best_rate; 87 return 0; 88 } 89 90 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) 91 { 92 struct clk_programmable *prog = to_clk_programmable(hw); 93 const struct clk_programmable_layout *layout = prog->layout; 94 unsigned int mask = layout->css_mask; 95 unsigned int pckr = index; 96 97 if (layout->have_slck_mck) 98 mask |= AT91_PMC_CSSMCK_MCK; 99 100 if (index > layout->css_mask) { 101 if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck) 102 return -EINVAL; 103 104 pckr |= AT91_PMC_CSSMCK_MCK; 105 } 106 107 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr); 108 109 return 0; 110 } 111 112 static u8 clk_programmable_get_parent(struct clk_hw *hw) 113 { 114 struct clk_programmable *prog = to_clk_programmable(hw); 115 const struct clk_programmable_layout *layout = prog->layout; 116 unsigned int pckr; 117 u8 ret; 118 119 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 120 121 ret = pckr & layout->css_mask; 122 123 if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret) 124 ret = PROG_MAX_RM9200_CSS + 1; 125 126 return ret; 127 } 128 129 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, 130 unsigned long parent_rate) 131 { 132 struct clk_programmable *prog = to_clk_programmable(hw); 133 const struct clk_programmable_layout *layout = prog->layout; 134 unsigned long div = parent_rate / rate; 135 unsigned int pckr; 136 int shift = 0; 137 138 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); 139 140 if (!div) 141 return -EINVAL; 142 143 shift = fls(div) - 1; 144 145 if (div != (1 << shift)) 146 return -EINVAL; 147 148 if (shift >= PROG_PRES_MASK) 149 return -EINVAL; 150 151 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), 152 PROG_PRES_MASK << layout->pres_shift, 153 shift << layout->pres_shift); 154 155 return 0; 156 } 157 158 static const struct clk_ops programmable_ops = { 159 .recalc_rate = clk_programmable_recalc_rate, 160 .determine_rate = clk_programmable_determine_rate, 161 .get_parent = clk_programmable_get_parent, 162 .set_parent = clk_programmable_set_parent, 163 .set_rate = clk_programmable_set_rate, 164 }; 165 166 struct clk_hw * __init 167 at91_clk_register_programmable(struct regmap *regmap, 168 const char *name, const char **parent_names, 169 u8 num_parents, u8 id, 170 const struct clk_programmable_layout *layout) 171 { 172 struct clk_programmable *prog; 173 struct clk_hw *hw; 174 struct clk_init_data init; 175 int ret; 176 177 if (id > PROG_ID_MAX) 178 return ERR_PTR(-EINVAL); 179 180 prog = kzalloc(sizeof(*prog), GFP_KERNEL); 181 if (!prog) 182 return ERR_PTR(-ENOMEM); 183 184 init.name = name; 185 init.ops = &programmable_ops; 186 init.parent_names = parent_names; 187 init.num_parents = num_parents; 188 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 189 190 prog->id = id; 191 prog->layout = layout; 192 prog->hw.init = &init; 193 prog->regmap = regmap; 194 195 hw = &prog->hw; 196 ret = clk_hw_register(NULL, &prog->hw); 197 if (ret) { 198 kfree(prog); 199 hw = ERR_PTR(ret); 200 } else { 201 pmc_register_pck(id); 202 } 203 204 return hw; 205 } 206 207 const struct clk_programmable_layout at91rm9200_programmable_layout = { 208 .pres_shift = 2, 209 .css_mask = 0x3, 210 .have_slck_mck = 0, 211 }; 212 213 const struct clk_programmable_layout at91sam9g45_programmable_layout = { 214 .pres_shift = 2, 215 .css_mask = 0x3, 216 .have_slck_mck = 1, 217 }; 218 219 const struct clk_programmable_layout at91sam9x5_programmable_layout = { 220 .pres_shift = 4, 221 .css_mask = 0x7, 222 .have_slck_mck = 0, 223 }; 224