media: rp1_cfe: Add LSB-aligned unpacked Bayer formats to the FE outputs

drivers/media/platform/raspberrypi/rp1_cfe/cfe.c

@@ -480,43 +480,74 @@ const struct cfe_fmt *find_format_by_pix(u32 pixelformat)
 }
 
 /*
- * Given the mbus code, find the 16 bit remapped code. Returns 0 if no remap
- * possible.
+ * Given the mbus code, find its remapped pixel format of the given type.
+ * Returns 0 if no remap possible.
  */
-u32 cfe_find_16bit_code(u32 code)
+static u32 cfe_find_remap_fourcc(u32 code, enum cfe_remap_types type)
 {
-	const struct cfe_fmt *cfe_fmt;
+	const struct cfe_fmt *cfe_fmt = find_format_by_code(code);
 
-	cfe_fmt = find_format_by_code(code);
+	return cfe_fmt ? cfe_fmt->remap[type] : 0;
+}
+
+/*
+ * Given the mbus code, find the code of its remapped pixel format of the given
+ * type. Returns 0 if no remap possible.
+ */
+static u32 cfe_find_remap_code(u32 code, enum cfe_remap_types type)
+{
+	const struct cfe_fmt *cfe_fmt;
+	u32 fourcc;
 
-	if (!cfe_fmt || !cfe_fmt->remap[CFE_REMAP_16BIT])
+	fourcc = cfe_find_remap_fourcc(code, type);
+	if (!fourcc)
 		return 0;
 
-	cfe_fmt = find_format_by_pix(cfe_fmt->remap[CFE_REMAP_16BIT]);
+	cfe_fmt = find_format_by_pix(fourcc);
 	if (!cfe_fmt)
 		return 0;
 
 	return cfe_fmt->code;
 }
 
 /*
- * Given the mbus code, find the 8 bit compressed code. Returns 0 if no remap
+ * Given the mbus code, find the 16 bit remapped code. Returns 0 if no remap
  * possible.
  */
-u32 cfe_find_compressed_code(u32 code)
+u32 cfe_find_16bit_code(u32 code)
 {
-	const struct cfe_fmt *cfe_fmt;
+	return cfe_find_remap_code(code, CFE_REMAP_16BIT);
+}
 
-	cfe_fmt = find_format_by_code(code);
+/*
+ * Given an unpacked LSB-aligned 16-bit pixel format, find the right shift of the
+ * FE output that writes it: 16 - the bit depth of the code it is the
+ * CFE_REMAP_LSB16 remap of. Returns 0 for any other pixel format. These formats
+ * are only available on the FE output nodes, as the CSI-2 REMAP mode is
+ * MSB-aligned.
+ */
+unsigned int cfe_find_lsb16_shift(u32 pixelformat)
+{
+	unsigned int i;
 
-	if (!cfe_fmt || !cfe_fmt->remap[CFE_REMAP_COMPRESSED])
+	if (!pixelformat)
 		return 0;
 
-	cfe_fmt = find_format_by_pix(cfe_fmt->remap[CFE_REMAP_COMPRESSED]);
-	if (!cfe_fmt)
-		return 0;
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].remap[CFE_REMAP_LSB16] == pixelformat)
+			return 16 - formats[i].depth;
+	}
 
