video: add LINK_FREQUENCY ctrl and a helper to retrieve it

Add a read-only ctrl VIDEO_CID_LINK_FREQUENCY to indicate
to a sink the frequency at which a device streams data over
CSI2. Since not all source device currently provide the
LINK_FREQUENCY control, add a helper function to retrieve
it, with a fall-back by doing an approximate via the
PIXEL_RATE control.

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

Author: Alain Volmat

drivers/video/video_common.c

@@ -10,6 +10,7 @@
 #include <zephyr/device.h>
 #include <zephyr/drivers/i2c.h>
 #include <zephyr/drivers/video.h>
+#include <zephyr/drivers/video-controls.h>
 #include <zephyr/kernel.h>
 #include <zephyr/logging/log.h>
 #include <zephyr/sys/byteorder.h>
@@ -365,3 +366,42 @@ int video_write_cci_multiregs16(const struct i2c_dt_spec *i2c, const struct vide
 
 	return 0;
 }
+
+int64_t video_get_link_frequency(const struct device *dev, uint8_t bpp, uint8_t lane_nb)
+{
+	int ret;
+
+	struct video_control ctrl = {
+		.id = VIDEO_CID_LINK_FREQUENCY,
+	};
+	struct video_ctrl_query ctrl_query = {
+		.id = VIDEO_CID_LINK_FREQUENCY,
+	};
+
+	/* Try to get the LINK_FREQUENCY value from the source device */
+	ret = video_get_ctrl(dev, &ctrl);
+	if (ret < 0) {
+		goto fallback;
+	}
+
+	ret = video_query_ctrl(dev, &ctrl_query);
+	if (ret < 0) {
+		return ret;
+	}
+
+	if (ctrl.val < ctrl_query.range.min || ctrl.val > ctrl_query.range.max) {
+		return -EIO;
+	}
+
+	return (int64_t)ctrl_query.int_menu[ctrl.val];
+
+fallback:
+	/* If VIDEO_CID_LINK_FREQUENCY is not available, approximate from VIDEO_CID_PIXEL_RATE */
+	ctrl.id = VIDEO_CID_PIXEL_RATE;
+	ret = video_get_ctrl(dev, &ctrl);
+	if (ret < 0) {
+		return ret;
+	}
+
+	return ctrl.val64 * bpp / (2 * lane_nb);
+}

drivers/video/video_ctrls.c

@@ -108,6 +108,10 @@ static inline void set_type_flag(uint32_t id, enum video_ctrl_type *type, uint32
 		*type = VIDEO_CTRL_TYPE_INTEGER64;
 		*flags |= VIDEO_CTRL_FLAG_READ_ONLY;
 		break;
+	case VIDEO_CID_LINK_FREQUENCY:
+		*type = VIDEO_CTRL_TYPE_MENU_INTEGER;
+		*flags |= VIDEO_CTRL_FLAG_READ_ONLY;
+		break;
 	default:
 		*type = VIDEO_CTRL_TYPE_INTEGER;
 		break;
@@ -528,6 +532,8 @@ static inline const char *video_get_ctrl_name(uint32_t id)
 		return "Pixel Rate";
 	case VIDEO_CID_TEST_PATTERN:
 		return "Test Pattern";
+	case VIDEO_CID_LINK_FREQUENCY:
+		return "Link Frequency";
 	default:
 		return NULL;
 	}

include/zephyr/drivers/video-controls.h

@@ -368,6 +368,9 @@ enum video_camera_orientation {
 /** Selection of the type of test pattern to represent */
 #define VIDEO_CID_TEST_PATTERN (VIDEO_CID_IMAGE_PROC_CLASS_BASE + 3)
 
+/** Link frequency, applicable for the CSI2 based devices. This control is read-only. */
+#define VIDEO_CID_LINK_FREQUENCY (VIDEO_CID_IMAGE_PROC_CLASS_BASE + 4)
+
 /**
  * @}
  */

include/zephyr/drivers/video.h

@@ -804,6 +804,23 @@ void video_closest_frmival_stepwise(const struct video_frmival_stepwise *stepwis
  */
 void video_closest_frmival(const struct device *dev, struct video_frmival_enum *match);
 
+/**
+ * @brief Return the link-frequency advertised by a device
+ *
+ * Device exposing a CSI link should advertise at least one of the following two controls:
+ *   - @ref VIDEO_CID_LINK_FREQUENCY
+ *   - @ref VIDEO_CID_PIXEL_RATE
+ *
+ * At first the helper will try read the @ref VIDEO_CID_LINK_FREQUENCY and if not available will
+ * approximate the link-frequency from the @ref VIDEO_CID_PIXEL_RATE value, taking into
+ * consideration the bits per pixel of the format and the number of lanes.
+ *
+ * @param dev Video device to query.
+ * @param bpp Amount of bits per pixel of the pixel format produced by the device
+ * @param lane_nb Number of CSI-2 lanes used
+ */
+int64_t video_get_link_frequency(const struct device *dev, uint8_t bpp, uint8_t lane_nb);
+
 /**
  * @defgroup video_pixel_formats Video pixel formats
  * The '|' characters separate the pixels or logical blocks, and spaces separate the bytes.