1 files changed, 443 insertions, 0 deletions

diff --git a/drivers/devfreq/devfreq_spdm.c b/drivers/devfreq/devfreq_spdm.c
new file mode 100644
index 000000000000..8e35570e0443
--- /dev/null
+++ b/drivers/devfreq/devfreq_spdm.c
@@ -0,0 +1,443 @@
+/*
+*Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+*
+*This program is free software; you can redistribute it and/or modify
+*it under the terms of the GNU General Public License version 2 and
+*only version 2 as published by the Free Software Foundation.
+*
+*This program is distributed in the hope that it will be useful,
+*but WITHOUT ANY WARRANTY; without even the implied warranty of
+*MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+*GNU General Public License for more details.
+*/
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/devfreq.h>
+#include <linux/init.h>
+#include <linux/ipc_logging.h>
+#include <linux/gfp.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/msm-bus.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include "governor.h"
+#include "devfreq_spdm.h"
+
+static void *spdm_ipc_log_ctxt;
+#define DEVFREQ_SPDM_DEFAULT_WINDOW_MS 100
+#define SPDM_IPC_LOG_PAGES	5
+
+#define SPDM_IPC_LOG(x...)	do { \
+	pr_debug(x); \
+	if (spdm_ipc_log_ctxt) \
+		ipc_log_string(spdm_ipc_log_ctxt, x); \
+} while (0)
+
+#define COPY_SIZE(x, y) ((x) <= (y) ? (x) : (y))
+
+static int change_bw(struct device *dev, unsigned long *freq, u32 flags)
+{
+	struct spdm_data *data = 0;
+	int i;
+	int next_idx;
+	int ret = 0;
+	struct spdm_args desc = { { 0 } };
+	int ext_status = 0;
+
+	if (!dev || !freq)
+		return -EINVAL;
+
+	data = dev_get_drvdata(dev);
+	if (!data)
+		return -EINVAL;
+
+	if (data->devfreq->previous_freq == *freq)
+		goto update_thresholds;
+
+	next_idx = data->cur_idx + 1;
+	next_idx = next_idx % 2;
+
+	for (i = 0; i < data->pdata->usecase[next_idx].num_paths; i++)
+		data->pdata->usecase[next_idx].vectors[i].ab = (*freq) << 6;
+
+	data->cur_idx = next_idx;
+	ret = msm_bus_scale_client_update_request(data->bus_scale_client_id,
+						  data->cur_idx);
+
+update_thresholds:
+	desc.arg[0] = SPDM_CMD_ENABLE;
+	desc.arg[1] = data->spdm_client;
+
+	if (data->cci_clk)
+		desc.arg[2] = (clk_get_rate(data->cci_clk)) / 1000;
+	else
+		desc.arg[2] = 0;
+
+	ext_status = spdm_ext_call(&desc, 3);
+	if (ext_status)
+		pr_err("External command %u failed with error %u",
+			(int)desc.arg[0], ext_status);
+	return ret;
+}
+
+static int get_cur_bw(struct device *dev, unsigned long *freq)
+{
+	struct spdm_data *data = 0;
+
+	if (!dev || !freq)
+		return -EINVAL;
+
+	data = dev_get_drvdata(dev);
+	if (!data)
+		return -EINVAL;
+
+	*freq = data->pdata->usecase[data->cur_idx].vectors[0].ab >> 6;
+
+	return 0;
+}
+
+static int get_dev_status(struct device *dev, struct devfreq_dev_status *status)
+{
+	struct spdm_data *data = 0;
+	int ret;
+
+	if (!dev || !status)
+		return -EINVAL;
+
+	data = dev_get_drvdata(dev);
+	if (!data)
+		return -EINVAL;
+
+	/* determine if we want to go up or down based on the notification */
+	if (data->action == SPDM_UP)
+		status->busy_time = 255;
+	else
+		status->busy_time = 0;
+	status->total_time = 255;
+	ret = get_cur_bw(dev, &status->current_frequency);
+	if (ret)
+		return ret;
+
+	return 0;
+
+}
+
+static int populate_config_data(struct spdm_data *data,
+				struct platform_device *pdev)
+{
+	int ret = -EINVAL;
+	struct device_node *node = pdev->dev.of_node;
+	struct property *prop = 0;
+
+	ret = of_property_read_u32(node, "qcom,max-vote",
+				   &data->config_data.max_vote);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(node, "qcom,bw-upstep",
+				   &data->config_data.upstep);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(node, "qcom,bw-dwnstep",
+				   &data->config_data.downstep);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(node, "qcom,alpha-up",
+				   &data->config_data.aup);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(node, "qcom,alpha-down",
+				   &data->config_data.adown);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(node, "qcom,bucket-size",
+				   &data->config_data.bucket_size);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32_array(node, "qcom,pl-freqs",
+					 data->config_data.pl_freqs,
+					 SPDM_PL_COUNT - 1);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32_array(node, "qcom,reject-rate",
+					 data->config_data.reject_rate,
+					 SPDM_PL_COUNT * 2);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32_array(node, "qcom,response-time-us",
+					 data->config_data.response_time_us,
+					 SPDM_PL_COUNT * 2);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32_array(node, "qcom,cci-response-time-us",
+					 data->config_data.cci_response_time_us,
+					 SPDM_PL_COUNT * 2);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(node, "qcom,max-cci-freq",
+				   &data->config_data.max_cci_freq);
+	if (ret)
+		return ret;
+	ret = of_property_read_u32(node, "qcom,up-step-multp",
+				   &data->config_data.up_step_multp);
+	if (ret)
+		return ret;
+
+	prop = of_find_property(node, "qcom,ports", 0);
+	if (!prop)
+		return -EINVAL;
+	data->config_data.num_ports = prop->length / sizeof(u32);
+	data->config_data.ports =
+	    devm_kzalloc(&pdev->dev, prop->length, GFP_KERNEL);
+	if (!data->config_data.ports)
+		return -ENOMEM;
+	ret = of_property_read_u32_array(node, "qcom,ports",
+					 data->config_data.ports,
+					 data->config_data.num_ports);
+	if (ret) {
+		devm_kfree(&pdev->dev, data->config_data.ports);
+		data->config_data.ports = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+
+static int populate_spdm_data(struct spdm_data *data,
+			      struct platform_device *pdev)
+{
+	int ret = -EINVAL;
+	struct device_node *node = pdev->dev.of_node;
+
+	ret = populate_config_data(data, pdev);
+	if (ret)
+		return ret;
+
+	ret =
+	    of_property_read_u32(node, "qcom,spdm-client", &data->spdm_client);
+	if (ret)
+		goto no_client;
+
+	ret = of_property_read_u32(node, "qcom,spdm-interval", &data->window);
+	if (ret)
+		data->window = DEVFREQ_SPDM_DEFAULT_WINDOW_MS;
+
+	data->pdata = msm_bus_cl_get_pdata(pdev);
+	if (!data->pdata) {
+		ret = -EINVAL;
+		goto no_pdata;
+	}
+
+	return 0;
+
+no_client:
+no_pdata:
+	devm_kfree(&pdev->dev, data->config_data.ports);
+	data->config_data.ports = NULL;
+	return ret;
+}
+
+int __spdm_hyp_call(struct spdm_args *args, int num_args)
+{
+	struct hvc_desc desc = { { 0 } };
+	int status;
+
+	memcpy(desc.arg, args->arg,
+		COPY_SIZE(sizeof(desc.arg), sizeof(args->arg)));
+	SPDM_IPC_LOG("hvc call fn:0x%x, cmd:%llu, num_args:%d\n",
+		HVC_FN_SIP(SPDM_HYP_FNID), desc.arg[0], num_args);
+
+	status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+
+	memcpy(args->ret, desc.ret,
+		COPY_SIZE(sizeof(args->ret), sizeof(desc.ret)));
+	SPDM_IPC_LOG("hvc return fn:0x%x cmd:%llu Ret[0]:%llu Ret[1]:%llu\n",
+			HVC_FN_SIP(SPDM_HYP_FNID), desc.arg[0],
+			desc.ret[0], desc.ret[1]);
+	return status;
+}
+
+int __spdm_scm_call(struct spdm_args *args, int num_args)
+{
+	int status = 0;
+
+	SPDM_IPC_LOG("%s:svc_id:%d,cmd_id:%d,cmd:%llu,num_args:%d\n",
+		__func__, SPDM_SCM_SVC_ID, SPDM_SCM_CMD_ID,
+		args->arg[0], num_args);
+
+	if (!is_scm_armv8()) {
+		status = scm_call(SPDM_SCM_SVC_ID, SPDM_SCM_CMD_ID, args->arg,
+				sizeof(args->arg), args->ret,
+				sizeof(args->ret));
+	} else {
+		struct scm_desc desc = {0};
+		/*
+		 * Need to hard code this, this is a requirement from TZ syscall
+		 * interface.
+		 */
+		desc.arginfo = SCM_ARGS(6);
+		memcpy(desc.args, args->arg,
+			COPY_SIZE(sizeof(desc.args), sizeof(args->arg)));
+
+		status = scm_call2(SCM_SIP_FNID(SPDM_SCM_SVC_ID,
+				SPDM_SCM_CMD_ID), &desc);
+
+		memcpy(args->ret, desc.ret,
+			COPY_SIZE(sizeof(args->ret), sizeof(desc.ret)));
+	}
+	SPDM_IPC_LOG("%s:svc_id:%d,cmd_id:%d,cmd:%llu,Ret[0]:%llu,Ret[1]:%llu\n"
+		, __func__, SPDM_SCM_SVC_ID, SPDM_SCM_CMD_ID, args->arg[0],
+		args->ret[0], args->ret[1]);
+	return status;
+}
+
+static int probe(struct platform_device *pdev)
+{
+	struct spdm_data *data = 0;
+	int ret = -EINVAL;
+	struct spdm_args desc = { { 0 } };
+	int ext_status = 0;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->action = SPDM_DOWN;
+
+	platform_set_drvdata(pdev, data);
+
+	ret = populate_spdm_data(data, pdev);
+	if (ret)
+		goto bad_of;
+
+	desc.arg[0] = SPDM_CMD_GET_VERSION;
+	ext_status = spdm_ext_call(&desc, 1);
+	if (ext_status) {
+		pr_err("%s:External command %u failed with error %u\n",
+			__func__, (int)desc.arg[0], ext_status);
+		goto bad_of;
+	}
+
+	if (desc.ret[0] < SPDM_TZ_VERSION) {
+		pr_err("%s: Version mismatch expected 0x%x got 0x%x", __func__,
+			SPDM_TZ_VERSION, (int)desc.arg[0]);
+		goto bad_of;
+	}
+
+	data->bus_scale_client_id = msm_bus_scale_register_client(data->pdata);
+	if (!data->bus_scale_client_id) {
+		ret = -EINVAL;
+		goto no_bus_scaling;
+	}
+
+	data->cci_clk = clk_get(&pdev->dev, "cci_clk");
+	if (IS_ERR(data->cci_clk)) {
+		data->cci_clk = NULL;
+	}
+
+	data->profile =
+	    devm_kzalloc(&pdev->dev, sizeof(*(data->profile)), GFP_KERNEL);
+	if (!data->profile) {
+		ret = -ENOMEM;
+		goto no_profile;
+	}
+	data->profile->target = change_bw;
+	data->profile->get_dev_status = get_dev_status;
+	data->profile->get_cur_freq = get_cur_bw;
+	data->profile->polling_ms = data->window;
+
+	data->devfreq =
+	    devfreq_add_device(&pdev->dev, data->profile, "spdm_bw_hyp", data);
+	if (IS_ERR(data->devfreq)) {
+		ret = PTR_ERR(data->devfreq);
+		goto no_spdm_device;
+	}
+
+	spdm_init_debugfs(&pdev->dev);
+	spdm_ipc_log_ctxt = ipc_log_context_create(SPDM_IPC_LOG_PAGES,
+							"devfreq_spdm", 0);
+
+	if (IS_ERR_OR_NULL(spdm_ipc_log_ctxt)) {
+		pr_err("%s: Failed to create IPC log context\n", __func__);
+		spdm_ipc_log_ctxt = NULL;
+	}
+
+
+	return 0;
+
+no_spdm_device:
+	devm_kfree(&pdev->dev, data->profile);
+no_profile:
+	msm_bus_scale_unregister_client(data->bus_scale_client_id);
+no_bus_scaling:
+	devm_kfree(&pdev->dev, data->config_data.ports);
+bad_of:
+	devm_kfree(&pdev->dev, data);
+	platform_set_drvdata(pdev, NULL);
+	return ret;
+}
+
+static int remove(struct platform_device *pdev)
+{
+	struct spdm_data *data = 0;
+
+	data = platform_get_drvdata(pdev);
+
+	spdm_remove_debugfs(data);
+
+	if (data->devfreq)
+		devfreq_remove_device(data->devfreq);
+
+	if (data->profile)
+		devm_kfree(&pdev->dev, data->profile);
+
+	if (data->bus_scale_client_id)
+		msm_bus_scale_unregister_client(data->bus_scale_client_id);
+
+	if (data->config_data.ports)
+		devm_kfree(&pdev->dev, data->config_data.ports);
+
+	devm_kfree(&pdev->dev, data);
+	platform_set_drvdata(pdev, NULL);
+
+	if (spdm_ipc_log_ctxt)
+		ipc_log_context_destroy(spdm_ipc_log_ctxt);
+
+	return 0;
+}
+
+static const struct of_device_id devfreq_spdm_match[] = {
+	{.compatible = "qcom,devfreq_spdm"},
+	{}
+};
+
+static struct platform_driver devfreq_spdm_drvr = {
+	.driver = {
+		   .name = "devfreq_spdm",
+		   .owner = THIS_MODULE,
+		   .of_match_table = devfreq_spdm_match,
+		   },
+	.probe = probe,
+	.remove = remove,
+};
+
+static int __init devfreq_spdm_init(void)
+{
+	return platform_driver_register(&devfreq_spdm_drvr);
+}
+
+module_init(devfreq_spdm_init);
+
+MODULE_LICENSE("GPL v2");