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                 node->data = &qp->clocks[i];
 120                 icc_node_add(node, provider);
 121                 /* link to the next node, slave */
 122                 icc_link_create(node, first_id + data[i].slave_id);
 123                 onecell->nodes[j++] = node;
 124
 125                 node = icc_node_create(first_id + data[i].slave_id);
 126                 if (IS_ERR(node)) {
 127                         ret = PTR_ERR(node);
 128                         goto err;
 129                 }
 130
 131                 node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_slave", data[i].name);
 132                 /* no data for slave node */
 133                 icc_node_add(node, provider);
 134                 onecell->nodes[j++] = node;
 135         }
 136
 137         ret = icc_provider_register(provider);
 138         if (ret)
 139                 goto err;
 140
 141         return provider;
 142
 143 err:
 144         icc_nodes_remove(provider);
 145
 146         return ERR_PTR(ret);
 147 }
 148 EXPORT_SYMBOL_GPL(icc_clk_register);
 149
 150 static void devm_icc_release(void *res)
 151 {
 152         icc_clk_unregister(res);
 153 }
 154
 155 int devm_icc_clk_register(struct device *dev, unsigned int first_id,
 156                           unsigned int num_clocks, const struct icc_clk_data *data)
 157 {
 158         struct icc_provider *prov;
 159
 160         prov = icc_clk_register(dev, first_id, num_clocks, data);
 161         if (IS_ERR(prov))
 162                 return PTR_ERR(prov);
 163
 164         return devm_add_action_or_reset(dev, devm_icc_release, prov);
 165 }
 166 EXPORT_SYMBOL_GPL(devm_icc_clk_register);
 167
 168 /**
 169  * icc_clk_unregister() - unregister a previously registered clk interconnect provider
 170  * @provider: provider returned by icc_clk_register()
 171  */
 172 void icc_clk_unregister(struct icc_provider *provider)
 173 {
 174         struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider);
 175         int i;
 176
 177         icc_provider_deregister(&qp->provider);
 178         icc_nodes_remove(&qp->provider);
 179
 180         for (i = 0; i < qp->num_clocks; i++) {
 181                 struct icc_clk_node *qn = &qp->clocks[i];
 182
 183                 if (qn->enabled)
 184                         clk_disable_unprepare(qn->clk);
 185         }
 186 }
 187 EXPORT_SYMBOL_GPL(icc_clk_unregister);
 188
 189 MODULE_LICENSE("GPL");
 190 MODULE_DESCRIPTION("Interconnect wrapper for clocks");
 191 MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>");