devfreq: devfreq_spdm: Introduce devfreq_spdm driver.

The devfreq_spdm driver implements support for bandwidth voting
based on input from the SPDM device on MSM SoC's.  The SPDM
governor registers for the SPDM interrupt and then calls
the hypervisor to determine the correct bandwidth to vote for.
The devfreq framework poll timer is used to perdiocially
ask the hypervisor for the new bandwidth to request from
the MSM bus scaling code.

Change-Id: I851457e40d49b5929f01c510249d3e6bb4ff2f1d
Signed-off-by: Dan Sneddon <dsneddon@codeaurora.org>
[junjiew@codeaurora.org: resolved trivial conflicts]
Signed-off-by: Junjie Wu <junjiew@codeaurora.org>

Documentation/devicetree/bindings/devfreq/devfreq-spdm.txt

@@ -0,0 +1,84 @@
+MSM SPDM bandwidth monitor device
+
+devfreq-spdm is a device that represents a device that is monitored by the SPDM
+hardware to measure the traffic status of configured master ports on the bus.
+
+
+Required properties:
+-compatible:			Must be "qcom,devfreq_spdm"
+-clock-names:			Clocks used to measure current bus frequency.
+				Expected names are "cci_clk"
+-clocks:			References to named clocks
+-qcom,spdm-client:		Client id of the port being monitored
+-qcom,bw-upstep:		Initial up vote size in MB/s
+-qcom,bw-dwnstep:		Initial down vote size in MB/s
+-qcom,max-vote:			Vote ceiling in MB/s
+-qcom,ports:			SPDM ports used by this device
+-qcom,alpha-up:			SPDM filter up alpha value
+-qcom,alpha-down:		SPDM filter down alpha value
+-qcom,bucket-size:		SPDM filter bucket size
+-qcom,pl-freqs:			The driver supports different filter values at
+				three different performance levels.  This value
+				defines the cut-over frequenices
+-qcom,reject-rate:		Desired rejection rate used to calculate
+				SPDM threshold
+-qcom,response-time-us:	Desired response time used to calculate
+				SPDM threshold
+-qcom,cci-response-time-us:	Desired response time used to calculate
+				SPDM threshold when CCI is under heavy load
+-qcom,max-cci-freq:		CCI frequency at which cci_response_time_us
+				is used
+-qcom,up-step-multp:		used to increase rate of growth on up votes
+-qcom,spdm-interval:		down-vote polling interval
+
+Example:
+devfreq_spdm_cpu {
+	compatible = "qcom,devfreq_spdm";
+	qcom,msm-bus,name = "devfreq_spdm";
+	qcom,msm-bus,num-cases = <2>;
+	qcom,msm-bus,num-paths = <1>;
+	qcom,msm-bus,vectors-KBps =
+			<1 512 0 0>,
+			<1 512 0 0>;
+	qcom,spdm-client = <0>;
+
+	clock-names = "cci_clk";
+	clocks = <&clock_cpu clk_cci_clk>;
+
+	qcom,bw-upstep = <100>;
+	qcom,bw-dwnstep = <100>;
+	qcom,max-vote = <10000>;
+	qcom,up-step-multp = <2>;
+	qcom,spdm-interval = <100>;
+
+	qcom,ports = <16>;
+	qcom,alpha-up = <7>;
+	qcom,alpha-down = <15>;
+	qcom,bucket-size = <8>;
+
+	/*max pl1 freq, max pl2 freq*/
+	qcom,pl-freqs = <149999999 150000000>;
+
+	/* pl1 low, pl1 high, pl2 low, pl2 high, pl3 low, pl3 high */
+	qcom,reject-rate = <5000 5000 5000 5000 5000 5000>;
+	/* pl1 low, pl1 high, pl2 low, pl2 high, pl3 low, pl3 high */
+	qcom,response-time-us = <220 220 2000 2000 900 900>;
+	/* pl1 low, pl1 high, pl2 low, pl2 high, pl3 low, pl3 high */
+	qcom,cci-response-time-us = <50 50 30 30 20 20>;
+	qcom,max-cci-freq = <600000000>;
+};
+
+This device is always used with the SPDM governor which requires a device tree
+entry to know what IRQ to respond to.
+
+Required properties:
+-compatible			Must be "qcom,gov_spdm_hyp"
+-interrupt-names		SPDM irq to handle.  Name should be "spdm-irq"
+-interrupts			The interrupt number the SPDM hw is assigned
+
+Example:
+devfreq_spdm_gov {
+	compatible = "qcom,gov_spdm_hyp";
+	interrupt-names = "spdm-irq";
+	interrupts = <0 192 0>;
+};

