| /* |
| *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; |
| desc.arg[2] = clk_get_rate(data->cci_clk); |
| 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}; |
| desc.arginfo = SCM_ARGS(num_args); |
| 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; |
| |
| 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_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: |
| 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); |
| 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"); |