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
clk_programmable_get_rate(struct clk * clk,unsigned long parent_rate)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
clk_programmable_set_parent(struct clk * clk,size_t index)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
clk_programmable_get_parent(struct clk * clk)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
flsi(unsigned int val)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
clk_programmable_set_rate(struct clk * clk,unsigned long rate,unsigned long parent_rate)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 *
at91_clk_register_programmable(struct pmc_data * pmc,const char * name,struct clk ** parents,uint8_t num_parents,uint8_t id,const struct clk_programmable_layout * layout)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