1
2
3
4
5
6
7
8
9
10
11
12
13
14#include <linux/kernel.h>
15#include <linux/module.h>
16#include <linux/slab.h>
17#include <linux/i2c.h>
18#include <linux/delay.h>
19#include <linux/videodev2.h>
20#include <linux/gpio.h>
21#include <linux/workqueue.h>
22#include <linux/hdmi.h>
23#include <linux/v4l2-dv-timings.h>
24#include <media/v4l2-device.h>
25#include <media/v4l2-common.h>
26#include <media/v4l2-ctrls.h>
27#include <media/v4l2-dv-timings.h>
28#include <media/i2c/adv7511.h>
29#include <media/cec.h>
30
31static int debug;
32module_param(debug, int, 0644);
33MODULE_PARM_DESC(debug, "debug level (0-2)");
34
35MODULE_DESCRIPTION("Analog Devices ADV7511 HDMI Transmitter Device Driver");
36MODULE_AUTHOR("Hans Verkuil");
37MODULE_LICENSE("GPL v2");
38
39#define MASK_ADV7511_EDID_RDY_INT 0x04
40#define MASK_ADV7511_MSEN_INT 0x40
41#define MASK_ADV7511_HPD_INT 0x80
42
43#define MASK_ADV7511_HPD_DETECT 0x40
44#define MASK_ADV7511_MSEN_DETECT 0x20
45#define MASK_ADV7511_EDID_RDY 0x10
46
47#define EDID_MAX_RETRIES (8)
48#define EDID_DELAY 250
49#define EDID_MAX_SEGM 8
50
51#define ADV7511_MAX_WIDTH 1920
52#define ADV7511_MAX_HEIGHT 1200
53#define ADV7511_MIN_PIXELCLOCK 20000000
54#define ADV7511_MAX_PIXELCLOCK 225000000
55
56#define ADV7511_MAX_ADDRS (3)
57
58
59
60
61
62
63
64
65
66struct i2c_reg_value {
67 unsigned char reg;
68 unsigned char value;
69};
70
71struct adv7511_state_edid {
72
73 u32 blocks;
74
75 u32 segments;
76 u8 data[EDID_MAX_SEGM * 256];
77
78 unsigned read_retries;
79 bool complete;
80};
81
82struct adv7511_state {
83 struct adv7511_platform_data pdata;
84 struct v4l2_subdev sd;
85 struct media_pad pad;
86 struct v4l2_ctrl_handler hdl;
87 int chip_revision;
88 u8 i2c_edid_addr;
89 u8 i2c_pktmem_addr;
90 u8 i2c_cec_addr;
91
92 struct i2c_client *i2c_cec;
93 struct cec_adapter *cec_adap;
94 u8 cec_addr[ADV7511_MAX_ADDRS];
95 u8 cec_valid_addrs;
96 bool cec_enabled_adap;
97
98
99 bool power_on;
100
101 bool have_monitor;
102 bool enabled_irq;
103
104 struct v4l2_dv_timings dv_timings;
105 u32 fmt_code;
106 u32 colorspace;
107 u32 ycbcr_enc;
108 u32 quantization;
109 u32 xfer_func;
110 u32 content_type;
111
112 struct v4l2_ctrl *hdmi_mode_ctrl;
113 struct v4l2_ctrl *hotplug_ctrl;
114 struct v4l2_ctrl *rx_sense_ctrl;
115 struct v4l2_ctrl *have_edid0_ctrl;
116 struct v4l2_ctrl *rgb_quantization_range_ctrl;
117 struct v4l2_ctrl *content_type_ctrl;
118 struct i2c_client *i2c_edid;
119 struct i2c_client *i2c_pktmem;
120 struct adv7511_state_edid edid;
121
122 unsigned edid_detect_counter;
123 struct workqueue_struct *work_queue;
124 struct delayed_work edid_handler;
125};
126
127static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd);
128static bool adv7511_check_edid_status(struct v4l2_subdev *sd);
129static void adv7511_setup(struct v4l2_subdev *sd);
130static int adv7511_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq);
131static int adv7511_s_clock_freq(struct v4l2_subdev *sd, u32 freq);
132
133
134static const struct v4l2_dv_timings_cap adv7511_timings_cap = {
135 .type = V4L2_DV_BT_656_1120,
136
137 .reserved = { 0 },
138 V4L2_INIT_BT_TIMINGS(640, ADV7511_MAX_WIDTH, 350, ADV7511_MAX_HEIGHT,
139 ADV7511_MIN_PIXELCLOCK, ADV7511_MAX_PIXELCLOCK,
140 V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
141 V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
142 V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
143 V4L2_DV_BT_CAP_CUSTOM)
144};
145
146static inline struct adv7511_state *get_adv7511_state(struct v4l2_subdev *sd)
147{
148 return container_of(sd, struct adv7511_state, sd);
149}
150
151static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
152{
153 return &container_of(ctrl->handler, struct adv7511_state, hdl)->sd;
154}
155
156
157
158static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
159 u8 command, bool check)
160{
161 union i2c_smbus_data data;
162
163 if (!i2c_smbus_xfer(client->adapter, client->addr, client->flags,
164 I2C_SMBUS_READ, command,
165 I2C_SMBUS_BYTE_DATA, &data))
166 return data.byte;
167 if (check)
168 v4l_err(client, "error reading %02x, %02x\n",
169 client->addr, command);
170 return -1;
171}
172
173static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
174{
175 int i;
176 for (i = 0; i < 3; i++) {
177 int ret = adv_smbus_read_byte_data_check(client, command, true);
178 if (ret >= 0) {
179 if (i)
180 v4l_err(client, "read ok after %d retries\n", i);
181 return ret;
182 }
183 }
184 v4l_err(client, "read failed\n");
185 return -1;
186}
187
188static int adv7511_rd(struct v4l2_subdev *sd, u8 reg)
189{
190 struct i2c_client *client = v4l2_get_subdevdata(sd);
191
192 return adv_smbus_read_byte_data(client, reg);
193}
194
195static int adv7511_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
196{
197 struct i2c_client *client = v4l2_get_subdevdata(sd);
198 int ret;
199 int i;
200
201 for (i = 0; i < 3; i++) {
202 ret = i2c_smbus_write_byte_data(client, reg, val);
203 if (ret == 0)
204 return 0;
205 }
206 v4l2_err(sd, "%s: i2c write error\n", __func__);
207 return ret;
208}
209
210
211
212static inline void adv7511_wr_and_or(struct v4l2_subdev *sd, u8 reg, u8 clr_mask, u8 val_mask)
213{
214 adv7511_wr(sd, reg, (adv7511_rd(sd, reg) & clr_mask) | val_mask);
215}
216
217static int adv7511_edid_rd(struct v4l2_subdev *sd, uint16_t len, uint8_t *buf)
218{
219 struct adv7511_state *state = get_adv7511_state(sd);
220 int i;
221
222 v4l2_dbg(1, debug, sd, "%s:\n", __func__);
223
224 for (i = 0; i < len; i += I2C_SMBUS_BLOCK_MAX) {
225 s32 ret;
226
227 ret = i2c_smbus_read_i2c_block_data(state->i2c_edid, i,
228 I2C_SMBUS_BLOCK_MAX, buf + i);
229 if (ret < 0) {
230 v4l2_err(sd, "%s: i2c read error\n", __func__);
231 return ret;
232 }
233 }
234
235 return 0;
236}
237
238static inline int adv7511_cec_read(struct v4l2_subdev *sd, u8 reg)
239{
240 struct adv7511_state *state = get_adv7511_state(sd);
241
242 return i2c_smbus_read_byte_data(state->i2c_cec, reg);
243}
244
245static int adv7511_cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
246{
247 struct adv7511_state *state = get_adv7511_state(sd);
248 int ret;
249 int i;
250
251 for (i = 0; i < 3; i++) {
252 ret = i2c_smbus_write_byte_data(state->i2c_cec, reg, val);
253 if (ret == 0)
254 return 0;
255 }
256 v4l2_err(sd, "%s: I2C Write Problem\n", __func__);
257 return ret;
258}
259
260static inline int adv7511_cec_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask,
261 u8 val)
262{
263 return adv7511_cec_write(sd, reg, (adv7511_cec_read(sd, reg) & mask) | val);
264}
265
266static int adv7511_pktmem_rd(struct v4l2_subdev *sd, u8 reg)
267{
268 struct adv7511_state *state = get_adv7511_state(sd);
269
270 return adv_smbus_read_byte_data(state->i2c_pktmem, reg);
271}
272
273static int adv7511_pktmem_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
274{
275 struct adv7511_state *state = get_adv7511_state(sd);
276 int ret;
277 int i;
278
279 for (i = 0; i < 3; i++) {
280 ret = i2c_smbus_write_byte_data(state->i2c_pktmem, reg, val);
281 if (ret == 0)
282 return 0;
283 }
284 v4l2_err(sd, "%s: i2c write error\n", __func__);
285 return ret;
286}
287
288
289
290static inline void adv7511_pktmem_wr_and_or(struct v4l2_subdev *sd, u8 reg, u8 clr_mask, u8 val_mask)
291{
292 adv7511_pktmem_wr(sd, reg, (adv7511_pktmem_rd(sd, reg) & clr_mask) | val_mask);
293}
294
295static inline bool adv7511_have_hotplug(struct v4l2_subdev *sd)
296{
297 return adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT;
298}
299
300static inline bool adv7511_have_rx_sense(struct v4l2_subdev *sd)
301{
302 return adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT;
303}
304
305static void adv7511_csc_conversion_mode(struct v4l2_subdev *sd, u8 mode)
306{
307 adv7511_wr_and_or(sd, 0x18, 0x9f, (mode & 0x3)<<5);
308}
309
310static void adv7511_csc_coeff(struct v4l2_subdev *sd,
311 u16 A1, u16 A2, u16 A3, u16 A4,
312 u16 B1, u16 B2, u16 B3, u16 B4,
313 u16 C1, u16 C2, u16 C3, u16 C4)
314{
315
316 adv7511_wr_and_or(sd, 0x18, 0xe0, A1>>8);
317 adv7511_wr(sd, 0x19, A1);
318 adv7511_wr_and_or(sd, 0x1A, 0xe0, A2>>8);
319 adv7511_wr(sd, 0x1B, A2);
320 adv7511_wr_and_or(sd, 0x1c, 0xe0, A3>>8);
321 adv7511_wr(sd, 0x1d, A3);
322 adv7511_wr_and_or(sd, 0x1e, 0xe0, A4>>8);
323 adv7511_wr(sd, 0x1f, A4);
324
325
326 adv7511_wr_and_or(sd, 0x20, 0xe0, B1>>8);
327 adv7511_wr(sd, 0x21, B1);
328 adv7511_wr_and_or(sd, 0x22, 0xe0, B2>>8);
329 adv7511_wr(sd, 0x23, B2);
330 adv7511_wr_and_or(sd, 0x24, 0xe0, B3>>8);
331 adv7511_wr(sd, 0x25, B3);
332 adv7511_wr_and_or(sd, 0x26, 0xe0, B4>>8);
333 adv7511_wr(sd, 0x27, B4);
334
335
336 adv7511_wr_and_or(sd, 0x28, 0xe0, C1>>8);
337 adv7511_wr(sd, 0x29, C1);
338 adv7511_wr_and_or(sd, 0x2A, 0xe0, C2>>8);
339 adv7511_wr(sd, 0x2B, C2);
340 adv7511_wr_and_or(sd, 0x2C, 0xe0, C3>>8);
341 adv7511_wr(sd, 0x2D, C3);
342 adv7511_wr_and_or(sd, 0x2E, 0xe0, C4>>8);
343 adv7511_wr(sd, 0x2F, C4);
344}
345
346static void adv7511_csc_rgb_full2limit(struct v4l2_subdev *sd, bool enable)
347{
348 if (enable) {
349 u8 csc_mode = 0;
350 adv7511_csc_conversion_mode(sd, csc_mode);
351 adv7511_csc_coeff(sd,
352 4096-564, 0, 0, 256,
353 0, 4096-564, 0, 256,
354 0, 0, 4096-564, 256);
355
356 adv7511_wr_and_or(sd, 0x18, 0x7f, 0x80);
357
358 adv7511_wr_and_or(sd, 0x57, 0xf3, 0x04);
359 } else {
360
361 adv7511_wr_and_or(sd, 0x18, 0x7f, 0x0);
362
363 adv7511_wr_and_or(sd, 0x57, 0xf3, 0x08);
364 }
365}
366
367static void adv7511_set_rgb_quantization_mode(struct v4l2_subdev *sd, struct v4l2_ctrl *ctrl)
368{
369 struct adv7511_state *state = get_adv7511_state(sd);
370
371
372 if (state->fmt_code != MEDIA_BUS_FMT_RGB888_1X24) {
373
374 adv7511_csc_rgb_full2limit(sd, false);
375 return;
376 }
377
378 switch (ctrl->val) {
379 case V4L2_DV_RGB_RANGE_AUTO:
380
381 if (state->dv_timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO) {
382
383 adv7511_csc_rgb_full2limit(sd, true);
384 } else {
385
386 adv7511_csc_rgb_full2limit(sd, false);
387 }
388 break;
389 case V4L2_DV_RGB_RANGE_LIMITED:
390
391 adv7511_csc_rgb_full2limit(sd, true);
392 break;
393 case V4L2_DV_RGB_RANGE_FULL:
394
395 adv7511_csc_rgb_full2limit(sd, false);
396 break;
397 }
398}
399
400
401
402static int adv7511_s_ctrl(struct v4l2_ctrl *ctrl)
403{
404 struct v4l2_subdev *sd = to_sd(ctrl);
405 struct adv7511_state *state = get_adv7511_state(sd);
406
407 v4l2_dbg(1, debug, sd, "%s: ctrl id: %d, ctrl->val %d\n", __func__, ctrl->id, ctrl->val);
408
409 if (state->hdmi_mode_ctrl == ctrl) {
410
411 adv7511_wr_and_or(sd, 0xaf, 0xfd, ctrl->val == V4L2_DV_TX_MODE_HDMI ? 0x02 : 0x00);
412 return 0;
413 }
414 if (state->rgb_quantization_range_ctrl == ctrl) {
415 adv7511_set_rgb_quantization_mode(sd, ctrl);
416 return 0;
417 }
418 if (state->content_type_ctrl == ctrl) {
419 u8 itc, cn;
420
421 state->content_type = ctrl->val;
422 itc = state->content_type != V4L2_DV_IT_CONTENT_TYPE_NO_ITC;
423 cn = itc ? state->content_type : V4L2_DV_IT_CONTENT_TYPE_GRAPHICS;
424 adv7511_wr_and_or(sd, 0x57, 0x7f, itc << 7);
425 adv7511_wr_and_or(sd, 0x59, 0xcf, cn << 4);
426 return 0;
427 }
428
429 return -EINVAL;
430}
431
432static const struct v4l2_ctrl_ops adv7511_ctrl_ops = {
433 .s_ctrl = adv7511_s_ctrl,
434};
435
436
437
438#ifdef CONFIG_VIDEO_ADV_DEBUG
439static void adv7511_inv_register(struct v4l2_subdev *sd)
440{
441 struct adv7511_state *state = get_adv7511_state(sd);
442
443 v4l2_info(sd, "0x000-0x0ff: Main Map\n");
444 if (state->i2c_cec)
445 v4l2_info(sd, "0x100-0x1ff: CEC Map\n");
446}
447
448static int adv7511_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
449{
450 struct adv7511_state *state = get_adv7511_state(sd);
451
452 reg->size = 1;
453 switch (reg->reg >> 8) {
454 case 0:
455 reg->val = adv7511_rd(sd, reg->reg & 0xff);
456 break;
457 case 1:
458 if (state->i2c_cec) {
459 reg->val = adv7511_cec_read(sd, reg->reg & 0xff);
460 break;
461 }
462 fallthrough;
463 default:
464 v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
465 adv7511_inv_register(sd);
466 break;
467 }
468 return 0;
469}
470
471static int adv7511_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
472{
473 struct adv7511_state *state = get_adv7511_state(sd);
474
475 switch (reg->reg >> 8) {
476 case 0:
477 adv7511_wr(sd, reg->reg & 0xff, reg->val & 0xff);
478 break;
479 case 1:
480 if (state->i2c_cec) {
481 adv7511_cec_write(sd, reg->reg & 0xff, reg->val & 0xff);
482 break;
483 }
484 fallthrough;
485 default:
486 v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
487 adv7511_inv_register(sd);
488 break;
489 }
490 return 0;
491}
492#endif
493
494struct adv7511_cfg_read_infoframe {
495 const char *desc;
496 u8 present_reg;
497 u8 present_mask;
498 u8 header[3];
499 u16 payload_addr;
500};
501
502static u8 hdmi_infoframe_checksum(u8 *ptr, size_t size)
503{
504 u8 csum = 0;
505 size_t i;
506
507
508 for (i = 0; i < size; i++)
509 csum += ptr[i];
510
511 return 256 - csum;
512}
513
514static void log_infoframe(struct v4l2_subdev *sd, const struct adv7511_cfg_read_infoframe *cri)
515{
516 struct i2c_client *client = v4l2_get_subdevdata(sd);
517 struct device *dev = &client->dev;
518 union hdmi_infoframe frame;
519 u8 buffer[32];
520 u8 len;
521 int i;
522
523 if (!(adv7511_rd(sd, cri->present_reg) & cri->present_mask)) {
524 v4l2_info(sd, "%s infoframe not transmitted\n", cri->desc);
525 return;
526 }
527
528 memcpy(buffer, cri->header, sizeof(cri->header));
529
530 len = buffer[2];
531
532 if (len + 4 > sizeof(buffer)) {
533 v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
534 return;
535 }
536
537 if (cri->payload_addr >= 0x100) {
538 for (i = 0; i < len; i++)
539 buffer[i + 4] = adv7511_pktmem_rd(sd, cri->payload_addr + i - 0x100);
540 } else {
541 for (i = 0; i < len; i++)
542 buffer[i + 4] = adv7511_rd(sd, cri->payload_addr + i);
543 }
544 buffer[3] = 0;
545 buffer[3] = hdmi_infoframe_checksum(buffer, len + 4);
546
547 if (hdmi_infoframe_unpack(&frame, buffer, sizeof(buffer)) < 0) {
548 v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
549 return;
550 }
551
552 hdmi_infoframe_log(KERN_INFO, dev, &frame);
553}
554
555static void adv7511_log_infoframes(struct v4l2_subdev *sd)
556{
557 static const struct adv7511_cfg_read_infoframe cri[] = {
558 { "AVI", 0x44, 0x10, { 0x82, 2, 13 }, 0x55 },
559 { "Audio", 0x44, 0x08, { 0x84, 1, 10 }, 0x73 },
560 { "SDP", 0x40, 0x40, { 0x83, 1, 25 }, 0x103 },
561 };
562 int i;
563
564 for (i = 0; i < ARRAY_SIZE(cri); i++)
565 log_infoframe(sd, &cri[i]);
566}
567
568static int adv7511_log_status(struct v4l2_subdev *sd)
569{
570 struct adv7511_state *state = get_adv7511_state(sd);
571 struct adv7511_state_edid *edid = &state->edid;
572 int i;
573
574 static const char * const states[] = {
575 "in reset",
576 "reading EDID",
577 "idle",
578 "initializing HDCP",
579 "HDCP enabled",
580 "initializing HDCP repeater",
581 "6", "7", "8", "9", "A", "B", "C", "D", "E", "F"
582 };
583 static const char * const errors[] = {
584 "no error",
585 "bad receiver BKSV",
586 "Ri mismatch",
587 "Pj mismatch",
588 "i2c error",
589 "timed out",
590 "max repeater cascade exceeded",
591 "hash check failed",
592 "too many devices",
593 "9", "A", "B", "C", "D", "E", "F"
594 };
595
596 v4l2_info(sd, "power %s\n", state->power_on ? "on" : "off");
597 v4l2_info(sd, "%s hotplug, %s Rx Sense, %s EDID (%d block(s))\n",
598 (adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT) ? "detected" : "no",
599 (adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT) ? "detected" : "no",
600 edid->segments ? "found" : "no",
601 edid->blocks);
602 v4l2_info(sd, "%s output %s\n",
603 (adv7511_rd(sd, 0xaf) & 0x02) ?
604 "HDMI" : "DVI-D",
605 (adv7511_rd(sd, 0xa1) & 0x3c) ?
606 "disabled" : "enabled");
607 v4l2_info(sd, "state: %s, error: %s, detect count: %u, msk/irq: %02x/%02x\n",
608 states[adv7511_rd(sd, 0xc8) & 0xf],
609 errors[adv7511_rd(sd, 0xc8) >> 4], state->edid_detect_counter,
610 adv7511_rd(sd, 0x94), adv7511_rd(sd, 0x96));
611 v4l2_info(sd, "RGB quantization: %s range\n", adv7511_rd(sd, 0x18) & 0x80 ? "limited" : "full");
612 if (adv7511_rd(sd, 0xaf) & 0x02) {
613
614 u8 manual_cts = adv7511_rd(sd, 0x0a) & 0x80;
615 u32 N = (adv7511_rd(sd, 0x01) & 0xf) << 16 |
616 adv7511_rd(sd, 0x02) << 8 |
617 adv7511_rd(sd, 0x03);
618 u8 vic_detect = adv7511_rd(sd, 0x3e) >> 2;
619 u8 vic_sent = adv7511_rd(sd, 0x3d) & 0x3f;
620 u32 CTS;
621
622 if (manual_cts)
623 CTS = (adv7511_rd(sd, 0x07) & 0xf) << 16 |
624 adv7511_rd(sd, 0x08) << 8 |
625 adv7511_rd(sd, 0x09);
626 else
627 CTS = (adv7511_rd(sd, 0x04) & 0xf) << 16 |
628 adv7511_rd(sd, 0x05) << 8 |
629 adv7511_rd(sd, 0x06);
630 v4l2_info(sd, "CTS %s mode: N %d, CTS %d\n",
631 manual_cts ? "manual" : "automatic", N, CTS);
632 v4l2_info(sd, "VIC: detected %d, sent %d\n",
633 vic_detect, vic_sent);
634 adv7511_log_infoframes(sd);
635 }
636 if (state->dv_timings.type == V4L2_DV_BT_656_1120)
637 v4l2_print_dv_timings(sd->name, "timings: ",
638 &state->dv_timings, false);
639 else
640 v4l2_info(sd, "no timings set\n");
641 v4l2_info(sd, "i2c edid addr: 0x%x\n", state->i2c_edid_addr);
642
643 if (state->i2c_cec == NULL)
644 return 0;
645
646 v4l2_info(sd, "i2c cec addr: 0x%x\n", state->i2c_cec_addr);
647
648 v4l2_info(sd, "CEC: %s\n", state->cec_enabled_adap ?
649 "enabled" : "disabled");
650 if (state->cec_enabled_adap) {
651 for (i = 0; i < ADV7511_MAX_ADDRS; i++) {
652 bool is_valid = state->cec_valid_addrs & (1 << i);
653
654 if (is_valid)
655 v4l2_info(sd, "CEC Logical Address: 0x%x\n",
656 state->cec_addr[i]);
657 }
658 }
659 v4l2_info(sd, "i2c pktmem addr: 0x%x\n", state->i2c_pktmem_addr);
660 return 0;
661}
662
663
664static int adv7511_s_power(struct v4l2_subdev *sd, int on)
665{
666 struct adv7511_state *state = get_adv7511_state(sd);
667 const int retries = 20;
668 int i;
669
670 v4l2_dbg(1, debug, sd, "%s: power %s\n", __func__, on ? "on" : "off");
671
672 state->power_on = on;
673
674 if (!on) {
675
676 adv7511_wr_and_or(sd, 0x41, 0xbf, 0x40);
677 return true;
678 }
679
680
681
682
683 for (i = 0; i < retries; i++) {
684 adv7511_wr_and_or(sd, 0x41, 0xbf, 0x0);
685 if ((adv7511_rd(sd, 0x41) & 0x40) == 0)
686 break;
687 adv7511_wr_and_or(sd, 0x41, 0xbf, 0x40);
688 msleep(10);
689 }
690 if (i == retries) {
691 v4l2_dbg(1, debug, sd, "%s: failed to powerup the adv7511!\n", __func__);
692 adv7511_s_power(sd, 0);
693 return false;
694 }
695 if (i > 1)
696 v4l2_dbg(1, debug, sd, "%s: needed %d retries to powerup the adv7511\n", __func__, i);
697
698
699 adv7511_wr(sd, 0x98, 0x03);
700 adv7511_wr_and_or(sd, 0x9a, 0xfe, 0x70);
701 adv7511_wr(sd, 0x9c, 0x30);
702 adv7511_wr_and_or(sd, 0x9d, 0xfc, 0x01);
703 adv7511_wr(sd, 0xa2, 0xa4);
704 adv7511_wr(sd, 0xa3, 0xa4);
705 adv7511_wr(sd, 0xe0, 0xd0);
706 adv7511_wr(sd, 0xf9, 0x00);
707
708 adv7511_wr(sd, 0x43, state->i2c_edid_addr);
709 adv7511_wr(sd, 0x45, state->i2c_pktmem_addr);
710
711
712 adv7511_wr(sd, 0xc9, 0xf);
713 return true;
714}
715
716#if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
717static int adv7511_cec_adap_enable(struct cec_adapter *adap, bool enable)
718{
719 struct adv7511_state *state = cec_get_drvdata(adap);
720 struct v4l2_subdev *sd = &state->sd;
721
722 if (state->i2c_cec == NULL)
723 return -EIO;
724
725 if (!state->cec_enabled_adap && enable) {
726
727 adv7511_cec_write_and_or(sd, 0x4e, 0xfc, 0x01);
728
729 adv7511_cec_write(sd, 0x4a, 0x00);
730 adv7511_cec_write(sd, 0x4a, 0x07);
731 adv7511_cec_write_and_or(sd, 0x11, 0xfe, 0);
732
733
734
735
736
737 if (state->enabled_irq)
738 adv7511_wr_and_or(sd, 0x95, 0xc0, 0x39);
739 } else if (state->cec_enabled_adap && !enable) {
740 if (state->enabled_irq)
741 adv7511_wr_and_or(sd, 0x95, 0xc0, 0x00);
742
743 adv7511_cec_write_and_or(sd, 0x4b, 0x8f, 0x00);
744
745 adv7511_cec_write_and_or(sd, 0x4e, 0xfc, 0x00);
746 state->cec_valid_addrs = 0;
747 }
748 state->cec_enabled_adap = enable;
749 return 0;
750}
751
752static int adv7511_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
753{
754 struct adv7511_state *state = cec_get_drvdata(adap);
755 struct v4l2_subdev *sd = &state->sd;
756 unsigned int i, free_idx = ADV7511_MAX_ADDRS;
757
758 if (!state->cec_enabled_adap)
759 return addr == CEC_LOG_ADDR_INVALID ? 0 : -EIO;
760
761 if (addr == CEC_LOG_ADDR_INVALID) {
762 adv7511_cec_write_and_or(sd, 0x4b, 0x8f, 0);
763 state->cec_valid_addrs = 0;
764 return 0;
765 }
766
767 for (i = 0; i < ADV7511_MAX_ADDRS; i++) {
768 bool is_valid = state->cec_valid_addrs & (1 << i);
769
770 if (free_idx == ADV7511_MAX_ADDRS && !is_valid)
771 free_idx = i;
772 if (is_valid && state->cec_addr[i] == addr)
773 return 0;
774 }
775 if (i == ADV7511_MAX_ADDRS) {
776 i = free_idx;
777 if (i == ADV7511_MAX_ADDRS)
778 return -ENXIO;
779 }
780 state->cec_addr[i] = addr;
781 state->cec_valid_addrs |= 1 << i;
782
783 switch (i) {
784 case 0:
785
786 adv7511_cec_write_and_or(sd, 0x4b, 0xef, 0x10);
787
788 adv7511_cec_write_and_or(sd, 0x4c, 0xf0, addr);
789 break;
790 case 1:
791
792 adv7511_cec_write_and_or(sd, 0x4b, 0xdf, 0x20);
793
794 adv7511_cec_write_and_or(sd, 0x4c, 0x0f, addr << 4);
795 break;
796 case 2:
797
798 adv7511_cec_write_and_or(sd, 0x4b, 0xbf, 0x40);
799
800 adv7511_cec_write_and_or(sd, 0x4d, 0xf0, addr);
801 break;
802 }
803 return 0;
804}
805
806static int adv7511_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
807 u32 signal_free_time, struct cec_msg *msg)
808{
809 struct adv7511_state *state = cec_get_drvdata(adap);
810 struct v4l2_subdev *sd = &state->sd;
811 u8 len = msg->len;
812 unsigned int i;
813
814 v4l2_dbg(1, debug, sd, "%s: len %d\n", __func__, len);
815
816 if (len > 16) {
817 v4l2_err(sd, "%s: len exceeded 16 (%d)\n", __func__, len);
818 return -EINVAL;
819 }
820
821
822
823
824
825
826 adv7511_cec_write_and_or(sd, 0x12, ~0x70, max(1, attempts - 1) << 4);
827
828
829 adv7511_wr(sd, 0x97, 0x38);
830
831
832 for (i = 0; i < len; i++)
833 adv7511_cec_write(sd, i, msg->msg[i]);
834
835
836 adv7511_cec_write(sd, 0x10, len);
837
838 adv7511_cec_write(sd, 0x11, 0x01);
839 return 0;
840}
841
842static void adv_cec_tx_raw_status(struct v4l2_subdev *sd, u8 tx_raw_status)
843{
844 struct adv7511_state *state = get_adv7511_state(sd);
845
846 if ((adv7511_cec_read(sd, 0x11) & 0x01) == 0) {
847 v4l2_dbg(1, debug, sd, "%s: tx raw: tx disabled\n", __func__);
848 return;
849 }
850
851 if (tx_raw_status & 0x10) {
852 v4l2_dbg(1, debug, sd,
853 "%s: tx raw: arbitration lost\n", __func__);
854 cec_transmit_done(state->cec_adap, CEC_TX_STATUS_ARB_LOST,
855 1, 0, 0, 0);
856 return;
857 }
858 if (tx_raw_status & 0x08) {
859 u8 status;
860 u8 nack_cnt;
861 u8 low_drive_cnt;
862
863 v4l2_dbg(1, debug, sd, "%s: tx raw: retry failed\n", __func__);
864
865
866
867
868 status = CEC_TX_STATUS_MAX_RETRIES;
869 nack_cnt = adv7511_cec_read(sd, 0x14) & 0xf;
870 if (nack_cnt)
871 status |= CEC_TX_STATUS_NACK;
872 low_drive_cnt = adv7511_cec_read(sd, 0x14) >> 4;
873 if (low_drive_cnt)
874 status |= CEC_TX_STATUS_LOW_DRIVE;
875 cec_transmit_done(state->cec_adap, status,
876 0, nack_cnt, low_drive_cnt, 0);
877 return;
878 }
879 if (tx_raw_status & 0x20) {
880 v4l2_dbg(1, debug, sd, "%s: tx raw: ready ok\n", __func__);
881 cec_transmit_done(state->cec_adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
882 return;
883 }
884}
885
886static const struct cec_adap_ops adv7511_cec_adap_ops = {
887 .adap_enable = adv7511_cec_adap_enable,
888 .adap_log_addr = adv7511_cec_adap_log_addr,
889 .adap_transmit = adv7511_cec_adap_transmit,
890};
891#endif
892
893
894static void adv7511_set_isr(struct v4l2_subdev *sd, bool enable)
895{
896 struct adv7511_state *state = get_adv7511_state(sd);
897 u8 irqs = MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT;
898 u8 irqs_rd;
899 int retries = 100;
900
901 v4l2_dbg(2, debug, sd, "%s: %s\n", __func__, enable ? "enable" : "disable");
902
903 if (state->enabled_irq == enable)
904 return;
905 state->enabled_irq = enable;
906
907
908
909 if (!enable)
910 irqs = 0;
911 else if (adv7511_have_hotplug(sd))
912 irqs |= MASK_ADV7511_EDID_RDY_INT;
913
914
915
916
917
918
919
920
921
922 do {
923 adv7511_wr(sd, 0x94, irqs);
924 irqs_rd = adv7511_rd(sd, 0x94);
925 } while (retries-- && irqs_rd != irqs);
926
927 if (irqs_rd != irqs)
928 v4l2_err(sd, "Could not set interrupts: hw failure?\n");
929
930 adv7511_wr_and_or(sd, 0x95, 0xc0,
931 (state->cec_enabled_adap && enable) ? 0x39 : 0x00);
932}
933
934
935static int adv7511_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
936{
937 u8 irq_status;
938 u8 cec_irq;
939
940
941 adv7511_set_isr(sd, false);
942 irq_status = adv7511_rd(sd, 0x96);
943 cec_irq = adv7511_rd(sd, 0x97);
944
945 adv7511_wr(sd, 0x96, irq_status);
946 adv7511_wr(sd, 0x97, cec_irq);
947
948 v4l2_dbg(1, debug, sd, "%s: irq 0x%x, cec-irq 0x%x\n", __func__,
949 irq_status, cec_irq);
950
951 if (irq_status & (MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT))
952 adv7511_check_monitor_present_status(sd);
953 if (irq_status & MASK_ADV7511_EDID_RDY_INT)
954 adv7511_check_edid_status(sd);
955
956#if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
957 if (cec_irq & 0x38)
958 adv_cec_tx_raw_status(sd, cec_irq);
959
960 if (cec_irq & 1) {
961 struct adv7511_state *state = get_adv7511_state(sd);
962 struct cec_msg msg;
963
964 msg.len = adv7511_cec_read(sd, 0x25) & 0x1f;
965
966 v4l2_dbg(1, debug, sd, "%s: cec msg len %d\n", __func__,
967 msg.len);
968
969 if (msg.len > 16)
970 msg.len = 16;
971
972 if (msg.len) {
973 u8 i;
974
975 for (i = 0; i < msg.len; i++)
976 msg.msg[i] = adv7511_cec_read(sd, i + 0x15);
977
978 adv7511_cec_write(sd, 0x4a, 0);
979 adv7511_cec_write(sd, 0x4a, 1);
980 cec_received_msg(state->cec_adap, &msg);
981 }
982 }
983#endif
984
985
986 adv7511_set_isr(sd, true);
987
988 if (handled)
989 *handled = true;
990 return 0;
991}
992
993static const struct v4l2_subdev_core_ops adv7511_core_ops = {
994 .log_status = adv7511_log_status,
995#ifdef CONFIG_VIDEO_ADV_DEBUG
996 .g_register = adv7511_g_register,
997 .s_register = adv7511_s_register,
998#endif
999 .s_power = adv7511_s_power,
1000 .interrupt_service_routine = adv7511_isr,
1001};
1002
1003
1004
1005
1006static int adv7511_s_stream(struct v4l2_subdev *sd, int enable)
1007{
1008 struct adv7511_state *state = get_adv7511_state(sd);
1009
1010 v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
1011 adv7511_wr_and_or(sd, 0xa1, ~0x3c, (enable ? 0 : 0x3c));
1012 if (enable) {
1013 adv7511_check_monitor_present_status(sd);
1014 } else {
1015 adv7511_s_power(sd, 0);
1016 state->have_monitor = false;
1017 }
1018 return 0;
1019}
1020
1021static int adv7511_s_dv_timings(struct v4l2_subdev *sd,
1022 struct v4l2_dv_timings *timings)
1023{
1024 struct adv7511_state *state = get_adv7511_state(sd);
1025 struct v4l2_bt_timings *bt = &timings->bt;
1026 u32 fps;
1027
1028 v4l2_dbg(1, debug, sd, "%s:\n", __func__);
1029
1030
1031 if (!v4l2_valid_dv_timings(timings, &adv7511_timings_cap, NULL, NULL))
1032 return -EINVAL;
1033
1034
1035
1036 v4l2_find_dv_timings_cap(timings, &adv7511_timings_cap, 0, NULL, NULL);
1037
1038
1039 state->dv_timings = *timings;
1040
1041
1042 adv7511_wr_and_or(sd, 0x17, 0x9f,
1043 ((bt->polarities & V4L2_DV_VSYNC_POS_POL) ? 0 : 0x40) |
1044 ((bt->polarities & V4L2_DV_HSYNC_POS_POL) ? 0 : 0x20));
1045
1046 fps = (u32)bt->pixelclock / (V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt));
1047 switch (fps) {
1048 case 24:
1049 adv7511_wr_and_or(sd, 0xfb, 0xf9, 1 << 1);
1050 break;
1051 case 25:
1052 adv7511_wr_and_or(sd, 0xfb, 0xf9, 2 << 1);
1053 break;
1054 case 30:
1055 adv7511_wr_and_or(sd, 0xfb, 0xf9, 3 << 1);
1056 break;
1057 default:
1058 adv7511_wr_and_or(sd, 0xfb, 0xf9, 0);
1059 break;
1060 }
1061
1062
1063 adv7511_set_rgb_quantization_mode(sd, state->rgb_quantization_range_ctrl);
1064
1065 return 0;
1066}
1067
1068static int adv7511_g_dv_timings(struct v4l2_subdev *sd,
1069 struct v4l2_dv_timings *timings)
1070{
1071 struct adv7511_state *state = get_adv7511_state(sd);
1072
1073 v4l2_dbg(1, debug, sd, "%s:\n", __func__);
1074
1075 if (!timings)
1076 return -EINVAL;
1077
1078 *timings = state->dv_timings;
1079
1080 return 0;
1081}
1082
1083static int adv7511_enum_dv_timings(struct v4l2_subdev *sd,
1084 struct v4l2_enum_dv_timings *timings)
1085{
1086 if (timings->pad != 0)
1087 return -EINVAL;
1088
1089 return v4l2_enum_dv_timings_cap(timings, &adv7511_timings_cap, NULL, NULL);
1090}
1091
1092static int adv7511_dv_timings_cap(struct v4l2_subdev *sd,
1093 struct v4l2_dv_timings_cap *cap)
1094{
1095 if (cap->pad != 0)
1096 return -EINVAL;
1097
1098 *cap = adv7511_timings_cap;
1099 return 0;
1100}
1101
1102static const struct v4l2_subdev_video_ops adv7511_video_ops = {
1103 .s_stream = adv7511_s_stream,
1104 .s_dv_timings = adv7511_s_dv_timings,
1105 .g_dv_timings = adv7511_g_dv_timings,
1106};
1107
1108
1109static int adv7511_s_audio_stream(struct v4l2_subdev *sd, int enable)
1110{
1111 v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
1112
1113 if (enable)
1114 adv7511_wr_and_or(sd, 0x4b, 0x3f, 0x80);
1115 else
1116 adv7511_wr_and_or(sd, 0x4b, 0x3f, 0x40);
1117
1118 return 0;
1119}
1120
1121static int adv7511_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
1122{
1123 u32 N;
1124
1125 switch (freq) {
1126 case 32000: N = 4096; break;
1127 case 44100: N = 6272; break;
1128 case 48000: N = 6144; break;
1129 case 88200: N = 12544; break;
1130 case 96000: N = 12288; break;
1131 case 176400: N = 25088; break;
1132 case 192000: N = 24576; break;
1133 default:
1134 return -EINVAL;
1135 }
1136
1137
1138 adv7511_wr(sd, 0x01, (N >> 16) & 0xf);
1139 adv7511_wr(sd, 0x02, (N >> 8) & 0xff);
1140 adv7511_wr(sd, 0x03, N & 0xff);
1141
1142 return 0;
1143}
1144
1145static int adv7511_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq)
1146{
1147 u32 i2s_sf;
1148
1149 switch (freq) {
1150 case 32000: i2s_sf = 0x30; break;
1151 case 44100: i2s_sf = 0x00; break;
1152 case 48000: i2s_sf = 0x20; break;
1153 case 88200: i2s_sf = 0x80; break;
1154 case 96000: i2s_sf = 0xa0; break;
1155 case 176400: i2s_sf = 0xc0; break;
1156 case 192000: i2s_sf = 0xe0; break;
1157 default:
1158 return -EINVAL;
1159 }
1160
1161
1162 adv7511_wr_and_or(sd, 0x15, 0xf, i2s_sf);
1163
1164 return 0;
1165}
1166
1167static int adv7511_s_routing(struct v4l2_subdev *sd, u32 input, u32 output, u32 config)
1168{
1169
1170 adv7511_wr_and_or(sd, 0x73, 0xf8, 0x1);
1171
1172 adv7511_wr(sd, 0x76, 0x00);
1173
1174
1175 adv7511_wr_and_or(sd, 0x14, 0xf0, 0x02);
1176
1177 return 0;
1178}
1179
1180static const struct v4l2_subdev_audio_ops adv7511_audio_ops = {
1181 .s_stream = adv7511_s_audio_stream,
1182 .s_clock_freq = adv7511_s_clock_freq,
1183 .s_i2s_clock_freq = adv7511_s_i2s_clock_freq,
1184 .s_routing = adv7511_s_routing,
1185};
1186
1187
1188
1189static int adv7511_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
1190{
1191 struct adv7511_state *state = get_adv7511_state(sd);
1192
1193 memset(edid->reserved, 0, sizeof(edid->reserved));
1194
1195 if (edid->pad != 0)
1196 return -EINVAL;
1197
1198 if (edid->start_block == 0 && edid->blocks == 0) {
1199 edid->blocks = state->edid.blocks;
1200 return 0;
1201 }
1202
1203 if (state->edid.blocks == 0)
1204 return -ENODATA;
1205
1206 if (edid->start_block >= state->edid.blocks)
1207 return -EINVAL;
1208
1209 if (edid->start_block + edid->blocks > state->edid.blocks)
1210 edid->blocks = state->edid.blocks - edid->start_block;
1211
1212 memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
1213 128 * edid->blocks);
1214
1215 return 0;
1216}
1217
1218static int adv7511_enum_mbus_code(struct v4l2_subdev *sd,
1219 struct v4l2_subdev_state *sd_state,
1220 struct v4l2_subdev_mbus_code_enum *code)
1221{
1222 if (code->pad != 0)
1223 return -EINVAL;
1224
1225 switch (code->index) {
1226 case 0:
1227 code->code = MEDIA_BUS_FMT_RGB888_1X24;
1228 break;
1229 case 1:
1230 code->code = MEDIA_BUS_FMT_YUYV8_1X16;
1231 break;
1232 case 2:
1233 code->code = MEDIA_BUS_FMT_UYVY8_1X16;
1234 break;
1235 default:
1236 return -EINVAL;
1237 }
1238 return 0;
1239}
1240
1241static void adv7511_fill_format(struct adv7511_state *state,
1242 struct v4l2_mbus_framefmt *format)
1243{
1244 format->width = state->dv_timings.bt.width;
1245 format->height = state->dv_timings.bt.height;
1246 format->field = V4L2_FIELD_NONE;
1247}
1248
1249static int adv7511_get_fmt(struct v4l2_subdev *sd,
1250 struct v4l2_subdev_state *sd_state,
1251 struct v4l2_subdev_format *format)
1252{
1253 struct adv7511_state *state = get_adv7511_state(sd);
1254
1255 if (format->pad != 0)
1256 return -EINVAL;
1257
1258 memset(&format->format, 0, sizeof(format->format));
1259 adv7511_fill_format(state, &format->format);
1260
1261 if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
1262 struct v4l2_mbus_framefmt *fmt;
1263
1264 fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
1265 format->format.code = fmt->code;
1266 format->format.colorspace = fmt->colorspace;
1267 format->format.ycbcr_enc = fmt->ycbcr_enc;
1268 format->format.quantization = fmt->quantization;
1269 format->format.xfer_func = fmt->xfer_func;
1270 } else {
1271 format->format.code = state->fmt_code;
1272 format->format.colorspace = state->colorspace;
1273 format->format.ycbcr_enc = state->ycbcr_enc;
1274 format->format.quantization = state->quantization;
1275 format->format.xfer_func = state->xfer_func;
1276 }
1277
1278 return 0;
1279}
1280
1281static int adv7511_set_fmt(struct v4l2_subdev *sd,
1282 struct v4l2_subdev_state *sd_state,
1283 struct v4l2_subdev_format *format)
1284{
1285 struct adv7511_state *state = get_adv7511_state(sd);
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296 u8 c = HDMI_COLORIMETRY_NONE;
1297 u8 ec = HDMI_EXTENDED_COLORIMETRY_XV_YCC_601;
1298 u8 y = HDMI_COLORSPACE_RGB;
1299 u8 q = HDMI_QUANTIZATION_RANGE_DEFAULT;
1300 u8 yq = HDMI_YCC_QUANTIZATION_RANGE_LIMITED;
1301 u8 itc = state->content_type != V4L2_DV_IT_CONTENT_TYPE_NO_ITC;
1302 u8 cn = itc ? state->content_type : V4L2_DV_IT_CONTENT_TYPE_GRAPHICS;
1303
1304 if (format->pad != 0)
1305 return -EINVAL;
1306 switch (format->format.code) {
1307 case MEDIA_BUS_FMT_UYVY8_1X16:
1308 case MEDIA_BUS_FMT_YUYV8_1X16:
1309 case MEDIA_BUS_FMT_RGB888_1X24:
1310 break;
1311 default:
1312 return -EINVAL;
1313 }
1314
1315 adv7511_fill_format(state, &format->format);
1316 if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
1317 struct v4l2_mbus_framefmt *fmt;
1318
1319 fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
1320 fmt->code = format->format.code;
1321 fmt->colorspace = format->format.colorspace;
1322 fmt->ycbcr_enc = format->format.ycbcr_enc;
1323 fmt->quantization = format->format.quantization;
1324 fmt->xfer_func = format->format.xfer_func;
1325 return 0;
1326 }
1327
1328 switch (format->format.code) {
1329 case MEDIA_BUS_FMT_UYVY8_1X16:
1330 adv7511_wr_and_or(sd, 0x15, 0xf0, 0x01);
1331 adv7511_wr_and_or(sd, 0x16, 0x03, 0xb8);
1332 y = HDMI_COLORSPACE_YUV422;
1333 break;
1334 case MEDIA_BUS_FMT_YUYV8_1X16:
1335 adv7511_wr_and_or(sd, 0x15, 0xf0, 0x01);
1336 adv7511_wr_and_or(sd, 0x16, 0x03, 0xbc);
1337 y = HDMI_COLORSPACE_YUV422;
1338 break;
1339 case MEDIA_BUS_FMT_RGB888_1X24:
1340 default:
1341 adv7511_wr_and_or(sd, 0x15, 0xf0, 0x00);
1342 adv7511_wr_and_or(sd, 0x16, 0x03, 0x00);
1343 break;
1344 }
1345 state->fmt_code = format->format.code;
1346 state->colorspace = format->format.colorspace;
1347 state->ycbcr_enc = format->format.ycbcr_enc;
1348 state->quantization = format->format.quantization;
1349 state->xfer_func = format->format.xfer_func;
1350
1351 switch (format->format.colorspace) {
1352 case V4L2_COLORSPACE_OPRGB:
1353 c = HDMI_COLORIMETRY_EXTENDED;
1354 ec = y ? HDMI_EXTENDED_COLORIMETRY_OPYCC_601 :
1355 HDMI_EXTENDED_COLORIMETRY_OPRGB;
1356 break;
1357 case V4L2_COLORSPACE_SMPTE170M:
1358 c = y ? HDMI_COLORIMETRY_ITU_601 : HDMI_COLORIMETRY_NONE;
1359 if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_XV601) {
1360 c = HDMI_COLORIMETRY_EXTENDED;
1361 ec = HDMI_EXTENDED_COLORIMETRY_XV_YCC_601;
1362 }
1363 break;
1364 case V4L2_COLORSPACE_REC709:
1365 c = y ? HDMI_COLORIMETRY_ITU_709 : HDMI_COLORIMETRY_NONE;
1366 if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_XV709) {
1367 c = HDMI_COLORIMETRY_EXTENDED;
1368 ec = HDMI_EXTENDED_COLORIMETRY_XV_YCC_709;
1369 }
1370 break;
1371 case V4L2_COLORSPACE_SRGB:
1372 c = y ? HDMI_COLORIMETRY_EXTENDED : HDMI_COLORIMETRY_NONE;
1373 ec = y ? HDMI_EXTENDED_COLORIMETRY_S_YCC_601 :
1374 HDMI_EXTENDED_COLORIMETRY_XV_YCC_601;
1375 break;
1376 case V4L2_COLORSPACE_BT2020:
1377 c = HDMI_COLORIMETRY_EXTENDED;
1378 if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_BT2020_CONST_LUM)
1379 ec = 5;
1380 else
1381 ec = 6;
1382 break;
1383 default:
1384 break;
1385 }
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400 switch (format->format.quantization) {
1401 case V4L2_QUANTIZATION_FULL_RANGE:
1402 q = y ? HDMI_QUANTIZATION_RANGE_DEFAULT :
1403 HDMI_QUANTIZATION_RANGE_FULL;
1404 yq = q ? q - 1 : HDMI_YCC_QUANTIZATION_RANGE_FULL;
1405 break;
1406 case V4L2_QUANTIZATION_LIM_RANGE:
1407 q = y ? HDMI_QUANTIZATION_RANGE_DEFAULT :
1408 HDMI_QUANTIZATION_RANGE_LIMITED;
1409 yq = q ? q - 1 : HDMI_YCC_QUANTIZATION_RANGE_LIMITED;
1410 break;
1411 }
1412
1413 adv7511_wr_and_or(sd, 0x4a, 0xbf, 0);
1414 adv7511_wr_and_or(sd, 0x55, 0x9f, y << 5);
1415 adv7511_wr_and_or(sd, 0x56, 0x3f, c << 6);
1416 adv7511_wr_and_or(sd, 0x57, 0x83, (ec << 4) | (q << 2) | (itc << 7));
1417 adv7511_wr_and_or(sd, 0x59, 0x0f, (yq << 6) | (cn << 4));
1418 adv7511_wr_and_or(sd, 0x4a, 0xff, 1);
1419 adv7511_set_rgb_quantization_mode(sd, state->rgb_quantization_range_ctrl);
1420
1421 return 0;
1422}
1423
1424static const struct v4l2_subdev_pad_ops adv7511_pad_ops = {
1425 .get_edid = adv7511_get_edid,
1426 .enum_mbus_code = adv7511_enum_mbus_code,
1427 .get_fmt = adv7511_get_fmt,
1428 .set_fmt = adv7511_set_fmt,
1429 .enum_dv_timings = adv7511_enum_dv_timings,
1430 .dv_timings_cap = adv7511_dv_timings_cap,
1431};
1432
1433
1434
1435static const struct v4l2_subdev_ops adv7511_ops = {
1436 .core = &adv7511_core_ops,
1437 .pad = &adv7511_pad_ops,
1438 .video = &adv7511_video_ops,
1439 .audio = &adv7511_audio_ops,
1440};
1441
1442
1443static void adv7511_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd, int segment, u8 *buf)
1444{
1445 if (debug >= lvl) {
1446 int i, j;
1447 v4l2_dbg(lvl, debug, sd, "edid segment %d\n", segment);
1448 for (i = 0; i < 256; i += 16) {
1449 u8 b[128];
1450 u8 *bp = b;
1451 if (i == 128)
1452 v4l2_dbg(lvl, debug, sd, "\n");
1453 for (j = i; j < i + 16; j++) {
1454 sprintf(bp, "0x%02x, ", buf[j]);
1455 bp += 6;
1456 }
1457 bp[0] = '\0';
1458 v4l2_dbg(lvl, debug, sd, "%s\n", b);
1459 }
1460 }
1461}
1462
1463static void adv7511_notify_no_edid(struct v4l2_subdev *sd)
1464{
1465 struct adv7511_state *state = get_adv7511_state(sd);
1466 struct adv7511_edid_detect ed;
1467
1468
1469 ed.present = false;
1470 ed.segment = adv7511_rd(sd, 0xc4);
1471 ed.phys_addr = CEC_PHYS_ADDR_INVALID;
1472 cec_s_phys_addr(state->cec_adap, ed.phys_addr, false);
1473 v4l2_subdev_notify(sd, ADV7511_EDID_DETECT, (void *)&ed);
1474 v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, 0x0);
1475}
1476
1477static void adv7511_edid_handler(struct work_struct *work)
1478{
1479 struct delayed_work *dwork = to_delayed_work(work);
1480 struct adv7511_state *state = container_of(dwork, struct adv7511_state, edid_handler);
1481 struct v4l2_subdev *sd = &state->sd;
1482
1483 v4l2_dbg(1, debug, sd, "%s:\n", __func__);
1484
1485 if (adv7511_check_edid_status(sd)) {
1486
1487 return;
1488 }
1489
1490 if (adv7511_have_hotplug(sd)) {
1491
1492
1493
1494 if (state->edid.read_retries) {
1495 state->edid.read_retries--;
1496 v4l2_dbg(1, debug, sd, "%s: edid read failed\n", __func__);
1497 state->have_monitor = false;
1498 adv7511_s_power(sd, false);
1499 adv7511_s_power(sd, true);
1500 queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
1501 return;
1502 }
1503 }
1504
1505
1506 adv7511_notify_no_edid(sd);
1507 v4l2_dbg(1, debug, sd, "%s: no edid found\n", __func__);
1508}
1509
1510static void adv7511_audio_setup(struct v4l2_subdev *sd)
1511{
1512 v4l2_dbg(1, debug, sd, "%s\n", __func__);
1513
1514 adv7511_s_i2s_clock_freq(sd, 48000);
1515 adv7511_s_clock_freq(sd, 48000);
1516 adv7511_s_routing(sd, 0, 0, 0);
1517}
1518
1519
1520static void adv7511_setup(struct v4l2_subdev *sd)
1521{
1522 struct adv7511_state *state = get_adv7511_state(sd);
1523 v4l2_dbg(1, debug, sd, "%s\n", __func__);
1524
1525
1526 adv7511_wr_and_or(sd, 0x15, 0xf0, 0x0);
1527
1528 adv7511_wr_and_or(sd, 0x16, 0x7f, 0x0);
1529
1530 adv7511_wr_and_or(sd, 0x17, 0xf9, 0x06);
1531
1532 adv7511_wr_and_or(sd, 0x3b, 0x9f, 0x0);
1533
1534 adv7511_wr_and_or(sd, 0x18, 0x7f, 0x0);
1535
1536
1537 adv7511_wr_and_or(sd, 0x55, 0x9c, 0x12);
1538
1539 adv7511_wr_and_or(sd, 0x44, 0xe7, 0x10);
1540
1541 adv7511_wr(sd, 0x56, 0xa8);
1542
1543 adv7511_wr_and_or(sd, 0xaf, 0xed, 0x0);
1544
1545
1546 adv7511_wr_and_or(sd, 0xba, 0x1f, 0x60);
1547
1548 adv7511_audio_setup(sd);
1549
1550 v4l2_ctrl_handler_setup(&state->hdl);
1551}
1552
1553static void adv7511_notify_monitor_detect(struct v4l2_subdev *sd)
1554{
1555 struct adv7511_monitor_detect mdt;
1556 struct adv7511_state *state = get_adv7511_state(sd);
1557
1558 mdt.present = state->have_monitor;
1559 v4l2_subdev_notify(sd, ADV7511_MONITOR_DETECT, (void *)&mdt);
1560}
1561
1562static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd)
1563{
1564 struct adv7511_state *state = get_adv7511_state(sd);
1565
1566 u8 status = adv7511_rd(sd, 0x42);
1567
1568 v4l2_dbg(1, debug, sd, "%s: status: 0x%x%s%s\n",
1569 __func__,
1570 status,
1571 status & MASK_ADV7511_HPD_DETECT ? ", hotplug" : "",
1572 status & MASK_ADV7511_MSEN_DETECT ? ", rx-sense" : "");
1573
1574
1575 v4l2_ctrl_s_ctrl(state->hotplug_ctrl, adv7511_have_hotplug(sd) ? 0x1 : 0x0);
1576 v4l2_ctrl_s_ctrl(state->rx_sense_ctrl, adv7511_have_rx_sense(sd) ? 0x1 : 0x0);
1577
1578 if ((status & MASK_ADV7511_HPD_DETECT) && ((status & MASK_ADV7511_MSEN_DETECT) || state->edid.segments)) {
1579 v4l2_dbg(1, debug, sd, "%s: hotplug and (rx-sense or edid)\n", __func__);
1580 if (!state->have_monitor) {
1581 v4l2_dbg(1, debug, sd, "%s: monitor detected\n", __func__);
1582 state->have_monitor = true;
1583 adv7511_set_isr(sd, true);
1584 if (!adv7511_s_power(sd, true)) {
1585 v4l2_dbg(1, debug, sd, "%s: monitor detected, powerup failed\n", __func__);
1586 return;
1587 }
1588 adv7511_setup(sd);
1589 adv7511_notify_monitor_detect(sd);
1590 state->edid.read_retries = EDID_MAX_RETRIES;
1591 queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
1592 }
1593 } else if (status & MASK_ADV7511_HPD_DETECT) {
1594 v4l2_dbg(1, debug, sd, "%s: hotplug detected\n", __func__);
1595 state->edid.read_retries = EDID_MAX_RETRIES;
1596 queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
1597 } else if (!(status & MASK_ADV7511_HPD_DETECT)) {
1598 v4l2_dbg(1, debug, sd, "%s: hotplug not detected\n", __func__);
1599 if (state->have_monitor) {
1600 v4l2_dbg(1, debug, sd, "%s: monitor not detected\n", __func__);
1601 state->have_monitor = false;
1602 adv7511_notify_monitor_detect(sd);
1603 }
1604 adv7511_s_power(sd, false);
1605 memset(&state->edid, 0, sizeof(struct adv7511_state_edid));
1606 adv7511_notify_no_edid(sd);
1607 }
1608}
1609
1610static bool edid_block_verify_crc(u8 *edid_block)
1611{
1612 u8 sum = 0;
1613 int i;
1614
1615 for (i = 0; i < 128; i++)
1616 sum += edid_block[i];
1617 return sum == 0;
1618}
1619
1620static bool edid_verify_crc(struct v4l2_subdev *sd, u32 segment)
1621{
1622 struct adv7511_state *state = get_adv7511_state(sd);
1623 u32 blocks = state->edid.blocks;
1624 u8 *data = state->edid.data;
1625
1626 if (!edid_block_verify_crc(&data[segment * 256]))
1627 return false;
1628 if ((segment + 1) * 2 <= blocks)
1629 return edid_block_verify_crc(&data[segment * 256 + 128]);
1630 return true;
1631}
1632
1633static bool edid_verify_header(struct v4l2_subdev *sd, u32 segment)
1634{
1635 static const u8 hdmi_header[] = {
1636 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00
1637 };
1638 struct adv7511_state *state = get_adv7511_state(sd);
1639 u8 *data = state->edid.data;
1640
1641 if (segment != 0)
1642 return true;
1643 return !memcmp(data, hdmi_header, sizeof(hdmi_header));
1644}
1645
1646static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
1647{
1648 struct adv7511_state *state = get_adv7511_state(sd);
1649 u8 edidRdy = adv7511_rd(sd, 0xc5);
1650
1651 v4l2_dbg(1, debug, sd, "%s: edid ready (retries: %d)\n",
1652 __func__, EDID_MAX_RETRIES - state->edid.read_retries);
1653
1654 if (state->edid.complete)
1655 return true;
1656
1657 if (edidRdy & MASK_ADV7511_EDID_RDY) {
1658 int segment = adv7511_rd(sd, 0xc4);
1659 struct adv7511_edid_detect ed;
1660 int err;
1661
1662 if (segment >= EDID_MAX_SEGM) {
1663 v4l2_err(sd, "edid segment number too big\n");
1664 return false;
1665 }
1666 v4l2_dbg(1, debug, sd, "%s: got segment %d\n", __func__, segment);
1667 err = adv7511_edid_rd(sd, 256, &state->edid.data[segment * 256]);
1668 if (!err) {
1669 adv7511_dbg_dump_edid(2, debug, sd, segment, &state->edid.data[segment * 256]);
1670 if (segment == 0) {
1671 state->edid.blocks = state->edid.data[0x7e] + 1;
1672 v4l2_dbg(1, debug, sd, "%s: %d blocks in total\n",
1673 __func__, state->edid.blocks);
1674 }
1675 }
1676
1677 if (err || !edid_verify_crc(sd, segment) || !edid_verify_header(sd, segment)) {
1678
1679 if (!err)
1680 v4l2_err(sd, "%s: edid crc or header error\n", __func__);
1681 state->have_monitor = false;
1682 adv7511_s_power(sd, false);
1683 adv7511_s_power(sd, true);
1684 return false;
1685 }
1686
1687 state->edid.segments = segment + 1;
1688 v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, 0x1);
1689 if (((state->edid.data[0x7e] >> 1) + 1) > state->edid.segments) {
1690
1691 v4l2_dbg(1, debug, sd, "%s: request segment %d\n", __func__, state->edid.segments);
1692 adv7511_wr(sd, 0xc9, 0xf);
1693 adv7511_wr(sd, 0xc4, state->edid.segments);
1694 state->edid.read_retries = EDID_MAX_RETRIES;
1695 queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
1696 return false;
1697 }
1698
1699 v4l2_dbg(1, debug, sd, "%s: edid complete with %d segment(s)\n", __func__, state->edid.segments);
1700 state->edid.complete = true;
1701 ed.phys_addr = cec_get_edid_phys_addr(state->edid.data,
1702 state->edid.segments * 256,
1703 NULL);
1704
1705
1706
1707 ed.present = true;
1708 ed.segment = 0;
1709 state->edid_detect_counter++;
1710 cec_s_phys_addr(state->cec_adap, ed.phys_addr, false);
1711 v4l2_subdev_notify(sd, ADV7511_EDID_DETECT, (void *)&ed);
1712 return ed.present;
1713 }
1714
1715 return false;
1716}
1717
1718static int adv7511_registered(struct v4l2_subdev *sd)
1719{
1720 struct adv7511_state *state = get_adv7511_state(sd);
1721 struct i2c_client *client = v4l2_get_subdevdata(sd);
1722 int err;
1723
1724 err = cec_register_adapter(state->cec_adap, &client->dev);
1725 if (err)
1726 cec_delete_adapter(state->cec_adap);
1727 return err;
1728}
1729
1730static void adv7511_unregistered(struct v4l2_subdev *sd)
1731{
1732 struct adv7511_state *state = get_adv7511_state(sd);
1733
1734 cec_unregister_adapter(state->cec_adap);
1735}
1736
1737static const struct v4l2_subdev_internal_ops adv7511_int_ops = {
1738 .registered = adv7511_registered,
1739 .unregistered = adv7511_unregistered,
1740};
1741
1742
1743
1744static void adv7511_init_setup(struct v4l2_subdev *sd)
1745{
1746 struct adv7511_state *state = get_adv7511_state(sd);
1747 struct adv7511_state_edid *edid = &state->edid;
1748 u32 cec_clk = state->pdata.cec_clk;
1749 u8 ratio;
1750
1751 v4l2_dbg(1, debug, sd, "%s\n", __func__);
1752
1753
1754 adv7511_wr(sd, 0x96, 0xff);
1755 adv7511_wr(sd, 0x97, 0xff);
1756
1757
1758
1759
1760
1761 adv7511_wr_and_or(sd, 0xd6, 0x3f, 0xc0);
1762 memset(edid, 0, sizeof(struct adv7511_state_edid));
1763 state->have_monitor = false;
1764 adv7511_set_isr(sd, false);
1765 adv7511_s_stream(sd, false);
1766 adv7511_s_audio_stream(sd, false);
1767
1768 if (state->i2c_cec == NULL)
1769 return;
1770
1771 v4l2_dbg(1, debug, sd, "%s: cec_clk %d\n", __func__, cec_clk);
1772
1773
1774 adv7511_cec_write(sd, 0x50, 0x01);
1775 adv7511_cec_write(sd, 0x50, 0x00);
1776
1777
1778 adv7511_cec_write(sd, 0x4a, 0x00);
1779 adv7511_cec_write(sd, 0x4a, 0x07);
1780
1781 if (cec_clk % 750000 != 0)
1782 v4l2_err(sd, "%s: cec_clk %d, not multiple of 750 Khz\n",
1783 __func__, cec_clk);
1784
1785 ratio = (cec_clk / 750000) - 1;
1786 adv7511_cec_write(sd, 0x4e, ratio << 2);
1787}
1788
1789static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *id)
1790{
1791 struct adv7511_state *state;
1792 struct adv7511_platform_data *pdata = client->dev.platform_data;
1793 struct v4l2_ctrl_handler *hdl;
1794 struct v4l2_subdev *sd;
1795 u8 chip_id[2];
1796 int err = -EIO;
1797
1798
1799 if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
1800 return -EIO;
1801
1802 state = devm_kzalloc(&client->dev, sizeof(struct adv7511_state), GFP_KERNEL);
1803 if (!state)
1804 return -ENOMEM;
1805
1806
1807 if (!pdata) {
1808 v4l_err(client, "No platform data!\n");
1809 return -ENODEV;
1810 }
1811 memcpy(&state->pdata, pdata, sizeof(state->pdata));
1812 state->fmt_code = MEDIA_BUS_FMT_RGB888_1X24;
1813 state->colorspace = V4L2_COLORSPACE_SRGB;
1814
1815 sd = &state->sd;
1816
1817 v4l2_dbg(1, debug, sd, "detecting adv7511 client on address 0x%x\n",
1818 client->addr << 1);
1819
1820 v4l2_i2c_subdev_init(sd, client, &adv7511_ops);
1821 sd->internal_ops = &adv7511_int_ops;
1822
1823 hdl = &state->hdl;
1824 v4l2_ctrl_handler_init(hdl, 10);
1825
1826 state->hdmi_mode_ctrl = v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
1827 V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
1828 0, V4L2_DV_TX_MODE_DVI_D);
1829 state->hotplug_ctrl = v4l2_ctrl_new_std(hdl, NULL,
1830 V4L2_CID_DV_TX_HOTPLUG, 0, 1, 0, 0);
1831 state->rx_sense_ctrl = v4l2_ctrl_new_std(hdl, NULL,
1832 V4L2_CID_DV_TX_RXSENSE, 0, 1, 0, 0);
1833 state->have_edid0_ctrl = v4l2_ctrl_new_std(hdl, NULL,
1834 V4L2_CID_DV_TX_EDID_PRESENT, 0, 1, 0, 0);
1835 state->rgb_quantization_range_ctrl =
1836 v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
1837 V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
1838 0, V4L2_DV_RGB_RANGE_AUTO);
1839 state->content_type_ctrl =
1840 v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
1841 V4L2_CID_DV_TX_IT_CONTENT_TYPE, V4L2_DV_IT_CONTENT_TYPE_NO_ITC,
1842 0, V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
1843 sd->ctrl_handler = hdl;
1844 if (hdl->error) {
1845 err = hdl->error;
1846 goto err_hdl;
1847 }
1848 state->pad.flags = MEDIA_PAD_FL_SINK;
1849 sd->entity.function = MEDIA_ENT_F_DV_ENCODER;
1850 err = media_entity_pads_init(&sd->entity, 1, &state->pad);
1851 if (err)
1852 goto err_hdl;
1853
1854
1855 state->i2c_edid_addr = state->pdata.i2c_edid << 1;
1856 state->i2c_cec_addr = state->pdata.i2c_cec << 1;
1857 state->i2c_pktmem_addr = state->pdata.i2c_pktmem << 1;
1858
1859 state->chip_revision = adv7511_rd(sd, 0x0);
1860 chip_id[0] = adv7511_rd(sd, 0xf5);
1861 chip_id[1] = adv7511_rd(sd, 0xf6);
1862 if (chip_id[0] != 0x75 || chip_id[1] != 0x11) {
1863 v4l2_err(sd, "chip_id != 0x7511, read 0x%02x%02x\n", chip_id[0],
1864 chip_id[1]);
1865 err = -EIO;
1866 goto err_entity;
1867 }
1868
1869 state->i2c_edid = i2c_new_dummy_device(client->adapter,
1870 state->i2c_edid_addr >> 1);
1871 if (IS_ERR(state->i2c_edid)) {
1872 v4l2_err(sd, "failed to register edid i2c client\n");
1873 err = PTR_ERR(state->i2c_edid);
1874 goto err_entity;
1875 }
1876
1877 adv7511_wr(sd, 0xe1, state->i2c_cec_addr);
1878 if (state->pdata.cec_clk < 3000000 ||
1879 state->pdata.cec_clk > 100000000) {
1880 v4l2_err(sd, "%s: cec_clk %u outside range, disabling cec\n",
1881 __func__, state->pdata.cec_clk);
1882 state->pdata.cec_clk = 0;
1883 }
1884
1885 if (state->pdata.cec_clk) {
1886 state->i2c_cec = i2c_new_dummy_device(client->adapter,
1887 state->i2c_cec_addr >> 1);
1888 if (IS_ERR(state->i2c_cec)) {
1889 v4l2_err(sd, "failed to register cec i2c client\n");
1890 err = PTR_ERR(state->i2c_cec);
1891 goto err_unreg_edid;
1892 }
1893 adv7511_wr(sd, 0xe2, 0x00);
1894 } else {
1895 adv7511_wr(sd, 0xe2, 0x01);
1896 }
1897
1898 state->i2c_pktmem = i2c_new_dummy_device(client->adapter, state->i2c_pktmem_addr >> 1);
1899 if (IS_ERR(state->i2c_pktmem)) {
1900 v4l2_err(sd, "failed to register pktmem i2c client\n");
1901 err = PTR_ERR(state->i2c_pktmem);
1902 goto err_unreg_cec;
1903 }
1904
1905 state->work_queue = create_singlethread_workqueue(sd->name);
1906 if (state->work_queue == NULL) {
1907 v4l2_err(sd, "could not create workqueue\n");
1908 err = -ENOMEM;
1909 goto err_unreg_pktmem;
1910 }
1911
1912 INIT_DELAYED_WORK(&state->edid_handler, adv7511_edid_handler);
1913
1914 adv7511_init_setup(sd);
1915
1916#if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
1917 state->cec_adap = cec_allocate_adapter(&adv7511_cec_adap_ops,
1918 state, dev_name(&client->dev), CEC_CAP_DEFAULTS,
1919 ADV7511_MAX_ADDRS);
1920 err = PTR_ERR_OR_ZERO(state->cec_adap);
1921 if (err) {
1922 destroy_workqueue(state->work_queue);
1923 goto err_unreg_pktmem;
1924 }
1925#endif
1926
1927 adv7511_set_isr(sd, true);
1928 adv7511_check_monitor_present_status(sd);
1929
1930 v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
1931 client->addr << 1, client->adapter->name);
1932 return 0;
1933
1934err_unreg_pktmem:
1935 i2c_unregister_device(state->i2c_pktmem);
1936err_unreg_cec:
1937 i2c_unregister_device(state->i2c_cec);
1938err_unreg_edid:
1939 i2c_unregister_device(state->i2c_edid);
1940err_entity:
1941 media_entity_cleanup(&sd->entity);
1942err_hdl:
1943 v4l2_ctrl_handler_free(&state->hdl);
1944 return err;
1945}
1946
1947
1948
1949static int adv7511_remove(struct i2c_client *client)
1950{
1951 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1952 struct adv7511_state *state = get_adv7511_state(sd);
1953
1954 state->chip_revision = -1;
1955
1956 v4l2_dbg(1, debug, sd, "%s removed @ 0x%x (%s)\n", client->name,
1957 client->addr << 1, client->adapter->name);
1958
1959 adv7511_set_isr(sd, false);
1960 adv7511_init_setup(sd);
1961 cancel_delayed_work_sync(&state->edid_handler);
1962 i2c_unregister_device(state->i2c_edid);
1963 i2c_unregister_device(state->i2c_cec);
1964 i2c_unregister_device(state->i2c_pktmem);
1965 destroy_workqueue(state->work_queue);
1966 v4l2_device_unregister_subdev(sd);
1967 media_entity_cleanup(&sd->entity);
1968 v4l2_ctrl_handler_free(sd->ctrl_handler);
1969 return 0;
1970}
1971
1972
1973
1974static const struct i2c_device_id adv7511_id[] = {
1975 { "adv7511-v4l2", 0 },
1976 { }
1977};
1978MODULE_DEVICE_TABLE(i2c, adv7511_id);
1979
1980static struct i2c_driver adv7511_driver = {
1981 .driver = {
1982 .name = "adv7511-v4l2",
1983 },
1984 .probe = adv7511_probe,
1985 .remove = adv7511_remove,
1986 .id_table = adv7511_id,
1987};
1988
1989module_i2c_driver(adv7511_driver);
1990