drivers/interconnect/icc-clk.c

   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>");