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 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 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 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 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