msm-mmrm: fixed para virtualization backend

fixed para virtualization backend.

Change-Id: Id7580187c444f5387ed7aa38e8ed441a02d4b2c6

config/waipiommrmconf.h

@@ -4,3 +4,6 @@
  */
 
 #define CONFIG_MSM_MMRM 1
+
+// if activate mmrm para virtualizaion, uncomment line below
+//#define CONFIG_MSM_MMRM_VM 1

vm/be/Kbuild

@@ -5,5 +5,5 @@ 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 \
-		../cmn/src/mmrm_vm_debug.o
+		../common/src/mmrm_vm_debug.o
 endif

vm/be/src/mmrm_vm_be.rc

@@ -1,7 +1,7 @@
-// SPDX-License-Identifier: GPL-2.0-only
-/*
- * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
- */
+# 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

vm/be/src/mmrm_vm_be_dispatch.c

@@ -2,7 +2,6 @@
 /*
  * 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"
@@ -62,8 +61,8 @@ static int mmrm_vm_be_client_register(struct mmrm_vm_driver_data *mmrm_vm,
 		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 = 0;
-		d_mpr_e("%s: client:%p\n", __func__, pClient);
+		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
@@ -96,6 +95,9 @@ static int mmrm_vm_be_client_setvalue(struct mmrm_vm_driver_data *mmrm_vm,
 	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;
@@ -242,3 +244,24 @@ int mmrm_vm_be_recv(struct mmrm_vm_driver_data *mmrm_vm, void *data, size_t size
 	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;
+}

vm/be/src/mmrm_vm_be_main.c

@@ -51,20 +51,18 @@ static int mmrm_vm_be_driver_probe(struct platform_device *pdev)
 		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;
-msgq_init_err:
-	kfree(drv_vm_be->clk_client_tbl);
-	msm_mmrm_debugfs_deinit(drv_vm_be->debugfs_root);
-	mmrm_vm_msgq_deinit(drv_vm_be);
-	dev_set_drvdata(&pdev->dev, NULL);
 clk_count_err:
-	kfree(drv_vm_be);
-	drv_vm_be = (void *) -EPROBE_DEFER;
 	return -EINVAL;
 }
 
@@ -74,7 +72,6 @@ static int mmrm_vm_be_driver_remove(struct platform_device *pdev)
 	msm_mmrm_debugfs_deinit(drv_vm_be->debugfs_root);
 
 	dev_set_drvdata(&pdev->dev, NULL);
-	kfree(drv_vm_be);
 	drv_vm_be = (void *) -EPROBE_DEFER;
 	return 0;
 }

vm/be/src/mmrm_vm_be_msgq.c

@@ -17,6 +17,20 @@
 
 #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
@@ -30,6 +44,7 @@ static void mmrm_vm_msgq_msg_handler(struct work_struct *work)
 	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;
@@ -39,7 +54,13 @@ static void mmrm_vm_msgq_msg_handler(struct work_struct *work)
 	mutex_unlock(&pthread_info->list_lock);
 
 	list_for_each_entry_safe(msg, next_msg, &head, link) {
-		mmrm_vm_be_recv(mmrm_vm, msg->msg_buf, msg->msg_size);
+		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);
 	}
@@ -88,7 +109,7 @@ static int mmrm_vm_be_msgq_listener(void *data)
 		msg->msg_size = size;
 
 		mutex_lock(&thread_info->list_lock);
-		list_add_tail(&thread_info->queued_msg, &msg->link);
+		list_add_tail(&msg->link, &thread_info->queued_msg);
 		mutex_unlock(&thread_info->list_lock);
 
 		queue_delayed_work(thread_info->msg_workq,
@@ -135,7 +156,6 @@ int mmrm_vm_be_gh_validate_register(struct mmrm_vm_gh_msgq_info *msg_info,
 	if (vm_status_payload->vm_status != GH_RM_VM_STATUS_READY)
 		return rc;
 
-	d_mpr_l("%s: status=%d\n", __func__, vm_status_payload->vm_status);
 	if (gh_rm_get_vmid(msg_info->peer_id, &peer_vmid))
 		return rc;
 
@@ -153,12 +173,12 @@ int mmrm_vm_be_gh_validate_register(struct mmrm_vm_gh_msgq_info *msg_info,
 
 	msg_info->msgq_handle = gh_msgq_register(msg_info->msgq_label);
 
-	d_mpr_l("%s: label=%d\n", __func__, 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\n", __func__);
+		d_mpr_e("%s: gunyah message queue registration failed :%ld\n", __func__,
+			PTR_ERR(msg_info->msgq_handle));
 	}
 
 	return rc;
@@ -194,9 +214,11 @@ static int mmrm_vm_be_msgq_cb(struct notifier_block *nb, unsigned long cmd, void
 	 */
 	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)
