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 switch (which) {
112 case V4L2_SUBDEV_FORMAT_TRY:
113 return v4l2_subdev_get_try_format(&xcfa->xvip.subdev, cfg, pad);
114 case V4L2_SUBDEV_FORMAT_ACTIVE:
115 return &xcfa->formats[pad];
116 default:
117 return NULL;
118 }
119}
120
121static int xcfa_get_format(struct v4l2_subdev *subdev,
122 struct v4l2_subdev_pad_config *cfg,
123 struct v4l2_subdev_format *fmt)
124{
125 struct xcfa_device *xcfa = to_cfa(subdev);
126
127 fmt->format = *__xcfa_get_pad_format(xcfa, cfg, fmt->pad, fmt->which);
128
129 return 0;
130}
131
132static int xcfa_set_format(struct v4l2_subdev *subdev,
133 struct v4l2_subdev_pad_config *cfg,
134 struct v4l2_subdev_format *fmt)
135{
136 struct xcfa_device *xcfa = to_cfa(subdev);
137 struct v4l2_mbus_framefmt *format;
138 int bayer_phase;
139
140 format = __xcfa_get_pad_format(xcfa, cfg, fmt->pad, fmt->which);
141
142 if (fmt->pad == XVIP_PAD_SOURCE) {
143 fmt->format = *format;
144 return 0;
145 }
146
147 bayer_phase = xcfa_get_bayer_phase(fmt->format.code);
148 if (bayer_phase >= 0) {
149 xcfa->vip_formats[XVIP_PAD_SINK] =
150 xvip_get_format_by_code(fmt->format.code);
151 format->code = fmt->format.code;
152 }
153
154 xvip_set_format_size(format, fmt);
155
156 fmt->format = *format;
157
158
159 format = __xcfa_get_pad_format(xcfa, cfg, XVIP_PAD_SOURCE, fmt->which);
160
161 xvip_set_format_size(format, fmt);
162
163 return 0;
164}
165
166
167
168
169
170static int xcfa_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
171{
172 struct xcfa_device *xcfa = to_cfa(subdev);
173 struct v4l2_mbus_framefmt *format;
174
175
176 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SINK);
177 *format = xcfa->default_formats[XVIP_PAD_SINK];
178
179 format = v4l2_subdev_get_try_format(subdev, fh->pad, XVIP_PAD_SOURCE);
180 *format = xcfa->default_formats[XVIP_PAD_SOURCE];
181
182 return 0;
183}
184
185static int xcfa_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
186{
187 return 0;
188}
189
190static struct v4l2_subdev_video_ops xcfa_video_ops = {
191 .s_stream = xcfa_s_stream,
192};
193
194static struct v4l2_subdev_pad_ops xcfa_pad_ops = {
195 .enum_mbus_code = xvip_enum_mbus_code,
196 .enum_frame_size = xvip_enum_frame_size,
197 .get_fmt = xcfa_get_format,
198 .set_fmt = xcfa_set_format,
199};
200
201static struct v4l2_subdev_ops xcfa_ops = {
202 .video = &xcfa_video_ops,
203 .pad = &xcfa_pad_ops,
204};
205
206static const struct v4l2_subdev_internal_ops xcfa_internal_ops = {
207 .open = xcfa_open,
208 .close = xcfa_close,
209};
210
211
212
213
214
215static const struct media_entity_operations xcfa_media_ops = {
216 .link_validate = v4l2_subdev_link_validate,
217};
218
219
220
221
222
223static int __maybe_unused xcfa_pm_suspend(struct device *dev)
224{
225 struct xcfa_device *xcfa = dev_get_drvdata(dev);
226
227 xvip_suspend(&xcfa->xvip);
228
229 return 0;
230}
231
232static int __maybe_unused xcfa_pm_resume(struct device *dev)
233{
234 struct xcfa_device *xcfa = dev_get_drvdata(dev);
235
236 xvip_resume(&xcfa->xvip);
237
238 return 0;
239}
240
241
242
243
244
245static int xcfa_parse_of(struct xcfa_device *xcfa)
246{
247 struct device *dev = xcfa->xvip.dev;
248 struct device_node *node = xcfa->xvip.dev->of_node;
249 struct device_node *ports;
250 struct device_node *port;
251 u32 port_id;
252 int ret;
253
254 ports = of_get_child_by_name(node, "ports");
255 if (ports == NULL)
256 ports = node;
257
258
259 for_each_child_of_node(ports, port) {
260 if (port->name && (of_node_cmp(port->name, "port") == 0)) {
261 const struct xvip_video_format *vip_format;
262
263 vip_format = xvip_of_get_format(port);
264 if (IS_ERR(vip_format)) {
265 dev_err(dev, "invalid format in DT");
266 return PTR_ERR(vip_format);
267 }
268
269 ret = of_property_read_u32(port, "reg", &port_id);
270 if (ret < 0) {
271 dev_err(dev, "no reg in DT");
272 return ret;
273 }
274
275 if (port_id != 0 && port_id != 1) {
276 dev_err(dev, "invalid reg in DT");
277 return -EINVAL;
278 }
279
280 xcfa->vip_formats[port_id] = vip_format;
281 }
282 }
283
284 return 0;
285}
286
287static int xcfa_probe(struct platform_device *pdev)
288{
289 struct xcfa_device *xcfa;
290 struct v4l2_subdev *subdev;
291 struct v4l2_mbus_framefmt *default_format;
292 int ret;
293
294 xcfa = devm_kzalloc(&pdev->dev, sizeof(*xcfa), GFP_KERNEL);
295 if (!xcfa)
296 return -ENOMEM;
297
298 xcfa->xvip.dev = &pdev->dev;
299
300 ret = xcfa_parse_of(xcfa);
301 if (ret < 0)
302 return ret;
303
304 ret = xvip_init_resources(&xcfa->xvip);
305 if (ret < 0)
306 return ret;
307
308
309 xvip_reset(&xcfa->xvip);
310
311
312 subdev = &xcfa->xvip.subdev;
313 v4l2_subdev_init(subdev, &xcfa_ops);
314 subdev->dev = &pdev->dev;
315 subdev->internal_ops = &xcfa_internal_ops;
316 strlcpy(subdev->name, dev_name(&pdev->dev), sizeof(subdev->name));
317 v4l2_set_subdevdata(subdev, xcfa);
318 subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
319
320
321 default_format = &xcfa->default_formats[XVIP_PAD_SINK];
322 default_format->code = xcfa->vip_formats[XVIP_PAD_SINK]->code;
323 default_format->field = V4L2_FIELD_NONE;
324 default_format->colorspace = V4L2_COLORSPACE_SRGB;
325 xvip_get_frame_size(&xcfa->xvip, default_format);
326
327 xcfa->formats[XVIP_PAD_SINK] = *default_format;
328
329 default_format = &xcfa->default_formats[XVIP_PAD_SOURCE];
330 *default_format = xcfa->default_formats[XVIP_PAD_SINK];
331 default_format->code = xcfa->vip_formats[XVIP_PAD_SOURCE]->code;
332
333 xcfa->formats[XVIP_PAD_SOURCE] = *default_format;
334
335 xcfa->pads[XVIP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
336 xcfa->pads[XVIP_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
337 subdev->entity.ops = &xcfa_media_ops;
338 ret = media_entity_pads_init(&subdev->entity, 2, xcfa->pads);
339 if (ret < 0)
340 goto error;
341
342 platform_set_drvdata(pdev, xcfa);
343
344 xvip_print_version(&xcfa->xvip);
345
346 ret = v4l2_async_register_subdev(subdev);
347 if (ret < 0) {
348 dev_err(&pdev->dev, "failed to register subdev\n");
349 goto error;
350 }
351
352 return 0;
353
354error:
355 media_entity_cleanup(&subdev->entity);
356 xvip_cleanup_resources(&xcfa->xvip);
357 return ret;
358}
359
360static int xcfa_remove(struct platform_device *pdev)
361{
362 struct xcfa_device *xcfa = platform_get_drvdata(pdev);
363 struct v4l2_subdev *subdev = &xcfa->xvip.subdev;
364
365 v4l2_async_unregister_subdev(subdev);
366 media_entity_cleanup(&subdev->entity);
367
368 xvip_cleanup_resources(&xcfa->xvip);
369
370 return 0;
371}
372
373static SIMPLE_DEV_PM_OPS(xcfa_pm_ops, xcfa_pm_suspend, xcfa_pm_resume);
374
375static const struct of_device_id xcfa_of_id_table[] = {
376 { .compatible = "xlnx,v-cfa-7.0" },
377 { }
378};
379MODULE_DEVICE_TABLE(of, xcfa_of_id_table);
380
381static struct platform_driver xcfa_driver = {
382 .driver = {
383 .name = "xilinx-cfa",
384 .pm = &xcfa_pm_ops,
385 .of_match_table = xcfa_of_id_table,
386 },
387 .probe = xcfa_probe,
388 .remove = xcfa_remove,
389};
390
391module_platform_driver(xcfa_driver);
392
393MODULE_DESCRIPTION("Xilinx Color Filter Array Driver");
394MODULE_LICENSE("GPL v2");
395
