From b4e935e69abd8746324c54ca7f7ba28844a615ec Mon Sep 17 00:00:00 2001 From: Vinay Sudra Date: Fri, 12 Feb 2021 12:30:25 +0530 Subject: [PATCH] driver: soc: adsp vote: QMI driver to send ADSP vote New QMI communication driver implements sending ADSP unvote while suspending APSS and sending ADSP vote while waking up. Change-Id: I2dd0768d48dffd2da14f3eccf02d2b088024ce07 Signed-off-by: Vinay Sudra --- drivers/soc/qcom/Kconfig | 8 + drivers/soc/qcom/Makefile | 1 + drivers/soc/qcom/adsp_lpm_voting_v01.c | 40 ++++ drivers/soc/qcom/adsp_lpm_voting_v01.h | 28 +++ drivers/soc/qcom/adsp_vote_qmi.c | 286 +++++++++++++++++++++++++ 5 files changed, 363 insertions(+) create mode 100644 drivers/soc/qcom/adsp_lpm_voting_v01.c create mode 100644 drivers/soc/qcom/adsp_lpm_voting_v01.h create mode 100644 drivers/soc/qcom/adsp_vote_qmi.c diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig index 60576ea67049..e6a259e6fb5b 100644 --- a/drivers/soc/qcom/Kconfig +++ b/drivers/soc/qcom/Kconfig @@ -1174,5 +1174,13 @@ config RENAME_BLOCK_DEVICE device names created by default as sda1, sda2 etc. to the names system is needed.this is applicable for block devices only. +config QCOM_ADSP_MANUAL_VOTE + bool "Send commands to ADSP to unvote/vote during Suspend/resume" + help + This driver is used to manually release and acquire the ADSP vote from + APPS processor during system suspend and resume. + This driver sends message over QMI to the service which is running on + ADSP. + source "drivers/soc/qcom/icnss2/Kconfig" endmenu diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile index d37792330201..8114b6afe539 100644 --- a/drivers/soc/qcom/Makefile +++ b/drivers/soc/qcom/Makefile @@ -105,3 +105,4 @@ obj-$(CONFIG_QTI_HW_MEMLAT) += rimps_memlat.o obj-$(CONFIG_QTI_HW_MEMLAT_LOG) += rimps_log.o obj-$(CONFIG_QCOM_QFPROM_SYS) += qfprom-sys.o obj-$(CONFIG_RENAME_BLOCK_DEVICE) += rename_block_device.o +obj-$(CONFIG_QCOM_ADSP_MANUAL_VOTE) += adsp_vote_qmi.o adsp_lpm_voting_v01.o diff --git a/drivers/soc/qcom/adsp_lpm_voting_v01.c b/drivers/soc/qcom/adsp_lpm_voting_v01.c new file mode 100644 index 000000000000..8af3d78b84fb --- /dev/null +++ b/drivers/soc/qcom/adsp_lpm_voting_v01.c @@ -0,0 +1,40 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2019,2021, The Linux Foundation. All rights reserved. + */ + +#include +#include "adsp_lpm_voting_v01.h" + +struct qmi_elem_info prod_set_lpm_vote_req_msg_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x01, + .offset = offsetof(struct prod_set_lpm_vote_req_msg_v01, + keep_adsp_out_of_lpm), + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + }, +}; + +struct qmi_elem_info prod_set_lpm_vote_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct prod_set_lpm_vote_resp_msg_v01, + resp), + .ei_array = qmi_response_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + }, +}; diff --git a/drivers/soc/qcom/adsp_lpm_voting_v01.h b/drivers/soc/qcom/adsp_lpm_voting_v01.h new file mode 100644 index 000000000000..6924ff44d07d --- /dev/null +++ b/drivers/soc/qcom/adsp_lpm_voting_v01.h @@ -0,0 +1,28 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * Copyright (c) 2019,2021, The Linux Foundation. All rights reserved. + */ + +#ifndef ADSP_LPM_VOTING_V01_H +#define ADSP_LPM_VOTING_V01_H + +#define PGS_SERVICE_ID_V01 0x428 +#define PGS_SERVICE_VERS_V01 0x01 + +#define PROD_SET_LPM_VOTE_REQ_V01 0x0001 +#define PROD_SET_LPM_VOTE_RESP_V01 0x0001 + + +struct prod_set_lpm_vote_req_msg_v01 { + u8 keep_adsp_out_of_lpm; +}; +#define PROD_SET_LPM_VOTE_REQ_MSG_V01_MAX_MSG_LEN 4 +extern struct qmi_elem_info prod_set_lpm_vote_req_msg_v01_ei[]; + +struct prod_set_lpm_vote_resp_msg_v01 { + struct qmi_response_type_v01 resp; +}; + +#define PROD_SET_LPM_VOTE_RESP_MSG_V01_MAX_MSG_LEN 7 +extern struct qmi_elem_info prod_set_lpm_vote_resp_msg_v01_ei[]; +#endif diff --git a/drivers/soc/qcom/adsp_vote_qmi.c b/drivers/soc/qcom/adsp_vote_qmi.c new file mode 100644 index 000000000000..c4a8c0e54920 --- /dev/null +++ b/drivers/soc/qcom/adsp_vote_qmi.c @@ -0,0 +1,286 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2019,2021, The Linux Foundation. All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "adsp_lpm_voting_v01.h" +#include + +#define PGS_TIMEOUT msecs_to_jiffies(3000) + +static struct sockaddr_qrtr sq; +static struct dentry *dent_adsp_vote, *adsp_vote; +struct platform_device *pgdata; + +static int pgs_vote_send_sync_msg(struct qmi_handle *dev, int vote) +{ + int ret; + struct prod_set_lpm_vote_req_msg_v01 *req; + struct prod_set_lpm_vote_resp_msg_v01 *resp; + struct qmi_txn txn; + + if (!dev) + return -ENODEV; + + req = kzalloc(sizeof(struct prod_set_lpm_vote_req_msg_v01), GFP_KERNEL); + if (!req) + return -ENOMEM; + + resp = kzalloc(sizeof(struct prod_set_lpm_vote_resp_msg_v01), + GFP_KERNEL); + if (!resp) { + kfree(req); + return -ENOMEM; + } + + req->keep_adsp_out_of_lpm = (u8) vote; + + ret = qmi_txn_init(dev, &txn, prod_set_lpm_vote_resp_msg_v01_ei, resp); + if (ret < 0) { + pr_err("Failed Init txn for Mode resp %d\n", ret); + goto out; + } + + ret = qmi_send_request(dev, &sq, &txn, + PROD_SET_LPM_VOTE_REQ_V01, + PROD_SET_LPM_VOTE_REQ_MSG_V01_MAX_MSG_LEN, + prod_set_lpm_vote_req_msg_v01_ei, req); + + if (ret < 0) { + qmi_txn_cancel(&txn); + pr_err("Fail to send Mode req %d\n", ret); + goto out; + } + + ret = qmi_txn_wait(&txn, PGS_TIMEOUT); + + if (ret < 0) { + pr_err("Mode resp wait failed with ret %d\n", ret); + goto out; + } + + if (resp->resp.result != QMI_RESULT_SUCCESS_V01) { + pr_err("QMI Mode request rejected, result:%d error:%d\n", + resp->resp.result, resp->resp.error); + ret = -resp->resp.result; + goto out; + } + + if (!vote) + pr_info("ADSP manual unvoting is successful\n"); + else + pr_info("ADSP manual voting is successful\n"); + +out: + kfree(req); + kfree(resp); + return ret; +} + +static struct qmi_handle adsp_qmi_client; + +static int send_adsp_manual_unvote(struct device *dev) +{ + int ret = 0; + struct qmi_handle *qmi_dev = (struct qmi_handle *) dev_get_drvdata(dev); + + pr_info("ADSP unvoting initiated\n"); + ret = pgs_vote_send_sync_msg(qmi_dev, 0); + + if (ret < 0) + pr_err("ADSP Unvoting is failed\n"); + return 0; +} + +static int send_adsp_manual_vote(struct device *dev) +{ + int ret = 0; + struct qmi_handle *qmi_dev = (struct qmi_handle *) dev_get_drvdata(dev); + + pr_info("ADSP voting initiated\n"); + ret = pgs_vote_send_sync_msg(qmi_dev, 1); + + if (ret < 0) + pr_err("ADSP voting is failed\n"); + return 0; +} + +static int qmi_adsp_manual_vote_new_server(struct qmi_handle *qmi, + struct qmi_service *service) +{ + struct platform_device *pdev; + int ret; + + sq.sq_family = AF_QIPCRTR; + sq.sq_node = service->node; + sq.sq_port = service->port; + + pdev = platform_device_alloc("qmi_adsp_vote_client", + PLATFORM_DEVID_AUTO); + if (!pdev) + return -ENOMEM; + + ret = platform_device_add_data(pdev, &sq, sizeof(sq)); + if (ret) + goto err_put_device; + ret = platform_device_add(pdev); + if (ret) + goto err_put_device; + + service->priv = pdev; + return 0; + +err_put_device: + platform_device_put(pdev); + return ret; +} + +static void qmi_adsp_manual_vote_del_server(struct qmi_handle *qmi, + struct qmi_service *service) +{ + struct platform_device *pdev = service->priv; + + platform_device_unregister(pdev); +} + +static struct qmi_ops adsp_qmi_ops = { + .new_server = qmi_adsp_manual_vote_new_server, + .del_server = qmi_adsp_manual_vote_del_server, +}; + +static const struct dev_pm_ops adsp_manual_vote_dev_pm_ops = { + SET_NOIRQ_SYSTEM_SLEEP_PM_OPS(send_adsp_manual_unvote, + send_adsp_manual_vote) +}; + +static int adsp_manual_vote_op(void *data, u64 vote) +{ + if (vote == 0) + send_adsp_manual_unvote(&pgdata->dev); + else if (vote == 1) + send_adsp_manual_vote(&pgdata->dev); + else + pr_err("%s: Invalid vote type\n", __func__); + return 0; +} +DEFINE_SIMPLE_ATTRIBUTE(fops_vote, NULL, adsp_manual_vote_op, "%ull\n"); + +static int adsp_manual_vote_driver_probe(struct platform_device *pdev) +{ + int ret = 0; + struct qmi_handle *dev; + + dev = kzalloc(sizeof(*dev), GFP_KERNEL); + if (!dev) + return -ENOMEM; + + ret = qmi_handle_init(dev, + PROD_SET_LPM_VOTE_REQ_MSG_V01_MAX_MSG_LEN, + &adsp_qmi_ops, NULL); + if (ret < 0) { + pr_err("Successfully got the qmi_handle for the client\n"); + goto err_handle; + } + + //Register a new lookup with the service PGS_SERVICE_ID_V01 + ret = qmi_add_lookup(dev, PGS_SERVICE_ID_V01, + PGS_SERVICE_VERS_V01, 0); + + if (ret < 0) + goto err_handle; + + dent_adsp_vote = debugfs_create_dir("adsp_manual_vote", NULL); + if (IS_ERR_OR_NULL(dent_adsp_vote)) { + dev_err(&pdev->dev, "%s: debugfs create dir failed %d\n", + __func__, ret); + ret = -ENODEV; + goto err_handle; + } + + adsp_vote = debugfs_create_file("vote", 0220, dent_adsp_vote, NULL, + &fops_vote); + if (IS_ERR_OR_NULL(adsp_vote)) { + debugfs_remove(dent_adsp_vote); + dent_adsp_vote = NULL; + dev_err(&pdev->dev, "%s: debugfs create file failed %d\n", + __func__, ret); + ret = -ENODEV; + goto err_handle; + } + + platform_set_drvdata(pdev, (void *) dev); + pgdata = pdev; + pr_info("ADSP vote device is registered successfully\n"); + return ret; + +err_handle: + kfree(dev); + return ret; +} + +static int adsp_manual_vote_driver_remove(struct platform_device *pdev) +{ + struct qmi_handle *dev = (struct qmi_handle *) + platform_get_drvdata(pdev); + + qmi_handle_release(dev); + return 0; +} + +static struct platform_device adsp_manual_vote_device = { + .name = "adsp_manual_vote", + .id = -1, +}; + +static struct platform_driver adsp_manual_vote_driver = { + .probe = adsp_manual_vote_driver_probe, + .remove = adsp_manual_vote_driver_remove, + .driver = { + .name = "adsp_manual_vote", + .pm = &adsp_manual_vote_dev_pm_ops, + }, +}; + +static int __init adsp_manual_vote_qmi_init(void) +{ + int ret = 0; + + ret = platform_device_register(&adsp_manual_vote_device); + if (ret) + return -ENODEV; + + pr_info("ADSP vote device is registered successfully\n"); + + ret = platform_driver_register(&adsp_manual_vote_driver); + if (ret) { + pr_err("%s: fail to register ADSP manual vote device driver\n", + __func__); + goto out; + } +out: + return ret; +} + +static void __exit adsp_manual_vote_qmi_deinit(void) +{ + debugfs_remove_recursive(dent_adsp_vote); + qmi_handle_release(&adsp_qmi_client); + platform_driver_unregister(&adsp_manual_vote_driver); + platform_device_unregister(&adsp_manual_vote_device); +} + +module_init(adsp_manual_vote_qmi_init); +module_exit(adsp_manual_vote_qmi_deinit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("ADSP Manual vote QMI client driver");