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 TEE_Result res = TEE_ERROR_GENERIC; 86 struct scmi_clk *clk = NULL; 87 88 clk = clk_scmi_get_by_id(channel_id, scmi_id); 89 if (!clk) 90 return SCMI_DENIED; 91 92 res = clk_get_rates_array(clk->clk, start_index, rates, nb_elts); 93 if (res == TEE_SUCCESS) 94 return SCMI_SUCCESS; 95 else if (res == TEE_ERROR_NOT_SUPPORTED) 96 return SCMI_NOT_SUPPORTED; 97 else 98 return SCMI_GENERIC_ERROR; 99 } 100 101 unsigned long plat_scmi_clock_get_rate(unsigned int channel_id, 102 unsigned int scmi_id) 103 { 104 struct scmi_clk *clk = NULL; 105 106 clk = clk_scmi_get_by_id(channel_id, scmi_id); 107 if (!clk) 108 return 0; 109 110 return clk_get_rate(clk->clk); 111 } 112 113 int32_t plat_scmi_clock_set_rate(unsigned int channel_id, 114 unsigned int scmi_id, 115 unsigned long rate) 116 { 117 TEE_Result res = TEE_ERROR_GENERIC; 118 struct scmi_clk *clk = NULL; 119 120 clk = clk_scmi_get_by_id(channel_id, scmi_id); 121 if (!clk) 122 return SCMI_DENIED; 123 124 res = clk_set_rate(clk->clk, rate); 125 if (res) 126 return SCMI_GENERIC_ERROR; 127 128 return SCMI_SUCCESS; 129 } 130 131 int32_t plat_scmi_clock_get_state(unsigned int channel_id, 132 unsigned int scmi_id) 133 { 134 struct scmi_clk *clk = NULL; 135 136 clk = clk_scmi_get_by_id(channel_id, scmi_id); 137 if (!clk) 138 return false; 139 140 return clk->enabled; 141 } 142 143 int32_t plat_scmi_clock_set_state(unsigned int channel_id, 144 unsigned int scmi_id, 145 bool enable_not_disable) 146 { 147 struct scmi_clk *clk = NULL; 148 149 clk = clk_scmi_get_by_id(channel_id, scmi_id); 150 if (!clk) { 151 if (enable_not_disable) 152 return SCMI_DENIED; 153 else 154 return SCMI_SUCCESS; 155 } 156 157 if (enable_not_disable) { 158 if (!clk->enabled) { 159 if (clk_enable(clk->clk)) 160 return SCMI_GENERIC_ERROR; 161 clk->enabled = true; 162 } 163 } else { 164 if (clk->enabled) { 165 clk_disable(clk->clk); 166 clk->enabled = false; 167 } 168 } 169 170 return SCMI_SUCCESS; 171 } 172 173 static TEE_Result clk_check_scmi_id(struct clk *new_clk, 174 unsigned int channel_id, 175 unsigned int scmi_id) 176 { 177 struct scmi_clk *clk = NULL; 178 179 SLIST_FOREACH(clk, &scmi_clk_list, link) { 180 if (clk->channel_id == channel_id && clk->scmi_id == scmi_id) { 181 EMSG("SCMI channel %u, clock %u already registered", 182 channel_id, scmi_id); 183 return TEE_ERROR_BAD_PARAMETERS; 184 } 185 } 186 187 if (strlen(clk_get_name(new_clk)) >= SCMI_CLOCK_NAME_LENGTH_MAX) 188 return TEE_ERROR_BAD_PARAMETERS; 189 190 return TEE_SUCCESS; 191 } 192 193 TEE_Result scmi_clk_add(struct clk *clk, unsigned int channel_id, 194 unsigned int scmi_id) 195 { 196 TEE_Result res = TEE_ERROR_GENERIC; 197 struct scmi_clk *scmi_clk = NULL; 198 199 if (scmi_clk_init_done) 200 return TEE_ERROR_BAD_STATE; 201 202 res = clk_check_scmi_id(clk, channel_id, scmi_id); 203 if (res) 204 return res; 205 206 scmi_clk = calloc(1, sizeof(*scmi_clk)); 207 if (!scmi_clk) 208 return TEE_ERROR_OUT_OF_MEMORY; 209 210 scmi_clk->clk = clk; 211 scmi_clk->channel_id = channel_id; 212 scmi_clk->scmi_id = scmi_id; 213 scmi_clk->enabled = false; 214 215 SLIST_INSERT_HEAD(&scmi_clk_list, scmi_clk, link); 216 217 return TEE_SUCCESS; 218 } 219 220 static TEE_Result scmi_clk_init_fini(void) 221 { 222 scmi_clk_init_done = true; 223 224 return TEE_SUCCESS; 225 } 226 227 release_init_resource(scmi_clk_init_fini); 228