media: i2c: imx258: Make HFLIP and VFLIP controls writable
[platform/kernel/linux-rpi.git] / drivers / media / i2c / imx258.c
index e196565..e30e14d 100644 (file)
@@ -7,6 +7,7 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
@@ -19,6 +20,8 @@
 #define IMX258_MODE_STANDBY            0x00
 #define IMX258_MODE_STREAMING          0x01
 
+#define IMX258_REG_RESET               0x0103
+
 /* Chip ID */
 #define IMX258_REG_CHIP_ID             0x0016
 #define IMX258_CHIP_ID                 0x0258
 #define IMX258_VTS_30FPS               0x0c50
 #define IMX258_VTS_30FPS_2K            0x0638
 #define IMX258_VTS_30FPS_VGA           0x034c
-#define IMX258_VTS_MAX                 0xffff
+#define IMX258_VTS_MAX                 65525
 
-/*Frame Length Line*/
-#define IMX258_FLL_MIN                 0x08a6
-#define IMX258_FLL_MAX                 0xffff
-#define IMX258_FLL_STEP                        1
-#define IMX258_FLL_DEFAULT             0x0c98
-
-/* HBLANK control - read only */
-#define IMX258_PPL_DEFAULT             5352
+#define IMX258_REG_VTS                 0x0340
 
 /* Exposure control */
 #define IMX258_REG_EXPOSURE            0x0202
+#define IMX258_EXPOSURE_OFFSET         10
 #define IMX258_EXPOSURE_MIN            4
 #define IMX258_EXPOSURE_STEP           1
 #define IMX258_EXPOSURE_DEFAULT                0x640
-#define IMX258_EXPOSURE_MAX            65535
+#define IMX258_EXPOSURE_MAX            (IMX258_VTS_MAX - IMX258_EXPOSURE_OFFSET)
+
+/* HBLANK control - read only */
+#define IMX258_PPL_DEFAULT             5352
 
 /* Analog gain control */
 #define IMX258_REG_ANALOG_GAIN         0x0204
 #define IMX258_HDR_RATIO_STEP          1
 #define IMX258_HDR_RATIO_DEFAULT       0x0
 
+/* Long exposure multiplier */
+#define IMX258_LONG_EXP_SHIFT_MAX      7
+#define IMX258_LONG_EXP_SHIFT_REG      0x3002
+
 /* Test Pattern Control */
 #define IMX258_REG_TEST_PATTERN                0x0600
 
+#define IMX258_CLK_BLANK_STOP          0x4040
+
 /* Orientation */
 #define REG_MIRROR_FLIP_CONTROL                0x0101
-#define REG_CONFIG_MIRROR_FLIP         0x03
+#define REG_CONFIG_MIRROR_HFLIP                0x01
+#define REG_CONFIG_MIRROR_VFLIP                0x02
 #define REG_CONFIG_FLIP_TEST_PATTERN   0x02
 