drivers/devfreq/Kconfig

@@ -111,6 +111,15 @@ config DEVFREQ_GOV_MSM_CACHE_HWMON
 	  conflict with existing profiling tools.  This governor is unlikely
 	  to be useful for other devices.
 
+config DEVFREQ_GOV_SPDM_HYP
+	bool "MSM SPDM Hypervisor Governor"
+	depends on ARCH_MSM
+	help
+	  Hypervisor based governor for CPU bandwidth voting
+	  for MSM chipsets.
+	  Sets the frequency using a "on-demand" algorithm.
+	  This governor is unlikely to be useful for other devices.
+
 comment "DEVFREQ Drivers"
 
 config ARM_EXYNOS4_BUS_DEVFREQ
@@ -169,6 +178,15 @@ config MSM_DEVFREQ_DEVBW
 	  agnostic interface to so that some of the devfreq governors can be
 	  shared across SoCs.
 
+config DEVFREQ_SPDM
+	bool "MSM SPDM based bandwidth voting"
+	depends on ARCH_MSM
+	select DEVFREQ_GOV_SPDM_HYP
+	help
+	  This adds the support for SPDM based bandwidth voting on MSM chipsets.
+	  This driver allows any SPDM based client to vote for bandwidth.
+	  Used with the MSM SPDM Hypervisor Governor.
+
 source "drivers/devfreq/event/Kconfig"
 
 endif # PM_DEVFREQ

drivers/devfreq/Makefile

@@ -10,6 +10,7 @@ obj-$(CONFIG_ARCH_MSM_KRAIT)		+= krait-l2pm.o
 obj-$(CONFIG_MSM_BIMC_BWMON)		+= bimc-bwmon.o
 obj-$(CONFIG_DEVFREQ_GOV_MSM_BW_HWMON)	+= governor_bw_hwmon.o
 obj-$(CONFIG_DEVFREQ_GOV_MSM_CACHE_HWMON)	+= governor_cache_hwmon.o
+obj-$(CONFIG_DEVFREQ_GOV_SPDM_HYP) 	+= governor_spdm_bw_hyp.o
 
 # DEVFREQ Drivers
 obj-$(CONFIG_ARM_EXYNOS4_BUS_DEVFREQ)	+= exynos/
@@ -17,6 +18,7 @@ obj-$(CONFIG_ARM_EXYNOS5_BUS_DEVFREQ)	+= exynos/
 obj-$(CONFIG_ARM_TEGRA_DEVFREQ)		+= tegra-devfreq.o
 obj-$(CONFIG_MSM_DEVFREQ_DEVBW)		+= devfreq_devbw.o
 obj-$(CONFIG_DEVFREQ_SIMPLE_DEV)	+= devfreq_simple_dev.o
+obj-$(CONFIG_DEVFREQ_SPDM) 		+= devfreq_spdm.o
 
 # DEVFREQ Event Drivers
 obj-$(CONFIG_PM_DEVFREQ_EVENT)		+= event/

drivers/devfreq/devfreq_spdm.c

