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/module.h>
22#include <linux/of.h>
23#include <linux/platform_device.h>
24
25#include <media/v4l2-async.h>
26#include <media/v4l2-subdev.h>
27
28#include "xilinx-vip.h"
29
30#define XCFA_BAYER_PHASE 0x100
31#define XCFA_BAYER_PHASE_RGGB 0
32#define XCFA_BAYER_PHASE_GRBG 1
33#define XCFA_BAYER_PHASE_GBRG 2
34#define XCFA_BAYER_PHASE_BGGR 3
35
36
37
38
39
40
41
42
43
44struct xcfa_device {
45 struct xvip_device xvip;
46
47 struct media_pad pads[2];
48
49 struct v4l2_mbus_framefmt formats[2];
50 struct v4l2_mbus_framefmt default_formats[2];
51 const struct xvip_video_format *vip_formats[2];
52};
53
54static inline struct xcfa_device *to_cfa(struct v4l2_subdev *subdev)
55{
56 return container_of(subdev, struct xcfa_device, xvip.subdev);
57}
58
59
60
61
62
63static int xcfa_get_bayer_phase(const unsigned int code)
64{
65 switch (code) {
66 case MEDIA_BUS_FMT_SRGGB8_1X8:
67 return XCFA_BAYER_PHASE_RGGB;
68 case MEDIA_BUS_FMT_SGRBG8_1X8:
69 return XCFA_BAYER_PHASE_GRBG;
70 case MEDIA_BUS_FMT_SGBRG8_1X8:
71 return XCFA_BAYER_PHASE_GBRG;
72 case MEDIA_BUS_FMT_SBGGR8_1X8:
73 return XCFA_BAYER_PHASE_BGGR;
74 }
75
76 return -EINVAL;
77}
78
79static int xcfa_s_stream(struct v4l2_subdev *subdev, int enable)
80{
81 struct xcfa_device *xcfa = to_cfa(subdev);
82 const unsigned int code = xcfa->formats[XVIP_PAD_SINK].code;
83 u32 bayer_phase;
84
85 if (!enable) {
86 xvip_stop(&xcfa->xvip);
87 return 0;
88 }
89
90
91 bayer_phase = xcfa_get_bayer_phase(code);
92
93 xvip_write(&xcfa->xvip, XCFA_BAYER_PHASE, bayer_phase);
94
95 xvip_set_frame_size(&xcfa->xvip, &xcfa->formats[XVIP_PAD_SINK]);
96
97 xvip_start(&xcfa->xvip);
98
99 return 0;
100}
101
102
103
104
105
106static struct v4l2_mbus_framefmt *
107__xcfa_get_pad_format(struct xcfa_device *xcfa,
108 struct v4l2_subdev_pad_config *cfg,
109 unsigned int pad, u32 which)
110{
111 struct v4l2_mbus_framefmt *format;
112
113 switch (which) {
114 case V4L2_SUBDEV_FORMAT_TRY:
115 format = v4l2_subdev_get_try_format(&xcfa->xvip.subdev, cfg,
116 pad);
117 break;
118 case V4L2_SUBDEV_FORMAT_ACTIVE:
119 format = &xcfa->formats[pad];
120 break;
121 default:
122 format = NULL;
123 break;
124 }
125
126 return format;
127}
128
129static int xcfa_get_format(struct v4l2_subdev *subdev,
130 struct v4l2_subdev_pad_config *cfg,
131 struct v4l2_subdev_format *fmt)
132{
133 struct xcfa_device *xcfa = to_cfa(subdev);
134 struct v4l2_mbus_framefmt *format;
135
136 format = __xcfa_get_pad_format(xcfa, cfg, fmt->pad, fmt->which);
137 if (!format)
138 return -EINVAL;
139
140 fmt->format = *format;
141
142 return 0;
143}
144
145static int xcfa_set_format(struct v4l2_subdev *subdev,
146 struct v4l2_subdev_pad_config *cfg,
147 struct v4l2_subdev_format *fmt)
148{
149 struct xcfa_device *xcfa = to_cfa(subdev);
150 struct v4l2_mbus_framefmt *format;
151 int bayer_phase;
152
153 format = __xcfa_get_pad_format(xcfa, cfg, fmt->pad, fmt->which);
154 if (!format)
155 return -EINVAL;
156
157 if (fmt->pad == XVIP_PAD_SOURCE) {
158 fmt->format = *format;
159 return 0;
160 }
161
162 bayer_phase = xcfa_get_bayer_phase(fmt->format.code);
163 if (bayer_phase >= 0) {
164 xcfa->vip_formats[XVIP_PAD_SINK] =
165 xvip_get_format_by_code(fmt->format.code);
166 format->code = fmt->format.code;
167 }
168
169 xvip_set_format_size(format, fmt);
170
171 fmt->format = *format;
172
173
174 format = __xcfa_get_pad_format(xcfa, cfg, XVIP_PAD_SOURCE, fmt->which);
175
176 xvip_set_format_size(format, fmt);
177
178 return 0;
179}
180
181
182
183
184
185static int xcfa_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
186{
187 struct xcfa_device *xcfa = to_cfa(subdev);
188 struct v4l2_mbus_framefmt *format;
189
190
191 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SINK);
192 *format = xcfa->default_formats[XVIP_PAD_SINK];
193
194 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SOURCE);
195 *format = xcfa->default_formats[XVIP_PAD_SOURCE];
196
197 return 0;
198}
199
200static int xcfa_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
201{
202 return 0;
203}
204
205static struct v4l2_subdev_video_ops xcfa_video_ops = {
206 .s_stream = xcfa_s_stream,
207};
208
209static struct v4l2_subdev_pad_ops xcfa_pad_ops = {
210 .enum_mbus_code = xvip_enum_mbus_code,
211 .enum_frame_size = xvip_enum_frame_size,
212 .get_fmt = xcfa_get_format,
213 .set_fmt = xcfa_set_format,
214};
215
216static struct v4l2_subdev_ops xcfa_ops = {
217 .video = &xcfa_video_ops,
218 .pad = &xcfa_pad_ops,
219};
220
221static const struct v4l2_subdev_internal_ops xcfa_internal_ops = {
222 .open = xcfa_open,
223 .close = xcfa_close,
224};
225
226
227
228
229
230static const struct media_entity_operations xcfa_media_ops = {
231 .link_validate = v4l2_subdev_link_validate,
232};
233
234
235
236
237
238static int __maybe_unused xcfa_pm_suspend(struct device *dev)
239{
240 struct xcfa_device *xcfa = dev_get_drvdata(dev);
241
242 xvip_suspend(&xcfa->xvip);
243
244 return 0;
245}
246
247static int __maybe_unused xcfa_pm_resume(struct device *dev)
248{
249 struct xcfa_device *xcfa = dev_get_drvdata(dev);
250
251 xvip_resume(&xcfa->xvip);
252
253 return 0;
254}
255
256
257
258
259
260static int xcfa_parse_of(struct xcfa_device *xcfa)
261{
262 struct device *dev = xcfa->xvip.dev;
263 struct device_node *node = xcfa->xvip.dev->of_node;
264 struct device_node *ports;
265 struct device_node *port;
266 u32 port_id;
267 int ret;
268
269 ports = of_get_child_by_name(node, "ports");
270 if (ports == NULL)
271 ports = node;
272
273
274 for_each_child_of_node(ports, port) {
275 if (port->name && (of_node_cmp(port->name, "port") == 0)) {
276 const struct xvip_video_format *vip_format;
277
278 vip_format = xvip_of_get_format(port);
279 if (IS_ERR(vip_format)) {
280 dev_err(dev, "invalid format in DT");
281 return PTR_ERR(vip_format);
282 }
283
284 ret = of_property_read_u32(port, "reg", &port_id);
285 if (ret < 0) {
286 dev_err(dev, "no reg in DT");
287 return ret;
288 }
289
290 if (port_id != 0 && port_id != 1) {
291 dev_err(dev, "invalid reg in DT");
292 return -EINVAL;
293 }
294
295 xcfa->vip_formats[port_id] = vip_format;
296 }
297 }
298
299 return 0;
300}
301
302static int xcfa_probe(struct platform_device *pdev)
303{
304 struct xcfa_device *xcfa;
305 struct v4l2_subdev *subdev;
306 struct v4l2_mbus_framefmt *default_format;
307 int ret;
308
309 xcfa = devm_kzalloc(&pdev->dev, sizeof(*xcfa), GFP_KERNEL);
310 if (!xcfa)
311 return -ENOMEM;
312
313 xcfa->xvip.dev = &pdev->dev;
314
315 ret = xcfa_parse_of(xcfa);
316 if (ret < 0)
317 return ret;
318
319 ret = xvip_init_resources(&xcfa->xvip);
320 if (ret < 0)
321 return ret;
322
323
324 xvip_reset(&xcfa->xvip);
325
326
327 subdev = &xcfa->xvip.subdev;
328 v4l2_subdev_init(subdev, &xcfa_ops);
329 subdev->dev = &pdev->dev;
330 subdev->internal_ops = &xcfa_internal_ops;
331 strlcpy(subdev->name, dev_name(&pdev->dev), sizeof(subdev->name));
332 v4l2_set_subdevdata(subdev, xcfa);
333 subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
334
335
336 default_format = &xcfa->default_formats[XVIP_PAD_SINK];
337 default_format->code = xcfa->vip_formats[XVIP_PAD_SINK]->code;
338 default_format->field = V4L2_FIELD_NONE;
339 default_format->colorspace = V4L2_COLORSPACE_SRGB;
340 xvip_get_frame_size(&xcfa->xvip, default_format);
341
342 xcfa->formats[XVIP_PAD_SINK] = *default_format;
343
344 default_format = &xcfa->default_formats[XVIP_PAD_SOURCE];
345 *default_format = xcfa->default_formats[XVIP_PAD_SINK];
346 default_format->code = xcfa->vip_formats[XVIP_PAD_SOURCE]->code;
347
348 xcfa->formats[XVIP_PAD_SOURCE] = *default_format;
349
350 xcfa->pads[XVIP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
351 xcfa->pads[XVIP_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
352 subdev->entity.ops = &xcfa_media_ops;
353 ret = media_entity_pads_init(&subdev->entity, 2, xcfa->pads);
354 if (ret < 0)
355 goto error;
356
357 platform_set_drvdata(pdev, xcfa);
358
359 xvip_print_version(&xcfa->xvip);
360
361 ret = v4l2_async_register_subdev(subdev);
362 if (ret < 0) {
363 dev_err(&pdev->dev, "failed to register subdev\n");
364 goto error;
365 }
366
367 return 0;
368
369error:
370 media_entity_cleanup(&subdev->entity);
371 xvip_cleanup_resources(&xcfa->xvip);
372 return ret;
373}
374
375static int xcfa_remove(struct platform_device *pdev)
376{
377 struct xcfa_device *xcfa = platform_get_drvdata(pdev);
378 struct v4l2_subdev *subdev = &xcfa->xvip.subdev;
379
380 v4l2_async_unregister_subdev(subdev);
381 media_entity_cleanup(&subdev->entity);
382
383 xvip_cleanup_resources(&xcfa->xvip);
384
385 return 0;
386}
387
388static SIMPLE_DEV_PM_OPS(xcfa_pm_ops, xcfa_pm_suspend, xcfa_pm_resume);
389
390static const struct of_device_id xcfa_of_id_table[] = {
391 { .compatible = "xlnx,v-cfa-7.0" },
392 { }
393};
394MODULE_DEVICE_TABLE(of, xcfa_of_id_table);
395
396static struct platform_driver xcfa_driver = {
397 .driver = {
398 .name = "xilinx-cfa",
399 .pm = &xcfa_pm_ops,
400 .of_match_table = xcfa_of_id_table,
401 },
402 .probe = xcfa_probe,
403 .remove = xcfa_remove,
404};
405
406module_platform_driver(xcfa_driver);
407
408MODULE_DESCRIPTION("Xilinx Color Filter Array Driver");
409MODULE_LICENSE("GPL v2");
410
