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 <mm/core_memprot.h> 11 #include <types_ext.h> 12 13 #include "at91_clk.h" 14 15 #define PROG_ID_MAX 7 16 17 #define PROG_STATUS_MASK(id) (1 << ((id) + 8)) 18 #define PROG_PRES(layout, pckr) \ 19 ({ \ 20 typeof(layout) __layout = layout; \ 21 \ 22 (((pckr) >> (__layout)->pres_shift) & (__layout)->pres_mask); \ 23 }) 24 #define PROG_MAX_RM9200_CSS 3 25 26 struct clk_programmable { 27 vaddr_t base; 28 uint8_t id; 29 const struct clk_programmable_layout *layout; 30 }; 31 32 static unsigned long clk_programmable_get_rate(struct clk *clk, 33 unsigned long parent_rate) 34 { 35 struct clk_programmable *prog = clk->priv; 36 const struct clk_programmable_layout *layout = prog->layout; 37 unsigned int pckr = io_read32(prog->base + AT91_PMC_PCKR(prog->id)); 38 unsigned long rate = 0; 39 40 if (layout->is_pres_direct) 41 rate = parent_rate / (PROG_PRES(layout, pckr) + 1); 42 else 43 rate = parent_rate >> PROG_PRES(layout, pckr); 44 45 return rate; 46 } 47 48 static TEE_Result clk_programmable_set_parent(struct clk *clk, size_t index) 49 { 50 struct clk_programmable *prog = clk->priv; 51 const struct clk_programmable_layout *layout = prog->layout; 52 unsigned int mask = layout->css_mask; 53 unsigned int pckr = index; 54 55 if (layout->have_slck_mck) 56 mask |= AT91_PMC_CSSMCK_MCK; 57 58 if (index > layout->css_mask) { 59 if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck) 60 return TEE_ERROR_BAD_PARAMETERS; 61 62 pckr |= AT91_PMC_CSSMCK_MCK; 63 } 64 65 io_clrsetbits32(prog->base + AT91_PMC_PCKR(prog->id), mask, pckr); 66 67 return TEE_SUCCESS; 68 } 69 70 static size_t clk_programmable_get_parent(struct clk *clk) 71 { 72 struct clk_programmable *prog = clk->priv; 73 const struct clk_programmable_layout *layout = prog->layout; 74 unsigned int pckr = io_read32(prog->base + AT91_PMC_PCKR(prog->id)); 75 size_t ret = 0; 76 77 ret = pckr & layout->css_mask; 78 79 if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret) 80 ret = PROG_MAX_RM9200_CSS + 1; 81 82 return ret; 83 } 84 85 static unsigned int flsi(unsigned int val) 86 { 87 if (val == 0) 88 return 0; 89 90 return sizeof(unsigned int) * 8 - __builtin_clz(val); 91 } 92 93 static TEE_Result clk_programmable_set_rate(struct clk *clk, unsigned long rate, 94 unsigned long parent_rate) 95 { 96 struct clk_programmable *prog = clk->priv; 97 const struct clk_programmable_layout *layout = prog->layout; 98 unsigned long div = parent_rate / rate; 99 int shift = 0; 100 101 if (!div) 102 return TEE_ERROR_BAD_PARAMETERS; 103 104 if (layout->is_pres_direct) { 105 shift = div - 1; 106 107 if (shift > layout->pres_mask) 108 return TEE_ERROR_BAD_PARAMETERS; 109 } else { 110 shift = flsi(div) - 1; 111 112 if (div != (1ULL << shift)) 113 return TEE_ERROR_BAD_PARAMETERS; 114 115 if (shift >= layout->pres_mask) 116 return TEE_ERROR_BAD_PARAMETERS; 117 } 118 119 io_clrsetbits32(prog->base + AT91_PMC_PCKR(prog->id), 120 layout->pres_mask << layout->pres_shift, 121 shift << layout->pres_shift); 122 123 return TEE_SUCCESS; 124 } 125 126 static const struct clk_ops programmable_ops = { 127 .get_rate = clk_programmable_get_rate, 128 .get_parent = clk_programmable_get_parent, 129 .set_parent = clk_programmable_set_parent, 130 .set_rate = clk_programmable_set_rate, 131 }; 132 133 struct clk * 134 at91_clk_register_programmable(struct pmc_data *pmc, 135 const char *name, struct clk **parents, 136 uint8_t num_parents, uint8_t id, 137 const struct clk_programmable_layout *layout) 138 { 139 struct clk_programmable *prog = NULL; 140 struct clk *clk = NULL; 141 142 assert(id <= PROG_ID_MAX); 143 144 clk = clk_alloc(name, &programmable_ops, parents, num_parents); 145 prog = calloc(1, sizeof(*prog)); 146 if (!prog || !clk) 147 return NULL; 148 149 prog->id = id; 150 prog->layout = layout; 151 prog->base = pmc->base; 152 153 clk->priv = prog; 154 clk->flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 155 156 if (clk_register(clk)) { 157 clk_free(clk); 158 free(prog); 159 return NULL; 160 } 161 162 pmc_register_pck(id); 163 164 return clk; 165 } 166