Add 'qcom/opensource/mmrm-driver/' from commit '35211a9255b69d52ddc2b82dc8b1dd2840b98868'
git-subtree-dir: qcom/opensource/mmrm-driver git-subtree-mainline:f9b254670fgit-subtree-split:35211a9255Change-Id: repo: https://git.codelinaro.org/clo/la/platform/vendor/opensource/mmrm-driver tag: VIDEO.LA.4.0.r2-06100-lanai.0
This commit is contained in:
David Wronek
9
qcom/opensource/mmrm-driver/vm/be/Kbuild
Normal file
9
qcom/opensource/mmrm-driver/vm/be/Kbuild
Normal file
@@ -0,0 +1,9 @@
|
||||
ifeq ($(CONFIG_MSM_MMRM_VM),y)
|
||||
LINUXINCLUDE += -I$(MMRM_ROOT)/vm/be/src -I$(MMRM_ROOT)/driver/src
|
||||
|
||||
obj-m += mmrm_vm_be.o
|
||||
mmrm_vm_be-objs := src/mmrm_vm_be_main.o \
|
||||
src/mmrm_vm_be_dispatch.o \
|
||||
src/mmrm_vm_be_msgq.o \
|
||||
../common/src/mmrm_vm_debug.o
|
||||
endif
|
||||
32
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be.h
Normal file
32
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be.h
Normal file
@@ -0,0 +1,32 @@
|
||||
/* SPDX-License-Identifier: GPL-2.0-only */
|
||||
/*
|
||||
* Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
|
||||
*/
|
||||
|
||||
#ifndef _MMRM_VM_BE_H_
|
||||
#define _MMRM_VM_BE_H_
|
||||
|
||||
#include <linux/soc/qcom/msm_mmrm.h>
|
||||
#include <mmrm_vm_interface.h>
|
||||
|
||||
/*
|
||||
* mmrm_vm_be_recv -- BE message receiving thread call this function
|
||||
* for transfer receiving packet to BE
|
||||
* @mmrm_vm: device driver info
|
||||
* @data: message pointer
|
||||
* @size: message size
|
||||
*/
|
||||
int mmrm_vm_be_recv(struct mmrm_vm_driver_data *mmrm_vm, void *data, size_t size);
|
||||
|
||||
/*
|
||||
* mmrm_vm_be_send_response -- BE message receiving thread call this function
|
||||
* for sending back API calling result to FE
|
||||
* @mmrm_vm: specific device driver info
|
||||
* @size: message size
|
||||
*/
|
||||
int mmrm_vm_be_send_response(struct mmrm_vm_driver_data *mmrm_vm, void *msg);
|
||||
|
||||
|
||||
#endif /* _MMRM_VM_BE_H_ */
|
||||
|
||||
|
||||
9
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be.rc
Normal file
9
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be.rc
Normal file
@@ -0,0 +1,9 @@
|
||||
# SPDX-License-Identifier: GPL-2.0-only
|
||||
#
|
||||
# Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
|
||||
#
|
||||
|
||||
on boot
|
||||
insmod /vendor/lib/modules/mmrm_vm_be.ko
|
||||
|
||||
|
||||
292
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be_dispatch.c
Normal file
292
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be_dispatch.c
Normal file
@@ -0,0 +1,292 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
|
||||
*/
|
||||
#include "mmrm_vm_debug.h"
|
||||
#include "mmrm_vm_interface.h"
|
||||
#include "mmrm_vm_msgq.h"
|
||||
#include "mmrm_vm_be.h"
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_send_response - send response to FE
|
||||
* mmrm_vm: driver private data
|
||||
* msg: response message
|
||||
*/
|
||||
|
||||
int mmrm_vm_be_send_response(struct mmrm_vm_driver_data *mmrm_vm, void *msg)
|
||||
{
|
||||
struct mmrm_vm_response_msg_pkt *ppkt = (struct mmrm_vm_response_msg_pkt *)msg;
|
||||
struct mmrm_vm_msg_hdr *hdr = &ppkt->hdr;
|
||||
size_t msg_size = sizeof(*hdr) + hdr->size;
|
||||
int rc;
|
||||
|
||||
hdr->version = MMRM_VM_VER_1;
|
||||
hdr->type = MMRM_VM_TYPE_DATA;
|
||||
hdr->flags = 0;
|
||||
|
||||
rc = mmrm_vm_msgq_send(mmrm_vm, msg, msg_size);
|
||||
d_mpr_l("%s: size:%d rc=%d\n", __func__, msg_size, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_client_register - call mmrm API to register client
|
||||
* mmrm_vm: driver private data
|
||||
* req: request parameters
|
||||
*/
|
||||
static int mmrm_vm_be_client_register(struct mmrm_vm_driver_data *mmrm_vm,
|
||||
struct mmrm_vm_api_request_msg *req)
|
||||
{
|
||||
struct mmrm_client *pClient;
|
||||
int rc;
|
||||
struct mmrm_vm_response_msg_pkt pkt;
|
||||
struct mmrm_client_desc client_desc;
|
||||
|
||||
// unpacketizing the call from fe on SVM
|
||||
client_desc.client_type = req->data.reg.client_type;
|
||||
memcpy(&(client_desc.client_info.desc), &(req->data.reg.desc),
|
||||
sizeof(client_desc.client_info.desc));
|
||||
client_desc.priority = req->data.reg.priority;
|
||||
|
||||
d_mpr_l("%s: register type:%d priority:%d\n", __func__,
|
||||
client_desc.client_type, client_desc.priority);
|
||||
d_mpr_l("%s: domain:%d client ID:%d\n", __func__,
|
||||
client_desc.client_info.desc.client_domain,
|
||||
client_desc.client_info.desc.client_id);
|
||||
d_mpr_l("%s: clk name:%s\n", __func__, client_desc.client_info.desc.name);
|
||||
|
||||
// call mmrm register function
|
||||
pClient = mmrm_client_register(&client_desc);
|
||||
if (pClient != NULL) {
|
||||
mmrm_vm->clk_client_tbl[pClient->client_uid] = pClient;
|
||||
pkt.msg.data.reg.client_id = pClient->client_uid;
|
||||
} else {
|
||||
pkt.msg.data.reg.client_id = U32_MAX;
|
||||
d_mpr_e("%s: client:%p client id:%d\n", __func__, pClient, pkt.msg.data.reg.client_id);
|
||||
}
|
||||
|
||||
// prepare response packet & send to fe on SVM
|
||||
pkt.msg.hd.cmd_id = MMRM_VM_RESPONSE_REGISTER;
|
||||
pkt.msg.hd.seq_no = req->hd.seq_no;
|
||||
pkt.hdr.size = sizeof(pkt.msg.hd) + sizeof(pkt.msg.data.reg);
|
||||
|
||||
d_mpr_l("%s: cmd_id:%d data size:%d\n", __func__, pkt.msg.hd.cmd_id, pkt.hdr.size);
|
||||
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_client_setvalue - call mmrm API to set client values
|
||||
* mmrm_vm: driver private data
|
||||
* req: set client value request parameters
|
||||
*/
|
||||
static int mmrm_vm_be_client_setvalue(struct mmrm_vm_driver_data *mmrm_vm,
|
||||
struct mmrm_vm_api_request_msg *req)
|
||||
{
|
||||
struct mmrm_vm_response_msg_pkt pkt_resp;
|
||||
int rc;
|
||||
struct mmrm_vm_setvalue_request *req_param = &req->data.setval;
|
||||
|
||||
// call mmrm client set value function, and fill response packet
|
||||
|
||||
rc = mmrm_client_set_value(mmrm_vm->clk_client_tbl[req_param->client_id],
|
||||
&req_param->data, req_param->val);
|
||||
|
||||
if (rc != 0) {
|
||||
d_mpr_e("%s: set value rc:%d client id:%d\n", __func__, rc, req_param->client_id);
|
||||
}
|
||||
// prepare response packet & send to fe on SVM
|
||||
pkt_resp.msg.hd.cmd_id = MMRM_VM_RESPONSE_SETVALUE;
|
||||
pkt_resp.msg.hd.seq_no = req->hd.seq_no;
|
||||
pkt_resp.hdr.size = sizeof(pkt_resp.msg.hd) + sizeof(pkt_resp.msg.data.setval);
|
||||
|
||||
pkt_resp.msg.data.setval.val = rc;
|
||||
|
||||
d_mpr_l("%s: cmd_id:%d data size:%d\n", __func__,
|
||||
pkt_resp.msg.hd.cmd_id, pkt_resp.hdr.size);
|
||||
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt_resp);
|
||||
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_client_setvalue_inrange - call mmrm API to set client range values
|
||||
* mmrm_vm: driver private data
|
||||
* req: set client value request parameters
|
||||
*/
|
||||
static int mmrm_vm_be_client_setvalue_inrange(struct mmrm_vm_driver_data *mmrm_vm,
|
||||
struct mmrm_vm_api_request_msg *req)
|
||||
{
|
||||
struct mmrm_vm_response_msg_pkt pkt;
|
||||
int rc;
|
||||
struct mmrm_vm_setvalue_inrange_request *req_param = &req->data.setval_range;
|
||||
|
||||
rc = mmrm_client_set_value_in_range(mmrm_vm->clk_client_tbl[req_param->client_id],
|
||||
&req_param->data, &req_param->val);
|
||||
|
||||
pkt.msg.hd.cmd_id = MMRM_VM_RESPONSE_SETVALUE_INRANGE;
|
||||
pkt.msg.hd.seq_no = req->hd.seq_no;
|
||||
pkt.msg.data.setval_range.ret_code = rc;
|
||||
pkt.hdr.size = sizeof(pkt.msg.hd) + sizeof(pkt.msg.data.setval_range);
|
||||
|
||||
d_mpr_l("%s: cmd_id:%d data size:%d\n", __func__, pkt.msg.hd.cmd_id, pkt.hdr.size);
|
||||
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_client_getvalue - call mmrm API to get client values
|
||||
* mmrm_vm: driver private data
|
||||
* req: set client value request parameters
|
||||
*/
|
||||
static int mmrm_vm_be_client_getvalue(struct mmrm_vm_driver_data *mmrm_vm,
|
||||
struct mmrm_vm_api_request_msg *req)
|
||||
{
|
||||
struct mmrm_vm_response_msg_pkt pkt;
|
||||
int rc;
|
||||
struct mmrm_vm_getvalue_request *req_param = &req->data.getval;
|
||||
struct mmrm_client_res_value val;
|
||||
struct mmrm_client_res_value *p_val = &pkt.msg.data.getval.val;
|
||||
|
||||
rc = mmrm_client_get_value(mmrm_vm->clk_client_tbl[req_param->client_id], &val);
|
||||
|
||||
pkt.msg.hd.cmd_id = MMRM_VM_RESPONSE_GETVALUE;
|
||||
pkt.msg.hd.seq_no = req->hd.seq_no;
|
||||
pkt.hdr.size = sizeof(pkt.msg.hd) + sizeof(pkt.msg.data.getval);
|
||||
|
||||
p_val->cur = val.cur;
|
||||
p_val->max = val.max;
|
||||
p_val->min = val.min;
|
||||
|
||||
// pr_err("%s: cmd_id:%d data size:%d\n", __func__, pkt.msg.hd.cmd_id, pkt.hdr.size);
|
||||
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_client_deregister - call mmrm API to deregister client
|
||||
* mmrm_vm: driver private data
|
||||
* req: set client value request parameters
|
||||
*/
|
||||
static int mmrm_vm_be_client_deregister(struct mmrm_vm_driver_data *mmrm_vm,
|
||||
struct mmrm_vm_api_request_msg *req)
|
||||
{
|
||||
int rc;
|
||||
struct mmrm_vm_response_msg_pkt pkt;
|
||||
struct mmrm_vm_deregister_request *req_param = &req->data.dereg;
|
||||
|
||||
rc = mmrm_client_deregister(mmrm_vm->clk_client_tbl[req_param->client_id]);
|
||||
// pr_err("%s: client:%d\n", __func__, req_param->client_id);
|
||||
|
||||
pkt.msg.hd.cmd_id = MMRM_VM_RESPONSE_DEREGISTER;
|
||||
pkt.msg.hd.seq_no = req->hd.seq_no;
|
||||
|
||||
pkt.hdr.size = sizeof(pkt.msg.hd) + sizeof(pkt.msg.data.dereg);
|
||||
pkt.msg.data.dereg.ret_code = rc;
|
||||
|
||||
// pr_err("%s: cmd_id:%d data size:%d ret:%d\n", __func__,
|
||||
// pkt.msg.hd.cmd_id, pkt.hdr.size, pkt.msg.data.dereg.ret_code);
|
||||
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_client_noop - call none mmrm API to calculate msgq roundtrip time
|
||||
* mmrm_vm: driver private data
|
||||
* req: request parameters
|
||||
*/
|
||||
static int mmrm_vm_be_client_noop(struct mmrm_vm_driver_data *mmrm_vm,
|
||||
struct mmrm_vm_api_request_msg *req)
|
||||
{
|
||||
int rc = 0;
|
||||
struct mmrm_vm_response_msg_pkt pkt;
|
||||
|
||||
pkt.msg.hd.cmd_id = MMRM_VM_RESPONSE_NOOP;
|
||||
pkt.msg.hd.seq_no = req->hd.seq_no;
|
||||
|
||||
pkt.hdr.size = sizeof(pkt.msg.hd) + sizeof(pkt.msg.data.lptest);
|
||||
pkt.msg.data.dereg.ret_code = rc;
|
||||
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_recv - be dispatch mmrm request to mmrm API call
|
||||
* mmrm_vm: driver private data
|
||||
* data: request message buffer pointer
|
||||
* size: request message size
|
||||
*/
|
||||
int mmrm_vm_be_recv(struct mmrm_vm_driver_data *mmrm_vm, void *data, size_t size)
|
||||
{
|
||||
struct mmrm_vm_api_request_msg *cmd = data;
|
||||
int rc = -1;
|
||||
|
||||
switch (cmd->hd.cmd_id) {
|
||||
case MMRM_VM_REQUEST_REGISTER:
|
||||
rc = mmrm_vm_be_client_register(mmrm_vm, cmd);
|
||||
break;
|
||||
|
||||
case MMRM_VM_REQUEST_SETVALUE:
|
||||
rc = mmrm_vm_be_client_setvalue(mmrm_vm, cmd);
|
||||
break;
|
||||
|
||||
case MMRM_VM_REQUEST_SETVALUE_INRANGE:
|
||||
rc = mmrm_vm_be_client_setvalue_inrange(mmrm_vm, cmd);
|
||||
break;
|
||||
|
||||
case MMRM_VM_REQUEST_GETVALUE:
|
||||
rc = mmrm_vm_be_client_getvalue(mmrm_vm, cmd);
|
||||
break;
|
||||
|
||||
case MMRM_VM_REQUEST_DEREGISTER:
|
||||
rc = mmrm_vm_be_client_deregister(mmrm_vm, cmd);
|
||||
break;
|
||||
case MMRM_VM_REQUEST_NOOP:
|
||||
rc = mmrm_vm_be_client_noop(mmrm_vm, cmd);
|
||||
break;
|
||||
default:
|
||||
pr_err("%s: cmd_id:%d unknown!!!\n", __func__, cmd->hd.cmd_id);
|
||||
break;
|
||||
}
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_wrong_request - deal with SVM wrong request
|
||||
* mmrm_vm: driver private data
|
||||
*/
|
||||
int mmrm_vm_be_wrong_request(struct mmrm_vm_driver_data *mmrm_vm)
|
||||
{
|
||||
int rc = 0;
|
||||
struct mmrm_vm_response_msg_pkt pkt;
|
||||
|
||||
pkt.msg.hd.cmd_id = MMRM_VM_RESPONSE_INVALID_PKT;
|
||||
pkt.msg.hd.seq_no = 0;
|
||||
|
||||
pkt.hdr.size = sizeof(pkt.msg.hd) + sizeof(pkt.msg.data.dereg);
|
||||
pkt.msg.data.dereg.ret_code = rc;
|
||||
|
||||
d_mpr_e("%s: wrong request\n", __func__);
|
||||
rc = mmrm_vm_be_send_response(mmrm_vm, &pkt);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: rc:%d\n", __func__, rc);
|
||||
return rc;
|
||||
}
|
||||
112
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be_main.c
Normal file
112
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be_main.c
Normal file
@@ -0,0 +1,112 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
|
||||
*/
|
||||
|
||||
#include <linux/module.h>
|
||||
#include <linux/platform_device.h>
|
||||
#include <linux/of.h>
|
||||
#include <linux/slab.h>
|
||||
|
||||
#include "mmrm_vm_be.h"
|
||||
#include "mmrm_vm_msgq.h"
|
||||
#include "mmrm_vm_interface.h"
|
||||
#include "mmrm_debug.h"
|
||||
|
||||
#define MMRM_CLK_CLIENTS_NUM_MAX 35
|
||||
|
||||
struct mmrm_vm_driver_data *drv_vm_be = (void *) -EPROBE_DEFER;
|
||||
|
||||
int msm_mmrm_debug = MMRM_ERR | MMRM_WARN | MMRM_PRINTK;
|
||||
|
||||
int mmrm_client_get_clk_count(void);
|
||||
|
||||
static int mmrm_vm_be_driver_probe(struct platform_device *pdev)
|
||||
{
|
||||
struct device *dev = &pdev->dev;
|
||||
int sz, clk_count, rc;
|
||||
|
||||
drv_vm_be = devm_kzalloc(dev, sizeof(*drv_vm_be), GFP_KERNEL);
|
||||
if (!drv_vm_be)
|
||||
return -ENOMEM;
|
||||
|
||||
clk_count = mmrm_client_get_clk_count();
|
||||
if (clk_count <= 0 || clk_count > MMRM_CLK_CLIENTS_NUM_MAX) {
|
||||
d_mpr_e("%s: clk count is not correct\n", __func__);
|
||||
goto clk_count_err;
|
||||
}
|
||||
sz = sizeof(struct mmrm_client *) * clk_count;
|
||||
drv_vm_be->clk_client_tbl = devm_kzalloc(dev, sz, GFP_KERNEL);
|
||||
if (!drv_vm_be->clk_client_tbl)
|
||||
goto client_tbl_err;
|
||||
|
||||
drv_vm_be->debugfs_root = msm_mmrm_debugfs_init();
|
||||
if (!drv_vm_be->debugfs_root)
|
||||
d_mpr_e("%s: failed to create debugfs for mmrm\n", __func__);
|
||||
|
||||
dev_set_drvdata(&pdev->dev, drv_vm_be);
|
||||
rc = mmrm_vm_msgq_init(drv_vm_be);
|
||||
if (rc != 0) {
|
||||
d_mpr_e("%s: failed to init msgq\n", __func__);
|
||||
goto msgq_init_err;
|
||||
}
|
||||
|
||||
drv_vm_be->dev = dev;
|
||||
dev_err(dev, "msgq probe success");
|
||||
return 0;
|
||||
|
||||
msgq_init_err:
|
||||
dev_set_drvdata(&pdev->dev, NULL);
|
||||
msm_mmrm_debugfs_deinit(drv_vm_be->debugfs_root);
|
||||
return -EINVAL;
|
||||
client_tbl_err:
|
||||
dev_err(dev, "msgq register alloc memory failed");
|
||||
return -ENOMEM;
|
||||
clk_count_err:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
static int mmrm_vm_be_driver_remove(struct platform_device *pdev)
|
||||
{
|
||||
mmrm_vm_msgq_deinit(drv_vm_be);
|
||||
msm_mmrm_debugfs_deinit(drv_vm_be->debugfs_root);
|
||||
|
||||
dev_set_drvdata(&pdev->dev, NULL);
|
||||
drv_vm_be = (void *) -EPROBE_DEFER;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct of_device_id mmrm_vm_be_match[] = {
|
||||
{ .compatible = "qcom,mmrm-vm-be" },
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(of, mmrm_vm_be_match);
|
||||
|
||||
static struct platform_driver mmrm_vm_be_driver = {
|
||||
.probe = mmrm_vm_be_driver_probe,
|
||||
.driver = {
|
||||
.name = "mmrm-vm-be",
|
||||
.of_match_table = mmrm_vm_be_match,
|
||||
},
|
||||
.remove = mmrm_vm_be_driver_remove,
|
||||
};
|
||||
|
||||
static int __init mmrm_vm_be_module_init(void)
|
||||
{
|
||||
pr_info("%s: init start\n", __func__);
|
||||
|
||||
return platform_driver_register(&mmrm_vm_be_driver);
|
||||
}
|
||||
subsys_initcall(mmrm_vm_be_module_init);
|
||||
|
||||
static void __exit mmrm_vm_be_module_exit(void)
|
||||
{
|
||||
platform_driver_unregister(&mmrm_vm_be_driver);
|
||||
}
|
||||
module_exit(mmrm_vm_be_module_exit);
|
||||
|
||||
MODULE_SOFTDEP("pre: gunyah_transport");
|
||||
MODULE_SOFTDEP("pre: msm-mmrm");
|
||||
|
||||
MODULE_DESCRIPTION("Qualcomm Technologies, Inc. MMRM BE Driver");
|
||||
MODULE_LICENSE("GPL v2");
|
||||
317
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be_msgq.c
Normal file
317
qcom/opensource/mmrm-driver/vm/be/src/mmrm_vm_be_msgq.c
Normal file
@@ -0,0 +1,317 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
|
||||
*/
|
||||
|
||||
#include <linux/gunyah/gh_msgq.h>
|
||||
#include <linux/kthread.h>
|
||||
#include <linux/slab.h>
|
||||
#include <linux/notifier.h>
|
||||
#include <linux/gunyah/gh_rm_drv.h>
|
||||
#include <linux/workqueue.h>
|
||||
#include <linux/list.h>
|
||||
|
||||
#include "mmrm_vm_be.h"
|
||||
#include "mmrm_vm_interface.h"
|
||||
#include "mmrm_vm_debug.h"
|
||||
|
||||
#define MAX_ERR_COUNT 5
|
||||
|
||||
int mmrm_vm_be_wrong_request(struct mmrm_vm_driver_data *mmrm_vm);
|
||||
|
||||
static int is_valid_mmrm_message(struct mmrm_vm_request_msg_pkt *pkt)
|
||||
{
|
||||
int rc = -1;
|
||||
struct mmrm_vm_msg_hdr *hdr = &pkt->hdr;
|
||||
|
||||
if (hdr->version == MMRM_VM_VER_1 &&
|
||||
hdr->type == MMRM_VM_TYPE_DATA)
|
||||
rc = 0;
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_msgq_msg_handler - fe request handler
|
||||
* work: work parameter that workqueue thread transfer
|
||||
*/
|
||||
static void mmrm_vm_msgq_msg_handler(struct work_struct *work)
|
||||
{
|
||||
struct mmrm_vm_thread_info *pthread_info =
|
||||
container_of(work, struct mmrm_vm_thread_info, msgq_work.work);
|
||||
struct mmrm_vm_driver_data *mmrm_vm =
|
||||
container_of(pthread_info, struct mmrm_vm_driver_data, thread_info);
|
||||
struct mmrm_vm_msg *msg;
|
||||
struct mmrm_vm_msg *next_msg;
|
||||
struct list_head head;
|
||||
struct mmrm_vm_request_msg_pkt *pkt;
|
||||
|
||||
if (IS_ERR_OR_NULL(work))
|
||||
return;
|
||||
|
||||
mutex_lock(&pthread_info->list_lock);
|
||||
list_replace_init(&pthread_info->queued_msg, &head);
|
||||
mutex_unlock(&pthread_info->list_lock);
|
||||
|
||||
list_for_each_entry_safe(msg, next_msg, &head, link) {
|
||||
pkt = (struct mmrm_vm_request_msg_pkt *)msg->msg_buf;
|
||||
if (is_valid_mmrm_message(pkt) == 0)
|
||||
mmrm_vm_be_recv(mmrm_vm, &pkt->msg, pkt->hdr.size);
|
||||
else {
|
||||
mmrm_vm_be_wrong_request(mmrm_vm);
|
||||
d_mpr_e("%s: wrong mmrm message\n", __func__);
|
||||
}
|
||||
list_del(&msg->link);
|
||||
kfree(msg);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_msgq_listener - gunyah's message receiving thread
|
||||
* data: parameter that caller transfer
|
||||
*/
|
||||
static int mmrm_vm_be_msgq_listener(void *data)
|
||||
{
|
||||
struct mmrm_vm_driver_data *mmrm_vm;
|
||||
struct mmrm_vm_gh_msgq_info *pmsg_info;
|
||||
struct mmrm_vm_thread_info *thread_info;
|
||||
|
||||
struct mmrm_vm_msg *msg;
|
||||
size_t size;
|
||||
int ret = 0;
|
||||
int err_count = 0;
|
||||
|
||||
if (IS_ERR_OR_NULL(data))
|
||||
return -EINVAL;
|
||||
|
||||
mmrm_vm = (struct mmrm_vm_driver_data *)data;
|
||||
pmsg_info = &mmrm_vm->msg_info;
|
||||
thread_info = &mmrm_vm->thread_info;
|
||||
|
||||
while (true) {
|
||||
msg = kzalloc(sizeof(struct mmrm_vm_msg), GFP_KERNEL);
|
||||
if (!msg)
|
||||
return -ENOMEM;
|
||||
|
||||
ret = gh_msgq_recv(pmsg_info->msgq_handle, msg->msg_buf,
|
||||
GH_MSGQ_MAX_MSG_SIZE_BYTES, &size, 0);
|
||||
if (ret < 0) {
|
||||
kfree(msg);
|
||||
d_mpr_e("gh_msgq_recv failed, rc=%d\n", ret);
|
||||
err_count++;
|
||||
if (err_count < MAX_ERR_COUNT)
|
||||
continue;
|
||||
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
err_count = 0;
|
||||
msg->msg_size = size;
|
||||
|
||||
mutex_lock(&thread_info->list_lock);
|
||||
list_add_tail(&msg->link, &thread_info->queued_msg);
|
||||
mutex_unlock(&thread_info->list_lock);
|
||||
|
||||
queue_delayed_work(thread_info->msg_workq,
|
||||
&thread_info->msgq_work, msecs_to_jiffies(0));
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_msgq_send - send response message by gunyah API
|
||||
* mmrm_vm: driver data
|
||||
* msg: message buffer pointer
|
||||
* msg_size: message size
|
||||
*/
|
||||
int mmrm_vm_msgq_send(struct mmrm_vm_driver_data *mmrm_vm, void *msg, size_t msg_size)
|
||||
{
|
||||
if (!mmrm_vm->msg_info.msgq_handle) {
|
||||
d_mpr_e("Failed to send msg, invalid msgq handle\n");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (msg_size > GH_MSGQ_MAX_MSG_SIZE_BYTES) {
|
||||
d_mpr_e("msg size unsupported for msgq: %ld > %d\n", msg_size,
|
||||
GH_MSGQ_MAX_MSG_SIZE_BYTES);
|
||||
return -E2BIG;
|
||||
}
|
||||
|
||||
return gh_msgq_send(mmrm_vm->msg_info.msgq_handle, msg, msg_size, GH_MSGQ_TX_PUSH);
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_gh_validate_register - check gunyah connection validation
|
||||
* msg_info: gunyah meesage info
|
||||
* vm_status_payload: gunyah notification message status info
|
||||
*/
|
||||
int mmrm_vm_be_gh_validate_register(struct mmrm_vm_gh_msgq_info *msg_info,
|
||||
struct gh_rm_notif_vm_status_payload *vm_status_payload)
|
||||
{
|
||||
gh_vmid_t peer_vmid;
|
||||
gh_vmid_t self_vmid;
|
||||
int rc = -1;
|
||||
|
||||
if (vm_status_payload->vm_status != GH_RM_VM_STATUS_READY)
|
||||
return rc;
|
||||
|
||||
if (gh_rm_get_vmid(msg_info->peer_id, &peer_vmid))
|
||||
return rc;
|
||||
|
||||
if (gh_rm_get_vmid(GH_PRIMARY_VM, &self_vmid))
|
||||
return rc;
|
||||
|
||||
if (peer_vmid != vm_status_payload->vmid)
|
||||
return NOTIFY_DONE;
|
||||
|
||||
d_mpr_l("%s: vmid=%d peer_vmid=%d\n", __func__, vm_status_payload->vmid, peer_vmid);
|
||||
|
||||
if (msg_info->msgq_handle) {
|
||||
return rc;
|
||||
}
|
||||
|
||||
msg_info->msgq_handle = gh_msgq_register(msg_info->msgq_label);
|
||||
|
||||
rc = 0;
|
||||
|
||||
if (IS_ERR_OR_NULL(msg_info->msgq_handle)) {
|
||||
rc = -1;
|
||||
d_mpr_e("%s: gunyah message queue registration failed :%ld\n", __func__,
|
||||
PTR_ERR(msg_info->msgq_handle));
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_be_msgq_cb - check gunyah connection validation
|
||||
* nb: gunyah notofier block info
|
||||
* cmd: gunyah notification status category info
|
||||
* data: user defined data pointer
|
||||
*/
|
||||
static int mmrm_vm_be_msgq_cb(struct notifier_block *nb, unsigned long cmd, void *data)
|
||||
{
|
||||
struct gh_rm_notif_vm_status_payload *vm_status_payload;
|
||||
struct mmrm_vm_driver_data *mmrm_vm;
|
||||
struct mmrm_vm_gh_msgq_info *msg_info;
|
||||
struct mmrm_vm_thread_info *thread_info;
|
||||
int rc;
|
||||
|
||||
if (IS_ERR_OR_NULL(nb))
|
||||
return -EINVAL;
|
||||
|
||||
msg_info = container_of(nb, struct mmrm_vm_gh_msgq_info, pvt_nb);
|
||||
mmrm_vm = container_of(msg_info, struct mmrm_vm_driver_data, msg_info);
|
||||
|
||||
thread_info = &mmrm_vm->thread_info;
|
||||
if (cmd != GH_RM_NOTIF_VM_STATUS)
|
||||
return NOTIFY_DONE;
|
||||
|
||||
/*
|
||||
* check VM status, only GH_TRUSTED_VM notification activate
|
||||
* GUNYAH message queue registering
|
||||
*/
|
||||
vm_status_payload = (struct gh_rm_notif_vm_status_payload *)data;
|
||||
rc = mmrm_vm_be_gh_validate_register(msg_info, vm_status_payload);
|
||||
if (rc != 0) {
|
||||
return NOTIFY_DONE;
|
||||
}
|
||||
|
||||
d_mpr_e("%s: msgq registration successful\n", __func__);
|
||||
|
||||
thread_info->msgq_listener_thread = kthread_run(mmrm_vm_be_msgq_listener,
|
||||
(void *)mmrm_vm, "mmrm_msgq_listener");
|
||||
if (IS_ERR_OR_NULL(thread_info->msgq_listener_thread)) {
|
||||
return NOTIFY_DONE;
|
||||
};
|
||||
|
||||
wake_up_process(thread_info->msgq_listener_thread);
|
||||
|
||||
return NOTIFY_DONE;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_msgq_init - gunyah message queue initialization
|
||||
* mmrm_vm: driver data
|
||||
*/
|
||||
int mmrm_vm_msgq_init(struct mmrm_vm_driver_data *mmrm_vm)
|
||||
{
|
||||
struct mmrm_vm_gh_msgq_info *msg_info;
|
||||
struct mmrm_vm_thread_info *thread_info;
|
||||
int rc = -1;
|
||||
|
||||
if (IS_ERR_OR_NULL(mmrm_vm)) {
|
||||
rc = -EINVAL;
|
||||
d_mpr_e("%s: driver init wrong\n", __func__);
|
||||
goto err;
|
||||
}
|
||||
msg_info = &mmrm_vm->msg_info;
|
||||
thread_info = &mmrm_vm->thread_info;
|
||||
|
||||
msg_info->msgq_label = GH_MSGQ_LABEL_MMRM;
|
||||
d_mpr_l("%s: msgq-label=%d\n", __func__, msg_info->msgq_label);
|
||||
|
||||
msg_info->peer_id = GH_TRUSTED_VM;
|
||||
msg_info->pvt_nb.notifier_call = mmrm_vm_be_msgq_cb;
|
||||
rc = gh_rm_register_notifier(&msg_info->pvt_nb);
|
||||
if (rc != 0) {
|
||||
d_mpr_e("%s: gunyah register notifier failed\n", __func__);
|
||||
goto err;
|
||||
}
|
||||
msg_info->status |= MMRM_VM_MSG_STATUS_NOTIFIER;
|
||||
mutex_init(&thread_info->list_lock);
|
||||
INIT_LIST_HEAD(&thread_info->queued_msg);
|
||||
thread_info->msg_workq = create_singlethread_workqueue("vm_message_workq");
|
||||
if (IS_ERR_OR_NULL(thread_info->msg_workq)) {
|
||||
d_mpr_e("%s: create workqueue thread failed\n", __func__);
|
||||
goto err_workqueue;
|
||||
};
|
||||
INIT_DELAYED_WORK(&thread_info->msgq_work, mmrm_vm_msgq_msg_handler);
|
||||
|
||||
return 0;
|
||||
|
||||
err_workqueue:
|
||||
gh_rm_unregister_notifier(&msg_info->pvt_nb);
|
||||
msg_info->status &= ~MMRM_VM_MSG_STATUS_NOTIFIER;
|
||||
|
||||
err:
|
||||
return rc;
|
||||
}
|
||||
|
||||
/**
|
||||
* mmrm_vm_msgq_init - gunyah message queue de-initialization
|
||||
* mmrm_vm: driver data
|
||||
*/
|
||||
int mmrm_vm_msgq_deinit(struct mmrm_vm_driver_data *mmrm_vm)
|
||||
{
|
||||
struct mmrm_vm_gh_msgq_info *msg_info;
|
||||
struct mmrm_vm_thread_info *thread_info;
|
||||
int rc = 0;
|
||||
|
||||
if (IS_ERR_OR_NULL(mmrm_vm))
|
||||
return -EINVAL;
|
||||
|
||||
msg_info = &mmrm_vm->msg_info;
|
||||
thread_info = &mmrm_vm->thread_info;
|
||||
if (thread_info->msgq_listener_thread) {
|
||||
kthread_stop(thread_info->msgq_listener_thread);
|
||||
thread_info->msgq_listener_thread = NULL;
|
||||
}
|
||||
|
||||
if (msg_info->status & MMRM_VM_MSG_STATUS_NOTIFIER)
|
||||
gh_rm_unregister_notifier(&msg_info->pvt_nb);
|
||||
|
||||
if (msg_info->msgq_handle) {
|
||||
rc = gh_msgq_unregister(msg_info->msgq_handle);
|
||||
if (rc != 0)
|
||||
d_mpr_e("%s: msgq gunyah unregistration failed: err:%d\n", __func__, rc);
|
||||
msg_info->msgq_handle = NULL;
|
||||
}
|
||||
|
||||
if (thread_info->msg_workq) {
|
||||
destroy_workqueue(thread_info->msg_workq);
|
||||
thread_info->msg_workq = NULL;
|
||||
}
|
||||
return rc;
|
||||
}
|
||||
Reference in New Issue
Block a user