imx355: Rework to compute link frequency and pixel clock

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

Author: 6by9

drivers/media/i2c/imx355.c

@@ -34,6 +34,8 @@
 #define IMX355_REG_PLL_OP_PREDIV	CCI_REG8(0x030d)
 #define IMX355_REG_PLL_OP_MUL		CCI_REG16(0x030e)
 #define IMX355_REG_PLL_IVT_SYSCK_DIV	CCI_REG8(0x0303)
+#define IMX355_REG_PLL_IVT_PCK_DIV      CCI_REG8(0x0301)
+#define IMX355_PLL_IVT_PCK_DIV		5
 
 /* V_TIMING internal */
 #define IMX355_REG_FLL			CCI_REG16(0x0340)
@@ -89,13 +91,6 @@
 /* Flip Control */
 #define IMX355_REG_ORIENTATION		CCI_REG8(0x0101)
 
-/* default link frequency and external clock */
-#define IMX355_LINK_FREQ_4LANE		360000000LL
-#define IMX355_LINK_FREQ_2LANE		444000000LL
-
-#define IMX355_PIXEL_RATE_4LANE		288000000
-#define IMX355_PIXEL_RATE_2LANE		177600000
-
 #define IMX355_PIXEL_ARRAY_TOP		0
 #define IMX355_PIXEL_ARRAY_LEFT		0
 #define IMX355_PIXEL_ARRAY_WIDTH	3280
@@ -142,7 +137,7 @@ static const struct imx355_clk_params imx355_clk_params[] = {
 	{
 		.ext_clk = 19200000,
 		.extclk_freq = 0x1333,
-		.pll_op_mpy = { 75, 93 },
+		.pll_op_mpy = { 75, 92 },
 		.pll_op_prediv = { 2, 2 }
 	},
 	{
@@ -154,6 +149,7 @@ static const struct imx355_clk_params imx355_clk_params[] = {
 };
 
 struct imx355_hwcfg {
+	s64 link_freq_menu;
 	unsigned long link_freq_bitmap;
 	unsigned int num_lanes;
 };
@@ -241,7 +237,7 @@ static const struct cci_reg_sequence imx355_global_regs[] = {
 	{ CCI_REG8(0x305a), 0x00 },
 	{ CCI_REG8(0x0112), 0x0a },
 	{ CCI_REG8(0x0113), 0x0a },
-	{ CCI_REG8(0x0301), 0x05 },
+	{ IMX355_REG_PLL_IVT_PCK_DIV, IMX355_PLL_IVT_PCK_DIV },
 	{ CCI_REG8(0x0305), 0x02 },
 	{ CCI_REG8(0x0220), 0x00 },
 	{ CCI_REG8(0x0222), 0x01 },
@@ -333,22 +329,6 @@ static const char * const imx355_test_pattern_menu[] = {
 	"Pseudorandom Sequence (PN9)",
 };
 
-/*
- * When adding more than the one below, make sure the disallowed ones will
- * actually be disabled in the LINK_FREQ control.
- */
-static const s64 link_freq_menu_items_4lane[] = {
-	IMX355_LINK_FREQ_4LANE,
-};
-
-static const s64 link_freq_menu_items_2lane[] = {
-	IMX355_LINK_FREQ_2LANE,
-};
-
-/* Avoid the two arrays getting out of sync */
-static_assert(ARRAY_SIZE(link_freq_menu_items_4lane) ==
-				ARRAY_SIZE(link_freq_menu_items_2lane));
-
 /* Mode configs */
 static const struct imx355_mode supported_modes[] = {
 	{
@@ -1032,33 +1012,30 @@ static int imx355_init_controls(struct imx355 *imx355)
 {
 	struct v4l2_fwnode_device_properties props;
 	struct v4l2_ctrl_handler *ctrl_hdlr;
+	int lane_idx = imx355->hwcfg->num_lanes == 4 ? 0 : 1;
 	const struct imx355_mode *mode = &supported_modes[0];
+	const struct imx355_clk_params *clk = imx355->clk_params;
 	s64 exposure_max;
 	s64 vblank_def;
 	s64 hblank;
-	const s64 *link_freq_menu;
 	s64 pixel_rate;
-	u32 max;
 	int ret;
 
 	ctrl_hdlr = &imx355->ctrl_handler;
 	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
 	if (ret)
 		return ret;
 
-	link_freq_menu = imx355->hwcfg->num_lanes == 4 ?
-				link_freq_menu_items_4lane :
-				link_freq_menu_items_2lane;
-	max = ARRAY_SIZE(link_freq_menu_items_4lane) - 1;
 	imx355->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx355_ctrl_ops,
-						   V4L2_CID_LINK_FREQ, max, 0,
-						   link_freq_menu);
+						   V4L2_CID_LINK_FREQ, 0, 0,
+						   &imx355->hwcfg->link_freq_menu);
 	if (imx355->link_freq)
 		imx355->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
 	/* By default, PIXEL_RATE is read only */
-	pixel_rate = imx355->hwcfg->num_lanes == 4 ? IMX355_PIXEL_RATE_4LANE :
-						     IMX355_PIXEL_RATE_2LANE;
+	pixel_rate = ((s64)clk->ext_clk * clk->pll_op_mpy[lane_idx] * 2) /
+		     (IMX355_PLL_IVT_PCK_DIV * clk->pll_op_prediv[lane_idx] *
+		      (lane_idx + 1));
 	v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops, V4L2_CID_PIXEL_RATE,
 			  pixel_rate, pixel_rate, 1, pixel_rate);
 
@@ -1130,14 +1107,17 @@ static int imx355_init_controls(struct imx355 *imx355)
 	return ret;
 }
 
-static struct imx355_hwcfg *imx355_get_hwcfg(struct device *dev)
+static struct imx355_hwcfg *imx355_get_hwcfg(struct imx355 *imx355)
 {
+	struct device *dev = imx355->dev;
 	struct imx355_hwcfg *cfg;
 	struct v4l2_fwnode_endpoint bus_cfg = {
 		.bus_type = V4L2_MBUS_CSI2_DPHY
 	};
 	struct fwnode_handle *ep;
 	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	const struct imx355_clk_params *clk = imx355->clk_params;
+	int lane_idx;
 	int ret;
 
 	if (!fwnode)
@@ -1161,12 +1141,12 @@ static struct imx355_hwcfg *imx355_get_hwcfg(struct device *dev)
 
 	cfg->num_lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
 
+	lane_idx = cfg->num_lanes == 4 ? 0 : 1;
+	cfg->link_freq_menu = (clk->ext_clk * clk->pll_op_mpy[lane_idx]) /
+				 (clk->pll_op_prediv[lane_idx] * 2);
 	ret = v4l2_link_freq_to_bitmap(dev, bus_cfg.link_frequencies,
 				       bus_cfg.nr_of_link_frequencies,
-				       cfg->num_lanes == 4 ?
-						link_freq_menu_items_4lane :
-						link_freq_menu_items_2lane,
-				       ARRAY_SIZE(link_freq_menu_items_4lane),
+				       &cfg->link_freq_menu, 1,
 				       &cfg->link_freq_bitmap);
 	if (ret)
 		goto out_err;
@@ -1236,7 +1216,7 @@ static int imx355_probe(struct i2c_client *client)
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&imx355->sd, client, &imx355_subdev_ops);
 
-	imx355->hwcfg = imx355_get_hwcfg(imx355->dev);
+	imx355->hwcfg = imx355_get_hwcfg(imx355);
 	if (!imx355->hwcfg) {
 		dev_err(imx355->dev, "failed to get hwcfg");
 		return -ENODEV;