[PATCH v5 09/14] media: stm32: dcmipp: addition of a dcmipp-isp subdev

The ISP subdev is the first element after input of the main pipeline.
Part (static configuration) of this block is done via this subdev while
other configuration done on a per-frame basis will be done via a output
metadata device attached to this subdev.
This subdev handled the following features of the ISP block:
  - statistic removal (top / bottom of the frame)
  - decimation
  - demosaicing
  - control of frame export to the aux pipeline

Signed-off-by: Alain Volmat <[email protected]>
---
 .../media/platform/st/stm32/stm32-dcmipp/Makefile  |   2 +-
 .../platform/st/stm32/stm32-dcmipp/dcmipp-common.h |   3 +
 .../platform/st/stm32/stm32-dcmipp/dcmipp-isp.c    | 491 +++++++++++++++++++++
 3 files changed, 495 insertions(+), 1 deletion(-)

diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile b/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile
index 54231569ed6fe..a708534a51af4 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile
@@ -1,5 +1,5 @@
 # SPDX-License-Identifier: GPL-2.0
 stm32-dcmipp-y := dcmipp-core.o dcmipp-common.o dcmipp-input.o dcmipp-byteproc.o dcmipp-bytecap.o
-stm32-dcmipp-y += dcmipp-pixelcommon.o
+stm32-dcmipp-y += dcmipp-pixelcommon.o dcmipp-isp.o
 
 obj-$(CONFIG_VIDEO_STM32_DCMIPP) += stm32-dcmipp.o
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h
index 9ec6d589b5036..c699905ee6e8c 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h
@@ -283,5 +283,8 @@ void dcmipp_byteproc_ent_release(struct dcmipp_ent_device *ved);
 struct dcmipp_ent_device *dcmipp_bytecap_ent_init(const char *entity_name,
 						  struct dcmipp_device *dcmipp);
 void dcmipp_bytecap_ent_release(struct dcmipp_ent_device *ved);
