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 /* Chip ID */
  24 #define IMX258_REG_CHIP_ID              0x0016
  25 #define IMX258_CHIP_ID                  0x0258
  26
  27 /* V_TIMING internal */
  28 #define IMX258_VTS_30FPS                0x0c50
  29 #define IMX258_VTS_30FPS_2K             0x0638
  30 #define IMX258_VTS_30FPS_VGA            0x034c
  31 #define IMX258_VTS_MAX                  0xffff
  32
  33 #define IMX258_REG_VTS                  0x0340
  34
  35 /* HBLANK control - read only */
  36 #define IMX258_PPL_DEFAULT              5352
  37
  38 /* Exposure control */
  39 #define IMX258_REG_EXPOSURE             0x0202
  40 #define IMX258_EXPOSURE_MIN             4
  41 #define IMX258_EXPOSURE_STEP            1
  42 #define IMX258_EXPOSURE_DEFAULT         0x640
  43 #define IMX258_EXPOSURE_MAX             65535
  44
  45 /* Analog gain control */
  46 #define IMX258_REG_ANALOG_GAIN          0x0204
  47 #define IMX258_ANA_GAIN_MIN             0
  48 #define IMX258_ANA_GAIN_MAX             480
  49 #define IMX258_ANA_GAIN_STEP            1
  50 #define IMX258_ANA_GAIN_DEFAULT         0x0
  51
  52 /* Digital gain control */
  53 #define IMX258_REG_GR_DIGITAL_GAIN      0x020e
  54 #define IMX258_REG_R_DIGITAL_GAIN       0x0210
  55 #define IMX258_REG_B_DIGITAL_GAIN       0x0212
  56 #define IMX258_REG_GB_DIGITAL_GAIN      0x0214
  57 #define IMX258_DGTL_GAIN_MIN            0
  58 #define IMX258_DGTL_GAIN_MAX            4096    /* Max = 0xFFF */
  59 #define IMX258_DGTL_GAIN_DEFAULT        1024
  60 #define IMX258_DGTL_GAIN_STEP           1
  61
  62 /* HDR control */
  63 #define IMX258_REG_HDR                  0x0220
  64 #define IMX258_HDR_ON                   BIT(0)
  65 #define IMX258_REG_HDR_RATIO            0x0222
  66 #define IMX258_HDR_RATIO_MIN            0
  67 #define IMX258_HDR_RATIO_MAX            5
  68 #define IMX258_HDR_RATIO_STEP           1
  69 #define IMX258_HDR_RATIO_DEFAULT        0x0
  70
  71 /* Test Pattern Control */
  72 #define IMX258_REG_TEST_PATTERN         0x0600
  73
  74 /* Orientation */
  75 #define REG_MIRROR_FLIP_CONTROL         0x0101
  76 #define REG_CONFIG_MIRROR_FLIP          0x03
  77 #define REG_CONFIG_FLIP_TEST_PATTERN    0x02
  78
  79 struct imx258_reg {
  80         u16 address;
  81         u8 val;
  82 };
  83
  84 struct imx258_reg_list {
  85         u32 num_of_regs;
  86         const struct imx258_reg *regs;
  87 };
  88
  89 /* Link frequency config */
  90 struct imx258_link_freq_config {
  91         u64 link_frequency;
  92         u32 pixels_per_line;
  93
  94         /* PLL registers for this link frequency */
  95         struct imx258_reg_list reg_list;
  96 };
  97
  98 /* Mode : resolution and related config&values */
  99 struct imx258_mode {
 100         /* Frame width */
 101         u32 width;
 102         /* Frame height */
 103         u32 height;
 104
 105         /* V-timing */
 106         u32 vts_def;
 107         u32 vts_min;
 108
 109         /* Index of Link frequency config to be used */
 110         u32 link_freq_index;
 111         /* Default register values */
 112         struct imx258_reg_list reg_list;
 113 };
 114
 115 /* 4208x3120 needs 1267Mbps/lane, 4 lanes */
 116 static const struct imx258_reg mipi_1267mbps_19_2mhz[] = {
 117         { 0x0136, 0x13 },
 118         { 0x0137, 0x33 },
 119         { 0x0301, 0x05 },
 120         { 0x0303, 0x02 },
 121         { 0x0305, 0x03 },
 122         { 0x0306, 0x00 },
 123         { 0x0307, 0xC6 },
 124         { 0x0309, 0x0A },
 125         { 0x030B, 0x01 },
 126         { 0x030D, 0x02 },
 127         { 0x030E, 0x00 },
 128         { 0x030F, 0xD8 },
 129         { 0x0310, 0x00 },
 130         { 0x0820, 0x13 },
 131         { 0x0821, 0x4C },
 132         { 0x0822, 0xCC },
 133         { 0x0823, 0xCC },
 134 };
 135
 136 static const struct imx258_reg mipi_1272mbps_24mhz[] = {
 137         { 0x0136, 0x18 },
 138         { 0x0137, 0x00 },
 139         { 0x0301, 0x05 },
 140         { 0x0303, 0x02 },
 141         { 0x0305, 0x04 },
 142         { 0x0306, 0x00 },
 143         { 0x0307, 0xD4 },
 144         { 0x0309, 0x0A },
 145         { 0x030B, 0x01 },
 146         { 0x030D, 0x02 },
 147         { 0x030E, 0x00 },
 148         { 0x030F, 0xD8 },
 149         { 0x0310, 0x00 },
 150         { 0x0820, 0x13 },
 151         { 0x0821, 0x4C },
 152         { 0x0822, 0xCC },
 153         { 0x0823, 0xCC },
 154 };
 155
 156 static const struct imx258_reg mipi_640mbps_19_2mhz[] = {
 157         { 0x0136, 0x13 },
 158         { 0x0137, 0x33 },
 159         { 0x0301, 0x05 },
 160         { 0x0303, 0x02 },
 161         { 0x0305, 0x03 },
 162         { 0x0306, 0x00 },
 163         { 0x0307, 0x64 },
 164         { 0x0309, 0x0A },
 165         { 0x030B, 0x01 },
 166         { 0x030D, 0x02 },
 167         { 0x030E, 0x00 },
 168         { 0x030F, 0xD8 },
 169         { 0x0310, 0x00 },
 170         { 0x0820, 0x0A },
 171         { 0x0821, 0x00 },
 172         { 0x0822, 0x00 },
 173         { 0x0823, 0x00 },
 174 };
 175
 176 static const struct imx258_reg mipi_642mbps_24mhz[] = {
 177         { 0x0136, 0x18 },
 178         { 0x0137, 0x00 },
 179         { 0x0301, 0x05 },
 180         { 0x0303, 0x02 },
 181         { 0x0305, 0x04 },
 182         { 0x0306, 0x00 },
 183         { 0x0307, 0x6B },
 184         { 0x0309, 0x0A },
 185         { 0x030B, 0x01 },
 186         { 0x030D, 0x02 },
 187         { 0x030E, 0x00 },
 188         { 0x030F, 0xD8 },
 189         { 0x0310, 0x00 },
 190         { 0x0820, 0x0A },
 191         { 0x0821, 0x00 },
 192         { 0x0822, 0x00 },
 193         { 0x0823, 0x00 },
 194 };
 195
 196 static const struct imx258_reg mode_4208x3120_regs[] = {
 197         { 0x3051, 0x00 },
 198         { 0x3052, 0x00 },
 199         { 0x4E21, 0x14 },
 200         { 0x6B11, 0xCF },
 201         { 0x7FF0, 0x08 },
 202         { 0x7FF1, 0x0F },
 203         { 0x7FF2, 0x08 },
 204         { 0x7FF3, 0x1B },
 205         { 0x7FF4, 0x23 },
 206         { 0x7FF5, 0x60 },
 207         { 0x7FF6, 0x00 },
 208         { 0x7FF7, 0x01 },
 209         { 0x7FF8, 0x00 },
 210         { 0x7FF9, 0x78 },
 211         { 0x7FFA, 0x00 },
 212         { 0x7FFB, 0x00 },
 213         { 0x7FFC, 0x00 },
 214         { 0x7FFD, 0x00 },
 215         { 0x7FFE, 0x00 },
 216         { 0x7FFF, 0x03 },
 217         { 0x7F76, 0x03 },
 218         { 0x7F77, 0xFE },
 219         { 0x7FA8, 0x03 },
 220         { 0x7FA9, 0xFE },
 221         { 0x7B24, 0x81 },
 222         { 0x7B25, 0x00 },
 223         { 0x6564, 0x07 },
 224         { 0x6B0D, 0x41 },
 225         { 0x653D, 0x04 },
 226         { 0x6B05, 0x8C },
 227         { 0x6B06, 0xF9 },
 228         { 0x6B08, 0x65 },
 229         { 0x6B09, 0xFC },
 230         { 0x6B0A, 0xCF },
 231         { 0x6B0B, 0xD2 },
 232         { 0x6700, 0x0E },
 233         { 0x6707, 0x0E },
 234         { 0x9104, 0x00 },
 235         { 0x4648, 0x7F },
 236         { 0x7420, 0x00 },
 237         { 0x7421, 0x1C },
 238         { 0x7422, 0x00 },
 239         { 0x7423, 0xD7 },
 240         { 0x5F04, 0x00 },
 241         { 0x5F05, 0xED },
 242         { 0x0112, 0x0A },
 243         { 0x0113, 0x0A },
 244         { 0x0114, 0x03 },
 245         { 0x0342, 0x14 },
 246         { 0x0343, 0xE8 },
 247         { 0x0344, 0x00 },
 248         { 0x0345, 0x00 },
 249         { 0x0346, 0x00 },
 250         { 0x0347, 0x00 },
 251         { 0x0348, 0x10 },
 252         { 0x0349, 0x6F },
 253         { 0x034A, 0x0C },
 254         { 0x034B, 0x2F },
 255         { 0x0381, 0x01 },
 256         { 0x0383, 0x01 },
 257         { 0x0385, 0x01 },
 258         { 0x0387, 0x01 },
 259         { 0x0900, 0x00 },
 260         { 0x0901, 0x11 },
 261         { 0x0401, 0x00 },
 262         { 0x0404, 0x00 },
 263         { 0x0405, 0x10 },
 264         { 0x0408, 0x00 },
 265         { 0x0409, 0x00 },
 266         { 0x040A, 0x00 },
 267         { 0x040B, 0x00 },
 268         { 0x040C, 0x10 },
 269         { 0x040D, 0x70 },
 270         { 0x040E, 0x0C },
 271         { 0x040F, 0x30 },
 272         { 0x3038, 0x00 },
 273         { 0x303A, 0x00 },
 274         { 0x303B, 0x10 },
 275         { 0x300D, 0x00 },
 276         { 0x034C, 0x10 },
 277         { 0x034D, 0x70 },
 278         { 0x034E, 0x0C },
 279         { 0x034F, 0x30 },
 280         { 0x0350, 0x01 },
 281         { 0x0204, 0x00 },
 282         { 0x0205, 0x00 },
 283         { 0x020E, 0x01 },
 284         { 0x020F, 0x00 },
 285         { 0x0210, 0x01 },
 286         { 0x0211, 0x00 },
 287         { 0x0212, 0x01 },
 288         { 0x0213, 0x00 },
 289         { 0x0214, 0x01 },
 290         { 0x0215, 0x00 },
 291         { 0x7BCD, 0x00 },
 292         { 0x94DC, 0x20 },
 293         { 0x94DD, 0x20 },
 294         { 0x94DE, 0x20 },
 295         { 0x95DC, 0x20 },
 296         { 0x95DD, 0x20 },
 297         { 0x95DE, 0x20 },
 298         { 0x7FB0, 0x00 },
 299         { 0x9010, 0x3E },
 300         { 0x9419, 0x50 },
 301         { 0x941B, 0x50 },
 302         { 0x9519, 0x50 },
 303         { 0x951B, 0x50 },
 304         { 0x3030, 0x00 },
 305         { 0x3032, 0x00 },
 306         { 0x0220, 0x00 },
 307 };
 308
 309 static const struct imx258_reg mode_2104_1560_regs[] = {
 310         { 0x3051, 0x00 },
 311         { 0x3052, 0x00 },
 312         { 0x4E21, 0x14 },
 313         { 0x6B11, 0xCF },
 314         { 0x7FF0, 0x08 },
 315         { 0x7FF1, 0x0F },
 316         { 0x7FF2, 0x08 },
 317         { 0x7FF3, 0x1B },
 318         { 0x7FF4, 0x23 },
 319         { 0x7FF5, 0x60 },
 320         { 0x7FF6, 0x00 },
 321         { 0x7FF7, 0x01 },
 322         { 0x7FF8, 0x00 },
 323         { 0x7FF9, 0x78 },
 324         { 0x7FFA, 0x00 },
 325         { 0x7FFB, 0x00 },
 326         { 0x7FFC, 0x00 },
 327         { 0x7FFD, 0x00 },
 328         { 0x7FFE, 0x00 },
 329         { 0x7FFF, 0x03 },
 330         { 0x7F76, 0x03 },
 331         { 0x7F77, 0xFE },
 332         { 0x7FA8, 0x03 },
 333         { 0x7FA9, 0xFE },
 334         { 0x7B24, 0x81 },
 335         { 0x7B25, 0x00 },
 336         { 0x6564, 0x07 },
 337         { 0x6B0D, 0x41 },
 338         { 0x653D, 0x04 },
 339         { 0x6B05, 0x8C },
 340         { 0x6B06, 0xF9 },
 341         { 0x6B08, 0x65 },
 342         { 0x6B09, 0xFC },
 343         { 0x6B0A, 0xCF },
 344         { 0x6B0B, 0xD2 },
 345         { 0x6700, 0x0E },
 346         { 0x6707, 0x0E },
 347         { 0x9104, 0x00 },
 348         { 0x4648, 0x7F },
 349         { 0x7420, 0x00 },
 350         { 0x7421, 0x1C },
 351         { 0x7422, 0x00 },
 352         { 0x7423, 0xD7 },
 353         { 0x5F04, 0x00 },
 354         { 0x5F05, 0xED },
 355         { 0x0112, 0x0A },
 356         { 0x0113, 0x0A },
 357         { 0x0114, 0x03 },
 358         { 0x0342, 0x14 },
 359         { 0x0343, 0xE8 },
 360         { 0x0344, 0x00 },
 361         { 0x0345, 0x00 },
 362         { 0x0346, 0x00 },
 363         { 0x0347, 0x00 },
 364         { 0x0348, 0x10 },
 365         { 0x0349, 0x6F },
 366         { 0x034A, 0x0C },
 367         { 0x034B, 0x2F },
 368         { 0x0381, 0x01 },
 369         { 0x0383, 0x01 },
 370         { 0x0385, 0x01 },
 371         { 0x0387, 0x01 },
 372         { 0x0900, 0x01 },
 373         { 0x0901, 0x12 },
 374         { 0x0401, 0x01 },
 375         { 0x0404, 0x00 },
 376         { 0x0405, 0x20 },
 377         { 0x0408, 0x00 },
 378         { 0x0409, 0x00 },
 379         { 0x040A, 0x00 },
 380         { 0x040B, 0x00 },
 381         { 0x040C, 0x10 },
 382         { 0x040D, 0x70 },
 383         { 0x040E, 0x06 },
 384         { 0x040F, 0x18 },
 385         { 0x3038, 0x00 },
 386         { 0x303A, 0x00 },
 387         { 0x303B, 0x10 },
 388         { 0x300D, 0x00 },
 389         { 0x034C, 0x08 },
 390         { 0x034D, 0x38 },
 391         { 0x034E, 0x06 },
 392         { 0x034F, 0x18 },
 393         { 0x0350, 0x01 },
 394         { 0x0204, 0x00 },
 395         { 0x0205, 0x00 },
 396         { 0x020E, 0x01 },
 397         { 0x020F, 0x00 },
 398         { 0x0210, 0x01 },
 399         { 0x0211, 0x00 },
 400         { 0x0212, 0x01 },
 401         { 0x0213, 0x00 },
 402         { 0x0214, 0x01 },
 403         { 0x0215, 0x00 },
 404         { 0x7BCD, 0x01 },
 405         { 0x94DC, 0x20 },
 406         { 0x94DD, 0x20 },
 407         { 0x94DE, 0x20 },
 408         { 0x95DC, 0x20 },
 409         { 0x95DD, 0x20 },
 410         { 0x95DE, 0x20 },
 411         { 0x7FB0, 0x00 },
 412         { 0x9010, 0x3E },
 413         { 0x9419, 0x50 },
 414         { 0x941B, 0x50 },
 415         { 0x9519, 0x50 },
 416         { 0x951B, 0x50 },
 417         { 0x3030, 0x00 },
 418         { 0x3032, 0x00 },
 419         { 0x0220, 0x00 },
 420 };
 421
 422 static const struct imx258_reg mode_1048_780_regs[] = {
 423         { 0x3051, 0x00 },
 424         { 0x3052, 0x00 },
 425         { 0x4E21, 0x14 },
 426         { 0x6B11, 0xCF },
 427         { 0x7FF0, 0x08 },
 428         { 0x7FF1, 0x0F },
 429         { 0x7FF2, 0x08 },
 430         { 0x7FF3, 0x1B },
 431         { 0x7FF4, 0x23 },
 432         { 0x7FF5, 0x60 },
 433         { 0x7FF6, 0x00 },
 434         { 0x7FF7, 0x01 },
 435         { 0x7FF8, 0x00 },
 436         { 0x7FF9, 0x78 },
 437         { 0x7FFA, 0x00 },
 438         { 0x7FFB, 0x00 },
 439         { 0x7FFC, 0x00 },
 440         { 0x7FFD, 0x00 },
 441         { 0x7FFE, 0x00 },
 442         { 0x7FFF, 0x03 },
 443         { 0x7F76, 0x03 },
 444         { 0x7F77, 0xFE },
 445         { 0x7FA8, 0x03 },
 446         { 0x7FA9, 0xFE },
 447         { 0x7B24, 0x81 },
 448         { 0x7B25, 0x00 },
 449         { 0x6564, 0x07 },
 450         { 0x6B0D, 0x41 },
 451         { 0x653D, 0x04 },
 452         { 0x6B05, 0x8C },
 453         { 0x6B06, 0xF9 },
 454         { 0x6B08, 0x65 },
 455         { 0x6B09, 0xFC },
 456         { 0x6B0A, 0xCF },
 457         { 0x6B0B, 0xD2 },
 458         { 0x6700, 0x0E },
 459         { 0x6707, 0x0E },
 460         { 0x9104, 0x00 },
 461         { 0x4648, 0x7F },
 462         { 0x7420, 0x00 },
 463         { 0x7421, 0x1C },
 464         { 0x7422, 0x00 },
 465         { 0x7423, 0xD7 },
 466         { 0x5F04, 0x00 },
 467         { 0x5F05, 0xED },
 468         { 0x0112, 0x0A },
 469         { 0x0113, 0x0A },
 470         { 0x0114, 0x03 },
 471         { 0x0342, 0x14 },
 472         { 0x0343, 0xE8 },
 473         { 0x0344, 0x00 },
 474         { 0x0345, 0x00 },
 475         { 0x0346, 0x00 },
 476         { 0x0347, 0x00 },
 477         { 0x0348, 0x10 },
 478         { 0x0349, 0x6F },
 479         { 0x034A, 0x0C },
 480         { 0x034B, 0x2F },
 481         { 0x0381, 0x01 },
 482         { 0x0383, 0x01 },
 483         { 0x0385, 0x01 },
 484         { 0x0387, 0x01 },
 485         { 0x0900, 0x01 },
 486         { 0x0901, 0x14 },
 487         { 0x0401, 0x01 },
 488         { 0x0404, 0x00 },
 489         { 0x0405, 0x40 },
 490         { 0x0408, 0x00 },
 491         { 0x0409, 0x00 },
 492         { 0x040A, 0x00 },
 493         { 0x040B, 0x00 },
 494         { 0x040C, 0x10 },
 495         { 0x040D, 0x70 },
 496         { 0x040E, 0x03 },
 497         { 0x040F, 0x0C },
 498         { 0x3038, 0x00 },
 499         { 0x303A, 0x00 },
 500         { 0x303B, 0x10 },
 501         { 0x300D, 0x00 },
 502         { 0x034C, 0x04 },
 503         { 0x034D, 0x18 },
 504         { 0x034E, 0x03 },
 505         { 0x034F, 0x0C },
 506         { 0x0350, 0x01 },
 507         { 0x0204, 0x00 },
 508         { 0x0205, 0x00 },
 509         { 0x020E, 0x01 },
 510         { 0x020F, 0x00 },
 511         { 0x0210, 0x01 },
 512         { 0x0211, 0x00 },
 513         { 0x0212, 0x01 },
 514         { 0x0213, 0x00 },
 515         { 0x0214, 0x01 },
 516         { 0x0215, 0x00 },
 517         { 0x7BCD, 0x00 },
 518         { 0x94DC, 0x20 },
 519         { 0x94DD, 0x20 },
 520         { 0x94DE, 0x20 },
 521         { 0x95DC, 0x20 },
 522         { 0x95DD, 0x20 },
 523         { 0x95DE, 0x20 },
 524         { 0x7FB0, 0x00 },
 525         { 0x9010, 0x3E },
 526         { 0x9419, 0x50 },
 527         { 0x941B, 0x50 },
 528         { 0x9519, 0x50 },
 529         { 0x951B, 0x50 },
 530         { 0x3030, 0x00 },
 531         { 0x3032, 0x00 },
 532         { 0x0220, 0x00 },
 533 };
 534
 535 static const char * const imx258_test_pattern_menu[] = {
 536         "Disabled",
 537         "Solid Colour",
 538         "Eight Vertical Colour Bars",
 539         "Colour Bars With Fade to Grey",
 540         "Pseudorandom Sequence (PN9)",
 541 };
 542
 543 /* regulator supplies */
 544 static const char * const imx258_supply_name[] = {
 545         /* Supplies can be enabled in any order */
 546         "vana",  /* Analog (2.8V) supply */
 547         "vdig",  /* Digital Core (1.05V) supply */
 548         "vif",  /* IF (1.8V) supply */
 549 };
 550
 551 #define IMX258_NUM_SUPPLIES ARRAY_SIZE(imx258_supply_name)
 552
 553 enum {
 554         IMX258_LINK_FREQ_1267MBPS,
 555         IMX258_LINK_FREQ_640MBPS,
 556 };
 557
 558 /*
 559  * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
 560  * data rate => double data rate; number of lanes => 4; bits per pixel => 10
 561  */
 562 static u64 link_freq_to_pixel_rate(u64 f)
 563 {
 564         f *= 2 * 4;
 565         do_div(f, 10);
 566
 567         return f;
 568 }
 569
 570 /* Menu items for LINK_FREQ V4L2 control */
 571 /* Configurations for supported link frequencies */
 572 #define IMX258_LINK_FREQ_634MHZ 633600000ULL
 573 #define IMX258_LINK_FREQ_320MHZ 320000000ULL
 574
 575 static const s64 link_freq_menu_items_19_2[] = {
 576         IMX258_LINK_FREQ_634MHZ,
 577         IMX258_LINK_FREQ_320MHZ,
 578 };
 579
 580 /* Configurations for supported link frequencies */
 581 #define IMX258_LINK_FREQ_636MHZ 636000000ULL
 582 #define IMX258_LINK_FREQ_321MHZ 321000000ULL
 583
 584 static const s64 link_freq_menu_items_24[] = {
 585         IMX258_LINK_FREQ_636MHZ,
 586         IMX258_LINK_FREQ_321MHZ,
 587 };
 588
 589 /* Link frequency configs */
 590 static const struct imx258_link_freq_config link_freq_configs_19_2[] = {
 591         [IMX258_LINK_FREQ_1267MBPS] = {
 592                 .pixels_per_line = IMX258_PPL_DEFAULT,
 593                 .reg_list = {
 594                         .num_of_regs = ARRAY_SIZE(mipi_1267mbps_19_2mhz),
 595                         .regs = mipi_1267mbps_19_2mhz,
 596                 }
 597         },
 598         [IMX258_LINK_FREQ_640MBPS] = {
 599                 .pixels_per_line = IMX258_PPL_DEFAULT,
 600                 .reg_list = {
 601                         .num_of_regs = ARRAY_SIZE(mipi_640mbps_19_2mhz),
 602                         .regs = mipi_640mbps_19_2mhz,
 603                 }
 604         },
 605 };
 606
 607 static const struct imx258_link_freq_config link_freq_configs_24[] = {
 608         [IMX258_LINK_FREQ_1267MBPS] = {
 609                 .pixels_per_line = IMX258_PPL_DEFAULT,
 610                 .reg_list = {
 611                         .num_of_regs = ARRAY_SIZE(mipi_1272mbps_24mhz),
 612                         .regs = mipi_1272mbps_24mhz,
 613                 }
 614         },
 615         [IMX258_LINK_FREQ_640MBPS] = {
 616                 .pixels_per_line = IMX258_PPL_DEFAULT,
 617                 .reg_list = {
 618                         .num_of_regs = ARRAY_SIZE(mipi_642mbps_24mhz),
 619                         .regs = mipi_642mbps_24mhz,
 620                 }
 621         },
 622 };
 623
 624 /* Mode configs */
 625 static const struct imx258_mode supported_modes[] = {
 626         {
 627                 .width = 4208,
 628                 .height = 3120,
 629                 .vts_def = IMX258_VTS_30FPS,
 630                 .vts_min = IMX258_VTS_30FPS,
 631                 .reg_list = {
 632                         .num_of_regs = ARRAY_SIZE(mode_4208x3120_regs),
 633                         .regs = mode_4208x3120_regs,
 634                 },
 635                 .link_freq_index = IMX258_LINK_FREQ_1267MBPS,
 636         },
 637         {
 638                 .width = 2104,
 639                 .height = 1560,
 640                 .vts_def = IMX258_VTS_30FPS_2K,
 641                 .vts_min = IMX258_VTS_30FPS_2K,
 642                 .reg_list = {
 643                         .num_of_regs = ARRAY_SIZE(mode_2104_1560_regs),
 644                         .regs = mode_2104_1560_regs,
 645                 },
 646                 .link_freq_index = IMX258_LINK_FREQ_640MBPS,
 647         },
 648         {
 649                 .width = 1048,
 650                 .height = 780,
 651                 .vts_def = IMX258_VTS_30FPS_VGA,
 652                 .vts_min = IMX258_VTS_30FPS_VGA,
 653                 .reg_list = {
 654                         .num_of_regs = ARRAY_SIZE(mode_1048_780_regs),
 655                         .regs = mode_1048_780_regs,
 656                 },
 657                 .link_freq_index = IMX258_LINK_FREQ_640MBPS,
 658         },
 659 };
 660
 661 struct imx258 {
 662         struct v4l2_subdev sd;
 663         struct media_pad pad;
 664
 665         struct v4l2_ctrl_handler ctrl_handler;
 666         /* V4L2 Controls */
 667         struct v4l2_ctrl *link_freq;
 668         struct v4l2_ctrl *pixel_rate;
 669         struct v4l2_ctrl *vblank;
 670         struct v4l2_ctrl *hblank;
 671         struct v4l2_ctrl *exposure;
 672
 673         /* Current mode */
 674         const struct imx258_mode *cur_mode;
 675
 676         const struct imx258_link_freq_config *link_freq_configs;
 677         const s64 *link_freq_menu_items;
 678
 679         /*
 680          * Mutex for serialized access:
 681          * Protect sensor module set pad format and start/stop streaming safely.
 682          */
 683         struct mutex mutex;
 684
 685         /* Streaming on/off */
 686         bool streaming;
 687
 688         struct clk *clk;
 689         struct regulator_bulk_data supplies[IMX258_NUM_SUPPLIES];
 690 };
 691
 692 static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
 693 {
 694         return container_of(_sd, struct imx258, sd);
 695 }
 696
 697 /* Read registers up to 2 at a time */
 698 static int imx258_read_reg(struct imx258 *imx258, u16 reg, u32 len, u32 *val)
 699 {
 700         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 701         struct i2c_msg msgs[2];
 702         u8 addr_buf[2] = { reg >> 8, reg & 0xff };
 703         u8 data_buf[4] = { 0, };
 704         int ret;
 705
 706         if (len > 4)
 707                 return -EINVAL;
 708
 709         /* Write register address */
 710         msgs[0].addr = client->addr;
 711         msgs[0].flags = 0;
 712         msgs[0].len = ARRAY_SIZE(addr_buf);
 713         msgs[0].buf = addr_buf;
 714
 715         /* Read data from register */
 716         msgs[1].addr = client->addr;
 717         msgs[1].flags = I2C_M_RD;
 718         msgs[1].len = len;
 719         msgs[1].buf = &data_buf[4 - len];
 720
 721         ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 722         if (ret != ARRAY_SIZE(msgs))
 723                 return -EIO;
 724
 725         *val = get_unaligned_be32(data_buf);
 726
 727         return 0;
 728 }
 729
 730 /* Write registers up to 2 at a time */
 731 static int imx258_write_reg(struct imx258 *imx258, u16 reg, u32 len, u32 val)
 732 {
 733         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 734         u8 buf[6];
 735
 736         if (len > 4)
 737                 return -EINVAL;
 738
 739         put_unaligned_be16(reg, buf);
 740         put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
 741         if (i2c_master_send(client, buf, len + 2) != len + 2)
 742                 return -EIO;
 743
 744         return 0;
 745 }
 746
 747 /* Write a list of registers */
 748 static int imx258_write_regs(struct imx258 *imx258,
 749                              const struct imx258_reg *regs, u32 len)
 750 {
 751         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 752         unsigned int i;
 753         int ret;
 754
 755         for (i = 0; i < len; i++) {
 756                 ret = imx258_write_reg(imx258, regs[i].address, 1,
 757                                         regs[i].val);
 758                 if (ret) {
 759                         dev_err_ratelimited(
 760                                 &client->dev,
 761                                 "Failed to write reg 0x%4.4x. error = %d\n",
 762                                 regs[i].address, ret);
 763
 764                         return ret;
 765                 }
 766         }
 767
 768         return 0;
 769 }
 770
 771 /* Open sub-device */
 772 static int imx258_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 773 {
 774         struct v4l2_mbus_framefmt *try_fmt =
 775                 v4l2_subdev_get_try_format(sd, fh->state, 0);
 776
 777         /* Initialize try_fmt */
 778         try_fmt->width = supported_modes[0].width;
 779         try_fmt->height = supported_modes[0].height;
 780         try_fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 781         try_fmt->field = V4L2_FIELD_NONE;
 782
 783         return 0;
 784 }
 785
 786 static int imx258_update_digital_gain(struct imx258 *imx258, u32 len, u32 val)
 787 {
 788         int ret;
 789
 790         ret = imx258_write_reg(imx258, IMX258_REG_GR_DIGITAL_GAIN,
 791                                 IMX258_REG_VALUE_16BIT,
 792                                 val);
 793         if (ret)
 794                 return ret;
 795         ret = imx258_write_reg(imx258, IMX258_REG_GB_DIGITAL_GAIN,
 796                                 IMX258_REG_VALUE_16BIT,
 797                                 val);
 798         if (ret)
 799                 return ret;
 800         ret = imx258_write_reg(imx258, IMX258_REG_R_DIGITAL_GAIN,
 801                                 IMX258_REG_VALUE_16BIT,
 802                                 val);
 803         if (ret)
 804                 return ret;
 805         ret = imx258_write_reg(imx258, IMX258_REG_B_DIGITAL_GAIN,
 806                                 IMX258_REG_VALUE_16BIT,
 807                                 val);
 808         if (ret)
 809                 return ret;
 810         return 0;
 811 }
 812
 813 static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
 814 {
 815         struct imx258 *imx258 =
 816                 container_of(ctrl->handler, struct imx258, ctrl_handler);
 817         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 818         int ret = 0;
 819
 820         /*
 821          * Applying V4L2 control value only happens
 822          * when power is up for streaming
 823          */
 824         if (pm_runtime_get_if_in_use(&client->dev) == 0)
 825                 return 0;
 826
 827         switch (ctrl->id) {
 828         case V4L2_CID_ANALOGUE_GAIN:
 829                 ret = imx258_write_reg(imx258, IMX258_REG_ANALOG_GAIN,
 830                                 IMX258_REG_VALUE_16BIT,
 831                                 ctrl->val);
 832                 break;
 833         case V4L2_CID_EXPOSURE:
 834                 ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
 835                                 IMX258_REG_VALUE_16BIT,
 836                                 ctrl->val);
 837                 break;
 838         case V4L2_CID_DIGITAL_GAIN:
 839                 ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
 840                                 ctrl->val);
 841                 break;
 842         case V4L2_CID_TEST_PATTERN:
 843                 ret = imx258_write_reg(imx258, IMX258_REG_TEST_PATTERN,
 844                                 IMX258_REG_VALUE_16BIT,
 845                                 ctrl->val);
 846                 ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
 847                                 IMX258_REG_VALUE_08BIT,
 848                                 !ctrl->val ? REG_CONFIG_MIRROR_FLIP :
 849                                 REG_CONFIG_FLIP_TEST_PATTERN);
 850                 break;
 851         case V4L2_CID_WIDE_DYNAMIC_RANGE:
 852                 if (!ctrl->val) {
 853                         ret = imx258_write_reg(imx258, IMX258_REG_HDR,
 854                                                IMX258_REG_VALUE_08BIT,
 855                                                IMX258_HDR_RATIO_MIN);
 856                 } else {
 857                         ret = imx258_write_reg(imx258, IMX258_REG_HDR,
 858                                                IMX258_REG_VALUE_08BIT,
 859                                                IMX258_HDR_ON);
 860                         if (ret)
 861                                 break;
 862                         ret = imx258_write_reg(imx258, IMX258_REG_HDR_RATIO,
 863                                                IMX258_REG_VALUE_08BIT,
 864                                                BIT(IMX258_HDR_RATIO_MAX));
 865                 }
 866                 break;
 867         case V4L2_CID_VBLANK:
 868                 ret = imx258_write_reg(imx258, IMX258_REG_VTS,
 869                                        IMX258_REG_VALUE_16BIT,
 870                                        imx258->cur_mode->height + ctrl->val);
 871                 break;
 872         default:
 873                 dev_info(&client->dev,
 874                          "ctrl(id:0x%x,val:0x%x) is not handled\n",
 875                          ctrl->id, ctrl->val);
 876                 ret = -EINVAL;
 877                 break;
 878         }
 879
 880         pm_runtime_put(&client->dev);
 881
 882         return ret;
 883 }
 884
 885 static const struct v4l2_ctrl_ops imx258_ctrl_ops = {
 886         .s_ctrl = imx258_set_ctrl,
 887 };
 888
 889 static int imx258_enum_mbus_code(struct v4l2_subdev *sd,
 890                                   struct v4l2_subdev_state *sd_state,
 891                                   struct v4l2_subdev_mbus_code_enum *code)
 892 {
 893         /* Only one bayer order(GRBG) is supported */
 894         if (code->index > 0)
 895                 return -EINVAL;
 896
 897         code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 898
 899         return 0;
 900 }
 901
 902 static int imx258_enum_frame_size(struct v4l2_subdev *sd,
 903                                   struct v4l2_subdev_state *sd_state,
 904                                   struct v4l2_subdev_frame_size_enum *fse)
 905 {
 906         if (fse->index >= ARRAY_SIZE(supported_modes))
 907                 return -EINVAL;
 908
 909         if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
 910                 return -EINVAL;
 911
 912         fse->min_width = supported_modes[fse->index].width;
 913         fse->max_width = fse->min_width;
 914         fse->min_height = supported_modes[fse->index].height;
 915         fse->max_height = fse->min_height;
 916
 917         return 0;
 918 }
 919
 920 static void imx258_update_pad_format(const struct imx258_mode *mode,
 921                                      struct v4l2_subdev_format *fmt)
 922 {
 923         fmt->format.width = mode->width;
 924         fmt->format.height = mode->height;
 925         fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 926         fmt->format.field = V4L2_FIELD_NONE;
 927 }
 928
 929 static int __imx258_get_pad_format(struct imx258 *imx258,
 930                                    struct v4l2_subdev_state *sd_state,
 931                                    struct v4l2_subdev_format *fmt)
 932 {
 933         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 934                 fmt->format = *v4l2_subdev_get_try_format(&imx258->sd,
 935                                                           sd_state,
 936                                                           fmt->pad);
 937         else
 938                 imx258_update_pad_format(imx258->cur_mode, fmt);
 939
 940         return 0;
 941 }
 942
 943 static int imx258_get_pad_format(struct v4l2_subdev *sd,
 944                                  struct v4l2_subdev_state *sd_state,
 945                                  struct v4l2_subdev_format *fmt)
 946 {
 947         struct imx258 *imx258 = to_imx258(sd);
 948         int ret;
 949
 950         mutex_lock(&imx258->mutex);
 951         ret = __imx258_get_pad_format(imx258, sd_state, fmt);
 952         mutex_unlock(&imx258->mutex);
 953
 954         return ret;
 955 }
 956
 957 static int imx258_set_pad_format(struct v4l2_subdev *sd,
 958                                  struct v4l2_subdev_state *sd_state,
 959                                  struct v4l2_subdev_format *fmt)
 960 {
 961         struct imx258 *imx258 = to_imx258(sd);
 962         const struct imx258_mode *mode;
 963         struct v4l2_mbus_framefmt *framefmt;
 964         s32 vblank_def;
 965         s32 vblank_min;
 966         s64 h_blank;
 967         s64 pixel_rate;
 968         s64 link_freq;
 969
 970         mutex_lock(&imx258->mutex);
 971
 972         /* Only one raw bayer(GBRG) order is supported */
 973         fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 974
 975         mode = v4l2_find_nearest_size(supported_modes,
 976                 ARRAY_SIZE(supported_modes), width, height,
 977                 fmt->format.width, fmt->format.height);
 978         imx258_update_pad_format(mode, fmt);
 979         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 980                 framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
 981                 *framefmt = fmt->format;
 982         } else {
 983                 imx258->cur_mode = mode;
 984                 __v4l2_ctrl_s_ctrl(imx258->link_freq, mode->link_freq_index);
 985
 986                 link_freq = imx258->link_freq_menu_items[mode->link_freq_index];
 987                 pixel_rate = link_freq_to_pixel_rate(link_freq);
 988                 __v4l2_ctrl_s_ctrl_int64(imx258->pixel_rate, pixel_rate);
 989                 /* Update limits and set FPS to default */
 990                 vblank_def = imx258->cur_mode->vts_def -
 991                              imx258->cur_mode->height;
 992                 vblank_min = imx258->cur_mode->vts_min -
 993                              imx258->cur_mode->height;
 994                 __v4l2_ctrl_modify_range(
 995                         imx258->vblank, vblank_min,
 996                         IMX258_VTS_MAX - imx258->cur_mode->height, 1,
 997                         vblank_def);
 998                 __v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
 999                 h_blank =
1000                         imx258->link_freq_configs[mode->link_freq_index].pixels_per_line
1001                          - imx258->cur_mode->width;
1002                 __v4l2_ctrl_modify_range(imx258->hblank, h_blank,
1003                                          h_blank, 1, h_blank);
1004         }
1005
1006         mutex_unlock(&imx258->mutex);
1007
1008         return 0;
1009 }
1010
1011 /* Start streaming */
1012 static int imx258_start_streaming(struct imx258 *imx258)
1013 {
1014         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1015         const struct imx258_reg_list *reg_list;
1016         int ret, link_freq_index;
1017
1018         /* Setup PLL */
1019         link_freq_index = imx258->cur_mode->link_freq_index;
1020         reg_list = &imx258->link_freq_configs[link_freq_index].reg_list;
1021         ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
1022         if (ret) {
1023                 dev_err(&client->dev, "%s failed to set plls\n", __func__);
1024                 return ret;
1025         }
1026
1027         /* Apply default values of current mode */
1028         reg_list = &imx258->cur_mode->reg_list;
1029         ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
1030         if (ret) {
1031                 dev_err(&client->dev, "%s failed to set mode\n", __func__);
1032                 return ret;
1033         }
1034
1035         /* Set Orientation be 180 degree */
1036         ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
1037                                IMX258_REG_VALUE_08BIT, REG_CONFIG_MIRROR_FLIP);
1038         if (ret) {
1039                 dev_err(&client->dev, "%s failed to set orientation\n",
1040                         __func__);
1041                 return ret;
1042         }
1043
1044         /* Apply customized values from user */
1045         ret =  __v4l2_ctrl_handler_setup(imx258->sd.ctrl_handler);
1046         if (ret)
1047                 return ret;
1048
1049         /* set stream on register */
1050         return imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
1051                                 IMX258_REG_VALUE_08BIT,
1052                                 IMX258_MODE_STREAMING);
1053 }
1054
1055 /* Stop streaming */
1056 static int imx258_stop_streaming(struct imx258 *imx258)
1057 {
1058         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1059         int ret;
1060
1061         /* set stream off register */
1062         ret = imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
1063                 IMX258_REG_VALUE_08BIT, IMX258_MODE_STANDBY);
1064         if (ret)
1065                 dev_err(&client->dev, "%s failed to set stream\n", __func__);
1066
1067         /*
1068          * Return success even if it was an error, as there is nothing the
1069          * caller can do about it.
1070          */
1071         return 0;
1072 }
1073
1074 static int imx258_power_on(struct device *dev)
1075 {
1076         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1077         struct imx258 *imx258 = to_imx258(sd);
1078         int ret;
1079
1080         ret = regulator_bulk_enable(IMX258_NUM_SUPPLIES,
1081                                     imx258->supplies);
1082         if (ret) {
1083                 dev_err(dev, "%s: failed to enable regulators\n",
1084                         __func__);
1085                 return ret;
1086         }
1087
1088         ret = clk_prepare_enable(imx258->clk);
1089         if (ret) {
1090                 dev_err(dev, "failed to enable clock\n");
1091                 regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
1092         }
1093
1094         return ret;
1095 }
1096
1097 static int imx258_power_off(struct device *dev)
1098 {
1099         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1100         struct imx258 *imx258 = to_imx258(sd);
1101
1102         clk_disable_unprepare(imx258->clk);
1103         regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
1104
1105         return 0;
1106 }
1107
1108 static int imx258_set_stream(struct v4l2_subdev *sd, int enable)
1109 {
1110         struct imx258 *imx258 = to_imx258(sd);
1111         struct i2c_client *client = v4l2_get_subdevdata(sd);
1112         int ret = 0;
1113
1114         mutex_lock(&imx258->mutex);
1115         if (imx258->streaming == enable) {
1116                 mutex_unlock(&imx258->mutex);
1117                 return 0;
1118         }
1119
1120         if (enable) {
1121                 ret = pm_runtime_resume_and_get(&client->dev);
1122                 if (ret < 0)
1123                         goto err_unlock;
1124
1125                 /*
1126                  * Apply default & customized values
1127                  * and then start streaming.
1128                  */
1129                 ret = imx258_start_streaming(imx258);
1130                 if (ret)
1131                         goto err_rpm_put;
1132         } else {
1133                 imx258_stop_streaming(imx258);
1134                 pm_runtime_put(&client->dev);
1135         }
1136
1137         imx258->streaming = enable;
1138         mutex_unlock(&imx258->mutex);
1139
1140         return ret;
1141
1142 err_rpm_put:
1143         pm_runtime_put(&client->dev);
1144 err_unlock:
1145         mutex_unlock(&imx258->mutex);
1146
1147         return ret;
1148 }
1149
1150 static int __maybe_unused imx258_suspend(struct device *dev)
1151 {
1152         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1153         struct imx258 *imx258 = to_imx258(sd);
1154
1155         if (imx258->streaming)
1156                 imx258_stop_streaming(imx258);
1157
1158         return 0;
1159 }
1160
1161 static int __maybe_unused imx258_resume(struct device *dev)
1162 {
1163         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1164         struct imx258 *imx258 = to_imx258(sd);
1165         int ret;
1166
1167         if (imx258->streaming) {
1168                 ret = imx258_start_streaming(imx258);
1169                 if (ret)
1170                         goto error;
1171         }
1172
1173         return 0;
1174
1175 error:
1176         imx258_stop_streaming(imx258);
1177         imx258->streaming = 0;
1178         return ret;
1179 }
1180
1181 /* Verify chip ID */
1182 static int imx258_identify_module(struct imx258 *imx258)
1183 {
1184         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1185         int ret;
1186         u32 val;
1187
1188         ret = imx258_read_reg(imx258, IMX258_REG_CHIP_ID,
1189                               IMX258_REG_VALUE_16BIT, &val);
1190         if (ret) {
1191                 dev_err(&client->dev, "failed to read chip id %x\n",
1192                         IMX258_CHIP_ID);
1193                 return ret;
1194         }
1195
1196         if (val != IMX258_CHIP_ID) {
1197                 dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
1198                         IMX258_CHIP_ID, val);
1199                 return -EIO;
1200         }
1201
1202         return 0;
1203 }
1204
1205 static const struct v4l2_subdev_video_ops imx258_video_ops = {
1206         .s_stream = imx258_set_stream,
1207 };
1208
1209 static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
1210         .enum_mbus_code = imx258_enum_mbus_code,
1211         .get_fmt = imx258_get_pad_format,
1212         .set_fmt = imx258_set_pad_format,
1213         .enum_frame_size = imx258_enum_frame_size,
1214 };
1215
1216 static const struct v4l2_subdev_ops imx258_subdev_ops = {
1217         .video = &imx258_video_ops,
1218         .pad = &imx258_pad_ops,
1219 };
1220
1221 static const struct v4l2_subdev_internal_ops imx258_internal_ops = {
1222         .open = imx258_open,
1223 };
1224
1225 /* Initialize control handlers */
1226 static int imx258_init_controls(struct imx258 *imx258)
1227 {
1228         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1229         struct v4l2_fwnode_device_properties props;
1230         struct v4l2_ctrl_handler *ctrl_hdlr;
1231         struct v4l2_ctrl *vflip, *hflip;
1232         s64 vblank_def;
1233         s64 vblank_min;
1234         s64 pixel_rate_min;
1235         s64 pixel_rate_max;
1236         int ret;
1237
1238         ctrl_hdlr = &imx258->ctrl_handler;
1239         ret = v4l2_ctrl_handler_init(ctrl_hdlr, 13);
1240         if (ret)
1241                 return ret;
1242
1243         mutex_init(&imx258->mutex);
1244         ctrl_hdlr->lock = &imx258->mutex;
1245         imx258->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
1246                                 &imx258_ctrl_ops,
1247                                 V4L2_CID_LINK_FREQ,
1248                                 ARRAY_SIZE(link_freq_menu_items_19_2) - 1,
1249                                 0,
1250                                 imx258->link_freq_menu_items);
1251
1252         if (imx258->link_freq)
1253                 imx258->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1254
1255         /* The driver only supports one bayer order and flips by default. */
1256         hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1257                                   V4L2_CID_HFLIP, 1, 1, 1, 1);
1258         if (hflip)
1259                 hflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1260
1261         vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1262                                   V4L2_CID_VFLIP, 1, 1, 1, 1);
1263         if (vflip)
1264                 vflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1265
1266         pixel_rate_max =
1267                 link_freq_to_pixel_rate(imx258->link_freq_menu_items[0]);
1268         pixel_rate_min =
1269                 link_freq_to_pixel_rate(imx258->link_freq_menu_items[1]);
1270         /* By default, PIXEL_RATE is read only */
1271         imx258->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1272                                 V4L2_CID_PIXEL_RATE,
1273                                 pixel_rate_min, pixel_rate_max,
1274                                 1, pixel_rate_max);
1275
1276
1277         vblank_def = imx258->cur_mode->vts_def - imx258->cur_mode->height;
1278         vblank_min = imx258->cur_mode->vts_min - imx258->cur_mode->height;
1279         imx258->vblank = v4l2_ctrl_new_std(
1280                                 ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_VBLANK,
1281                                 vblank_min,
1282                                 IMX258_VTS_MAX - imx258->cur_mode->height, 1,
1283                                 vblank_def);
1284
1285         imx258->hblank = v4l2_ctrl_new_std(
1286                                 ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_HBLANK,
1287                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width,
1288                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width,
1289                                 1,
1290                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width);
1291
1292         if (imx258->hblank)
1293                 imx258->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1294
1295         imx258->exposure = v4l2_ctrl_new_std(
1296                                 ctrl_hdlr, &imx258_ctrl_ops,
1297                                 V4L2_CID_EXPOSURE, IMX258_EXPOSURE_MIN,
1298                                 IMX258_EXPOSURE_MAX, IMX258_EXPOSURE_STEP,
1299                                 IMX258_EXPOSURE_DEFAULT);
1300
1301         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
1302                                 IMX258_ANA_GAIN_MIN, IMX258_ANA_GAIN_MAX,
1303                                 IMX258_ANA_GAIN_STEP, IMX258_ANA_GAIN_DEFAULT);
1304
1305         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
1306                                 IMX258_DGTL_GAIN_MIN, IMX258_DGTL_GAIN_MAX,
1307                                 IMX258_DGTL_GAIN_STEP,
1308                                 IMX258_DGTL_GAIN_DEFAULT);
1309
1310         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_WIDE_DYNAMIC_RANGE,
1311                                 0, 1, 1, IMX258_HDR_RATIO_DEFAULT);
1312
1313         v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx258_ctrl_ops,
1314                                 V4L2_CID_TEST_PATTERN,
1315                                 ARRAY_SIZE(imx258_test_pattern_menu) - 1,
1316                                 0, 0, imx258_test_pattern_menu);
1317
1318         if (ctrl_hdlr->error) {
1319                 ret = ctrl_hdlr->error;
1320                 dev_err(&client->dev, "%s control init failed (%d)\n",
1321                                 __func__, ret);
1322                 goto error;
1323         }
1324
1325         ret = v4l2_fwnode_device_parse(&client->dev, &props);
1326         if (ret)
1327                 goto error;
1328
1329         ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx258_ctrl_ops,
1330                                               &props);
1331         if (ret)
1332                 goto error;
1333
1334         imx258->sd.ctrl_handler = ctrl_hdlr;
1335
1336         return 0;
1337
1338 error:
1339         v4l2_ctrl_handler_free(ctrl_hdlr);
1340         mutex_destroy(&imx258->mutex);
1341
1342         return ret;
1343 }
1344
1345 static void imx258_free_controls(struct imx258 *imx258)
1346 {
1347         v4l2_ctrl_handler_free(imx258->sd.ctrl_handler);
1348         mutex_destroy(&imx258->mutex);
1349 }
1350
1351 static int imx258_get_regulators(struct imx258 *imx258,
1352                                  struct i2c_client *client)
1353 {
1354         unsigned int i;
1355
1356         for (i = 0; i < IMX258_NUM_SUPPLIES; i++)
1357                 imx258->supplies[i].supply = imx258_supply_name[i];
1358
1359         return devm_regulator_bulk_get(&client->dev,
1360                                        IMX258_NUM_SUPPLIES,
1361                                        imx258->supplies);
1362 }
1363
1364 static int imx258_probe(struct i2c_client *client)
1365 {
1366         struct imx258 *imx258;
1367         int ret;
1368         u32 val = 0;
1369
1370         imx258 = devm_kzalloc(&client->dev, sizeof(*imx258), GFP_KERNEL);
1371         if (!imx258)
1372                 return -ENOMEM;
1373
1374         ret = imx258_get_regulators(imx258, client);
1375         if (ret)
1376                 return ret;
1377
1378         imx258->clk = devm_clk_get_optional(&client->dev, NULL);
1379         if (IS_ERR(imx258->clk))
1380                 return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
1381                                      "error getting clock\n");
1382         if (!imx258->clk) {
1383                 dev_dbg(&client->dev,
1384                         "no clock provided, using clock-frequency property\n");
1385
1386                 device_property_read_u32(&client->dev, "clock-frequency", &val);
1387         } else if (IS_ERR(imx258->clk)) {
1388                 return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
1389                                      "error getting clock\n");
1390         } else {
1391                 val = clk_get_rate(imx258->clk);
1392         }
1393
1394         switch (val) {
1395         case 19200000:
1396                 imx258->link_freq_configs = link_freq_configs_19_2;
1397                 imx258->link_freq_menu_items = link_freq_menu_items_19_2;
1398                 break;
1399         case 24000000:
1400                 imx258->link_freq_configs = link_freq_configs_24;
1401                 imx258->link_freq_menu_items = link_freq_menu_items_24;
1402                 break;
1403         default:
1404                 dev_err(&client->dev, "input clock frequency of %u not supported\n",
1405                         val);
1406                 return -EINVAL;
1407         }
1408
1409         /* Initialize subdev */
1410         v4l2_i2c_subdev_init(&imx258->sd, client, &imx258_subdev_ops);
1411
1412         /* Will be powered off via pm_runtime_idle */
1413         ret = imx258_power_on(&client->dev);
1414         if (ret)
1415                 return ret;
1416
1417         /* Check module identity */
1418         ret = imx258_identify_module(imx258);
1419         if (ret)
1420                 goto error_identify;
1421
1422         /* Set default mode to max resolution */
1423         imx258->cur_mode = &supported_modes[0];
1424
1425         ret = imx258_init_controls(imx258);
1426         if (ret)
1427                 goto error_identify;
1428
1429         /* Initialize subdev */
1430         imx258->sd.internal_ops = &imx258_internal_ops;
1431         imx258->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1432         imx258->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1433
1434         /* Initialize source pad */
1435         imx258->pad.flags = MEDIA_PAD_FL_SOURCE;
1436
1437         ret = media_entity_pads_init(&imx258->sd.entity, 1, &imx258->pad);
1438         if (ret)
1439                 goto error_handler_free;
1440
1441         ret = v4l2_async_register_subdev_sensor(&imx258->sd);
1442         if (ret < 0)
1443                 goto error_media_entity;
1444
1445         pm_runtime_set_active(&client->dev);
1446         pm_runtime_enable(&client->dev);
1447         pm_runtime_idle(&client->dev);
1448
1449         return 0;
1450
1451 error_media_entity:
1452         media_entity_cleanup(&imx258->sd.entity);
1453
1454 error_handler_free:
1455         imx258_free_controls(imx258);
1456
1457 error_identify:
1458         imx258_power_off(&client->dev);
1459
1460         return ret;
1461 }
1462
1463 static void imx258_remove(struct i2c_client *client)
1464 {
1465         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1466         struct imx258 *imx258 = to_imx258(sd);
1467
1468         v4l2_async_unregister_subdev(sd);
1469         media_entity_cleanup(&sd->entity);
1470         imx258_free_controls(imx258);
1471
1472         pm_runtime_disable(&client->dev);
1473         if (!pm_runtime_status_suspended(&client->dev))
1474                 imx258_power_off(&client->dev);
1475         pm_runtime_set_suspended(&client->dev);
1476 }
1477
1478 static const struct dev_pm_ops imx258_pm_ops = {
1479         SET_SYSTEM_SLEEP_PM_OPS(imx258_suspend, imx258_resume)
1480         SET_RUNTIME_PM_OPS(imx258_power_off, imx258_power_on, NULL)
1481 };
1482
1483 #ifdef CONFIG_ACPI
1484 static const struct acpi_device_id imx258_acpi_ids[] = {
1485         { "SONY258A" },
1486         { /* sentinel */ }
1487 };
1488
1489 MODULE_DEVICE_TABLE(acpi, imx258_acpi_ids);
1490 #endif
1491
1492 static const struct of_device_id imx258_dt_ids[] = {
1493         { .compatible = "sony,imx258" },
1494         { /* sentinel */ }
1495 };
1496 MODULE_DEVICE_TABLE(of, imx258_dt_ids);
1497
1498 static struct i2c_driver imx258_i2c_driver = {
1499         .driver = {
1500                 .name = "imx258",
1501                 .pm = &imx258_pm_ops,
1502                 .acpi_match_table = ACPI_PTR(imx258_acpi_ids),
1503                 .of_match_table = imx258_dt_ids,
1504         },
1505         .probe = imx258_probe,
1506         .remove = imx258_remove,
1507 };
1508
1509 module_i2c_driver(imx258_i2c_driver);
1510
1511 MODULE_AUTHOR("Yeh, Andy <andy.yeh@intel.com>");
1512 MODULE_AUTHOR("Chiang, Alan");
1513 MODULE_AUTHOR("Chen, Jason");
1514 MODULE_DESCRIPTION("Sony IMX258 sensor driver");
1515 MODULE_LICENSE("GPL v2");