-/* Input clock frequency in Hz */
-#define IMX258_INPUT_CLOCK_FREQ                19200000
+/* IMX258 native and active pixel array size. */
+#define IMX258_NATIVE_WIDTH            4224U
+#define IMX258_NATIVE_HEIGHT           3192U
+#define IMX258_PIXEL_ARRAY_LEFT                8U
+#define IMX258_PIXEL_ARRAY_TOP         16U
+#define IMX258_PIXEL_ARRAY_WIDTH       4208U
+#define IMX258_PIXEL_ARRAY_HEIGHT      3120U
 
 struct imx258_reg {
        u16 address;
@@ -92,12 +104,22 @@ struct imx258_reg_list {
        const struct imx258_reg *regs;
 };
 
+struct imx258_link_cfg {
+       unsigned int lf_to_pix_rate_factor;
+       struct imx258_reg_list reg_list;
+};
+
+#define IMX258_LANE_CONFIGS    2
+#define IMX258_2_LANE_MODE     0
+#define IMX258_4_LANE_MODE     1
+
 /* Link frequency config */
 struct imx258_link_freq_config {
+       u64 link_frequency;
        u32 pixels_per_line;
 
-       /* PLL registers for this link frequency */
-       struct imx258_reg_list reg_list;
+       /* Configuration for this link frequency / num lanes selection */
+       struct imx258_link_cfg link_cfg[IMX258_LANE_CONFIGS];
 };
 
 /* Mode : resolution and related config&values */
@@ -115,10 +137,37 @@ struct imx258_mode {
        u32 link_freq_index;
        /* Default register values */
        struct imx258_reg_list reg_list;
+
+       /* Analog crop rectangle. */
+       struct v4l2_rect crop;
+};
+
+/* 4208x3120 needs 1267Mbps/lane, 4 lanes. Use that rate on 2 lanes as well */
+static const struct imx258_reg mipi_1267mbps_19_2mhz_2l[] = {
+       { 0x0136, 0x13 },
+       { 0x0137, 0x33 },
+       { 0x0301, 0x0A },
+       { 0x0303, 0x02 },
+       { 0x0305, 0x03 },
+       { 0x0306, 0x00 },
+       { 0x0307, 0xC6 },
+       { 0x0309, 0x0A },
+       { 0x030B, 0x01 },
+       { 0x030D, 0x02 },
+       { 0x030E, 0x00 },
+       { 0x030F, 0xD8 },
+       { 0x0310, 0x00 },
+
+       { 0x0114, 0x01 },
+       { 0x0820, 0x09 },
+       { 0x0821, 0xa6 },
+       { 0x0822, 0x66 },
+       { 0x0823, 0x66 },
 };
 
-/* 4208x3118 needs 1267Mbps/lane, 4 lanes */
-static const struct imx258_reg mipi_data_rate_1267mbps[] = {
+static const struct imx258_reg mipi_1267mbps_19_2mhz_4l[] = {
+       { 0x0136, 0x13 },
+       { 0x0137, 0x33 },
        { 0x0301, 0x05 },
        { 0x0303, 0x02 },
        { 0x0305, 0x03 },
@@ -130,13 +179,61 @@ static const struct imx258_reg mipi_data_rate_1267mbps[] = {
        { 0x030E, 0x00 },
        { 0x030F, 0xD8 },
        { 0x0310, 0x00 },
+
+       { 0x0114, 0x03 },
        { 0x0820, 0x13 },
        { 0x0821, 0x4C },
        { 0x0822, 0xCC },
        { 0x0823, 0xCC },
 };
 
-static const struct imx258_reg mipi_data_rate_640mbps[] = {
+static const struct imx258_reg mipi_1272mbps_24mhz_2l[] = {
+       { 0x0136, 0x18 },
+       { 0x0137, 0x00 },
+       { 0x0301, 0x0a },
+       { 0x0303, 0x02 },
+       { 0x0305, 0x04 },
+       { 0x0306, 0x00 },
+       { 0x0307, 0xD4 },
+       { 0x0309, 0x0A },
+       { 0x030B, 0x01 },
+       { 0x030D, 0x02 },
+       { 0x030E, 0x00 },
+       { 0x030F, 0xD8 },
+       { 0x0310, 0x00 },
+
+       { 0x0114, 0x01 },
+       { 0x0820, 0x13 },
+       { 0x0821, 0x4C },
+       { 0x0822, 0xCC },
+       { 0x0823, 0xCC },
+};
+
+static const struct imx258_reg mipi_1272mbps_24mhz_4l[] = {
+       { 0x0136, 0x18 },
+       { 0x0137, 0x00 },
+       { 0x0301, 0x05 },
+       { 0x0303, 0x02 },
+       { 0x0305, 0x04 },
+       { 0x0306, 0x00 },
+       { 0x0307, 0xD4 },
+       { 0x0309, 0x0A },
+       { 0x030B, 0x01 },
+       { 0x030D, 0x02 },
+       { 0x030E, 0x00 },
+       { 0x030F, 0xD8 },
+       { 0x0310, 0x00 },
+
+       { 0x0114, 0x03 },
+       { 0x0820, 0x13 },
+       { 0x0821, 0xE0 },
+       { 0x0822, 0x00 },
+       { 0x0823, 0x00 },
+};
+
+static const struct imx258_reg mipi_640mbps_19_2mhz_2l[] = {
+       { 0x0136, 0x13 },
+       { 0x0137, 0x33 },
        { 0x0301, 0x05 },
        { 0x0303, 0x02 },
        { 0x0305, 0x03 },
@@ -148,18 +245,82 @@ static const struct imx258_reg mipi_data_rate_640mbps[] = {
        { 0x030E, 0x00 },
        { 0x030F, 0xD8 },
        { 0x0310, 0x00 },
-       { 0x0820, 0x0A },
+
+       { 0x0114, 0x01 },
+       { 0x0820, 0x05 },
        { 0x0821, 0x00 },
        { 0x0822, 0x00 },
        { 0x0823, 0x00 },
 };
 
-static const struct imx258_reg mode_4208x3118_regs[] = {
+static const struct imx258_reg mipi_640mbps_19_2mhz_4l[] = {
        { 0x0136, 0x13 },
        { 0x0137, 0x33 },
+       { 0x0301, 0x05 },
+       { 0x0303, 0x02 },
+       { 0x0305, 0x03 },
+       { 0x0306, 0x00 },
+       { 0x0307, 0x64 },
+       { 0x0309, 0x0A },
+       { 0x030B, 0x01 },
+       { 0x030D, 0x02 },
+       { 0x030E, 0x00 },
+       { 0x030F, 0xD8 },
+       { 0x0310, 0x00 },
+
+       { 0x0114, 0x03 },
+       { 0x0820, 0x0A },
+       { 0x0821, 0x00 },
+       { 0x0822, 0x00 },
+       { 0x0823, 0x00 },
+};
+
+static const struct imx258_reg mipi_642mbps_24mhz_2l[] = {
+       { 0x0136, 0x18 },
+       { 0x0137, 0x00 },
+       { 0x0301, 0x05 },
+       { 0x0303, 0x02 },
+       { 0x0305, 0x04 },
+       { 0x0306, 0x00 },
+       { 0x0307, 0x6B },
+       { 0x0309, 0x0A },
+       { 0x030B, 0x01 },
+       { 0x030D, 0x02 },
+       { 0x030E, 0x00 },
+       { 0x030F, 0xD8 },
+       { 0x0310, 0x00 },
+
+       { 0x0114, 0x01 },
+       { 0x0820, 0x0A },
+       { 0x0821, 0x00 },
+       { 0x0822, 0x00 },
+       { 0x0823, 0x00 },
+};
+
+static const struct imx258_reg mipi_642mbps_24mhz_4l[] = {
+       { 0x0136, 0x18 },
+       { 0x0137, 0x00 },
+       { 0x0301, 0x05 },
+       { 0x0303, 0x02 },
+       { 0x0305, 0x04 },
+       { 0x0306, 0x00 },
+       { 0x0307, 0x6B },
+       { 0x0309, 0x0A },
+       { 0x030B, 0x01 },
+       { 0x030D, 0x02 },
+       { 0x030E, 0x00 },
+       { 0x030F, 0xD8 },
+       { 0x0310, 0x00 },
+
+       { 0x0114, 0x03 },
+       { 0x0820, 0x0A },
+       { 0x0821, 0x00 },
+       { 0x0822, 0x00 },
+       { 0x0823, 0x00 },
+};
+
+static const struct imx258_reg mode_4208x3120_regs[] = {
        { 0x3051, 0x00 },
-       { 0x3052, 0x00 },
-       { 0x4E21, 0x14 },
        { 0x6B11, 0xCF },
        { 0x7FF0, 0x08 },
        { 0x7FF1, 0x0F },
@@ -182,7 +343,6 @@ static const struct imx258_reg mode_4208x3118_regs[] = {
        { 0x7FA8, 0x03 },
        { 0x7FA9, 0xFE },
        { 0x7B24, 0x81 },
-       { 0x7B25, 0x00 },
        { 0x6564, 0x07 },
        { 0x6B0D, 0x41 },
        { 0x653D, 0x04 },
@@ -204,11 +364,8 @@ static const struct imx258_reg mode_4208x3118_regs[] = {
        { 0x5F05, 0xED },
        { 0x0112, 0x0A },
        { 0x0113, 0x0A },
-       { 0x0114, 0x03 },
        { 0x0342, 0x14 },
        { 0x0343, 0xE8 },
-       { 0x0340, 0x0C },
-       { 0x0341, 0x50 },
        { 0x0344, 0x00 },
        { 0x0345, 0x00 },
        { 0x0346, 0x00 },
@@ -216,7 +373,7 @@ static const struct imx258_reg mode_4208x3118_regs[] = {
        { 0x0348, 0x10 },
        { 0x0349, 0x6F },
        { 0x034A, 0x0C },
-       { 0x034B, 0x2E },
+       { 0x034B, 0x2F },
        { 0x0381, 0x01 },
        { 0x0383, 0x01 },
        { 0x0385, 0x01 },
@@ -242,9 +399,7 @@ static const struct imx258_reg mode_4208x3118_regs[] = {
        { 0x034D, 0x70 },
        { 0x034E, 0x0C },
        { 0x034F, 0x30 },
-       { 0x0350, 0x01 },
-       { 0x0202, 0x0C },
-       { 0x0203, 0x46 },
+       { 0x0350, 0x00 },
        { 0x0204, 0x00 },
        { 0x0205, 0x00 },
        { 0x020E, 0x01 },
@@ -274,11 +429,7 @@ static const struct imx258_reg mode_4208x3118_regs[] = {
 };
 
 static const struct imx258_reg mode_2104_1560_regs[] = {
-       { 0x0136, 0x13 },
-       { 0x0137, 0x33 },
        { 0x3051, 0x00 },
-       { 0x3052, 0x00 },
-       { 0x4E21, 0x14 },
        { 0x6B11, 0xCF },
        { 0x7FF0, 0x08 },
        { 0x7FF1, 0x0F },
@@ -301,7 +452,6 @@ static const struct imx258_reg mode_2104_1560_regs[] = {
        { 0x7FA8, 0x03 },
        { 0x7FA9, 0xFE },
        { 0x7B24, 0x81 },
-       { 0x7B25, 0x00 },
        { 0x6564, 0x07 },
        { 0x6B0D, 0x41 },
        { 0x653D, 0x04 },
@@ -323,11 +473,8 @@ static const struct imx258_reg mode_2104_1560_regs[] = {
        { 0x5F05, 0xED },
        { 0x0112, 0x0A },
        { 0x0113, 0x0A },
-       { 0x0114, 0x03 },
        { 0x0342, 0x14 },
        { 0x0343, 0xE8 },
-       { 0x0340, 0x06 },
-       { 0x0341, 0x38 },
        { 0x0344, 0x00 },
        { 0x0345, 0x00 },
        { 0x0346, 0x00 },
@@ -335,7 +482,7 @@ static const struct imx258_reg mode_2104_1560_regs[] = {
        { 0x0348, 0x10 },
        { 0x0349, 0x6F },
        { 0x034A, 0x0C },
-       { 0x034B, 0x2E },
+       { 0x034B, 0x2F },
        { 0x0381, 0x01 },
        { 0x0383, 0x01 },
        { 0x0385, 0x01 },
@@ -346,11 +493,11 @@ static const struct imx258_reg mode_2104_1560_regs[] = {
        { 0x0404, 0x00 },
        { 0x0405, 0x20 },
        { 0x0408, 0x00 },
-       { 0x0409, 0x02 },
+       { 0x0409, 0x00 },
        { 0x040A, 0x00 },
        { 0x040B, 0x00 },
        { 0x040C, 0x10 },
-       { 0x040D, 0x6A },
+       { 0x040D, 0x70 },
        { 0x040E, 0x06 },
        { 0x040F, 0x18 },
        { 0x3038, 0x00 },
@@ -361,9 +508,7 @@ static const struct imx258_reg mode_2104_1560_regs[] = {
        { 0x034D, 0x38 },
        { 0x034E, 0x06 },
        { 0x034F, 0x18 },
-       { 0x0350, 0x01 },
-       { 0x0202, 0x06 },
-       { 0x0203, 0x2E },
+       { 0x0350, 0x00 },
        { 0x0204, 0x00 },
        { 0x0205, 0x00 },
        { 0x020E, 0x01 },
@@ -393,11 +538,7 @@ static const struct imx258_reg mode_2104_1560_regs[] = {
 };
 
 static const struct imx258_reg mode_1048_780_regs[] = {
-       { 0x0136, 0x13 },
-       { 0x0137, 0x33 },
        { 0x3051, 0x00 },
-       { 0x3052, 0x00 },
-       { 0x4E21, 0x14 },
        { 0x6B11, 0xCF },
        { 0x7FF0, 0x08 },
        { 0x7FF1, 0x0F },
@@ -420,7 +561,6 @@ static const struct imx258_reg mode_1048_780_regs[] = {
        { 0x7FA8, 0x03 },
        { 0x7FA9, 0xFE },
        { 0x7B24, 0x81 },
-       { 0x7B25, 0x00 },
        { 0x6564, 0x07 },
        { 0x6B0D, 0x41 },
        { 0x653D, 0x04 },
@@ -442,11 +582,8 @@ static const struct imx258_reg mode_1048_780_regs[] = {
        { 0x5F05, 0xED },
        { 0x0112, 0x0A },
        { 0x0113, 0x0A },
-       { 0x0114, 0x03 },
        { 0x0342, 0x14 },
        { 0x0343, 0xE8 },
-       { 0x0340, 0x03 },
-       { 0x0341, 0x4C },
        { 0x0344, 0x00 },
        { 0x0345, 0x00 },
        { 0x0346, 0x00 },
@@ -454,7 +591,7 @@ static const struct imx258_reg mode_1048_780_regs[] = {
        { 0x0348, 0x10 },
        { 0x0349, 0x6F },
        { 0x034A, 0x0C },
-       { 0x034B, 0x2E },
+       { 0x034B, 0x2F },
        { 0x0381, 0x01 },
        { 0x0383, 0x01 },
        { 0x0385, 0x01 },
@@ -465,11 +602,11 @@ static const struct imx258_reg mode_1048_780_regs[] = {
        { 0x0404, 0x00 },
        { 0x0405, 0x40 },
        { 0x0408, 0x00 },
-       { 0x0409, 0x06 },
+       { 0x0409, 0x00 },
        { 0x040A, 0x00 },
        { 0x040B, 0x00 },
        { 0x040C, 0x10 },
-       { 0x040D, 0x64 },
+       { 0x040D, 0x70 },
        { 0x040E, 0x03 },
        { 0x040F, 0x0C },
        { 0x3038, 0x00 },
@@ -480,9 +617,7 @@ static const struct imx258_reg mode_1048_780_regs[] = {
        { 0x034D, 0x18 },
        { 0x034E, 0x03 },
        { 0x034F, 0x0C },
-       { 0x0350, 0x01 },
-       { 0x0202, 0x03 },
-       { 0x0203, 0x42 },
+       { 0x0350, 0x00 },
        { 0x0204, 0x00 },
        { 0x0205, 0x00 },
        { 0x020E, 0x01 },
@@ -511,6 +646,50 @@ static const struct imx258_reg mode_1048_780_regs[] = {
        { 0x0220, 0x00 },
 };
 
+struct imx258_variant_cfg {
+       const struct imx258_reg *regs;
+       unsigned int num_regs;
+};
+
+static const struct imx258_reg imx258_cfg_regs[] = {
+       { 0x3052, 0x00 },
+       { 0x4E21, 0x14 },
+       { 0x7B25, 0x00 },
+};
+
+static const struct imx258_variant_cfg imx258_cfg = {
+       .regs = imx258_cfg_regs,
+       .num_regs = ARRAY_SIZE(imx258_cfg_regs),
+};
+
+static const struct imx258_reg imx258_pdaf_cfg_regs[] = {
+       { 0x3052, 0x01 },
+       { 0x4E21, 0x10 },
+       { 0x7B25, 0x01 },
+};
+
+static const struct imx258_variant_cfg imx258_pdaf_cfg = {
+       .regs = imx258_pdaf_cfg_regs,
+       .num_regs = ARRAY_SIZE(imx258_pdaf_cfg_regs),
+};
+
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 codes[] = {
+       /* 10-bit modes. */
+       MEDIA_BUS_FMT_SRGGB10_1X10,
+       MEDIA_BUS_FMT_SGRBG10_1X10,
+       MEDIA_BUS_FMT_SGBRG10_1X10,
+       MEDIA_BUS_FMT_SBGGR10_1X10
+};
+
 static const char * const imx258_test_pattern_menu[] = {
        "Disabled",
        "Solid Colour",
@@ -519,9 +698,15 @@ static const char * const imx258_test_pattern_menu[] = {
        "Pseudorandom Sequence (PN9)",
 };
 
-/* Configurations for supported link frequencies */
-#define IMX258_LINK_FREQ_634MHZ        633600000ULL
-#define IMX258_LINK_FREQ_320MHZ        320000000ULL
+/* regulator supplies */
+static const char * const imx258_supply_name[] = {
+       /* Supplies can be enabled in any order */
+       "vana",  /* Analog (2.8V) supply */
+       "vdig",  /* Digital Core (1.05V) supply */
+       "vif",  /* IF (1.8V) supply */
+};
+
+#define IMX258_NUM_SUPPLIES ARRAY_SIZE(imx258_supply_name)
 
 enum {
        IMX258_LINK_FREQ_1267MBPS,
@@ -529,37 +714,103 @@ enum {
 };
 
 /*
- * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
- * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ * Pixel rate does not necessarily relate to link frequency on this sensor as
+ * there is a FIFO between the pixel array pipeline and the MIPI serializer.
+ * The recommendation from Sony is that the pixel array is always run with a
+ * line length of 5352 pixels, which means that there is a large amount of
+ * blanking time for the 1048x780 mode. There is no need to replicate this
+ * blanking on the CSI2 bus, and the configuration of register 0x0301 allows the
+ * divider to be altered.
+ *
+ * The actual factor between link frequency and pixel rate is in the
+ * imx258_link_cfg, so use this to convert between the two.
+ * bits per pixel being 10, and D-PHY being DDR is assumed by this function, so
+ * the value is only the combination of number of lanes and pixel clock divider.
  */
-static u64 link_freq_to_pixel_rate(u64 f)
+static u64 link_freq_to_pixel_rate(u64 f, const struct imx258_link_cfg *link_cfg)
 {
-       f *= 2 * 4;
+       f *= 2 * link_cfg->lf_to_pix_rate_factor;
        do_div(f, 10);
 
        return f;
 }
 
 /* Menu items for LINK_FREQ V4L2 control */
-static const s64 link_freq_menu_items[] = {
+/* Configurations for supported link frequencies */
+#define IMX258_LINK_FREQ_634MHZ        633600000ULL
+#define IMX258_LINK_FREQ_320MHZ        320000000ULL
+
+static const s64 link_freq_menu_items_19_2[] = {
        IMX258_LINK_FREQ_634MHZ,
        IMX258_LINK_FREQ_320MHZ,
 };
 
+/* Configurations for supported link frequencies */
+#define IMX258_LINK_FREQ_636MHZ        636000000ULL
+#define IMX258_LINK_FREQ_321MHZ        321000000ULL
+
+static const s64 link_freq_menu_items_24[] = {
+       IMX258_LINK_FREQ_636MHZ,
+       IMX258_LINK_FREQ_321MHZ,
+};
+
+#define REGS(_list) { .num_of_regs = ARRAY_SIZE(_list), .regs = _list, }
+
 /* Link frequency configs */
-static const struct imx258_link_freq_config link_freq_configs[] = {
+static const struct imx258_link_freq_config link_freq_configs_19_2[] = {
        [IMX258_LINK_FREQ_1267MBPS] = {
                .pixels_per_line = IMX258_PPL_DEFAULT,
-               .reg_list = {
-                       .num_of_regs = ARRAY_SIZE(mipi_data_rate_1267mbps),
-                       .regs = mipi_data_rate_1267mbps,
+               .link_cfg = {
+                       [IMX258_2_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 2 * 2,
+                               .reg_list = REGS(mipi_1267mbps_19_2mhz_2l),
+                       },
+                       [IMX258_4_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 4,
+                               .reg_list = REGS(mipi_1267mbps_19_2mhz_4l),
+                       },
                }
        },
        [IMX258_LINK_FREQ_640MBPS] = {
                .pixels_per_line = IMX258_PPL_DEFAULT,
-               .reg_list = {
-                       .num_of_regs = ARRAY_SIZE(mipi_data_rate_640mbps),
-                       .regs = mipi_data_rate_640mbps,
+               .link_cfg = {
+                       [IMX258_2_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 2,
+                               .reg_list = REGS(mipi_640mbps_19_2mhz_2l),
+                       },
+                       [IMX258_4_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 4,
+                               .reg_list = REGS(mipi_640mbps_19_2mhz_4l),
+                       },
+               }
+       },
+};
+
+static const struct imx258_link_freq_config link_freq_configs_24[] = {
+       [IMX258_LINK_FREQ_1267MBPS] = {
+               .pixels_per_line = IMX258_PPL_DEFAULT,
+               .link_cfg = {
+                       [IMX258_2_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 2,
+                               .reg_list = REGS(mipi_1272mbps_24mhz_2l),
+                       },
+                       [IMX258_4_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 4,
+                               .reg_list = REGS(mipi_1272mbps_24mhz_4l),
+                       },
+               }
+       },
+       [IMX258_LINK_FREQ_640MBPS] = {
+               .pixels_per_line = IMX258_PPL_DEFAULT,
+               .link_cfg = {
+                       [IMX258_2_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 2 * 2,
+                               .reg_list = REGS(mipi_642mbps_24mhz_2l),
+                       },
+                       [IMX258_4_LANE_MODE] = {
+                               .lf_to_pix_rate_factor = 4,
+                               .reg_list = REGS(mipi_642mbps_24mhz_4l),
+                       },
                }
        },
 };
@@ -568,14 +819,20 @@ static const struct imx258_link_freq_config link_freq_configs[] = {
 static const struct imx258_mode supported_modes[] = {
        {
                .width = 4208,
-               .height = 3118,
+               .height = 3120,
                .vts_def = IMX258_VTS_30FPS,
                .vts_min = IMX258_VTS_30FPS,
                .reg_list = {
-                       .num_of_regs = ARRAY_SIZE(mode_4208x3118_regs),
-                       .regs = mode_4208x3118_regs,
+                       .num_of_regs = ARRAY_SIZE(mode_4208x3120_regs),
+                       .regs = mode_4208x3120_regs,
                },
                .link_freq_index = IMX258_LINK_FREQ_1267MBPS,
+               .crop = {
+                       .left = IMX258_PIXEL_ARRAY_LEFT,
+                       .top = IMX258_PIXEL_ARRAY_TOP,
+                       .width = 4208,
+                       .height = 3120,
+               },
        },
        {
                .width = 2104,
@@ -587,6 +844,12 @@ static const struct imx258_mode supported_modes[] = {
                        .regs = mode_2104_1560_regs,
                },
                .link_freq_index = IMX258_LINK_FREQ_640MBPS,
+               .crop = {
+                       .left = IMX258_PIXEL_ARRAY_LEFT,
+                       .top = IMX258_PIXEL_ARRAY_TOP,
+                       .width = 4208,
+                       .height = 3120,
+               },
        },
        {
                .width = 1048,
@@ -598,6 +861,12 @@ static const struct imx258_mode supported_modes[] = {
                        .regs = mode_1048_780_regs,
                },
                .link_freq_index = IMX258_LINK_FREQ_640MBPS,
+               .crop = {
+                       .left = IMX258_PIXEL_ARRAY_LEFT,
+                       .top = IMX258_PIXEL_ARRAY_TOP,
+                       .width = 4208,
+                       .height = 3120,
+               },
        },
 };
 
@@ -605,6 +874,8 @@ struct imx258 {
        struct v4l2_subdev sd;
        struct media_pad pad;
 
+       const struct imx258_variant_cfg *variant_cfg;
+
        struct v4l2_ctrl_handler ctrl_handler;
        /* V4L2 Controls */
        struct v4l2_ctrl *link_freq;
@@ -612,10 +883,19 @@ struct imx258 {
        struct v4l2_ctrl *vblank;
        struct v4l2_ctrl *hblank;
        struct v4l2_ctrl *exposure;
+       struct v4l2_ctrl *hflip;
+       struct v4l2_ctrl *vflip;
+       /* Current long exposure factor in use. Set through V4L2_CID_VBLANK */
+       unsigned int long_exp_shift;
 
        /* Current mode */
        const struct imx258_mode *cur_mode;
 
+       const struct imx258_link_freq_config *link_freq_configs;
+       const s64 *link_freq_menu_items;
+       unsigned int lane_mode_idx;
+       unsigned int csi2_flags;
+
        /*
         * Mutex for serialized access:
         * Protect sensor module set pad format and start/stop streaming safely.
@@ -626,6 +906,7 @@ struct imx258 {
        bool streaming;
 
        struct clk *clk;
+       struct regulator_bulk_data supplies[IMX258_NUM_SUPPLIES];
 };
 
 static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
@@ -707,18 +988,39 @@ static int imx258_write_regs(struct imx258 *imx258,
        return 0;
 }
 
+/* Get bayer order based on flip setting. */
+static u32 imx258_get_format_code(struct imx258 *imx258)
+{
+       unsigned int i;
+
+       lockdep_assert_held(&imx258->mutex);
+
+       i = (imx258->vflip->val ? 2 : 0) |
+           (imx258->hflip->val ? 1 : 0);
+
+       return codes[i];
+}
 /* Open sub-device */
 static int imx258_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 {
+       struct imx258 *imx258 = to_imx258(sd);
        struct v4l2_mbus_framefmt *try_fmt =
                v4l2_subdev_get_try_format(sd, fh->state, 0);
+       struct v4l2_rect *try_crop;
 
        /* Initialize try_fmt */
        try_fmt->width = supported_modes[0].width;
        try_fmt->height = supported_modes[0].height;
-       try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+       try_fmt->code = imx258_get_format_code(imx258);
        try_fmt->field = V4L2_FIELD_NONE;
 
+       /* Initialize try_crop */
+       try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
+       try_crop->left = IMX258_PIXEL_ARRAY_LEFT;
+       try_crop->top = IMX258_PIXEL_ARRAY_TOP;
+       try_crop->width = IMX258_PIXEL_ARRAY_WIDTH;
+       try_crop->height = IMX258_PIXEL_ARRAY_HEIGHT;
+
        return 0;
 }
 
@@ -749,6 +1051,39 @@ static int imx258_update_digital_gain(struct imx258 *imx258, u32 len, u32 val)
        return 0;
 }
 
+static void imx258_adjust_exposure_range(struct imx258 *imx258)
+{
+       int exposure_max, exposure_def;
+
+       /* Honour the VBLANK limits when setting exposure. */
+       exposure_max = imx258->cur_mode->height + imx258->vblank->val -
+                      IMX258_EXPOSURE_OFFSET;
+       exposure_def = min(exposure_max, imx258->exposure->val);
+       __v4l2_ctrl_modify_range(imx258->exposure, imx258->exposure->minimum,
+                                exposure_max, imx258->exposure->step,
+                                exposure_def);
+}
+
+static int imx258_set_frame_length(struct imx258 *imx258, unsigned int val)
+{
+       int ret;
+
+       imx258->long_exp_shift = 0;
+
+       while (val > IMX258_VTS_MAX) {
+               imx258->long_exp_shift++;
+               val >>= 1;
+       }
+
+       ret = imx258_write_reg(imx258, IMX258_REG_VTS,
+                              IMX258_REG_VALUE_16BIT, val);
+       if (ret)
+               return ret;
+
+       return imx258_write_reg(imx258, IMX258_LONG_EXP_SHIFT_REG,
+                               IMX258_REG_VALUE_08BIT, imx258->long_exp_shift);
+}
+
 static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
 {
        struct imx258 *imx258 =
@@ -757,6 +1092,13 @@ static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
        int ret = 0;
 
        /*
+        * The VBLANK control may change the limits of usable exposure, so check
+        * and adjust if necessary.
+        */
+       if (ctrl->id == V4L2_CID_VBLANK)
+               imx258_adjust_exposure_range(imx258);
+
+       /*
         * Applying V4L2 control value only happens
         * when power is up for streaming
         */
@@ -772,7 +1114,7 @@ static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
        case V4L2_CID_EXPOSURE:
                ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
                                IMX258_REG_VALUE_16BIT,
-                               ctrl->val);
+                               ctrl->val >> imx258->long_exp_shift);
                break;
        case V4L2_CID_DIGITAL_GAIN:
                ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
@@ -782,10 +1124,6 @@ static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
                ret = imx258_write_reg(imx258, IMX258_REG_TEST_PATTERN,
                                IMX258_REG_VALUE_16BIT,
                                ctrl->val);
-               ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
-                               IMX258_REG_VALUE_08BIT,
-                               !ctrl->val ? REG_CONFIG_MIRROR_FLIP :
-                               REG_CONFIG_FLIP_TEST_PATTERN);
                break;
        case V4L2_CID_WIDE_DYNAMIC_RANGE:
                if (!ctrl->val) {
@@ -803,6 +1141,19 @@ static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
                                               BIT(IMX258_HDR_RATIO_MAX));
                }
                break;
+       case V4L2_CID_VBLANK:
+               ret = imx258_set_frame_length(imx258,
+                                             imx258->cur_mode->height + ctrl->val);
+               break;
+       case V4L2_CID_VFLIP:
+       case V4L2_CID_HFLIP:
+               ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
+                                      IMX258_REG_VALUE_08BIT,
+                                      (imx258->hflip->val ?
+                                       REG_CONFIG_MIRROR_HFLIP : 0) |
+                                      (imx258->vflip->val ?
+                                       REG_CONFIG_MIRROR_VFLIP : 0));
+               break;
        default:
                dev_info(&client->dev,
                         "ctrl(id:0x%x,val:0x%x) is not handled\n",
@@ -824,11 +1175,13 @@ static int imx258_enum_mbus_code(struct v4l2_subdev *sd,
                                  struct v4l2_subdev_state *sd_state,
                                  struct v4l2_subdev_mbus_code_enum *code)
 {
-       /* Only one bayer order(GRBG) is supported */
+       struct imx258 *imx258 = to_imx258(sd);
+
+       /* Only one bayer format (10 bit) is supported */
        if (code->index > 0)
                return -EINVAL;
 
-       code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+       code->code = imx258_get_format_code(imx258);
 
        return 0;
 }
@@ -837,10 +1190,11 @@ static int imx258_enum_frame_size(struct v4l2_subdev *sd,
                                  struct v4l2_subdev_state *sd_state,
                                  struct v4l2_subdev_frame_size_enum *fse)
 {
+       struct imx258 *imx258 = to_imx258(sd);
        if (fse->index >= ARRAY_SIZE(supported_modes))
                return -EINVAL;
 
-       if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+       if (fse->code != imx258_get_format_code(imx258))
                return -EINVAL;
 
        fse->min_width = supported_modes[fse->index].width;
@@ -851,12 +1205,13 @@ static int imx258_enum_frame_size(struct v4l2_subdev *sd,
        return 0;
 }
 
-static void imx258_update_pad_format(const struct imx258_mode *mode,
+static void imx258_update_pad_format(struct imx258 *imx258,
+                                    const struct imx258_mode *mode,
                                     struct v4l2_subdev_format *fmt)
 {
        fmt->format.width = mode->width;
        fmt->format.height = mode->height;
-       fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+       fmt->format.code = imx258_get_format_code(imx258);
        fmt->format.field = V4L2_FIELD_NONE;
 }
 
@@ -869,7 +1224,7 @@ static int __imx258_get_pad_format(struct imx258 *imx258,
                                                          sd_state,
                                                          fmt->pad);
        else
-               imx258_update_pad_format(imx258->cur_mode, fmt);
+               imx258_update_pad_format(imx258, imx258->cur_mode, fmt);
 
        return 0;
 }
@@ -893,8 +1248,10 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
                                 struct v4l2_subdev_format *fmt)
 {
        struct imx258 *imx258 = to_imx258(sd);
-       const struct imx258_mode *mode;
+       const struct imx258_link_freq_config *link_freq_cfgs;
+       const struct imx258_link_cfg *link_cfg;
        struct v4l2_mbus_framefmt *framefmt;
+       const struct imx258_mode *mode;
        s32 vblank_def;
        s32 vblank_min;
        s64 h_blank;
@@ -903,13 +1260,12 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
 
        mutex_lock(&imx258->mutex);
 
-       /* Only one raw bayer(GBRG) order is supported */
-       fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+       fmt->format.code = imx258_get_format_code(imx258);
 
        mode = v4l2_find_nearest_size(supported_modes,
                ARRAY_SIZE(supported_modes), width, height,
                fmt->format.width, fmt->format.height);
-       imx258_update_pad_format(mode, fmt);
+       imx258_update_pad_format(imx258, mode, fmt);
        if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
                framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
                *framefmt = fmt->format;
@@ -917,9 +1273,14 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
                imx258->cur_mode = mode;
                __v4l2_ctrl_s_ctrl(imx258->link_freq, mode->link_freq_index);
 
-               link_freq = link_freq_menu_items[mode->link_freq_index];
-               pixel_rate = link_freq_to_pixel_rate(link_freq);
-               __v4l2_ctrl_s_ctrl_int64(imx258->pixel_rate, pixel_rate);
+               link_freq = imx258->link_freq_menu_items[mode->link_freq_index];
+               link_freq_cfgs =
+                       &imx258->link_freq_configs[mode->link_freq_index];
+
+               link_cfg = &link_freq_cfgs->link_cfg[imx258->lane_mode_idx];
+               pixel_rate = link_freq_to_pixel_rate(link_freq, link_cfg);
+               __v4l2_ctrl_modify_range(imx258->pixel_rate, pixel_rate,
+                                        pixel_rate, 1, pixel_rate);
                /* Update limits and set FPS to default */
                vblank_def = imx258->cur_mode->vts_def -
                             imx258->cur_mode->height;
@@ -927,11 +1288,12 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
                             imx258->cur_mode->height;
                __v4l2_ctrl_modify_range(
                        imx258->vblank, vblank_min,
-                       IMX258_VTS_MAX - imx258->cur_mode->height, 1,
-                       vblank_def);
+                       ((1 << IMX258_LONG_EXP_SHIFT_MAX) * IMX258_VTS_MAX) -
+                                               imx258->cur_mode->height,
+                       1, vblank_def);
                __v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
                h_blank =
-                       link_freq_configs[mode->link_freq_index].pixels_per_line
+                       imx258->link_freq_configs[mode->link_freq_index].pixels_per_line
                         - imx258->cur_mode->width;
                __v4l2_ctrl_modify_range(imx258->hblank, h_blank,
                                         h_blank, 1, h_blank);
@@ -942,36 +1304,107 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
        return 0;
 }
 
+static const struct v4l2_rect *
+__imx258_get_pad_crop(struct imx258 *imx258,
+                     struct v4l2_subdev_state *sd_state,
+                     unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+       switch (which) {
+       case V4L2_SUBDEV_FORMAT_TRY:
+               return v4l2_subdev_get_try_crop(&imx258->sd, sd_state, pad);
+       case V4L2_SUBDEV_FORMAT_ACTIVE:
+               return &imx258->cur_mode->crop;
+       }
+
+       return NULL;
+}
+
+static int imx258_get_selection(struct v4l2_subdev *sd,
+                               struct v4l2_subdev_state *sd_state,
+                               struct v4l2_subdev_selection *sel)
+{
+       switch (sel->target) {
+       case V4L2_SEL_TGT_CROP: {
+               struct imx258 *imx258 = to_imx258(sd);
+
+               mutex_lock(&imx258->mutex);
+               sel->r = *__imx258_get_pad_crop(imx258, sd_state, sel->pad,
+                                               sel->which);
+               mutex_unlock(&imx258->mutex);
+
+               return 0;
+       }
+
+       case V4L2_SEL_TGT_NATIVE_SIZE:
+               sel->r.left = 0;
+               sel->r.top = 0;
+               sel->r.width = IMX258_NATIVE_WIDTH;
+               sel->r.height = IMX258_NATIVE_HEIGHT;
+
+               return 0;
+
+       case V4L2_SEL_TGT_CROP_DEFAULT:
+       case V4L2_SEL_TGT_CROP_BOUNDS:
+               sel->r.left = IMX258_PIXEL_ARRAY_LEFT;
+               sel->r.top = IMX258_PIXEL_ARRAY_TOP;
+               sel->r.width = IMX258_PIXEL_ARRAY_WIDTH;
+               sel->r.height = IMX258_PIXEL_ARRAY_HEIGHT;
+
+               return 0;
+       }
+
+       return -EINVAL;
+}
+
 /* Start streaming */
 static int imx258_start_streaming(struct imx258 *imx258)
 {
        struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
        const struct imx258_reg_list *reg_list;
+       const struct imx258_link_freq_config *link_freq_cfg;
        int ret, link_freq_index;
 
+       ret = imx258_write_reg(imx258, IMX258_REG_RESET, IMX258_REG_VALUE_08BIT,
+                              0x01);
+       if (ret) {
+               dev_err(&client->dev, "%s failed to reset sensor\n", __func__);
+               return ret;
+       }
+       usleep_range(10000, 15000);
+
        /* Setup PLL */
        link_freq_index = imx258->cur_mode->link_freq_index;
-       reg_list = &link_freq_configs[link_freq_index].reg_list;
+       link_freq_cfg = &imx258->link_freq_configs[link_freq_index];
+
+       reg_list = &link_freq_cfg->link_cfg[imx258->lane_mode_idx].reg_list;
        ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
        if (ret) {
                dev_err(&client->dev, "%s failed to set plls\n", __func__);
                return ret;
        }
 
-       /* Apply default values of current mode */
-       reg_list = &imx258->cur_mode->reg_list;
-       ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
+       ret = imx258_write_regs(imx258, imx258->variant_cfg->regs,
+                               imx258->variant_cfg->num_regs);
        if (ret) {
-               dev_err(&client->dev, "%s failed to set mode\n", __func__);
+               dev_err(&client->dev, "%s failed to set variant config\n",
+                       __func__);
                return ret;
        }
 
-       /* Set Orientation be 180 degree */
-       ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
-                              IMX258_REG_VALUE_08BIT, REG_CONFIG_MIRROR_FLIP);
+       ret = imx258_write_reg(imx258, IMX258_CLK_BLANK_STOP,
+                              IMX258_REG_VALUE_08BIT,
+                              imx258->csi2_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK ?
+                              1 : 0);
        if (ret) {
-               dev_err(&client->dev, "%s failed to set orientation\n",
-                       __func__);
+               dev_err(&client->dev, "%s failed to set clock lane mode\n", __func__);
+               return ret;
+       }
+
+       /* Apply default values of current mode */
+       reg_list = &imx258->cur_mode->reg_list;
+       ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
+       if (ret) {
+               dev_err(&client->dev, "%s failed to set mode\n", __func__);
                return ret;
        }
 
@@ -1011,9 +1444,19 @@ static int imx258_power_on(struct device *dev)
        struct imx258 *imx258 = to_imx258(sd);
        int ret;
 
+       ret = regulator_bulk_enable(IMX258_NUM_SUPPLIES,
+                                   imx258->supplies);
+       if (ret) {
+               dev_err(dev, "%s: failed to enable regulators\n",
+                       __func__);
+               return ret;
+       }
+
        ret = clk_prepare_enable(imx258->clk);
-       if (ret)
+       if (ret) {
                dev_err(dev, "failed to enable clock\n");
+               regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
+       }
 
        return ret;
 }
@@ -1024,6 +1467,7 @@ static int imx258_power_off(struct device *dev)
        struct imx258 *imx258 = to_imx258(sd);
 
        clk_disable_unprepare(imx258->clk);
+       regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
 
        return 0;
 }
@@ -1134,6 +1578,7 @@ static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
        .get_fmt = imx258_get_pad_format,
        .set_fmt = imx258_set_pad_format,
        .enum_frame_size = imx258_enum_frame_size,
+       .get_selection = imx258_get_selection,
 };
 
 static const struct v4l2_subdev_ops imx258_subdev_ops = {
@@ -1149,13 +1594,13 @@ static const struct v4l2_subdev_internal_ops imx258_internal_ops = {
 static int imx258_init_controls(struct imx258 *imx258)
 {
        struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+       const struct imx258_link_freq_config *link_freq_cfgs;
        struct v4l2_fwnode_device_properties props;
        struct v4l2_ctrl_handler *ctrl_hdlr;
-       struct v4l2_ctrl *vflip, *hflip;
+       const struct imx258_link_cfg *link_cfg;
        s64 vblank_def;
        s64 vblank_min;
-       s64 pixel_rate_min;
-       s64 pixel_rate_max;
+       s64 pixel_rate;
        int ret;
 
        ctrl_hdlr = &imx258->ctrl_handler;
@@ -1168,31 +1613,33 @@ static int imx258_init_controls(struct imx258 *imx258)
        imx258->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
                                &imx258_ctrl_ops,
                                V4L2_CID_LINK_FREQ,
-                               ARRAY_SIZE(link_freq_menu_items) - 1,
+                               ARRAY_SIZE(link_freq_menu_items_19_2) - 1,
                                0,
-                               link_freq_menu_items);
+                               imx258->link_freq_menu_items);
 
        if (imx258->link_freq)
                imx258->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
-       /* The driver only supports one bayer order and flips by default. */
-       hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
-                                 V4L2_CID_HFLIP, 1, 1, 1, 1);
-       if (hflip)
-               hflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+       imx258->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+                                         V4L2_CID_HFLIP, 0, 1, 1, 1);
+       if (imx258->hflip)
+               imx258->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+       imx258->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+                                         V4L2_CID_VFLIP, 0, 1, 1, 1);
+       if (imx258->vflip)
+               imx258->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
 
-       vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
-                                 V4L2_CID_VFLIP, 1, 1, 1, 1);
-       if (vflip)
-               vflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+       link_freq_cfgs = &imx258->link_freq_configs[0];
+       link_cfg = link_freq_cfgs[imx258->lane_mode_idx].link_cfg;
+       pixel_rate = link_freq_to_pixel_rate(imx258->link_freq_menu_items[0],
+                                            link_cfg);
 
-       pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
-       pixel_rate_min = link_freq_to_pixel_rate(link_freq_menu_items[1]);
        /* By default, PIXEL_RATE is read only */
        imx258->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
                                V4L2_CID_PIXEL_RATE,
-                               pixel_rate_min, pixel_rate_max,
-                               1, pixel_rate_max);
+                               pixel_rate, pixel_rate,
+                               1, pixel_rate);
 
 
        vblank_def = imx258->cur_mode->vts_def - imx258->cur_mode->height;
@@ -1203,9 +1650,6 @@ static int imx258_init_controls(struct imx258 *imx258)
                                IMX258_VTS_MAX - imx258->cur_mode->height, 1,
                                vblank_def);
 
-       if (imx258->vblank)
-               imx258->vblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
-
        imx258->hblank = v4l2_ctrl_new_std(
                                ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_HBLANK,
                                IMX258_PPL_DEFAULT - imx258->cur_mode->width,
@@ -1272,9 +1716,33 @@ static void imx258_free_controls(struct imx258 *imx258)
        mutex_destroy(&imx258->mutex);
 }
 
+static int imx258_get_regulators(struct imx258 *imx258,
+                                struct i2c_client *client)
+{
+       unsigned int i;
+
+       for (i = 0; i < IMX258_NUM_SUPPLIES; i++)
+               imx258->supplies[i].supply = imx258_supply_name[i];
+
+       return devm_regulator_bulk_get(&client->dev,
+                                      IMX258_NUM_SUPPLIES,
+                                      imx258->supplies);
+}
+
+static const struct of_device_id imx258_dt_ids[] = {
+       { .compatible = "sony,imx258", .data = &imx258_cfg },
+       { .compatible = "sony,imx258-pdaf", .data = &imx258_pdaf_cfg },
+       { /* sentinel */ }
+};
+
 static int imx258_probe(struct i2c_client *client)
 {
        struct imx258 *imx258;
+       struct fwnode_handle *endpoint;
+       struct v4l2_fwnode_endpoint ep = {
+               .bus_type = V4L2_MBUS_CSI2_DPHY
+       };
+       const struct of_device_id *match;
        int ret;
        u32 val = 0;
 
@@ -1282,6 +1750,10 @@ static int imx258_probe(struct i2c_client *client)
        if (!imx258)
                return -ENOMEM;
 
+       ret = imx258_get_regulators(imx258, client);
+       if (ret)
+               return ret;
+
        imx258->clk = devm_clk_get_optional(&client->dev, NULL);
        if (IS_ERR(imx258->clk))
                return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
@@ -1291,21 +1763,75 @@ static int imx258_probe(struct i2c_client *client)
                        "no clock provided, using clock-frequency property\n");
 
                device_property_read_u32(&client->dev, "clock-frequency", &val);
+       } else if (IS_ERR(imx258->clk)) {
+               return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
+                                    "error getting clock\n");
        } else {
                val = clk_get_rate(imx258->clk);
        }
-       if (val != IMX258_INPUT_CLOCK_FREQ) {
-               dev_err(&client->dev, "input clock frequency not supported\n");
+
+       switch (val) {
+       case 19200000:
+               imx258->link_freq_configs = link_freq_configs_19_2;
+               imx258->link_freq_menu_items = link_freq_menu_items_19_2;
+               break;
+       case 24000000:
+               imx258->link_freq_configs = link_freq_configs_24;
+               imx258->link_freq_menu_items = link_freq_menu_items_24;
+               break;
+       default:
+               dev_err(&client->dev, "input clock frequency of %u not supported\n",
+                       val);
                return -EINVAL;
        }
 
+       endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
+       if (!endpoint) {
+               dev_err(&client->dev, "Endpoint node not found\n");
+               return -EINVAL;
+       }
+
+       ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
+       fwnode_handle_put(endpoint);
+       if (ret == -ENXIO) {
+               dev_err(&client->dev, "Unsupported bus type, should be CSI2\n");
+               goto error_endpoint_poweron;
+       } else if (ret) {
+               dev_err(&client->dev, "Parsing endpoint node failed\n");
+               goto error_endpoint_poweron;
+       }
+
+       /* Get number of data lanes */
+       switch (ep.bus.mipi_csi2.num_data_lanes) {
+       case 2:
+               imx258->lane_mode_idx = IMX258_2_LANE_MODE;
+               break;
+       case 4:
+               imx258->lane_mode_idx = IMX258_4_LANE_MODE;
+               break;
+       default:
+               dev_err(&client->dev, "Invalid data lanes: %u\n",
+                       ep.bus.mipi_csi2.num_data_lanes);
+               ret = -EINVAL;
+               goto error_endpoint_poweron;
+       }
+
+       imx258->csi2_flags = ep.bus.mipi_csi2.flags;
+
+       match = i2c_of_match_device(imx258_dt_ids, client);
+       if (!match || !match->data)
+               imx258->variant_cfg = &imx258_cfg;
+       else
+               imx258->variant_cfg =
+                       (const struct imx258_variant_cfg *)match->data;
+
        /* Initialize subdev */
        v4l2_i2c_subdev_init(&imx258->sd, client, &imx258_subdev_ops);
 
        /* Will be powered off via pm_runtime_idle */
        ret = imx258_power_on(&client->dev);
        if (ret)
-               return ret;
+               goto error_endpoint_poweron;
 
        /* Check module identity */
        ret = imx258_identify_module(imx258);
@@ -1350,6 +1876,9 @@ error_handler_free:
 error_identify:
        imx258_power_off(&client->dev);
 
+error_endpoint_poweron:
+       v4l2_fwnode_endpoint_free(&ep);
+
        return ret;
 }
 
@@ -1382,10 +1911,6 @@ static const struct acpi_device_id imx258_acpi_ids[] = {
 MODULE_DEVICE_TABLE(acpi, imx258_acpi_ids);
 #endif
 
-static const struct of_device_id imx258_dt_ids[] = {
-       { .compatible = "sony,imx258" },
-       { /* sentinel */ }
-};
 MODULE_DEVICE_TABLE(of, imx258_dt_ids);
 
 static struct i2c_driver imx258_i2c_driver = {