+struct dcmipp_ent_device *dcmipp_isp_ent_init(const char *entity_name,
+					      struct dcmipp_device *dcmipp);
+void dcmipp_isp_ent_release(struct dcmipp_ent_device *ved);
 
 #endif
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-isp.c b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-isp.c
new file mode 100644
index 0000000000000..1586cc9c59a6b
--- /dev/null
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-isp.c
@@ -0,0 +1,491 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for STM32 Digital Camera Memory Interface Pixel Processor
+ *
+ * Copyright (C) STMicroelectronics SA 2026
+ * Authors: Hugues Fruchet <[email protected]>
+ *          Alain Volmat <[email protected]>
+ *          for STMicroelectronics.
+ */
+
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-rect.h>
+#include <media/v4l2-subdev.h>
+
+#include "dcmipp-common.h"
+#include "dcmipp-pixelcommon.h"
+
+#define DCMIPP_P1FSCR	0x804
+#define DCMIPP_P1FSCR_PIPEDIFF BIT(18)
+
+#define DCMIPP_P1SRCR	0x820
+#define DCMIPP_P1SRCR_LASTLINE_SHIFT	0
+#define DCMIPP_P1SRCR_FIRSTLINEDEL_SHIFT	12
+#define DCMIPP_P1SRCR_CROPEN		BIT(15)
+
+#define DCMIPP_P1DECR	0x830
+#define DCMIPP_P1DECR_ENABLE		BIT(0)
+#define DCMIPP_P1DECR_HDEC_SHIFT	1
+#define DCMIPP_P1DECR_VDEC_SHIFT	3
+
+#define DCMIPP_P1DMCR	0x870
+#define DCMIPP_P1DMCR_ENABLE		BIT(0)
+#define DCMIPP_P1DMCR_TYPE_SHIFT	1
+#define DCMIPP_P1DMCR_TYPE_MASK		GENMASK(2, 1)
+#define DCMIPP_P1DMCR_TYPE_RGGB		0x0
+#define DCMIPP_P1DMCR_TYPE_GRBG		0x1
+#define DCMIPP_P1DMCR_TYPE_GBRG		0x2
+#define DCMIPP_P1DMCR_TYPE_BGGR		0x3
+
+#define ISP_MEDIA_BUS_SINK_FMT_DEFAULT MEDIA_BUS_FMT_RGB565_1X16
+#define ISP_MEDIA_BUS_SRC_FMT_DEFAULT MEDIA_BUS_FMT_RGB888_1X24
+
+struct dcmipp_isp_device {
+	struct dcmipp_ent_device ved;
+	struct v4l2_subdev sd;
+	struct device *dev;
+
+	void __iomem *regs;
+};
+
+static const struct v4l2_mbus_framefmt fmt_default = {
+	.width = DCMIPP_FMT_WIDTH_DEFAULT,
+	.height = DCMIPP_FMT_HEIGHT_DEFAULT,
+	.code = ISP_MEDIA_BUS_SINK_FMT_DEFAULT,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = DCMIPP_COLORSPACE_DEFAULT,
+	.ycbcr_enc = DCMIPP_YCBCR_ENC_DEFAULT,
+	.quantization = DCMIPP_QUANTIZATION_DEFAULT,
+	.xfer_func = DCMIPP_XFER_FUNC_DEFAULT,
+};
+
+static inline unsigned int dcmipp_isp_set_compose(__u32 size, __u32 req)
+{
+	unsigned int i = 0;
+
+	if (req > size)
+		return size;
+
+	/* Maximum decimation factor is 8 */
+	while (size > req && i++ < 3)
+		size /= 2;
+
+	return size;
+}
+
+static void dcmipp_isp_adjust_fmt(struct v4l2_mbus_framefmt *fmt, u32 pad)
+{
+	/* Only accept code in the pix map table */
+	if (!dcmipp_pixelpipe_pix_map_by_code(fmt->code, DCMIPP_ISP, pad))
+		fmt->code = IS_SRC(pad) ? ISP_MEDIA_BUS_SRC_FMT_DEFAULT :
+					  ISP_MEDIA_BUS_SINK_FMT_DEFAULT;
+
+	fmt->width = clamp_t(u32, fmt->width, DCMIPP_FRAME_MIN_WIDTH,
+			     DCMIPP_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, DCMIPP_FRAME_MIN_HEIGHT,
+			      DCMIPP_FRAME_MAX_HEIGHT);
+
+	if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
+		fmt->field = V4L2_FIELD_NONE;
+
+	dcmipp_colorimetry_clamp(fmt);
+}
+
+static int dcmipp_isp_init_state(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state)
+{
+	for (unsigned int i = 0; i < sd->entity.num_pads; i++) {
+		struct v4l2_mbus_framefmt *mf;
+
+		mf = v4l2_subdev_state_get_format(state, i);
+		*mf = fmt_default;
+		mf->code = IS_SRC(i) ? ISP_MEDIA_BUS_SRC_FMT_DEFAULT :
+				       ISP_MEDIA_BUS_SINK_FMT_DEFAULT;
+
+		if (IS_SINK(i)) {
+			struct v4l2_rect r = {
+				.top = 0,
+				.left = 0,
+				.width = DCMIPP_FMT_WIDTH_DEFAULT,
+				.height = DCMIPP_FMT_HEIGHT_DEFAULT,
+			};
+
+			*v4l2_subdev_state_get_crop(state, i) = r;
+			*v4l2_subdev_state_get_compose(state, i) = r;
+		}
+	}
+
+	return 0;
+}
+
+static int dcmipp_isp_enum_mbus_code(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     struct v4l2_subdev_mbus_code_enum *code)
+{
+	return dcmipp_pixelpipe_enum_mbus_code(DCMIPP_ISP, code);
+}
+
+static int dcmipp_isp_enum_frame_size(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      struct v4l2_subdev_frame_size_enum *fse)
+{
+	return dcmipp_pixelpipe_enum_frame_size(DCMIPP_ISP, fse);
+}
+
+static int dcmipp_isp_set_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_format *fmt)
+{
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    v4l2_subdev_is_streaming(sd))
+		return -EBUSY;
+
+	dcmipp_isp_adjust_fmt(&fmt->format, fmt->pad);
+
+	if (IS_SINK(fmt->pad)) {
+		struct v4l2_mbus_framefmt *src_fmt =
+			v4l2_subdev_state_get_format(state, 1);
+		struct v4l2_rect r = {
+			.top = 0,
+			.left = 0,
+			.width = fmt->format.width,
+			.height = fmt->format.height,
+		};
+
+		/* Adjust SINK pad crop/compose */
+		*v4l2_subdev_state_get_crop(state, 0) = r;
+		*v4l2_subdev_state_get_compose(state, 0) = r;
+
+		/* Forward format to SRC pads */
+		*src_fmt = fmt->format;
+		src_fmt->code = dcmipp_pixelpipe_src_format(fmt->format.code);
+		*v4l2_subdev_state_get_format(state, 2) = *src_fmt;
+	} else {
+		struct v4l2_mbus_framefmt *sink_fmt =
+			v4l2_subdev_state_get_format(state, 0);
+		struct v4l2_rect *compose =
+			v4l2_subdev_state_get_compose(state, 0);
+
+		fmt->format = *sink_fmt;
+		fmt->format.code = dcmipp_pixelpipe_src_format(sink_fmt->code);
+		if (compose->width && compose->height) {
+			fmt->format.width = compose->width;
+			fmt->format.height = compose->height;
+		}
+		/* Set to the 2nd SRC pad */
+		*v4l2_subdev_state_get_format(state, fmt->pad == 1 ? 2 : 1) =
+			fmt->format;
+	}
+
+	/* Update the selected pad format */
+	*v4l2_subdev_state_get_format(state, fmt->pad) = fmt->format;
+
+	return 0;
+}
+
+static void dcmipp_isp_adjust_crop(struct v4l2_rect *r,
+				   const struct v4l2_mbus_framefmt *fmt)
+{
+	struct v4l2_rect crop_min = {
+		.width = fmt->width,
+		.height = DCMIPP_FRAME_MIN_HEIGHT,
+	};
+
+	/*
+	 * Crop is related here to the statistics removal, that is
+	 * firsts and/or lasts lines of the frame. Up to the first
+	 * 7 lines can be skipped
+	 */
+#define DCMIPP_ISP_MAX_TOP_CROP	7
+	v4l2_rect_set_min_size(r, &crop_min);
+	if (r->top > DCMIPP_ISP_MAX_TOP_CROP)
+		r->top = DCMIPP_ISP_MAX_TOP_CROP;
+	if ((r->height + r->top) > fmt->height)
+		r->height = fmt->height - r->top;
+}
+
+static int dcmipp_isp_set_selection(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_selection *s)
+{
+	struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *sink_fmt, *src_fmt;
+	struct v4l2_rect *crop, *compose;
+	__u32 pad;
+
+	if (IS_SRC(s->pad))
+		return -EINVAL;
+
+	if (s->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    v4l2_subdev_is_streaming(sd))
+		return -EBUSY;
+
+	crop = v4l2_subdev_state_get_crop(state, s->pad);
+	compose = v4l2_subdev_state_get_compose(state, s->pad);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		sink_fmt = v4l2_subdev_state_get_format(state, s->pad);
+		dcmipp_isp_adjust_crop(&s->r, sink_fmt);
+
+		*crop = s->r;
+		*compose = s->r;
+
+		dev_dbg(isp->dev, "s_selection: crop (%d,%d)/%ux%u\n",
+			crop->left, crop->top, crop->width, crop->height);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		s->r.top = 0;
+		s->r.left = 0;
+		if (s->r.width < DCMIPP_FRAME_MIN_WIDTH)
+			s->r.width = DCMIPP_FRAME_MIN_WIDTH;
+		s->r.width = dcmipp_isp_set_compose(crop->width, s->r.width);
+		if (s->r.height < DCMIPP_FRAME_MIN_HEIGHT)
+			s->r.height = DCMIPP_FRAME_MIN_HEIGHT;
+		s->r.height = dcmipp_isp_set_compose(crop->height, s->r.height);
+		*compose = s->r;
+
+		dev_dbg(isp->dev, "s_selection: compose (%d,%d)/%ux%u\n",
+			compose->left, compose->top,
+			compose->width, compose->height);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Update the source pad size */
+	for (pad = 1; pad < sd->entity.num_pads; pad++) {
+		src_fmt = v4l2_subdev_state_get_format(state, pad);
+		src_fmt->width = s->r.width;
+		src_fmt->height = s->r.height;
+	}
+
+	return 0;
+}
+
+#define STM32_DCMIPP_IS_BAYER_VARIANT(code, variant)	\
+	((code) == MEDIA_BUS_FMT_S##variant##8_1X8 ||	\
+	 (code) == MEDIA_BUS_FMT_S##variant##10_1X10 ||	\
+	 (code) == MEDIA_BUS_FMT_S##variant##12_1X12 ||	\
+	 (code) == MEDIA_BUS_FMT_S##variant##14_1X14 ||	\
+	 (code) == MEDIA_BUS_FMT_S##variant##16_1X16)
+static void dcmipp_isp_config_demosaicing(struct dcmipp_isp_device *isp,
+					  struct v4l2_subdev_state *state)
+{
+	__u32 code = v4l2_subdev_state_get_format(state, 0)->code;
+	unsigned int val = 0;
+
+	/* Disable demosaicing */
+	reg_clear(isp, DCMIPP_P1DMCR,
+		  DCMIPP_P1DMCR_ENABLE | DCMIPP_P1DMCR_TYPE_MASK);
+
+	/* Only perform demosaicing if format is bayer */
+	if (code < MEDIA_BUS_FMT_SBGGR8_1X8 || code >= MEDIA_BUS_FMT_JPEG_1X8)
+		return;
+
+	dev_dbg(isp->dev, "Input is RawBayer, enable Demosaicing\n");
+
+	if (STM32_DCMIPP_IS_BAYER_VARIANT(code, BGGR))
+		val = DCMIPP_P1DMCR_TYPE_BGGR << DCMIPP_P1DMCR_TYPE_SHIFT;
+	else if (STM32_DCMIPP_IS_BAYER_VARIANT(code, GBRG))
+		val = DCMIPP_P1DMCR_TYPE_GBRG << DCMIPP_P1DMCR_TYPE_SHIFT;
+	else if (STM32_DCMIPP_IS_BAYER_VARIANT(code, GRBG))
+		val = DCMIPP_P1DMCR_TYPE_GRBG << DCMIPP_P1DMCR_TYPE_SHIFT;
+	else if (STM32_DCMIPP_IS_BAYER_VARIANT(code, RGGB))
+		val = DCMIPP_P1DMCR_TYPE_RGGB << DCMIPP_P1DMCR_TYPE_SHIFT;
+
+	val |= DCMIPP_P1DMCR_ENABLE;
+
+	reg_set(isp, DCMIPP_P1DMCR, val);
+}
+
+static bool dcmipp_isp_is_aux_output_enabled(struct dcmipp_isp_device *isp)
+{
+	struct media_link *link;
+
+	for_each_media_entity_data_link(isp->ved.ent, link) {
+		if (link->source != &isp->ved.pads[2])
+			continue;
+
+		if (!(link->flags & MEDIA_LNK_FL_ENABLED))
+			continue;
+
+		if (!strcmp(link->sink->entity->name, "dcmipp_aux_postproc"))
+			return true;
+	}
+
+	return false;
+}
+
+static void dcmipp_isp_config_decimation(struct dcmipp_isp_device *isp,
+					 struct v4l2_subdev_state *state)
+{
+	struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
+	struct v4l2_rect *compose = v4l2_subdev_state_get_compose(state, 0);
+	u32 decr;
+
+	decr = (fls(crop->width / compose->width) - 1) << DCMIPP_P1DECR_HDEC_SHIFT |
+	       (fls(crop->height / compose->height) - 1) << DCMIPP_P1DECR_VDEC_SHIFT;
+	if (decr)
+		decr |= DCMIPP_P1DECR_ENABLE;
+
+	reg_write(isp, DCMIPP_P1DECR, decr);
+}
+
+static int dcmipp_isp_enable_streams(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     u32 pad, u64 streams_mask)
+{
+	struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
+	struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
+	struct v4l2_subdev *s_subdev;
+	struct media_pad *s_pad;
+	int ret;
+
+	/* Perform configuration only if no other pad is enabled */
+	if (sd->enabled_pads)
+		return 0;
+
+	/* Get source subdev */
+	s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
+	if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
+		return -EINVAL;
+	s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
+
+	/* Check if link between ISP & Pipe2 postproc is enabled */
+	if (dcmipp_isp_is_aux_output_enabled(isp))
+		reg_clear(isp, DCMIPP_P1FSCR, DCMIPP_P1FSCR_PIPEDIFF);
+	else
+		reg_set(isp, DCMIPP_P1FSCR, DCMIPP_P1FSCR_PIPEDIFF);
+
+	/* Configure Statistic Removal */
+	crop = v4l2_subdev_state_get_crop(state, 0);
+	reg_write(isp, DCMIPP_P1SRCR,
+		  ((crop->top << DCMIPP_P1SRCR_FIRSTLINEDEL_SHIFT) |
+		   (crop->height << DCMIPP_P1SRCR_LASTLINE_SHIFT) |
+		   DCMIPP_P1SRCR_CROPEN));
+
+	/* Configure Decimation */
+	dcmipp_isp_config_decimation(isp, state);
+
+	/* Configure Demosaicing */
+	dcmipp_isp_config_demosaicing(isp, state);
+
+	ret = v4l2_subdev_enable_streams(s_subdev, s_pad->index, BIT_ULL(0));
+	if (ret < 0) {
+		dev_err(isp->dev,
+			"failed to start source subdev streaming (%d)\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dcmipp_isp_disable_streams(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      u32 pad, u64 streams_mask)
+{
+	struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *s_subdev;
+	struct media_pad *s_pad;
+	int ret;
+
+	/* Don't do anything if there are still other pads enabled */
+	if ((sd->enabled_pads & ~BIT(pad)))
+		return 0;
+
+	/* Get source subdev */
+	s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
+	if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
+		return -EINVAL;
+	s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
+
+	/* Disable all blocks */
+	reg_write(isp, DCMIPP_P1SRCR, 0);
+	reg_write(isp, DCMIPP_P1DECR, 0);
+	reg_write(isp, DCMIPP_P1DMCR, 0);
+
+	ret = v4l2_subdev_disable_streams(s_subdev, s_pad->index, BIT_ULL(0));
+	if (ret < 0) {
+		dev_err(isp->dev,
+			"failed to disable source subdev streaming (%d)\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops dcmipp_isp_pad_ops = {
+	.enum_mbus_code		= dcmipp_isp_enum_mbus_code,
+	.enum_frame_size	= dcmipp_isp_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= dcmipp_isp_set_fmt,
+	.get_selection		= dcmipp_pixelpipe_get_selection,
+	.set_selection		= dcmipp_isp_set_selection,
+	.enable_streams		= dcmipp_isp_enable_streams,
+	.disable_streams	= dcmipp_isp_disable_streams,
+};
+
+static const struct v4l2_subdev_video_ops dcmipp_isp_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_ops dcmipp_isp_ops = {
+	.pad = &dcmipp_isp_pad_ops,
+	.video = &dcmipp_isp_video_ops,
+};
+
+static void dcmipp_isp_release(struct v4l2_subdev *sd)
+{
+	struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
+
+	kfree(isp);
+}
+
+static const struct v4l2_subdev_internal_ops dcmipp_isp_int_ops = {
+	.init_state = dcmipp_isp_init_state,
+	.release = dcmipp_isp_release,
+};
+
+void dcmipp_isp_ent_release(struct dcmipp_ent_device *ved)
+{
+	struct dcmipp_isp_device *isp =
+			container_of(ved, struct dcmipp_isp_device, ved);
+
+	dcmipp_ent_sd_unregister(ved, &isp->sd);
+}
+
+struct dcmipp_ent_device *dcmipp_isp_ent_init(const char *entity_name,
+					      struct dcmipp_device *dcmipp)
+{
+	struct dcmipp_isp_device *isp;
+	const unsigned long pads_flag[] = {
+		MEDIA_PAD_FL_SINK, MEDIA_PAD_FL_SOURCE,
+		MEDIA_PAD_FL_SOURCE,
+	};
+	int ret;
+
+	/* Allocate the isp struct */
+	isp = kzalloc_obj(*isp);
+	if (!isp)
+		return ERR_PTR(-ENOMEM);
+
+	isp->regs = dcmipp->regs;
+
+	/* Initialize ved and sd */
+	ret = dcmipp_ent_sd_register(&isp->ved, &isp->sd,
+				     &dcmipp->v4l2_dev, entity_name,
+				     MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER,
+				     ARRAY_SIZE(pads_flag), pads_flag,
+				     &dcmipp_isp_int_ops, &dcmipp_isp_ops,
+				     NULL, NULL);
+	if (ret) {
+		kfree(isp);
+		return ERR_PTR(ret);
+	}
+
+	isp->ved.dcmipp = dcmipp;
+	isp->dev = dcmipp->dev;
+
+	return &isp->ved;
+}

-- 
2.34.1