-	return cfe_fmt->code;
+	return 0;
+}
+
+/*
+ * Given the mbus code, find the 8 bit compressed code. Returns 0 if no remap
+ * possible.
+ */
+u32 cfe_find_compressed_code(u32 code)
+{
+	return cfe_find_remap_code(code, CFE_REMAP_COMPRESSED);
 }
 
 static int cfe_calc_format_size_bpl(struct cfe_device *cfe,
@@ -1294,6 +1325,9 @@ static int cfe_enum_fmt_vid_cap(struct file *file, void *priv,
 		    !(formats[i].flags & CFE_FORMAT_FLAG_FE_OUT))
 			continue;
 
+		if (is_csi2_node(node) && cfe_find_lsb16_shift(formats[i].fourcc))
+			continue;
+
 		if (j == f->index) {
 			f->pixelformat = formats[i].fourcc;
 			f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -1338,7 +1372,7 @@ static int try_fmt_vid_cap(struct cfe_node *node, struct v4l2_format *f)
 	 * Default to a format that works for both CSI2 and FE.
 	 */
 	fmt = find_format_by_pix(f->fmt.pix.pixelformat);
-	if (!fmt)
+	if (!fmt || (is_csi2_node(node) && cfe_find_lsb16_shift(fmt->fourcc)))
 		fmt = find_format_by_code(MEDIA_BUS_FMT_SBGGR10_1X10);
 
 	f->fmt.pix.pixelformat = fmt->fourcc;
@@ -1663,6 +1697,25 @@ static const struct v4l2_file_operations cfe_fops = {
 	.mmap = vb2_fop_mmap,
 };
 
+/* The mbus code of the CSI-2 data feeding the FE, 0 if none. */
+static u32 cfe_fe_input_code(struct cfe_device *cfe)
+{
+	struct v4l2_subdev_state *state;
+	struct v4l2_mbus_framefmt *fmt;
+	u32 code = 0;
+
+	if (!is_fe_enabled(cfe))
+		return 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd);
+	fmt = v4l2_subdev_state_get_format(state, cfe->fe_csi2_channel);
+	if (fmt)
+		code = fmt->code;
+	v4l2_subdev_unlock_state(state);
+
+	return code;
+}
+
 static int cfe_video_link_validate(struct cfe_node *node,
 				   struct v4l2_subdev *remote_sd,
 				   struct v4l2_mbus_framefmt *remote_fmt)
@@ -1694,6 +1747,27 @@ static int cfe_video_link_validate(struct cfe_node *node,
 			return -EINVAL;
 		}
 
+		if (is_fe_node(node)) {
+			const struct cfe_fmt *pix = find_format_by_pix(pix_fmt->pixelformat);
+
+			/*
+			 * An LSB-aligned format of the FE source pad's code, at
+			 * the bit depth of the CSI-2 data feeding the FE.
+			 */
+			if (pix && cfe_find_lsb16_shift(pix->fourcc) &&
+			    pix->code == remote_fmt->code) {
+				u32 code = cfe_fe_input_code(cfe);
+
+				if (cfe_find_remap_fourcc(code, CFE_REMAP_LSB16) ==
+				    pix_fmt->pixelformat)
+					return 0;
+
+				cfe_err("Format %p4cc doesn't match the CSI-2 input code 0x%04x\n",
+					&pix_fmt->pixelformat, code);
+				return -EINVAL;
+			}
+		}
+
 		if (fmt->fourcc != pix_fmt->pixelformat &&
 		    fmt->remap[CFE_REMAP_16BIT] != pix_fmt->pixelformat &&
 		    fmt->remap[CFE_REMAP_COMPRESSED] != pix_fmt->pixelformat)  {

drivers/media/platform/raspberrypi/rp1_cfe/cfe.h

@@ -16,6 +16,8 @@ extern bool cfe_debug_verbose;
 enum cfe_remap_types {
 	CFE_REMAP_16BIT,
 	CFE_REMAP_COMPRESSED,
+	/* Unpacked, LSB-aligned 16-bit output of the FE */
+	CFE_REMAP_LSB16,
 	CFE_NUM_REMAP,
 };
 
@@ -39,5 +41,6 @@ const struct cfe_fmt *find_format_by_code(u32 code);
 const struct cfe_fmt *find_format_by_pix(u32 pixelformat);
 u32 cfe_find_16bit_code(u32 code);
 u32 cfe_find_compressed_code(u32 code);
+unsigned int cfe_find_lsb16_shift(u32 pixelformat);
 
 #endif

drivers/media/platform/raspberrypi/rp1_cfe/cfe_fmts.h

@@ -114,56 +114,64 @@ static const struct cfe_fmt formats[] = {
 		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
 		.depth = 10,
 		.csi_dt = MIPI_CSI2_DT_RAW10,
-		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR,
+			   V4L2_PIX_FMT_SBGGR10 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SGBRG10P,
 		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
 		.depth = 10,
 		.csi_dt = MIPI_CSI2_DT_RAW10,
-		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG,
+			   V4L2_PIX_FMT_SGBRG10 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SGRBG10P,
 		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
 		.depth = 10,
 		.csi_dt = MIPI_CSI2_DT_RAW10,
-		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG,
+			   V4L2_PIX_FMT_SGRBG10 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SRGGB10P,
 		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
 		.depth = 10,
 		.csi_dt = MIPI_CSI2_DT_RAW10,
-		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB,
+			   V4L2_PIX_FMT_SRGGB10 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SBGGR12P,
 		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
 		.depth = 12,
 		.csi_dt = MIPI_CSI2_DT_RAW12,
-		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR,
+			   V4L2_PIX_FMT_SBGGR12 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SGBRG12P,
 		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
 		.depth = 12,
 		.csi_dt = MIPI_CSI2_DT_RAW12,
-		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG,
+			   V4L2_PIX_FMT_SGBRG12 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SGRBG12P,
 		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
 		.depth = 12,
 		.csi_dt = MIPI_CSI2_DT_RAW12,
-		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG,
+			   V4L2_PIX_FMT_SGRBG12 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SRGGB12P,
 		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
 		.depth = 12,
 		.csi_dt = MIPI_CSI2_DT_RAW12,
-		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB,
+			   V4L2_PIX_FMT_SRGGB12 },
 	},
 	{
 		.fourcc = V4L2_PIX_FMT_SBGGR14P,
@@ -225,6 +233,59 @@ static const struct cfe_fmt formats[] = {
 		.flags = CFE_FORMAT_FLAG_FE_OUT,
 		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
 	},
+	/*
+	 * Unpacked LSB-aligned Bayer FE output: code is the FE source pad code, so
+	 * these come after the 16-bit entries (find_format_by_code() returns
+	 * the first match).
+	 */
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR10,
+		.code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR12,
+		.code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG10,
+		.code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG12,
+		.code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG10,
+		.code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG12,
+		.code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB10,
+		.code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB12,
+		.code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.depth = 16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
 	/* PiSP Compressed Mode 1 */
 	{
 		.fourcc = V4L2_PIX_FMT_PISP_COMP1_RGGB,

drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c

@@ -222,6 +222,7 @@ void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof)
 static bool pisp_fe_validate_output(struct pisp_fe_config const *cfg,
 				    unsigned int c, struct v4l2_format const *f)
 {
+	unsigned int shift;
 	unsigned int wbytes;
 
 	wbytes = cfg->ch[c].output.format.width;
@@ -236,6 +237,22 @@ static bool pisp_fe_validate_output(struct pisp_fe_config const *cfg,
 	    wbytes > f->fmt.pix.bytesperline)
 		return false;
 
+	/*
+	 * An LSB-aligned node format needs the FE output to match it: 16 bits per
+	 * sample, uncompressed, shifted right by 16 - bit depth.
+	 */
+	shift = cfe_find_lsb16_shift(f->fmt.pix.pixelformat);
+	if (shift) {
+		u32 format = cfg->ch[c].output.format.format;
+
+		if ((format & PISP_IMAGE_FORMAT_BPS_MASK) !=
+			    PISP_IMAGE_FORMAT_BPS_16 ||
+		    (format & PISP_IMAGE_FORMAT_COMPRESSION_MASK) ||
+		    (format & PISP_IMAGE_FORMAT_SHIFT_MASK) !=
+			    shift * PISP_IMAGE_FORMAT_SHIFT_1)
+			return false;
+	}
+
 	/* Check for zero-sized crops, which could cause lockup */
 	if ((cfg->global.enables & PISP_FE_ENABLE_CROP(c)) &&
 	    ((cfg->ch[c].crop.offset_x >= (cfg->input.format.width & ~1) ||