1 /* SPDX-License-Identifier: GPL-2.0 */ 2 /* 3 * Copyright (c) 2023, Linaro Ltd. 4 */ 5 6 #include <linux/clk.h> 7 #include <linux/device.h> 8 #include <linux/interconnect-clk.h> 9 #include <linux/interconnect-provider.h> 10 11 struct icc_clk_node { 12 struct clk *clk; 13 bool enabled; 14 }; 15 16 struct icc_clk_provider { 17 struct icc_provider provider; 18 int num_clocks; 19 struct icc_clk_node clocks[] __counted_by(num_clocks); 20 }; 21 22 #define to_icc_clk_provider(_provider) \ 23 container_of(_provider, struct icc_clk_provider, provider) 24 25 static int icc_clk_set(struct icc_node *src, struct icc_node *dst) 26 { 27 struct icc_clk_node *qn = src->data; 28 int ret; 29 30 if (!qn || !qn->clk) 31 return 0; 32 33 if (!src->peak_bw) { 34 if (qn->enabled) 35 clk_disable_unprepare(qn->clk); 36 qn->enabled = false; 37 38 return 0; 39 } 40 41 if (!qn->enabled) { 42 ret = clk_prepare_enable(qn->clk); 43 if (ret) 44 return ret; 45 qn->enabled = true; 46 } 47 48 return clk_set_rate(qn->clk, icc_units_to_bps(src->peak_bw)); 49 } 50 51 static int icc_clk_get_bw(struct icc_node *node, u32 *avg, u32 *peak) 52 { 53 struct icc_clk_node *qn = node->data; 54 55 if (!qn || !qn->clk) 56 *peak = INT_MAX; 57 else 58 *peak = Bps_to_icc(clk_get_rate(qn->clk)); 59 60 return 0; 61 } 62 63 /** 64 * icc_clk_register() - register a new clk-based interconnect provider 65 * @dev: device supporting this provider 66 * @first_id: an ID of the first provider's node 67 * @num_clocks: number of instances of struct icc_clk_data 68 * @data: data for the provider 69 * 70 * Registers and returns a clk-based interconnect provider. It is a simple 71 * wrapper around COMMON_CLK framework, allowing other devices to vote on the 72 * clock rate. 73 * 74 * Return: 0 on success, or an error code otherwise 75 */ 76 struct icc_provider *icc_clk_register(struct device *dev, 77 unsigned int first_id, 78 unsigned int num_clocks, 79 const struct icc_clk_data *data) 80 { 81 struct icc_clk_provider *qp; 82 struct icc_provider *provider; 83 struct icc_onecell_data *onecell; 84 struct icc_node *node; 85 int ret, i, j; 86 87 onecell = devm_kzalloc(dev, struct_size(onecell, nodes, 2 * num_clocks), GFP_KERNEL); 88 if (!onecell) 89 return ERR_PTR(-ENOMEM); 90 onecell->num_nodes = 2 * num_clocks; 91 92 qp = devm_kzalloc(dev, struct_size(qp, clocks, num_clocks), GFP_KERNEL); 93 if (!qp) 94 return ERR_PTR(-ENOMEM); 95 96 qp->num_clocks = num_clocks; 97 98 provider = &qp->provider; 99 provider->dev = dev; 100 provider->get_bw = icc_clk_get_bw; 101 provider->set = icc_clk_set; 102 provider->aggregate = icc_std_aggregate; 103 provider->xlate = of_icc_xlate_onecell; 104 INIT_LIST_HEAD(&provider->nodes); 105 provider->data = onecell; 106 107 icc_provider_init(provider); 108 109 for (i = 0, j = 0; i < num_clocks; i++) { 110 qp->clocks[i].clk = data[i].clk; 111 112 node = icc_node_create(first_id + data[i].master_id); 113 if (IS_ERR(node)) { 114 ret = PTR_ERR(node); 115 goto err; 116 } 117 118 node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_master", data[i].name); 119 if (!node->name) { 120 ret = -ENOMEM; 121 goto err; 122 } 123 124 node->data = &qp->clocks[i]; 125 icc_node_add(node, provider); 126 /* link to the next node, slave */ 127 icc_link_create(node, first_id + data[i].slave_id); 128 onecell->nodes[j++] = node; 129 130 node = icc_node_create(first_id + data[i].slave_id); 131 if (IS_ERR(node)) { 132 ret = PTR_ERR(node); 133 goto err; 134 } 135 136 node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_slave", data[i].name); 137 if (!node->name) { 138 ret = -ENOMEM; 139 goto err; 140 } 141 142 /* no data for slave node */ 143 icc_node_add(node, provider); 144 onecell->nodes[j++] = node; 145 } 146 147 ret = icc_provider_register(provider); 148 if (ret) 149 goto err; 150 151 return provider; 152 153 err: 154 icc_nodes_remove(provider); 155 156 return ERR_PTR(ret); 157 } 158 EXPORT_SYMBOL_GPL(icc_clk_register); 159 160 static void devm_icc_release(void *res) 161 { 162 icc_clk_unregister(res); 163 } 164 165 int devm_icc_clk_register(struct device *dev, unsigned int first_id, 166 unsigned int num_clocks, const struct icc_clk_data *data) 167 { 168 struct icc_provider *prov; 169 170 prov = icc_clk_register(dev, first_id, num_clocks, data); 171 if (IS_ERR(prov)) 172 return PTR_ERR(prov); 173 174 return devm_add_action_or_reset(dev, devm_icc_release, prov); 175 } 176 EXPORT_SYMBOL_GPL(devm_icc_clk_register); 177 178 /** 179 * icc_clk_unregister() - unregister a previously registered clk interconnect provider 180 * @provider: provider returned by icc_clk_register() 181 */ 182 void icc_clk_unregister(struct icc_provider *provider) 183 { 184 struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider); 185 int i; 186 187 icc_provider_deregister(&qp->provider); 188 icc_nodes_remove(&qp->provider); 189 190 for (i = 0; i < qp->num_clocks; i++) { 191 struct icc_clk_node *qn = &qp->clocks[i]; 192 193 if (qn->enabled) 194 clk_disable_unprepare(qn->clk); 195 } 196 } 197 EXPORT_SYMBOL_GPL(icc_clk_unregister); 198 199 MODULE_LICENSE("GPL"); 200 MODULE_DESCRIPTION("Interconnect wrapper for clocks"); 201 MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>"); 202