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 PERIPHERAL_ID_MIN 2
16 #define PERIPHERAL_ID_MASK 31
17 #define PERIPHERAL_MASK(id) BIT((id) & PERIPHERAL_ID_MASK)
18
19 #define PERIPHERAL_MAX_SHIFT 3
20
21 struct clk_sam9x5_peripheral {
22 vaddr_t base;
23 struct clk_range range;
24 uint32_t id;
25 uint32_t div;
26 const struct clk_pcr_layout *layout;
27 bool auto_div;
28 };
29
clk_sam9x5_peripheral_autodiv(struct clk * clk)30 static void clk_sam9x5_peripheral_autodiv(struct clk *clk)
31 {
32 struct clk *parent = NULL;
33 struct clk_sam9x5_peripheral *periph = clk->priv;
34 unsigned long parent_rate = 0;
35 int shift = 0;
36
37 if (!periph->auto_div)
38 return;
39
40 if (periph->range.max) {
41 parent = clk_get_parent_by_index(clk, 0);
42 parent_rate = clk_get_rate(parent);
43 if (!parent_rate)
44 return;
45
46 for (shift = 0; shift < PERIPHERAL_MAX_SHIFT; shift++) {
47 if (parent_rate >> shift <= periph->range.max)
48 break;
49 }
50 }
51
52 periph->auto_div = false;
53 periph->div = shift;
54 }
55
clk_sam9x5_peripheral_enable(struct clk * clk)56 static TEE_Result clk_sam9x5_peripheral_enable(struct clk *clk)
57 {
58 struct clk_sam9x5_peripheral *periph = clk->priv;
59
60 if (periph->id < PERIPHERAL_ID_MIN)
61 return TEE_SUCCESS;
62
63 io_write32(periph->base + periph->layout->offset,
64 (periph->id & periph->layout->pid_mask));
65 io_clrsetbits32(periph->base + periph->layout->offset,
66 periph->layout->div_mask | periph->layout->cmd |
67 AT91_PMC_PCR_EN,
68 field_prep(periph->layout->div_mask, periph->div) |
69 periph->layout->cmd |
70 AT91_PMC_PCR_EN);
71
72 return TEE_SUCCESS;
73 }
74
clk_sam9x5_peripheral_disable(struct clk * clk)75 static void clk_sam9x5_peripheral_disable(struct clk *clk)
76 {
77 struct clk_sam9x5_peripheral *periph = clk->priv;
78
79 if (periph->id < PERIPHERAL_ID_MIN)
80 return;
81
82 io_write32(periph->base + periph->layout->offset,
83 (periph->id & periph->layout->pid_mask));
84 io_clrsetbits32(periph->base + periph->layout->offset,
85 AT91_PMC_PCR_EN | periph->layout->cmd,
86 periph->layout->cmd);
87 }
88
89 static unsigned long
clk_sam9x5_peripheral_get_rate(struct clk * clk,unsigned long parent_rate)90 clk_sam9x5_peripheral_get_rate(struct clk *clk,
91 unsigned long parent_rate)
92 {
93 struct clk_sam9x5_peripheral *periph = clk->priv;
94 uint32_t status = 0;
95
96 if (periph->id < PERIPHERAL_ID_MIN)
97 return parent_rate;
98
99 io_write32(periph->base + periph->layout->offset,
100 periph->id & periph->layout->pid_mask);
101 status = io_read32(periph->base + periph->layout->offset);
102
103 if (status & AT91_PMC_PCR_EN) {
104 periph->div = field_get(periph->layout->div_mask, status);
105 periph->auto_div = false;
106 } else {
107 clk_sam9x5_peripheral_autodiv(clk);
108 }
109
110 return parent_rate >> periph->div;
111 }
112
clk_sam9x5_peripheral_set_rate(struct clk * clk,unsigned long rate,unsigned long parent_rate)113 static TEE_Result clk_sam9x5_peripheral_set_rate(struct clk *clk,
114 unsigned long rate,
115 unsigned long parent_rate)
116 {
117 unsigned int shift = 0;
118 struct clk_sam9x5_peripheral *periph = clk->priv;
119
120 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
121 if (parent_rate == rate)
122 return TEE_SUCCESS;
123 else
124 return TEE_ERROR_GENERIC;
125 }
126
127 if (periph->range.max && rate > periph->range.max)
128 return TEE_ERROR_GENERIC;
129
130 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
131 if (parent_rate >> shift == rate) {
132 periph->auto_div = false;
133 periph->div = shift;
134 return TEE_SUCCESS;
135 }
136 }
137
138 return TEE_ERROR_GENERIC;
139 }
140
141 static const struct clk_ops sam9x5_peripheral_ops = {
142 .enable = clk_sam9x5_peripheral_enable,
143 .disable = clk_sam9x5_peripheral_disable,
144 .get_rate = clk_sam9x5_peripheral_get_rate,
145 .set_rate = clk_sam9x5_peripheral_set_rate,
146 };
147
148 struct clk *
at91_clk_register_sam9x5_periph(struct pmc_data * pmc,const struct clk_pcr_layout * layout,const char * name,struct clk * parent,uint32_t id,const struct clk_range * range)149 at91_clk_register_sam9x5_periph(struct pmc_data *pmc,
150 const struct clk_pcr_layout *layout,
151 const char *name, struct clk *parent,
152 uint32_t id, const struct clk_range *range)
153 {
154 struct clk_sam9x5_peripheral *periph = NULL;
155 struct clk *clk = NULL;
156
157 if (!name || !parent)
158 return NULL;
159
160 clk = clk_alloc(name, &sam9x5_peripheral_ops, &parent, 1);
161 if (!clk)
162 return NULL;
163
164 periph = calloc(1, sizeof(*periph));
165 if (!periph) {
166 clk_free(clk);
167 return NULL;
168 }
169
170 periph->id = id;
171 periph->div = 0;
172 periph->base = pmc->base;
173 if (layout->div_mask)
174 periph->auto_div = true;
175 periph->layout = layout;
176 periph->range = *range;
177
178 clk->priv = periph;
179
180 if (clk_register(clk)) {
181 clk_free(clk);
182 free(periph);
183 return 0;
184 }
185
186 clk_sam9x5_peripheral_autodiv(clk);
187
188 return clk;
189 }
190