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 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)

Reviewed-on: http://git-master/r/40597
(cherry picked from commit ea44e0325596c4e78fbd2bcf67648fb524b0c7bc)

Change-Id: I5167f3c3efa80cc581bddd7e7e8826da198b7266
Signed-off-by: Sanjay Singh Rawat <srawat@nvidia.com>
Reviewed-on: http://git-master/r/42797
Reviewed-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 249b7daf008d..1edcec32c9c1 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);