drivers/media/i2c/adv7511.c

   1 /*
   2  * Analog Devices ADV7511 HDMI Transmitter Device Driver
   3  *
   4  * Copyright 2013 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
   5  *
   6  * This program is free software; you may redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; version 2 of the License.
   9  *
  10  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  11  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  12  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  13  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  14  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  15  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  16  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  17  * SOFTWARE.
  18  */
  19
  20
  21 #include <linux/kernel.h>
  22 #include <linux/module.h>
  23 #include <linux/slab.h>
  24 #include <linux/i2c.h>
  25 #include <linux/delay.h>
  26 #include <linux/videodev2.h>
  27 #include <linux/gpio.h>
  28 #include <linux/workqueue.h>
  29 #include <linux/v4l2-dv-timings.h>
  30 #include <media/v4l2-device.h>
  31 #include <media/v4l2-common.h>
  32 #include <media/v4l2-ctrls.h>
  33 #include <media/v4l2-dv-timings.h>
  34 #include <media/adv7511.h>
  35
  36 static int debug;
  37 module_param(debug, int, 0644);
  38 MODULE_PARM_DESC(debug, "debug level (0-2)");
  39
  40 MODULE_DESCRIPTION("Analog Devices ADV7511 HDMI Transmitter Device Driver");
  41 MODULE_AUTHOR("Hans Verkuil");
  42 MODULE_LICENSE("GPL");
  43
  44 #define MASK_ADV7511_EDID_RDY_INT   0x04
  45 #define MASK_ADV7511_MSEN_INT       0x40
  46 #define MASK_ADV7511_HPD_INT        0x80
  47
  48 #define MASK_ADV7511_HPD_DETECT     0x40
  49 #define MASK_ADV7511_MSEN_DETECT    0x20
  50 #define MASK_ADV7511_EDID_RDY       0x10
  51
  52 #define EDID_MAX_RETRIES (8)
  53 #define EDID_DELAY 250
  54 #define EDID_MAX_SEGM 8
  55
  56 #define ADV7511_MAX_WIDTH 1920
  57 #define ADV7511_MAX_HEIGHT 1200
  58 #define ADV7511_MIN_PIXELCLOCK 20000000
  59 #define ADV7511_MAX_PIXELCLOCK 225000000
  60
  61 /*
  62 **********************************************************************
  63 *
  64 *  Arrays with configuration parameters for the ADV7511
  65 *
  66 **********************************************************************
  67 */
  68
  69 struct i2c_reg_value {
  70         unsigned char reg;
  71         unsigned char value;
  72 };
  73
  74 struct adv7511_state_edid {
  75         /* total number of blocks */
  76         u32 blocks;
  77         /* Number of segments read */
  78         u32 segments;
  79         uint8_t data[EDID_MAX_SEGM * 256];
  80         /* Number of EDID read retries left */
  81         unsigned read_retries;
  82         bool complete;
  83 };
  84
  85 struct adv7511_state {
  86         struct adv7511_platform_data pdata;
  87         struct v4l2_subdev sd;
  88         struct media_pad pad;
  89         struct v4l2_ctrl_handler hdl;
  90         int chip_revision;
  91         uint8_t i2c_edid_addr;
  92         uint8_t i2c_cec_addr;
  93         /* Is the adv7511 powered on? */
  94         bool power_on;
  95         /* Did we receive hotplug and rx-sense signals? */
  96         bool have_monitor;
  97         /* timings from s_dv_timings */
  98         struct v4l2_dv_timings dv_timings;
  99         /* controls */
 100         struct v4l2_ctrl *hdmi_mode_ctrl;
 101         struct v4l2_ctrl *hotplug_ctrl;
 102         struct v4l2_ctrl *rx_sense_ctrl;
 103         struct v4l2_ctrl *have_edid0_ctrl;
 104         struct v4l2_ctrl *rgb_quantization_range_ctrl;
 105         struct i2c_client *i2c_edid;
 106         struct adv7511_state_edid edid;
 107         /* Running counter of the number of detected EDIDs (for debugging) */
 108         unsigned edid_detect_counter;
 109         struct workqueue_struct *work_queue;
 110         struct delayed_work edid_handler; /* work entry */
 111 };
 112
 113 static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd);
 114 static bool adv7511_check_edid_status(struct v4l2_subdev *sd);
 115 static void adv7511_setup(struct v4l2_subdev *sd);
 116 static int adv7511_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq);
 117 static int adv7511_s_clock_freq(struct v4l2_subdev *sd, u32 freq);
 118
 119
 120 static const struct v4l2_dv_timings_cap adv7511_timings_cap = {
 121         .type = V4L2_DV_BT_656_1120,
 122         /* keep this initialization for compatibility with GCC < 4.4.6 */
 123         .reserved = { 0 },
 124         V4L2_INIT_BT_TIMINGS(0, ADV7511_MAX_WIDTH, 0, ADV7511_MAX_HEIGHT,
 125                 ADV7511_MIN_PIXELCLOCK, ADV7511_MAX_PIXELCLOCK,
 126                 V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
 127                         V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
 128                 V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
 129                         V4L2_DV_BT_CAP_CUSTOM)
 130 };
 131
 132 static inline struct adv7511_state *get_adv7511_state(struct v4l2_subdev *sd)
 133 {
 134         return container_of(sd, struct adv7511_state, sd);
 135 }
 136
 137 static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
 138 {
 139         return &container_of(ctrl->handler, struct adv7511_state, hdl)->sd;
 140 }
 141
 142 /* ------------------------ I2C ----------------------------------------------- */
 143
 144 static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
 145                                           u8 command, bool check)
 146 {
 147         union i2c_smbus_data data;
 148
 149         if (!i2c_smbus_xfer(client->adapter, client->addr, client->flags,
 150                             I2C_SMBUS_READ, command,
 151                             I2C_SMBUS_BYTE_DATA, &data))
 152                 return data.byte;
 153         if (check)
 154                 v4l_err(client, "error reading %02x, %02x\n",
 155                         client->addr, command);
 156         return -1;
 157 }
 158
 159 static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
 160 {
 161         int i;
 162         for (i = 0; i < 3; i++) {
 163                 int ret = adv_smbus_read_byte_data_check(client, command, true);
 164                 if (ret >= 0) {
 165                         if (i)
 166                                 v4l_err(client, "read ok after %d retries\n", i);
 167                         return ret;
 168                 }
 169         }
 170         v4l_err(client, "read failed\n");
 171         return -1;
 172 }
 173
 174 static int adv7511_rd(struct v4l2_subdev *sd, u8 reg)
 175 {
 176         struct i2c_client *client = v4l2_get_subdevdata(sd);
 177
 178         return adv_smbus_read_byte_data(client, reg);
 179 }
 180
 181 static int adv7511_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
 182 {
 183         struct i2c_client *client = v4l2_get_subdevdata(sd);
 184         int ret;
 185         int i;
 186
 187         for (i = 0; i < 3; i++) {
 188                 ret = i2c_smbus_write_byte_data(client, reg, val);
 189                 if (ret == 0)
 190                         return 0;
 191         }
 192         v4l2_err(sd, "%s: i2c write error\n", __func__);
 193         return ret;
 194 }
 195
 196 /* To set specific bits in the register, a clear-mask is given (to be AND-ed),
 197    and then the value-mask (to be OR-ed). */
 198 static inline void adv7511_wr_and_or(struct v4l2_subdev *sd, u8 reg, uint8_t clr_mask, uint8_t val_mask)
 199 {
 200         adv7511_wr(sd, reg, (adv7511_rd(sd, reg) & clr_mask) | val_mask);
 201 }
 202
 203 static int adv_smbus_read_i2c_block_data(struct i2c_client *client,
 204                                          u8 command, unsigned length, u8 *values)
 205 {
 206         union i2c_smbus_data data;
 207         int ret;
 208
 209         if (length > I2C_SMBUS_BLOCK_MAX)
 210                 length = I2C_SMBUS_BLOCK_MAX;
 211         data.block[0] = length;
 212
 213         ret = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
 214                              I2C_SMBUS_READ, command,
 215                              I2C_SMBUS_I2C_BLOCK_DATA, &data);
 216         memcpy(values, data.block + 1, length);
 217         return ret;
 218 }
 219
 220 static inline void adv7511_edid_rd(struct v4l2_subdev *sd, uint16_t len, uint8_t *buf)
 221 {
 222         struct adv7511_state *state = get_adv7511_state(sd);
 223         int i;
 224         int err = 0;
 225
 226         v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 227
 228         for (i = 0; !err && i < len; i += I2C_SMBUS_BLOCK_MAX)
 229                 err = adv_smbus_read_i2c_block_data(state->i2c_edid, i,
 230                                                     I2C_SMBUS_BLOCK_MAX, buf + i);
 231         if (err)
 232                 v4l2_err(sd, "%s: i2c read error\n", __func__);
 233 }
 234
 235 static inline bool adv7511_have_hotplug(struct v4l2_subdev *sd)
 236 {
 237         return adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT;
 238 }
 239
 240 static inline bool adv7511_have_rx_sense(struct v4l2_subdev *sd)
 241 {
 242         return adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT;
 243 }
 244
 245 static void adv7511_csc_conversion_mode(struct v4l2_subdev *sd, uint8_t mode)
 246 {
 247         adv7511_wr_and_or(sd, 0x18, 0x9f, (mode & 0x3)<<5);
 248 }
 249
 250 static void adv7511_csc_coeff(struct v4l2_subdev *sd,
 251                               u16 A1, u16 A2, u16 A3, u16 A4,
 252                               u16 B1, u16 B2, u16 B3, u16 B4,
 253                               u16 C1, u16 C2, u16 C3, u16 C4)
 254 {
 255         /* A */
 256         adv7511_wr_and_or(sd, 0x18, 0xe0, A1>>8);
 257         adv7511_wr(sd, 0x19, A1);
 258         adv7511_wr_and_or(sd, 0x1A, 0xe0, A2>>8);
 259         adv7511_wr(sd, 0x1B, A2);
 260         adv7511_wr_and_or(sd, 0x1c, 0xe0, A3>>8);
 261         adv7511_wr(sd, 0x1d, A3);
 262         adv7511_wr_and_or(sd, 0x1e, 0xe0, A4>>8);
 263         adv7511_wr(sd, 0x1f, A4);
 264
 265         /* B */
 266         adv7511_wr_and_or(sd, 0x20, 0xe0, B1>>8);
 267         adv7511_wr(sd, 0x21, B1);
 268         adv7511_wr_and_or(sd, 0x22, 0xe0, B2>>8);
 269         adv7511_wr(sd, 0x23, B2);
 270         adv7511_wr_and_or(sd, 0x24, 0xe0, B3>>8);
 271         adv7511_wr(sd, 0x25, B3);
 272         adv7511_wr_and_or(sd, 0x26, 0xe0, B4>>8);
 273         adv7511_wr(sd, 0x27, B4);
 274
 275         /* C */
 276         adv7511_wr_and_or(sd, 0x28, 0xe0, C1>>8);
 277         adv7511_wr(sd, 0x29, C1);
 278         adv7511_wr_and_or(sd, 0x2A, 0xe0, C2>>8);
 279         adv7511_wr(sd, 0x2B, C2);
 280         adv7511_wr_and_or(sd, 0x2C, 0xe0, C3>>8);
 281         adv7511_wr(sd, 0x2D, C3);
 282         adv7511_wr_and_or(sd, 0x2E, 0xe0, C4>>8);
 283         adv7511_wr(sd, 0x2F, C4);
 284 }
 285
 286 static void adv7511_csc_rgb_full2limit(struct v4l2_subdev *sd, bool enable)
 287 {
 288         if (enable) {
 289                 uint8_t csc_mode = 0;
 290                 adv7511_csc_conversion_mode(sd, csc_mode);
 291                 adv7511_csc_coeff(sd,
 292                                   4096-564, 0, 0, 256,
 293                                   0, 4096-564, 0, 256,
 294                                   0, 0, 4096-564, 256);
 295                 /* enable CSC */
 296                 adv7511_wr_and_or(sd, 0x18, 0x7f, 0x80);
 297                 /* AVI infoframe: Limited range RGB (16-235) */
 298                 adv7511_wr_and_or(sd, 0x57, 0xf3, 0x04);
 299         } else {
 300                 /* disable CSC */
 301                 adv7511_wr_and_or(sd, 0x18, 0x7f, 0x0);
 302                 /* AVI infoframe: Full range RGB (0-255) */
 303                 adv7511_wr_and_or(sd, 0x57, 0xf3, 0x08);
 304         }
 305 }
 306
 307 static void adv7511_set_IT_content_AVI_InfoFrame(struct v4l2_subdev *sd)
 308 {
 309         struct adv7511_state *state = get_adv7511_state(sd);
 310         if (state->dv_timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
 311                 /* CEA format, not IT  */
 312                 adv7511_wr_and_or(sd, 0x57, 0x7f, 0x00);
 313         } else {
 314                 /* IT format */
 315                 adv7511_wr_and_or(sd, 0x57, 0x7f, 0x80);
 316         }
 317 }
 318
 319 static int adv7511_set_rgb_quantization_mode(struct v4l2_subdev *sd, struct v4l2_ctrl *ctrl)
 320 {
 321         switch (ctrl->val) {
 322         default:
 323                 return -EINVAL;
 324                 break;
 325         case V4L2_DV_RGB_RANGE_AUTO: {
 326                 /* automatic */
 327                 struct adv7511_state *state = get_adv7511_state(sd);
 328
 329                 if (state->dv_timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
 330                         /* cea format, RGB limited range (16-235) */
 331                         adv7511_csc_rgb_full2limit(sd, true);
 332                 } else {
 333                         /* not cea format, RGB full range (0-255) */
 334                         adv7511_csc_rgb_full2limit(sd, false);
 335                 }
 336         }
 337                 break;
 338         case V4L2_DV_RGB_RANGE_LIMITED:
 339                 /* RGB limited range (16-235) */
 340                 adv7511_csc_rgb_full2limit(sd, true);
 341                 break;
 342         case V4L2_DV_RGB_RANGE_FULL:
 343                 /* RGB full range (0-255) */
 344                 adv7511_csc_rgb_full2limit(sd, false);
 345                 break;
 346         }
 347         return 0;
 348 }
 349
 350 /* ------------------------------ CTRL OPS ------------------------------ */
 351
 352 static int adv7511_s_ctrl(struct v4l2_ctrl *ctrl)
 353 {
 354         struct v4l2_subdev *sd = to_sd(ctrl);
 355         struct adv7511_state *state = get_adv7511_state(sd);
 356
 357         v4l2_dbg(1, debug, sd, "%s: ctrl id: %d, ctrl->val %d\n", __func__, ctrl->id, ctrl->val);
 358
 359         if (state->hdmi_mode_ctrl == ctrl) {
 360                 /* Set HDMI or DVI-D */
 361                 adv7511_wr_and_or(sd, 0xaf, 0xfd, ctrl->val == V4L2_DV_TX_MODE_HDMI ? 0x02 : 0x00);
 362                 return 0;
 363         }
 364         if (state->rgb_quantization_range_ctrl == ctrl)
 365                 return adv7511_set_rgb_quantization_mode(sd, ctrl);
 366
 367         return -EINVAL;
 368 }
 369
 370 static const struct v4l2_ctrl_ops adv7511_ctrl_ops = {
 371         .s_ctrl = adv7511_s_ctrl,
 372 };
 373
 374 /* ---------------------------- CORE OPS ------------------------------------------- */
 375
 376 #ifdef CONFIG_VIDEO_ADV_DEBUG
 377 static void adv7511_inv_register(struct v4l2_subdev *sd)
 378 {
 379         v4l2_info(sd, "0x000-0x0ff: Main Map\n");
 380 }
 381
 382 static int adv7511_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 383 {
 384         reg->size = 1;
 385         switch (reg->reg >> 8) {
 386         case 0:
 387                 reg->val = adv7511_rd(sd, reg->reg & 0xff);
 388                 break;
 389         default:
 390                 v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
 391                 adv7511_inv_register(sd);
 392                 break;
 393         }
 394         return 0;
 395 }
 396
 397 static int adv7511_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 398 {
 399         switch (reg->reg >> 8) {
 400         case 0:
 401                 adv7511_wr(sd, reg->reg & 0xff, reg->val & 0xff);
 402                 break;
 403         default:
 404                 v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
 405                 adv7511_inv_register(sd);
 406                 break;
 407         }
 408         return 0;
 409 }
 410 #endif
 411
 412 static int adv7511_log_status(struct v4l2_subdev *sd)
 413 {
 414         struct adv7511_state *state = get_adv7511_state(sd);
 415         struct adv7511_state_edid *edid = &state->edid;
 416
 417         static const char * const states[] = {
 418                 "in reset",
 419                 "reading EDID",
 420                 "idle",
 421                 "initializing HDCP",
 422                 "HDCP enabled",
 423                 "initializing HDCP repeater",
 424                 "6", "7", "8", "9", "A", "B", "C", "D", "E", "F"
 425         };
 426         static const char * const errors[] = {
 427                 "no error",
 428                 "bad receiver BKSV",
 429                 "Ri mismatch",
 430                 "Pj mismatch",
 431                 "i2c error",
 432                 "timed out",
 433                 "max repeater cascade exceeded",
 434                 "hash check failed",
 435                 "too many devices",
 436                 "9", "A", "B", "C", "D", "E", "F"
 437         };
 438
 439         v4l2_info(sd, "power %s\n", state->power_on ? "on" : "off");
 440         v4l2_info(sd, "%s hotplug, %s Rx Sense, %s EDID (%d block(s))\n",
 441                   (adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT) ? "detected" : "no",
 442                   (adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT) ? "detected" : "no",
 443                   edid->segments ? "found" : "no",
 444                   edid->blocks);
 445         v4l2_info(sd, "%s output %s\n",
 446                   (adv7511_rd(sd, 0xaf) & 0x02) ?
 447                   "HDMI" : "DVI-D",
 448                   (adv7511_rd(sd, 0xa1) & 0x3c) ?
 449                   "disabled" : "enabled");
 450         v4l2_info(sd, "state: %s, error: %s, detect count: %u, msk/irq: %02x/%02x\n",
 451                           states[adv7511_rd(sd, 0xc8) & 0xf],
 452                           errors[adv7511_rd(sd, 0xc8) >> 4], state->edid_detect_counter,
 453                           adv7511_rd(sd, 0x94), adv7511_rd(sd, 0x96));
 454         v4l2_info(sd, "RGB quantization: %s range\n", adv7511_rd(sd, 0x18) & 0x80 ? "limited" : "full");
 455         if (adv7511_rd(sd, 0xaf) & 0x02) {
 456                 /* HDMI only */
 457                 u8 manual_cts = adv7511_rd(sd, 0x0a) & 0x80;
 458                 u32 N = (adv7511_rd(sd, 0x01) & 0xf) << 16 |
 459                         adv7511_rd(sd, 0x02) << 8 |
 460                         adv7511_rd(sd, 0x03);
 461                 u8 vic_detect = adv7511_rd(sd, 0x3e) >> 2;
 462                 u8 vic_sent = adv7511_rd(sd, 0x3d) & 0x3f;
 463                 u32 CTS;
 464
 465                 if (manual_cts)
 466                         CTS = (adv7511_rd(sd, 0x07) & 0xf) << 16 |
 467                               adv7511_rd(sd, 0x08) << 8 |
 468                               adv7511_rd(sd, 0x09);
 469                 else
 470                         CTS = (adv7511_rd(sd, 0x04) & 0xf) << 16 |
 471                               adv7511_rd(sd, 0x05) << 8 |
 472                               adv7511_rd(sd, 0x06);
 473                 v4l2_info(sd, "CTS %s mode: N %d, CTS %d\n",
 474                           manual_cts ? "manual" : "automatic", N, CTS);
 475                 v4l2_info(sd, "VIC: detected %d, sent %d\n",
 476                           vic_detect, vic_sent);
 477         }
 478         if (state->dv_timings.type == V4L2_DV_BT_656_1120)
 479                 v4l2_print_dv_timings(sd->name, "timings: ",
 480                                 &state->dv_timings, false);
 481         else
 482                 v4l2_info(sd, "no timings set\n");
 483         v4l2_info(sd, "i2c edid addr: 0x%x\n", state->i2c_edid_addr);
 484         v4l2_info(sd, "i2c cec addr: 0x%x\n", state->i2c_cec_addr);
 485         return 0;
 486 }
 487
 488 /* Power up/down adv7511 */
 489 static int adv7511_s_power(struct v4l2_subdev *sd, int on)
 490 {
 491         struct adv7511_state *state = get_adv7511_state(sd);
 492         const int retries = 20;
 493         int i;
 494
 495         v4l2_dbg(1, debug, sd, "%s: power %s\n", __func__, on ? "on" : "off");
 496
 497         state->power_on = on;
 498
 499         if (!on) {
 500                 /* Power down */
 501                 adv7511_wr_and_or(sd, 0x41, 0xbf, 0x40);
 502                 return true;
 503         }
 504
 505         /* Power up */
 506         /* The adv7511 does not always come up immediately.
 507            Retry multiple times. */
 508         for (i = 0; i < retries; i++) {
 509                 adv7511_wr_and_or(sd, 0x41, 0xbf, 0x0);
 510                 if ((adv7511_rd(sd, 0x41) & 0x40) == 0)
 511                         break;
 512                 adv7511_wr_and_or(sd, 0x41, 0xbf, 0x40);
 513                 msleep(10);
 514         }
 515         if (i == retries) {
 516                 v4l2_dbg(1, debug, sd, "%s: failed to powerup the adv7511!\n", __func__);
 517                 adv7511_s_power(sd, 0);
 518                 return false;
 519         }
 520         if (i > 1)
 521                 v4l2_dbg(1, debug, sd, "%s: needed %d retries to powerup the adv7511\n", __func__, i);
 522
 523         /* Reserved registers that must be set */
 524         adv7511_wr(sd, 0x98, 0x03);
 525         adv7511_wr_and_or(sd, 0x9a, 0xfe, 0x70);
 526         adv7511_wr(sd, 0x9c, 0x30);
 527         adv7511_wr_and_or(sd, 0x9d, 0xfc, 0x01);
 528         adv7511_wr(sd, 0xa2, 0xa4);
 529         adv7511_wr(sd, 0xa3, 0xa4);
 530         adv7511_wr(sd, 0xe0, 0xd0);
 531         adv7511_wr(sd, 0xf9, 0x00);
 532
 533         adv7511_wr(sd, 0x43, state->i2c_edid_addr);
 534
 535         /* Set number of attempts to read the EDID */
 536         adv7511_wr(sd, 0xc9, 0xf);
 537         return true;
 538 }
 539
 540 /* Enable interrupts */
 541 static void adv7511_set_isr(struct v4l2_subdev *sd, bool enable)
 542 {
 543         uint8_t irqs = MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT;
 544         uint8_t irqs_rd;
 545         int retries = 100;
 546
 547         v4l2_dbg(2, debug, sd, "%s: %s\n", __func__, enable ? "enable" : "disable");
 548
 549         /* The datasheet says that the EDID ready interrupt should be
 550            disabled if there is no hotplug. */
 551         if (!enable)
 552                 irqs = 0;
 553         else if (adv7511_have_hotplug(sd))
 554                 irqs |= MASK_ADV7511_EDID_RDY_INT;
 555
 556         /*
 557          * This i2c write can fail (approx. 1 in 1000 writes). But it
 558          * is essential that this register is correct, so retry it
 559          * multiple times.
 560          *
 561          * Note that the i2c write does not report an error, but the readback
 562          * clearly shows the wrong value.
 563          */
 564         do {
 565                 adv7511_wr(sd, 0x94, irqs);
 566                 irqs_rd = adv7511_rd(sd, 0x94);
 567         } while (retries-- && irqs_rd != irqs);
 568
 569         if (irqs_rd == irqs)
 570                 return;
 571         v4l2_err(sd, "Could not set interrupts: hw failure?\n");
 572 }
 573
 574 /* Interrupt handler */
 575 static int adv7511_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 576 {
 577         uint8_t irq_status;
 578
 579         /* disable interrupts to prevent a race condition */
 580         adv7511_set_isr(sd, false);
 581         irq_status = adv7511_rd(sd, 0x96);
 582         /* clear detected interrupts */
 583         adv7511_wr(sd, 0x96, irq_status);
 584
 585         v4l2_dbg(1, debug, sd, "%s: irq 0x%x\n", __func__, irq_status);
 586
 587         if (irq_status & (MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT))
 588                 adv7511_check_monitor_present_status(sd);
 589         if (irq_status & MASK_ADV7511_EDID_RDY_INT)
 590                 adv7511_check_edid_status(sd);
 591
 592         /* enable interrupts */
 593         adv7511_set_isr(sd, true);
 594
 595         if (handled)
 596                 *handled = true;
 597         return 0;
 598 }
 599
 600 static const struct v4l2_subdev_core_ops adv7511_core_ops = {
 601         .log_status = adv7511_log_status,
 602 #ifdef CONFIG_VIDEO_ADV_DEBUG
 603         .g_register = adv7511_g_register,
 604         .s_register = adv7511_s_register,
 605 #endif
 606         .s_power = adv7511_s_power,
 607         .interrupt_service_routine = adv7511_isr,
 608 };
 609
 610 /* ------------------------------ VIDEO OPS ------------------------------ */
 611
 612 /* Enable/disable adv7511 output */
 613 static int adv7511_s_stream(struct v4l2_subdev *sd, int enable)
 614 {
 615         struct adv7511_state *state = get_adv7511_state(sd);
 616
 617         v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
 618         adv7511_wr_and_or(sd, 0xa1, ~0x3c, (enable ? 0 : 0x3c));
 619         if (enable) {
 620                 adv7511_check_monitor_present_status(sd);
 621         } else {
 622                 adv7511_s_power(sd, 0);
 623                 state->have_monitor = false;
 624         }
 625         return 0;
 626 }
 627
 628 static int adv7511_s_dv_timings(struct v4l2_subdev *sd,
 629                                struct v4l2_dv_timings *timings)
 630 {
 631         struct adv7511_state *state = get_adv7511_state(sd);
 632
 633         v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 634
 635         /* quick sanity check */
 636         if (!v4l2_valid_dv_timings(timings, &adv7511_timings_cap, NULL, NULL))
 637                 return -EINVAL;
 638
 639         /* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
 640            if the format is one of the CEA or DMT timings. */
 641         v4l2_find_dv_timings_cap(timings, &adv7511_timings_cap, 0, NULL, NULL);
 642
 643         timings->bt.flags &= ~V4L2_DV_FL_REDUCED_FPS;
 644
 645         /* save timings */
 646         state->dv_timings = *timings;
 647
 648         /* update quantization range based on new dv_timings */
 649         adv7511_set_rgb_quantization_mode(sd, state->rgb_quantization_range_ctrl);
 650
 651         /* update AVI infoframe */
 652         adv7511_set_IT_content_AVI_InfoFrame(sd);
 653
 654         return 0;
 655 }
 656
 657 static int adv7511_g_dv_timings(struct v4l2_subdev *sd,
 658                                 struct v4l2_dv_timings *timings)
 659 {
 660         struct adv7511_state *state = get_adv7511_state(sd);
 661
 662         v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 663
 664         if (!timings)
 665                 return -EINVAL;
 666
 667         *timings = state->dv_timings;
 668
 669         return 0;
 670 }
 671
 672 static int adv7511_enum_dv_timings(struct v4l2_subdev *sd,
 673                                    struct v4l2_enum_dv_timings *timings)
 674 {
 675         if (timings->pad != 0)
 676                 return -EINVAL;
 677
 678         return v4l2_enum_dv_timings_cap(timings, &adv7511_timings_cap, NULL, NULL);
 679 }
 680
 681 static int adv7511_dv_timings_cap(struct v4l2_subdev *sd,
 682                                   struct v4l2_dv_timings_cap *cap)
 683 {
 684         if (cap->pad != 0)
 685                 return -EINVAL;
 686
 687         *cap = adv7511_timings_cap;
 688         return 0;
 689 }
 690
 691 static const struct v4l2_subdev_video_ops adv7511_video_ops = {
 692         .s_stream = adv7511_s_stream,
 693         .s_dv_timings = adv7511_s_dv_timings,
 694         .g_dv_timings = adv7511_g_dv_timings,
 695 };
 696
 697 /* ------------------------------ AUDIO OPS ------------------------------ */
 698 static int adv7511_s_audio_stream(struct v4l2_subdev *sd, int enable)
 699 {
 700         v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
 701
 702         if (enable)
 703                 adv7511_wr_and_or(sd, 0x4b, 0x3f, 0x80);
 704         else
 705                 adv7511_wr_and_or(sd, 0x4b, 0x3f, 0x40);
 706
 707         return 0;
 708 }
 709
 710 static int adv7511_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 711 {
 712         u32 N;
 713
 714         switch (freq) {
 715         case 32000:  N = 4096;  break;
 716         case 44100:  N = 6272;  break;
 717         case 48000:  N = 6144;  break;
 718         case 88200:  N = 12544; break;
 719         case 96000:  N = 12288; break;
 720         case 176400: N = 25088; break;
 721         case 192000: N = 24576; break;
 722         default:
 723                 return -EINVAL;
 724         }
 725
 726         /* Set N (used with CTS to regenerate the audio clock) */
 727         adv7511_wr(sd, 0x01, (N >> 16) & 0xf);
 728         adv7511_wr(sd, 0x02, (N >> 8) & 0xff);
 729         adv7511_wr(sd, 0x03, N & 0xff);
 730
 731         return 0;
 732 }
 733
 734 static int adv7511_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 735 {
 736         u32 i2s_sf;
 737
 738         switch (freq) {
 739         case 32000:  i2s_sf = 0x30; break;
 740         case 44100:  i2s_sf = 0x00; break;
 741         case 48000:  i2s_sf = 0x20; break;
 742         case 88200:  i2s_sf = 0x80; break;
 743         case 96000:  i2s_sf = 0xa0; break;
 744         case 176400: i2s_sf = 0xc0; break;
 745         case 192000: i2s_sf = 0xe0; break;
 746         default:
 747                 return -EINVAL;
 748         }
 749
 750         /* Set sampling frequency for I2S audio to 48 kHz */
 751         adv7511_wr_and_or(sd, 0x15, 0xf, i2s_sf);
 752
 753         return 0;
 754 }
 755
 756 static int adv7511_s_routing(struct v4l2_subdev *sd, u32 input, u32 output, u32 config)
 757 {
 758         /* Only 2 channels in use for application */
 759         adv7511_wr_and_or(sd, 0x73, 0xf8, 0x1);
 760         /* Speaker mapping */
 761         adv7511_wr(sd, 0x76, 0x00);
 762
 763         /* 16 bit audio word length */
 764         adv7511_wr_and_or(sd, 0x14, 0xf0, 0x02);
 765
 766         return 0;
 767 }
 768
 769 static const struct v4l2_subdev_audio_ops adv7511_audio_ops = {
 770         .s_stream = adv7511_s_audio_stream,
 771         .s_clock_freq = adv7511_s_clock_freq,
 772         .s_i2s_clock_freq = adv7511_s_i2s_clock_freq,
 773         .s_routing = adv7511_s_routing,
 774 };
 775
 776 /* ---------------------------- PAD OPS ------------------------------------- */
 777
 778 static int adv7511_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 779 {
 780         struct adv7511_state *state = get_adv7511_state(sd);
 781
 782         if (edid->pad != 0)
 783                 return -EINVAL;
 784         if ((edid->blocks == 0) || (edid->blocks > 256))
 785                 return -EINVAL;
 786         if (!state->edid.segments) {
 787                 v4l2_dbg(1, debug, sd, "EDID segment 0 not found\n");
 788                 return -ENODATA;
 789         }
 790         if (edid->start_block >= state->edid.segments * 2)
 791                 return -E2BIG;
 792         if ((edid->blocks + edid->start_block) >= state->edid.segments * 2)
 793                 edid->blocks = state->edid.segments * 2 - edid->start_block;
 794
 795         memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
 796                         128 * edid->blocks);
 797         return 0;
 798 }
 799
 800 static const struct v4l2_subdev_pad_ops adv7511_pad_ops = {
 801         .get_edid = adv7511_get_edid,
 802         .enum_dv_timings = adv7511_enum_dv_timings,
 803         .dv_timings_cap = adv7511_dv_timings_cap,
 804 };
 805
 806 /* --------------------- SUBDEV OPS --------------------------------------- */
 807
 808 static const struct v4l2_subdev_ops adv7511_ops = {
 809         .core  = &adv7511_core_ops,
 810         .pad  = &adv7511_pad_ops,
 811         .video = &adv7511_video_ops,
 812         .audio = &adv7511_audio_ops,
 813 };
 814
 815 /* ----------------------------------------------------------------------- */
 816 static void adv7511_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd, int segment, uint8_t *buf)
 817 {
 818         if (debug >= lvl) {
 819                 int i, j;
 820                 v4l2_dbg(lvl, debug, sd, "edid segment %d\n", segment);
 821                 for (i = 0; i < 256; i += 16) {
 822                         u8 b[128];
 823                         u8 *bp = b;
 824                         if (i == 128)
 825                                 v4l2_dbg(lvl, debug, sd, "\n");
 826                         for (j = i; j < i + 16; j++) {
 827                                 sprintf(bp, "0x%02x, ", buf[j]);
 828                                 bp += 6;
 829                         }
 830                         bp[0] = '\0';
 831                         v4l2_dbg(lvl, debug, sd, "%s\n", b);
 832                 }
 833         }
 834 }
 835
 836 static void adv7511_edid_handler(struct work_struct *work)
 837 {
 838         struct delayed_work *dwork = to_delayed_work(work);
 839         struct adv7511_state *state = container_of(dwork, struct adv7511_state, edid_handler);
 840         struct v4l2_subdev *sd = &state->sd;
 841         struct adv7511_edid_detect ed;
 842
 843         v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 844
 845         if (adv7511_check_edid_status(sd)) {
 846                 /* Return if we received the EDID. */
 847                 return;
 848         }
 849
 850         if (adv7511_have_hotplug(sd)) {
 851                 /* We must retry reading the EDID several times, it is possible
 852                  * that initially the EDID couldn't be read due to i2c errors
 853                  * (DVI connectors are particularly prone to this problem). */
 854                 if (state->edid.read_retries) {
 855                         state->edid.read_retries--;
 856                         v4l2_dbg(1, debug, sd, "%s: edid read failed\n", __func__);
 857                         state->have_monitor = false;
 858                         adv7511_s_power(sd, false);
 859                         adv7511_s_power(sd, true);
 860                         queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
 861                         return;
 862                 }
 863         }
 864
 865         /* We failed to read the EDID, so send an event for this. */
 866         ed.present = false;
 867         ed.segment = adv7511_rd(sd, 0xc4);
 868         v4l2_subdev_notify(sd, ADV7511_EDID_DETECT, (void *)&ed);
 869         v4l2_dbg(1, debug, sd, "%s: no edid found\n", __func__);
 870 }
 871
 872 static void adv7511_audio_setup(struct v4l2_subdev *sd)
 873 {
 874         v4l2_dbg(1, debug, sd, "%s\n", __func__);
 875
 876         adv7511_s_i2s_clock_freq(sd, 48000);
 877         adv7511_s_clock_freq(sd, 48000);
 878         adv7511_s_routing(sd, 0, 0, 0);
 879 }
 880
 881 /* Configure hdmi transmitter. */
 882 static void adv7511_setup(struct v4l2_subdev *sd)
 883 {
 884         struct adv7511_state *state = get_adv7511_state(sd);
 885         v4l2_dbg(1, debug, sd, "%s\n", __func__);
 886
 887         /* Input format: RGB 4:4:4 */
 888         adv7511_wr_and_or(sd, 0x15, 0xf0, 0x0);
 889         /* Output format: RGB 4:4:4 */
 890         adv7511_wr_and_or(sd, 0x16, 0x7f, 0x0);
 891         /* 1st order interpolation 4:2:2 -> 4:4:4 up conversion, Aspect ratio: 16:9 */
 892         adv7511_wr_and_or(sd, 0x17, 0xf9, 0x06);
 893         /* Disable pixel repetition */
 894         adv7511_wr_and_or(sd, 0x3b, 0x9f, 0x0);
 895         /* Disable CSC */
 896         adv7511_wr_and_or(sd, 0x18, 0x7f, 0x0);
 897         /* Output format: RGB 4:4:4, Active Format Information is valid,
 898          * underscanned */
 899         adv7511_wr_and_or(sd, 0x55, 0x9c, 0x12);
 900         /* AVI Info frame packet enable, Audio Info frame disable */
 901         adv7511_wr_and_or(sd, 0x44, 0xe7, 0x10);
 902         /* Colorimetry, Active format aspect ratio: same as picure. */
 903         adv7511_wr(sd, 0x56, 0xa8);
 904         /* No encryption */
 905         adv7511_wr_and_or(sd, 0xaf, 0xed, 0x0);
 906
 907         /* Positive clk edge capture for input video clock */
 908         adv7511_wr_and_or(sd, 0xba, 0x1f, 0x60);
 909
 910         adv7511_audio_setup(sd);
 911
 912         v4l2_ctrl_handler_setup(&state->hdl);
 913 }
 914
 915 static void adv7511_notify_monitor_detect(struct v4l2_subdev *sd)
 916 {
 917         struct adv7511_monitor_detect mdt;
 918         struct adv7511_state *state = get_adv7511_state(sd);
 919
 920         mdt.present = state->have_monitor;
 921         v4l2_subdev_notify(sd, ADV7511_MONITOR_DETECT, (void *)&mdt);
 922 }
 923
 924 static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd)
 925 {
 926         struct adv7511_state *state = get_adv7511_state(sd);
 927         /* read hotplug and rx-sense state */
 928         uint8_t status = adv7511_rd(sd, 0x42);
 929
 930         v4l2_dbg(1, debug, sd, "%s: status: 0x%x%s%s\n",
 931                          __func__,
 932                          status,
 933                          status & MASK_ADV7511_HPD_DETECT ? ", hotplug" : "",
 934                          status & MASK_ADV7511_MSEN_DETECT ? ", rx-sense" : "");
 935
 936         /* update read only ctrls */
 937         v4l2_ctrl_s_ctrl(state->hotplug_ctrl, adv7511_have_hotplug(sd) ? 0x1 : 0x0);
 938         v4l2_ctrl_s_ctrl(state->rx_sense_ctrl, adv7511_have_rx_sense(sd) ? 0x1 : 0x0);
 939         v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, state->edid.segments ? 0x1 : 0x0);
 940
 941         if ((status & MASK_ADV7511_HPD_DETECT) && ((status & MASK_ADV7511_MSEN_DETECT) || state->edid.segments)) {
 942                 v4l2_dbg(1, debug, sd, "%s: hotplug and (rx-sense or edid)\n", __func__);
 943                 if (!state->have_monitor) {
 944                         v4l2_dbg(1, debug, sd, "%s: monitor detected\n", __func__);
 945                         state->have_monitor = true;
 946                         adv7511_set_isr(sd, true);
 947                         if (!adv7511_s_power(sd, true)) {
 948                                 v4l2_dbg(1, debug, sd, "%s: monitor detected, powerup failed\n", __func__);
 949                                 return;
 950                         }
 951                         adv7511_setup(sd);
 952                         adv7511_notify_monitor_detect(sd);
 953                         state->edid.read_retries = EDID_MAX_RETRIES;
 954                         queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
 955                 }
 956         } else if (status & MASK_ADV7511_HPD_DETECT) {
 957                 v4l2_dbg(1, debug, sd, "%s: hotplug detected\n", __func__);
 958                 state->edid.read_retries = EDID_MAX_RETRIES;
 959                 queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
 960         } else if (!(status & MASK_ADV7511_HPD_DETECT)) {
 961                 v4l2_dbg(1, debug, sd, "%s: hotplug not detected\n", __func__);
 962                 if (state->have_monitor) {
 963                         v4l2_dbg(1, debug, sd, "%s: monitor not detected\n", __func__);
 964                         state->have_monitor = false;
 965                         adv7511_notify_monitor_detect(sd);
 966                 }
 967                 adv7511_s_power(sd, false);
 968                 memset(&state->edid, 0, sizeof(struct adv7511_state_edid));
 969         }
 970 }
 971
 972 static bool edid_block_verify_crc(uint8_t *edid_block)
 973 {
 974         uint8_t sum = 0;
 975         int i;
 976
 977         for (i = 0; i < 128; i++)
 978                 sum += edid_block[i];
 979         return sum == 0;
 980 }
 981
 982 static bool edid_verify_crc(struct v4l2_subdev *sd, u32 segment)
 983 {
 984         struct adv7511_state *state = get_adv7511_state(sd);
 985         u32 blocks = state->edid.blocks;
 986         uint8_t *data = state->edid.data;
 987
 988         if (!edid_block_verify_crc(&data[segment * 256]))
 989                 return false;
 990         if ((segment + 1) * 2 <= blocks)
 991                 return edid_block_verify_crc(&data[segment * 256 + 128]);
 992         return true;
 993 }
 994
 995 static bool edid_verify_header(struct v4l2_subdev *sd, u32 segment)
 996 {
 997         static const u8 hdmi_header[] = {
 998                 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00
 999         };
1000         struct adv7511_state *state = get_adv7511_state(sd);
1001         u8 *data = state->edid.data;
1002
1003         if (segment != 0)
1004                 return true;
1005         return !memcmp(data, hdmi_header, sizeof(hdmi_header));
1006 }
1007
1008 static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
1009 {
1010         struct adv7511_state *state = get_adv7511_state(sd);
1011         uint8_t edidRdy = adv7511_rd(sd, 0xc5);
1012
1013         v4l2_dbg(1, debug, sd, "%s: edid ready (retries: %d)\n",
1014                          __func__, EDID_MAX_RETRIES - state->edid.read_retries);
1015
1016         if (state->edid.complete)
1017                 return true;
1018
1019         if (edidRdy & MASK_ADV7511_EDID_RDY) {
1020                 int segment = adv7511_rd(sd, 0xc4);
1021                 struct adv7511_edid_detect ed;
1022
1023                 if (segment >= EDID_MAX_SEGM) {
1024                         v4l2_err(sd, "edid segment number too big\n");
1025                         return false;
1026                 }
1027                 v4l2_dbg(1, debug, sd, "%s: got segment %d\n", __func__, segment);
1028                 adv7511_edid_rd(sd, 256, &state->edid.data[segment * 256]);
1029                 adv7511_dbg_dump_edid(2, debug, sd, segment, &state->edid.data[segment * 256]);
1030                 if (segment == 0) {
1031                         state->edid.blocks = state->edid.data[0x7e] + 1;
1032                         v4l2_dbg(1, debug, sd, "%s: %d blocks in total\n", __func__, state->edid.blocks);
1033                 }
1034                 if (!edid_verify_crc(sd, segment) ||
1035                     !edid_verify_header(sd, segment)) {
1036                         /* edid crc error, force reread of edid segment */
1037                         v4l2_err(sd, "%s: edid crc or header error\n", __func__);
1038                         state->have_monitor = false;
1039                         adv7511_s_power(sd, false);
1040                         adv7511_s_power(sd, true);
1041                         return false;
1042                 }
1043                 /* one more segment read ok */
1044                 state->edid.segments = segment + 1;
1045                 if (((state->edid.data[0x7e] >> 1) + 1) > state->edid.segments) {
1046                         /* Request next EDID segment */
1047                         v4l2_dbg(1, debug, sd, "%s: request segment %d\n", __func__, state->edid.segments);
1048                         adv7511_wr(sd, 0xc9, 0xf);
1049                         adv7511_wr(sd, 0xc4, state->edid.segments);
1050                         state->edid.read_retries = EDID_MAX_RETRIES;
1051                         queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
1052                         return false;
1053                 }
1054
1055                 v4l2_dbg(1, debug, sd, "%s: edid complete with %d segment(s)\n", __func__, state->edid.segments);
1056                 state->edid.complete = true;
1057
1058                 /* report when we have all segments
1059                    but report only for segment 0
1060                  */
1061                 ed.present = true;
1062                 ed.segment = 0;
1063                 state->edid_detect_counter++;
1064                 v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, state->edid.segments ? 0x1 : 0x0);
1065                 v4l2_subdev_notify(sd, ADV7511_EDID_DETECT, (void *)&ed);
1066                 return ed.present;
1067         }
1068
1069         return false;
1070 }
1071
1072 /* ----------------------------------------------------------------------- */
1073 /* Setup ADV7511 */
1074 static void adv7511_init_setup(struct v4l2_subdev *sd)
1075 {
1076         struct adv7511_state *state = get_adv7511_state(sd);
1077         struct adv7511_state_edid *edid = &state->edid;
1078
1079         v4l2_dbg(1, debug, sd, "%s\n", __func__);
1080
1081         /* clear all interrupts */
1082         adv7511_wr(sd, 0x96, 0xff);
1083         /*
1084          * Stop HPD from resetting a lot of registers.
1085          * It might leave the chip in a partly un-initialized state,
1086          * in particular with regards to hotplug bounces.
1087          */
1088         adv7511_wr_and_or(sd, 0xd6, 0x3f, 0xc0);
1089         memset(edid, 0, sizeof(struct adv7511_state_edid));
1090         state->have_monitor = false;
1091         adv7511_set_isr(sd, false);
1092         adv7511_s_stream(sd, false);
1093         adv7511_s_audio_stream(sd, false);
1094 }
1095
1096 static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *id)
1097 {
1098         struct adv7511_state *state;
1099         struct adv7511_platform_data *pdata = client->dev.platform_data;
1100         struct v4l2_ctrl_handler *hdl;
1101         struct v4l2_subdev *sd;
1102         u8 chip_id[2];
1103         int err = -EIO;
1104
1105         /* Check if the adapter supports the needed features */
1106         if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
1107                 return -EIO;
1108
1109         state = devm_kzalloc(&client->dev, sizeof(struct adv7511_state), GFP_KERNEL);
1110         if (!state)
1111                 return -ENOMEM;
1112
1113         /* Platform data */
1114         if (!pdata) {
1115                 v4l_err(client, "No platform data!\n");
1116                 return -ENODEV;
1117         }
1118         memcpy(&state->pdata, pdata, sizeof(state->pdata));
1119
1120         sd = &state->sd;
1121
1122         v4l2_dbg(1, debug, sd, "detecting adv7511 client on address 0x%x\n",
1123                          client->addr << 1);
1124
1125         v4l2_i2c_subdev_init(sd, client, &adv7511_ops);
1126
1127         hdl = &state->hdl;
1128         v4l2_ctrl_handler_init(hdl, 10);
1129         /* add in ascending ID order */
1130         state->hdmi_mode_ctrl = v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
1131                         V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
1132                         0, V4L2_DV_TX_MODE_DVI_D);
1133         state->hotplug_ctrl = v4l2_ctrl_new_std(hdl, NULL,
1134                         V4L2_CID_DV_TX_HOTPLUG, 0, 1, 0, 0);
1135         state->rx_sense_ctrl = v4l2_ctrl_new_std(hdl, NULL,
1136                         V4L2_CID_DV_TX_RXSENSE, 0, 1, 0, 0);
1137         state->have_edid0_ctrl = v4l2_ctrl_new_std(hdl, NULL,
1138                         V4L2_CID_DV_TX_EDID_PRESENT, 0, 1, 0, 0);
1139         state->rgb_quantization_range_ctrl =
1140                 v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
1141                         V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
1142                         0, V4L2_DV_RGB_RANGE_AUTO);
1143         sd->ctrl_handler = hdl;
1144         if (hdl->error) {
1145                 err = hdl->error;
1146                 goto err_hdl;
1147         }
1148         state->hdmi_mode_ctrl->is_private = true;
1149         state->hotplug_ctrl->is_private = true;
1150         state->rx_sense_ctrl->is_private = true;
1151         state->have_edid0_ctrl->is_private = true;
1152         state->rgb_quantization_range_ctrl->is_private = true;
1153
1154         state->pad.flags = MEDIA_PAD_FL_SINK;
1155         err = media_entity_init(&sd->entity, 1, &state->pad, 0);
1156         if (err)
1157                 goto err_hdl;
1158
1159         /* EDID and CEC i2c addr */
1160         state->i2c_edid_addr = state->pdata.i2c_edid << 1;
1161         state->i2c_cec_addr = state->pdata.i2c_cec << 1;
1162
1163         state->chip_revision = adv7511_rd(sd, 0x0);
1164         chip_id[0] = adv7511_rd(sd, 0xf5);
1165         chip_id[1] = adv7511_rd(sd, 0xf6);
1166         if (chip_id[0] != 0x75 || chip_id[1] != 0x11) {
1167                 v4l2_err(sd, "chip_id != 0x7511, read 0x%02x%02x\n", chip_id[0], chip_id[1]);
1168                 err = -EIO;
1169                 goto err_entity;
1170         }
1171
1172         state->i2c_edid = i2c_new_dummy(client->adapter, state->i2c_edid_addr >> 1);
1173         if (state->i2c_edid == NULL) {
1174                 v4l2_err(sd, "failed to register edid i2c client\n");
1175                 err = -ENOMEM;
1176                 goto err_entity;
1177         }
1178
1179         adv7511_wr(sd, 0xe2, 0x01); /* power down cec section */
1180         state->work_queue = create_singlethread_workqueue(sd->name);
1181         if (state->work_queue == NULL) {
1182                 v4l2_err(sd, "could not create workqueue\n");
1183                 err = -ENOMEM;
1184                 goto err_unreg_cec;
1185         }
1186
1187         INIT_DELAYED_WORK(&state->edid_handler, adv7511_edid_handler);
1188
1189         adv7511_init_setup(sd);
1190         adv7511_set_isr(sd, true);
1191         adv7511_check_monitor_present_status(sd);
1192
1193         v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
1194                           client->addr << 1, client->adapter->name);
1195         return 0;
1196
1197 err_unreg_cec:
1198         i2c_unregister_device(state->i2c_edid);
1199 err_entity:
1200         media_entity_cleanup(&sd->entity);
1201 err_hdl:
1202         v4l2_ctrl_handler_free(&state->hdl);
1203         return err;
1204 }
1205
1206 /* ----------------------------------------------------------------------- */
1207
1208 static int adv7511_remove(struct i2c_client *client)
1209 {
1210         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1211         struct adv7511_state *state = get_adv7511_state(sd);
1212
1213         state->chip_revision = -1;
1214
1215         v4l2_dbg(1, debug, sd, "%s removed @ 0x%x (%s)\n", client->name,
1216                  client->addr << 1, client->adapter->name);
1217
1218         adv7511_init_setup(sd);
1219         cancel_delayed_work(&state->edid_handler);
1220         i2c_unregister_device(state->i2c_edid);
1221         destroy_workqueue(state->work_queue);
1222         v4l2_device_unregister_subdev(sd);
1223         media_entity_cleanup(&sd->entity);
1224         v4l2_ctrl_handler_free(sd->ctrl_handler);
1225         return 0;
1226 }
1227
1228 /* ----------------------------------------------------------------------- */
1229
1230 static struct i2c_device_id adv7511_id[] = {
1231         { "adv7511", 0 },
1232         { }
1233 };
1234 MODULE_DEVICE_TABLE(i2c, adv7511_id);
1235
1236 static struct i2c_driver adv7511_driver = {
1237         .driver = {
1238                 .owner = THIS_MODULE,
1239                 .name = "adv7511",
1240         },
1241         .probe = adv7511_probe,
1242         .remove = adv7511_remove,
1243         .id_table = adv7511_id,
1244 };
1245
1246 module_i2c_driver(adv7511_driver);