drivers: video: 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 <[email protected]>
Signed-off-by: Phi Bang Nguyen <[email protected]>

Author: trunghieulenxp

drivers/clock_control/clock_control_mcux_ccm_rev2.c

@@ -225,6 +225,19 @@ static int mcux_ccm_get_subsys_rate(const struct device *dev,
 		clock_root = kCLOCK_Root_Netc;
 		break;
 #endif
+
+#if defined(CONFIG_VIDEO_MCUX_MIPI_CSI2RX)
+	case IMX_CCM_MIPI_CSI2RX_ROOT_CLK:
+		clock_root = kCLOCK_Root_Csi2;
+		break;
+	case IMX_CCM_MIPI_CSI2RX_ESC_CLK:
+		clock_root = kCLOCK_Root_Csi2_Esc;
+		break;
+	case IMX_CCM_MIPI_CSI2RX_UI_CLK:
+		clock_root = kCLOCK_Root_Csi2_Ui;
+		break;
+#endif
+
 	default:
 		return -EINVAL;
 	}
@@ -264,6 +277,16 @@ static int CCM_SET_FUNC_ATTR mcux_ccm_set_subsys_rate(const struct device *dev,
 		 */
 		return flexspi_clock_set_freq(clock_name, clock_rate);
 #endif
+
+#if defined(CONFIG_VIDEO_MCUX_MIPI_CSI2RX)
+	case IMX_CCM_MIPI_CSI2RX_ROOT_CLK:
+		return mipi_csi2rx_clock_set_freq(kCLOCK_Root_Csi2, clock_rate);
+	case IMX_CCM_MIPI_CSI2RX_UI_CLK:
+		return mipi_csi2rx_clock_set_freq(kCLOCK_Root_Csi2_Ui, clock_rate);
+	case IMX_CCM_MIPI_CSI2RX_ESC_CLK:
+		return mipi_csi2rx_clock_set_freq(kCLOCK_Root_Csi2_Esc, clock_rate);
+#endif
+
 	default:
 		/* Silence unused variable warning */
 		ARG_UNUSED(clock_rate);

drivers/video/video_mcux_mipi_csi2rx.c

@@ -6,15 +6,19 @@
 
 #define DT_DRV_COMPAT nxp_mipi_csi2rx
 
+#include <zephyr/drivers/clock_control.h>
 #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)
 
 #define DEVICE_DT_INST_GET_SENSOR_DEV(n)                                                           \
 	DEVICE_DT_GET(DT_GPARENT(DT_NODELABEL(                                                     \
@@ -28,100 +32,106 @@ struct mipi_csi2rx_config {
 
 struct mipi_csi2rx_data {
 	csi2rx_config_t csi2rxConfig;
+	const struct device *clock_dev;
+	clock_control_subsys_t clock_root;
+	clock_control_subsys_t clock_ui;
+	clock_control_subsys_t clock_esc;
 };
 
-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;
+	uint32_t root_clk_rate, ui_clk_rate, sensor_byte_clk, 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");
+	if (sensor_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
+		LOG_ERR("Sensor pixel rate is not supported");
 		return -ENOTSUP;
 	}
 
-	drv_data->csi2rxConfig = csi2rxConfig;
+	bpp = video_pix_fmt_bpp(fmt.pixelformat) * 8;
+	sensor_byte_clk = sensor_pixel_rate * bpp / drv_data->csi2rxConfig.laneNum / 8;
+
+	ret = clock_control_get_rate(drv_data->clock_dev, drv_data->clock_root, &root_clk_rate);
+	if (ret) {
+		return ret;
+	}
+
+	if (sensor_byte_clk > root_clk_rate) {
+		ret = clock_control_set_rate(drv_data->clock_dev, drv_data->clock_root,
+					     (clock_control_subsys_rate_t)sensor_byte_clk);
+		if (ret) {
+			return ret;
+		}
+	}
+
+	ret = clock_control_get_rate(drv_data->clock_dev, drv_data->clock_ui, &ui_clk_rate);
+	if (ret) {
+		return ret;
+	}
+
+	if (sensor_pixel_rate > ui_clk_rate) {
+		ret = clock_control_set_rate(
+			drv_data->clock_dev, drv_data->clock_ui,
+			(clock_control_subsys_rate_t)(uint32_t)sensor_pixel_rate);
+		if (ret) {
+			return ret;
+		}
+	}
+
+	/* 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,
@@ -203,20 +213,36 @@ static const struct video_driver_api mipi_csi2rx_driver_api = {
 static int mipi_csi2rx_init(const struct device *dev)
 {
 	const struct mipi_csi2rx_config *config = dev->config;
+	struct mipi_csi2rx_data *drv_data = dev->data;
+	int ret;
 
 	/* Check if there is any sensor device */
 	if (!device_is_ready(config->sensor_dev)) {
 		return -ENODEV;
 	}
 
-	return 0;
+	/*
+	 * CSI2 escape clock should be in the range [60, 80] Mhz. We set it
+	 * to 60 Mhz.
+	 */
+	ret = clock_control_set_rate(drv_data->clock_dev, drv_data->clock_esc,
+				     (clock_control_subsys_rate_t)MHZ(60));
+	if (ret) {
+		return ret;
+	}
+
+	return mipi_csi2rx_update_settings(dev, VIDEO_EP_ALL);
 }
 
 #define MIPI_CSI2RX_INIT(n)                                                                        \
 	static struct mipi_csi2rx_data mipi_csi2rx_data_##n = {                                    \
 		.csi2rxConfig.laneNum =                                                            \
 			DT_PROP_LEN(DT_CHILD(DT_CHILD(DT_INST_CHILD(n, ports), port_1), endpoint), \
 				    data_lanes),                                                   \
+		.clock_dev = DEVICE_DT_GET(DT_INST_CLOCKS_CTLR(n)),                                \
+		.clock_root = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 0, name),      \
+		.clock_ui = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 1, name),        \
+		.clock_esc = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 2, name),       \
 	};                                                                                         \
                                                                                                    \
 	static const struct mipi_csi2rx_config mipi_csi2rx_config_##n = {                          \