@@ -0,0 +1,341 @@
+/*
+*Copyright (c) 2014, 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/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 <linux/slab.h>
+#include <linux/uaccess.h>
+#include <soc/qcom/hvc.h>
+
+#include "governor.h"
+#include "devfreq_spdm.h"
+
+#define DEVFREQ_SPDM_DEFAULT_WINDOW_MS 100
+
+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 hvc_desc desc = { { 0 } };
+	int hvc_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;
+	desc.arg[2] = clk_get_rate(data->cci_clk);
+	hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+	if (hvc_status)
+		pr_err("HVC command %u failed with error %u", (int)desc.arg[0],
+			hvc_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);
+		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);
+	return ret;
+}
+
+static int probe(struct platform_device *pdev)
+{
+	struct spdm_data *data = 0;
+	int ret = -EINVAL;
+
+	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;
+
+	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)) {
+		ret = PTR_ERR(data->cci_clk);
+		goto no_clock;
+	}
+
+	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_profile;
+	}
+
+	return 0;
+
+no_profile:
+no_clock:
+	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);
+	return ret;
+}
+
+static int remove(struct platform_device *pdev)
+{
+	struct spdm_data *data = 0;
+
+	data = platform_get_drvdata(pdev);
+
+	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);
+
+	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");

drivers/devfreq/devfreq_spdm.h

@@ -0,0 +1,103 @@
+/*
+*Copyright (c) 2014, 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.
+*/
+
+#ifndef DEVFREQ_SPDM_H
+#define DEVFREQ_SPDM_H
+
+#include <linux/list.h>
+
+enum pl_levels { SPDM_PL1, SPDM_PL2, SPDM_PL3, SPDM_PL_COUNT };
+enum actions { SPDM_UP, SPDM_DOWN };
+enum spdm_client { SPDM_CLIENT_CPU, SPDM_CLIENT_GPU, SPDM_CLIENT_COUNT };
+
+struct spdm_config_data {
+	/* in MB/s */
+	u32 upstep;
+	u32 downstep;
+	u32 up_step_multp;
+
+	u32 num_ports;
+	u32 *ports;
+	u32 aup;
+	u32 adown;
+	u32 bucket_size;
+
+	/*
+	 * If We define n PL levels we need n-1 frequencies to tell
+	 * where to change from one pl to another
+	 */
+	/* hz */
+	u32 pl_freqs[SPDM_PL_COUNT - 1];
+	/*
+	 * We have a low threshold and a high threhold for each pl to support
+	 * the two port solution so we need twice as many entries as
+	 * performance levels
+	 */
+	/* in 100th's of a percent */
+	u32 reject_rate[SPDM_PL_COUNT * 2];
+	u32 response_time_us[SPDM_PL_COUNT * 2];
+	u32 cci_response_time_us[SPDM_PL_COUNT * 2];
+	/* hz */
+	u32 max_cci_freq;
+	/* in MB/s */
+	u32 max_vote;
+
+};
+
+struct spdm_data {
+	/* bus scaling data */
+	int cur_idx;
+	struct msm_bus_scale_pdata *pdata;
+	u32 bus_scale_client_id;
+	/* in mb/s */
+	u32 new_bw;
+
+	/* devfreq data */
+	struct devfreq *devfreq;
+	struct devfreq_dev_profile *profile;
+	unsigned long action;
+	int window;
+	struct clk *cci_clk;
+
+	/* spdm hw/gov data */
+	struct spdm_config_data config_data;
+
+	enum spdm_client spdm_client;
+	/* list used by governor to keep track of spdm devices */
+	struct list_head list;
+
+	struct dentry *debugfs_dir;
+};
+
+#define SPDM_HYP_FNID 5
+/* CMD ID's for hypervisor */
+#define SPDM_CMD_GET_BW_ALL 1
+#define SPDM_CMD_GET_BW_SPECIFIC 2
+#define SPDM_CMD_ENABLE 3
+#define SPDM_CMD_DISABLE 4
+#define SPDM_CMD_CFG_PORTS 5
+#define SPDM_CMD_CFG_FLTR 6
+#define SPDM_CMD_CFG_PL 7
+#define SPDM_CMD_CFG_REJRATE_LOW 8
+#define SPDM_CMD_CFG_REJRATE_MED 9
+#define SPDM_CMD_CFG_REJRATE_HIGH 10
+#define SPDM_CMD_CFG_RESPTIME_LOW 11
+#define SPDM_CMD_CFG_RESPTIME_MED 12
+#define SPDM_CMD_CFG_RESPTIME_HIGH 13
+#define SPDM_CMD_CFG_CCIRESPTIME_LOW 14
+#define SPDM_CMD_CFG_CCIRESPTIME_MED 15
+#define SPDM_CMD_CFG_CCIRESPTIME_HIGH 16
+#define SPDM_CMD_CFG_MAXCCI 17
+#define SPDM_CMD_CFG_VOTES 18
+
+#endif

