|
| 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[]; |
| 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 + j); |
| 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 + j + 1); |
| 122 | + onecell->nodes[j++] = node; |
| 123 | + |
| 124 | + node = icc_node_create(first_id + j); |
| 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 | +/** |
| 152 | + * icc_clk_unregister() - unregister a previously registered clk interconnect provider |
| 153 | + * @provider: provider returned by icc_clk_register() |
| 154 | + */ |
| 155 | +void icc_clk_unregister(struct icc_provider *provider) |
| 156 | +{ |
| 157 | + struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider); |
| 158 | + int i; |
| 159 | + |
| 160 | + icc_provider_deregister(&qp->provider); |
| 161 | + icc_nodes_remove(&qp->provider); |
| 162 | + |
| 163 | + for (i = 0; i < qp->num_clocks; i++) { |
| 164 | + struct icc_clk_node *qn = &qp->clocks[i]; |
| 165 | + |
| 166 | + if (qn->enabled) |
| 167 | + clk_disable_unprepare(qn->clk); |
| 168 | + } |
| 169 | +} |
| 170 | +EXPORT_SYMBOL_GPL(icc_clk_unregister); |
| 171 | + |
| 172 | +MODULE_LICENSE("GPL"); |
| 173 | +MODULE_DESCRIPTION("Interconnect wrapper for clocks"); |
| 174 | +MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>"); |
0 commit comments