[PATCH v3 14/31] media: staging: media: imx6-mipi-csi2: move number pad macro define into imx6-csi2.c

@@ -4,6 +4,7 @@
  *
  * Copyright (c) 2012-2017 Mentor Graphics Inc.
  */
+#include <linux/bits.h>
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>

@@ -60,12 +61,23 @@ static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
 	.registered = csi2_registered,
 };
 
+/*
+ * there must be 5 pads: 1 input pad from sensor, and
+ * the 4 virtual channel output pads
+ */
+#define CSI2_SINK_PAD		0
+#define CSI2_NUM_SINK_PADS	1
+#define CSI2_NUM_SRC_PADS	4
+#define CSI2_NUM_PADS		5
+
 static const struct dw_mipi_csi2_config imx6_config = {
 	.module = THIS_MODULE,
 	.name = "imx6-mipi-csi2",
 	.internal_ops = &csi2_internal_ops,
 	.grp_id = IMX_MEDIA_GRP_ID_CSI2,
 	.gasket_init = csi2ipu_gasket_init,
+	.num_pads = CSI2_NUM_PADS,
+	.sink_pad_mask = BIT(CSI2_NUM_SINK_PADS),
 };
 
 static int csi2_probe(struct platform_device *pdev)

@@ -399,11 +399,11 @@ static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
 	if (!csi2->src_sd)
 		return -EPIPE;
 
-	for (i = 0; i < CSI2_NUM_SRC_PADS; i++) {
+	for (i = 0; i < csi2->num_sink_pads; i++) {
 		if (csi2->sink_linked[i])
 			break;
 	}
-	if (i >= CSI2_NUM_SRC_PADS)
+	if (i >= csi2->num_sink_pads)
 		return -EPIPE;
 
 	/*

@@ -500,7 +500,7 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 	struct dw_mipi_csi2_dev *csi2 = sd_to_dev(sd);
 	struct v4l2_mbus_framefmt *fmt;
 
-	if (sdformat->pad >= CSI2_NUM_PADS)
+	if (sdformat->pad >= (csi2->num_src_pads + csi2->num_sink_pads))
 		return -EINVAL;
 
 	guard(mutex)(&csi2->lock);

@@ -509,7 +509,7 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 		return -EBUSY;
 
 	/* Output pads mirror active input pad, no limits on input pads */
-	if (sdformat->pad != CSI2_SINK_PAD)
+	if (!(csi2->config->sink_pad_mask & BIT(sdformat->pad)))
 		sdformat->format = csi2->format_mbus;
 
 	fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);

@@ -574,7 +574,8 @@ static int csi2_notify_bound(struct v4l2_async_notifier *notifier,
 			     struct v4l2_async_connection *asd)
 {
 	struct dw_mipi_csi2_dev *csi2 = notifier_to_dev(notifier);
-	struct media_pad *sink = &csi2->sd.entity.pads[CSI2_SINK_PAD];
+	int sink_index = ffs(csi2->config->sink_pad_mask) - 1;
+	struct media_pad *sink = &csi2->sd.entity.pads[sink_index];
 	int pad;
 
 	pad = media_entity_get_fwnode_pad(&sd->entity, asd->match.fwnode,

@@ -679,12 +680,20 @@ int dw_mipi_csi2_init(struct platform_device *pdev, struct dw_mipi_csi2_dev *csi
 	csi2->sd.grp_id = config->grp_id;
 	csi2->config = config;
 
-	for (i = 0; i < CSI2_NUM_PADS; i++) {
-		csi2->pad[i].flags = (i == CSI2_SINK_PAD) ?
-		MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	if (!config)
+		return -EINVAL;
+
+	for (i = 0; i < config->num_pads; i++) {
+		if (config->sink_pad_mask & BIT(i)) {
+			csi2->pad[i].flags = MEDIA_PAD_FL_SINK;
+			csi2->num_sink_pads++;
+		} else {
+			csi2->pad[i].flags = MEDIA_PAD_FL_SOURCE;
+			csi2->num_src_pads++;
+		}
 	}
 
-	ret = media_entity_pads_init(&csi2->sd.entity, CSI2_NUM_PADS,
+	ret = media_entity_pads_init(&csi2->sd.entity, config->num_pads,
 				     csi2->pad);
 	if (ret)
 		return ret;

@@ -11,6 +11,8 @@
 
 struct dw_mipi_csi2_dev;
 
+#define DW_MAX_PAD_NUM		32
+
 struct dw_mipi_csi2_config {
 	struct module *module;
 	const char *name;

@@ -18,22 +20,18 @@ struct dw_mipi_csi2_config {
 	const struct v4l2_subdev_internal_ops *internal_ops;
 	/* Deprecated, should go through phy interface */
 	void (*gasket_init)(struct dw_mipi_csi2_dev *dev);
+	u32	num_pads;	/* Max 64 pad now */
+	u32	sink_pad_mask;
 };
 
-/*
- * there must be 5 pads: 1 input pad from sensor, and
- * the 4 virtual channel output pads
- */
-#define CSI2_SINK_PAD		0
-#define CSI2_NUM_SINK_PADS	1
-#define CSI2_NUM_SRC_PADS	4
-#define CSI2_NUM_PADS		5
-
 struct dw_mipi_csi2_dev {
 	struct device			*dev;
 	struct v4l2_subdev		sd;
 	struct v4l2_async_notifier	notifier;
-	struct media_pad		pad[CSI2_NUM_PADS];
+	struct media_pad		pad[DW_MAX_PAD_NUM];
+	int				num_src_pads;
+	int				num_sink_pads;
+
 	void __iomem			*base;
 	const struct dw_csi2_regs	*regs;
 

@@ -51,8 +49,7 @@ struct dw_mipi_csi2_dev {
 
 	int				stream_count;
 	struct v4l2_subdev		*src_sd;
-	bool				sink_linked[CSI2_NUM_SRC_PADS];
-
+	bool				sink_linked[DW_MAX_PAD_NUM];
 	const struct dw_mipi_csi2_config *config;
 };