linux/drivers/interconnect/qcom/icc-rpm.c
<<
>>
Prefs
   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
  19static 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
  89int 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;
 174err:
 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}
 181EXPORT_SYMBOL(qnoc_probe);
 182
 183int 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}
 191EXPORT_SYMBOL(qnoc_remove);
 192