drivers: video: mcux_mipi_csi2rx: Set clocks according to pixel rate

Instead of fixing csi2rx clock frequencies, set them according to the
pixel rate got from the camera sensor.

Signed-off-by: Trung Hieu Le <trunghieu.le@nxp.com>
Signed-off-by: Phi Bang Nguyen <phibang.nguyen@nxp.com>

Author: trunghieulenxp

drivers/video/video_mcux_mipi_csi2rx.c

@@ -9,12 +9,15 @@
 #include <zephyr/drivers/video.h>
 #include <zephyr/kernel.h>
 #include <zephyr/logging/log.h>
+#include <soc.h>
 
 #include <fsl_mipi_csi2rx.h>
 
 LOG_MODULE_REGISTER(video_mipi_csi2rx, CONFIG_VIDEO_LOG_LEVEL);
 
-#define DEFAULT_CAMERA_FRAME_RATE  30
+#define MAX_SUPPORTED_PIXEL_RATE MHZ(96)
+
+#define ABS(a, b) (a > b ? a - b : b - a)
 
 struct mipi_csi2rx_config {
 	const MIPI_CSI2RX_Type *base;
@@ -25,98 +28,82 @@ struct mipi_csi2rx_data {
 	csi2rx_config_t csi2rxConfig;
 };
 
-static int mipi_csi2rx_set_fmt(const struct device *dev, enum video_endpoint_id ep,
-			       struct video_format *fmt)
+struct mipi_csi2rx_tHsSettleEscClk_config {
+	uint64_t pixel_rate;
+	uint8_t tHsSettle_EscClk;
+};
+
+/* Must be in pixel rate ascending order */
+const struct mipi_csi2rx_tHsSettleEscClk_config tHsSettleEscClk_configs[] = {
+	{MHZ(24), 0x24},
+	{MHZ(48), 0x12},
+	{MHZ(96), 0x09},
+};
+
+static int mipi_csi2rx_update_settings(const struct device *dev, enum video_endpoint_id ep)
 {
 	const struct mipi_csi2rx_config *config = dev->config;
 	struct mipi_csi2rx_data *drv_data = dev->data;
-	csi2rx_config_t csi2rxConfig = {0};
-	uint8_t i = 0;
+	uint8_t bpp;
+	uint64_t sensor_pixel_rate, sensor_lane_rate, sensor_byte_clk;
+	uint32_t best_match;
+	int ret, ind = 0;
+	struct video_format fmt;
+
+	ret = video_get_format(config->sensor_dev, ep, &fmt);
+	if (ret) {
+		LOG_ERR("Cannot get sensor_dev pixel format");
+		return ret;
+	}
 
-	/*
-	 * Initialize the MIPI CSI2
-	 *
-	 * From D-PHY specification, the T-HSSETTLE should in the range of 85ns+6*UI to 145ns+10*UI
-	 * UI is Unit Interval, equal to the duration of any HS state on the Clock Lane
-	 *
-	 * T-HSSETTLE = csi2rxConfig.tHsSettle_EscClk * (Tperiod of RxClkInEsc)
-	 *
-	 * csi2rxConfig.tHsSettle_EscClk setting for camera:
-	 *
-	 *    Resolution  |  frame rate  |  T_HS_SETTLE
-	 *  =============================================
-	 *     720P       |     30       |     0x12
-	 *  ---------------------------------------------
-	 *     720P       |     15       |     0x17
-	 *  ---------------------------------------------
-	 *      VGA       |     30       |     0x1F
-	 *  ---------------------------------------------
-	 *      VGA       |     15       |     0x24
-	 *  ---------------------------------------------
-	 *     QVGA       |     30       |     0x1F
-	 *  ---------------------------------------------
-	 *     QVGA       |     15       |     0x24
-	 *  ---------------------------------------------
-	 */
-	static const uint32_t csi2rxHsSettle[][4] = {
-		{
-			1280,
-			720,
-			30,
-			0x12,
-		},
-		{
-			1280,
-			720,
-			15,
-			0x17,
-		},
-		{
-			640,
-			480,
-			30,
-			0x1F,
-		},
-		{
-			640,
-			480,
-			15,
-			0x24,
-		},
-		{
-			320,
-			240,
-			30,
-			0x1F,
-		},
-		{
-			320,
-			240,
-			15,
-			0x24,
-		},
-	};
-
-	for (i = 0; i < ARRAY_SIZE(csi2rxHsSettle); i++) {
-		if ((fmt->width == csi2rxHsSettle[i][0]) && (fmt->height == csi2rxHsSettle[i][1]) &&
-		    (DEFAULT_CAMERA_FRAME_RATE == csi2rxHsSettle[i][2])) {
-			csi2rxConfig.tHsSettle_EscClk = csi2rxHsSettle[i][3];
-			break;
-		}
+	ret = video_get_ctrl(config->sensor_dev, VIDEO_CID_PIXEL_RATE, &sensor_pixel_rate);
+	if (ret) {
+		LOG_ERR("Can not get sensor_dev pixel rate");
+		return ret;
 	}
 
-	if (i == ARRAY_SIZE(csi2rxHsSettle)) {
-		LOG_ERR("Unsupported resolution");
+	bpp = video_pix_fmt_bpp(fmt.pixelformat) * 8;
+	sensor_lane_rate = sensor_pixel_rate * bpp / drv_data->csi2rxConfig.laneNum;
+
+	if (sensor_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
+		LOG_ERR("Sensor pixel rate is not supported");
 		return -ENOTSUP;
 	}
 
-	drv_data->csi2rxConfig = csi2rxConfig;
+	sensor_byte_clk = sensor_pixel_rate * bpp / drv_data->csi2rxConfig.laneNum / 8;
+	if (sensor_byte_clk > CLOCK_GetRootClockFreq(kCLOCK_Root_Csi2)) {
+		mipi_csi2rx_clock_set_freq(kCLOCK_Root_Csi2, sensor_byte_clk);
+	}
+
+	if (sensor_pixel_rate > CLOCK_GetRootClockFreq(kCLOCK_Root_Csi2_Ui)) {
+		mipi_csi2rx_clock_set_freq(kCLOCK_Root_Csi2_Ui, sensor_pixel_rate);
+	}
+
+	/* Find the supported sensor_pixel_rate closest to the desired one */
+	best_match = tHsSettleEscClk_configs[ind].pixel_rate;
+	for (uint8_t i = 0; i < ARRAY_SIZE(tHsSettleEscClk_configs); i++) {
+		if (ABS(tHsSettleEscClk_configs[i].pixel_rate, sensor_pixel_rate) <
+		    ABS(tHsSettleEscClk_configs[i].pixel_rate, best_match)) {
+			best_match = tHsSettleEscClk_configs[i].pixel_rate;
+			ind = i;
+		}
+	}
+
+	drv_data->csi2rxConfig.tHsSettle_EscClk = tHsSettleEscClk_configs[ind].tHsSettle_EscClk;
+
+	return ret;
+}
+
+static int mipi_csi2rx_set_fmt(const struct device *dev, enum video_endpoint_id ep,
+			       struct video_format *fmt)
+{
+	const struct mipi_csi2rx_config *config = dev->config;
 
 	if (video_set_format(config->sensor_dev, ep, fmt)) {
 		return -EIO;
 	}
 
-	return 0;
+	return mipi_csi2rx_update_settings(dev, ep);
 }
 
 static int mipi_csi2rx_get_fmt(const struct device *dev, enum video_endpoint_id ep,
@@ -204,7 +191,13 @@ static int mipi_csi2rx_init(const struct device *dev)
 		return -ENODEV;
 	}
 
-	return 0;
+	/*
+	 * CSI2 escape clock should be in the range [60, 80] Mhz. We set it
+	 * to 60 Mhz.
+	 */
+	mipi_csi2rx_clock_set_freq(kCLOCK_Root_Csi2_Esc, MHZ(60));
+
+	return mipi_csi2rx_update_settings(dev, VIDEO_EP_ALL);
 }
 
 #define MIPI_CSI2RX_INIT(n)                                                                        \

soc/nxp/imxrt/imxrt11xx/soc.c

@@ -474,17 +474,6 @@ static ALWAYS_INLINE void clock_init(void)
 	CLOCK_EnableClock(kCLOCK_Video_Mux);
 	VIDEO_MUX->VID_MUX_CTRL.SET = VIDEO_MUX_VID_MUX_CTRL_CSI_SEL_MASK;
 
-	/* Configure MIPI CSI-2 Rx clocks */
-	rootCfg.div = 8;
-	rootCfg.mux = kCLOCK_CSI2_ClockRoot_MuxSysPll3Out;
-	CLOCK_SetRootClock(kCLOCK_Root_Csi2, &rootCfg);
-
-	rootCfg.mux = kCLOCK_CSI2_ESC_ClockRoot_MuxSysPll3Out;
-	CLOCK_SetRootClock(kCLOCK_Root_Csi2_Esc, &rootCfg);
-
-	rootCfg.mux = kCLOCK_CSI2_UI_ClockRoot_MuxSysPll3Out;
-	CLOCK_SetRootClock(kCLOCK_Root_Csi2_Ui, &rootCfg);
-
 	/* Enable power domain for MIPI CSI-2 */
 	PGMC_BPC4->BPC_POWER_CTRL |= (PGMC_BPC_BPC_POWER_CTRL_PSW_ON_SOFT_MASK |
 				      PGMC_BPC_BPC_POWER_CTRL_ISO_OFF_SOFT_MASK);
@@ -682,6 +671,35 @@ void imxrt_post_init_display_interface(void)
 
 #endif
 
+#if CONFIG_VIDEO_MCUX_MIPI_CSI2RX
+void mipi_csi2rx_clock_set_freq(clock_root_t clock_root, uint32_t rate)
+{
+	clock_root_config_t rootCfg = {0};
+	uint32_t freq;
+	clock_name_t clk_source;
+
+	switch (clock_root) {
+	case kCLOCK_Root_Csi2:
+		rootCfg.mux = kCLOCK_CSI2_ClockRoot_MuxSysPll3Out;
+		break;
+	case kCLOCK_Root_Csi2_Esc:
+		rootCfg.mux = kCLOCK_CSI2_ESC_ClockRoot_MuxSysPll3Out;
+		break;
+	case kCLOCK_Root_Csi2_Ui:
+		rootCfg.mux = kCLOCK_CSI2_UI_ClockRoot_MuxSysPll3Out;
+		break;
+	default:
+		return;
+	}
+
+	clk_source = CLOCK_GetRootClockSource(clock_root, rootCfg.mux);
+	freq = CLOCK_GetFreq(clk_source);
+	__ASSERT(rate < freq, "Requested rate is higher than the maximum clock frequency");
+	rootCfg.div = (uint32_t)freq / rate;
+	CLOCK_SetRootClock(clock_root, &rootCfg);
+}
+#endif
+
 /**
  *
  * @brief Perform basic hardware initialization

soc/nxp/imxrt/imxrt11xx/soc.h

@@ -32,6 +32,10 @@ void imxrt_pre_init_display_interface(void);
 void imxrt_post_init_display_interface(void);
 #endif
 
+#if CONFIG_VIDEO_MCUX_MIPI_CSI2RX
+void mipi_csi2rx_clock_set_freq(clock_root_t clock_root, uint32_t rate);
+#endif
+
 void flexspi_clock_set_div(uint32_t value);
 uint32_t flexspi_clock_get_freq(void);