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 <mm/core_memprot.h> 10 #include <types_ext.h> 11 12 #include "at91_clk.h" 13 14 #define MASTER_PRES_MASK 0x7 15 #define MASTER_PRES_MAX MASTER_PRES_MASK 16 #define MASTER_DIV_SHIFT 8 17 #define MASTER_DIV_MASK 0x7 18 #define MASTER_MAX_ID 4 /* Total 5 MCK clocks for SAMA7G5 */ 19 20 struct clk_master { 21 vaddr_t base; 22 const struct clk_master_layout *layout; 23 const struct clk_master_charac *charac; 24 uint32_t *mux_table; 25 uint32_t mckr; 26 int chg_pid; 27 uint8_t div; 28 uint8_t id; /* ID of MCK clocks for SAMA7G5, MCK0 ~ MCK4 */ 29 uint8_t parent; /* the source clock for SAMA7G5 MCKx */ 30 }; 31 32 static bool clk_master_ready(struct clk_master *master) 33 { 34 uint32_t status = io_read32(master->base + AT91_PMC_SR); 35 36 return status & AT91_PMC_MCKRDY; 37 } 38 39 static TEE_Result clk_master_enable(struct clk *clk) 40 { 41 struct clk_master *master = clk->priv; 42 43 while (!clk_master_ready(master)) 44 ; 45 46 return TEE_SUCCESS; 47 } 48 49 static unsigned long clk_master_div_get_rate(struct clk *clk, 50 unsigned long parent_rate) 51 { 52 uint8_t div = 1; 53 uint32_t mckr = 0; 54 unsigned long rate = parent_rate; 55 struct clk_master *master = clk->priv; 56 const struct clk_master_layout *layout = master->layout; 57 const struct clk_master_charac *charac = master->charac; 58 59 mckr = io_read32(master->base + master->layout->offset); 60 61 mckr &= layout->mask; 62 63 div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; 64 65 rate /= charac->divisors[div]; 66 67 if (rate < charac->output.min) 68 IMSG("master clk div is underclocked"); 69 else if (rate > charac->output.max) 70 IMSG("master clk div is overclocked"); 71 72 return rate; 73 } 74 75 static const struct clk_ops master_div_ops = { 76 .enable = clk_master_enable, 77 .get_rate = clk_master_div_get_rate, 78 }; 79 80 static unsigned long clk_master_pres_get_rate(struct clk *clk, 81 unsigned long parent_rate) 82 { 83 struct clk_master *master = clk->priv; 84 const struct clk_master_charac *charac = master->charac; 85 uint32_t val = 0; 86 unsigned int pres = 0; 87 88 val = io_read32(master->base + master->layout->offset); 89 90 pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 91 if (pres != 3 || !charac->have_div3_pres) 92 pres = BIT(pres); 93 94 return UDIV_ROUND_NEAREST(parent_rate, pres); 95 } 96 97 static size_t clk_master_pres_get_parent(struct clk *clk) 98 { 99 struct clk_master *master = clk->priv; 100 uint32_t mckr = 0; 101 102 mckr = io_read32(master->base + master->layout->offset); 103 104 return mckr & AT91_PMC_CSS; 105 } 106 107 static const struct clk_ops master_pres_ops = { 108 .enable = clk_master_enable, 109 .get_rate = clk_master_pres_get_rate, 110 .get_parent = clk_master_pres_get_parent, 111 }; 112 113 static struct clk * 114 at91_clk_register_master_internal(struct pmc_data *pmc, 115 const char *name, int num_parents, 116 struct clk **parents, 117 const struct clk_master_layout *layout, 118 const struct clk_master_charac *charac, 119 const struct clk_ops *ops, int chg_pid) 120 { 121 struct clk_master *master = NULL; 122 struct clk *clk = NULL; 123 124 if (!name || !num_parents || !parents) 125 return NULL; 126 127 clk = clk_alloc(name, ops, parents, num_parents); 128 if (!clk) 129 return NULL; 130 131 master = calloc(1, sizeof(*master)); 132 if (!master) { 133 clk_free(clk); 134 return NULL; 135 } 136 137 master->layout = layout; 138 master->charac = charac; 139 master->base = pmc->base; 140 master->chg_pid = chg_pid; 141 142 clk->priv = master; 143 clk->flags = CLK_SET_RATE_GATE; 144 145 if (clk_register(clk)) { 146 clk_free(clk); 147 free(master); 148 return NULL; 149 } 150 151 return clk; 152 } 153 154 struct clk * 155 at91_clk_register_master_pres(struct pmc_data *pmc, 156 const char *name, int num_parents, 157 struct clk **parents, 158 const struct clk_master_layout *layout, 159 const struct clk_master_charac *charac, 160 int chg_pid) 161 { 162 return at91_clk_register_master_internal(pmc, name, num_parents, 163 parents, layout, 164 charac, 165 &master_pres_ops, chg_pid); 166 } 167 168 struct clk * 169 at91_clk_register_master_div(struct pmc_data *pmc, 170 const char *name, struct clk *parent, 171 const struct clk_master_layout *layout, 172 const struct clk_master_charac *charac) 173 { 174 return at91_clk_register_master_internal(pmc, name, 1, 175 &parent, layout, 176 charac, 177 &master_div_ops, -1); 178 } 179 180 const struct clk_master_layout at91sam9x5_master_layout = { 181 .mask = 0x373, 182 .pres_shift = 4, 183 .offset = AT91_PMC_MCKR, 184 }; 185 186 static size_t clk_sama7g5_master_get_parent(struct clk *hw) 187 { 188 struct clk_master *master = hw->priv; 189 size_t i = 0; 190 191 for (i = 0; i < hw->num_parents; i++) 192 if (master->mux_table[i] == master->parent) 193 return i; 194 195 panic("Can't get correct parent of clock"); 196 } 197 198 static TEE_Result clk_sama7g5_master_set_parent(struct clk *hw, size_t index) 199 { 200 struct clk_master *master = hw->priv; 201 202 if (index >= hw->num_parents) 203 return TEE_ERROR_BAD_PARAMETERS; 204 205 master->parent = master->mux_table[index]; 206 207 return TEE_SUCCESS; 208 } 209 210 static TEE_Result clk_sama7g5_master_set_rate(struct clk *hw, 211 unsigned long rate, 212 unsigned long parent_rate) 213 { 214 struct clk_master *master = hw->priv; 215 unsigned long div = 0; 216 217 div = UDIV_ROUND_NEAREST(parent_rate, rate); 218 if (div > (1 << (MASTER_PRES_MAX - 1)) || 219 (!IS_POWER_OF_TWO(div) && div != 3)) 220 return TEE_ERROR_BAD_PARAMETERS; 221 222 /* 223 * Divisor Value: Select the division ratio to be applied to the 224 * selected clock to generate the corresponding MCKx. 225 * Value | Description 226 * 0 | Selected clock divided by 1 227 * 1 | Selected clock divided by 2 228 * 2 | Selected clock divided by 4 229 * 3 | Selected clock divided by 8 230 * 4 | Selected clock divided by 16 231 * 5 | Selected clock divided by 32 232 * 6 | Selected clock divided by 64 233 * 7 | Selected clock divided by 3 234 */ 235 if (div == 3) 236 master->div = MASTER_PRES_MAX; 237 else 238 master->div = ffs(div) - 1; 239 240 return TEE_SUCCESS; 241 } 242 243 static unsigned long clk_sama7g5_master_get_rate(struct clk *hw, 244 unsigned long parent_rate) 245 { 246 struct clk_master *master = hw->priv; 247 unsigned long rate = parent_rate >> master->div; 248 249 if (master->div == 7) 250 rate = parent_rate / 3; 251 252 return rate; 253 } 254 255 static const struct clk_ops sama7g5_master_ops = { 256 .set_rate = clk_sama7g5_master_set_rate, 257 .get_rate = clk_sama7g5_master_get_rate, 258 .get_parent = clk_sama7g5_master_get_parent, 259 .set_parent = clk_sama7g5_master_set_parent, 260 }; 261 262 struct clk *at91_clk_sama7g5_register_master(struct pmc_data *pmc, 263 const char *name, 264 int num_parents, 265 struct clk **parent, 266 uint32_t *mux_table, 267 uint8_t id, 268 int chg_pid) 269 { 270 struct clk_master *master = NULL; 271 struct clk *hw = NULL; 272 unsigned int val = 0; 273 274 if (!name || !num_parents || !parent || !mux_table || 275 id > MASTER_MAX_ID) 276 return NULL; 277 278 master = calloc(1, sizeof(*master)); 279 if (!master) 280 return NULL; 281 282 hw = clk_alloc(name, &sama7g5_master_ops, parent, num_parents); 283 if (!hw) { 284 free(master); 285 return NULL; 286 } 287 288 hw->priv = master; 289 master->base = pmc->base; 290 master->id = id; 291 master->chg_pid = chg_pid; 292 master->mux_table = mux_table; 293 294 io_write32(master->base + AT91_PMC_MCR_V2, master->id); 295 val = io_read32(master->base + AT91_PMC_MCR_V2); 296 master->parent = (val & AT91_PMC_MCR_V2_CSS_MASK) >> 297 AT91_PMC_MCR_V2_CSS_SHIFT; 298 master->div = (val & AT91_PMC_MCR_V2_DIV_MASK) >> MASTER_DIV_SHIFT; 299 300 if (clk_register(hw)) { 301 clk_free(hw); 302 free(master); 303 return NULL; 304 } 305 306 return hw; 307 } 308