1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC |
4 | * |
5 | * Copyright (c) 2017 Mentor Graphics Inc. |
6 | */ |
7 | #include <media/v4l2-ctrls.h> |
8 | #include <media/v4l2-device.h> |
9 | #include <media/v4l2-ioctl.h> |
10 | #include <media/v4l2-mc.h> |
11 | #include <media/v4l2-subdev.h> |
12 | #include <media/imx.h> |
13 | #include "imx-media.h" |
14 | |
15 | /* |
16 | * This subdev implements two different video pipelines: |
17 | * |
18 | * CSI -> VDIC |
19 | * |
20 | * In this pipeline, the CSI sends a single interlaced field F(n-1) |
21 | * directly to the VDIC (and optionally the following field F(n) |
22 | * can be sent to memory via IDMAC channel 13). This pipeline only works |
23 | * in VDIC's high motion mode, which only requires a single field for |
24 | * processing. The other motion modes (low and medium) require three |
25 | * fields, so this pipeline does not work in those modes. Also, it is |
26 | * not clear how this pipeline can deal with the various field orders |
27 | * (sequential BT/TB, interlaced BT/TB). |
28 | * |
29 | * MEM -> CH8,9,10 -> VDIC |
30 | * |
31 | * In this pipeline, previous field F(n-1), current field F(n), and next |
32 | * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10. |
33 | * These memory buffers can come from a video output or mem2mem device. |
34 | * All motion modes are supported by this pipeline. |
35 | * |
36 | * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be |
37 | * used in high motion mode. |
38 | */ |
39 | |
40 | struct vdic_priv; |
41 | |
42 | struct vdic_pipeline_ops { |
43 | int (*setup)(struct vdic_priv *priv); |
44 | void (*start)(struct vdic_priv *priv); |
45 | void (*stop)(struct vdic_priv *priv); |
46 | void (*disable)(struct vdic_priv *priv); |
47 | }; |
48 | |
49 | /* |
50 | * Min/Max supported width and heights. |
51 | */ |
52 | #define MIN_W 32 |
53 | #define MIN_H 32 |
54 | #define MAX_W_VDIC 968 |
55 | #define MAX_H_VDIC 2048 |
56 | #define W_ALIGN 4 /* multiple of 16 pixels */ |
57 | #define H_ALIGN 1 /* multiple of 2 lines */ |
58 | #define S_ALIGN 1 /* multiple of 2 */ |
59 | |
60 | struct vdic_priv { |
61 | struct device *ipu_dev; |
62 | struct ipu_soc *ipu; |
63 | |
64 | struct v4l2_subdev sd; |
65 | struct media_pad pad[VDIC_NUM_PADS]; |
66 | |
67 | /* lock to protect all members below */ |
68 | struct mutex lock; |
69 | |
70 | /* IPU units we require */ |
71 | struct ipu_vdi *vdi; |
72 | |
73 | int active_input_pad; |
74 | |
75 | struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */ |
76 | struct ipuv3_channel *vdi_in_ch; /* F(n) transfer channel */ |
77 | struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */ |
78 | |
79 | /* pipeline operations */ |
80 | struct vdic_pipeline_ops *ops; |
81 | |
82 | /* current and previous input buffers indirect path */ |
83 | struct imx_media_buffer *curr_in_buf; |
84 | struct imx_media_buffer *prev_in_buf; |
85 | |
86 | /* |
87 | * translated field type, input line stride, and field size |
88 | * for indirect path |
89 | */ |
90 | u32 fieldtype; |
91 | u32 in_stride; |
92 | u32 field_size; |
93 | |
94 | /* the source (a video device or subdev) */ |
95 | struct media_entity *src; |
96 | /* the sink that will receive the progressive out buffers */ |
97 | struct v4l2_subdev *sink_sd; |
98 | |
99 | struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS]; |
100 | const struct imx_media_pixfmt *cc[VDIC_NUM_PADS]; |
101 | struct v4l2_fract frame_interval[VDIC_NUM_PADS]; |
102 | |
103 | /* the video device at IDMAC input pad */ |
104 | struct imx_media_video_dev *vdev; |
105 | |
106 | bool csi_direct; /* using direct CSI->VDIC->IC pipeline */ |
107 | |
108 | /* motion select control */ |
109 | struct v4l2_ctrl_handler ctrl_hdlr; |
110 | enum ipu_motion_sel motion; |
111 | |
112 | int stream_count; |
113 | }; |
114 | |
115 | static void vdic_put_ipu_resources(struct vdic_priv *priv) |
116 | { |
117 | if (priv->vdi_in_ch_p) |
118 | ipu_idmac_put(priv->vdi_in_ch_p); |
119 | priv->vdi_in_ch_p = NULL; |
120 | |
121 | if (priv->vdi_in_ch) |
122 | ipu_idmac_put(priv->vdi_in_ch); |
123 | priv->vdi_in_ch = NULL; |
124 | |
125 | if (priv->vdi_in_ch_n) |
126 | ipu_idmac_put(priv->vdi_in_ch_n); |
127 | priv->vdi_in_ch_n = NULL; |
128 | |
129 | if (!IS_ERR_OR_NULL(ptr: priv->vdi)) |
130 | ipu_vdi_put(vdi: priv->vdi); |
131 | priv->vdi = NULL; |
132 | } |
133 | |
134 | static int vdic_get_ipu_resources(struct vdic_priv *priv) |
135 | { |
136 | int ret, err_chan; |
137 | struct ipuv3_channel *ch; |
138 | struct ipu_vdi *vdi; |
139 | |
140 | vdi = ipu_vdi_get(ipu: priv->ipu); |
141 | if (IS_ERR(ptr: vdi)) { |
142 | v4l2_err(&priv->sd, "failed to get VDIC\n" ); |
143 | ret = PTR_ERR(ptr: vdi); |
144 | goto out; |
145 | } |
146 | priv->vdi = vdi; |
147 | |
148 | if (!priv->csi_direct) { |
149 | ch = ipu_idmac_get(ipu: priv->ipu, IPUV3_CHANNEL_MEM_VDI_PREV); |
150 | if (IS_ERR(ptr: ch)) { |
151 | err_chan = IPUV3_CHANNEL_MEM_VDI_PREV; |
152 | ret = PTR_ERR(ptr: ch); |
153 | goto out_err_chan; |
154 | } |
155 | priv->vdi_in_ch_p = ch; |
156 | |
157 | ch = ipu_idmac_get(ipu: priv->ipu, IPUV3_CHANNEL_MEM_VDI_CUR); |
158 | if (IS_ERR(ptr: ch)) { |
159 | err_chan = IPUV3_CHANNEL_MEM_VDI_CUR; |
160 | ret = PTR_ERR(ptr: ch); |
161 | goto out_err_chan; |
162 | } |
163 | priv->vdi_in_ch = ch; |
164 | |
165 | ch = ipu_idmac_get(ipu: priv->ipu, IPUV3_CHANNEL_MEM_VDI_NEXT); |
166 | if (IS_ERR(ptr: ch)) { |
167 | err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT; |
168 | ret = PTR_ERR(ptr: ch); |
169 | goto out_err_chan; |
170 | } |
171 | priv->vdi_in_ch_n = ch; |
172 | } |
173 | |
174 | return 0; |
175 | |
176 | out_err_chan: |
177 | v4l2_err(&priv->sd, "could not get IDMAC channel %u\n" , err_chan); |
178 | out: |
179 | vdic_put_ipu_resources(priv); |
180 | return ret; |
181 | } |
182 | |
183 | /* |
184 | * This function is currently unused, but will be called when the |
185 | * output/mem2mem device at the IDMAC input pad sends us a new |
186 | * buffer. It kicks off the IDMAC read channels to bring in the |
187 | * buffer fields from memory and begin the conversions. |
188 | */ |
189 | static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv, |
190 | struct imx_media_buffer *curr) |
191 | { |
192 | dma_addr_t prev_phys, curr_phys, next_phys; |
193 | struct imx_media_buffer *prev; |
194 | struct vb2_buffer *curr_vb, *prev_vb; |
195 | u32 fs = priv->field_size; |
196 | u32 is = priv->in_stride; |
197 | |
198 | /* current input buffer is now previous */ |
199 | priv->prev_in_buf = priv->curr_in_buf; |
200 | priv->curr_in_buf = curr; |
201 | prev = priv->prev_in_buf ? priv->prev_in_buf : curr; |
202 | |
203 | prev_vb = &prev->vbuf.vb2_buf; |
204 | curr_vb = &curr->vbuf.vb2_buf; |
205 | |
206 | switch (priv->fieldtype) { |
207 | case V4L2_FIELD_SEQ_TB: |
208 | case V4L2_FIELD_SEQ_BT: |
209 | prev_phys = vb2_dma_contig_plane_dma_addr(vb: prev_vb, plane_no: 0) + fs; |
210 | curr_phys = vb2_dma_contig_plane_dma_addr(vb: curr_vb, plane_no: 0); |
211 | next_phys = vb2_dma_contig_plane_dma_addr(vb: curr_vb, plane_no: 0) + fs; |
212 | break; |
213 | case V4L2_FIELD_INTERLACED_TB: |
214 | case V4L2_FIELD_INTERLACED_BT: |
215 | case V4L2_FIELD_INTERLACED: |
216 | prev_phys = vb2_dma_contig_plane_dma_addr(vb: prev_vb, plane_no: 0) + is; |
217 | curr_phys = vb2_dma_contig_plane_dma_addr(vb: curr_vb, plane_no: 0); |
218 | next_phys = vb2_dma_contig_plane_dma_addr(vb: curr_vb, plane_no: 0) + is; |
219 | break; |
220 | default: |
221 | /* |
222 | * can't get here, priv->fieldtype can only be one of |
223 | * the above. This is to quiet smatch errors. |
224 | */ |
225 | return; |
226 | } |
227 | |
228 | ipu_cpmem_set_buffer(ch: priv->vdi_in_ch_p, bufnum: 0, buf: prev_phys); |
229 | ipu_cpmem_set_buffer(ch: priv->vdi_in_ch, bufnum: 0, buf: curr_phys); |
230 | ipu_cpmem_set_buffer(ch: priv->vdi_in_ch_n, bufnum: 0, buf: next_phys); |
231 | |
232 | ipu_idmac_select_buffer(channel: priv->vdi_in_ch_p, buf_num: 0); |
233 | ipu_idmac_select_buffer(channel: priv->vdi_in_ch, buf_num: 0); |
234 | ipu_idmac_select_buffer(channel: priv->vdi_in_ch_n, buf_num: 0); |
235 | } |
236 | |
237 | static int setup_vdi_channel(struct vdic_priv *priv, |
238 | struct ipuv3_channel *channel, |
239 | dma_addr_t phys0, dma_addr_t phys1) |
240 | { |
241 | struct imx_media_video_dev *vdev = priv->vdev; |
242 | unsigned int burst_size; |
243 | struct ipu_image image; |
244 | int ret; |
245 | |
246 | ipu_cpmem_zero(ch: channel); |
247 | |
248 | memset(&image, 0, sizeof(image)); |
249 | image.pix = vdev->fmt; |
250 | image.rect = vdev->compose; |
251 | /* one field to VDIC channels */ |
252 | image.pix.height /= 2; |
253 | image.rect.height /= 2; |
254 | image.phys0 = phys0; |
255 | image.phys1 = phys1; |
256 | |
257 | ret = ipu_cpmem_set_image(ch: channel, image: &image); |
258 | if (ret) |
259 | return ret; |
260 | |
261 | burst_size = (image.pix.width & 0xf) ? 8 : 16; |
262 | ipu_cpmem_set_burstsize(ch: channel, burstsize: burst_size); |
263 | |
264 | ipu_cpmem_set_axi_id(ch: channel, id: 1); |
265 | |
266 | ipu_idmac_set_double_buffer(channel, doublebuffer: false); |
267 | |
268 | return 0; |
269 | } |
270 | |
271 | static int vdic_setup_direct(struct vdic_priv *priv) |
272 | { |
273 | /* set VDIC to receive from CSI for direct path */ |
274 | ipu_fsu_link(ipu: priv->ipu, IPUV3_CHANNEL_CSI_DIRECT, |
275 | IPUV3_CHANNEL_CSI_VDI_PREV); |
276 | |
277 | return 0; |
278 | } |
279 | |
280 | static void vdic_start_direct(struct vdic_priv *priv) |
281 | { |
282 | } |
283 | |
284 | static void vdic_stop_direct(struct vdic_priv *priv) |
285 | { |
286 | } |
287 | |
288 | static void vdic_disable_direct(struct vdic_priv *priv) |
289 | { |
290 | ipu_fsu_unlink(ipu: priv->ipu, IPUV3_CHANNEL_CSI_DIRECT, |
291 | IPUV3_CHANNEL_CSI_VDI_PREV); |
292 | } |
293 | |
294 | static int vdic_setup_indirect(struct vdic_priv *priv) |
295 | { |
296 | struct v4l2_mbus_framefmt *infmt; |
297 | const struct imx_media_pixfmt *incc; |
298 | int in_size, ret; |
299 | |
300 | infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC]; |
301 | incc = priv->cc[VDIC_SINK_PAD_IDMAC]; |
302 | |
303 | in_size = (infmt->width * incc->bpp * infmt->height) >> 3; |
304 | |
305 | /* 1/2 full image size */ |
306 | priv->field_size = in_size / 2; |
307 | priv->in_stride = incc->planar ? |
308 | infmt->width : (infmt->width * incc->bpp) >> 3; |
309 | |
310 | priv->prev_in_buf = NULL; |
311 | priv->curr_in_buf = NULL; |
312 | |
313 | priv->fieldtype = infmt->field; |
314 | |
315 | /* init the vdi-in channels */ |
316 | ret = setup_vdi_channel(priv, channel: priv->vdi_in_ch_p, phys0: 0, phys1: 0); |
317 | if (ret) |
318 | return ret; |
319 | ret = setup_vdi_channel(priv, channel: priv->vdi_in_ch, phys0: 0, phys1: 0); |
320 | if (ret) |
321 | return ret; |
322 | return setup_vdi_channel(priv, channel: priv->vdi_in_ch_n, phys0: 0, phys1: 0); |
323 | } |
324 | |
325 | static void vdic_start_indirect(struct vdic_priv *priv) |
326 | { |
327 | /* enable the channels */ |
328 | ipu_idmac_enable_channel(channel: priv->vdi_in_ch_p); |
329 | ipu_idmac_enable_channel(channel: priv->vdi_in_ch); |
330 | ipu_idmac_enable_channel(channel: priv->vdi_in_ch_n); |
331 | } |
332 | |
333 | static void vdic_stop_indirect(struct vdic_priv *priv) |
334 | { |
335 | /* disable channels */ |
336 | ipu_idmac_disable_channel(channel: priv->vdi_in_ch_p); |
337 | ipu_idmac_disable_channel(channel: priv->vdi_in_ch); |
338 | ipu_idmac_disable_channel(channel: priv->vdi_in_ch_n); |
339 | } |
340 | |
341 | static void vdic_disable_indirect(struct vdic_priv *priv) |
342 | { |
343 | } |
344 | |
345 | static struct vdic_pipeline_ops direct_ops = { |
346 | .setup = vdic_setup_direct, |
347 | .start = vdic_start_direct, |
348 | .stop = vdic_stop_direct, |
349 | .disable = vdic_disable_direct, |
350 | }; |
351 | |
352 | static struct vdic_pipeline_ops indirect_ops = { |
353 | .setup = vdic_setup_indirect, |
354 | .start = vdic_start_indirect, |
355 | .stop = vdic_stop_indirect, |
356 | .disable = vdic_disable_indirect, |
357 | }; |
358 | |
359 | static int vdic_start(struct vdic_priv *priv) |
360 | { |
361 | struct v4l2_mbus_framefmt *infmt; |
362 | int ret; |
363 | |
364 | infmt = &priv->format_mbus[priv->active_input_pad]; |
365 | |
366 | priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops; |
367 | |
368 | ret = vdic_get_ipu_resources(priv); |
369 | if (ret) |
370 | return ret; |
371 | |
372 | /* |
373 | * init the VDIC. |
374 | * |
375 | * note we don't give infmt->code to ipu_vdi_setup(). The VDIC |
376 | * only supports 4:2:2 or 4:2:0, and this subdev will only |
377 | * negotiate 4:2:2 at its sink pads. |
378 | */ |
379 | ipu_vdi_setup(vdi: priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8, |
380 | xres: infmt->width, yres: infmt->height); |
381 | ipu_vdi_set_field_order(vdi: priv->vdi, V4L2_STD_UNKNOWN, field: infmt->field); |
382 | ipu_vdi_set_motion(vdi: priv->vdi, motion_sel: priv->motion); |
383 | |
384 | ret = priv->ops->setup(priv); |
385 | if (ret) |
386 | goto out_put_ipu; |
387 | |
388 | ipu_vdi_enable(vdi: priv->vdi); |
389 | |
390 | priv->ops->start(priv); |
391 | |
392 | return 0; |
393 | |
394 | out_put_ipu: |
395 | vdic_put_ipu_resources(priv); |
396 | return ret; |
397 | } |
398 | |
399 | static void vdic_stop(struct vdic_priv *priv) |
400 | { |
401 | priv->ops->stop(priv); |
402 | ipu_vdi_disable(vdi: priv->vdi); |
403 | priv->ops->disable(priv); |
404 | |
405 | vdic_put_ipu_resources(priv); |
406 | } |
407 | |
408 | /* |
409 | * V4L2 subdev operations. |
410 | */ |
411 | |
412 | static int vdic_s_ctrl(struct v4l2_ctrl *ctrl) |
413 | { |
414 | struct vdic_priv *priv = container_of(ctrl->handler, |
415 | struct vdic_priv, ctrl_hdlr); |
416 | enum ipu_motion_sel motion; |
417 | int ret = 0; |
418 | |
419 | mutex_lock(&priv->lock); |
420 | |
421 | switch (ctrl->id) { |
422 | case V4L2_CID_DEINTERLACING_MODE: |
423 | motion = ctrl->val; |
424 | if (motion != priv->motion) { |
425 | /* can't change motion control mid-streaming */ |
426 | if (priv->stream_count > 0) { |
427 | ret = -EBUSY; |
428 | goto out; |
429 | } |
430 | priv->motion = motion; |
431 | } |
432 | break; |
433 | default: |
434 | v4l2_err(&priv->sd, "Invalid control\n" ); |
435 | ret = -EINVAL; |
436 | } |
437 | |
438 | out: |
439 | mutex_unlock(lock: &priv->lock); |
440 | return ret; |
441 | } |
442 | |
443 | static const struct v4l2_ctrl_ops vdic_ctrl_ops = { |
444 | .s_ctrl = vdic_s_ctrl, |
445 | }; |
446 | |
447 | static const char * const [] = { |
448 | "No Motion Compensation" , |
449 | "Low Motion" , |
450 | "Medium Motion" , |
451 | "High Motion" , |
452 | }; |
453 | |
454 | static int vdic_init_controls(struct vdic_priv *priv) |
455 | { |
456 | struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr; |
457 | int ret; |
458 | |
459 | v4l2_ctrl_handler_init(hdlr, 1); |
460 | |
461 | v4l2_ctrl_new_std_menu_items(hdl: hdlr, ops: &vdic_ctrl_ops, |
462 | V4L2_CID_DEINTERLACING_MODE, |
463 | max: HIGH_MOTION, mask: 0, def: HIGH_MOTION, |
464 | qmenu: vdic_ctrl_motion_menu); |
465 | |
466 | priv->sd.ctrl_handler = hdlr; |
467 | |
468 | if (hdlr->error) { |
469 | ret = hdlr->error; |
470 | goto out_free; |
471 | } |
472 | |
473 | v4l2_ctrl_handler_setup(hdl: hdlr); |
474 | return 0; |
475 | |
476 | out_free: |
477 | v4l2_ctrl_handler_free(hdl: hdlr); |
478 | return ret; |
479 | } |
480 | |
481 | static int vdic_s_stream(struct v4l2_subdev *sd, int enable) |
482 | { |
483 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
484 | struct v4l2_subdev *src_sd = NULL; |
485 | int ret = 0; |
486 | |
487 | mutex_lock(&priv->lock); |
488 | |
489 | if (!priv->src || !priv->sink_sd) { |
490 | ret = -EPIPE; |
491 | goto out; |
492 | } |
493 | |
494 | if (priv->csi_direct) |
495 | src_sd = media_entity_to_v4l2_subdev(priv->src); |
496 | |
497 | /* |
498 | * enable/disable streaming only if stream_count is |
499 | * going from 0 to 1 / 1 to 0. |
500 | */ |
501 | if (priv->stream_count != !enable) |
502 | goto update_count; |
503 | |
504 | dev_dbg(priv->ipu_dev, "%s: stream %s\n" , sd->name, |
505 | enable ? "ON" : "OFF" ); |
506 | |
507 | if (enable) |
508 | ret = vdic_start(priv); |
509 | else |
510 | vdic_stop(priv); |
511 | if (ret) |
512 | goto out; |
513 | |
514 | if (src_sd) { |
515 | /* start/stop upstream */ |
516 | ret = v4l2_subdev_call(src_sd, video, s_stream, enable); |
517 | ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; |
518 | if (ret) { |
519 | if (enable) |
520 | vdic_stop(priv); |
521 | goto out; |
522 | } |
523 | } |
524 | |
525 | update_count: |
526 | priv->stream_count += enable ? 1 : -1; |
527 | if (priv->stream_count < 0) |
528 | priv->stream_count = 0; |
529 | out: |
530 | mutex_unlock(lock: &priv->lock); |
531 | return ret; |
532 | } |
533 | |
534 | static struct v4l2_mbus_framefmt * |
535 | __vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_state *sd_state, |
536 | unsigned int pad, enum v4l2_subdev_format_whence which) |
537 | { |
538 | if (which == V4L2_SUBDEV_FORMAT_TRY) |
539 | return v4l2_subdev_state_get_format(sd_state, pad); |
540 | else |
541 | return &priv->format_mbus[pad]; |
542 | } |
543 | |
544 | static int vdic_enum_mbus_code(struct v4l2_subdev *sd, |
545 | struct v4l2_subdev_state *sd_state, |
546 | struct v4l2_subdev_mbus_code_enum *code) |
547 | { |
548 | if (code->pad >= VDIC_NUM_PADS) |
549 | return -EINVAL; |
550 | |
551 | return imx_media_enum_ipu_formats(code: &code->code, index: code->index, |
552 | fmt_sel: PIXFMT_SEL_YUV); |
553 | } |
554 | |
555 | static int vdic_get_fmt(struct v4l2_subdev *sd, |
556 | struct v4l2_subdev_state *sd_state, |
557 | struct v4l2_subdev_format *sdformat) |
558 | { |
559 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
560 | struct v4l2_mbus_framefmt *fmt; |
561 | int ret = 0; |
562 | |
563 | if (sdformat->pad >= VDIC_NUM_PADS) |
564 | return -EINVAL; |
565 | |
566 | mutex_lock(&priv->lock); |
567 | |
568 | fmt = __vdic_get_fmt(priv, sd_state, pad: sdformat->pad, which: sdformat->which); |
569 | if (!fmt) { |
570 | ret = -EINVAL; |
571 | goto out; |
572 | } |
573 | |
574 | sdformat->format = *fmt; |
575 | out: |
576 | mutex_unlock(lock: &priv->lock); |
577 | return ret; |
578 | } |
579 | |
580 | static void vdic_try_fmt(struct vdic_priv *priv, |
581 | struct v4l2_subdev_state *sd_state, |
582 | struct v4l2_subdev_format *sdformat, |
583 | const struct imx_media_pixfmt **cc) |
584 | { |
585 | struct v4l2_mbus_framefmt *infmt; |
586 | |
587 | *cc = imx_media_find_ipu_format(code: sdformat->format.code, |
588 | fmt_sel: PIXFMT_SEL_YUV); |
589 | if (!*cc) { |
590 | u32 code; |
591 | |
592 | imx_media_enum_ipu_formats(code: &code, index: 0, fmt_sel: PIXFMT_SEL_YUV); |
593 | *cc = imx_media_find_ipu_format(code, fmt_sel: PIXFMT_SEL_YUV); |
594 | sdformat->format.code = (*cc)->codes[0]; |
595 | } |
596 | |
597 | infmt = __vdic_get_fmt(priv, sd_state, pad: priv->active_input_pad, |
598 | which: sdformat->which); |
599 | |
600 | switch (sdformat->pad) { |
601 | case VDIC_SRC_PAD_DIRECT: |
602 | sdformat->format = *infmt; |
603 | /* output is always progressive! */ |
604 | sdformat->format.field = V4L2_FIELD_NONE; |
605 | break; |
606 | case VDIC_SINK_PAD_DIRECT: |
607 | case VDIC_SINK_PAD_IDMAC: |
608 | v4l_bound_align_image(width: &sdformat->format.width, |
609 | MIN_W, MAX_W_VDIC, W_ALIGN, |
610 | height: &sdformat->format.height, |
611 | MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN); |
612 | |
613 | /* input must be interlaced! Choose SEQ_TB if not */ |
614 | if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field)) |
615 | sdformat->format.field = V4L2_FIELD_SEQ_TB; |
616 | break; |
617 | } |
618 | |
619 | imx_media_try_colorimetry(tryfmt: &sdformat->format, ic_route: true); |
620 | } |
621 | |
622 | static int vdic_set_fmt(struct v4l2_subdev *sd, |
623 | struct v4l2_subdev_state *sd_state, |
624 | struct v4l2_subdev_format *sdformat) |
625 | { |
626 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
627 | const struct imx_media_pixfmt *cc; |
628 | struct v4l2_mbus_framefmt *fmt; |
629 | int ret = 0; |
630 | |
631 | if (sdformat->pad >= VDIC_NUM_PADS) |
632 | return -EINVAL; |
633 | |
634 | mutex_lock(&priv->lock); |
635 | |
636 | if (priv->stream_count > 0) { |
637 | ret = -EBUSY; |
638 | goto out; |
639 | } |
640 | |
641 | vdic_try_fmt(priv, sd_state, sdformat, cc: &cc); |
642 | |
643 | fmt = __vdic_get_fmt(priv, sd_state, pad: sdformat->pad, which: sdformat->which); |
644 | *fmt = sdformat->format; |
645 | |
646 | /* propagate format to source pad */ |
647 | if (sdformat->pad == VDIC_SINK_PAD_DIRECT || |
648 | sdformat->pad == VDIC_SINK_PAD_IDMAC) { |
649 | const struct imx_media_pixfmt *outcc; |
650 | struct v4l2_mbus_framefmt *outfmt; |
651 | struct v4l2_subdev_format format; |
652 | |
653 | format.pad = VDIC_SRC_PAD_DIRECT; |
654 | format.which = sdformat->which; |
655 | format.format = sdformat->format; |
656 | vdic_try_fmt(priv, sd_state, sdformat: &format, cc: &outcc); |
657 | |
658 | outfmt = __vdic_get_fmt(priv, sd_state, pad: VDIC_SRC_PAD_DIRECT, |
659 | which: sdformat->which); |
660 | *outfmt = format.format; |
661 | if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE) |
662 | priv->cc[VDIC_SRC_PAD_DIRECT] = outcc; |
663 | } |
664 | |
665 | if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE) |
666 | priv->cc[sdformat->pad] = cc; |
667 | out: |
668 | mutex_unlock(lock: &priv->lock); |
669 | return ret; |
670 | } |
671 | |
672 | static int vdic_link_setup(struct media_entity *entity, |
673 | const struct media_pad *local, |
674 | const struct media_pad *remote, u32 flags) |
675 | { |
676 | struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); |
677 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
678 | struct v4l2_subdev *remote_sd; |
679 | int ret = 0; |
680 | |
681 | dev_dbg(priv->ipu_dev, "%s: link setup %s -> %s" , |
682 | sd->name, remote->entity->name, local->entity->name); |
683 | |
684 | mutex_lock(&priv->lock); |
685 | |
686 | if (local->flags & MEDIA_PAD_FL_SOURCE) { |
687 | if (!is_media_entity_v4l2_subdev(entity: remote->entity)) { |
688 | ret = -EINVAL; |
689 | goto out; |
690 | } |
691 | |
692 | remote_sd = media_entity_to_v4l2_subdev(remote->entity); |
693 | |
694 | if (flags & MEDIA_LNK_FL_ENABLED) { |
695 | if (priv->sink_sd) { |
696 | ret = -EBUSY; |
697 | goto out; |
698 | } |
699 | priv->sink_sd = remote_sd; |
700 | } else { |
701 | priv->sink_sd = NULL; |
702 | } |
703 | |
704 | goto out; |
705 | } |
706 | |
707 | /* this is a sink pad */ |
708 | |
709 | if (flags & MEDIA_LNK_FL_ENABLED) { |
710 | if (priv->src) { |
711 | ret = -EBUSY; |
712 | goto out; |
713 | } |
714 | } else { |
715 | priv->src = NULL; |
716 | goto out; |
717 | } |
718 | |
719 | if (local->index == VDIC_SINK_PAD_IDMAC) { |
720 | struct imx_media_video_dev *vdev = priv->vdev; |
721 | |
722 | if (!is_media_entity_v4l2_video_device(entity: remote->entity)) { |
723 | ret = -EINVAL; |
724 | goto out; |
725 | } |
726 | if (!vdev) { |
727 | ret = -ENODEV; |
728 | goto out; |
729 | } |
730 | |
731 | priv->csi_direct = false; |
732 | } else { |
733 | if (!is_media_entity_v4l2_subdev(entity: remote->entity)) { |
734 | ret = -EINVAL; |
735 | goto out; |
736 | } |
737 | |
738 | remote_sd = media_entity_to_v4l2_subdev(remote->entity); |
739 | |
740 | /* direct pad must connect to a CSI */ |
741 | if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) || |
742 | remote->index != CSI_SRC_PAD_DIRECT) { |
743 | ret = -EINVAL; |
744 | goto out; |
745 | } |
746 | |
747 | priv->csi_direct = true; |
748 | } |
749 | |
750 | priv->src = remote->entity; |
751 | /* record which input pad is now active */ |
752 | priv->active_input_pad = local->index; |
753 | out: |
754 | mutex_unlock(lock: &priv->lock); |
755 | return ret; |
756 | } |
757 | |
758 | static int vdic_link_validate(struct v4l2_subdev *sd, |
759 | struct media_link *link, |
760 | struct v4l2_subdev_format *source_fmt, |
761 | struct v4l2_subdev_format *sink_fmt) |
762 | { |
763 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
764 | int ret; |
765 | |
766 | ret = v4l2_subdev_link_validate_default(sd, link, |
767 | source_fmt, sink_fmt); |
768 | if (ret) |
769 | return ret; |
770 | |
771 | mutex_lock(&priv->lock); |
772 | |
773 | if (priv->csi_direct && priv->motion != HIGH_MOTION) { |
774 | v4l2_err(&priv->sd, |
775 | "direct CSI pipeline requires high motion\n" ); |
776 | ret = -EINVAL; |
777 | } |
778 | |
779 | mutex_unlock(lock: &priv->lock); |
780 | return ret; |
781 | } |
782 | |
783 | static int vdic_get_frame_interval(struct v4l2_subdev *sd, |
784 | struct v4l2_subdev_state *sd_state, |
785 | struct v4l2_subdev_frame_interval *fi) |
786 | { |
787 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
788 | |
789 | /* |
790 | * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2 |
791 | * subdev active state API. |
792 | */ |
793 | if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE) |
794 | return -EINVAL; |
795 | |
796 | if (fi->pad >= VDIC_NUM_PADS) |
797 | return -EINVAL; |
798 | |
799 | mutex_lock(&priv->lock); |
800 | |
801 | fi->interval = priv->frame_interval[fi->pad]; |
802 | |
803 | mutex_unlock(lock: &priv->lock); |
804 | |
805 | return 0; |
806 | } |
807 | |
808 | static int vdic_set_frame_interval(struct v4l2_subdev *sd, |
809 | struct v4l2_subdev_state *sd_state, |
810 | struct v4l2_subdev_frame_interval *fi) |
811 | { |
812 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
813 | struct v4l2_fract *input_fi, *output_fi; |
814 | int ret = 0; |
815 | |
816 | /* |
817 | * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2 |
818 | * subdev active state API. |
819 | */ |
820 | if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE) |
821 | return -EINVAL; |
822 | |
823 | mutex_lock(&priv->lock); |
824 | |
825 | input_fi = &priv->frame_interval[priv->active_input_pad]; |
826 | output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT]; |
827 | |
828 | switch (fi->pad) { |
829 | case VDIC_SINK_PAD_DIRECT: |
830 | case VDIC_SINK_PAD_IDMAC: |
831 | /* No limits on valid input frame intervals */ |
832 | if (fi->interval.numerator == 0 || |
833 | fi->interval.denominator == 0) |
834 | fi->interval = priv->frame_interval[fi->pad]; |
835 | /* Reset output interval */ |
836 | *output_fi = fi->interval; |
837 | if (priv->csi_direct) |
838 | output_fi->denominator *= 2; |
839 | break; |
840 | case VDIC_SRC_PAD_DIRECT: |
841 | /* |
842 | * frame rate at output pad is double input |
843 | * rate when using direct CSI->VDIC pipeline. |
844 | * |
845 | * TODO: implement VDIC frame skipping |
846 | */ |
847 | fi->interval = *input_fi; |
848 | if (priv->csi_direct) |
849 | fi->interval.denominator *= 2; |
850 | break; |
851 | default: |
852 | ret = -EINVAL; |
853 | goto out; |
854 | } |
855 | |
856 | priv->frame_interval[fi->pad] = fi->interval; |
857 | out: |
858 | mutex_unlock(lock: &priv->lock); |
859 | return ret; |
860 | } |
861 | |
862 | static int vdic_registered(struct v4l2_subdev *sd) |
863 | { |
864 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
865 | int i, ret; |
866 | u32 code; |
867 | |
868 | for (i = 0; i < VDIC_NUM_PADS; i++) { |
869 | code = 0; |
870 | if (i != VDIC_SINK_PAD_IDMAC) |
871 | imx_media_enum_ipu_formats(code: &code, index: 0, fmt_sel: PIXFMT_SEL_YUV); |
872 | |
873 | /* set a default mbus format */ |
874 | ret = imx_media_init_mbus_fmt(mbus: &priv->format_mbus[i], |
875 | IMX_MEDIA_DEF_PIX_WIDTH, |
876 | IMX_MEDIA_DEF_PIX_HEIGHT, code, |
877 | field: V4L2_FIELD_NONE, cc: &priv->cc[i]); |
878 | if (ret) |
879 | return ret; |
880 | |
881 | /* init default frame interval */ |
882 | priv->frame_interval[i].numerator = 1; |
883 | priv->frame_interval[i].denominator = 30; |
884 | if (i == VDIC_SRC_PAD_DIRECT) |
885 | priv->frame_interval[i].denominator *= 2; |
886 | } |
887 | |
888 | priv->active_input_pad = VDIC_SINK_PAD_DIRECT; |
889 | |
890 | return vdic_init_controls(priv); |
891 | } |
892 | |
893 | static void vdic_unregistered(struct v4l2_subdev *sd) |
894 | { |
895 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
896 | |
897 | v4l2_ctrl_handler_free(hdl: &priv->ctrl_hdlr); |
898 | } |
899 | |
900 | static const struct v4l2_subdev_pad_ops vdic_pad_ops = { |
901 | .enum_mbus_code = vdic_enum_mbus_code, |
902 | .get_fmt = vdic_get_fmt, |
903 | .set_fmt = vdic_set_fmt, |
904 | .get_frame_interval = vdic_get_frame_interval, |
905 | .set_frame_interval = vdic_set_frame_interval, |
906 | .link_validate = vdic_link_validate, |
907 | }; |
908 | |
909 | static const struct v4l2_subdev_video_ops vdic_video_ops = { |
910 | .s_stream = vdic_s_stream, |
911 | }; |
912 | |
913 | static const struct media_entity_operations vdic_entity_ops = { |
914 | .link_setup = vdic_link_setup, |
915 | .link_validate = v4l2_subdev_link_validate, |
916 | }; |
917 | |
918 | static const struct v4l2_subdev_ops vdic_subdev_ops = { |
919 | .video = &vdic_video_ops, |
920 | .pad = &vdic_pad_ops, |
921 | }; |
922 | |
923 | static const struct v4l2_subdev_internal_ops vdic_internal_ops = { |
924 | .init_state = imx_media_init_state, |
925 | .registered = vdic_registered, |
926 | .unregistered = vdic_unregistered, |
927 | }; |
928 | |
929 | struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev, |
930 | struct device *ipu_dev, |
931 | struct ipu_soc *ipu, |
932 | u32 grp_id) |
933 | { |
934 | struct vdic_priv *priv; |
935 | int i, ret; |
936 | |
937 | priv = devm_kzalloc(dev: ipu_dev, size: sizeof(*priv), GFP_KERNEL); |
938 | if (!priv) |
939 | return ERR_PTR(error: -ENOMEM); |
940 | |
941 | priv->ipu_dev = ipu_dev; |
942 | priv->ipu = ipu; |
943 | |
944 | v4l2_subdev_init(sd: &priv->sd, ops: &vdic_subdev_ops); |
945 | v4l2_set_subdevdata(sd: &priv->sd, p: priv); |
946 | priv->sd.internal_ops = &vdic_internal_ops; |
947 | priv->sd.entity.ops = &vdic_entity_ops; |
948 | priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; |
949 | priv->sd.owner = ipu_dev->driver->owner; |
950 | priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE; |
951 | priv->sd.grp_id = grp_id; |
952 | imx_media_grp_id_to_sd_name(sd_name: priv->sd.name, sz: sizeof(priv->sd.name), |
953 | grp_id: priv->sd.grp_id, ipu_id: ipu_get_num(ipu)); |
954 | |
955 | mutex_init(&priv->lock); |
956 | |
957 | for (i = 0; i < VDIC_NUM_PADS; i++) |
958 | priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ? |
959 | MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK; |
960 | |
961 | ret = media_entity_pads_init(entity: &priv->sd.entity, num_pads: VDIC_NUM_PADS, |
962 | pads: priv->pad); |
963 | if (ret) |
964 | goto free; |
965 | |
966 | ret = v4l2_device_register_subdev(v4l2_dev, sd: &priv->sd); |
967 | if (ret) |
968 | goto free; |
969 | |
970 | return &priv->sd; |
971 | free: |
972 | mutex_destroy(lock: &priv->lock); |
973 | return ERR_PTR(error: ret); |
974 | } |
975 | |
976 | int imx_media_vdic_unregister(struct v4l2_subdev *sd) |
977 | { |
978 | struct vdic_priv *priv = v4l2_get_subdevdata(sd); |
979 | |
980 | v4l2_info(sd, "Removing\n" ); |
981 | |
982 | v4l2_device_unregister_subdev(sd); |
983 | mutex_destroy(lock: &priv->lock); |
984 | media_entity_cleanup(entity: &sd->entity); |
985 | |
986 | return 0; |
987 | } |
988 | |