1
2
3
4
5
6
7
8
9
10
11#include <linux/device.h>
12#include <linux/gpio/consumer.h>
13#include <linux/module.h>
14#include <linux/of.h>
15#include <linux/platform_device.h>
16#include <linux/xilinx-v4l2-controls.h>
17
18#include <media/v4l2-async.h>
19#include <media/v4l2-ctrls.h>
20#include <media/v4l2-subdev.h>
21
22#include "xilinx-gamma-coeff.h"
23#include "xilinx-vip.h"
24
25#define XGAMMA_MIN_HEIGHT (64)
26#define XGAMMA_MAX_HEIGHT (4320)
27#define XGAMMA_DEF_HEIGHT (720)
28#define XGAMMA_MIN_WIDTH (64)
29#define XGAMMA_MAX_WIDTH (8192)
30#define XGAMMA_DEF_WIDTH (1280)
31
32#define XGAMMA_AP_CTRL (0x0000)
33#define XGAMMA_GIE (0x0004)
34#define XGAMMA_IER (0x0008)
35#define XGAMMA_ISR (0x000c)
36#define XGAMMA_WIDTH (0x0010)
37#define XGAMMA_HEIGHT (0x0018)
38#define XGAMMA_VIDEO_FORMAT (0x0020)
39#define XGAMMA_GAMMA_LUT_0_BASE (0x0800)
40#define XGAMMA_GAMMA_LUT_1_BASE (0x1000)
41#define XGAMMA_GAMMA_LUT_2_BASE (0x1800)
42
43#define XGAMMA_RESET_DEASSERT (0)
44#define XGAMMA_RESET_ASSERT (1)
45#define XGAMMA_START BIT(0)
46#define XGAMMA_AUTO_RESTART BIT(7)
47#define XGAMMA_STREAM_ON (XGAMMA_START | XGAMMA_AUTO_RESTART)
48
49enum xgamma_video_format {
50 XGAMMA_RGB = 0,
51};
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69struct xgamma_dev {
70 struct xvip_device xvip;
71 struct media_pad pads[2];
72 struct v4l2_mbus_framefmt formats[2];
73 struct v4l2_mbus_framefmt default_formats[2];
74 struct v4l2_ctrl_handler ctrl_handler;
75
76 const u16 *red_lut;
77 const u16 *green_lut;
78 const u16 *blue_lut;
79 u32 color_depth;
80 const u16 **gamma_table;
81 struct gpio_desc *rst_gpio;
82 u32 max_width;
83 u32 max_height;
84};
85
86static inline u32 xg_read(struct xgamma_dev *xg, u32 reg)
87{
88 u32 data;
89
90 data = xvip_read(&xg->xvip, reg);
91 dev_dbg(xg->xvip.dev,
92 "Reading 0x%x from reg offset 0x%x", data, reg);
93 return data;
94}
95
96static inline void xg_write(struct xgamma_dev *xg, u32 reg, u32 data)
97{
98 dev_dbg(xg->xvip.dev,
99 "Writing 0x%x to reg offset 0x%x", data, reg);
100 xvip_write(&xg->xvip, reg, data);
101#ifdef DEBUG
102 if (xg_read(xg, reg) != data)
103 dev_err(xg->xvip.dev,
104 "Write 0x%x does not match read back", data);
105#endif
106}
107
108static inline struct xgamma_dev *to_xg(struct v4l2_subdev *subdev)
109{
110 return container_of(subdev, struct xgamma_dev, xvip.subdev);
111}
112
113static struct v4l2_mbus_framefmt *
114__xg_get_pad_format(struct xgamma_dev *xg,
115 struct v4l2_subdev_pad_config *cfg,
116 unsigned int pad, u32 which)
117{
118 switch (which) {
119 case V4L2_SUBDEV_FORMAT_TRY:
120 return v4l2_subdev_get_try_format(
121 &xg->xvip.subdev, cfg, pad);
122 case V4L2_SUBDEV_FORMAT_ACTIVE:
123 return &xg->formats[pad];
124 default:
125 return NULL;
126 }
127}
128
129static void xg_set_lut_entries(struct xgamma_dev *xg,
130 const u16 *lut, const u32 lut_base)
131{
132 int itr;
133 u32 lut_offset, lut_data;
134
135 lut_offset = lut_base;
136
137 for (itr = 0; itr < BIT(xg->color_depth - 1); itr++) {
138 lut_data = (lut[2 * itr + 1] << 16) | lut[2 * itr];
139 xg_write(xg, lut_offset, lut_data);
140 lut_offset += 4;
141 }
142}
143
144static int xg_s_stream(struct v4l2_subdev *subdev, int enable)
145{
146 struct xgamma_dev *xg = to_xg(subdev);
147
148 if (!enable) {
149 dev_dbg(xg->xvip.dev, "%s : Off", __func__);
150 gpiod_set_value_cansleep(xg->rst_gpio, XGAMMA_RESET_ASSERT);
151 gpiod_set_value_cansleep(xg->rst_gpio, XGAMMA_RESET_DEASSERT);
152 return 0;
153 }
154 dev_dbg(xg->xvip.dev, "%s : Started", __func__);
155
156 dev_dbg(xg->xvip.dev, "%s : Setting width %d and height %d",
157 __func__, xg->formats[XVIP_PAD_SINK].width,
158 xg->formats[XVIP_PAD_SINK].height);
159 xg_write(xg, XGAMMA_WIDTH, xg->formats[XVIP_PAD_SINK].width);
160 xg_write(xg, XGAMMA_HEIGHT, xg->formats[XVIP_PAD_SINK].height);
161 xg_write(xg, XGAMMA_VIDEO_FORMAT, XGAMMA_RGB);
162 xg_set_lut_entries(xg, xg->red_lut, XGAMMA_GAMMA_LUT_0_BASE);
163 xg_set_lut_entries(xg, xg->green_lut, XGAMMA_GAMMA_LUT_1_BASE);
164 xg_set_lut_entries(xg, xg->blue_lut, XGAMMA_GAMMA_LUT_2_BASE);
165
166
167 xg_write(xg, XGAMMA_AP_CTRL, XGAMMA_STREAM_ON);
168 return 0;
169}
170
171static const struct v4l2_subdev_video_ops xg_video_ops = {
172 .s_stream = xg_s_stream,
173};
174
175static int xg_get_format(struct v4l2_subdev *subdev,
176 struct v4l2_subdev_pad_config *cfg,
177 struct v4l2_subdev_format *fmt)
178{
179 struct xgamma_dev *xg = to_xg(subdev);
180
181 fmt->format = *__xg_get_pad_format(xg, cfg, fmt->pad, fmt->which);
182 return 0;
183}
184
185static int xg_set_format(struct v4l2_subdev *subdev,
186 struct v4l2_subdev_pad_config *cfg,
187 struct v4l2_subdev_format *fmt)
188{
189 struct xgamma_dev *xg = to_xg(subdev);
190 struct v4l2_mbus_framefmt *__format;
191
192 __format = __xg_get_pad_format(xg, cfg, fmt->pad, fmt->which);
193 *__format = fmt->format;
194
195 if (fmt->pad == XVIP_PAD_SINK) {
196 if (__format->code != MEDIA_BUS_FMT_RBG888_1X24) {
197 dev_dbg(xg->xvip.dev,
198 "Unsupported sink media bus code format");
199 __format->code = MEDIA_BUS_FMT_RBG888_1X24;
200 }
201 }
202 __format->width = clamp_t(unsigned int, fmt->format.width,
203 XGAMMA_MIN_WIDTH, xg->max_width);
204 __format->height = clamp_t(unsigned int, fmt->format.height,
205 XGAMMA_MIN_HEIGHT, xg->max_height);
206
207 fmt->format = *__format;
208
209 __format = __xg_get_pad_format(xg, cfg, XVIP_PAD_SOURCE, fmt->which);
210 *__format = fmt->format;
211 return 0;
212}
213
214static int xg_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
215{
216 struct xgamma_dev *xg = to_xg(subdev);
217 struct v4l2_mbus_framefmt *format;
218
219 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SINK);
220 *format = xg->default_formats[XVIP_PAD_SINK];
221
222 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SOURCE);
223 *format = xg->default_formats[XVIP_PAD_SOURCE];
224 return 0;
225}
226
227static int xg_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
228{
229 return 0;
230}
231
232static const struct v4l2_subdev_internal_ops xg_internal_ops = {
233 .open = xg_open,
234 .close = xg_close,
235};
236
237static const struct v4l2_subdev_pad_ops xg_pad_ops = {
238 .enum_mbus_code = xvip_enum_mbus_code,
239 .enum_frame_size = xvip_enum_frame_size,
240 .get_fmt = xg_get_format,
241 .set_fmt = xg_set_format,
242};
243
244static const struct v4l2_subdev_ops xg_ops = {
245 .video = &xg_video_ops,
246 .pad = &xg_pad_ops,
247};
248
249static int
250select_gamma(s32 value, const u16 **coeff, const u16 **xgamma_curves)
251{
252 if (!coeff)
253 return -EINVAL;
254 if (value <= 0 || value > GAMMA_CURVE_LENGTH)
255 return -EINVAL;
256
257 *coeff = *(xgamma_curves + value - 1);
258 return 0;
259}
260
261static int xg_s_ctrl(struct v4l2_ctrl *ctrl)
262{
263 int rval;
264 struct xgamma_dev *xg =
265 container_of(ctrl->handler,
266 struct xgamma_dev, ctrl_handler);
267 dev_dbg(xg->xvip.dev, "%s called", __func__);
268 switch (ctrl->id) {
269 case V4L2_CID_XILINX_GAMMA_CORR_RED_GAMMA:
270 rval = select_gamma(ctrl->val, &xg->red_lut, xg->gamma_table);
271 if (rval < 0) {
272 dev_err(xg->xvip.dev, "Invalid Red Gamma");
273 return rval;
274 }
275 dev_dbg(xg->xvip.dev, "%s: Setting Red Gamma to %d.%d",
276 __func__, ctrl->val / 10, ctrl->val % 10);
277 xg_set_lut_entries(xg, xg->red_lut, XGAMMA_GAMMA_LUT_0_BASE);
278 break;
279 case V4L2_CID_XILINX_GAMMA_CORR_BLUE_GAMMA:
280 rval = select_gamma(ctrl->val, &xg->blue_lut, xg->gamma_table);
281 if (rval < 0) {
282 dev_err(xg->xvip.dev, "Invalid Blue Gamma");
283 return rval;
284 }
285 dev_dbg(xg->xvip.dev, "%s: Setting Blue Gamma to %d.%d",
286 __func__, ctrl->val / 10, ctrl->val % 10);
287 xg_set_lut_entries(xg, xg->blue_lut, XGAMMA_GAMMA_LUT_1_BASE);
288 break;
289 case V4L2_CID_XILINX_GAMMA_CORR_GREEN_GAMMA:
290 rval = select_gamma(ctrl->val, &xg->green_lut, xg->gamma_table);
291 if (rval < 0) {
292 dev_err(xg->xvip.dev, "Invalid Green Gamma");
293 return -EINVAL;
294 }
295 dev_dbg(xg->xvip.dev, "%s: Setting Green Gamma to %d.%d",
296 __func__, ctrl->val / 10, ctrl->val % 10);
297 xg_set_lut_entries(xg, xg->green_lut, XGAMMA_GAMMA_LUT_2_BASE);
298 break;
299 }
300 return 0;
301}
302
303static const struct v4l2_ctrl_ops xg_ctrl_ops = {
304 .s_ctrl = xg_s_ctrl,
305};
306
307static struct v4l2_ctrl_config xg_ctrls[] = {
308
309 {
310 .ops = &xg_ctrl_ops,
311 .id = V4L2_CID_XILINX_GAMMA_CORR_RED_GAMMA,
312 .name = "Red Gamma Correction|1->0.1|10->1.0",
313 .type = V4L2_CTRL_TYPE_INTEGER,
314 .min = 1,
315 .max = 40,
316 .step = 1,
317 .def = 10,
318 .flags = V4L2_CTRL_FLAG_SLIDER,
319 },
320
321 {
322 .ops = &xg_ctrl_ops,
323 .id = V4L2_CID_XILINX_GAMMA_CORR_BLUE_GAMMA,
324 .name = "Blue Gamma Correction|1->0.1|10->1.0",
325 .type = V4L2_CTRL_TYPE_INTEGER,
326 .min = 1,
327 .max = 40,
328 .step = 1,
329 .def = 10,
330 .flags = V4L2_CTRL_FLAG_SLIDER,
331 },
332
333 {
334 .ops = &xg_ctrl_ops,
335 .id = V4L2_CID_XILINX_GAMMA_CORR_GREEN_GAMMA,
336 .name = "Green Gamma Correction|1->0.1|10->1.0)",
337 .type = V4L2_CTRL_TYPE_INTEGER,
338 .min = 1,
339 .max = 40,
340 .step = 1,
341 .def = 10,
342 .flags = V4L2_CTRL_FLAG_SLIDER,
343 },
344};
345
346static const struct media_entity_operations xg_media_ops = {
347 .link_validate = v4l2_subdev_link_validate,
348};
349
350static int xg_parse_of(struct xgamma_dev *xg)
351{
352 struct device *dev = xg->xvip.dev;
353 struct device_node *node = dev->of_node;
354 struct device_node *ports;
355 struct device_node *port;
356 u32 port_id = 0;
357 int rval;
358
359 rval = of_property_read_u32(node, "xlnx,max-height", &xg->max_height);
360 if (rval < 0) {
361 dev_err(dev, "xlnx,max-height is missing!");
362 return -EINVAL;
363 } else if (xg->max_height > XGAMMA_MAX_HEIGHT ||
364 xg->max_height < XGAMMA_MIN_HEIGHT) {
365 dev_err(dev, "Invalid height in dt");
366 return -EINVAL;
367 }
368
369 rval = of_property_read_u32(node, "xlnx,max-width", &xg->max_width);
370 if (rval < 0) {
371 dev_err(dev, "xlnx,max-width is missing!");
372 return -EINVAL;
373 } else if (xg->max_width > XGAMMA_MAX_WIDTH ||
374 xg->max_width < XGAMMA_MIN_WIDTH) {
375 dev_err(dev, "Invalid width in dt");
376 return -EINVAL;
377 }
378
379 ports = of_get_child_by_name(node, "ports");
380 if (!ports)
381 ports = node;
382
383
384 for_each_child_of_node(ports, port) {
385 if (port->name && (of_node_cmp(port->name, "port") == 0)) {
386 rval = of_property_read_u32(port, "reg", &port_id);
387 if (rval < 0) {
388 dev_err(dev, "No reg in DT");
389 return rval;
390 }
391 if (port_id != 0 && port_id != 1) {
392 dev_err(dev, "Invalid reg in DT");
393 return -EINVAL;
394 }
395
396 rval = of_property_read_u32(port, "xlnx,video-width",
397 &xg->color_depth);
398 if (rval < 0) {
399 dev_err(dev, "Missing xlnx-video-width in DT");
400 return rval;
401 }
402 switch (xg->color_depth) {
403 case GAMMA_BPC_8:
404 xg->gamma_table = xgamma8_curves;
405 break;
406 case GAMMA_BPC_10:
407 xg->gamma_table = xgamma10_curves;
408 break;
409 default:
410 dev_err(dev, "Unsupported color depth %d",
411 xg->color_depth);
412 return -EINVAL;
413 }
414 }
415 }
416
417 xg->rst_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
418 if (IS_ERR(xg->rst_gpio)) {
419 if (PTR_ERR(xg->rst_gpio) != -EPROBE_DEFER)
420 dev_err(dev, "Reset GPIO not setup in DT");
421 return PTR_ERR(xg->rst_gpio);
422 }
423 return 0;
424}
425
426static int xg_probe(struct platform_device *pdev)
427{
428 struct xgamma_dev *xg;
429 struct v4l2_subdev *subdev;
430 struct v4l2_mbus_framefmt *def_fmt;
431 int rval, itr;
432
433 dev_dbg(&pdev->dev, "Gamma LUT Probe Started");
434 xg = devm_kzalloc(&pdev->dev, sizeof(*xg), GFP_KERNEL);
435 if (!xg)
436 return -ENOMEM;
437 xg->xvip.dev = &pdev->dev;
438 rval = xg_parse_of(xg);
439 if (rval < 0)
440 return rval;
441 rval = xvip_init_resources(&xg->xvip);
442
443 dev_dbg(xg->xvip.dev, "Reset Xilinx Video Gamma Corrrection");
444 gpiod_set_value_cansleep(xg->rst_gpio, XGAMMA_RESET_DEASSERT);
445
446
447 subdev = &xg->xvip.subdev;
448 v4l2_subdev_init(subdev, &xg_ops);
449 subdev->dev = &pdev->dev;
450 subdev->internal_ops = &xg_internal_ops;
451 strlcpy(subdev->name, dev_name(&pdev->dev), sizeof(subdev->name));
452 subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
453
454
455 def_fmt = &xg->default_formats[XVIP_PAD_SINK];
456
457 def_fmt->code = MEDIA_BUS_FMT_RBG888_1X24;
458 def_fmt->field = V4L2_FIELD_NONE;
459 def_fmt->colorspace = V4L2_COLORSPACE_SRGB;
460 def_fmt->width = XGAMMA_DEF_WIDTH;
461 def_fmt->height = XGAMMA_DEF_HEIGHT;
462 xg->formats[XVIP_PAD_SINK] = *def_fmt;
463
464 def_fmt = &xg->default_formats[XVIP_PAD_SOURCE];
465 *def_fmt = xg->default_formats[XVIP_PAD_SINK];
466 xg->formats[XVIP_PAD_SOURCE] = *def_fmt;
467
468 xg->pads[XVIP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
469 xg->pads[XVIP_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
470
471
472 subdev->entity.ops = &xg_media_ops;
473 rval = media_entity_pads_init(&subdev->entity, 2, xg->pads);
474 if (rval < 0)
475 goto media_error;
476
477
478 v4l2_ctrl_handler_init(&xg->ctrl_handler, ARRAY_SIZE(xg_ctrls));
479 for (itr = 0; itr < ARRAY_SIZE(xg_ctrls); itr++) {
480 v4l2_ctrl_new_custom(&xg->ctrl_handler,
481 &xg_ctrls[itr], NULL);
482 }
483 if (xg->ctrl_handler.error) {
484 dev_err(&pdev->dev, "Failed to add V4L2 controls");
485 rval = xg->ctrl_handler.error;
486 goto ctrl_error;
487 }
488 subdev->ctrl_handler = &xg->ctrl_handler;
489 rval = v4l2_ctrl_handler_setup(&xg->ctrl_handler);
490 if (rval < 0) {
491 dev_err(&pdev->dev, "Failed to setup control handler");
492 goto ctrl_error;
493 }
494
495 platform_set_drvdata(pdev, xg);
496 rval = v4l2_async_register_subdev(subdev);
497 if (rval < 0) {
498 dev_err(&pdev->dev, "failed to register subdev");
499 goto v4l2_subdev_error;
500 }
501 dev_info(&pdev->dev,
502 "Xilinx %d-bit Video Gamma Correction LUT registered",
503 xg->color_depth);
504 return 0;
505ctrl_error:
506 v4l2_ctrl_handler_free(&xg->ctrl_handler);
507v4l2_subdev_error:
508 media_entity_cleanup(&subdev->entity);
509media_error:
510 xvip_cleanup_resources(&xg->xvip);
511 return rval;
512}
513
514static int xg_remove(struct platform_device *pdev)
515{
516 struct xgamma_dev *xg = platform_get_drvdata(pdev);
517 struct v4l2_subdev *subdev = &xg->xvip.subdev;
518
519 v4l2_async_unregister_subdev(subdev);
520
521 media_entity_cleanup(&subdev->entity);
522 xvip_cleanup_resources(&xg->xvip);
523 return 0;
524}
525
526static const struct of_device_id xg_of_id_table[] = {
527 {.compatible = "xlnx,v-gamma-lut"},
528 { }
529};
530MODULE_DEVICE_TABLE(of, xg_of_id_table);
531
532static struct platform_driver xg_driver = {
533 .driver = {
534 .name = "xilinx-gamma-lut",
535 .of_match_table = xg_of_id_table,
536 },
537 .probe = xg_probe,
538 .remove = xg_remove,
539};
540
541module_platform_driver(xg_driver);
542MODULE_DESCRIPTION("Xilinx Video Gamma Correction LUT Driver");
543MODULE_LICENSE("GPL v2");
544
