1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2021, Microchip 4 */ 5 6 #include <drivers/clk.h> 7 #include <drivers/scmi.h> 8 #include <drivers/scmi-msg.h> 9 #include <kernel/boot.h> 10 #include <string.h> 11 #include <sys/queue.h> 12 13 #include "clock.h" 14 15 /** 16 * struct scmi_clk - Binds an SCMI channel/clock to a core clk reference 17 * @clk: Core clock reference 18 * @channel_id: SCMI server channel handle exposing the clock 19 * @scmi_id: SCMI clock domain ID 20 * @enabled: SCMI clock state 21 * @link: Reference in SCMI server clock list 22 */ 23 struct scmi_clk { 24 struct clk *clk; 25 unsigned int channel_id; 26 unsigned int scmi_id; 27 bool enabled; 28 SLIST_ENTRY(scmi_clk) link; 29 }; 30 31 static bool scmi_clk_init_done; 32 static SLIST_HEAD(, scmi_clk) scmi_clk_list = 33 SLIST_HEAD_INITIALIZER(scmi_clk_list); 34 35 size_t plat_scmi_clock_count(unsigned int channel_id) 36 { 37 unsigned int count = 0; 38 unsigned int max_id = 0; 39 struct scmi_clk *clk = NULL; 40 41 SLIST_FOREACH(clk, &scmi_clk_list, link) { 42 if (clk->channel_id == channel_id) { 43 count++; 44 max_id = MAX(max_id, clk->scmi_id); 45 } 46 } 47 48 if (!count) 49 return 0; 50 51 /* IDs are starting from 0 so we need to return max_id + 1 for count */ 52 return max_id + 1; 53 } 54 55 static struct scmi_clk *clk_scmi_get_by_id(unsigned int channel_id, 56 unsigned int scmi_id) 57 { 58 struct scmi_clk *clk = NULL; 59 60 SLIST_FOREACH(clk, &scmi_clk_list, link) 61 if (clk->channel_id == channel_id && clk->scmi_id == scmi_id) 62 return clk; 63 64 return NULL; 65 } 66 67 const char *plat_scmi_clock_get_name(unsigned int channel_id, 68 unsigned int scmi_id) 69 { 70 struct scmi_clk *clk = NULL; 71 72 clk = clk_scmi_get_by_id(channel_id, scmi_id); 73 if (!clk) 74 return "dummy"; 75 76 return clk_get_name(clk->clk); 77 } 78 79 int32_t plat_scmi_clock_rates_array(unsigned int channel_id, 80 unsigned int scmi_id, 81 size_t start_index, 82 unsigned long *rates, 83 size_t *nb_elts) 84 { 85 struct scmi_clk *clk = NULL; 86 87 clk = clk_scmi_get_by_id(channel_id, scmi_id); 88 if (!clk) 89 return SCMI_DENIED; 90 91 if (clk_get_rates_array(clk->clk, start_index, rates, nb_elts)) 92 return SCMI_GENERIC_ERROR; 93 94 return SCMI_SUCCESS; 95 } 96 97 unsigned long plat_scmi_clock_get_rate(unsigned int channel_id, 98 unsigned int scmi_id) 99 { 100 struct scmi_clk *clk = NULL; 101 102 clk = clk_scmi_get_by_id(channel_id, scmi_id); 103 if (!clk) 104 return 0; 105 106 return clk_get_rate(clk->clk); 107 } 108 109 int32_t plat_scmi_clock_set_rate(unsigned int channel_id, 110 unsigned int scmi_id, 111 unsigned long rate) 112 { 113 TEE_Result res = TEE_ERROR_GENERIC; 114 struct scmi_clk *clk = NULL; 115 116 clk = clk_scmi_get_by_id(channel_id, scmi_id); 117 if (!clk) 118 return SCMI_DENIED; 119 120 res = clk_set_rate(clk->clk, rate); 121 if (res) 122 return SCMI_GENERIC_ERROR; 123 124 return SCMI_SUCCESS; 125 } 126 127 int32_t plat_scmi_clock_get_state(unsigned int channel_id, 128 unsigned int scmi_id) 129 { 130 struct scmi_clk *clk = NULL; 131 132 clk = clk_scmi_get_by_id(channel_id, scmi_id); 133 if (!clk) 134 return false; 135 136 return clk->enabled; 137 } 138 139 int32_t plat_scmi_clock_set_state(unsigned int channel_id, 140 unsigned int scmi_id, 141 bool enable_not_disable) 142 { 143 struct scmi_clk *clk = NULL; 144 145 clk = clk_scmi_get_by_id(channel_id, scmi_id); 146 if (!clk) { 147 if (enable_not_disable) 148 return SCMI_DENIED; 149 else 150 return SCMI_SUCCESS; 151 } 152 153 if (enable_not_disable) { 154 if (!clk->enabled) { 155 if (clk_enable(clk->clk)) 156 return SCMI_GENERIC_ERROR; 157 clk->enabled = true; 158 } 159 } else { 160 if (clk->enabled) { 161 clk_disable(clk->clk); 162 clk->enabled = false; 163 } 164 } 165 166 return SCMI_SUCCESS; 167 } 168 169 static TEE_Result clk_check_scmi_id(struct clk *new_clk, 170 unsigned int channel_id, 171 unsigned int scmi_id) 172 { 173 struct scmi_clk *clk = NULL; 174 175 SLIST_FOREACH(clk, &scmi_clk_list, link) { 176 if (clk->channel_id == channel_id && clk->scmi_id == scmi_id) { 177 EMSG("SCMI channel %u, clock %u already registered", 178 channel_id, scmi_id); 179 return TEE_ERROR_BAD_PARAMETERS; 180 } 181 } 182 183 if (strlen(clk_get_name(new_clk)) >= SCMI_CLOCK_NAME_LENGTH_MAX) 184 return TEE_ERROR_BAD_PARAMETERS; 185 186 return TEE_SUCCESS; 187 } 188 189 TEE_Result scmi_clk_add(struct clk *clk, unsigned int channel_id, 190 unsigned int scmi_id) 191 { 192 TEE_Result res = TEE_ERROR_GENERIC; 193 struct scmi_clk *scmi_clk = NULL; 194 195 if (scmi_clk_init_done) 196 return TEE_ERROR_BAD_STATE; 197 198 res = clk_check_scmi_id(clk, channel_id, scmi_id); 199 if (res) 200 return res; 201 202 scmi_clk = calloc(1, sizeof(*scmi_clk)); 203 if (!scmi_clk) 204 return TEE_ERROR_OUT_OF_MEMORY; 205 206 scmi_clk->clk = clk; 207 scmi_clk->channel_id = channel_id; 208 scmi_clk->scmi_id = scmi_id; 209 scmi_clk->enabled = false; 210 211 SLIST_INSERT_HEAD(&scmi_clk_list, scmi_clk, link); 212 213 return TEE_SUCCESS; 214 } 215 216 static TEE_Result scmi_clk_init_fini(void) 217 { 218 scmi_clk_init_done = true; 219 220 return TEE_SUCCESS; 221 } 222 223 release_init_resource(scmi_clk_init_fini); 224