dts/arm/nxp/nxp_rt11xx.dtsi

@@ -901,6 +901,9 @@
 			compatible = "nxp,mipi-csi2rx";
 			reg = <0x40810000 0x200>;
 			status = "disabled";
+			clocks = <&ccm IMX_CCM_MIPI_CSI2RX_ROOT_CLK 0 0>,
+			<&ccm IMX_CCM_MIPI_CSI2RX_UI_CLK 0 0>,
+			<&ccm IMX_CCM_MIPI_CSI2RX_ESC_CLK 0 0>;
 
 			ports {
 				#address-cells = <1>;

include/zephyr/dt-bindings/clock/imx_ccm_rev2.h

@@ -134,6 +134,11 @@
 /* NETC */
 #define IMX_CCM_NETC_CLK 0x1800UL
 
+/* MIPI CSI2RX */
+#define IMX_CCM_MIPI_CSI2RX_ROOT_CLK 0x1900UL
+#define IMX_CCM_MIPI_CSI2RX_UI_CLK   0x2000UL
+#define IMX_CCM_MIPI_CSI2RX_ESC_CLK  0x2100UL
+
 /* QTMR */
 #define IMX_CCM_QTMR_CLK               0x6000UL
 #define IMX_CCM_QTMR1_CLK              0x6000UL

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,41 @@ void imxrt_post_init_display_interface(void)
 
 #endif
 
+#if CONFIG_VIDEO_MCUX_MIPI_CSI2RX
+int 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 -EINVAL;
+	}
+
+	clk_source = CLOCK_GetRootClockSource(clock_root, rootCfg.mux);
+	freq = CLOCK_GetFreq(clk_source);
+	if (rate > freq) {
+		LOG_ERR("Requested rate is higher than the maximum clock frequency");
+		return -EINVAL;
+	}
+
+	rootCfg.div = (uint32_t)freq / rate;
+	CLOCK_SetRootClock(clock_root, &rootCfg);
+
+	return 0;
+}
+#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
+int 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);
 

tests/drivers/build_all/video/mimxrt1170_evk_mimxrt1176_cm7.overlay

@@ -60,6 +60,9 @@
 			compatible = "nxp,mipi-csi2rx";
 			reg = <0x33334444 0x200>;
 			status = "okay";
+			clocks = <&ccm IMX_CCM_MIPI_CSI2RX_ROOT_CLK 0 0>,
+			<&ccm IMX_CCM_MIPI_CSI2RX_UI_CLK 0 0>,
+			<&ccm IMX_CCM_MIPI_CSI2RX_ESC_CLK 0 0>;
 
 			ports {
 				#address-cells = <1>;