Merge "driver: soc: adsp vote: QMI driver to send ADSP vote" (7c1dca23) · Commits · e / devices / android_kernel_fairphone_FP5

drivers/soc/qcom/Kconfig

+8 −0

Original line number	Diff line number	Diff line
		@@ -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

drivers/soc/qcom/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -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

drivers/soc/qcom/adsp_lpm_voting_v01.c

0 → 100644

+40 −0

Original line number	Diff line number	Diff line
		// SPDX-License-Identifier: GPL-2.0-only
		/*
		* Copyright (c) 2019,2021, The Linux Foundation. All rights reserved.
		*/

		#include <linux/soc/qcom/qmi.h>
		#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,
		},
		};

drivers/soc/qcom/adsp_lpm_voting_v01.h

0 → 100644

+28 −0

Original line number	Diff line number	Diff line
		/* 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

drivers/soc/qcom/adsp_vote_qmi.c

0 → 100644

+286 −0

Original line number	Diff line number	Diff line
		// SPDX-License-Identifier: GPL-2.0-only
		/*
		* Copyright (c) 2019,2021, The Linux Foundation. All rights reserved.
		*/

		#include <linux/kernel.h>
		#include <linux/module.h>
		#include <linux/device.h>
		#include <linux/platform_device.h>
		#include <linux/workqueue.h>
		#include <linux/soc/qcom/qmi.h>
		#include <linux/slab.h>
		#include <linux/iommu.h>
		#include <linux/cpu_pm.h>
		#include <linux/pm.h>
		#include "adsp_lpm_voting_v01.h"
		#include <linux/debugfs.h>

		#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");