From 7a46088760f8891e6790a98b82efc1eb386c0d80 Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.com>
Date: Wed, 15 Sep 2021 17:54:11 +0100
Subject: [PATCH] staging/mmal-vchiq: Add module parameter to enable
 logging.

Adds a module parameter "debug" to enable various logging levels.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
---
 .../vc04_services/vchiq-mmal/mmal-vchiq.c     | 156 ++++++++++--------
 1 file changed, 85 insertions(+), 71 deletions(-)

--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c
@@ -31,6 +31,17 @@
 #include "mmal-msg.h"
 
 #include "vc-sm-cma/vc_sm_knl.h"
+
+#define pr_dbg_lvl(__level, __debug, __fmt, __arg...)		\
+	do {							\
+		if (__debug >= (__level))			\
+			printk(KERN_DEBUG __fmt, ##__arg);	\
+	} while (0)
+
+static unsigned int debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, "activates debug info (0-3)");
+
 /*
  * maximum number of components supported.
  * This matches the maximum permitted by default on the VPU
@@ -381,7 +392,8 @@ buffer_from_host(struct vchiq_mmal_insta
 	if (!port->enabled)
 		return -EINVAL;
 
-	pr_debug("instance:%u buffer:%p\n", instance->service_handle, buf);
+	pr_dbg_lvl(3, debug, "instance:%u buffer:%p\n",
+		   instance->service_handle, buf);
 
 	/* get context */
 	if (!buf->msg_context) {
@@ -549,11 +561,11 @@ static void event_to_host_cb(struct vchi
 		msg_context->u.bulk.pts = MMAL_TIME_UNKNOWN;
 		msg_context->u.bulk.cmd = msg->u.event_to_host.cmd;
 
-		pr_debug("event component:%u port type:%d num:%d cmd:0x%x length:%d\n",
-			 msg->u.event_to_host.client_component,
-			 msg->u.event_to_host.port_type,
-			 msg->u.event_to_host.port_num,
-			 msg->u.event_to_host.cmd, msg->u.event_to_host.length);
+		pr_dbg_lvl(3, debug, "event component:%u port type:%d num:%d cmd:0x%x length:%d\n",
+			   msg->u.event_to_host.client_component,
+			   msg->u.event_to_host.port_type,
+			   msg->u.event_to_host.port_num,
+			   msg->u.event_to_host.cmd, msg->u.event_to_host.length);
 	}
 
 	schedule_work(&msg_context->u.bulk.work);
@@ -566,8 +578,8 @@ static void buffer_to_host_cb(struct vch
 	struct mmal_msg_context *msg_context;
 	u32 handle;
 
-	pr_debug("%s: instance:%p msg:%p msg_len:%d\n",
-		 __func__, instance, msg, msg_len);
+	pr_dbg_lvl(3, debug, "%s: instance:%p msg:%p msg_len:%d\n",
+		   __func__, instance, msg, msg_len);
 
 	if (msg->u.buffer_from_host.drvbuf.magic == MMAL_MAGIC) {
 		handle = msg->u.buffer_from_host.drvbuf.client_context;
@@ -837,42 +849,43 @@ static int send_synchronous_mmal_msg(str
 
 static void dump_port_info(struct vchiq_mmal_port *port)
 {
-	pr_debug("port handle:0x%x enabled:%d\n", port->handle, port->enabled);
+	pr_dbg_lvl(3, debug, "port handle:0x%x enabled:%d\n", port->handle,
+		   port->enabled);
 
-	pr_debug("buffer minimum num:%d size:%d align:%d\n",
-		 port->minimum_buffer.num,
-		 port->minimum_buffer.size, port->minimum_buffer.alignment);
-
-	pr_debug("buffer recommended num:%d size:%d align:%d\n",
-		 port->recommended_buffer.num,
-		 port->recommended_buffer.size,
-		 port->recommended_buffer.alignment);
-
-	pr_debug("buffer current values num:%d size:%d align:%d\n",
-		 port->current_buffer.num,
-		 port->current_buffer.size, port->current_buffer.alignment);
-
-	pr_debug("elementary stream: type:%d encoding:0x%x variant:0x%x\n",
-		 port->format.type,
-		 port->format.encoding, port->format.encoding_variant);
+	pr_dbg_lvl(3, debug, "buffer minimum num:%d size:%d align:%d\n",
+		   port->minimum_buffer.num,
+		   port->minimum_buffer.size, port->minimum_buffer.alignment);
+
+	pr_dbg_lvl(3, debug, "buffer recommended num:%d size:%d align:%d\n",
+		   port->recommended_buffer.num,
+		   port->recommended_buffer.size,
+		   port->recommended_buffer.alignment);
+
+	pr_dbg_lvl(3, debug, "buffer current values num:%d size:%d align:%d\n",
+		   port->current_buffer.num,
+		   port->current_buffer.size, port->current_buffer.alignment);
+
+	pr_dbg_lvl(3, debug, "elementary stream: type:%d encoding:0x%x variant:0x%x\n",
+		   port->format.type,
+		   port->format.encoding, port->format.encoding_variant);
 
-	pr_debug("		    bitrate:%d flags:0x%x\n",
-		 port->format.bitrate, port->format.flags);
+	pr_dbg_lvl(3, debug, "		    bitrate:%d flags:0x%x\n",
+		   port->format.bitrate, port->format.flags);
 
 	if (port->format.type == MMAL_ES_TYPE_VIDEO) {
-		pr_debug
-		    ("es video format: width:%d height:%d colourspace:0x%x\n",
-		     port->es.video.width, port->es.video.height,
-		     port->es.video.color_space);
-
-		pr_debug("		 : crop xywh %d,%d,%d,%d\n",
-			 port->es.video.crop.x,
-			 port->es.video.crop.y,
-			 port->es.video.crop.width, port->es.video.crop.height);
-		pr_debug("		 : framerate %d/%d  aspect %d/%d\n",
-			 port->es.video.frame_rate.num,
-			 port->es.video.frame_rate.den,
-			 port->es.video.par.num, port->es.video.par.den);
+		pr_dbg_lvl(3, debug,
+			   "es video format: width:%d height:%d colourspace:0x%x\n",
+			   port->es.video.width, port->es.video.height,
+			   port->es.video.color_space);
+
+		pr_dbg_lvl(3, debug, "		 : crop xywh %d,%d,%d,%d\n",
+			   port->es.video.crop.x,
+			   port->es.video.crop.y,
+			   port->es.video.crop.width, port->es.video.crop.height);
+		pr_dbg_lvl(3, debug, "		 : framerate %d/%d  aspect %d/%d\n",
+			   port->es.video.frame_rate.num,
+			   port->es.video.frame_rate.den,
+			   port->es.video.par.num, port->es.video.par.den);
 	}
 }
 
@@ -903,7 +916,7 @@ static int port_info_set(struct vchiq_mm
 	struct mmal_msg *rmsg;
 	struct vchiq_header *rmsg_handle;
 
-	pr_debug("setting port info port %p\n", port);
+	pr_dbg_lvl(1, debug, "setting port info port %p\n", port);
 	if (!port)
 		return -1;
 	dump_port_info(port);
@@ -946,8 +959,8 @@ static int port_info_set(struct vchiq_mm
 	/* return operation status */
 	ret = -rmsg->u.port_info_get_reply.status;
 
-	pr_debug("%s:result:%d component:0x%x port:%d\n", __func__, ret,
-		 port->component->handle, port->handle);
+	pr_dbg_lvl(1, debug, "%s:result:%d component:0x%x port:%d\n", __func__,
+		   ret, port->component->handle, port->handle);
 
 release_msg:
 	vchiq_release_message(instance->service_handle, rmsg_handle);
@@ -1037,13 +1050,13 @@ static int port_info_get(struct vchiq_mm
 	       rmsg->u.port_info_get_reply.extradata,
 	       port->format.extradata_size);
 
-	pr_debug("received port info\n");
+	pr_dbg_lvl(1, debug, "received port info\n");
 	dump_port_info(port);
 
 release_msg:
 
-	pr_debug("%s:result:%d component:0x%x port:%d\n",
-		 __func__, ret, port->component->handle, port->handle);
+	pr_dbg_lvl(1, debug, "%s:result:%d component:0x%x port:%d\n",
+		   __func__, ret, port->component->handle, port->handle);
 
 	vchiq_release_message(instance->service_handle, rmsg_handle);
 
@@ -1089,9 +1102,9 @@ static int create_component(struct vchiq
 	component->outputs = rmsg->u.component_create_reply.output_num;
 	component->clocks = rmsg->u.component_create_reply.clock_num;
 
-	pr_debug("Component handle:0x%x in:%d out:%d clock:%d\n",
-		 component->handle,
-		 component->inputs, component->outputs, component->clocks);
+	pr_dbg_lvl(2, debug, "Component handle:0x%x in:%d out:%d clock:%d\n",
+		   component->handle,
+		   component->inputs, component->outputs, component->clocks);
 
 release_msg:
 	vchiq_release_message(instance->service_handle, rmsg_handle);
@@ -1260,10 +1273,9 @@ static int port_action_port(struct vchiq
 
 	ret = -rmsg->u.port_action_reply.status;
 
-	pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d)\n",
-		 __func__,
-		 ret, port->component->handle, port->handle,
-		 port_action_type_names[action_type], action_type);
+	pr_dbg_lvl(2, debug, "%s:result:%d component:0x%x port:%d action:%s(%d)\n",
+		   __func__, ret, port->component->handle, port->handle,
+		   port_action_type_names[action_type], action_type);
 
 release_msg:
 	vchiq_release_message(instance->service_handle, rmsg_handle);
@@ -1307,11 +1319,11 @@ static int port_action_handle(struct vch
 
 	ret = -rmsg->u.port_action_reply.status;
 
-	pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d) connect component:0x%x connect port:%d\n",
-		 __func__,
-		 ret, port->component->handle, port->handle,
-		 port_action_type_names[action_type],
-		 action_type, connect_component_handle, connect_port_handle);
+	pr_dbg_lvl(2, debug,
+		   "%s:result:%d component:0x%x port:%d action:%s(%d) connect component:0x%x connect port:%d\n",
+		   __func__, ret, port->component->handle, port->handle,
+		   port_action_type_names[action_type],
+		   action_type, connect_component_handle, connect_port_handle);
 
 release_msg:
 	vchiq_release_message(instance->service_handle, rmsg_handle);
@@ -1350,9 +1362,9 @@ static int port_parameter_set(struct vch
 
 	ret = -rmsg->u.port_parameter_set_reply.status;
 
-	pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n",
-		 __func__,
-		 ret, port->component->handle, port->handle, parameter_id);
+	pr_dbg_lvl(1, debug, "%s:result:%d component:0x%x port:%d parameter:%d\n",
+		   __func__, ret, port->component->handle, port->handle,
+		   parameter_id);
 
 release_msg:
 	vchiq_release_message(instance->service_handle, rmsg_handle);
@@ -1410,8 +1422,9 @@ static int port_parameter_get(struct vch
 	/* Always report the size of the returned parameter to the caller */
 	*value_size = rmsg->u.port_parameter_get_reply.size;
 
-	pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n", __func__,
-		 ret, port->component->handle, port->handle, parameter_id);
+	pr_dbg_lvl(1, debug, "%s:result:%d component:0x%x port:%d parameter:%d\n",
+		   __func__, ret, port->component->handle, port->handle,
+		   parameter_id);
 
 release_msg:
 	vchiq_release_message(instance->service_handle, rmsg_handle);
@@ -1668,7 +1681,7 @@ int vchiq_mmal_port_connect_tunnel(struc
 	if (!dst) {
 		/* do not make new connection */
 		ret = 0;
-		pr_debug("not making new connection\n");
+		pr_dbg_lvl(3, debug, "not making new connection\n");
 		goto release_unlock;
 	}
 
@@ -1686,14 +1699,14 @@ int vchiq_mmal_port_connect_tunnel(struc
 	/* set new format */
 	ret = port_info_set(instance, dst);
 	if (ret) {
-		pr_debug("setting port info failed\n");
+		pr_dbg_lvl(1, debug, "setting port info failed\n");
 		goto release_unlock;
 	}
 
 	/* read what has actually been set */
 	ret = port_info_get(instance, dst);
 	if (ret) {
-		pr_debug("read back port info failed\n");
+		pr_dbg_lvl(1, debug, "read back port info failed\n");
 		goto release_unlock;
 	}
 
@@ -1702,9 +1715,9 @@ int vchiq_mmal_port_connect_tunnel(struc
 				 MMAL_MSG_PORT_ACTION_TYPE_CONNECT,
 				 dst->component->handle, dst->handle);
 	if (ret < 0) {
-		pr_debug("connecting port %d:%d to %d:%d failed\n",
-			 src->component->handle, src->handle,
-			 dst->component->handle, dst->handle);
+		pr_dbg_lvl(2, debug, "connecting port %d:%d to %d:%d failed\n",
+			   src->component->handle, src->handle,
+			   dst->component->handle, dst->handle);
 		goto release_unlock;
 	}
 	src->connected = dst;
@@ -1729,7 +1742,8 @@ int vchiq_mmal_submit_buffer(struct vchi
 	 * videobuf2 won't let us have the dmabuf there.
 	 */
 	if (port->zero_copy && buffer->dma_buf && !buffer->vcsm_handle) {
-		pr_debug("%s: import dmabuf %p\n", __func__, buffer->dma_buf);
+		pr_dbg_lvl(2, debug, "%s: import dmabuf %p\n",
+			   __func__, buffer->dma_buf);
 		ret = vc_sm_cma_import_dmabuf(buffer->dma_buf,
 					      &buffer->vcsm_handle);
 		if (ret) {
@@ -1745,8 +1759,8 @@ int vchiq_mmal_submit_buffer(struct vchi
 			vc_sm_cma_free(buffer->vcsm_handle);
 			return ret;
 		}
-		pr_debug("%s: import dmabuf %p - got vc handle %08X\n",
-			 __func__, buffer->dma_buf, buffer->vc_handle);
+		pr_dbg_lvl(2, debug, "%s: import dmabuf %p - got vc handle %08X\n",
+			   __func__, buffer->dma_buf, buffer->vc_handle);
 	}
 
 	ret = buffer_from_host(instance, port, buffer);
@@ -1785,8 +1799,8 @@ int mmal_vchi_buffer_cleanup(struct mmal
 	if (buf->vcsm_handle) {
 		int ret;
 
-		pr_debug("%s: vc_sm_cma_free on handle %p\n", __func__,
-			 buf->vcsm_handle);
+		pr_dbg_lvl(2, debug, "%s: vc_sm_cma_free on handle %p\n", __func__,
+			   buf->vcsm_handle);
 		ret = vc_sm_cma_free(buf->vcsm_handle);
 		if (ret)
 			pr_err("%s: vcsm_free failed, ret %d\n", __func__, ret);
