1 // SPDX-License-Identifier: BSD-3-Clause
2 /*
3 * Copyright (c) 2025, Rockchip Electronics Co., Ltd.
4 */
5
6 #include <assert.h>
7 #include <errno.h>
8
9 #include <drivers/scmi.h>
10 #include <lib/mmio.h>
11 #include <platform_def.h>
12
13 #include <plat_private.h>
14 #include <scmi_clock.h>
15
16 #define MUX_ADDR_INFO 0
17 #define MUX_SHIFT_INFO 1
18 #define MUX_WIDTH_INFO 2
19 #define DIV_ADDR_INFO 3
20 #define DIV_SHIFT_INFO 4
21 #define DIV_WIDTH_INFO 5
22 #define GATE_ADDR_INFO 6
23 #define GATE_SHIFT_INFO 7
24
25 #define DIV_ROUND_UP(n, d) (((n) + (d) - 1) / (d))
26
27 #define abs(x) ({ \
28 long ret; \
29 if (sizeof(x) == sizeof(long)) { \
30 long __x = (x); \
31 ret = (__x < 0) ? -__x : __x; \
32 } else { \
33 int __x = (x); \
34 ret = (__x < 0) ? -__x : __x; \
35 } \
36 ret; \
37 })
38
clk_scmi_common_get_parent_rate(rk_scmi_clock_t * clock,int id)39 static unsigned long clk_scmi_common_get_parent_rate(rk_scmi_clock_t *clock,
40 int id)
41 {
42 rk_scmi_clock_t *p_clock;
43
44 if (clock->is_dynamic_prate != 0) {
45 p_clock = rockchip_scmi_get_clock(0, clock->parent_table[id]);
46 if (p_clock == NULL)
47 return 0;
48 if ((p_clock->clk_ops != NULL) && (p_clock->clk_ops->get_rate != NULL))
49 return p_clock->clk_ops->get_rate(p_clock);
50 else
51 return 0;
52 } else {
53 return clock->parent_table[id];
54 }
55 }
56
clk_scmi_common_get_rate(rk_scmi_clock_t * clock)57 unsigned long clk_scmi_common_get_rate(rk_scmi_clock_t *clock)
58 {
59 unsigned long parent_rate, sel, div;
60
61 sel = mmio_read_32(clock->info[MUX_ADDR_INFO]) >>
62 clock->info[MUX_SHIFT_INFO];
63 sel = sel & (BIT(clock->info[MUX_WIDTH_INFO]) - 1);
64 div = mmio_read_32(clock->info[DIV_ADDR_INFO]) >>
65 clock->info[DIV_SHIFT_INFO];
66 div = div & (BIT(clock->info[DIV_WIDTH_INFO]) - 1);
67 parent_rate = clk_scmi_common_get_parent_rate(clock, sel);
68
69 return parent_rate / (div + 1);
70 }
71
clk_scmi_common_set_rate(rk_scmi_clock_t * clock,unsigned long rate)72 int clk_scmi_common_set_rate(rk_scmi_clock_t *clock, unsigned long rate)
73 {
74 unsigned long parent_rate, now, best_rate = 0;
75 int i = 0, sel_mask, div_mask, best_sel = 0, best_div = 0, div;
76
77 if ((rate == 0) ||
78 (clock->info[MUX_WIDTH_INFO] == 0 && clock->info[DIV_WIDTH_INFO] == 0))
79 return SCMI_INVALID_PARAMETERS;
80
81 sel_mask = BIT(clock->info[MUX_WIDTH_INFO]) - 1;
82 div_mask = BIT(clock->info[DIV_WIDTH_INFO]) - 1;
83 if (clock->info[MUX_WIDTH_INFO] == 0) {
84 parent_rate = clk_scmi_common_get_parent_rate(clock, 0);
85 div = DIV_ROUND_UP(parent_rate, rate);
86 if (div > div_mask + 1)
87 div = div_mask + 1;
88 mmio_write_32(clock->info[DIV_ADDR_INFO],
89 BITS_WITH_WMASK(div - 1, div_mask,
90 clock->info[DIV_SHIFT_INFO]));
91 } else if (clock->info[DIV_WIDTH_INFO] == 0) {
92 for (i = 0; i <= sel_mask; i++) {
93 parent_rate = clk_scmi_common_get_parent_rate(clock, i);
94 now = parent_rate;
95 if (abs(rate - now) < abs(rate - best_rate)) {
96 best_rate = now;
97 best_sel = i;
98 }
99 }
100 if (best_rate == 0)
101 best_sel = 0;
102 mmio_write_32(clock->info[MUX_ADDR_INFO],
103 BITS_WITH_WMASK(best_sel, sel_mask,
104 clock->info[MUX_SHIFT_INFO]));
105 } else {
106 for (i = 0; i <= sel_mask; i++) {
107 parent_rate = clk_scmi_common_get_parent_rate(clock, i);
108 div = DIV_ROUND_UP(parent_rate, rate);
109 if (div > div_mask + 1)
110 div = div_mask + 1;
111 now = parent_rate / div;
112 if (abs(rate - now) < abs(rate - best_rate)) {
113 best_rate = now;
114 best_div = div;
115 best_sel = i;
116 }
117 }
118 if (best_rate == 0) {
119 best_div = div_mask + 1;
120 best_sel = 0;
121 }
122
123 mmio_write_32(clock->info[DIV_ADDR_INFO],
124 BITS_WITH_WMASK(div_mask, div_mask,
125 clock->info[DIV_SHIFT_INFO]));
126 mmio_write_32(clock->info[MUX_ADDR_INFO],
127 BITS_WITH_WMASK(best_sel, sel_mask,
128 clock->info[MUX_SHIFT_INFO]));
129 mmio_write_32(clock->info[DIV_ADDR_INFO],
130 BITS_WITH_WMASK(best_div - 1, div_mask,
131 clock->info[DIV_SHIFT_INFO]));
132 }
133 return 0;
134 }
135
clk_scmi_common_set_status(rk_scmi_clock_t * clock,bool status)136 int clk_scmi_common_set_status(rk_scmi_clock_t *clock, bool status)
137 {
138 mmio_write_32(clock->info[GATE_ADDR_INFO],
139 BITS_WITH_WMASK(!status, 0x1U,
140 clock->info[GATE_SHIFT_INFO]));
141
142 return 0;
143 }
144