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 	return clk;
163 }
164