video: stm32: dcmipp: add (semi)planar support

Add support for NV12/NV21, NV16/NV61 and YUV420/YVU420
(semi)planar formats which can be output by the main #1 pipe.

Signed-off-by: Alain Volmat <alain.volmat@foss.st.com>

Author: Alain Volmat

drivers/video/video_stm32_dcmipp.c

@@ -134,6 +134,62 @@ struct stm32_dcmipp_config {
 #define STM32_DCMIPP_WIDTH_MAX	4094
 #define STM32_DCMIPP_HEIGHT_MAX	4094
 
+#define VIDEO_FMT_IS_SEMI_PLANAR(fmt)			\
+	(((fmt)->pixelformat == VIDEO_PIX_FMT_NV12 ||	\
+	  (fmt)->pixelformat == VIDEO_PIX_FMT_NV21 ||	\
+	  (fmt)->pixelformat == VIDEO_PIX_FMT_NV16 ||	\
+	  (fmt)->pixelformat == VIDEO_PIX_FMT_NV61) ? true : false)
+
+#define VIDEO_FMT_IS_PLANAR(fmt)			\
+	(((fmt)->pixelformat == VIDEO_PIX_FMT_YUV420 ||	\
+	  (fmt)->pixelformat == VIDEO_PIX_FMT_YVU420) ? true : false)
+
+#define VIDEO_Y_PLANE_PITCH(fmt)						\
+	((VIDEO_FMT_IS_PLANAR(fmt) || VIDEO_FMT_IS_SEMI_PLANAR(fmt)) ?		\
+	 (fmt)->width : (fmt)->pitch)
+
+#define VIDEO_FMT_PLANAR_Y_PLANE_SIZE(fmt)	((fmt)->width * (fmt)->height)
+
+static void stm32_dcmipp_set_next_buffer_addr(struct stm32_dcmipp_pipe_data *pipe)
+{
+	struct stm32_dcmipp_data *dcmipp = pipe->dcmipp;
+#if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
+	struct video_format *fmt = &pipe->fmt;
+#endif
+	uint8_t *plane = pipe->next->buffer;
+
+	/* TODO - the HAL is missing a SetMemoryAddress for auxiliary addresses */
+	/* Update main buffer address */
+	if (pipe->id == DCMIPP_PIPE0) {
+		WRITE_REG(dcmipp->hdcmipp.Instance->P0PPM0AR1, (uint32_t)plane);
+	}
+#if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
+	else if (pipe->id == DCMIPP_PIPE1) {
+		WRITE_REG(dcmipp->hdcmipp.Instance->P1PPM0AR1, (uint32_t)plane);
+	} else {
+		WRITE_REG(dcmipp->hdcmipp.Instance->P2PPM0AR1, (uint32_t)plane);
+	}
+
+	if (pipe->id != DCMIPP_PIPE1) {
+		return;
+	}
+
+	if (VIDEO_FMT_IS_SEMI_PLANAR(fmt) || VIDEO_FMT_IS_PLANAR(fmt)) {
+		/* Y plane has 8 bit per pixel, next plane is located at off + width * height */
+		plane += VIDEO_FMT_PLANAR_Y_PLANE_SIZE(fmt);
+
+		WRITE_REG(dcmipp->hdcmipp.Instance->P1PPM1AR1, (uint32_t)plane);
+
+		if (VIDEO_FMT_IS_PLANAR(fmt)) {
+			/* In case of YUV420 / YVU420, U plane has half width / half height */
+			plane += VIDEO_FMT_PLANAR_Y_PLANE_SIZE(fmt) / 4;
+
+			WRITE_REG(dcmipp->hdcmipp.Instance->P1PPM2AR1, (uint32_t)plane);
+		}
+	}
+#endif
+}
+
 /* Callback getting called for each frame written into memory */
 void HAL_DCMIPP_PIPE_FrameEventCallback(DCMIPP_HandleTypeDef *hdcmipp, uint32_t Pipe)
 {
@@ -171,7 +227,6 @@ void HAL_DCMIPP_PIPE_VsyncEventCallback(DCMIPP_HandleTypeDef *hdcmipp, uint32_t
 	struct stm32_dcmipp_data *dcmipp =
 			CONTAINER_OF(hdcmipp, struct stm32_dcmipp_data, hdcmipp);
 	struct stm32_dcmipp_pipe_data *pipe = dcmipp->pipe[Pipe];
-	int ret;
 
 #if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
 	/*
@@ -209,17 +264,8 @@ void HAL_DCMIPP_PIPE_VsyncEventCallback(DCMIPP_HandleTypeDef *hdcmipp, uint32_t
 		return;
 	}
 
-	/*
-	 * TODO - we only support 1 buffer formats for the time being, setting of
-	 * MEMORY_ADDRESS_1 and MEMORY_ADDRESS_2 required depending on the pixelformat
-	 * for Pipe1
-	 */
-	ret = HAL_DCMIPP_PIPE_SetMemoryAddress(&dcmipp->hdcmipp, Pipe, DCMIPP_MEMORY_ADDRESS_0,
-					       (uint32_t)pipe->next->buffer);
-	if (ret != HAL_OK) {
-		LOG_ERR("Failed to update memory address");
-		return;
-	}
+	/* Update buffer address */
+	stm32_dcmipp_set_next_buffer_addr(pipe);
 }
 
 #if defined(STM32_DCMIPP_HAS_CSI)
@@ -443,7 +489,13 @@ static const struct stm32_dcmipp_mapping {
 	PIXEL_PIPE_FMT(ABGR32, ARGB8888, 0, (BIT(1) | BIT(2))),
 	PIXEL_PIPE_FMT(RGBA32, ARGB8888, 1, (BIT(1) | BIT(2))),
 	PIXEL_PIPE_FMT(BGRA32, RGBA888, 0, (BIT(1) | BIT(2))),
-	/* TODO - need to add the semiplanar & planar formats */
+	/* Multi-planes are only available on Pipe main (1) */
+	PIXEL_PIPE_FMT(NV12, YUV420_2, 0, BIT(1)),
+	PIXEL_PIPE_FMT(NV21, YUV420_2, 1, BIT(1)),
+	PIXEL_PIPE_FMT(NV16, YUV422_2, 0, BIT(1)),
+	PIXEL_PIPE_FMT(NV61, YUV422_2, 1, BIT(1)),
+	PIXEL_PIPE_FMT(YUV420, YUV420_3, 0, BIT(1)),
+	PIXEL_PIPE_FMT(YVU420, YUV420_3, 1, BIT(1)),
 #endif
 };
 
@@ -464,6 +516,9 @@ static const struct stm32_dcmipp_mapping {
 	 ((fmt) == VIDEO_PIX_FMT_GREY ||					\
 	  (fmt) == VIDEO_PIX_FMT_YUYV || (fmt) == VIDEO_PIX_FMT_YVYU ||		\
 	  (fmt) == VIDEO_PIX_FMT_VYUY || (fmt) == VIDEO_PIX_FMT_UYVY ||		\
+	  (fmt) == VIDEO_PIX_FMT_NV12 || (fmt) == VIDEO_PIX_FMT_NV21 ||		\
+	  (fmt) == VIDEO_PIX_FMT_NV16 || (fmt) == VIDEO_PIX_FMT_NV61 ||		\
+	  (fmt) == VIDEO_PIX_FMT_YUV420 || (fmt) == VIDEO_PIX_FMT_YVU420 ||	\
 	  (fmt) == VIDEO_PIX_FMT_XYUV32) ? VIDEO_COLORSPACE_YUV :		\
 										\
 	  VIDEO_COLORSPACE_RAW)
@@ -859,6 +914,95 @@ static int stm32_dcmipp_set_yuv_conversion(struct stm32_dcmipp_pipe_data *pipe,
 }
 #endif
 
+static int stm32_dcmipp_start_pipeline(const struct device *dev,
+				       struct stm32_dcmipp_pipe_data *pipe)
+{
+	const struct stm32_dcmipp_config *config = dev->config;
+	struct stm32_dcmipp_data *dcmipp = pipe->dcmipp;
+#if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
+	struct video_format *fmt = &pipe->fmt;
+#endif
+	int ret;
+
+#if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
+	if (VIDEO_FMT_IS_PLANAR(fmt)) {
+		uint8_t *u_addr = pipe->next->buffer + VIDEO_FMT_PLANAR_Y_PLANE_SIZE(fmt);
+		uint8_t *v_addr = u_addr + (VIDEO_FMT_PLANAR_Y_PLANE_SIZE(fmt) / 4);
+		DCMIPP_FullPlanarDstAddressTypeDef planar_addr = {
+			.YAddress = (uint32_t)pipe->next->buffer,
+			.UAddress = (uint32_t)u_addr,
+			.VAddress = (uint32_t)v_addr,
+		};
+
+		if (config->bus_type == VIDEO_BUS_TYPE_PARALLEL) {
+			ret = HAL_DCMIPP_PIPE_FullPlanarStart(&dcmipp->hdcmipp, pipe->id,
+							      &planar_addr, DCMIPP_MODE_CONTINUOUS);
+		}
+#if defined(STM32_DCMIPP_HAS_CSI)
+		else if (config->bus_type == VIDEO_BUS_TYPE_CSI2_DPHY) {
+			ret = HAL_DCMIPP_CSI_PIPE_FullPlanarStart(&dcmipp->hdcmipp, pipe->id,
+								  DCMIPP_VIRTUAL_CHANNEL0,
+								  &planar_addr,
+								  DCMIPP_MODE_CONTINUOUS);
+		}
+#endif
+		else {
+			LOG_ERR("Invalid bus_type");
+			ret = -EINVAL;
+		}
+	} else if (VIDEO_FMT_IS_SEMI_PLANAR(fmt)) {
+		uint8_t *uv_addr = pipe->next->buffer + VIDEO_FMT_PLANAR_Y_PLANE_SIZE(fmt);
+		DCMIPP_SemiPlanarDstAddressTypeDef semiplanar_addr = {
+			.YAddress = (uint32_t)pipe->next->buffer,
+			.UVAddress = (uint32_t)uv_addr,
+		};
+
+		if (config->bus_type == VIDEO_BUS_TYPE_PARALLEL) {
+			ret = HAL_DCMIPP_PIPE_SemiPlanarStart(&dcmipp->hdcmipp, pipe->id,
+							      &semiplanar_addr,
+							      DCMIPP_MODE_CONTINUOUS);
+		}
+#if defined(STM32_DCMIPP_HAS_CSI)
+		else if (config->bus_type == VIDEO_BUS_TYPE_CSI2_DPHY) {
+			ret = HAL_DCMIPP_CSI_PIPE_SemiPlanarStart(&dcmipp->hdcmipp, pipe->id,
+								  DCMIPP_VIRTUAL_CHANNEL0,
+								  &semiplanar_addr,
+								  DCMIPP_MODE_CONTINUOUS);
+		}
+#endif
+		else {
+			LOG_ERR("Invalid bus_type");
+			ret = -EINVAL;
+		}
+	} else {
+#endif
+		if (config->bus_type == VIDEO_BUS_TYPE_PARALLEL) {
+			ret = HAL_DCMIPP_PIPE_Start(&dcmipp->hdcmipp, pipe->id,
+						    (uint32_t)pipe->next->buffer,
+						    DCMIPP_MODE_CONTINUOUS);
+		}
+#if defined(STM32_DCMIPP_HAS_CSI)
+		else if (config->bus_type == VIDEO_BUS_TYPE_CSI2_DPHY) {
+			ret = HAL_DCMIPP_CSI_PIPE_Start(&dcmipp->hdcmipp, pipe->id,
+							DCMIPP_VIRTUAL_CHANNEL0,
+							(uint32_t)pipe->next->buffer,
+							DCMIPP_MODE_CONTINUOUS);
+		}
+#endif
+		else {
+			LOG_ERR("Invalid bus_type");
+			ret = -EINVAL;
+		}
+#if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
+	}
+#endif
+	if (ret != HAL_OK) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
 static int stm32_dcmipp_stream_enable(const struct device *dev)
 {
 	struct stm32_dcmipp_pipe_data *pipe = dev->data;
@@ -946,7 +1090,7 @@ static int stm32_dcmipp_stream_enable(const struct device *dev)
 	pipe_cfg.FrameRate  = DCMIPP_FRAME_RATE_ALL;
 #if defined(STM32_DCMIPP_HAS_PIXEL_PIPES)
 	if (pipe->id == DCMIPP_PIPE1 || pipe->id == DCMIPP_PIPE2) {
-		pipe_cfg.PixelPipePitch = fmt->pitch;
+		pipe_cfg.PixelPipePitch = VIDEO_Y_PLANE_PITCH(fmt);
 		pipe_cfg.PixelPackerFormat = mapping->pixels.dcmipp_format;
 	}
 #endif
@@ -1042,25 +1186,9 @@ static int stm32_dcmipp_stream_enable(const struct device *dev)
 #endif
 
 	/* Enable the DCMIPP Pipeline */
-	if (config->bus_type == VIDEO_BUS_TYPE_PARALLEL) {
-		ret = HAL_DCMIPP_PIPE_Start(&dcmipp->hdcmipp, pipe->id,
-					(uint32_t)pipe->next->buffer, DCMIPP_MODE_CONTINUOUS);
-	}
-#if defined(STM32_DCMIPP_HAS_CSI)
-	else if (config->bus_type == VIDEO_BUS_TYPE_CSI2_DPHY) {
-		ret = HAL_DCMIPP_CSI_PIPE_Start(&dcmipp->hdcmipp, pipe->id, DCMIPP_VIRTUAL_CHANNEL0,
-						(uint32_t)pipe->next->buffer,
-						DCMIPP_MODE_CONTINUOUS);
-	}
-#endif
-	else {
-		LOG_ERR("Invalid bus_type");
-		ret = -EINVAL;
-		goto out;
-	}
-	if (ret != HAL_OK) {
+	ret = stm32_dcmipp_start_pipeline(dev, pipe);
+	if (ret < 0) {
 		LOG_ERR("Failed to start the pipeline");
-		ret = -EIO;
 		goto out;
 	}
 
@@ -1183,7 +1311,6 @@ static int stm32_dcmipp_enqueue(const struct device *dev, struct video_buffer *v
 {
 	struct stm32_dcmipp_pipe_data *pipe = dev->data;
 	struct stm32_dcmipp_data *dcmipp = pipe->dcmipp;
-	int ret;
 
 	k_mutex_lock(&pipe->lock, K_FOREVER);
 
@@ -1194,13 +1321,7 @@ static int stm32_dcmipp_enqueue(const struct device *dev, struct video_buffer *v
 	if (pipe->state == STM32_DCMIPP_WAIT_FOR_BUFFER) {
 		LOG_DBG("Restart CPTREQ after wait for buffer");
 		pipe->next = vbuf;
-		ret = HAL_DCMIPP_PIPE_SetMemoryAddress(&dcmipp->hdcmipp, pipe->id,
-						       DCMIPP_MEMORY_ADDRESS_0,
-						       (uint32_t)pipe->next->buffer);
-		if (ret != HAL_OK) {
-			LOG_ERR("Failed to update memory address");
-			return -EIO;
-		}
+		stm32_dcmipp_set_next_buffer_addr(pipe);
 		if (pipe->id == DCMIPP_PIPE0) {
 			SET_BIT(dcmipp->hdcmipp.Instance->P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
 		}