drivers/interconnect/qcom/icc-rpm.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2020 Linaro Ltd
   4  */
   5
   6 #include <linux/clk.h>
   7 #include <linux/device.h>
   8 #include <linux/interconnect-provider.h>
   9 #include <linux/io.h>
  10 #include <linux/module.h>
  11 #include <linux/of_device.h>
  12 #include <linux/of_platform.h>
  13 #include <linux/platform_device.h>
  14 #include <linux/slab.h>
  15
  16 #include "smd-rpm.h"
  17 #include "icc-rpm.h"
  18
  19 static int qcom_icc_set(struct icc_node *src, struct icc_node *dst)
  20 {
  21         struct qcom_icc_provider *qp;
  22         struct qcom_icc_node *qn;
  23         struct icc_provider *provider;
  24         struct icc_node *n;
  25         u64 sum_bw;
  26         u64 max_peak_bw;
  27         u64 rate;
  28         u32 agg_avg = 0;
  29         u32 agg_peak = 0;
  30         int ret, i;
  31
  32         qn = src->data;
  33         provider = src->provider;
  34         qp = to_qcom_provider(provider);
  35
  36         list_for_each_entry(n, &provider->nodes, node_list)
  37                 provider->aggregate(n, 0, n->avg_bw, n->peak_bw,
  38                                     &agg_avg, &agg_peak);
  39
  40         sum_bw = icc_units_to_bps(agg_avg);
  41         max_peak_bw = icc_units_to_bps(agg_peak);
  42
  43         /* send bandwidth request message to the RPM processor */
  44         if (qn->mas_rpm_id != -1) {
  45                 ret = qcom_icc_rpm_smd_send(QCOM_SMD_RPM_ACTIVE_STATE,
  46                                             RPM_BUS_MASTER_REQ,
  47                                             qn->mas_rpm_id,
  48                                             sum_bw);
  49                 if (ret) {
  50                         pr_err("qcom_icc_rpm_smd_send mas %d error %d\n",
  51                                qn->mas_rpm_id, ret);
  52                         return ret;
  53                 }
  54         }
  55
  56         if (qn->slv_rpm_id != -1) {
  57                 ret = qcom_icc_rpm_smd_send(QCOM_SMD_RPM_ACTIVE_STATE,
  58                                             RPM_BUS_SLAVE_REQ,
  59                                             qn->slv_rpm_id,
  60                                             sum_bw);
  61                 if (ret) {
  62                         pr_err("qcom_icc_rpm_smd_send slv %d error %d\n",
  63                                qn->slv_rpm_id, ret);
  64                         return ret;
  65                 }
  66         }
  67
  68         rate = max(sum_bw, max_peak_bw);
  69
  70         do_div(rate, qn->buswidth);
  71
  72         if (qn->rate == rate)
  73                 return 0;
  74
  75         for (i = 0; i < qp->num_clks; i++) {
  76                 ret = clk_set_rate(qp->bus_clks[i].clk, rate);
  77                 if (ret) {
  78                         pr_err("%s clk_set_rate error: %d\n",
  79                                qp->bus_clks[i].id, ret);
  80                         return ret;
  81                 }
  82         }
  83
  84         qn->rate = rate;
  85
  86         return 0;
  87 }
  88
  89 int qnoc_probe(struct platform_device *pdev, size_t cd_size, int cd_num,
  90                const struct clk_bulk_data *cd)
  91 {
  92         struct device *dev = &pdev->dev;
  93         const struct qcom_icc_desc *desc;
  94         struct icc_onecell_data *data;
  95         struct icc_provider *provider;
  96         struct qcom_icc_node **qnodes;
  97         struct qcom_icc_provider *qp;
  98         struct icc_node *node;
  99         size_t num_nodes, i;
 100         int ret;
 101
 102         /* wait for the RPM proxy */
 103         if (!qcom_icc_rpm_smd_available())
 104                 return -EPROBE_DEFER;
 105
 106         desc = of_device_get_match_data(dev);
 107         if (!desc)
 108                 return -EINVAL;
 109
 110         qnodes = desc->nodes;
 111         num_nodes = desc->num_nodes;
 112
 113         qp = devm_kzalloc(dev, sizeof(*qp), GFP_KERNEL);
 114         if (!qp)
 115                 return -ENOMEM;
 116
 117         data = devm_kzalloc(dev, struct_size(data, nodes, num_nodes),
 118                             GFP_KERNEL);
 119         if (!data)
 120                 return -ENOMEM;
 121
 122         qp->bus_clks = devm_kmemdup(dev, cd, cd_size,
 123                                     GFP_KERNEL);
 124         if (!qp->bus_clks)
 125                 return -ENOMEM;
 126
 127         qp->num_clks = cd_num;
 128         ret = devm_clk_bulk_get(dev, qp->num_clks, qp->bus_clks);
 129         if (ret)
 130                 return ret;
 131
 132         ret = clk_bulk_prepare_enable(qp->num_clks, qp->bus_clks);
 133         if (ret)
 134                 return ret;
 135
 136         provider = &qp->provider;
 137         INIT_LIST_HEAD(&provider->nodes);
 138         provider->dev = dev;
 139         provider->set = qcom_icc_set;
 140         provider->aggregate = icc_std_aggregate;
 141         provider->xlate = of_icc_xlate_onecell;
 142         provider->data = data;
 143
 144         ret = icc_provider_add(provider);
 145         if (ret) {
 146                 dev_err(dev, "error adding interconnect provider: %d\n", ret);
 147                 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
 148                 return ret;
 149         }
 150
 151         for (i = 0; i < num_nodes; i++) {
 152                 size_t j;
 153
 154                 node = icc_node_create(qnodes[i]->id);
 155                 if (IS_ERR(node)) {
 156                         ret = PTR_ERR(node);
 157                         goto err;
 158                 }
 159
 160                 node->name = qnodes[i]->name;
 161                 node->data = qnodes[i];
 162                 icc_node_add(node, provider);
 163
 164                 for (j = 0; j < qnodes[i]->num_links; j++)
 165                         icc_link_create(node, qnodes[i]->links[j]);
 166
 167                 data->nodes[i] = node;
 168         }
 169         data->num_nodes = num_nodes;
 170
 171         platform_set_drvdata(pdev, qp);
 172
 173         return 0;
 174 err:
 175         icc_nodes_remove(provider);
 176         clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
 177         icc_provider_del(provider);
 178
 179         return ret;
 180 }
 181 EXPORT_SYMBOL(qnoc_probe);
 182
 183 int qnoc_remove(struct platform_device *pdev)
 184 {
 185         struct qcom_icc_provider *qp = platform_get_drvdata(pdev);
 186
 187         icc_nodes_remove(&qp->provider);
 188         clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
 189         return icc_provider_del(&qp->provider);
 190 }
 191 EXPORT_SYMBOL(qnoc_remove);