414 lines
12 KiB
C
414 lines
12 KiB
C
|
/*
|
||
|
|
*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/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 "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 spdm_args desc = { { 0 } };
|
||
|
|
int ext_status = 0;
|
||
|
|
|
||
|
|
/* call hyp to get bw_vote */
|
||
|
|
desc.arg[0] = SPDM_CMD_GET_BW_ALL;
|
||
|
|
ext_status = spdm_ext_call(&desc, 1);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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] * 1000) >> 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 spdm_args desc = { { 0 } };
|
||
|
|
int ext_status = 0;
|
||
|
|
u64 bw_ret;
|
||
|
|
|
||
|
|
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;
|
||
|
|
ext_status = spdm_ext_call(&desc, 2);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_status);
|
||
|
|
bw_ret = desc.ret[0] * 1000;
|
||
|
|
*freq = bw_ret >> 6;
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int gov_spdm_hyp_eh(struct devfreq *devfreq, unsigned int event,
|
||
|
|
void *data)
|
||
|
|
{
|
||
|
|
struct spdm_args desc = { { 0 } };
|
||
|
|
int ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc,
|
||
|
|
spdm_data->config_data.num_ports + 3);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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;
|
||
|
|
ext_status = spdm_ext_call(&desc, 5);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, SPDM_PL_COUNT + 1);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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];
|
||
|
|
ext_status = spdm_ext_call(&desc, 4);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_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;
|
||
|
|
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);
|
||
|
|
|
||
|
|
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;
|
||
|
|
ext_status = spdm_ext_call(&desc, 6);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_status);
|
||
|
|
|
||
|
|
/* call hyp enable/commit */
|
||
|
|
desc.arg[0] = SPDM_CMD_ENABLE;
|
||
|
|
desc.arg[1] = spdm_data->spdm_client;
|
||
|
|
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);
|
||
|
|
mutex_lock(&devfreqs_lock);
|
||
|
|
/*
|
||
|
|
* the spdm device probe will fail so remove it from
|
||
|
|
* the list to prevent accessing a deleted pointer in
|
||
|
|
* the future
|
||
|
|
* */
|
||
|
|
list_del(&spdm_data->list);
|
||
|
|
mutex_unlock(&devfreqs_lock);
|
||
|
|
return -EINVAL;
|
||
|
|
}
|
||
|
|
spdm_data->enabled = true;
|
||
|
|
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;
|
||
|
|
ext_status = spdm_ext_call(&desc, 2);
|
||
|
|
if (ext_status)
|
||
|
|
pr_err("External command %u failed with error %u",
|
||
|
|
(int)desc.arg[0], ext_status);
|
||
|
|
spdm_data->enabled = false;
|
||
|
|
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");
|