1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20#include <linux/device.h>
21#include <linux/fixp-arith.h>
22#include <linux/module.h>
23#include <linux/of.h>
24#include <linux/platform_device.h>
25#include <linux/slab.h>
26
27#include <media/v4l2-async.h>
28#include <media/v4l2-subdev.h>
29
30#include "xilinx-vip.h"
31
32#define XSCALER_MIN_WIDTH 32
33#define XSCALER_MAX_WIDTH 4096
34#define XSCALER_MIN_HEIGHT 32
35#define XSCALER_MAX_HEIGHT 4096
36
37#define XSCALER_HSF 0x0100
38#define XSCALER_VSF 0x0104
39#define XSCALER_SF_SHIFT 20
40#define XSCALER_SF_MASK 0xffffff
41#define XSCALER_SOURCE_SIZE 0x0108
42#define XSCALER_SIZE_HORZ_SHIFT 0
43#define XSCALER_SIZE_VERT_SHIFT 16
44#define XSCALER_SIZE_MASK 0xfff
45#define XSCALER_HAPERTURE 0x010c
46#define XSCALER_VAPERTURE 0x0110
47#define XSCALER_APERTURE_START_SHIFT 0
48#define XSCALER_APERTURE_END_SHIFT 16
49#define XSCALER_OUTPUT_SIZE 0x0114
50#define XSCALER_COEF_DATA_IN 0x0134
51#define XSCALER_COEF_DATA_IN_SHIFT 16
52
53
54#define FRAC_N 8
55
56static inline s16 fixp_new(s16 a)
57{
58 return a << FRAC_N;
59}
60
61static inline s16 fixp_mult(s16 a, s16 b)
62{
63 return ((s32)(a * b)) >> FRAC_N;
64}
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80struct xscaler_device {
81 struct xvip_device xvip;
82
83 struct media_pad pads[2];
84
85 struct v4l2_mbus_framefmt formats[2];
86 struct v4l2_mbus_framefmt default_formats[2];
87 const struct xvip_video_format *vip_format;
88 struct v4l2_rect crop;
89
90 u32 num_hori_taps;
91 u32 num_vert_taps;
92 u32 max_num_phases;
93 bool separate_yc_coef;
94 bool separate_hv_coef;
95};
96
97static inline struct xscaler_device *to_scaler(struct v4l2_subdev *subdev)
98{
99 return container_of(subdev, struct xscaler_device, xvip.subdev);
100}
101
102
103
104
105
106
107
108
109
110
111
112
113static s16 lanczos(s16 x, s16 a)
114{
115 s16 pi;
116 s16 numerator;
117 s16 denominator;
118 s16 temp;
119
120 if (x < -a || x > a)
121 return 0;
122 else if (x == 0)
123 return fixp_new(1);
124
125
126
127 pi = (fixp_new(157) << FRAC_N) / fixp_new(50);
128
129 if (x < 0)
130 x = -x;
131
132
133 temp = fixp_mult(fixp_new(180), x);
134 temp = fixp_sin16(temp >> FRAC_N);
135
136
137 numerator = fixp_mult(temp , a);
138
139
140 temp = (fixp_mult(fixp_new(180), x) << FRAC_N) / a;
141 temp = fixp_sin16(temp >> FRAC_N);
142
143
144 numerator = fixp_mult(temp, numerator);
145
146
147 denominator = fixp_mult(pi, pi);
148 temp = fixp_mult(x, x);
149 denominator = fixp_mult(temp, denominator);
150
151 return (numerator << FRAC_N) / denominator;
152}
153
154
155
156
157
158
159
160
161
162
163
164
165
166static int xscaler_set_coefs(struct xscaler_device *xscaler, s16 taps)
167{
168 s16 *coef;
169 s16 dy;
170 u32 coef_val;
171 u16 phases = xscaler->max_num_phases;
172 u16 i;
173 u16 j;
174
175 coef = kcalloc(phases, sizeof(*coef), GFP_KERNEL);
176 if (!coef)
177 return -ENOMEM;
178
179 for (i = 0; i < phases; i++) {
180 s16 sum = 0;
181
182 dy = ((fixp_new(i) << FRAC_N) / fixp_new(phases));
183
184
185 for (j = 0; j < taps; j++) {
186 coef[j] = lanczos(fixp_new(j - (taps >> 1)) + dy,
187 fixp_new(taps >> 1));
188 sum += coef[j];
189 }
190
191
192 for (j = 0; j < taps; j += 2) {
193
194 coef_val = (((coef[j] << FRAC_N) << (FRAC_N - 2)) /
195 sum) & 0xffff;
196 if (j + 1 < taps)
197 coef_val |= ((((coef[j + 1] << FRAC_N) <<
198 (FRAC_N - 2)) / sum) & 0xffff) <<
199 16;
200
201 xvip_write(&xscaler->xvip, XSCALER_COEF_DATA_IN,
202 coef_val);
203 }
204 }
205
206 kfree(coef);
207
208 return 0;
209}
210
211static void xscaler_set_aperture(struct xscaler_device *xscaler)
212{
213 u16 start;
214 u16 end;
215 u32 scale_factor;
216
217 xvip_disable_reg_update(&xscaler->xvip);
218
219
220 start = xscaler->crop.left;
221 end = start + xscaler->crop.width - 1;
222 xvip_write(&xscaler->xvip, XSCALER_HAPERTURE,
223 (end << XSCALER_APERTURE_END_SHIFT) |
224 (start << XSCALER_APERTURE_START_SHIFT));
225
226
227 start = xscaler->crop.top;
228 end = start + xscaler->crop.height - 1;
229 xvip_write(&xscaler->xvip, XSCALER_VAPERTURE,
230 (end << XSCALER_APERTURE_END_SHIFT) |
231 (start << XSCALER_APERTURE_START_SHIFT));
232
233
234 scale_factor = ((xscaler->crop.width << XSCALER_SF_SHIFT) /
235 xscaler->formats[XVIP_PAD_SOURCE].width) &
236 XSCALER_SF_MASK;
237 xvip_write(&xscaler->xvip, XSCALER_HSF, scale_factor);
238
239 scale_factor = ((xscaler->crop.height << XSCALER_SF_SHIFT) /
240 xscaler->formats[XVIP_PAD_SOURCE].height) &
241 XSCALER_SF_MASK;
242 xvip_write(&xscaler->xvip, XSCALER_VSF, scale_factor);
243
244 xvip_enable_reg_update(&xscaler->xvip);
245}
246
247static int xscaler_s_stream(struct v4l2_subdev *subdev, int enable)
248{
249 struct xscaler_device *xscaler = to_scaler(subdev);
250 u32 width;
251 u32 height;
252
253 if (!enable) {
254 xvip_stop(&xscaler->xvip);
255 return 0;
256 }
257
258
259 width = xscaler->formats[XVIP_PAD_SINK].width;
260 height = xscaler->formats[XVIP_PAD_SINK].height;
261 xvip_write(&xscaler->xvip, XSCALER_SOURCE_SIZE,
262 (height << XSCALER_SIZE_VERT_SHIFT) |
263 (width << XSCALER_SIZE_HORZ_SHIFT));
264
265
266 width = xscaler->formats[XVIP_PAD_SOURCE].width;
267 height = xscaler->formats[XVIP_PAD_SOURCE].height;
268 xvip_write(&xscaler->xvip, XSCALER_OUTPUT_SIZE,
269 (height << XSCALER_SIZE_VERT_SHIFT) |
270 (width << XSCALER_SIZE_HORZ_SHIFT));
271
272
273 xscaler_set_aperture(xscaler);
274
275 xvip_start(&xscaler->xvip);
276
277 return 0;
278}
279
280
281
282
283
284static int xscaler_enum_frame_size(struct v4l2_subdev *subdev,
285 struct v4l2_subdev_pad_config *cfg,
286 struct v4l2_subdev_frame_size_enum *fse)
287{
288 struct v4l2_mbus_framefmt *format;
289
290 format = v4l2_subdev_get_try_format(subdev, cfg, fse->pad);
291
292 if (fse->index || fse->code != format->code)
293 return -EINVAL;
294
295 fse->min_width = XSCALER_MIN_WIDTH;
296 fse->max_width = XSCALER_MAX_WIDTH;
297 fse->min_height = XSCALER_MIN_HEIGHT;
298 fse->max_height = XSCALER_MAX_HEIGHT;
299
300 return 0;
301}
302
303static struct v4l2_mbus_framefmt *
304__xscaler_get_pad_format(struct xscaler_device *xscaler,
305 struct v4l2_subdev_pad_config *cfg,
306 unsigned int pad, u32 which)
307{
308 switch (which) {
309 case V4L2_SUBDEV_FORMAT_TRY:
310 return v4l2_subdev_get_try_format(&xscaler->xvip.subdev, cfg,
311 pad);
312 case V4L2_SUBDEV_FORMAT_ACTIVE:
313 return &xscaler->formats[pad];
314 default:
315 return NULL;
316 }
317}
318
319static struct v4l2_rect *__xscaler_get_crop(struct xscaler_device *xscaler,
320 struct v4l2_subdev_pad_config *cfg,
321 u32 which)
322{
323 switch (which) {
324 case V4L2_SUBDEV_FORMAT_TRY:
325 return v4l2_subdev_get_try_crop(&xscaler->xvip.subdev, cfg,
326 XVIP_PAD_SINK);
327 case V4L2_SUBDEV_FORMAT_ACTIVE:
328 return &xscaler->crop;
329 default:
330 return NULL;
331 }
332}
333
334static int xscaler_get_format(struct v4l2_subdev *subdev,
335 struct v4l2_subdev_pad_config *cfg,
336 struct v4l2_subdev_format *fmt)
337{
338 struct xscaler_device *xscaler = to_scaler(subdev);
339
340 fmt->format = *__xscaler_get_pad_format(xscaler, cfg, fmt->pad,
341 fmt->which);
342
343 return 0;
344}
345
346static void xscaler_try_crop(const struct v4l2_mbus_framefmt *sink,
347 struct v4l2_rect *crop)
348{
349
350 crop->left = min_t(u32, crop->left, sink->width - XSCALER_MIN_WIDTH);
351 crop->top = min_t(u32, crop->top, sink->height - XSCALER_MIN_HEIGHT);
352 crop->width = clamp_t(u32, crop->width, XSCALER_MIN_WIDTH,
353 sink->width - crop->left);
354 crop->height = clamp_t(u32, crop->height, XSCALER_MIN_HEIGHT,
355 sink->height - crop->top);
356}
357
358static int xscaler_set_format(struct v4l2_subdev *subdev,
359 struct v4l2_subdev_pad_config *cfg,
360 struct v4l2_subdev_format *fmt)
361{
362 struct xscaler_device *xscaler = to_scaler(subdev);
363 struct v4l2_mbus_framefmt *format;
364 struct v4l2_rect *crop;
365
366 format = __xscaler_get_pad_format(xscaler, cfg, fmt->pad, fmt->which);
367
368 format->width = clamp_t(unsigned int, fmt->format.width,
369 XSCALER_MIN_WIDTH, XSCALER_MAX_WIDTH);
370 format->height = clamp_t(unsigned int, fmt->format.height,
371 XSCALER_MIN_HEIGHT, XSCALER_MAX_HEIGHT);
372
373 fmt->format = *format;
374
375 if (fmt->pad == XVIP_PAD_SINK) {
376
377 crop = __xscaler_get_crop(xscaler, cfg, fmt->which);
378 crop->left = 0;
379 crop->top = 0;
380 crop->width = fmt->format.width;
381 crop->height = fmt->format.height;
382 }
383
384 return 0;
385}
386
387static int xscaler_get_selection(struct v4l2_subdev *subdev,
388 struct v4l2_subdev_pad_config *cfg,
389 struct v4l2_subdev_selection *sel)
390{
391 struct xscaler_device *xscaler = to_scaler(subdev);
392 struct v4l2_mbus_framefmt *format;
393
394 if (sel->pad != XVIP_PAD_SINK)
395 return -EINVAL;
396
397 switch (sel->target) {
398 case V4L2_SEL_TGT_CROP_BOUNDS:
399 format = __xscaler_get_pad_format(xscaler, cfg, XVIP_PAD_SINK,
400 sel->which);
401 sel->r.left = 0;
402 sel->r.top = 0;
403 sel->r.width = format->width;
404 sel->r.height = format->height;
405 return 0;
406 case V4L2_SEL_TGT_CROP:
407 sel->r = *__xscaler_get_crop(xscaler, cfg, sel->which);
408 return 0;
409 default:
410 return -EINVAL;
411 }
412}
413
414static int xscaler_set_selection(struct v4l2_subdev *subdev,
415 struct v4l2_subdev_pad_config *cfg,
416 struct v4l2_subdev_selection *sel)
417{
418 struct xscaler_device *xscaler = to_scaler(subdev);
419 struct v4l2_mbus_framefmt *format;
420
421 if ((sel->target != V4L2_SEL_TGT_CROP) || (sel->pad != XVIP_PAD_SINK))
422 return -EINVAL;
423
424 format = __xscaler_get_pad_format(xscaler, cfg, XVIP_PAD_SINK,
425 sel->which);
426 xscaler_try_crop(format, &sel->r);
427 *__xscaler_get_crop(xscaler, cfg, sel->which) = sel->r;
428
429 return 0;
430}
431
432
433
434
435
436static int xscaler_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
437{
438 struct xscaler_device *xscaler = to_scaler(subdev);
439 struct v4l2_mbus_framefmt *format;
440
441
442 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SINK);
443 *format = xscaler->default_formats[XVIP_PAD_SINK];
444
445 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SOURCE);
446 *format = xscaler->default_formats[XVIP_PAD_SOURCE];
447
448 return 0;
449}
450
451static int xscaler_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
452{
453 return 0;
454}
455
456static struct v4l2_subdev_video_ops xscaler_video_ops = {
457 .s_stream = xscaler_s_stream,
458};
459
460static struct v4l2_subdev_pad_ops xscaler_pad_ops = {
461 .enum_mbus_code = xvip_enum_mbus_code,
462 .enum_frame_size = xscaler_enum_frame_size,
463 .get_fmt = xscaler_get_format,
464 .set_fmt = xscaler_set_format,
465 .get_selection = xscaler_get_selection,
466 .set_selection = xscaler_set_selection,
467};
468
469static struct v4l2_subdev_ops xscaler_ops = {
470 .video = &xscaler_video_ops,
471 .pad = &xscaler_pad_ops,
472};
473
474static const struct v4l2_subdev_internal_ops xscaler_internal_ops = {
475 .open = xscaler_open,
476 .close = xscaler_close,
477};
478
479
480
481
482
483static const struct media_entity_operations xscaler_media_ops = {
484 .link_validate = v4l2_subdev_link_validate,
485};
486
487
488
489
490
491static int __maybe_unused xscaler_pm_suspend(struct device *dev)
492{
493 struct xscaler_device *xscaler = dev_get_drvdata(dev);
494
495 xvip_suspend(&xscaler->xvip);
496
497 return 0;
498}
499
500static int __maybe_unused xscaler_pm_resume(struct device *dev)
501{
502 struct xscaler_device *xscaler = dev_get_drvdata(dev);
503
504 xvip_resume(&xscaler->xvip);
505
506 return 0;
507}
508
509
510
511
512
513static int xscaler_parse_of(struct xscaler_device *xscaler)
514{
515 struct device *dev = xscaler->xvip.dev;
516 struct device_node *node = xscaler->xvip.dev->of_node;
517 struct device_node *ports;
518 struct device_node *port;
519 int ret;
520
521 ports = of_get_child_by_name(node, "ports");
522 if (ports == NULL)
523 ports = node;
524
525
526 for_each_child_of_node(ports, port) {
527 if (port->name && (of_node_cmp(port->name, "port") == 0)) {
528 const struct xvip_video_format *vip_format;
529
530 vip_format = xvip_of_get_format(port);
531 if (IS_ERR(vip_format)) {
532 dev_err(dev, "invalid format in DT");
533 return PTR_ERR(vip_format);
534 }
535
536 if (!xscaler->vip_format) {
537 xscaler->vip_format = vip_format;
538 } else if (xscaler->vip_format != vip_format) {
539 dev_err(dev, "in/out format mismatch in DT");
540 return -EINVAL;
541 }
542 }
543 }
544
545 ret = of_property_read_u32(node, "xlnx,num-hori-taps",
546 &xscaler->num_hori_taps);
547 if (ret < 0)
548 return ret;
549
550 ret = of_property_read_u32(node, "xlnx,num-vert-taps",
551 &xscaler->num_vert_taps);
552 if (ret < 0)
553 return ret;
554
555 ret = of_property_read_u32(node, "xlnx,max-num-phases",
556 &xscaler->max_num_phases);
557 if (ret < 0)
558 return ret;
559
560 xscaler->separate_yc_coef =
561 of_property_read_bool(node, "xlnx,separate-yc-coef");
562
563 xscaler->separate_hv_coef =
564 of_property_read_bool(node, "xlnx,separate-hv-coef");
565
566 return 0;
567}
568
569static int xscaler_probe(struct platform_device *pdev)
570{
571 struct xscaler_device *xscaler;
572 struct v4l2_subdev *subdev;
573 struct v4l2_mbus_framefmt *default_format;
574 u32 size;
575 int ret;
576
577 xscaler = devm_kzalloc(&pdev->dev, sizeof(*xscaler), GFP_KERNEL);
578 if (!xscaler)
579 return -ENOMEM;
580
581 xscaler->xvip.dev = &pdev->dev;
582
583 ret = xscaler_parse_of(xscaler);
584 if (ret < 0)
585 return ret;
586
587 ret = xvip_init_resources(&xscaler->xvip);
588 if (ret < 0)
589 return ret;
590
591
592 xvip_reset(&xscaler->xvip);
593
594
595 subdev = &xscaler->xvip.subdev;
596 v4l2_subdev_init(subdev, &xscaler_ops);
597 subdev->dev = &pdev->dev;
598 subdev->internal_ops = &xscaler_internal_ops;
599 strlcpy(subdev->name, dev_name(&pdev->dev), sizeof(subdev->name));
600 v4l2_set_subdevdata(subdev, xscaler);
601 subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
602
603
604 default_format = &xscaler->default_formats[XVIP_PAD_SINK];
605 default_format->code = xscaler->vip_format->code;
606 default_format->field = V4L2_FIELD_NONE;
607 default_format->colorspace = V4L2_COLORSPACE_SRGB;
608 size = xvip_read(&xscaler->xvip, XSCALER_SOURCE_SIZE);
609 default_format->width = (size >> XSCALER_SIZE_HORZ_SHIFT) &
610 XSCALER_SIZE_MASK;
611 default_format->height = (size >> XSCALER_SIZE_VERT_SHIFT) &
612 XSCALER_SIZE_MASK;
613
614 xscaler->formats[XVIP_PAD_SINK] = *default_format;
615
616 default_format = &xscaler->default_formats[XVIP_PAD_SOURCE];
617 *default_format = xscaler->default_formats[XVIP_PAD_SINK];
618 size = xvip_read(&xscaler->xvip, XSCALER_OUTPUT_SIZE);
619 default_format->width = (size >> XSCALER_SIZE_HORZ_SHIFT) &
620 XSCALER_SIZE_MASK;
621 default_format->height = (size >> XSCALER_SIZE_VERT_SHIFT) &
622 XSCALER_SIZE_MASK;
623
624 xscaler->formats[XVIP_PAD_SOURCE] = *default_format;
625
626 xscaler->pads[XVIP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
627 xscaler->pads[XVIP_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
628 subdev->entity.ops = &xscaler_media_ops;
629
630 ret = media_entity_pads_init(&subdev->entity, 2, xscaler->pads);
631 if (ret < 0)
632 goto error;
633
634 platform_set_drvdata(pdev, xscaler);
635
636 xvip_print_version(&xscaler->xvip);
637
638 ret = xscaler_set_coefs(xscaler, (s16)xscaler->num_hori_taps);
639 if (ret < 0)
640 goto error;
641
642 if (xscaler->separate_hv_coef) {
643 ret = xscaler_set_coefs(xscaler, (s16)xscaler->num_vert_taps);
644 if (ret < 0)
645 goto error;
646 }
647
648 if (xscaler->separate_yc_coef) {
649 ret = xscaler_set_coefs(xscaler, (s16)xscaler->num_hori_taps);
650 if (ret < 0)
651 goto error;
652
653 if (xscaler->separate_hv_coef) {
654 ret = xscaler_set_coefs(xscaler,
655 (s16)xscaler->num_vert_taps);
656 if (ret < 0)
657 goto error;
658 }
659 }
660
661 ret = v4l2_async_register_subdev(subdev);
662 if (ret < 0) {
663 dev_err(&pdev->dev, "failed to register subdev\n");
664 goto error;
665 }
666
667 return 0;
668
669error:
670 media_entity_cleanup(&subdev->entity);
671 xvip_cleanup_resources(&xscaler->xvip);
672 return ret;
673}
674
675static int xscaler_remove(struct platform_device *pdev)
676{
677 struct xscaler_device *xscaler = platform_get_drvdata(pdev);
678 struct v4l2_subdev *subdev = &xscaler->xvip.subdev;
679
680 v4l2_async_unregister_subdev(subdev);
681 media_entity_cleanup(&subdev->entity);
682
683 xvip_cleanup_resources(&xscaler->xvip);
684
685 return 0;
686}
687
688static SIMPLE_DEV_PM_OPS(xscaler_pm_ops, xscaler_pm_suspend, xscaler_pm_resume);
689
690static const struct of_device_id xscaler_of_id_table[] = {
691 { .compatible = "xlnx,v-scaler-8.1" },
692 { }
693};
694MODULE_DEVICE_TABLE(of, xscaler_of_id_table);
695
696static struct platform_driver xscaler_driver = {
697 .driver = {
698 .name = "xilinx-scaler",
699 .of_match_table = xscaler_of_id_table,
700 },
701 .probe = xscaler_probe,
702 .remove = xscaler_remove,
703};
704
705module_platform_driver(xscaler_driver);
706
707MODULE_DESCRIPTION("Xilinx Scaler Driver");
708MODULE_LICENSE("GPL v2");
709
