media: video: tegra: avp: get module clock freq

AVP wants to know the clock freq of the modules like
VDE and AVP itself.
Added interface to handle the get clock message from AVP.

Bug 843882

Signed-off-by: Sachin Nikam <snikam@nvidia.com>
Reviewed-on: http://git-master/r/38506
(cherry picked from commit 7ab89380014b5b5f368548615f5ab2e144b3e57a)

Change-Id: I83e06877f36ed2901e7bd481915d3b38929cd52a
Reviewed-on: http://git-master/r/40597
Reviewed-by: Bharat Nihalani <bnihalani@nvidia.com>
Tested-by: Bharat Nihalani <bnihalani@nvidia.com>

2 files changed, 41 insertions, 0 deletions

diff --git a/drivers/media/video/tegra/avp/avp_msg.h b/drivers/media/video/tegra/avp/avp_msg.h
index c6685a235f5a..615d890d5444 100644
--- a/drivers/media/video/tegra/avp/avp_msg.h
+++ b/drivers/media/video/tegra/avp/avp_msg.h
@@ -71,6 +71,8 @@ enum {
 	SVC_DFS_GET_CLK_UTIL_RESPONSE	= 36,
 	SVC_MODULE_CLOCK_SET		= 37,
 	SVC_MODULE_CLOCK_SET_RESPONSE	= 38,
+	SVC_MODULE_CLOCK_GET		= 39,
+	SVC_MODULE_CLOCK_GET_RESPONSE	= 40,
 };
 
 struct svc_msg {
diff --git a/drivers/media/video/tegra/avp/avp_svc.c b/drivers/media/video/tegra/avp/avp_svc.c
index 62cf76bb0ccb..b357c64a30e9 100644
--- a/drivers/media/video/tegra/avp/avp_svc.c
+++ b/drivers/media/video/tegra/avp/avp_svc.c
@@ -48,6 +48,7 @@ enum {
 	CLK_REQUEST_VCP		= 0,
 	CLK_REQUEST_BSEA	= 1,
 	CLK_REQUEST_VDE		= 2,
+	CLK_REQUEST_AVP		= 3,
 	NUM_CLK_REQUESTS,
 };
 
@@ -57,6 +58,10 @@ struct avp_module {
 };
 
 static struct avp_module avp_modules[] = {
+	[AVP_MODULE_ID_AVP] = {
+		.name		= "cop",
+		.clk_req	= CLK_REQUEST_AVP,
+	},
 	[AVP_MODULE_ID_VCP] = {
 		.name		= "vcp",
 		.clk_req	= CLK_REQUEST_VCP,
@@ -487,6 +492,36 @@ static void do_svc_unsupported_msg(struct avp_svc_info *avp_svc,
 			sizeof(resp), GFP_KERNEL);
 }
 
+static void do_svc_module_clock_get(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_clock_ctrl *msg = (struct svc_clock_ctrl *)_msg;
+	struct svc_clock_ctrl_response resp;
+	struct avp_module *mod;
+	struct avp_clk *aclk;
+	int ret = 0;
+
+	mod = find_avp_module(avp_svc, msg->module_id);
+	if (!mod) {
+		pr_err("avp_svc: unknown module get clock requested: %d\n",
+		       msg->module_id);
+		resp.err = AVP_ERR_EINVAL;
+		goto send_response;
+	}
+
+	mutex_lock(&avp_svc->clk_lock);
+	aclk = &avp_svc->clks[mod->clk_req];
+	resp.act_freq = clk_get_rate(aclk->clk);
+	mutex_unlock(&avp_svc->clk_lock);
+	resp.err = 0;
+
+send_response:
+	resp.svc_id = SVC_MODULE_CLOCK_GET_RESPONSE;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+			sizeof(resp), GFP_KERNEL);
+}
+
 static int dispatch_svc_message(struct avp_svc_info *avp_svc,
 				struct svc_msg *msg,
 				size_t len)
@@ -573,6 +608,10 @@ static int dispatch_svc_message(struct avp_svc_info *avp_svc,
 		DBG(AVP_DBG_TRACE_SVC, "%s: got module_clock_set\n", __func__);
 		do_svc_module_clock_set(avp_svc, msg, len);
 		break;
+	case SVC_MODULE_CLOCK_GET:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got module_clock_get\n", __func__);
+		do_svc_module_clock_get(avp_svc, msg, len);
+		break;
 	default:
 		pr_warning("avp_svc: Unsupported SVC call 0x%x\n", msg->svc_id);
 		do_svc_unsupported_msg(avp_svc, msg->svc_id);