-		d_mpr_e("%s: msgq registration failed: err:%d\n",
-			__func__, PTR_ERR(msg_info->msgq_handle));
+	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");
@@ -217,11 +239,13 @@ 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;
-
-	if (IS_ERR_OR_NULL(mmrm_vm))
-		return -EINVAL;
+	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;
 
@@ -231,18 +255,28 @@ int mmrm_vm_msgq_init(struct mmrm_vm_driver_data *mmrm_vm)
 	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)
-		return -1;
-
+	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)) {
-		return -1;
+		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;
 }
 
 /**
@@ -260,15 +294,24 @@ int mmrm_vm_msgq_deinit(struct mmrm_vm_driver_data *mmrm_vm)
 
 	msg_info = &mmrm_vm->msg_info;
 	thread_info = &mmrm_vm->thread_info;
-	if (thread_info->msgq_listener_thread)
+	if (thread_info->msgq_listener_thread) {
 		kthread_stop(thread_info->msgq_listener_thread);
+		thread_info->msgq_listener_thread = NULL;
+	}
 
-	gh_rm_unregister_notifier(&msg_info->pvt_nb);
+	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 unregistration failed: err:%d\n", __func__, rc);
+			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;
 }

vm/common/inc/mmrm_vm_interface.h

@@ -27,8 +27,9 @@ struct mmrm_vm_thread_info {
 	struct list_head   queued_msg;
 };
 
-/*
+/**
  * struct mmrm_vm_data_priv -- device driver private part
+ * @dev: device pointer
  * @msg_info: gunyah message info
  * @thread_info: message lister & workqueue info
  * @clk_client_tbl: index and client handler LUT
@@ -36,6 +37,7 @@ struct mmrm_vm_thread_info {
  * @vm_pvt_data: pointer to fe/be specific data
  */
 struct mmrm_vm_driver_data {
+	struct device *dev;
 	struct mmrm_vm_gh_msgq_info msg_info;
 	struct mmrm_vm_thread_info thread_info;
 	struct mmrm_client **clk_client_tbl;
@@ -45,7 +47,7 @@ struct mmrm_vm_driver_data {
 	void *vm_pvt_data;
 };
 
-/*
+/**
  * enum mmrm_vm_api_msg_id -- request/response cmd ID
  */
 enum mmrm_vm_api_msg_id {
@@ -60,9 +62,10 @@ enum mmrm_vm_api_msg_id {
 	MMRM_VM_RESPONSE_SETVALUE_INRANGE,
 	MMRM_VM_RESPONSE_GETVALUE,
 	MMRM_VM_RESPONSE_DEREGISTER,
+	MMRM_VM_RESPONSE_INVALID_PKT,
 };
 
-/*
+/**
  * struct msg_head -- message head
  * @cmd_id: mmrm API message cmd id
  * @seq_no: message sequence id
@@ -72,7 +75,7 @@ struct mmrm_vm_api_msg_head {
 	int  seq_no;
 };
 
-/*
+/**
  * struct register_request -- mmrm register parameters
  * @client_type: client type, definition see msm_mmrm.h
  * @priority: client priority, definition see msm_mmrm.h
@@ -84,7 +87,7 @@ struct mmrm_vm_register_request {
 	struct mmrm_clk_client_desc desc;
 };
 
-/*
+/**
  * struct deregister_request -- mmrm deregister parameters
  * @client: client registered handle
  */
@@ -92,7 +95,7 @@ struct mmrm_vm_deregister_request {
 	u32 client_id;
 };
 
-/*
+/**
  * struct setvalue_request -- mmrm setvalue parameters
  * @client: client type, definition see msm_mmrm.h
  * @data: client info, definition see msm_mmrm.h
@@ -104,7 +107,7 @@ struct mmrm_vm_setvalue_request {
 	unsigned long val;
 };
 
-/*
+/**
  * struct mmrm_vm_setvalue_inrange_request -- mmrm setvalue_inrange parameters
  * @client: client type, definition see msm_mmrm.h
  * @data: client info, definition see msm_mmrm.h
@@ -116,7 +119,7 @@ struct mmrm_vm_setvalue_inrange_request {
 	struct mmrm_client_res_value val;
 };
 
-/*
+/**
  * struct mmrm_vm_getvalue_request -- mmrm getvalue parameters
  * @client: client type, definition see msm_mmrm.h
  * @val: current clock rate value range, definition see msm_mmrm.h
@@ -125,7 +128,7 @@ struct mmrm_vm_getvalue_request {
 	u32 client_id;
 };
 
-/*
+/**
  * struct mmrm_vm_api_request_msg -- mmrm request API message unified data definition
  * @hd: mmrm API request message head
  * @data: parameters mmrm API needs per API message cmd id
@@ -141,7 +144,7 @@ struct mmrm_vm_api_request_msg {
 	} data;
 };
 
-/*
+/**
  * struct mmrm_vm_register_response -- mmrm_client_register API response message
  * @client: handle for registered client
  */
@@ -149,7 +152,7 @@ struct mmrm_vm_register_response {
 	u32 client_id;
 };
 
-/*
+/**
  * struct mmrm_vm_deregister_response -- mmrm_client_deregister API response message
  * @ret_code: indicates if the mmrm_client_deregister is successful
  */
@@ -157,7 +160,7 @@ struct mmrm_vm_deregister_response {
 	int ret_code;
 };
 
-/*
+/**
  * struct mmrm_vm_setvalue_response -- mmrm_client_set_value API response message
  * @val: value that mmrm_client_set_value return
  */
@@ -165,7 +168,7 @@ struct mmrm_vm_setvalue_response {
 	unsigned long val;
 };
 
-/*
+/**
  * struct mmrm_vm_setvalue_inrange_response -- mmrm_client_set_value_in_range API response message
  * @ret_code: value that mmrm_client_set_value_in_range return
  */
@@ -173,7 +176,7 @@ struct mmrm_vm_setvalue_inrange_response {
 	int ret_code;
 };
 
-/*
+/**
  * struct mmrm_vm_getvalue_response -- mmrm_client_get_value API response message
  * @val: value that mmrm_client_get_value return
  */
@@ -181,7 +184,7 @@ struct mmrm_vm_getvalue_response {
 	struct mmrm_client_res_value val;
 };
 
-/*
+/**
  * struct mmrm_vm_api_response_msg -- mmrm response message unified data
  * @hd: mmrm API response message head
  * @data: data that mmrm API return per API response message id
@@ -197,7 +200,7 @@ struct mmrm_vm_api_response_msg {
 	} data;
 };
 
-/*
+/**
  * struct mmrm_vm_request_msg_pkt -- mmrm request packet that is sent through gunyah API
  * @hdr: message head for checking message valid
  * @msg: data that is needed by mmrm API
@@ -207,7 +210,7 @@ struct mmrm_vm_request_msg_pkt {
 	struct mmrm_vm_api_request_msg msg;
 };
 
-/*
+/**
  * struct mmrm_vm_response_msg_pkt -- mmrm response packet that is sent through gunyah API
  * @hdr: message head for checking message valid
  * @msg: data that is returned by mmrm API

vm/common/inc/mmrm_vm_msgq.h

@@ -12,7 +12,10 @@
 
 #define MMRM_VM_MAX_PKT_SZ  1024      // mmrm max gunyah packet size
 
-/* mmrm_vm_pkt_type: mmrm transfer type, for message valid check
+#define MMRM_VM_MSG_STATUS_NOTIFIER   0x01
+
+/**
+ * mmrm_vm_pkt_type: mmrm transfer type, for message valid check
  * @MMRM_VM_TYPE_DATA: request/response data
  */
 enum mmrm_vm_pkt_type {
@@ -45,7 +48,7 @@ struct mmrm_vm_msg_hdr {
 struct mmrm_vm_msg {
 	struct list_head link;
 	size_t msg_size;
-	unsigned char msg_buf[GH_MSGQ_MAX_MSG_SIZE_BYTES];
+	u8 msg_buf[GH_MSGQ_MAX_MSG_SIZE_BYTES];
 };
 
 /**
@@ -53,12 +56,14 @@ struct mmrm_vm_msg {
  * @peer_id: notification callback check if message is from SVM
  * @msgq_handle - registered msg queue handle with gunyah api
  * @msgq_label - message queue label
+ * @status: indicate init status
  * @pvt_nb - notifier info
  */
 struct mmrm_vm_gh_msgq_info {
 	int  peer_id;
 	void *msgq_handle;
 	int  msgq_label;
+	int status;
 	struct notifier_block pvt_nb;
 };