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