drivers/media/i2c/imx258.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 // Copyright (C) 2018 Intel Corporation
   3
   4 #include <linux/acpi.h>
   5 #include <linux/clk.h>
   6 #include <linux/delay.h>
   7 #include <linux/i2c.h>
   8 #include <linux/module.h>
   9 #include <linux/pm_runtime.h>
  10 #include <linux/regulator/consumer.h>
  11 #include <media/v4l2-ctrls.h>
  12 #include <media/v4l2-device.h>
  13 #include <media/v4l2-fwnode.h>
  14 #include <asm/unaligned.h>
  15
  16 #define IMX258_REG_VALUE_08BIT          1
  17 #define IMX258_REG_VALUE_16BIT          2
  18
  19 #define IMX258_REG_MODE_SELECT          0x0100
  20 #define IMX258_MODE_STANDBY             0x00
  21 #define IMX258_MODE_STREAMING           0x01
  22
  23 #define IMX258_REG_RESET                0x0103
  24
  25 /* Chip ID */
  26 #define IMX258_REG_CHIP_ID              0x0016
  27 #define IMX258_CHIP_ID                  0x0258
  28
  29 /* V_TIMING internal */
  30 #define IMX258_VTS_30FPS                0x0c50
  31 #define IMX258_VTS_30FPS_2K             0x0638
  32 #define IMX258_VTS_30FPS_VGA            0x034c
  33 #define IMX258_VTS_MAX                  65525
  34
  35 #define IMX258_REG_VTS                  0x0340
  36
  37 /* Exposure control */
  38 #define IMX258_REG_EXPOSURE             0x0202
  39 #define IMX258_EXPOSURE_OFFSET          10
  40 #define IMX258_EXPOSURE_MIN             4
  41 #define IMX258_EXPOSURE_STEP            1
  42 #define IMX258_EXPOSURE_DEFAULT         0x640
  43 #define IMX258_EXPOSURE_MAX             (IMX258_VTS_MAX - IMX258_EXPOSURE_OFFSET)
  44
  45 /* HBLANK control - read only */
  46 #define IMX258_PPL_DEFAULT              5352
  47
  48 /* Analog gain control */
  49 #define IMX258_REG_ANALOG_GAIN          0x0204
  50 #define IMX258_ANA_GAIN_MIN             0
  51 #define IMX258_ANA_GAIN_MAX             480
  52 #define IMX258_ANA_GAIN_STEP            1
  53 #define IMX258_ANA_GAIN_DEFAULT         0x0
  54
  55 /* Digital gain control */
  56 #define IMX258_REG_GR_DIGITAL_GAIN      0x020e
  57 #define IMX258_REG_R_DIGITAL_GAIN       0x0210
  58 #define IMX258_REG_B_DIGITAL_GAIN       0x0212
  59 #define IMX258_REG_GB_DIGITAL_GAIN      0x0214
  60 #define IMX258_DGTL_GAIN_MIN            0
  61 #define IMX258_DGTL_GAIN_MAX            4096    /* Max = 0xFFF */
  62 #define IMX258_DGTL_GAIN_DEFAULT        1024
  63 #define IMX258_DGTL_GAIN_STEP           1
  64
  65 /* HDR control */
  66 #define IMX258_REG_HDR                  0x0220
  67 #define IMX258_HDR_ON                   BIT(0)
  68 #define IMX258_REG_HDR_RATIO            0x0222
  69 #define IMX258_HDR_RATIO_MIN            0
  70 #define IMX258_HDR_RATIO_MAX            5
  71 #define IMX258_HDR_RATIO_STEP           1
  72 #define IMX258_HDR_RATIO_DEFAULT        0x0
  73
  74 /* Long exposure multiplier */
  75 #define IMX258_LONG_EXP_SHIFT_MAX       7
  76 #define IMX258_LONG_EXP_SHIFT_REG       0x3002
  77
  78 /* Test Pattern Control */
  79 #define IMX258_REG_TEST_PATTERN         0x0600
  80
  81 #define IMX258_CLK_BLANK_STOP           0x4040
  82
  83 /* Orientation */
  84 #define REG_MIRROR_FLIP_CONTROL         0x0101
  85 #define REG_CONFIG_MIRROR_HFLIP         0x01
  86 #define REG_CONFIG_MIRROR_VFLIP         0x02
  87 #define REG_CONFIG_FLIP_TEST_PATTERN    0x02
  88
  89 /* IMX258 native and active pixel array size. */
  90 #define IMX258_NATIVE_WIDTH             4224U
  91 #define IMX258_NATIVE_HEIGHT            3192U
  92 #define IMX258_PIXEL_ARRAY_LEFT         8U
  93 #define IMX258_PIXEL_ARRAY_TOP          16U
  94 #define IMX258_PIXEL_ARRAY_WIDTH        4208U
  95 #define IMX258_PIXEL_ARRAY_HEIGHT       3120U
  96
  97 struct imx258_reg {
  98         u16 address;
  99         u8 val;
 100 };
 101
 102 struct imx258_reg_list {
 103         u32 num_of_regs;
 104         const struct imx258_reg *regs;
 105 };
 106
 107 struct imx258_link_cfg {
 108         unsigned int lf_to_pix_rate_factor;
 109         struct imx258_reg_list reg_list;
 110 };
 111
 112 #define IMX258_LANE_CONFIGS     2
 113 #define IMX258_2_LANE_MODE      0
 114 #define IMX258_4_LANE_MODE      1
 115
 116 /* Link frequency config */
 117 struct imx258_link_freq_config {
 118         u64 link_frequency;
 119         u32 pixels_per_line;
 120
 121         /* Configuration for this link frequency / num lanes selection */
 122         struct imx258_link_cfg link_cfg[IMX258_LANE_CONFIGS];
 123 };
 124
 125 /* Mode : resolution and related config&values */
 126 struct imx258_mode {
 127         /* Frame width */
 128         u32 width;
 129         /* Frame height */
 130         u32 height;
 131
 132         /* V-timing */
 133         u32 vts_def;
 134         u32 vts_min;
 135
 136         /* Index of Link frequency config to be used */
 137         u32 link_freq_index;
 138         /* Default register values */
 139         struct imx258_reg_list reg_list;
 140
 141         /* Analog crop rectangle. */
 142         struct v4l2_rect crop;
 143 };
 144
 145 /* 4208x3120 needs 1267Mbps/lane, 4 lanes. Use that rate on 2 lanes as well */
 146 static const struct imx258_reg mipi_1267mbps_19_2mhz_2l[] = {
 147         { 0x0136, 0x13 },
 148         { 0x0137, 0x33 },
 149         { 0x0301, 0x0A },
 150         { 0x0303, 0x02 },
 151         { 0x0305, 0x03 },
 152         { 0x0306, 0x00 },
 153         { 0x0307, 0xC6 },
 154         { 0x0309, 0x0A },
 155         { 0x030B, 0x01 },
 156         { 0x030D, 0x02 },
 157         { 0x030E, 0x00 },
 158         { 0x030F, 0xD8 },
 159         { 0x0310, 0x00 },
 160
 161         { 0x0114, 0x01 },
 162         { 0x0820, 0x09 },
 163         { 0x0821, 0xa6 },
 164         { 0x0822, 0x66 },
 165         { 0x0823, 0x66 },
 166 };
 167
 168 static const struct imx258_reg mipi_1267mbps_19_2mhz_4l[] = {
 169         { 0x0136, 0x13 },
 170         { 0x0137, 0x33 },
 171         { 0x0301, 0x05 },
 172         { 0x0303, 0x02 },
 173         { 0x0305, 0x03 },
 174         { 0x0306, 0x00 },
 175         { 0x0307, 0xC6 },
 176         { 0x0309, 0x0A },
 177         { 0x030B, 0x01 },
 178         { 0x030D, 0x02 },
 179         { 0x030E, 0x00 },
 180         { 0x030F, 0xD8 },
 181         { 0x0310, 0x00 },
 182
 183         { 0x0114, 0x03 },
 184         { 0x0820, 0x13 },
 185         { 0x0821, 0x4C },
 186         { 0x0822, 0xCC },
 187         { 0x0823, 0xCC },
 188 };
 189
 190 static const struct imx258_reg mipi_1272mbps_24mhz_2l[] = {
 191         { 0x0136, 0x18 },
 192         { 0x0137, 0x00 },
 193         { 0x0301, 0x0a },
 194         { 0x0303, 0x02 },
 195         { 0x0305, 0x04 },
 196         { 0x0306, 0x00 },
 197         { 0x0307, 0xD4 },
 198         { 0x0309, 0x0A },
 199         { 0x030B, 0x01 },
 200         { 0x030D, 0x02 },
 201         { 0x030E, 0x00 },
 202         { 0x030F, 0xD8 },
 203         { 0x0310, 0x00 },
 204
 205         { 0x0114, 0x01 },
 206         { 0x0820, 0x13 },
 207         { 0x0821, 0x4C },
 208         { 0x0822, 0xCC },
 209         { 0x0823, 0xCC },
 210 };
 211
 212 static const struct imx258_reg mipi_1272mbps_24mhz_4l[] = {
 213         { 0x0136, 0x18 },
 214         { 0x0137, 0x00 },
 215         { 0x0301, 0x05 },
 216         { 0x0303, 0x02 },
 217         { 0x0305, 0x04 },
 218         { 0x0306, 0x00 },
 219         { 0x0307, 0xD4 },
 220         { 0x0309, 0x0A },
 221         { 0x030B, 0x01 },
 222         { 0x030D, 0x02 },
 223         { 0x030E, 0x00 },
 224         { 0x030F, 0xD8 },
 225         { 0x0310, 0x00 },
 226
 227         { 0x0114, 0x03 },
 228         { 0x0820, 0x13 },
 229         { 0x0821, 0xE0 },
 230         { 0x0822, 0x00 },
 231         { 0x0823, 0x00 },
 232 };
 233
 234 static const struct imx258_reg mipi_640mbps_19_2mhz_2l[] = {
 235         { 0x0136, 0x13 },
 236         { 0x0137, 0x33 },
 237         { 0x0301, 0x05 },
 238         { 0x0303, 0x02 },
 239         { 0x0305, 0x03 },
 240         { 0x0306, 0x00 },
 241         { 0x0307, 0x64 },
 242         { 0x0309, 0x0A },
 243         { 0x030B, 0x01 },
 244         { 0x030D, 0x02 },
 245         { 0x030E, 0x00 },
 246         { 0x030F, 0xD8 },
 247         { 0x0310, 0x00 },
 248
 249         { 0x0114, 0x01 },
 250         { 0x0820, 0x05 },
 251         { 0x0821, 0x00 },
 252         { 0x0822, 0x00 },
 253         { 0x0823, 0x00 },
 254 };
 255
 256 static const struct imx258_reg mipi_640mbps_19_2mhz_4l[] = {
 257         { 0x0136, 0x13 },
 258         { 0x0137, 0x33 },
 259         { 0x0301, 0x05 },
 260         { 0x0303, 0x02 },
 261         { 0x0305, 0x03 },
 262         { 0x0306, 0x00 },
 263         { 0x0307, 0x64 },
 264         { 0x0309, 0x0A },
 265         { 0x030B, 0x01 },
 266         { 0x030D, 0x02 },
 267         { 0x030E, 0x00 },
 268         { 0x030F, 0xD8 },
 269         { 0x0310, 0x00 },
 270
 271         { 0x0114, 0x03 },
 272         { 0x0820, 0x0A },
 273         { 0x0821, 0x00 },
 274         { 0x0822, 0x00 },
 275         { 0x0823, 0x00 },
 276 };
 277
 278 static const struct imx258_reg mipi_642mbps_24mhz_2l[] = {
 279         { 0x0136, 0x18 },
 280         { 0x0137, 0x00 },
 281         { 0x0301, 0x05 },
 282         { 0x0303, 0x02 },
 283         { 0x0305, 0x04 },
 284         { 0x0306, 0x00 },
 285         { 0x0307, 0x6B },
 286         { 0x0309, 0x0A },
 287         { 0x030B, 0x01 },
 288         { 0x030D, 0x02 },
 289         { 0x030E, 0x00 },
 290         { 0x030F, 0xD8 },
 291         { 0x0310, 0x00 },
 292
 293         { 0x0114, 0x01 },
 294         { 0x0820, 0x0A },
 295         { 0x0821, 0x00 },
 296         { 0x0822, 0x00 },
 297         { 0x0823, 0x00 },
 298 };
 299
 300 static const struct imx258_reg mipi_642mbps_24mhz_4l[] = {
 301         { 0x0136, 0x18 },
 302         { 0x0137, 0x00 },
 303         { 0x0301, 0x05 },
 304         { 0x0303, 0x02 },
 305         { 0x0305, 0x04 },
 306         { 0x0306, 0x00 },
 307         { 0x0307, 0x6B },
 308         { 0x0309, 0x0A },
 309         { 0x030B, 0x01 },
 310         { 0x030D, 0x02 },
 311         { 0x030E, 0x00 },
 312         { 0x030F, 0xD8 },
 313         { 0x0310, 0x00 },
 314
 315         { 0x0114, 0x03 },
 316         { 0x0820, 0x0A },
 317         { 0x0821, 0x00 },
 318         { 0x0822, 0x00 },
 319         { 0x0823, 0x00 },
 320 };
 321
 322 static const struct imx258_reg mode_4208x3120_regs[] = {
 323         { 0x3051, 0x00 },
 324         { 0x6B11, 0xCF },
 325         { 0x7FF0, 0x08 },
 326         { 0x7FF1, 0x0F },
 327         { 0x7FF2, 0x08 },
 328         { 0x7FF3, 0x1B },
 329         { 0x7FF4, 0x23 },
 330         { 0x7FF5, 0x60 },
 331         { 0x7FF6, 0x00 },
 332         { 0x7FF7, 0x01 },
 333         { 0x7FF8, 0x00 },
 334         { 0x7FF9, 0x78 },
 335         { 0x7FFA, 0x00 },
 336         { 0x7FFB, 0x00 },
 337         { 0x7FFC, 0x00 },
 338         { 0x7FFD, 0x00 },
 339         { 0x7FFE, 0x00 },
 340         { 0x7FFF, 0x03 },
 341         { 0x7F76, 0x03 },
 342         { 0x7F77, 0xFE },
 343         { 0x7FA8, 0x03 },
 344         { 0x7FA9, 0xFE },
 345         { 0x7B24, 0x81 },
 346         { 0x6564, 0x07 },
 347         { 0x6B0D, 0x41 },
 348         { 0x653D, 0x04 },
 349         { 0x6B05, 0x8C },
 350         { 0x6B06, 0xF9 },
 351         { 0x6B08, 0x65 },
 352         { 0x6B09, 0xFC },
 353         { 0x6B0A, 0xCF },
 354         { 0x6B0B, 0xD2 },
 355         { 0x6700, 0x0E },
 356         { 0x6707, 0x0E },
 357         { 0x9104, 0x00 },
 358         { 0x4648, 0x7F },
 359         { 0x7420, 0x00 },
 360         { 0x7421, 0x1C },
 361         { 0x7422, 0x00 },
 362         { 0x7423, 0xD7 },
 363         { 0x5F04, 0x00 },
 364         { 0x5F05, 0xED },
 365         { 0x0112, 0x0A },
 366         { 0x0113, 0x0A },
 367         { 0x0342, 0x14 },
 368         { 0x0343, 0xE8 },
 369         { 0x0344, 0x00 },
 370         { 0x0345, 0x00 },
 371         { 0x0346, 0x00 },
 372         { 0x0347, 0x00 },
 373         { 0x0348, 0x10 },
 374         { 0x0349, 0x6F },
 375         { 0x034A, 0x0C },
 376         { 0x034B, 0x2F },
 377         { 0x0381, 0x01 },
 378         { 0x0383, 0x01 },
 379         { 0x0385, 0x01 },
 380         { 0x0387, 0x01 },
 381         { 0x0900, 0x00 },
 382         { 0x0901, 0x11 },
 383         { 0x0401, 0x00 },
 384         { 0x0404, 0x00 },
 385         { 0x0405, 0x10 },
 386         { 0x0408, 0x00 },
 387         { 0x0409, 0x00 },
 388         { 0x040A, 0x00 },
 389         { 0x040B, 0x00 },
 390         { 0x040C, 0x10 },
 391         { 0x040D, 0x70 },
 392         { 0x040E, 0x0C },
 393         { 0x040F, 0x30 },
 394         { 0x3038, 0x00 },
 395         { 0x303A, 0x00 },
 396         { 0x303B, 0x10 },
 397         { 0x300D, 0x00 },
 398         { 0x034C, 0x10 },
 399         { 0x034D, 0x70 },
 400         { 0x034E, 0x0C },
 401         { 0x034F, 0x30 },
 402         { 0x0350, 0x00 },
 403         { 0x0204, 0x00 },
 404         { 0x0205, 0x00 },
 405         { 0x020E, 0x01 },
 406         { 0x020F, 0x00 },
 407         { 0x0210, 0x01 },
 408         { 0x0211, 0x00 },
 409         { 0x0212, 0x01 },
 410         { 0x0213, 0x00 },
 411         { 0x0214, 0x01 },
 412         { 0x0215, 0x00 },
 413         { 0x7BCD, 0x00 },
 414         { 0x94DC, 0x20 },
 415         { 0x94DD, 0x20 },
 416         { 0x94DE, 0x20 },
 417         { 0x95DC, 0x20 },
 418         { 0x95DD, 0x20 },
 419         { 0x95DE, 0x20 },
 420         { 0x7FB0, 0x00 },
 421         { 0x9010, 0x3E },
 422         { 0x9419, 0x50 },
 423         { 0x941B, 0x50 },
 424         { 0x9519, 0x50 },
 425         { 0x951B, 0x50 },
 426         { 0x3030, 0x00 },
 427         { 0x3032, 0x00 },
 428         { 0x0220, 0x00 },
 429 };
 430
 431 static const struct imx258_reg mode_2104_1560_regs[] = {
 432         { 0x3051, 0x00 },
 433         { 0x6B11, 0xCF },
 434         { 0x7FF0, 0x08 },
 435         { 0x7FF1, 0x0F },
 436         { 0x7FF2, 0x08 },
 437         { 0x7FF3, 0x1B },
 438         { 0x7FF4, 0x23 },
 439         { 0x7FF5, 0x60 },
 440         { 0x7FF6, 0x00 },
 441         { 0x7FF7, 0x01 },
 442         { 0x7FF8, 0x00 },
 443         { 0x7FF9, 0x78 },
 444         { 0x7FFA, 0x00 },
 445         { 0x7FFB, 0x00 },
 446         { 0x7FFC, 0x00 },
 447         { 0x7FFD, 0x00 },
 448         { 0x7FFE, 0x00 },
 449         { 0x7FFF, 0x03 },
 450         { 0x7F76, 0x03 },
 451         { 0x7F77, 0xFE },
 452         { 0x7FA8, 0x03 },
 453         { 0x7FA9, 0xFE },
 454         { 0x7B24, 0x81 },
 455         { 0x6564, 0x07 },
 456         { 0x6B0D, 0x41 },
 457         { 0x653D, 0x04 },
 458         { 0x6B05, 0x8C },
 459         { 0x6B06, 0xF9 },
 460         { 0x6B08, 0x65 },
 461         { 0x6B09, 0xFC },
 462         { 0x6B0A, 0xCF },
 463         { 0x6B0B, 0xD2 },
 464         { 0x6700, 0x0E },
 465         { 0x6707, 0x0E },
 466         { 0x9104, 0x00 },
 467         { 0x4648, 0x7F },
 468         { 0x7420, 0x00 },
 469         { 0x7421, 0x1C },
 470         { 0x7422, 0x00 },
 471         { 0x7423, 0xD7 },
 472         { 0x5F04, 0x00 },
 473         { 0x5F05, 0xED },
 474         { 0x0112, 0x0A },
 475         { 0x0113, 0x0A },
 476         { 0x0342, 0x14 },
 477         { 0x0343, 0xE8 },
 478         { 0x0344, 0x00 },
 479         { 0x0345, 0x00 },
 480         { 0x0346, 0x00 },
 481         { 0x0347, 0x00 },
 482         { 0x0348, 0x10 },
 483         { 0x0349, 0x6F },
 484         { 0x034A, 0x0C },
 485         { 0x034B, 0x2F },
 486         { 0x0381, 0x01 },
 487         { 0x0383, 0x01 },
 488         { 0x0385, 0x01 },
 489         { 0x0387, 0x01 },
 490         { 0x0900, 0x01 },
 491         { 0x0901, 0x12 },
 492         { 0x0401, 0x01 },
 493         { 0x0404, 0x00 },
 494         { 0x0405, 0x20 },
 495         { 0x0408, 0x00 },
 496         { 0x0409, 0x00 },
 497         { 0x040A, 0x00 },
 498         { 0x040B, 0x00 },
 499         { 0x040C, 0x10 },
 500         { 0x040D, 0x70 },
 501         { 0x040E, 0x06 },
 502         { 0x040F, 0x18 },
 503         { 0x3038, 0x00 },
 504         { 0x303A, 0x00 },
 505         { 0x303B, 0x10 },
 506         { 0x300D, 0x00 },
 507         { 0x034C, 0x08 },
 508         { 0x034D, 0x38 },
 509         { 0x034E, 0x06 },
 510         { 0x034F, 0x18 },
 511         { 0x0350, 0x00 },
 512         { 0x0204, 0x00 },
 513         { 0x0205, 0x00 },
 514         { 0x020E, 0x01 },
 515         { 0x020F, 0x00 },
 516         { 0x0210, 0x01 },
 517         { 0x0211, 0x00 },
 518         { 0x0212, 0x01 },
 519         { 0x0213, 0x00 },
 520         { 0x0214, 0x01 },
 521         { 0x0215, 0x00 },
 522         { 0x7BCD, 0x01 },
 523         { 0x94DC, 0x20 },
 524         { 0x94DD, 0x20 },
 525         { 0x94DE, 0x20 },
 526         { 0x95DC, 0x20 },
 527         { 0x95DD, 0x20 },
 528         { 0x95DE, 0x20 },
 529         { 0x7FB0, 0x00 },
 530         { 0x9010, 0x3E },
 531         { 0x9419, 0x50 },
 532         { 0x941B, 0x50 },
 533         { 0x9519, 0x50 },
 534         { 0x951B, 0x50 },
 535         { 0x3030, 0x00 },
 536         { 0x3032, 0x00 },
 537         { 0x0220, 0x00 },
 538 };
 539
 540 static const struct imx258_reg mode_1048_780_regs[] = {
 541         { 0x3051, 0x00 },
 542         { 0x6B11, 0xCF },
 543         { 0x7FF0, 0x08 },
 544         { 0x7FF1, 0x0F },
 545         { 0x7FF2, 0x08 },
 546         { 0x7FF3, 0x1B },
 547         { 0x7FF4, 0x23 },
 548         { 0x7FF5, 0x60 },
 549         { 0x7FF6, 0x00 },
 550         { 0x7FF7, 0x01 },
 551         { 0x7FF8, 0x00 },
 552         { 0x7FF9, 0x78 },
 553         { 0x7FFA, 0x00 },
 554         { 0x7FFB, 0x00 },
 555         { 0x7FFC, 0x00 },
 556         { 0x7FFD, 0x00 },
 557         { 0x7FFE, 0x00 },
 558         { 0x7FFF, 0x03 },
 559         { 0x7F76, 0x03 },
 560         { 0x7F77, 0xFE },
 561         { 0x7FA8, 0x03 },
 562         { 0x7FA9, 0xFE },
 563         { 0x7B24, 0x81 },
 564         { 0x6564, 0x07 },
 565         { 0x6B0D, 0x41 },
 566         { 0x653D, 0x04 },
 567         { 0x6B05, 0x8C },
 568         { 0x6B06, 0xF9 },
 569         { 0x6B08, 0x65 },
 570         { 0x6B09, 0xFC },
 571         { 0x6B0A, 0xCF },
 572         { 0x6B0B, 0xD2 },
 573         { 0x6700, 0x0E },
 574         { 0x6707, 0x0E },
 575         { 0x9104, 0x00 },
 576         { 0x4648, 0x7F },
 577         { 0x7420, 0x00 },
 578         { 0x7421, 0x1C },
 579         { 0x7422, 0x00 },
 580         { 0x7423, 0xD7 },
 581         { 0x5F04, 0x00 },
 582         { 0x5F05, 0xED },
 583         { 0x0112, 0x0A },
 584         { 0x0113, 0x0A },
 585         { 0x0342, 0x14 },
 586         { 0x0343, 0xE8 },
 587         { 0x0344, 0x00 },
 588         { 0x0345, 0x00 },
 589         { 0x0346, 0x00 },
 590         { 0x0347, 0x00 },
 591         { 0x0348, 0x10 },
 592         { 0x0349, 0x6F },
 593         { 0x034A, 0x0C },
 594         { 0x034B, 0x2F },
 595         { 0x0381, 0x01 },
 596         { 0x0383, 0x01 },
 597         { 0x0385, 0x01 },
 598         { 0x0387, 0x01 },
 599         { 0x0900, 0x01 },
 600         { 0x0901, 0x14 },
 601         { 0x0401, 0x01 },
 602         { 0x0404, 0x00 },
 603         { 0x0405, 0x40 },
 604         { 0x0408, 0x00 },
 605         { 0x0409, 0x00 },
 606         { 0x040A, 0x00 },
 607         { 0x040B, 0x00 },
 608         { 0x040C, 0x10 },
 609         { 0x040D, 0x70 },
 610         { 0x040E, 0x03 },
 611         { 0x040F, 0x0C },
 612         { 0x3038, 0x00 },
 613         { 0x303A, 0x00 },
 614         { 0x303B, 0x10 },
 615         { 0x300D, 0x00 },
 616         { 0x034C, 0x04 },
 617         { 0x034D, 0x18 },
 618         { 0x034E, 0x03 },
 619         { 0x034F, 0x0C },
 620         { 0x0350, 0x00 },
 621         { 0x0204, 0x00 },
 622         { 0x0205, 0x00 },
 623         { 0x020E, 0x01 },
 624         { 0x020F, 0x00 },
 625         { 0x0210, 0x01 },
 626         { 0x0211, 0x00 },
 627         { 0x0212, 0x01 },
 628         { 0x0213, 0x00 },
 629         { 0x0214, 0x01 },
 630         { 0x0215, 0x00 },
 631         { 0x7BCD, 0x00 },
 632         { 0x94DC, 0x20 },
 633         { 0x94DD, 0x20 },
 634         { 0x94DE, 0x20 },
 635         { 0x95DC, 0x20 },
 636         { 0x95DD, 0x20 },
 637         { 0x95DE, 0x20 },
 638         { 0x7FB0, 0x00 },
 639         { 0x9010, 0x3E },
 640         { 0x9419, 0x50 },
 641         { 0x941B, 0x50 },
 642         { 0x9519, 0x50 },
 643         { 0x951B, 0x50 },
 644         { 0x3030, 0x00 },
 645         { 0x3032, 0x00 },
 646         { 0x0220, 0x00 },
 647 };
 648
 649 struct imx258_variant_cfg {
 650         const struct imx258_reg *regs;
 651         unsigned int num_regs;
 652 };
 653
 654 static const struct imx258_reg imx258_cfg_regs[] = {
 655         { 0x3052, 0x00 },
 656         { 0x4E21, 0x14 },
 657         { 0x7B25, 0x00 },
 658 };
 659
 660 static const struct imx258_variant_cfg imx258_cfg = {
 661         .regs = imx258_cfg_regs,
 662         .num_regs = ARRAY_SIZE(imx258_cfg_regs),
 663 };
 664
 665 static const struct imx258_reg imx258_pdaf_cfg_regs[] = {
 666         { 0x3052, 0x01 },
 667         { 0x4E21, 0x10 },
 668         { 0x7B25, 0x01 },
 669 };
 670
 671 static const struct imx258_variant_cfg imx258_pdaf_cfg = {
 672         .regs = imx258_pdaf_cfg_regs,
 673         .num_regs = ARRAY_SIZE(imx258_pdaf_cfg_regs),
 674 };
 675
 676 /*
 677  * The supported formats.
 678  * This table MUST contain 4 entries per format, to cover the various flip
 679  * combinations in the order
 680  * - no flip
 681  * - h flip
 682  * - v flip
 683  * - h&v flips
 684  */
 685 static const u32 codes[] = {
 686         /* 10-bit modes. */
 687         MEDIA_BUS_FMT_SRGGB10_1X10,
 688         MEDIA_BUS_FMT_SGRBG10_1X10,
 689         MEDIA_BUS_FMT_SGBRG10_1X10,
 690         MEDIA_BUS_FMT_SBGGR10_1X10
 691 };
 692 static const char * const imx258_test_pattern_menu[] = {
 693         "Disabled",
 694         "Solid Colour",
 695         "Eight Vertical Colour Bars",
 696         "Colour Bars With Fade to Grey",
 697         "Pseudorandom Sequence (PN9)",
 698 };
 699
 700 /* regulator supplies */
 701 static const char * const imx258_supply_name[] = {
 702         /* Supplies can be enabled in any order */
 703         "vana",  /* Analog (2.8V) supply */
 704         "vdig",  /* Digital Core (1.05V) supply */
 705         "vif",  /* IF (1.8V) supply */
 706 };
 707
 708 #define IMX258_NUM_SUPPLIES ARRAY_SIZE(imx258_supply_name)
 709
 710 enum {
 711         IMX258_LINK_FREQ_1267MBPS,
 712         IMX258_LINK_FREQ_640MBPS,
 713 };
 714
 715 /*
 716  * Pixel rate does not necessarily relate to link frequency on this sensor as
 717  * there is a FIFO between the pixel array pipeline and the MIPI serializer.
 718  * The recommendation from Sony is that the pixel array is always run with a
 719  * line length of 5352 pixels, which means that there is a large amount of
 720  * blanking time for the 1048x780 mode. There is no need to replicate this
 721  * blanking on the CSI2 bus, and the configuration of register 0x0301 allows the
 722  * divider to be altered.
 723  *
 724  * The actual factor between link frequency and pixel rate is in the
 725  * imx258_link_cfg, so use this to convert between the two.
 726  * bits per pixel being 10, and D-PHY being DDR is assumed by this function, so
 727  * the value is only the combination of number of lanes and pixel clock divider.
 728  */
 729 static u64 link_freq_to_pixel_rate(u64 f, const struct imx258_link_cfg *link_cfg)
 730 {
 731         f *= 2 * link_cfg->lf_to_pix_rate_factor;
 732         do_div(f, 10);
 733
 734         return f;
 735 }
 736
 737 /* Menu items for LINK_FREQ V4L2 control */
 738 /* Configurations for supported link frequencies */
 739 #define IMX258_LINK_FREQ_634MHZ 633600000ULL
 740 #define IMX258_LINK_FREQ_320MHZ 320000000ULL
 741
 742 static const s64 link_freq_menu_items_19_2[] = {
 743         IMX258_LINK_FREQ_634MHZ,
 744         IMX258_LINK_FREQ_320MHZ,
 745 };
 746
 747 /* Configurations for supported link frequencies */
 748 #define IMX258_LINK_FREQ_636MHZ 636000000ULL
 749 #define IMX258_LINK_FREQ_321MHZ 321000000ULL
 750
 751 static const s64 link_freq_menu_items_24[] = {
 752         IMX258_LINK_FREQ_636MHZ,
 753         IMX258_LINK_FREQ_321MHZ,
 754 };
 755
 756 #define REGS(_list) { .num_of_regs = ARRAY_SIZE(_list), .regs = _list, }
 757
 758 /* Link frequency configs */
 759 static const struct imx258_link_freq_config link_freq_configs_19_2[] = {
 760         [IMX258_LINK_FREQ_1267MBPS] = {
 761                 .pixels_per_line = IMX258_PPL_DEFAULT,
 762                 .link_cfg = {
 763                         [IMX258_2_LANE_MODE] = {
 764                                 .lf_to_pix_rate_factor = 2 * 2,
 765                                 .reg_list = REGS(mipi_1267mbps_19_2mhz_2l),
 766                         },
 767                         [IMX258_4_LANE_MODE] = {
 768                                 .lf_to_pix_rate_factor = 4,
 769                                 .reg_list = REGS(mipi_1267mbps_19_2mhz_4l),
 770                         },
 771                 }
 772         },
 773         [IMX258_LINK_FREQ_640MBPS] = {
 774                 .pixels_per_line = IMX258_PPL_DEFAULT,
 775                 .link_cfg = {
 776                         [IMX258_2_LANE_MODE] = {
 777                                 .lf_to_pix_rate_factor = 2,
 778                                 .reg_list = REGS(mipi_640mbps_19_2mhz_2l),
 779                         },
 780                         [IMX258_4_LANE_MODE] = {
 781                                 .lf_to_pix_rate_factor = 4,
 782                                 .reg_list = REGS(mipi_640mbps_19_2mhz_4l),
 783                         },
 784                 }
 785         },
 786 };
 787
 788 static const struct imx258_link_freq_config link_freq_configs_24[] = {
 789         [IMX258_LINK_FREQ_1267MBPS] = {
 790                 .pixels_per_line = IMX258_PPL_DEFAULT,
 791                 .link_cfg = {
 792                         [IMX258_2_LANE_MODE] = {
 793                                 .lf_to_pix_rate_factor = 2,
 794                                 .reg_list = REGS(mipi_1272mbps_24mhz_2l),
 795                         },
 796                         [IMX258_4_LANE_MODE] = {
 797                                 .lf_to_pix_rate_factor = 4,
 798                                 .reg_list = REGS(mipi_1272mbps_24mhz_4l),
 799                         },
 800                 }
 801         },
 802         [IMX258_LINK_FREQ_640MBPS] = {
 803                 .pixels_per_line = IMX258_PPL_DEFAULT,
 804                 .link_cfg = {
 805                         [IMX258_2_LANE_MODE] = {
 806                                 .lf_to_pix_rate_factor = 2 * 2,
 807                                 .reg_list = REGS(mipi_642mbps_24mhz_2l),
 808                         },
 809                         [IMX258_4_LANE_MODE] = {
 810                                 .lf_to_pix_rate_factor = 4,
 811                                 .reg_list = REGS(mipi_642mbps_24mhz_4l),
 812                         },
 813                 }
 814         },
 815 };
 816
 817 /* Mode configs */
 818 static const struct imx258_mode supported_modes[] = {
 819         {
 820                 .width = 4208,
 821                 .height = 3120,
 822                 .vts_def = IMX258_VTS_30FPS,
 823                 .vts_min = IMX258_VTS_30FPS,
 824                 .reg_list = {
 825                         .num_of_regs = ARRAY_SIZE(mode_4208x3120_regs),
 826                         .regs = mode_4208x3120_regs,
 827                 },
 828                 .link_freq_index = IMX258_LINK_FREQ_1267MBPS,
 829                 .crop = {
 830                         .left = IMX258_PIXEL_ARRAY_LEFT,
 831                         .top = IMX258_PIXEL_ARRAY_TOP,
 832                         .width = 4208,
 833                         .height = 3120,
 834                 },
 835         },
 836         {
 837                 .width = 2104,
 838                 .height = 1560,
 839                 .vts_def = IMX258_VTS_30FPS_2K,
 840                 .vts_min = IMX258_VTS_30FPS_2K,
 841                 .reg_list = {
 842                         .num_of_regs = ARRAY_SIZE(mode_2104_1560_regs),
 843                         .regs = mode_2104_1560_regs,
 844                 },
 845                 .link_freq_index = IMX258_LINK_FREQ_640MBPS,
 846                 .crop = {
 847                         .left = IMX258_PIXEL_ARRAY_LEFT,
 848                         .top = IMX258_PIXEL_ARRAY_TOP,
 849                         .width = 4208,
 850                         .height = 3120,
 851                 },
 852         },
 853         {
 854                 .width = 1048,
 855                 .height = 780,
 856                 .vts_def = IMX258_VTS_30FPS_VGA,
 857                 .vts_min = IMX258_VTS_30FPS_VGA,
 858                 .reg_list = {
 859                         .num_of_regs = ARRAY_SIZE(mode_1048_780_regs),
 860                         .regs = mode_1048_780_regs,
 861                 },
 862                 .link_freq_index = IMX258_LINK_FREQ_640MBPS,
 863                 .crop = {
 864                         .left = IMX258_PIXEL_ARRAY_LEFT,
 865                         .top = IMX258_PIXEL_ARRAY_TOP,
 866                         .width = 4208,
 867                         .height = 3120,
 868                 },
 869         },
 870 };
 871
 872 struct imx258 {
 873         struct v4l2_subdev sd;
 874         struct media_pad pad;
 875
 876         const struct imx258_variant_cfg *variant_cfg;
 877
 878         struct v4l2_ctrl_handler ctrl_handler;
 879         /* V4L2 Controls */
 880         struct v4l2_ctrl *link_freq;
 881         struct v4l2_ctrl *pixel_rate;
 882         struct v4l2_ctrl *vblank;
 883         struct v4l2_ctrl *hblank;
 884         struct v4l2_ctrl *exposure;
 885         struct v4l2_ctrl *hflip;
 886         struct v4l2_ctrl *vflip;
 887         /* Current long exposure factor in use. Set through V4L2_CID_VBLANK */
 888         unsigned int long_exp_shift;
 889
 890         /* Current mode */
 891         const struct imx258_mode *cur_mode;
 892
 893         const struct imx258_link_freq_config *link_freq_configs;
 894         const s64 *link_freq_menu_items;
 895         unsigned int lane_mode_idx;
 896         unsigned int csi2_flags;
 897
 898         /*
 899          * Mutex for serialized access:
 900          * Protect sensor module set pad format and start/stop streaming safely.
 901          */
 902         struct mutex mutex;
 903
 904         /* Streaming on/off */
 905         bool streaming;
 906
 907         struct clk *clk;
 908         struct regulator_bulk_data supplies[IMX258_NUM_SUPPLIES];
 909 };
 910
 911 static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
 912 {
 913         return container_of(_sd, struct imx258, sd);
 914 }
 915
 916 /* Read registers up to 2 at a time */
 917 static int imx258_read_reg(struct imx258 *imx258, u16 reg, u32 len, u32 *val)
 918 {
 919         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 920         struct i2c_msg msgs[2];
 921         u8 addr_buf[2] = { reg >> 8, reg & 0xff };
 922         u8 data_buf[4] = { 0, };
 923         int ret;
 924
 925         if (len > 4)
 926                 return -EINVAL;
 927
 928         /* Write register address */
 929         msgs[0].addr = client->addr;
 930         msgs[0].flags = 0;
 931         msgs[0].len = ARRAY_SIZE(addr_buf);
 932         msgs[0].buf = addr_buf;
 933
 934         /* Read data from register */
 935         msgs[1].addr = client->addr;
 936         msgs[1].flags = I2C_M_RD;
 937         msgs[1].len = len;
 938         msgs[1].buf = &data_buf[4 - len];
 939
 940         ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 941         if (ret != ARRAY_SIZE(msgs))
 942                 return -EIO;
 943
 944         *val = get_unaligned_be32(data_buf);
 945
 946         return 0;
 947 }
 948
 949 /* Write registers up to 2 at a time */
 950 static int imx258_write_reg(struct imx258 *imx258, u16 reg, u32 len, u32 val)
 951 {
 952         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 953         u8 buf[6];
 954
 955         if (len > 4)
 956                 return -EINVAL;
 957
 958         put_unaligned_be16(reg, buf);
 959         put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
 960         if (i2c_master_send(client, buf, len + 2) != len + 2)
 961                 return -EIO;
 962
 963         return 0;
 964 }
 965
 966 /* Write a list of registers */
 967 static int imx258_write_regs(struct imx258 *imx258,
 968                              const struct imx258_reg *regs, u32 len)
 969 {
 970         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 971         unsigned int i;
 972         int ret;
 973
 974         for (i = 0; i < len; i++) {
 975                 ret = imx258_write_reg(imx258, regs[i].address, 1,
 976                                         regs[i].val);
 977                 if (ret) {
 978                         dev_err_ratelimited(
 979                                 &client->dev,
 980                                 "Failed to write reg 0x%4.4x. error = %d\n",
 981                                 regs[i].address, ret);
 982
 983                         return ret;
 984                 }
 985         }
 986
 987         return 0;
 988 }
 989
 990 /* Get bayer order based on flip setting. */
 991 static u32 imx258_get_format_code(struct imx258 *imx258)
 992 {
 993         unsigned int i;
 994
 995         lockdep_assert_held(&imx258->mutex);
 996
 997         i = (imx258->vflip->val ? 2 : 0) |
 998             (imx258->hflip->val ? 1 : 0);
 999
1000         return codes[i];
1001 }
1002 /* Open sub-device */
1003 static int imx258_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1004 {
1005         struct imx258 *imx258 = to_imx258(sd);
1006         struct v4l2_mbus_framefmt *try_fmt =
1007                 v4l2_subdev_get_try_format(sd, fh->state, 0);
1008         struct v4l2_rect *try_crop;
1009
1010         /* Initialize try_fmt */
1011         try_fmt->width = supported_modes[0].width;
1012         try_fmt->height = supported_modes[0].height;
1013         try_fmt->code = imx258_get_format_code(imx258);
1014         try_fmt->field = V4L2_FIELD_NONE;
1015
1016         /* Initialize try_crop */
1017         try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
1018         try_crop->left = IMX258_PIXEL_ARRAY_LEFT;
1019         try_crop->top = IMX258_PIXEL_ARRAY_TOP;
1020         try_crop->width = IMX258_PIXEL_ARRAY_WIDTH;
1021         try_crop->height = IMX258_PIXEL_ARRAY_HEIGHT;
1022
1023         return 0;
1024 }
1025
1026 static int imx258_update_digital_gain(struct imx258 *imx258, u32 len, u32 val)
1027 {
1028         int ret;
1029
1030         ret = imx258_write_reg(imx258, IMX258_REG_GR_DIGITAL_GAIN,
1031                                 IMX258_REG_VALUE_16BIT,
1032                                 val);
1033         if (ret)
1034                 return ret;
1035         ret = imx258_write_reg(imx258, IMX258_REG_GB_DIGITAL_GAIN,
1036                                 IMX258_REG_VALUE_16BIT,
1037                                 val);
1038         if (ret)
1039                 return ret;
1040         ret = imx258_write_reg(imx258, IMX258_REG_R_DIGITAL_GAIN,
1041                                 IMX258_REG_VALUE_16BIT,
1042                                 val);
1043         if (ret)
1044                 return ret;
1045         ret = imx258_write_reg(imx258, IMX258_REG_B_DIGITAL_GAIN,
1046                                 IMX258_REG_VALUE_16BIT,
1047                                 val);
1048         if (ret)
1049                 return ret;
1050         return 0;
1051 }
1052
1053 static void imx258_adjust_exposure_range(struct imx258 *imx258)
1054 {
1055         int exposure_max, exposure_def;
1056
1057         /* Honour the VBLANK limits when setting exposure. */
1058         exposure_max = imx258->cur_mode->height + imx258->vblank->val -
1059                        IMX258_EXPOSURE_OFFSET;
1060         exposure_def = min(exposure_max, imx258->exposure->val);
1061         __v4l2_ctrl_modify_range(imx258->exposure, imx258->exposure->minimum,
1062                                  exposure_max, imx258->exposure->step,
1063                                  exposure_def);
1064 }
1065
1066 static int imx258_set_frame_length(struct imx258 *imx258, unsigned int val)
1067 {
1068         int ret;
1069
1070         imx258->long_exp_shift = 0;
1071
1072         while (val > IMX258_VTS_MAX) {
1073                 imx258->long_exp_shift++;
1074                 val >>= 1;
1075         }
1076
1077         ret = imx258_write_reg(imx258, IMX258_REG_VTS,
1078                                IMX258_REG_VALUE_16BIT, val);
1079         if (ret)
1080                 return ret;
1081
1082         return imx258_write_reg(imx258, IMX258_LONG_EXP_SHIFT_REG,
1083                                 IMX258_REG_VALUE_08BIT, imx258->long_exp_shift);
1084 }
1085
1086 static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
1087 {
1088         struct imx258 *imx258 =
1089                 container_of(ctrl->handler, struct imx258, ctrl_handler);
1090         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1091         int ret = 0;
1092
1093         /*
1094          * The VBLANK control may change the limits of usable exposure, so check
1095          * and adjust if necessary.
1096          */
1097         if (ctrl->id == V4L2_CID_VBLANK)
1098                 imx258_adjust_exposure_range(imx258);
1099
1100         /*
1101          * Applying V4L2 control value only happens
1102          * when power is up for streaming
1103          */
1104         if (pm_runtime_get_if_in_use(&client->dev) == 0)
1105                 return 0;
1106
1107         switch (ctrl->id) {
1108         case V4L2_CID_ANALOGUE_GAIN:
1109                 ret = imx258_write_reg(imx258, IMX258_REG_ANALOG_GAIN,
1110                                 IMX258_REG_VALUE_16BIT,
1111                                 ctrl->val);
1112                 break;
1113         case V4L2_CID_EXPOSURE:
1114                 ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
1115                                 IMX258_REG_VALUE_16BIT,
1116                                 ctrl->val >> imx258->long_exp_shift);
1117                 break;
1118         case V4L2_CID_DIGITAL_GAIN:
1119                 ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
1120                                 ctrl->val);
1121                 break;
1122         case V4L2_CID_TEST_PATTERN:
1123                 ret = imx258_write_reg(imx258, IMX258_REG_TEST_PATTERN,
1124                                 IMX258_REG_VALUE_16BIT,
1125                                 ctrl->val);
1126                 break;
1127         case V4L2_CID_WIDE_DYNAMIC_RANGE:
1128                 if (!ctrl->val) {
1129                         ret = imx258_write_reg(imx258, IMX258_REG_HDR,
1130                                                IMX258_REG_VALUE_08BIT,
1131                                                IMX258_HDR_RATIO_MIN);
1132                 } else {
1133                         ret = imx258_write_reg(imx258, IMX258_REG_HDR,
1134                                                IMX258_REG_VALUE_08BIT,
1135                                                IMX258_HDR_ON);
1136                         if (ret)
1137                                 break;
1138                         ret = imx258_write_reg(imx258, IMX258_REG_HDR_RATIO,
1139                                                IMX258_REG_VALUE_08BIT,
1140                                                BIT(IMX258_HDR_RATIO_MAX));
1141                 }
1142                 break;
1143         case V4L2_CID_VBLANK:
1144                 ret = imx258_set_frame_length(imx258,
1145                                               imx258->cur_mode->height + ctrl->val);
1146                 break;
1147         case V4L2_CID_VFLIP:
1148         case V4L2_CID_HFLIP:
1149                 ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
1150                                        IMX258_REG_VALUE_08BIT,
1151                                        (imx258->hflip->val ?
1152                                         REG_CONFIG_MIRROR_HFLIP : 0) |
1153                                        (imx258->vflip->val ?
1154                                         REG_CONFIG_MIRROR_VFLIP : 0));
1155                 break;
1156         default:
1157                 dev_info(&client->dev,
1158                          "ctrl(id:0x%x,val:0x%x) is not handled\n",
1159                          ctrl->id, ctrl->val);
1160                 ret = -EINVAL;
1161                 break;
1162         }
1163
1164         pm_runtime_put(&client->dev);
1165
1166         return ret;
1167 }
1168
1169 static const struct v4l2_ctrl_ops imx258_ctrl_ops = {
1170         .s_ctrl = imx258_set_ctrl,
1171 };
1172
1173 static int imx258_enum_mbus_code(struct v4l2_subdev *sd,
1174                                   struct v4l2_subdev_state *sd_state,
1175                                   struct v4l2_subdev_mbus_code_enum *code)
1176 {
1177         struct imx258 *imx258 = to_imx258(sd);
1178
1179         /* Only one bayer format (10 bit) is supported */
1180         if (code->index > 0)
1181                 return -EINVAL;
1182
1183         code->code = imx258_get_format_code(imx258);
1184
1185         return 0;
1186 }
1187
1188 static int imx258_enum_frame_size(struct v4l2_subdev *sd,
1189                                   struct v4l2_subdev_state *sd_state,
1190                                   struct v4l2_subdev_frame_size_enum *fse)
1191 {
1192         struct imx258 *imx258 = to_imx258(sd);
1193         if (fse->index >= ARRAY_SIZE(supported_modes))
1194                 return -EINVAL;
1195
1196         if (fse->code != imx258_get_format_code(imx258))
1197                 return -EINVAL;
1198
1199         fse->min_width = supported_modes[fse->index].width;
1200         fse->max_width = fse->min_width;
1201         fse->min_height = supported_modes[fse->index].height;
1202         fse->max_height = fse->min_height;
1203
1204         return 0;
1205 }
1206
1207 static void imx258_update_pad_format(struct imx258 *imx258,
1208                                      const struct imx258_mode *mode,
1209                                      struct v4l2_subdev_format *fmt)
1210 {
1211         fmt->format.width = mode->width;
1212         fmt->format.height = mode->height;
1213         fmt->format.code = imx258_get_format_code(imx258);
1214         fmt->format.field = V4L2_FIELD_NONE;
1215 }
1216
1217 static int __imx258_get_pad_format(struct imx258 *imx258,
1218                                    struct v4l2_subdev_state *sd_state,
1219                                    struct v4l2_subdev_format *fmt)
1220 {
1221         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
1222                 fmt->format = *v4l2_subdev_get_try_format(&imx258->sd,
1223                                                           sd_state,
1224                                                           fmt->pad);
1225         else
1226                 imx258_update_pad_format(imx258, imx258->cur_mode, fmt);
1227
1228         return 0;
1229 }
1230
1231 static int imx258_get_pad_format(struct v4l2_subdev *sd,
1232                                  struct v4l2_subdev_state *sd_state,
1233                                  struct v4l2_subdev_format *fmt)
1234 {
1235         struct imx258 *imx258 = to_imx258(sd);
1236         int ret;
1237
1238         mutex_lock(&imx258->mutex);
1239         ret = __imx258_get_pad_format(imx258, sd_state, fmt);
1240         mutex_unlock(&imx258->mutex);
1241
1242         return ret;
1243 }
1244
1245 static int imx258_set_pad_format(struct v4l2_subdev *sd,
1246                                  struct v4l2_subdev_state *sd_state,
1247                                  struct v4l2_subdev_format *fmt)
1248 {
1249         struct imx258 *imx258 = to_imx258(sd);
1250         const struct imx258_link_freq_config *link_freq_cfgs;
1251         const struct imx258_link_cfg *link_cfg;
1252         struct v4l2_mbus_framefmt *framefmt;
1253         const struct imx258_mode *mode;
1254         s32 vblank_def;
1255         s32 vblank_min;
1256         s64 h_blank;
1257         s64 pixel_rate;
1258         s64 link_freq;
1259
1260         mutex_lock(&imx258->mutex);
1261
1262         fmt->format.code = imx258_get_format_code(imx258);
1263
1264         mode = v4l2_find_nearest_size(supported_modes,
1265                 ARRAY_SIZE(supported_modes), width, height,
1266                 fmt->format.width, fmt->format.height);
1267         imx258_update_pad_format(imx258, mode, fmt);
1268         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1269                 framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
1270                 *framefmt = fmt->format;
1271         } else {
1272                 imx258->cur_mode = mode;
1273                 __v4l2_ctrl_s_ctrl(imx258->link_freq, mode->link_freq_index);
1274
1275                 link_freq = imx258->link_freq_menu_items[mode->link_freq_index];
1276                 link_freq_cfgs =
1277                         &imx258->link_freq_configs[mode->link_freq_index];
1278
1279                 link_cfg = &link_freq_cfgs->link_cfg[imx258->lane_mode_idx];
1280                 pixel_rate = link_freq_to_pixel_rate(link_freq, link_cfg);
1281                 __v4l2_ctrl_modify_range(imx258->pixel_rate, pixel_rate,
1282                                          pixel_rate, 1, pixel_rate);
1283                 /* Update limits and set FPS to default */
1284                 vblank_def = imx258->cur_mode->vts_def -
1285                              imx258->cur_mode->height;
1286                 vblank_min = imx258->cur_mode->vts_min -
1287                              imx258->cur_mode->height;
1288                 __v4l2_ctrl_modify_range(
1289                         imx258->vblank, vblank_min,
1290                         ((1 << IMX258_LONG_EXP_SHIFT_MAX) * IMX258_VTS_MAX) -
1291                                                 imx258->cur_mode->height,
1292                         1, vblank_def);
1293                 __v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
1294                 h_blank =
1295                         imx258->link_freq_configs[mode->link_freq_index].pixels_per_line
1296                          - imx258->cur_mode->width;
1297                 __v4l2_ctrl_modify_range(imx258->hblank, h_blank,
1298                                          h_blank, 1, h_blank);
1299         }
1300
1301         mutex_unlock(&imx258->mutex);
1302
1303         return 0;
1304 }
1305
1306 static const struct v4l2_rect *
1307 __imx258_get_pad_crop(struct imx258 *imx258,
1308                       struct v4l2_subdev_state *sd_state,
1309                       unsigned int pad, enum v4l2_subdev_format_whence which)
1310 {
1311         switch (which) {
1312         case V4L2_SUBDEV_FORMAT_TRY:
1313                 return v4l2_subdev_get_try_crop(&imx258->sd, sd_state, pad);
1314         case V4L2_SUBDEV_FORMAT_ACTIVE:
1315                 return &imx258->cur_mode->crop;
1316         }
1317
1318         return NULL;
1319 }
1320
1321 static int imx258_get_selection(struct v4l2_subdev *sd,
1322                                 struct v4l2_subdev_state *sd_state,
1323                                 struct v4l2_subdev_selection *sel)
1324 {
1325         switch (sel->target) {
1326         case V4L2_SEL_TGT_CROP: {
1327                 struct imx258 *imx258 = to_imx258(sd);
1328
1329                 mutex_lock(&imx258->mutex);
1330                 sel->r = *__imx258_get_pad_crop(imx258, sd_state, sel->pad,
1331                                                 sel->which);
1332                 mutex_unlock(&imx258->mutex);
1333
1334                 return 0;
1335         }
1336
1337         case V4L2_SEL_TGT_NATIVE_SIZE:
1338                 sel->r.left = 0;
1339                 sel->r.top = 0;
1340                 sel->r.width = IMX258_NATIVE_WIDTH;
1341                 sel->r.height = IMX258_NATIVE_HEIGHT;
1342
1343                 return 0;
1344
1345         case V4L2_SEL_TGT_CROP_DEFAULT:
1346         case V4L2_SEL_TGT_CROP_BOUNDS:
1347                 sel->r.left = IMX258_PIXEL_ARRAY_LEFT;
1348                 sel->r.top = IMX258_PIXEL_ARRAY_TOP;
1349                 sel->r.width = IMX258_PIXEL_ARRAY_WIDTH;
1350                 sel->r.height = IMX258_PIXEL_ARRAY_HEIGHT;
1351
1352                 return 0;
1353         }
1354
1355         return -EINVAL;
1356 }
1357
1358 /* Start streaming */
1359 static int imx258_start_streaming(struct imx258 *imx258)
1360 {
1361         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1362         const struct imx258_reg_list *reg_list;
1363         const struct imx258_link_freq_config *link_freq_cfg;
1364         int ret, link_freq_index;
1365
1366         ret = imx258_write_reg(imx258, IMX258_REG_RESET, IMX258_REG_VALUE_08BIT,
1367                                0x01);
1368         if (ret) {
1369                 dev_err(&client->dev, "%s failed to reset sensor\n", __func__);
1370                 return ret;
1371         }
1372         usleep_range(10000, 15000);
1373
1374         /* Setup PLL */
1375         link_freq_index = imx258->cur_mode->link_freq_index;
1376         link_freq_cfg = &imx258->link_freq_configs[link_freq_index];
1377
1378         reg_list = &link_freq_cfg->link_cfg[imx258->lane_mode_idx].reg_list;
1379         ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
1380         if (ret) {
1381                 dev_err(&client->dev, "%s failed to set plls\n", __func__);
1382                 return ret;
1383         }
1384
1385         ret = imx258_write_regs(imx258, imx258->variant_cfg->regs,
1386                                 imx258->variant_cfg->num_regs);
1387         if (ret) {
1388                 dev_err(&client->dev, "%s failed to set variant config\n",
1389                         __func__);
1390                 return ret;
1391         }
1392
1393         ret = imx258_write_reg(imx258, IMX258_CLK_BLANK_STOP,
1394                                IMX258_REG_VALUE_08BIT,
1395                                imx258->csi2_flags & V4L2_MBUS_CSI2_CONTINUOUS_CLOCK ?
1396                                0 : 1);
1397         if (ret) {
1398                 dev_err(&client->dev, "%s failed to set clock lane mode\n", __func__);
1399                 return ret;
1400         }
1401
1402         /* Apply default values of current mode */
1403         reg_list = &imx258->cur_mode->reg_list;
1404         ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
1405         if (ret) {
1406                 dev_err(&client->dev, "%s failed to set mode\n", __func__);
1407                 return ret;
1408         }
1409
1410         /* Apply customized values from user */
1411         ret =  __v4l2_ctrl_handler_setup(imx258->sd.ctrl_handler);
1412         if (ret)
1413                 return ret;
1414
1415         /* set stream on register */
1416         return imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
1417                                 IMX258_REG_VALUE_08BIT,
1418                                 IMX258_MODE_STREAMING);
1419 }
1420
1421 /* Stop streaming */
1422 static int imx258_stop_streaming(struct imx258 *imx258)
1423 {
1424         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1425         int ret;
1426
1427         /* set stream off register */
1428         ret = imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
1429                 IMX258_REG_VALUE_08BIT, IMX258_MODE_STANDBY);
1430         if (ret)
1431                 dev_err(&client->dev, "%s failed to set stream\n", __func__);
1432
1433         /*
1434          * Return success even if it was an error, as there is nothing the
1435          * caller can do about it.
1436          */
1437         return 0;
1438 }
1439
1440 static int imx258_power_on(struct device *dev)
1441 {
1442         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1443         struct imx258 *imx258 = to_imx258(sd);
1444         int ret;
1445
1446         ret = regulator_bulk_enable(IMX258_NUM_SUPPLIES,
1447                                     imx258->supplies);
1448         if (ret) {
1449                 dev_err(dev, "%s: failed to enable regulators\n",
1450                         __func__);
1451                 return ret;
1452         }
1453
1454         ret = clk_prepare_enable(imx258->clk);
1455         if (ret) {
1456                 dev_err(dev, "failed to enable clock\n");
1457                 regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
1458         }
1459
1460         return ret;
1461 }
1462
1463 static int imx258_power_off(struct device *dev)
1464 {
1465         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1466         struct imx258 *imx258 = to_imx258(sd);
1467
1468         clk_disable_unprepare(imx258->clk);
1469         regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
1470
1471         return 0;
1472 }
1473
1474 static int imx258_set_stream(struct v4l2_subdev *sd, int enable)
1475 {
1476         struct imx258 *imx258 = to_imx258(sd);
1477         struct i2c_client *client = v4l2_get_subdevdata(sd);
1478         int ret = 0;
1479
1480         mutex_lock(&imx258->mutex);
1481         if (imx258->streaming == enable) {
1482                 mutex_unlock(&imx258->mutex);
1483                 return 0;
1484         }
1485
1486         if (enable) {
1487                 ret = pm_runtime_resume_and_get(&client->dev);
1488                 if (ret < 0)
1489                         goto err_unlock;
1490
1491                 /*
1492                  * Apply default & customized values
1493                  * and then start streaming.
1494                  */
1495                 ret = imx258_start_streaming(imx258);
1496                 if (ret)
1497                         goto err_rpm_put;
1498         } else {
1499                 imx258_stop_streaming(imx258);
1500                 pm_runtime_put(&client->dev);
1501         }
1502
1503         imx258->streaming = enable;
1504         mutex_unlock(&imx258->mutex);
1505
1506         return ret;
1507
1508 err_rpm_put:
1509         pm_runtime_put(&client->dev);
1510 err_unlock:
1511         mutex_unlock(&imx258->mutex);
1512
1513         return ret;
1514 }
1515
1516 static int __maybe_unused imx258_suspend(struct device *dev)
1517 {
1518         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1519         struct imx258 *imx258 = to_imx258(sd);
1520
1521         if (imx258->streaming)
1522                 imx258_stop_streaming(imx258);
1523
1524         return 0;
1525 }
1526
1527 static int __maybe_unused imx258_resume(struct device *dev)
1528 {
1529         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1530         struct imx258 *imx258 = to_imx258(sd);
1531         int ret;
1532
1533         if (imx258->streaming) {
1534                 ret = imx258_start_streaming(imx258);
1535                 if (ret)
1536                         goto error;
1537         }
1538
1539         return 0;
1540
1541 error:
1542         imx258_stop_streaming(imx258);
1543         imx258->streaming = 0;
1544         return ret;
1545 }
1546
1547 /* Verify chip ID */
1548 static int imx258_identify_module(struct imx258 *imx258)
1549 {
1550         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1551         int ret;
1552         u32 val;
1553
1554         ret = imx258_read_reg(imx258, IMX258_REG_CHIP_ID,
1555                               IMX258_REG_VALUE_16BIT, &val);
1556         if (ret) {
1557                 dev_err(&client->dev, "failed to read chip id %x\n",
1558                         IMX258_CHIP_ID);
1559                 return ret;
1560         }
1561
1562         if (val != IMX258_CHIP_ID) {
1563                 dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
1564                         IMX258_CHIP_ID, val);
1565                 return -EIO;
1566         }
1567
1568         return 0;
1569 }
1570
1571 static const struct v4l2_subdev_video_ops imx258_video_ops = {
1572         .s_stream = imx258_set_stream,
1573 };
1574
1575 static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
1576         .enum_mbus_code = imx258_enum_mbus_code,
1577         .get_fmt = imx258_get_pad_format,
1578         .set_fmt = imx258_set_pad_format,
1579         .enum_frame_size = imx258_enum_frame_size,
1580         .get_selection = imx258_get_selection,
1581 };
1582
1583 static const struct v4l2_subdev_ops imx258_subdev_ops = {
1584         .video = &imx258_video_ops,
1585         .pad = &imx258_pad_ops,
1586 };
1587
1588 static const struct v4l2_subdev_internal_ops imx258_internal_ops = {
1589         .open = imx258_open,
1590 };
1591
1592 /* Initialize control handlers */
1593 static int imx258_init_controls(struct imx258 *imx258)
1594 {
1595         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1596         const struct imx258_link_freq_config *link_freq_cfgs;
1597         struct v4l2_fwnode_device_properties props;
1598         struct v4l2_ctrl_handler *ctrl_hdlr;
1599         const struct imx258_link_cfg *link_cfg;
1600         s64 vblank_def;
1601         s64 vblank_min;
1602         s64 pixel_rate;
1603         int ret;
1604
1605         ctrl_hdlr = &imx258->ctrl_handler;
1606         ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
1607         if (ret)
1608                 return ret;
1609
1610         mutex_init(&imx258->mutex);
1611         ctrl_hdlr->lock = &imx258->mutex;
1612         imx258->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
1613                                 &imx258_ctrl_ops,
1614                                 V4L2_CID_LINK_FREQ,
1615                                 ARRAY_SIZE(link_freq_menu_items_19_2) - 1,
1616                                 0,
1617                                 imx258->link_freq_menu_items);
1618
1619         if (imx258->link_freq)
1620                 imx258->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1621
1622         link_freq_cfgs = &imx258->link_freq_configs[0];
1623         link_cfg = link_freq_cfgs[imx258->lane_mode_idx].link_cfg;
1624         pixel_rate = link_freq_to_pixel_rate(imx258->link_freq_menu_items[0],
1625                                              link_cfg);
1626
1627         /* By default, PIXEL_RATE is read only */
1628         imx258->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1629                                 V4L2_CID_PIXEL_RATE,
1630                                 pixel_rate, pixel_rate,
1631                                 1, pixel_rate);
1632
1633
1634         vblank_def = imx258->cur_mode->vts_def - imx258->cur_mode->height;
1635         vblank_min = imx258->cur_mode->vts_min - imx258->cur_mode->height;
1636         imx258->vblank = v4l2_ctrl_new_std(
1637                                 ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_VBLANK,
1638                                 vblank_min,
1639                                 IMX258_VTS_MAX - imx258->cur_mode->height, 1,
1640                                 vblank_def);
1641
1642         imx258->hblank = v4l2_ctrl_new_std(
1643                                 ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_HBLANK,
1644                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width,
1645                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width,
1646                                 1,
1647                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width);
1648
1649         if (imx258->hblank)
1650                 imx258->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1651
1652         imx258->exposure = v4l2_ctrl_new_std(
1653                                 ctrl_hdlr, &imx258_ctrl_ops,
1654                                 V4L2_CID_EXPOSURE, IMX258_EXPOSURE_MIN,
1655                                 IMX258_EXPOSURE_MAX, IMX258_EXPOSURE_STEP,
1656                                 IMX258_EXPOSURE_DEFAULT);
1657
1658         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
1659                                 IMX258_ANA_GAIN_MIN, IMX258_ANA_GAIN_MAX,
1660                                 IMX258_ANA_GAIN_STEP, IMX258_ANA_GAIN_DEFAULT);
1661
1662         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
1663                                 IMX258_DGTL_GAIN_MIN, IMX258_DGTL_GAIN_MAX,
1664                                 IMX258_DGTL_GAIN_STEP,
1665                                 IMX258_DGTL_GAIN_DEFAULT);
1666
1667         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_WIDE_DYNAMIC_RANGE,
1668                                 0, 1, 1, IMX258_HDR_RATIO_DEFAULT);
1669
1670         v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx258_ctrl_ops,
1671                                 V4L2_CID_TEST_PATTERN,
1672                                 ARRAY_SIZE(imx258_test_pattern_menu) - 1,
1673                                 0, 0, imx258_test_pattern_menu);
1674
1675         ret = v4l2_fwnode_device_parse(&client->dev, &props);
1676         if (ret)
1677                 goto error;
1678         ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx258_ctrl_ops,
1679                                               &props);
1680         if (ret)
1681                 goto error;
1682
1683         imx258->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1684                                           V4L2_CID_HFLIP, 0, 1, 1,
1685                                           props.rotation == 180 ? 1 : 0);
1686         if (imx258->hflip)
1687                 imx258->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1688
1689         imx258->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1690                                           V4L2_CID_VFLIP, 0, 1, 1,
1691                                           props.rotation == 180 ? 1 : 0);
1692         if (imx258->vflip)
1693                 imx258->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1694         if (ctrl_hdlr->error) {
1695                 ret = ctrl_hdlr->error;
1696                 dev_err(&client->dev, "%s control init failed (%d)\n",
1697                                 __func__, ret);
1698                 goto error;
1699         }
1700
1701         imx258->sd.ctrl_handler = ctrl_hdlr;
1702
1703         return 0;
1704
1705 error:
1706         v4l2_ctrl_handler_free(ctrl_hdlr);
1707         mutex_destroy(&imx258->mutex);
1708
1709         return ret;
1710 }
1711
1712 static void imx258_free_controls(struct imx258 *imx258)
1713 {
1714         v4l2_ctrl_handler_free(imx258->sd.ctrl_handler);
1715         mutex_destroy(&imx258->mutex);
1716 }
1717
1718 static int imx258_get_regulators(struct imx258 *imx258,
1719                                  struct i2c_client *client)
1720 {
1721         unsigned int i;
1722
1723         for (i = 0; i < IMX258_NUM_SUPPLIES; i++)
1724                 imx258->supplies[i].supply = imx258_supply_name[i];
1725
1726         return devm_regulator_bulk_get(&client->dev,
1727                                        IMX258_NUM_SUPPLIES,
1728                                        imx258->supplies);
1729 }
1730
1731 static const struct of_device_id imx258_dt_ids[] = {
1732         { .compatible = "sony,imx258", .data = &imx258_cfg },
1733         { .compatible = "sony,imx258-pdaf", .data = &imx258_pdaf_cfg },
1734         { /* sentinel */ }
1735 };
1736
1737 static int imx258_probe(struct i2c_client *client)
1738 {
1739         struct imx258 *imx258;
1740         struct fwnode_handle *endpoint;
1741         struct v4l2_fwnode_endpoint ep = {
1742                 .bus_type = V4L2_MBUS_CSI2_DPHY
1743         };
1744         const struct of_device_id *match;
1745         int ret;
1746         u32 val = 0;
1747
1748         imx258 = devm_kzalloc(&client->dev, sizeof(*imx258), GFP_KERNEL);
1749         if (!imx258)
1750                 return -ENOMEM;
1751
1752         ret = imx258_get_regulators(imx258, client);
1753         if (ret)
1754                 return ret;
1755
1756         imx258->clk = devm_clk_get_optional(&client->dev, NULL);
1757         if (IS_ERR(imx258->clk))
1758                 return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
1759                                      "error getting clock\n");
1760         if (!imx258->clk) {
1761                 dev_dbg(&client->dev,
1762                         "no clock provided, using clock-frequency property\n");
1763
1764                 device_property_read_u32(&client->dev, "clock-frequency", &val);
1765         } else if (IS_ERR(imx258->clk)) {
1766                 return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
1767                                      "error getting clock\n");
1768         } else {
1769                 val = clk_get_rate(imx258->clk);
1770         }
1771
1772         switch (val) {
1773         case 19200000:
1774                 imx258->link_freq_configs = link_freq_configs_19_2;
1775                 imx258->link_freq_menu_items = link_freq_menu_items_19_2;
1776                 break;
1777         case 24000000:
1778                 imx258->link_freq_configs = link_freq_configs_24;
1779                 imx258->link_freq_menu_items = link_freq_menu_items_24;
1780                 break;
1781         default:
1782                 dev_err(&client->dev, "input clock frequency of %u not supported\n",
1783                         val);
1784                 return -EINVAL;
1785         }
1786
1787         endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
1788         if (!endpoint) {
1789                 dev_err(&client->dev, "Endpoint node not found\n");
1790                 return -EINVAL;
1791         }
1792
1793         ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
1794         fwnode_handle_put(endpoint);
1795         if (ret == -ENXIO) {
1796                 dev_err(&client->dev, "Unsupported bus type, should be CSI2\n");
1797                 goto error_endpoint_poweron;
1798         } else if (ret) {
1799                 dev_err(&client->dev, "Parsing endpoint node failed\n");
1800                 goto error_endpoint_poweron;
1801         }
1802
1803         /* Get number of data lanes */
1804         switch (ep.bus.mipi_csi2.num_data_lanes) {
1805         case 2:
1806                 imx258->lane_mode_idx = IMX258_2_LANE_MODE;
1807                 break;
1808         case 4:
1809                 imx258->lane_mode_idx = IMX258_4_LANE_MODE;
1810                 break;
1811         default:
1812                 dev_err(&client->dev, "Invalid data lanes: %u\n",
1813                         ep.bus.mipi_csi2.num_data_lanes);
1814                 ret = -EINVAL;
1815                 goto error_endpoint_poweron;
1816         }
1817
1818         imx258->csi2_flags = ep.bus.mipi_csi2.flags;
1819
1820         match = i2c_of_match_device(imx258_dt_ids, client);
1821         if (!match || !match->data)
1822                 imx258->variant_cfg = &imx258_cfg;
1823         else
1824                 imx258->variant_cfg =
1825                         (const struct imx258_variant_cfg *)match->data;
1826
1827         /* Initialize subdev */
1828         v4l2_i2c_subdev_init(&imx258->sd, client, &imx258_subdev_ops);
1829
1830         /* Will be powered off via pm_runtime_idle */
1831         ret = imx258_power_on(&client->dev);
1832         if (ret)
1833                 goto error_endpoint_poweron;
1834
1835         /* Check module identity */
1836         ret = imx258_identify_module(imx258);
1837         if (ret)
1838                 goto error_identify;
1839
1840         /* Set default mode to max resolution */
1841         imx258->cur_mode = &supported_modes[0];
1842
1843         ret = imx258_init_controls(imx258);
1844         if (ret)
1845                 goto error_identify;
1846
1847         /* Initialize subdev */
1848         imx258->sd.internal_ops = &imx258_internal_ops;
1849         imx258->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1850         imx258->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1851
1852         /* Initialize source pad */
1853         imx258->pad.flags = MEDIA_PAD_FL_SOURCE;
1854
1855         ret = media_entity_pads_init(&imx258->sd.entity, 1, &imx258->pad);
1856         if (ret)
1857                 goto error_handler_free;
1858
1859         ret = v4l2_async_register_subdev_sensor(&imx258->sd);
1860         if (ret < 0)
1861                 goto error_media_entity;
1862
1863         pm_runtime_set_active(&client->dev);
1864         pm_runtime_enable(&client->dev);
1865         pm_runtime_idle(&client->dev);
1866
1867         return 0;
1868
1869 error_media_entity:
1870         media_entity_cleanup(&imx258->sd.entity);
1871
1872 error_handler_free:
1873         imx258_free_controls(imx258);
1874
1875 error_identify:
1876         imx258_power_off(&client->dev);
1877
1878 error_endpoint_poweron:
1879         v4l2_fwnode_endpoint_free(&ep);
1880
1881         return ret;
1882 }
1883
1884 static int imx258_remove(struct i2c_client *client)
1885 {
1886         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1887         struct imx258 *imx258 = to_imx258(sd);
1888
1889         v4l2_async_unregister_subdev(sd);
1890         media_entity_cleanup(&sd->entity);
1891         imx258_free_controls(imx258);
1892
1893         pm_runtime_disable(&client->dev);
1894         if (!pm_runtime_status_suspended(&client->dev))
1895                 imx258_power_off(&client->dev);
1896         pm_runtime_set_suspended(&client->dev);
1897
1898         return 0;
1899 }
1900
1901 static const struct dev_pm_ops imx258_pm_ops = {
1902         SET_SYSTEM_SLEEP_PM_OPS(imx258_suspend, imx258_resume)
1903         SET_RUNTIME_PM_OPS(imx258_power_off, imx258_power_on, NULL)
1904 };
1905
1906 #ifdef CONFIG_ACPI
1907 static const struct acpi_device_id imx258_acpi_ids[] = {
1908         { "SONY258A" },
1909         { /* sentinel */ }
1910 };
1911
1912 MODULE_DEVICE_TABLE(acpi, imx258_acpi_ids);
1913 #endif
1914
1915 MODULE_DEVICE_TABLE(of, imx258_dt_ids);
1916
1917 static struct i2c_driver imx258_i2c_driver = {
1918         .driver = {
1919                 .name = "imx258",
1920                 .pm = &imx258_pm_ops,
1921                 .acpi_match_table = ACPI_PTR(imx258_acpi_ids),
1922                 .of_match_table = imx258_dt_ids,
1923         },
1924         .probe_new = imx258_probe,
1925         .remove = imx258_remove,
1926 };
1927
1928 module_i2c_driver(imx258_i2c_driver);
1929
1930 MODULE_AUTHOR("Yeh, Andy <andy.yeh@intel.com>");
1931 MODULE_AUTHOR("Chiang, Alan");
1932 MODULE_AUTHOR("Chen, Jason");
1933 MODULE_DESCRIPTION("Sony IMX258 sensor driver");
1934 MODULE_LICENSE("GPL v2");