drivers/media/i2c/imx334.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Sony imx334 sensor driver
   4  *
   5  * Copyright (C) 2021 Intel Corporation
   6  */
   7 #include <asm/unaligned.h>
   8
   9 #include <linux/clk.h>
  10 #include <linux/delay.h>
  11 #include <linux/i2c.h>
  12 #include <linux/module.h>
  13 #include <linux/pm_runtime.h>
  14
  15 #include <media/v4l2-ctrls.h>
  16 #include <media/v4l2-fwnode.h>
  17 #include <media/v4l2-subdev.h>
  18
  19 /* Streaming Mode */
  20 #define IMX334_REG_MODE_SELECT  0x3000
  21 #define IMX334_MODE_STANDBY     0x01
  22 #define IMX334_MODE_STREAMING   0x00
  23
  24 /* Lines per frame */
  25 #define IMX334_REG_LPFR         0x3030
  26
  27 /* Chip ID */
  28 #define IMX334_REG_ID           0x3044
  29 #define IMX334_ID               0x1e
  30
  31 /* Exposure control */
  32 #define IMX334_REG_SHUTTER      0x3058
  33 #define IMX334_EXPOSURE_MIN     1
  34 #define IMX334_EXPOSURE_OFFSET  5
  35 #define IMX334_EXPOSURE_STEP    1
  36 #define IMX334_EXPOSURE_DEFAULT 0x0648
  37
  38 /* Analog gain control */
  39 #define IMX334_REG_AGAIN        0x30e8
  40 #define IMX334_AGAIN_MIN        0
  41 #define IMX334_AGAIN_MAX        240
  42 #define IMX334_AGAIN_STEP       1
  43 #define IMX334_AGAIN_DEFAULT    0
  44
  45 /* Group hold register */
  46 #define IMX334_REG_HOLD         0x3001
  47
  48 /* Input clock rate */
  49 #define IMX334_INCLK_RATE       24000000
  50
  51 /* CSI2 HW configuration */
  52 #define IMX334_LINK_FREQ_891M   891000000
  53 #define IMX334_LINK_FREQ_445M   445500000
  54 #define IMX334_NUM_DATA_LANES   4
  55
  56 #define IMX334_REG_MIN          0x00
  57 #define IMX334_REG_MAX          0xfffff
  58
  59 /* Test Pattern Control */
  60 #define IMX334_REG_TP           0x329e
  61 #define IMX334_TP_COLOR_HBARS   0xA
  62 #define IMX334_TP_COLOR_VBARS   0xB
  63
  64 #define IMX334_TPG_EN_DOUT      0x329c
  65 #define IMX334_TP_ENABLE        0x1
  66 #define IMX334_TP_DISABLE       0x0
  67
  68 #define IMX334_TPG_COLORW       0x32a0
  69 #define IMX334_TPG_COLORW_120P  0x13
  70
  71 #define IMX334_TP_CLK_EN        0x3148
  72 #define IMX334_TP_CLK_EN_VAL    0x10
  73 #define IMX334_TP_CLK_DIS_VAL   0x0
  74
  75 #define IMX334_DIG_CLP_MODE     0x3280
  76
  77 /**
  78  * struct imx334_reg - imx334 sensor register
  79  * @address: Register address
  80  * @val: Register value
  81  */
  82 struct imx334_reg {
  83         u16 address;
  84         u8 val;
  85 };
  86
  87 /**
  88  * struct imx334_reg_list - imx334 sensor register list
  89  * @num_of_regs: Number of registers in the list
  90  * @regs: Pointer to register list
  91  */
  92 struct imx334_reg_list {
  93         u32 num_of_regs;
  94         const struct imx334_reg *regs;
  95 };
  96
  97 /**
  98  * struct imx334_mode - imx334 sensor mode structure
  99  * @width: Frame width
 100  * @height: Frame height
 101  * @hblank: Horizontal blanking in lines
 102  * @vblank: Vertical blanking in lines
 103  * @vblank_min: Minimal vertical blanking in lines
 104  * @vblank_max: Maximum vertical blanking in lines
 105  * @pclk: Sensor pixel clock
 106  * @link_freq_idx: Link frequency index
 107  * @reg_list: Register list for sensor mode
 108  */
 109 struct imx334_mode {
 110         u32 width;
 111         u32 height;
 112         u32 hblank;
 113         u32 vblank;
 114         u32 vblank_min;
 115         u32 vblank_max;
 116         u64 pclk;
 117         u32 link_freq_idx;
 118         struct imx334_reg_list reg_list;
 119 };
 120
 121 /**
 122  * struct imx334 - imx334 sensor device structure
 123  * @dev: Pointer to generic device
 124  * @client: Pointer to i2c client
 125  * @sd: V4L2 sub-device
 126  * @pad: Media pad. Only one pad supported
 127  * @reset_gpio: Sensor reset gpio
 128  * @inclk: Sensor input clock
 129  * @ctrl_handler: V4L2 control handler
 130  * @link_freq_ctrl: Pointer to link frequency control
 131  * @pclk_ctrl: Pointer to pixel clock control
 132  * @hblank_ctrl: Pointer to horizontal blanking control
 133  * @vblank_ctrl: Pointer to vertical blanking control
 134  * @exp_ctrl: Pointer to exposure control
 135  * @again_ctrl: Pointer to analog gain control
 136  * @vblank: Vertical blanking in lines
 137  * @cur_mode: Pointer to current selected sensor mode
 138  * @mutex: Mutex for serializing sensor controls
 139  * @link_freq_bitmap: Menu bitmap for link_freq_ctrl
 140  * @cur_code: current selected format code
 141  */
 142 struct imx334 {
 143         struct device *dev;
 144         struct i2c_client *client;
 145         struct v4l2_subdev sd;
 146         struct media_pad pad;
 147         struct gpio_desc *reset_gpio;
 148         struct clk *inclk;
 149         struct v4l2_ctrl_handler ctrl_handler;
 150         struct v4l2_ctrl *link_freq_ctrl;
 151         struct v4l2_ctrl *pclk_ctrl;
 152         struct v4l2_ctrl *hblank_ctrl;
 153         struct v4l2_ctrl *vblank_ctrl;
 154         struct {
 155                 struct v4l2_ctrl *exp_ctrl;
 156                 struct v4l2_ctrl *again_ctrl;
 157         };
 158         u32 vblank;
 159         const struct imx334_mode *cur_mode;
 160         struct mutex mutex;
 161         unsigned long link_freq_bitmap;
 162         u32 cur_code;
 163 };
 164
 165 static const s64 link_freq[] = {
 166         IMX334_LINK_FREQ_891M,
 167         IMX334_LINK_FREQ_445M,
 168 };
 169
 170 /* Sensor mode registers for 1920x1080@30fps */
 171 static const struct imx334_reg mode_1920x1080_regs[] = {
 172         {0x3000, 0x01},
 173         {0x3018, 0x04},
 174         {0x3030, 0xca},
 175         {0x3031, 0x08},
 176         {0x3032, 0x00},
 177         {0x3034, 0x4c},
 178         {0x3035, 0x04},
 179         {0x302c, 0xf0},
 180         {0x302d, 0x03},
 181         {0x302e, 0x80},
 182         {0x302f, 0x07},
 183         {0x3074, 0xcc},
 184         {0x3075, 0x02},
 185         {0x308e, 0xcd},
 186         {0x308f, 0x02},
 187         {0x3076, 0x38},
 188         {0x3077, 0x04},
 189         {0x3090, 0x38},
 190         {0x3091, 0x04},
 191         {0x3308, 0x38},
 192         {0x3309, 0x04},
 193         {0x30C6, 0x00},
 194         {0x30c7, 0x00},
 195         {0x30ce, 0x00},
 196         {0x30cf, 0x00},
 197         {0x30d8, 0x18},
 198         {0x30d9, 0x0a},
 199         {0x304c, 0x00},
 200         {0x304e, 0x00},
 201         {0x304f, 0x00},
 202         {0x3050, 0x00},
 203         {0x30b6, 0x00},
 204         {0x30b7, 0x00},
 205         {0x3116, 0x08},
 206         {0x3117, 0x00},
 207         {0x31a0, 0x20},
 208         {0x31a1, 0x0f},
 209         {0x300c, 0x3b},
 210         {0x300d, 0x29},
 211         {0x314c, 0x29},
 212         {0x314d, 0x01},
 213         {0x315a, 0x06},
 214         {0x3168, 0xa0},
 215         {0x316a, 0x7e},
 216         {0x319e, 0x02},
 217         {0x3199, 0x00},
 218         {0x319d, 0x00},
 219         {0x31dd, 0x03},
 220         {0x3300, 0x00},
 221         {0x341c, 0xff},
 222         {0x341d, 0x01},
 223         {0x3a01, 0x03},
 224         {0x3a18, 0x7f},
 225         {0x3a19, 0x00},
 226         {0x3a1a, 0x37},
 227         {0x3a1b, 0x00},
 228         {0x3a1c, 0x37},
 229         {0x3a1d, 0x00},
 230         {0x3a1e, 0xf7},
 231         {0x3a1f, 0x00},
 232         {0x3a20, 0x3f},
 233         {0x3a21, 0x00},
 234         {0x3a20, 0x6f},
 235         {0x3a21, 0x00},
 236         {0x3a20, 0x3f},
 237         {0x3a21, 0x00},
 238         {0x3a20, 0x5f},
 239         {0x3a21, 0x00},
 240         {0x3a20, 0x2f},
 241         {0x3a21, 0x00},
 242         {0x3078, 0x02},
 243         {0x3079, 0x00},
 244         {0x307a, 0x00},
 245         {0x307b, 0x00},
 246         {0x3080, 0x02},
 247         {0x3081, 0x00},
 248         {0x3082, 0x00},
 249         {0x3083, 0x00},
 250         {0x3088, 0x02},
 251         {0x3094, 0x00},
 252         {0x3095, 0x00},
 253         {0x3096, 0x00},
 254         {0x309b, 0x02},
 255         {0x309c, 0x00},
 256         {0x309d, 0x00},
 257         {0x309e, 0x00},
 258         {0x30a4, 0x00},
 259         {0x30a5, 0x00},
 260         {0x3288, 0x21},
 261         {0x328a, 0x02},
 262         {0x3414, 0x05},
 263         {0x3416, 0x18},
 264         {0x35Ac, 0x0e},
 265         {0x3648, 0x01},
 266         {0x364a, 0x04},
 267         {0x364c, 0x04},
 268         {0x3678, 0x01},
 269         {0x367c, 0x31},
 270         {0x367e, 0x31},
 271         {0x3708, 0x02},
 272         {0x3714, 0x01},
 273         {0x3715, 0x02},
 274         {0x3716, 0x02},
 275         {0x3717, 0x02},
 276         {0x371c, 0x3d},
 277         {0x371d, 0x3f},
 278         {0x372c, 0x00},
 279         {0x372d, 0x00},
 280         {0x372e, 0x46},
 281         {0x372f, 0x00},
 282         {0x3730, 0x89},
 283         {0x3731, 0x00},
 284         {0x3732, 0x08},
 285         {0x3733, 0x01},
 286         {0x3734, 0xfe},
 287         {0x3735, 0x05},
 288         {0x375d, 0x00},
 289         {0x375e, 0x00},
 290         {0x375f, 0x61},
 291         {0x3760, 0x06},
 292         {0x3768, 0x1b},
 293         {0x3769, 0x1b},
 294         {0x376a, 0x1a},
 295         {0x376b, 0x19},
 296         {0x376c, 0x18},
 297         {0x376d, 0x14},
 298         {0x376e, 0x0f},
 299         {0x3776, 0x00},
 300         {0x3777, 0x00},
 301         {0x3778, 0x46},
 302         {0x3779, 0x00},
 303         {0x377a, 0x08},
 304         {0x377b, 0x01},
 305         {0x377c, 0x45},
 306         {0x377d, 0x01},
 307         {0x377e, 0x23},
 308         {0x377f, 0x02},
 309         {0x3780, 0xd9},
 310         {0x3781, 0x03},
 311         {0x3782, 0xf5},
 312         {0x3783, 0x06},
 313         {0x3784, 0xa5},
 314         {0x3788, 0x0f},
 315         {0x378a, 0xd9},
 316         {0x378b, 0x03},
 317         {0x378c, 0xeb},
 318         {0x378d, 0x05},
 319         {0x378e, 0x87},
 320         {0x378f, 0x06},
 321         {0x3790, 0xf5},
 322         {0x3792, 0x43},
 323         {0x3794, 0x7a},
 324         {0x3796, 0xa1},
 325         {0x37b0, 0x37},
 326         {0x3e04, 0x0e},
 327         {0x30e8, 0x50},
 328         {0x30e9, 0x00},
 329         {0x3e04, 0x0e},
 330         {0x3002, 0x00},
 331 };
 332
 333 /* Sensor mode registers for 3840x2160@30fps */
 334 static const struct imx334_reg mode_3840x2160_regs[] = {
 335         {0x3000, 0x01},
 336         {0x3002, 0x00},
 337         {0x3018, 0x04},
 338         {0x37b0, 0x36},
 339         {0x304c, 0x00},
 340         {0x300c, 0x3b},
 341         {0x300d, 0x2a},
 342         {0x3034, 0x26},
 343         {0x3035, 0x02},
 344         {0x314c, 0x29},
 345         {0x314d, 0x01},
 346         {0x315a, 0x02},
 347         {0x3168, 0xa0},
 348         {0x316a, 0x7e},
 349         {0x3288, 0x21},
 350         {0x328a, 0x02},
 351         {0x302c, 0x3c},
 352         {0x302d, 0x00},
 353         {0x302e, 0x00},
 354         {0x302f, 0x0f},
 355         {0x3076, 0x70},
 356         {0x3077, 0x08},
 357         {0x3090, 0x70},
 358         {0x3091, 0x08},
 359         {0x30d8, 0x20},
 360         {0x30d9, 0x12},
 361         {0x3308, 0x70},
 362         {0x3309, 0x08},
 363         {0x3414, 0x05},
 364         {0x3416, 0x18},
 365         {0x35ac, 0x0e},
 366         {0x3648, 0x01},
 367         {0x364a, 0x04},
 368         {0x364c, 0x04},
 369         {0x3678, 0x01},
 370         {0x367c, 0x31},
 371         {0x367e, 0x31},
 372         {0x3708, 0x02},
 373         {0x3714, 0x01},
 374         {0x3715, 0x02},
 375         {0x3716, 0x02},
 376         {0x3717, 0x02},
 377         {0x371c, 0x3d},
 378         {0x371d, 0x3f},
 379         {0x372c, 0x00},
 380         {0x372d, 0x00},
 381         {0x372e, 0x46},
 382         {0x372f, 0x00},
 383         {0x3730, 0x89},
 384         {0x3731, 0x00},
 385         {0x3732, 0x08},
 386         {0x3733, 0x01},
 387         {0x3734, 0xfe},
 388         {0x3735, 0x05},
 389         {0x375d, 0x00},
 390         {0x375e, 0x00},
 391         {0x375f, 0x61},
 392         {0x3760, 0x06},
 393         {0x3768, 0x1b},
 394         {0x3769, 0x1b},
 395         {0x376a, 0x1a},
 396         {0x376b, 0x19},
 397         {0x376c, 0x18},
 398         {0x376d, 0x14},
 399         {0x376e, 0x0f},
 400         {0x3776, 0x00},
 401         {0x3777, 0x00},
 402         {0x3778, 0x46},
 403         {0x3779, 0x00},
 404         {0x377a, 0x08},
 405         {0x377b, 0x01},
 406         {0x377c, 0x45},
 407         {0x377d, 0x01},
 408         {0x377e, 0x23},
 409         {0x377f, 0x02},
 410         {0x3780, 0xd9},
 411         {0x3781, 0x03},
 412         {0x3782, 0xf5},
 413         {0x3783, 0x06},
 414         {0x3784, 0xa5},
 415         {0x3788, 0x0f},
 416         {0x378a, 0xd9},
 417         {0x378b, 0x03},
 418         {0x378c, 0xeb},
 419         {0x378d, 0x05},
 420         {0x378e, 0x87},
 421         {0x378f, 0x06},
 422         {0x3790, 0xf5},
 423         {0x3792, 0x43},
 424         {0x3794, 0x7a},
 425         {0x3796, 0xa1},
 426         {0x3e04, 0x0e},
 427         {0x319e, 0x00},
 428         {0x3a00, 0x01},
 429         {0x3a18, 0xbf},
 430         {0x3a19, 0x00},
 431         {0x3a1a, 0x67},
 432         {0x3a1b, 0x00},
 433         {0x3a1c, 0x6f},
 434         {0x3a1d, 0x00},
 435         {0x3a1e, 0xd7},
 436         {0x3a1f, 0x01},
 437         {0x3a20, 0x6f},
 438         {0x3a21, 0x00},
 439         {0x3a22, 0xcf},
 440         {0x3a23, 0x00},
 441         {0x3a24, 0x6f},
 442         {0x3a25, 0x00},
 443         {0x3a26, 0xb7},
 444         {0x3a27, 0x00},
 445         {0x3a28, 0x5f},
 446         {0x3a29, 0x00},
 447 };
 448
 449 static const char * const imx334_test_pattern_menu[] = {
 450         "Disabled",
 451         "Vertical Color Bars",
 452         "Horizontal Color Bars",
 453 };
 454
 455 static const int imx334_test_pattern_val[] = {
 456         IMX334_TP_DISABLE,
 457         IMX334_TP_COLOR_HBARS,
 458         IMX334_TP_COLOR_VBARS,
 459 };
 460
 461 static const struct imx334_reg raw10_framefmt_regs[] = {
 462         {0x3050, 0x00},
 463         {0x319d, 0x00},
 464         {0x341c, 0xff},
 465         {0x341d, 0x01},
 466 };
 467
 468 static const struct imx334_reg raw12_framefmt_regs[] = {
 469         {0x3050, 0x01},
 470         {0x319d, 0x01},
 471         {0x341c, 0x47},
 472         {0x341d, 0x00},
 473 };
 474
 475 static const u32 imx334_mbus_codes[] = {
 476         MEDIA_BUS_FMT_SRGGB12_1X12,
 477         MEDIA_BUS_FMT_SRGGB10_1X10,
 478 };
 479
 480 /* Supported sensor mode configurations */
 481 static const struct imx334_mode supported_modes[] = {
 482         {
 483                 .width = 3840,
 484                 .height = 2160,
 485                 .hblank = 560,
 486                 .vblank = 2340,
 487                 .vblank_min = 90,
 488                 .vblank_max = 132840,
 489                 .pclk = 594000000,
 490                 .link_freq_idx = 0,
 491                 .reg_list = {
 492                         .num_of_regs = ARRAY_SIZE(mode_3840x2160_regs),
 493                         .regs = mode_3840x2160_regs,
 494                 },
 495         }, {
 496                 .width = 1920,
 497                 .height = 1080,
 498                 .hblank = 2480,
 499                 .vblank = 1170,
 500                 .vblank_min = 45,
 501                 .vblank_max = 132840,
 502                 .pclk = 297000000,
 503                 .link_freq_idx = 1,
 504                 .reg_list = {
 505                         .num_of_regs = ARRAY_SIZE(mode_1920x1080_regs),
 506                         .regs = mode_1920x1080_regs,
 507                 },
 508         },
 509 };
 510
 511 /**
 512  * to_imx334() - imv334 V4L2 sub-device to imx334 device.
 513  * @subdev: pointer to imx334 V4L2 sub-device
 514  *
 515  * Return: pointer to imx334 device
 516  */
 517 static inline struct imx334 *to_imx334(struct v4l2_subdev *subdev)
 518 {
 519         return container_of(subdev, struct imx334, sd);
 520 }
 521
 522 /**
 523  * imx334_read_reg() - Read registers.
 524  * @imx334: pointer to imx334 device
 525  * @reg: register address
 526  * @len: length of bytes to read. Max supported bytes is 4
 527  * @val: pointer to register value to be filled.
 528  *
 529  * Big endian register addresses with little endian values.
 530  *
 531  * Return: 0 if successful, error code otherwise.
 532  */
 533 static int imx334_read_reg(struct imx334 *imx334, u16 reg, u32 len, u32 *val)
 534 {
 535         struct i2c_client *client = v4l2_get_subdevdata(&imx334->sd);
 536         struct i2c_msg msgs[2] = {0};
 537         u8 addr_buf[2] = {0};
 538         u8 data_buf[4] = {0};
 539         int ret;
 540
 541         if (WARN_ON(len > 4))
 542                 return -EINVAL;
 543
 544         put_unaligned_be16(reg, addr_buf);
 545
 546         /* Write register address */
 547         msgs[0].addr = client->addr;
 548         msgs[0].flags = 0;
 549         msgs[0].len = ARRAY_SIZE(addr_buf);
 550         msgs[0].buf = addr_buf;
 551
 552         /* Read data from register */
 553         msgs[1].addr = client->addr;
 554         msgs[1].flags = I2C_M_RD;
 555         msgs[1].len = len;
 556         msgs[1].buf = data_buf;
 557
 558         ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 559         if (ret != ARRAY_SIZE(msgs))
 560                 return -EIO;
 561
 562         *val = get_unaligned_le32(data_buf);
 563
 564         return 0;
 565 }
 566
 567 /**
 568  * imx334_write_reg() - Write register
 569  * @imx334: pointer to imx334 device
 570  * @reg: register address
 571  * @len: length of bytes. Max supported bytes is 4
 572  * @val: register value
 573  *
 574  * Big endian register addresses with little endian values.
 575  *
 576  * Return: 0 if successful, error code otherwise.
 577  */
 578 static int imx334_write_reg(struct imx334 *imx334, u16 reg, u32 len, u32 val)
 579 {
 580         struct i2c_client *client = v4l2_get_subdevdata(&imx334->sd);
 581         u8 buf[6] = {0};
 582
 583         if (WARN_ON(len > 4))
 584                 return -EINVAL;
 585
 586         put_unaligned_be16(reg, buf);
 587         put_unaligned_le32(val, buf + 2);
 588         if (i2c_master_send(client, buf, len + 2) != len + 2)
 589                 return -EIO;
 590
 591         return 0;
 592 }
 593
 594 /**
 595  * imx334_write_regs() - Write a list of registers
 596  * @imx334: pointer to imx334 device
 597  * @regs: list of registers to be written
 598  * @len: length of registers array
 599  *
 600  * Return: 0 if successful, error code otherwise.
 601  */
 602 static int imx334_write_regs(struct imx334 *imx334,
 603                              const struct imx334_reg *regs, u32 len)
 604 {
 605         unsigned int i;
 606         int ret;
 607
 608         for (i = 0; i < len; i++) {
 609                 ret = imx334_write_reg(imx334, regs[i].address, 1, regs[i].val);
 610                 if (ret)
 611                         return ret;
 612         }
 613
 614         return 0;
 615 }
 616
 617 /**
 618  * imx334_update_controls() - Update control ranges based on streaming mode
 619  * @imx334: pointer to imx334 device
 620  * @mode: pointer to imx334_mode sensor mode
 621  *
 622  * Return: 0 if successful, error code otherwise.
 623  */
 624 static int imx334_update_controls(struct imx334 *imx334,
 625                                   const struct imx334_mode *mode)
 626 {
 627         int ret;
 628
 629         ret = __v4l2_ctrl_s_ctrl(imx334->link_freq_ctrl, mode->link_freq_idx);
 630         if (ret)
 631                 return ret;
 632
 633         ret = __v4l2_ctrl_modify_range(imx334->pclk_ctrl, mode->pclk,
 634                                        mode->pclk, 1, mode->pclk);
 635         if (ret)
 636                 return ret;
 637
 638         ret = __v4l2_ctrl_modify_range(imx334->hblank_ctrl, mode->hblank,
 639                                        mode->hblank, 1, mode->hblank);
 640         if (ret)
 641                 return ret;
 642
 643         ret =  __v4l2_ctrl_modify_range(imx334->vblank_ctrl, mode->vblank_min,
 644                                         mode->vblank_max, 1, mode->vblank);
 645         if (ret)
 646                 return ret;
 647
 648         return __v4l2_ctrl_s_ctrl(imx334->vblank_ctrl, mode->vblank);
 649 }
 650
 651 /**
 652  * imx334_update_exp_gain() - Set updated exposure and gain
 653  * @imx334: pointer to imx334 device
 654  * @exposure: updated exposure value
 655  * @gain: updated analog gain value
 656  *
 657  * Return: 0 if successful, error code otherwise.
 658  */
 659 static int imx334_update_exp_gain(struct imx334 *imx334, u32 exposure, u32 gain)
 660 {
 661         u32 lpfr, shutter;
 662         int ret;
 663
 664         lpfr = imx334->vblank + imx334->cur_mode->height;
 665         shutter = lpfr - exposure;
 666
 667         dev_dbg(imx334->dev, "Set long exp %u analog gain %u sh0 %u lpfr %u",
 668                 exposure, gain, shutter, lpfr);
 669
 670         ret = imx334_write_reg(imx334, IMX334_REG_HOLD, 1, 1);
 671         if (ret)
 672                 return ret;
 673
 674         ret = imx334_write_reg(imx334, IMX334_REG_LPFR, 3, lpfr);
 675         if (ret)
 676                 goto error_release_group_hold;
 677
 678         ret = imx334_write_reg(imx334, IMX334_REG_SHUTTER, 3, shutter);
 679         if (ret)
 680                 goto error_release_group_hold;
 681
 682         ret = imx334_write_reg(imx334, IMX334_REG_AGAIN, 1, gain);
 683
 684 error_release_group_hold:
 685         imx334_write_reg(imx334, IMX334_REG_HOLD, 1, 0);
 686
 687         return ret;
 688 }
 689
 690 /**
 691  * imx334_set_ctrl() - Set subdevice control
 692  * @ctrl: pointer to v4l2_ctrl structure
 693  *
 694  * Supported controls:
 695  * - V4L2_CID_VBLANK
 696  * - cluster controls:
 697  *   - V4L2_CID_ANALOGUE_GAIN
 698  *   - V4L2_CID_EXPOSURE
 699  *
 700  * Return: 0 if successful, error code otherwise.
 701  */
 702 static int imx334_set_ctrl(struct v4l2_ctrl *ctrl)
 703 {
 704         struct imx334 *imx334 =
 705                 container_of(ctrl->handler, struct imx334, ctrl_handler);
 706         u32 analog_gain;
 707         u32 exposure;
 708         int ret;
 709
 710         switch (ctrl->id) {
 711         case V4L2_CID_VBLANK:
 712                 imx334->vblank = imx334->vblank_ctrl->val;
 713
 714                 dev_dbg(imx334->dev, "Received vblank %u, new lpfr %u",
 715                         imx334->vblank,
 716                         imx334->vblank + imx334->cur_mode->height);
 717
 718                 ret = __v4l2_ctrl_modify_range(imx334->exp_ctrl,
 719                                                IMX334_EXPOSURE_MIN,
 720                                                imx334->vblank +
 721                                                imx334->cur_mode->height -
 722                                                IMX334_EXPOSURE_OFFSET,
 723                                                1, IMX334_EXPOSURE_DEFAULT);
 724                 break;
 725         case V4L2_CID_EXPOSURE:
 726
 727                 /* Set controls only if sensor is in power on state */
 728                 if (!pm_runtime_get_if_in_use(imx334->dev))
 729                         return 0;
 730
 731                 exposure = ctrl->val;
 732                 analog_gain = imx334->again_ctrl->val;
 733
 734                 dev_dbg(imx334->dev, "Received exp %u analog gain %u",
 735                         exposure, analog_gain);
 736
 737                 ret = imx334_update_exp_gain(imx334, exposure, analog_gain);
 738
 739                 pm_runtime_put(imx334->dev);
 740
 741                 break;
 742         case V4L2_CID_PIXEL_RATE:
 743         case V4L2_CID_LINK_FREQ:
 744         case V4L2_CID_HBLANK:
 745                 ret = 0;
 746                 break;
 747         case V4L2_CID_TEST_PATTERN:
 748                 if (ctrl->val) {
 749                         imx334_write_reg(imx334, IMX334_TP_CLK_EN, 1,
 750                                          IMX334_TP_CLK_EN_VAL);
 751                         imx334_write_reg(imx334, IMX334_DIG_CLP_MODE, 1, 0x0);
 752                         imx334_write_reg(imx334, IMX334_TPG_COLORW, 1,
 753                                          IMX334_TPG_COLORW_120P);
 754                         imx334_write_reg(imx334, IMX334_REG_TP, 1,
 755                                          imx334_test_pattern_val[ctrl->val]);
 756                         imx334_write_reg(imx334, IMX334_TPG_EN_DOUT, 1,
 757                                          IMX334_TP_ENABLE);
 758                 } else {
 759                         imx334_write_reg(imx334, IMX334_DIG_CLP_MODE, 1, 0x1);
 760                         imx334_write_reg(imx334, IMX334_TP_CLK_EN, 1,
 761                                          IMX334_TP_CLK_DIS_VAL);
 762                         imx334_write_reg(imx334, IMX334_TPG_EN_DOUT, 1,
 763                                          IMX334_TP_DISABLE);
 764                 }
 765                 ret = 0;
 766                 break;
 767         default:
 768                 dev_err(imx334->dev, "Invalid control %d", ctrl->id);
 769                 ret = -EINVAL;
 770         }
 771
 772         return ret;
 773 }
 774
 775 /* V4l2 subdevice control ops*/
 776 static const struct v4l2_ctrl_ops imx334_ctrl_ops = {
 777         .s_ctrl = imx334_set_ctrl,
 778 };
 779
 780 static int imx334_get_format_code(struct imx334 *imx334, u32 code)
 781 {
 782         unsigned int i;
 783
 784         for (i = 0; i < ARRAY_SIZE(imx334_mbus_codes); i++) {
 785                 if (imx334_mbus_codes[i] == code)
 786                         return imx334_mbus_codes[i];
 787         }
 788
 789         return imx334_mbus_codes[0];
 790 }
 791
 792 /**
 793  * imx334_enum_mbus_code() - Enumerate V4L2 sub-device mbus codes
 794  * @sd: pointer to imx334 V4L2 sub-device structure
 795  * @sd_state: V4L2 sub-device state
 796  * @code: V4L2 sub-device code enumeration need to be filled
 797  *
 798  * Return: 0 if successful, error code otherwise.
 799  */
 800 static int imx334_enum_mbus_code(struct v4l2_subdev *sd,
 801                                  struct v4l2_subdev_state *sd_state,
 802                                  struct v4l2_subdev_mbus_code_enum *code)
 803 {
 804         if (code->index >= ARRAY_SIZE(imx334_mbus_codes))
 805                 return -EINVAL;
 806
 807         code->code = imx334_mbus_codes[code->index];
 808
 809         return 0;
 810 }
 811
 812 /**
 813  * imx334_enum_frame_size() - Enumerate V4L2 sub-device frame sizes
 814  * @sd: pointer to imx334 V4L2 sub-device structure
 815  * @sd_state: V4L2 sub-device state
 816  * @fsize: V4L2 sub-device size enumeration need to be filled
 817  *
 818  * Return: 0 if successful, error code otherwise.
 819  */
 820 static int imx334_enum_frame_size(struct v4l2_subdev *sd,
 821                                   struct v4l2_subdev_state *sd_state,
 822                                   struct v4l2_subdev_frame_size_enum *fsize)
 823 {
 824         struct imx334 *imx334 = to_imx334(sd);
 825         u32 code;
 826
 827         if (fsize->index >= ARRAY_SIZE(supported_modes))
 828                 return -EINVAL;
 829
 830         code = imx334_get_format_code(imx334, fsize->code);
 831
 832         if (fsize->code != code)
 833                 return -EINVAL;
 834
 835         fsize->min_width = supported_modes[fsize->index].width;
 836         fsize->max_width = fsize->min_width;
 837         fsize->min_height = supported_modes[fsize->index].height;
 838         fsize->max_height = fsize->min_height;
 839
 840         return 0;
 841 }
 842
 843 /**
 844  * imx334_fill_pad_format() - Fill subdevice pad format
 845  *                            from selected sensor mode
 846  * @imx334: pointer to imx334 device
 847  * @mode: pointer to imx334_mode sensor mode
 848  * @fmt: V4L2 sub-device format need to be filled
 849  */
 850 static void imx334_fill_pad_format(struct imx334 *imx334,
 851                                    const struct imx334_mode *mode,
 852                                    struct v4l2_subdev_format *fmt)
 853 {
 854         fmt->format.width = mode->width;
 855         fmt->format.height = mode->height;
 856         fmt->format.field = V4L2_FIELD_NONE;
 857         fmt->format.colorspace = V4L2_COLORSPACE_RAW;
 858         fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 859         fmt->format.quantization = V4L2_QUANTIZATION_DEFAULT;
 860         fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
 861 }
 862
 863 /**
 864  * imx334_get_pad_format() - Get subdevice pad format
 865  * @sd: pointer to imx334 V4L2 sub-device structure
 866  * @sd_state: V4L2 sub-device state
 867  * @fmt: V4L2 sub-device format need to be set
 868  *
 869  * Return: 0 if successful, error code otherwise.
 870  */
 871 static int imx334_get_pad_format(struct v4l2_subdev *sd,
 872                                  struct v4l2_subdev_state *sd_state,
 873                                  struct v4l2_subdev_format *fmt)
 874 {
 875         struct imx334 *imx334 = to_imx334(sd);
 876
 877         mutex_lock(&imx334->mutex);
 878
 879         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 880                 struct v4l2_mbus_framefmt *framefmt;
 881
 882                 framefmt = v4l2_subdev_state_get_format(sd_state, fmt->pad);
 883                 fmt->format = *framefmt;
 884         } else {
 885                 fmt->format.code = imx334->cur_code;
 886                 imx334_fill_pad_format(imx334, imx334->cur_mode, fmt);
 887         }
 888
 889         mutex_unlock(&imx334->mutex);
 890
 891         return 0;
 892 }
 893
 894 /**
 895  * imx334_set_pad_format() - Set subdevice pad format
 896  * @sd: pointer to imx334 V4L2 sub-device structure
 897  * @sd_state: V4L2 sub-device state
 898  * @fmt: V4L2 sub-device format need to be set
 899  *
 900  * Return: 0 if successful, error code otherwise.
 901  */
 902 static int imx334_set_pad_format(struct v4l2_subdev *sd,
 903                                  struct v4l2_subdev_state *sd_state,
 904                                  struct v4l2_subdev_format *fmt)
 905 {
 906         struct imx334 *imx334 = to_imx334(sd);
 907         const struct imx334_mode *mode;
 908         int ret = 0;
 909
 910         mutex_lock(&imx334->mutex);
 911
 912         mode = v4l2_find_nearest_size(supported_modes,
 913                                       ARRAY_SIZE(supported_modes),
 914                                       width, height,
 915                                       fmt->format.width, fmt->format.height);
 916
 917         imx334_fill_pad_format(imx334, mode, fmt);
 918         fmt->format.code = imx334_get_format_code(imx334, fmt->format.code);
 919
 920         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 921                 struct v4l2_mbus_framefmt *framefmt;
 922
 923                 framefmt = v4l2_subdev_state_get_format(sd_state, fmt->pad);
 924                 *framefmt = fmt->format;
 925         } else if (imx334->cur_mode != mode || imx334->cur_code != fmt->format.code) {
 926                 imx334->cur_code = fmt->format.code;
 927                 ret = imx334_update_controls(imx334, mode);
 928                 if (!ret)
 929                         imx334->cur_mode = mode;
 930         }
 931
 932         mutex_unlock(&imx334->mutex);
 933
 934         return ret;
 935 }
 936
 937 /**
 938  * imx334_init_state() - Initialize sub-device state
 939  * @sd: pointer to imx334 V4L2 sub-device structure
 940  * @sd_state: V4L2 sub-device state
 941  *
 942  * Return: 0 if successful, error code otherwise.
 943  */
 944 static int imx334_init_state(struct v4l2_subdev *sd,
 945                              struct v4l2_subdev_state *sd_state)
 946 {
 947         struct imx334 *imx334 = to_imx334(sd);
 948         struct v4l2_subdev_format fmt = { 0 };
 949
 950         fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
 951
 952         mutex_lock(&imx334->mutex);
 953
 954         imx334_fill_pad_format(imx334, imx334->cur_mode, &fmt);
 955
 956         __v4l2_ctrl_modify_range(imx334->link_freq_ctrl, 0,
 957                                  __fls(imx334->link_freq_bitmap),
 958                                  ~(imx334->link_freq_bitmap),
 959                                  __ffs(imx334->link_freq_bitmap));
 960
 961         mutex_unlock(&imx334->mutex);
 962
 963         return imx334_set_pad_format(sd, sd_state, &fmt);
 964 }
 965
 966 static int imx334_set_framefmt(struct imx334 *imx334)
 967 {
 968         switch (imx334->cur_code) {
 969         case MEDIA_BUS_FMT_SRGGB10_1X10:
 970                 return imx334_write_regs(imx334, raw10_framefmt_regs,
 971                                          ARRAY_SIZE(raw10_framefmt_regs));
 972
 973         case MEDIA_BUS_FMT_SRGGB12_1X12:
 974                 return imx334_write_regs(imx334, raw12_framefmt_regs,
 975                                          ARRAY_SIZE(raw12_framefmt_regs));
 976         }
 977
 978         return -EINVAL;
 979 }
 980
 981 /**
 982  * imx334_start_streaming() - Start sensor stream
 983  * @imx334: pointer to imx334 device
 984  *
 985  * Return: 0 if successful, error code otherwise.
 986  */
 987 static int imx334_start_streaming(struct imx334 *imx334)
 988 {
 989         const struct imx334_reg_list *reg_list;
 990         int ret;
 991
 992         /* Write sensor mode registers */
 993         reg_list = &imx334->cur_mode->reg_list;
 994         ret = imx334_write_regs(imx334, reg_list->regs,
 995                                 reg_list->num_of_regs);
 996         if (ret) {
 997                 dev_err(imx334->dev, "fail to write initial registers");
 998                 return ret;
 999         }
1000
1001         ret = imx334_set_framefmt(imx334);
1002         if (ret) {
1003                 dev_err(imx334->dev, "%s failed to set frame format: %d\n",
1004                         __func__, ret);
1005                 return ret;
1006         }
1007
1008         /* Setup handler will write actual exposure and gain */
1009         ret =  __v4l2_ctrl_handler_setup(imx334->sd.ctrl_handler);
1010         if (ret) {
1011                 dev_err(imx334->dev, "fail to setup handler");
1012                 return ret;
1013         }
1014
1015         /* Start streaming */
1016         ret = imx334_write_reg(imx334, IMX334_REG_MODE_SELECT,
1017                                1, IMX334_MODE_STREAMING);
1018         if (ret) {
1019                 dev_err(imx334->dev, "fail to start streaming");
1020                 return ret;
1021         }
1022
1023         return 0;
1024 }
1025
1026 /**
1027  * imx334_stop_streaming() - Stop sensor stream
1028  * @imx334: pointer to imx334 device
1029  *
1030  * Return: 0 if successful, error code otherwise.
1031  */
1032 static int imx334_stop_streaming(struct imx334 *imx334)
1033 {
1034         return imx334_write_reg(imx334, IMX334_REG_MODE_SELECT,
1035                                 1, IMX334_MODE_STANDBY);
1036 }
1037
1038 /**
1039  * imx334_set_stream() - Enable sensor streaming
1040  * @sd: pointer to imx334 subdevice
1041  * @enable: set to enable sensor streaming
1042  *
1043  * Return: 0 if successful, error code otherwise.
1044  */
1045 static int imx334_set_stream(struct v4l2_subdev *sd, int enable)
1046 {
1047         struct imx334 *imx334 = to_imx334(sd);
1048         int ret;
1049
1050         mutex_lock(&imx334->mutex);
1051
1052         if (enable) {
1053                 ret = pm_runtime_resume_and_get(imx334->dev);
1054                 if (ret < 0)
1055                         goto error_unlock;
1056
1057                 ret = imx334_start_streaming(imx334);
1058                 if (ret)
1059                         goto error_power_off;
1060         } else {
1061                 imx334_stop_streaming(imx334);
1062                 pm_runtime_put(imx334->dev);
1063         }
1064
1065         mutex_unlock(&imx334->mutex);
1066
1067         return 0;
1068
1069 error_power_off:
1070         pm_runtime_put(imx334->dev);
1071 error_unlock:
1072         mutex_unlock(&imx334->mutex);
1073
1074         return ret;
1075 }
1076
1077 /**
1078  * imx334_detect() - Detect imx334 sensor
1079  * @imx334: pointer to imx334 device
1080  *
1081  * Return: 0 if successful, -EIO if sensor id does not match
1082  */
1083 static int imx334_detect(struct imx334 *imx334)
1084 {
1085         int ret;
1086         u32 val;
1087
1088         ret = imx334_read_reg(imx334, IMX334_REG_ID, 2, &val);
1089         if (ret)
1090                 return ret;
1091
1092         if (val != IMX334_ID) {
1093                 dev_err(imx334->dev, "chip id mismatch: %x!=%x",
1094                         IMX334_ID, val);
1095                 return -ENXIO;
1096         }
1097
1098         return 0;
1099 }
1100
1101 /**
1102  * imx334_parse_hw_config() - Parse HW configuration and check if supported
1103  * @imx334: pointer to imx334 device
1104  *
1105  * Return: 0 if successful, error code otherwise.
1106  */
1107 static int imx334_parse_hw_config(struct imx334 *imx334)
1108 {
1109         struct fwnode_handle *fwnode = dev_fwnode(imx334->dev);
1110         struct v4l2_fwnode_endpoint bus_cfg = {
1111                 .bus_type = V4L2_MBUS_CSI2_DPHY
1112         };
1113         struct fwnode_handle *ep;
1114         unsigned long rate;
1115         int ret;
1116
1117         if (!fwnode)
1118                 return -ENXIO;
1119
1120         /* Request optional reset pin */
1121         imx334->reset_gpio = devm_gpiod_get_optional(imx334->dev, "reset",
1122                                                      GPIOD_OUT_LOW);
1123         if (IS_ERR(imx334->reset_gpio)) {
1124                 dev_err(imx334->dev, "failed to get reset gpio %ld",
1125                         PTR_ERR(imx334->reset_gpio));
1126                 return PTR_ERR(imx334->reset_gpio);
1127         }
1128
1129         /* Get sensor input clock */
1130         imx334->inclk = devm_clk_get(imx334->dev, NULL);
1131         if (IS_ERR(imx334->inclk)) {
1132                 dev_err(imx334->dev, "could not get inclk");
1133                 return PTR_ERR(imx334->inclk);
1134         }
1135
1136         rate = clk_get_rate(imx334->inclk);
1137         if (rate != IMX334_INCLK_RATE) {
1138                 dev_err(imx334->dev, "inclk frequency mismatch");
1139                 return -EINVAL;
1140         }
1141
1142         ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
1143         if (!ep)
1144                 return -ENXIO;
1145
1146         ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
1147         fwnode_handle_put(ep);
1148         if (ret)
1149                 return ret;
1150
1151         if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX334_NUM_DATA_LANES) {
1152                 dev_err(imx334->dev,
1153                         "number of CSI2 data lanes %d is not supported",
1154                         bus_cfg.bus.mipi_csi2.num_data_lanes);
1155                 ret = -EINVAL;
1156                 goto done_endpoint_free;
1157         }
1158
1159         ret = v4l2_link_freq_to_bitmap(imx334->dev, bus_cfg.link_frequencies,
1160                                        bus_cfg.nr_of_link_frequencies,
1161                                        link_freq, ARRAY_SIZE(link_freq),
1162                                        &imx334->link_freq_bitmap);
1163
1164 done_endpoint_free:
1165         v4l2_fwnode_endpoint_free(&bus_cfg);
1166
1167         return ret;
1168 }
1169
1170 /* V4l2 subdevice ops */
1171 static const struct v4l2_subdev_video_ops imx334_video_ops = {
1172         .s_stream = imx334_set_stream,
1173 };
1174
1175 static const struct v4l2_subdev_pad_ops imx334_pad_ops = {
1176         .enum_mbus_code = imx334_enum_mbus_code,
1177         .enum_frame_size = imx334_enum_frame_size,
1178         .get_fmt = imx334_get_pad_format,
1179         .set_fmt = imx334_set_pad_format,
1180 };
1181
1182 static const struct v4l2_subdev_ops imx334_subdev_ops = {
1183         .video = &imx334_video_ops,
1184         .pad = &imx334_pad_ops,
1185 };
1186
1187 static const struct v4l2_subdev_internal_ops imx334_internal_ops = {
1188         .init_state = imx334_init_state,
1189 };
1190
1191 /**
1192  * imx334_power_on() - Sensor power on sequence
1193  * @dev: pointer to i2c device
1194  *
1195  * Return: 0 if successful, error code otherwise.
1196  */
1197 static int imx334_power_on(struct device *dev)
1198 {
1199         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1200         struct imx334 *imx334 = to_imx334(sd);
1201         int ret;
1202
1203         gpiod_set_value_cansleep(imx334->reset_gpio, 1);
1204
1205         ret = clk_prepare_enable(imx334->inclk);
1206         if (ret) {
1207                 dev_err(imx334->dev, "fail to enable inclk");
1208                 goto error_reset;
1209         }
1210
1211         usleep_range(18000, 20000);
1212
1213         return 0;
1214
1215 error_reset:
1216         gpiod_set_value_cansleep(imx334->reset_gpio, 0);
1217
1218         return ret;
1219 }
1220
1221 /**
1222  * imx334_power_off() - Sensor power off sequence
1223  * @dev: pointer to i2c device
1224  *
1225  * Return: 0 if successful, error code otherwise.
1226  */
1227 static int imx334_power_off(struct device *dev)
1228 {
1229         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1230         struct imx334 *imx334 = to_imx334(sd);
1231
1232         gpiod_set_value_cansleep(imx334->reset_gpio, 0);
1233
1234         clk_disable_unprepare(imx334->inclk);
1235
1236         return 0;
1237 }
1238
1239 /**
1240  * imx334_init_controls() - Initialize sensor subdevice controls
1241  * @imx334: pointer to imx334 device
1242  *
1243  * Return: 0 if successful, error code otherwise.
1244  */
1245 static int imx334_init_controls(struct imx334 *imx334)
1246 {
1247         struct v4l2_ctrl_handler *ctrl_hdlr = &imx334->ctrl_handler;
1248         const struct imx334_mode *mode = imx334->cur_mode;
1249         u32 lpfr;
1250         int ret;
1251
1252         ret = v4l2_ctrl_handler_init(ctrl_hdlr, 7);
1253         if (ret)
1254                 return ret;
1255
1256         /* Serialize controls with sensor device */
1257         ctrl_hdlr->lock = &imx334->mutex;
1258
1259         /* Initialize exposure and gain */
1260         lpfr = mode->vblank + mode->height;
1261         imx334->exp_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
1262                                              &imx334_ctrl_ops,
1263                                              V4L2_CID_EXPOSURE,
1264                                              IMX334_EXPOSURE_MIN,
1265                                              lpfr - IMX334_EXPOSURE_OFFSET,
1266                                              IMX334_EXPOSURE_STEP,
1267                                              IMX334_EXPOSURE_DEFAULT);
1268
1269         imx334->again_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
1270                                                &imx334_ctrl_ops,
1271                                                V4L2_CID_ANALOGUE_GAIN,
1272                                                IMX334_AGAIN_MIN,
1273                                                IMX334_AGAIN_MAX,
1274                                                IMX334_AGAIN_STEP,
1275                                                IMX334_AGAIN_DEFAULT);
1276
1277         v4l2_ctrl_cluster(2, &imx334->exp_ctrl);
1278
1279         imx334->vblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
1280                                                 &imx334_ctrl_ops,
1281                                                 V4L2_CID_VBLANK,
1282                                                 mode->vblank_min,
1283                                                 mode->vblank_max,
1284                                                 1, mode->vblank);
1285
1286         /* Read only controls */
1287         imx334->pclk_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
1288                                               &imx334_ctrl_ops,
1289                                               V4L2_CID_PIXEL_RATE,
1290                                               mode->pclk, mode->pclk,
1291                                               1, mode->pclk);
1292
1293         imx334->link_freq_ctrl = v4l2_ctrl_new_int_menu(ctrl_hdlr,
1294                                                         &imx334_ctrl_ops,
1295                                                         V4L2_CID_LINK_FREQ,
1296                                                         __fls(imx334->link_freq_bitmap),
1297                                                         __ffs(imx334->link_freq_bitmap),
1298                                                         link_freq);
1299
1300         if (imx334->link_freq_ctrl)
1301                 imx334->link_freq_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1302
1303         imx334->hblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
1304                                                 &imx334_ctrl_ops,
1305                                                 V4L2_CID_HBLANK,
1306                                                 IMX334_REG_MIN,
1307                                                 IMX334_REG_MAX,
1308                                                 1, mode->hblank);
1309         if (imx334->hblank_ctrl)
1310                 imx334->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1311
1312         v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx334_ctrl_ops,
1313                                      V4L2_CID_TEST_PATTERN,
1314                                      ARRAY_SIZE(imx334_test_pattern_menu) - 1,
1315                                      0, 0, imx334_test_pattern_menu);
1316
1317         if (ctrl_hdlr->error) {
1318                 dev_err(imx334->dev, "control init failed: %d",
1319                         ctrl_hdlr->error);
1320                 v4l2_ctrl_handler_free(ctrl_hdlr);
1321                 return ctrl_hdlr->error;
1322         }
1323
1324         imx334->sd.ctrl_handler = ctrl_hdlr;
1325
1326         return 0;
1327 }
1328
1329 /**
1330  * imx334_probe() - I2C client device binding
1331  * @client: pointer to i2c client device
1332  *
1333  * Return: 0 if successful, error code otherwise.
1334  */
1335 static int imx334_probe(struct i2c_client *client)
1336 {
1337         struct imx334 *imx334;
1338         int ret;
1339
1340         imx334 = devm_kzalloc(&client->dev, sizeof(*imx334), GFP_KERNEL);
1341         if (!imx334)
1342                 return -ENOMEM;
1343
1344         imx334->dev = &client->dev;
1345
1346         /* Initialize subdev */
1347         v4l2_i2c_subdev_init(&imx334->sd, client, &imx334_subdev_ops);
1348         imx334->sd.internal_ops = &imx334_internal_ops;
1349
1350         ret = imx334_parse_hw_config(imx334);
1351         if (ret) {
1352                 dev_err(imx334->dev, "HW configuration is not supported");
1353                 return ret;
1354         }
1355
1356         mutex_init(&imx334->mutex);
1357
1358         ret = imx334_power_on(imx334->dev);
1359         if (ret) {
1360                 dev_err(imx334->dev, "failed to power-on the sensor");
1361                 goto error_mutex_destroy;
1362         }
1363
1364         /* Check module identity */
1365         ret = imx334_detect(imx334);
1366         if (ret) {
1367                 dev_err(imx334->dev, "failed to find sensor: %d", ret);
1368                 goto error_power_off;
1369         }
1370
1371         /* Set default mode to max resolution */
1372         imx334->cur_mode = &supported_modes[__ffs(imx334->link_freq_bitmap)];
1373         imx334->cur_code = imx334_mbus_codes[0];
1374         imx334->vblank = imx334->cur_mode->vblank;
1375
1376         ret = imx334_init_controls(imx334);
1377         if (ret) {
1378                 dev_err(imx334->dev, "failed to init controls: %d", ret);
1379                 goto error_power_off;
1380         }
1381
1382         /* Initialize subdev */
1383         imx334->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1384         imx334->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1385
1386         /* Initialize source pad */
1387         imx334->pad.flags = MEDIA_PAD_FL_SOURCE;
1388         ret = media_entity_pads_init(&imx334->sd.entity, 1, &imx334->pad);
1389         if (ret) {
1390                 dev_err(imx334->dev, "failed to init entity pads: %d", ret);
1391                 goto error_handler_free;
1392         }
1393
1394         ret = v4l2_async_register_subdev_sensor(&imx334->sd);
1395         if (ret < 0) {
1396                 dev_err(imx334->dev,
1397                         "failed to register async subdev: %d", ret);
1398                 goto error_media_entity;
1399         }
1400
1401         pm_runtime_set_active(imx334->dev);
1402         pm_runtime_enable(imx334->dev);
1403         pm_runtime_idle(imx334->dev);
1404
1405         return 0;
1406
1407 error_media_entity:
1408         media_entity_cleanup(&imx334->sd.entity);
1409 error_handler_free:
1410         v4l2_ctrl_handler_free(imx334->sd.ctrl_handler);
1411 error_power_off:
1412         imx334_power_off(imx334->dev);
1413 error_mutex_destroy:
1414         mutex_destroy(&imx334->mutex);
1415
1416         return ret;
1417 }
1418
1419 /**
1420  * imx334_remove() - I2C client device unbinding
1421  * @client: pointer to I2C client device
1422  *
1423  * Return: 0 if successful, error code otherwise.
1424  */
1425 static void imx334_remove(struct i2c_client *client)
1426 {
1427         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1428         struct imx334 *imx334 = to_imx334(sd);
1429
1430         v4l2_async_unregister_subdev(sd);
1431         media_entity_cleanup(&sd->entity);
1432         v4l2_ctrl_handler_free(sd->ctrl_handler);
1433
1434         pm_runtime_disable(&client->dev);
1435         pm_runtime_suspended(&client->dev);
1436
1437         mutex_destroy(&imx334->mutex);
1438 }
1439
1440 static const struct dev_pm_ops imx334_pm_ops = {
1441         SET_RUNTIME_PM_OPS(imx334_power_off, imx334_power_on, NULL)
1442 };
1443
1444 static const struct of_device_id imx334_of_match[] = {
1445         { .compatible = "sony,imx334" },
1446         { }
1447 };
1448
1449 MODULE_DEVICE_TABLE(of, imx334_of_match);
1450
1451 static struct i2c_driver imx334_driver = {
1452         .probe = imx334_probe,
1453         .remove = imx334_remove,
1454         .driver = {
1455                 .name = "imx334",
1456                 .pm = &imx334_pm_ops,
1457                 .of_match_table = imx334_of_match,
1458         },
1459 };
1460
1461 module_i2c_driver(imx334_driver);
1462
1463 MODULE_DESCRIPTION("Sony imx334 sensor driver");
1464 MODULE_LICENSE("GPL");