drivers/devfreq/governor_spdm_bw_hyp.c

@@ -0,0 +1,398 @@
+/*
+*Copyright (c) 2014, 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/devfreq.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irqreturn.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <soc/qcom/rpm-smd.h>
+#include <soc/qcom/hvc.h>
+#include "governor.h"
+#include "devfreq_spdm.h"
+
+enum msm_spdm_rt_res {
+	SPDM_RES_ID = 1,
+	SPDM_RES_TYPE = 0x63707362,
+	SPDM_KEY = 0x00006e65,
+	SPDM_SIZE = 4,
+};
+
+static LIST_HEAD(devfreqs);
+static DEFINE_MUTEX(devfreqs_lock);
+
+static int enable_clocks(void)
+{
+	struct msm_rpm_request *rpm_req;
+	int id;
+	const int one = 1;
+	rpm_req = msm_rpm_create_request(MSM_RPM_CTX_ACTIVE_SET, SPDM_RES_TYPE,
+					 SPDM_RES_ID, 1);
+	if (!rpm_req)
+		return -ENODEV;
+	msm_rpm_add_kvp_data(rpm_req, SPDM_KEY, (const uint8_t *)&one,
+			     sizeof(int));
+	id = msm_rpm_send_request(rpm_req);
+	msm_rpm_wait_for_ack(id);
+	msm_rpm_free_request(rpm_req);
+
+	return 0;
+}
+
+static int disable_clocks(void)
+{
+	struct msm_rpm_request *rpm_req;
+	int id;
+	const int zero = 0;
+	rpm_req = msm_rpm_create_request(MSM_RPM_CTX_ACTIVE_SET, SPDM_RES_TYPE,
+					 SPDM_RES_ID, 1);
+	if (!rpm_req)
+		return -ENODEV;
+	msm_rpm_add_kvp_data(rpm_req, SPDM_KEY, (const uint8_t *)&zero,
+			     sizeof(int));
+	id = msm_rpm_send_request(rpm_req);
+	msm_rpm_wait_for_ack(id);
+	msm_rpm_free_request(rpm_req);
+
+	return 0;
+}
+
+static irqreturn_t threaded_isr(int irq, void *dev_id)
+{
+	struct spdm_data *data;
+	struct hvc_desc desc = { { 0 } };
+	int hvc_status = 0;
+
+	/* call hyp to get bw_vote */
+	desc.arg[0] = SPDM_CMD_GET_BW_ALL;
+	hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+	if (hvc_status)
+		pr_err("HVC command %u failed with error %u", (int)desc.arg[0],
+			hvc_status);
+	mutex_lock(&devfreqs_lock);
+	list_for_each_entry(data, &devfreqs, list) {
+		if (data->spdm_client == desc.ret[0]) {
+			devfreq_monitor_suspend(data->devfreq);
+			mutex_lock(&data->devfreq->lock);
+			data->action = SPDM_UP;
+			data->new_bw = desc.ret[1] >> 6;
+			update_devfreq(data->devfreq);
+			data->action = SPDM_DOWN;
+			mutex_unlock(&data->devfreq->lock);
+			devfreq_monitor_resume(data->devfreq);
+			break;
+		}
+	}
+	mutex_unlock(&devfreqs_lock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t isr(int irq, void *dev_id)
+{
+	return IRQ_WAKE_THREAD;
+}
+
+static int gov_spdm_hyp_target_bw(struct devfreq *devfreq, unsigned long *freq,
+				  u32 *flag)
+{
+	struct devfreq_dev_status status;
+	int ret = -EINVAL;
+	int usage;
+	struct hvc_desc desc = { { 0 } };
+	int hvc_status = 0;
+
+	if (!devfreq || !devfreq->profile || !devfreq->profile->get_dev_status)
+		return ret;
+
+	ret = devfreq->profile->get_dev_status(devfreq->dev.parent, &status);
+	if (ret)
+		return ret;
+
+	usage = (status.busy_time * 100) / status.total_time;
+
+	if (usage > 0) {
+		/* up was already called as part of hyp, so just use the
+		 * already stored values */
+		*freq = ((struct spdm_data *)devfreq->data)->new_bw;
+	} else {
+		desc.arg[0] = SPDM_CMD_GET_BW_SPECIFIC;
+		desc.arg[1] = ((struct spdm_data *)devfreq->data)->spdm_client;
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		*freq = desc.ret[0] >> 6;
+	}
+
+	return 0;
+}
+
+static int gov_spdm_hyp_eh(struct devfreq *devfreq, unsigned int event,
+			   void *data)
+{
+	struct hvc_desc desc = { { 0 } };
+	int hvc_status = 0;
+	struct spdm_data *spdm_data = (struct spdm_data *)devfreq->data;
+	int i;
+
+	switch (event) {
+	case DEVFREQ_GOV_START:
+		mutex_lock(&devfreqs_lock);
+		list_add(&spdm_data->list, &devfreqs);
+		mutex_unlock(&devfreqs_lock);
+		/* call hyp with config data */
+		desc.arg[0] = SPDM_CMD_CFG_PORTS;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.num_ports;
+		for (i = 0; i < spdm_data->config_data.num_ports; i++)
+			desc.arg[i+3] = spdm_data->config_data.ports[i];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_FLTR;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.aup;
+		desc.arg[3] = spdm_data->config_data.adown;
+		desc.arg[4] = spdm_data->config_data.bucket_size;
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_PL;
+		desc.arg[1] = spdm_data->spdm_client;
+		for (i = 0; i < SPDM_PL_COUNT - 1; i++)
+			desc.arg[i+2] = spdm_data->config_data.pl_freqs[i];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_REJRATE_LOW;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.reject_rate[0];
+		desc.arg[3] = spdm_data->config_data.reject_rate[1];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		desc.arg[0] = SPDM_CMD_CFG_REJRATE_MED;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.reject_rate[2];
+		desc.arg[3] = spdm_data->config_data.reject_rate[3];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		desc.arg[0] = SPDM_CMD_CFG_REJRATE_HIGH;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.reject_rate[4];
+		desc.arg[3] = spdm_data->config_data.reject_rate[5];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_RESPTIME_LOW;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.response_time_us[0];
+		desc.arg[3] = spdm_data->config_data.response_time_us[1];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		desc.arg[0] = SPDM_CMD_CFG_RESPTIME_MED;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.response_time_us[2];
+		desc.arg[3] = spdm_data->config_data.response_time_us[3];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		desc.arg[0] = SPDM_CMD_CFG_RESPTIME_HIGH;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.response_time_us[4];
+		desc.arg[3] = spdm_data->config_data.response_time_us[5];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_CCIRESPTIME_LOW;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.cci_response_time_us[0];
+		desc.arg[3] = spdm_data->config_data.cci_response_time_us[1];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		desc.arg[0] = SPDM_CMD_CFG_CCIRESPTIME_MED;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.cci_response_time_us[2];
+		desc.arg[3] = spdm_data->config_data.cci_response_time_us[3];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		desc.arg[0] = SPDM_CMD_CFG_CCIRESPTIME_HIGH;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.cci_response_time_us[4];
+		desc.arg[3] = spdm_data->config_data.cci_response_time_us[5];
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_MAXCCI;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.max_cci_freq;
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		desc.arg[0] = SPDM_CMD_CFG_VOTES;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = spdm_data->config_data.upstep;
+		desc.arg[3] = spdm_data->config_data.downstep;
+		desc.arg[4] = spdm_data->config_data.max_vote;
+		desc.arg[5] = spdm_data->config_data.up_step_multp;
+		hvc_status = hvc(HVC_FN_SIP(SPDM_HYP_FNID), &desc);
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+
+		/* call hyp enable/commit */
+		desc.arg[0] = SPDM_CMD_ENABLE;
+		desc.arg[1] = spdm_data->spdm_client;
+		desc.arg[2] = 0;
+		if (hvc_status) {
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+			return -EINVAL;
+		}
+		devfreq_monitor_start(devfreq);
+		break;
+
+	case DEVFREQ_GOV_STOP:
+		devfreq_monitor_stop(devfreq);
+		/* find devfreq in list and remove it */
+		mutex_lock(&devfreqs_lock);
+		list_del(&spdm_data->list);
+		mutex_unlock(&devfreqs_lock);
+
+		/* call hypvervisor to disable */
+		desc.arg[0] = SPDM_CMD_DISABLE;
+		desc.arg[1] = spdm_data->spdm_client;
+		if (hvc_status)
+			pr_err("HVC command %u failed with error %u",
+				(int)desc.arg[0], hvc_status);
+		break;
+
+	case DEVFREQ_GOV_INTERVAL:
+		devfreq_interval_update(devfreq, (unsigned int *)data);
+		break;
+
+	case DEVFREQ_GOV_SUSPEND:
+		devfreq_monitor_suspend(devfreq);
+		break;
+
+	case DEVFREQ_GOV_RESUME:
+		devfreq_monitor_resume(devfreq);
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static struct devfreq_governor spdm_hyp_gov = {
+	.name = "spdm_bw_hyp",
+	.get_target_freq = gov_spdm_hyp_target_bw,
+	.event_handler = gov_spdm_hyp_eh,
+};
+
+static int probe(struct platform_device *pdev)
+{
+	int ret = -EINVAL;
+	int *irq = 0;
+
+	irq = devm_kzalloc(&pdev->dev, sizeof(int), GFP_KERNEL);
+	if (!irq)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, irq);
+
+	ret = devfreq_add_governor(&spdm_hyp_gov);
+	if (ret)
+		goto nogov;
+
+	*irq = platform_get_irq_byname(pdev, "spdm-irq");
+	ret = request_threaded_irq(*irq, isr, threaded_isr,
+				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				   spdm_hyp_gov.name, pdev);
+	if (ret)
+		goto no_irq;
+
+	enable_clocks();
+	return 0;
+
+no_irq:
+	devfreq_remove_governor(&spdm_hyp_gov);
+nogov:
+	devm_kfree(&pdev->dev, irq);
+	return ret;
+}
+
+static int remove(struct platform_device *pdev)
+{
+	int *irq = 0;
+
+	disable_clocks();
+	irq = platform_get_drvdata(pdev);
+	free_irq(*irq, pdev);
+	devfreq_remove_governor(&spdm_hyp_gov);
+	devm_kfree(&pdev->dev, irq);
+	return 0;
+}
+
+static const struct of_device_id gov_spdm_match[] = {
+	{.compatible = "qcom,gov_spdm_hyp"},
+	{}
+};
+
+static struct platform_driver gov_spdm_hyp_drvr = {
+	.driver = {
+		   .name = "gov_spdm_hyp",
+		   .owner = THIS_MODULE,
+		   .of_match_table = gov_spdm_match,
+		   },
+	.probe = probe,
+	.remove = remove,
+};
+
+static int __init governor_spdm_bw_hyp(void)
+{
+	return platform_driver_register(&gov_spdm_hyp_drvr);
+}
+
+module_init(governor_spdm_bw_hyp);
+
+MODULE_LICENSE("GPL v2");