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
  91         qp = devm_kzalloc(dev, struct_size(qp, clocks, num_clocks), GFP_KERNEL);
  92         if (!qp)
  93                 return ERR_PTR(-ENOMEM);
  94
  95         qp->num_clocks = num_clocks;
  96
  97         provider = &qp->provider;
  98         provider->dev = dev;
  99         provider->get_bw = icc_clk_get_bw;
 100         provider->set = icc_clk_set;
 101         provider->aggregate = icc_std_aggregate;
 102         provider->xlate = of_icc_xlate_onecell;
 103         INIT_LIST_HEAD(&provider->nodes);
 104         provider->data = onecell;
 105
 106         icc_provider_init(provider);
 107
 108         for (i = 0, j = 0; i < num_clocks; i++) {
 109                 qp->clocks[i].clk = data[i].clk;
 110
 111                 node = icc_node_create(first_id + data[i].master_id);
 112                 if (IS_ERR(node)) {
 113                         ret = PTR_ERR(node);
 114                         goto err;
 115                 }
 116
 117                 node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_master", data[i].name);
 118                 node->data = &qp->clocks[i];
 119                 icc_node_add(node, provider);
 120                 /* link to the next node, slave */
 121                 icc_link_create(node, first_id + data[i].slave_id);
 122                 onecell->nodes[j++] = node;
 123
 124                 node = icc_node_create(first_id + data[i].slave_id);
 125                 if (IS_ERR(node)) {
 126                         ret = PTR_ERR(node);
 127                         goto err;
 128                 }
 129
 130                 node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_slave", data[i].name);
 131                 /* no data for slave node */
 132                 icc_node_add(node, provider);
 133                 onecell->nodes[j++] = node;
 134         }
 135
 136         onecell->num_nodes = j;
 137
 138         ret = icc_provider_register(provider);
 139         if (ret)
 140                 goto err;
 141
 142         return provider;
 143
 144 err:
 145         icc_nodes_remove(provider);
 146
 147         return ERR_PTR(ret);
 148 }
 149 EXPORT_SYMBOL_GPL(icc_clk_register);
 150
 151 static void devm_icc_release(void *res)
 152 {
 153         icc_clk_unregister(res);
 154 }
 155
 156 int devm_icc_clk_register(struct device *dev, unsigned int first_id,
 157                           unsigned int num_clocks, const struct icc_clk_data *data)
 158 {
 159         struct icc_provider *prov;
 160
 161         prov = icc_clk_register(dev, first_id, num_clocks, data);
 162         if (IS_ERR(prov))
 163                 return PTR_ERR(prov);
 164
 165         return devm_add_action_or_reset(dev, devm_icc_release, prov);
 166 }
 167 EXPORT_SYMBOL_GPL(devm_icc_clk_register);
 168
 169 /**
 170  * icc_clk_unregister() - unregister a previously registered clk interconnect provider
 171  * @provider: provider returned by icc_clk_register()
 172  */
 173 void icc_clk_unregister(struct icc_provider *provider)
 174 {
 175         struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider);
 176         int i;
 177
 178         icc_provider_deregister(&qp->provider);
 179         icc_nodes_remove(&qp->provider);
 180
 181         for (i = 0; i < qp->num_clocks; i++) {
 182                 struct icc_clk_node *qn = &qp->clocks[i];
 183
 184                 if (qn->enabled)
 185                         clk_disable_unprepare(qn->clk);
 186         }
 187 }
 188 EXPORT_SYMBOL_GPL(icc_clk_unregister);
 189
 190 MODULE_LICENSE("GPL");
 191 MODULE_DESCRIPTION("Interconnect wrapper for clocks");
 192 MODULE_AUTHOR("Dmitry Baryshkov <[email protected]>");