diff --git a/Documentation/devicetree/bindings/devfreq/devfreq-spdm.txt b/Documentation/devicetree/bindings/devfreq/devfreq-spdm.txt new file mode 100644 index 000000000000..16303f769d42 --- /dev/null +++ b/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>; +}; diff --git a/drivers/devfreq/Kconfig b/drivers/devfreq/Kconfig index bc8945e7e41b..e2222972dae2 100644 --- a/drivers/devfreq/Kconfig +++ b/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 diff --git a/drivers/devfreq/Makefile b/drivers/devfreq/Makefile index f18ddfbfe677..08f561d860b0 100644 --- a/drivers/devfreq/Makefile +++ b/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/ diff --git a/drivers/devfreq/devfreq_spdm.c b/drivers/devfreq/devfreq_spdm.c new file mode 100644 index 000000000000..749cfe1aaefa --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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"); diff --git a/drivers/devfreq/devfreq_spdm.h b/drivers/devfreq/devfreq_spdm.h new file mode 100644 index 000000000000..24c2b2be7f06 --- /dev/null +++ b/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 + +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 diff --git a/drivers/devfreq/governor_spdm_bw_hyp.c b/drivers/devfreq/governor_spdm_bw_hyp.c new file mode 100644 index 000000000000..dbb6d9beabbe --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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");