1 // SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause
2 /*
3 * Copyright (C) 2015 - 2021 Atmel Corporation,
4 * Nicolas Ferre <nicolas.ferre@atmel.com>
5 *
6 * Based on clk-programmable & clk-peripheral drivers by Boris BREZILLON.
7 */
8
9 #include <io.h>
10 #include <kernel/delay.h>
11 #include <kernel/panic.h>
12 #include <mm/core_memprot.h>
13 #include <string.h>
14 #include <types_ext.h>
15
16 #include "at91_clk.h"
17
18 #define GENERATED_MAX_DIV 255
19
20 struct clk_generated {
21 vaddr_t base;
22 struct clk_range range;
23 uint32_t *mux_table;
24 uint32_t id;
25 uint32_t gckdiv;
26 const struct clk_pcr_layout *layout;
27 uint8_t parent_id;
28 int chg_pid;
29 };
30
clk_generated_enable(struct clk * clk)31 static TEE_Result clk_generated_enable(struct clk *clk)
32 {
33 struct clk_generated *gck = clk->priv;
34
35 io_write32(gck->base + gck->layout->offset,
36 (gck->id & gck->layout->pid_mask));
37 io_clrsetbits32(gck->base + gck->layout->offset,
38 AT91_PMC_PCR_GCKDIV_MASK | gck->layout->gckcss_mask |
39 gck->layout->cmd | AT91_PMC_PCR_GCKEN,
40 field_prep(gck->layout->gckcss_mask, gck->parent_id) |
41 gck->layout->cmd |
42 ((gck->gckdiv << AT91_PMC_PCR_GCKDIV_SHIFT) &
43 AT91_PMC_PCR_GCKDIV_MASK) |
44 AT91_PMC_PCR_GCKEN);
45
46 return TEE_SUCCESS;
47 }
48
clk_generated_disable(struct clk * clk)49 static void clk_generated_disable(struct clk *clk)
50 {
51 struct clk_generated *gck = clk->priv;
52
53 io_write32(gck->base + gck->layout->offset,
54 gck->id & gck->layout->pid_mask);
55 io_clrsetbits32(gck->base + gck->layout->offset, AT91_PMC_PCR_GCKEN,
56 gck->layout->cmd);
57 }
58
59 static unsigned long
clk_generated_get_rate(struct clk * clk,unsigned long parent_rate)60 clk_generated_get_rate(struct clk *clk, unsigned long parent_rate)
61 {
62 struct clk_generated *gck = clk->priv;
63
64 return UDIV_ROUND_NEAREST(parent_rate, gck->gckdiv + 1);
65 }
66
67 /* No modification of hardware as we have the flag CLK_SET_PARENT_GATE set */
clk_generated_set_parent(struct clk * clk,size_t index)68 static TEE_Result clk_generated_set_parent(struct clk *clk, size_t index)
69 {
70 struct clk_generated *gck = clk->priv;
71
72 if (index >= clk_get_num_parents(clk))
73 return TEE_ERROR_BAD_PARAMETERS;
74
75 if (gck->mux_table)
76 gck->parent_id = gck->mux_table[index];
77 else
78 gck->parent_id = index;
79
80 return TEE_SUCCESS;
81 }
82
clk_generated_get_parent(struct clk * clk)83 static size_t clk_generated_get_parent(struct clk *clk)
84 {
85 struct clk_generated *gck = clk->priv;
86 unsigned int i = 0;
87
88 if (gck->mux_table) {
89 for (i = 0; i < clk_get_num_parents(clk); i++)
90 if (gck->mux_table[i] == gck->parent_id)
91 return i;
92 panic("Can't get correct parent of clock");
93 } else {
94 return gck->parent_id;
95 }
96 }
97
98 /* No modification of hardware as we have the flag CLK_SET_RATE_GATE set */
clk_generated_set_rate(struct clk * clk,unsigned long rate,unsigned long parent_rate)99 static TEE_Result clk_generated_set_rate(struct clk *clk, unsigned long rate,
100 unsigned long parent_rate)
101 {
102 struct clk_generated *gck = clk->priv;
103 uint32_t div = 1;
104
105 if (!rate)
106 return TEE_ERROR_BAD_PARAMETERS;
107
108 if (gck->range.max && rate > gck->range.max)
109 return TEE_ERROR_BAD_PARAMETERS;
110
111 div = UDIV_ROUND_NEAREST(parent_rate, rate);
112 if (div > GENERATED_MAX_DIV + 1 || !div)
113 return TEE_ERROR_GENERIC;
114
115 gck->gckdiv = div - 1;
116
117 return TEE_SUCCESS;
118 }
119
120 static const struct clk_ops generated_ops = {
121 .enable = clk_generated_enable,
122 .disable = clk_generated_disable,
123 .get_rate = clk_generated_get_rate,
124 .get_parent = clk_generated_get_parent,
125 .set_parent = clk_generated_set_parent,
126 .set_rate = clk_generated_set_rate,
127 };
128
129 /**
130 * clk_generated_startup - Initialize a given clock to its default parent and
131 * divisor parameter.
132 *
133 * @gck: Generated clock to set the startup parameters for.
134 *
135 * Take parameters from the hardware and update local clock configuration
136 * accordingly.
137 */
clk_generated_startup(struct clk_generated * gck)138 static void clk_generated_startup(struct clk_generated *gck)
139 {
140 uint32_t tmp = 0;
141
142 io_write32(gck->base + gck->layout->offset,
143 (gck->id & gck->layout->pid_mask));
144 tmp = io_read32(gck->base + gck->layout->offset);
145
146 gck->parent_id = field_get(gck->layout->gckcss_mask, tmp);
147 gck->gckdiv = (tmp & AT91_PMC_PCR_GCKDIV_MASK) >>
148 AT91_PMC_PCR_GCKDIV_SHIFT;
149 }
150
151 struct clk *
at91_clk_register_generated(struct pmc_data * pmc,const struct clk_pcr_layout * layout,const char * name,struct clk ** parents,uint32_t * mux_table,uint8_t num_parents,uint8_t id,const struct clk_range * range,int chg_pid)152 at91_clk_register_generated(struct pmc_data *pmc,
153 const struct clk_pcr_layout *layout,
154 const char *name, struct clk **parents,
155 uint32_t *mux_table,
156 uint8_t num_parents, uint8_t id,
157 const struct clk_range *range,
158 int chg_pid)
159 {
160 struct clk_generated *gck = NULL;
161 struct clk *clk = NULL;
162
163 clk = clk_alloc(name, &generated_ops, parents, num_parents);
164 if (!clk)
165 return NULL;
166
167 gck = calloc(1, sizeof(*gck));
168 if (!gck) {
169 clk_free(clk);
170 return NULL;
171 }
172
173 clk->flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
174
175 gck->id = id;
176 gck->base = pmc->base;
177 memcpy(&gck->range, range, sizeof(gck->range));
178 gck->chg_pid = chg_pid;
179 gck->layout = layout;
180 gck->mux_table = mux_table;
181
182 clk->priv = gck;
183
184 clk_generated_startup(gck);
185
186 if (clk_register(clk)) {
187 clk_free(clk);
188 free(gck);
189 return NULL;
190 }
191 pmc_register_id(id);
192
193 return clk;
194 }
195