1 // SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause 2 /* 3 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 4 * Copyright (C) 2021 Microchip 5 */ 6 7 #include <io.h> 8 #include <kernel/delay.h> 9 #include <kernel/panic.h> 10 #include <util.h> 11 #include <mm/core_memprot.h> 12 #include <types_ext.h> 13 14 #include "at91_clk.h" 15 16 #define PLL_STATUS_MASK(id) BIT(1 + (id)) 17 #define PLL_REG(id) (AT91_CKGR_PLLAR + ((id) * 4)) 18 #define PLL_DIV_MASK 0xff 19 #define PLL_DIV_MAX PLL_DIV_MASK 20 #define PLL_DIV(reg) ((reg) & PLL_DIV_MASK) 21 #define PLL_MUL(reg, layout) \ 22 ({ \ 23 typeof(layout) __layout = layout; \ 24 \ 25 (((reg) >> (__layout)->mul_shift) & (__layout)->mul_mask); \ 26 }) 27 #define PLL_MUL_MIN 2 28 #define PLL_MUL_MASK(layout) ((layout)->mul_mask) 29 #define PLL_MUL_MAX(layout) (PLL_MUL_MASK(layout) + 1) 30 #define PLL_ICPR_SHIFT(id) ((id) * 16) 31 #define PLL_ICPR_MASK(id) (0xffff << PLL_ICPR_SHIFT(id)) 32 #define PLL_MAX_COUNT 0x3f 33 #define PLL_COUNT_SHIFT 8 34 #define PLL_OUT_SHIFT 14 35 #define PLL_MAX_ID 1 36 37 struct clk_pll { 38 vaddr_t base; 39 uint8_t id; 40 uint8_t div; 41 uint8_t range; 42 uint16_t mul; 43 const struct clk_pll_layout *layout; 44 const struct clk_pll_charac *charac; 45 }; 46 47 static bool clk_pll_ready(vaddr_t base, int id) 48 { 49 unsigned int status = io_read32(base + AT91_PMC_SR); 50 51 return status & PLL_STATUS_MASK(id); 52 } 53 54 static TEE_Result clk_pll_enable(struct clk *clk) 55 { 56 struct clk_pll *pll = clk->priv; 57 const struct clk_pll_layout *layout = pll->layout; 58 const struct clk_pll_charac *charac = pll->charac; 59 uint8_t id = pll->id; 60 uint32_t mask = PLL_STATUS_MASK(id); 61 int offset = PLL_REG(id); 62 uint8_t out = 0; 63 unsigned int pllr = 0; 64 unsigned int status = 0; 65 uint8_t div = 0; 66 uint16_t mul = 0; 67 68 pllr = io_read32(pll->base + offset); 69 div = PLL_DIV(pllr); 70 mul = PLL_MUL(pllr, layout); 71 72 status = io_read32(pll->base + AT91_PMC_SR); 73 if ((status & mask) && 74 (div == pll->div && mul == pll->mul)) 75 return TEE_SUCCESS; 76 77 if (charac->out) 78 out = charac->out[pll->range]; 79 80 if (charac->icpll) 81 io_clrsetbits32(pll->base + AT91_PMC_PLLICPR, PLL_ICPR_MASK(id), 82 charac->icpll[pll->range] << 83 PLL_ICPR_SHIFT(id)); 84 85 io_clrsetbits32(pll->base + offset, layout->pllr_mask, 86 pll->div | (PLL_MAX_COUNT << PLL_COUNT_SHIFT) | 87 (out << PLL_OUT_SHIFT) | 88 ((pll->mul & layout->mul_mask) << layout->mul_shift)); 89 90 while (!clk_pll_ready(pll->base, pll->id)) 91 ; 92 93 return TEE_SUCCESS; 94 } 95 96 static void clk_pll_disable(struct clk *clk) 97 { 98 struct clk_pll *pll = clk->priv; 99 unsigned int mask = pll->layout->pllr_mask; 100 101 io_clrsetbits32(pll->base + PLL_REG(pll->id), mask, ~mask); 102 } 103 104 static unsigned long clk_pll_get_rate(struct clk *clk, 105 unsigned long parent_rate) 106 { 107 struct clk_pll *pll = clk->priv; 108 109 if (!pll->div || !pll->mul) 110 return 0; 111 112 return (parent_rate / pll->div) * (pll->mul + 1); 113 } 114 115 static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate, 116 unsigned long parent_rate, 117 uint32_t *div, uint32_t *mul, 118 uint32_t *index) 119 { 120 const struct clk_pll_layout *layout = pll->layout; 121 const struct clk_pll_charac *charac = pll->charac; 122 unsigned long bestremainder = ULONG_MAX; 123 unsigned long maxdiv = 1; 124 unsigned long mindiv = 1; 125 unsigned long tmpdiv = 1; 126 long bestrate = -1; 127 unsigned long bestdiv = 0; 128 unsigned long bestmul = 0; 129 int i = 0; 130 131 /* Check if parent_rate is a valid input rate */ 132 if (parent_rate < charac->input.min) 133 return -1; 134 135 /* 136 * Calculate minimum divider based on the minimum multiplier, the 137 * parent_rate and the requested rate. 138 * Should always be 2 according to the input and output charac 139 * of the PLL blocks. 140 */ 141 mindiv = (parent_rate * PLL_MUL_MIN) / rate; 142 if (!mindiv) 143 mindiv = 1; 144 145 if (parent_rate > charac->input.max) { 146 tmpdiv = DIV_ROUND_UP(parent_rate, charac->input.max); 147 if (tmpdiv > PLL_DIV_MAX) 148 return -1; 149 150 if (tmpdiv > mindiv) 151 mindiv = tmpdiv; 152 } 153 154 /* 155 * Calculate the maximum divider which is limited by PLL register 156 * layout (limited by the MUL or DIV field size). 157 */ 158 maxdiv = DIV_ROUND_UP(parent_rate * PLL_MUL_MAX(layout), rate); 159 if (maxdiv > PLL_DIV_MAX) 160 maxdiv = PLL_DIV_MAX; 161 162 /* 163 * Iterate over the acceptable divider values to find the best 164 * divider/multiplier pair (the one that generates the closest 165 * rate to the requested one). 166 */ 167 for (tmpdiv = mindiv; tmpdiv <= maxdiv; tmpdiv++) { 168 unsigned long remainder = 0; 169 unsigned long tmprate = 0; 170 unsigned long tmpmul = 0; 171 172 /* 173 * Calculate the multiplier associated with the current 174 * divider that provide the closest rate to the requested one. 175 */ 176 tmpmul = UDIV_ROUND_NEAREST(rate, parent_rate / tmpdiv); 177 tmprate = (parent_rate / tmpdiv) * tmpmul; 178 if (tmprate > rate) 179 remainder = tmprate - rate; 180 else 181 remainder = rate - tmprate; 182 183 /* 184 * Compare the remainder with the best remainder found until 185 * now and elect a new best multiplier/divider pair if the 186 * current remainder is smaller than the best one. 187 */ 188 if (remainder < bestremainder) { 189 bestremainder = remainder; 190 bestdiv = tmpdiv; 191 bestmul = tmpmul; 192 bestrate = tmprate; 193 } 194 195 /* 196 * We've found a perfect match! 197 * Stop searching now and use this multiplier/divider pair. 198 */ 199 if (!remainder) 200 break; 201 } 202 203 /* We haven't found any multiplier/divider pair => return -ERANGE */ 204 if (bestrate < 0) 205 return bestrate; 206 207 /* Check if bestrate is a valid output rate */ 208 for (i = 0; i < charac->num_output; i++) { 209 if (bestrate >= (long)charac->output[i].min && 210 bestrate <= (long)charac->output[i].max) 211 break; 212 } 213 214 if (i >= charac->num_output) 215 return -1; 216 217 if (div) 218 *div = bestdiv; 219 if (mul) 220 *mul = bestmul - 1; 221 if (index) 222 *index = i; 223 224 return bestrate; 225 } 226 227 static TEE_Result clk_pll_set_rate(struct clk *clk, unsigned long rate, 228 unsigned long parent_rate) 229 { 230 struct clk_pll *pll = clk->priv; 231 long ret = -1; 232 uint32_t div = 1; 233 uint32_t mul = 0; 234 uint32_t index = 0; 235 236 ret = clk_pll_get_best_div_mul(pll, rate, parent_rate, 237 &div, &mul, &index); 238 if (ret < 0) 239 return TEE_ERROR_BAD_PARAMETERS; 240 241 pll->range = index; 242 pll->div = div; 243 pll->mul = mul; 244 245 return TEE_SUCCESS; 246 } 247 248 static const struct clk_ops pll_ops = { 249 .enable = clk_pll_enable, 250 .disable = clk_pll_disable, 251 .get_rate = clk_pll_get_rate, 252 .set_rate = clk_pll_set_rate, 253 }; 254 255 struct clk * 256 at91_clk_register_pll(struct pmc_data *pmc, const char *name, 257 struct clk *parent, uint8_t id, 258 const struct clk_pll_layout *layout, 259 const struct clk_pll_charac *charac) 260 { 261 struct clk *clk = NULL; 262 struct clk_pll *pll = NULL; 263 int offset = PLL_REG(id); 264 uint32_t pllr = 0; 265 266 if (!name || !parent) 267 return NULL; 268 269 clk = clk_alloc(name, &pll_ops, &parent, 1); 270 if (!clk) 271 return NULL; 272 273 if (id > PLL_MAX_ID) 274 return NULL; 275 276 pll = calloc(1, sizeof(*pll)); 277 if (!pll) { 278 clk_free(clk); 279 return NULL; 280 } 281 282 pll->id = id; 283 pll->layout = layout; 284 pll->charac = charac; 285 pll->base = pmc->base; 286 pllr = io_read32(pmc->base + offset); 287 pll->div = PLL_DIV(pllr); 288 pll->mul = PLL_MUL(pllr, layout); 289 290 clk->flags = CLK_SET_RATE_GATE; 291 clk->priv = pll; 292 293 if (clk_register(clk)) { 294 clk_free(clk); 295 free(pll); 296 return NULL; 297 } 298 299 return clk; 300 } 301 302 const struct clk_pll_layout sama5d3_pll_layout = { 303 .pllr_mask = 0x1FFFFFF, 304 .mul_shift = 18, 305 .mul_mask = 0x7F, 306 }; 307